多旋翼飛行器設(shè)計與控制理論 -第09講-狀態(tài)估計V2

多旋翼飛行器設(shè)計與控制

第九講狀態(tài)估計

全2024/5/282東方智慧兼聽則明2024/5/283如何對多傳感器信息進(jìn)行融合？前言大綱1.姿態(tài)估計2.位置估計3.速度估計4.障礙估計5.本講小結(jié)2024/5/2841.姿態(tài)估計2024/5/285測量原理（1）俯仰角和滾轉(zhuǎn)角測量原理根據(jù)第六講氣動阻力模型，忽略速度與角速度的交叉項(xiàng)，比力滿足其中是加速度計測量值。當(dāng)系統(tǒng)處于穩(wěn)定狀態(tài)，比如勻速運(yùn)動或者靜止，即1.姿態(tài)估計2024/5/286測量原理（1）俯仰角和滾轉(zhuǎn)角測量原理因此，低頻的俯仰角和滾轉(zhuǎn)角信號觀測量可以由加速度計讀數(shù)近似獲得，表示如下其中是加速度計測量值。注：如果機(jī)體振動很大，那么將被噪聲嚴(yán)重污染，這樣將進(jìn)一步影響角度的估計。因此，機(jī)體的減振很重要。1.姿態(tài)估計2024/5/287測量原理（2）偏航角測量原理圖9.1地磁場示意圖（感謝愛丁堡大學(xué)PeterReid提供的圖片）磁軸和地軸不是重合的。磁場在地球表面的水平投影不是嚴(yán)格指向地軸。磁偏角是磁場強(qiáng)度矢量的水平投影與正北方向之間的夾角，即磁子午線與地理子午線之間的夾角。如果磁場強(qiáng)度矢量的指向偏向正北方向以東稱東偏，偏向正北方向以西稱西偏。各個地方的磁偏角不同，而且由于磁極也處在運(yùn)動之中，某一地點(diǎn)磁偏角會隨時間而改變。因此，要精確找到正北方向，需要經(jīng)過兩步。1.姿態(tài)估計2024/5/288測量原理（2）偏航角測量原理

1）第一步，確定磁場方向在水平面的向量，求出方位角。磁力計的讀數(shù)為?？紤]到磁力計可能不是水平放置，所以需要利用兩軸傾角傳感器測量的角度將磁力計的測量值投影到水平面。因此，先做如下變換[1]其中表示磁力計讀數(shù)在水平面的投影。[1]CarusoMJ.Applicationsofmagnetoresistivesensorsinnavigationsystems[R].SAETechnicalPaper,1997.1.姿態(tài)估計2024/5/289測量原理（2）偏航角測量原理

1）第一步，確定磁場方向在水平面的向量，求出方位角。定義，那么可以表示為：定義，那么可以表示為：機(jī)頭順時針方向轉(zhuǎn)動，偏航角為正。1.姿態(tài)估計2024/5/2810測量原理（2）偏航角測量原理

2）第二步，加上或減去磁偏角修正到正北方向。北京磁偏角約為6°偏西。因此，在北京磁場方向上加上6°的磁偏角，能找到正北方。圖9.22015年世界磁偏角分布圖（圖片源自/geomag/WMM/image.shtml）1.姿態(tài)估計2024/5/2811測量原理（2）偏航角測量原理

2）第二步，加上或減去磁偏角修正到正北方向。半自主飛行時，我們可以將地球固連坐標(biāo)系軸指向本地磁場方向，如左圖所示。從圖中看出，本地磁場偏西。當(dāng)作全自主飛行時，因?yàn)樾枰c地球的經(jīng)緯度一致，我們可以將地球固連坐標(biāo)系軸指向正北方向。圖9.3本地磁場和正北方向1.姿態(tài)估計2024/5/2812測量原理（2）偏航角測量原理對于體型較大的飛行器，可以分別在機(jī)頭和機(jī)尾安裝GPS天線。通過測量它們的地理位置，來確定飛行器的偏航角。然而，對于小型多旋翼來說，安裝空間很小，多個GPS之間基線很短，較難準(zhǔn)確測定偏航角，除非利用高精度差分GPS。

如右圖為差分GPS測向的原理。在多旋翼機(jī)頭機(jī)尾裝上兩個差分GPS移動站（或雙天線GPS），可以獲取它們在地球固連坐標(biāo)系下厘米級的定位精度，根據(jù)兩點(diǎn)確定一條直線的方法，可以較簡單地獲取多旋翼的機(jī)頭朝向。注：兩移動站分的越開，精度越高，距離越短精度越低，一般需要大于30cm。差分GPS測向原理1.姿態(tài)估計2024/5/2813線性互補(bǔ)濾波器姿態(tài)角變化率和陀螺儀的角速度有如下關(guān)系因?yàn)檎J(rèn)為多旋翼工作過程中，所以上式近似為姿態(tài)角可以由加速度計和電子羅盤測量得到，漂移小，但噪聲大。另一方面，姿態(tài)角也可以通過陀螺儀測得的角速度積分得到，該方法噪聲小，但漂移大?；パa(bǔ)濾波器的基本思想是利用它們各自的優(yōu)勢，在頻域上特征互補(bǔ)，得到更精確的姿態(tài)角。1.姿態(tài)估計2024/5/2814線性互補(bǔ)濾波器

下面我們著重以俯仰角為例，詳細(xì)推導(dǎo)線性互補(bǔ)濾波器。俯仰角的拉氏變換可以表示為為了估計俯仰角，以上式子的需要用傳感器信息替代。低通濾波器，表示時間常數(shù)高通濾波器1）加速度計測量的俯仰角無漂移但噪聲大，為了簡便，俯仰角測量值建模為2）陀螺儀的角速度測量會有漂移但噪聲小，可以建模為其中表示高頻噪聲，表示真值。角速率積分的拉氏變換常值漂移的拉氏變換（1）俯仰角1.姿態(tài)估計2024/5/2815線性互補(bǔ)濾波器

下面我們著重以俯仰角為例，詳細(xì)推導(dǎo)線性互補(bǔ)濾波器。俯仰角的拉氏變換可以表示為為了估計俯仰角，以上式子的需要用傳感器信息替代。低通濾波器，表示時間常數(shù)高通濾波器（1）俯仰角線性互補(bǔ)濾波器的標(biāo)準(zhǔn)形式以傳遞函數(shù)方式表示為陀螺儀的角速度積分加速度計測量的俯仰角1.姿態(tài)估計2024/5/2816線性互補(bǔ)濾波器

下面我們著重以俯仰角為例，詳細(xì)推導(dǎo)線性互補(bǔ)濾波器。俯仰角的拉氏變換可以表示為為了估計俯仰角，以上式子的需要用傳感器信息替代。低通濾波器，表示時間常數(shù)高通濾波器（1）俯仰角線性互補(bǔ)濾波器的標(biāo)準(zhǔn)形式以傳遞函數(shù)方式表示為加速度計測量的俯仰角陀螺儀的角速度積分衰減為零1.姿態(tài)估計2024/5/2817線性互補(bǔ)濾波器圖9.4互補(bǔ)濾波器估計俯仰角流程圖低通濾波器將無漂移的優(yōu)勢保留下來，而高通濾波器將噪聲小的優(yōu)勢保留下來為了計算機(jī)算法實(shí)現(xiàn)，需要對其進(jìn)行離散化通過一階向后差分法，將表示為[2]進(jìn)一步表示為表示采樣周期再把上式轉(zhuǎn)化為差分形式得到（1）俯仰角[2].PerdikarisGA.ComputerControlledSystems.Berlin:Springer-Netherlands,1991.1.姿態(tài)估計2024/5/2818線性互補(bǔ)濾波器如果，那么

。這樣以上的互補(bǔ)濾波算法可以寫為圖9.5互補(bǔ)濾波器估計俯仰角實(shí)驗(yàn)仿真圖，其中，acc、wx和cf分別表示利用加速度估計的俯仰角、角速度直接積分后的俯仰角以及互補(bǔ)濾波后的俯仰角我們利用Pixhawk自駕儀傳感器實(shí)時傳回的數(shù)據(jù)進(jìn)行一階向后差分法估計俯仰角，效果如右圖所示。從結(jié)果不難看出通過互補(bǔ)濾波器，可以得到平滑的俯仰角估計值，而對陀螺儀直接進(jìn)行積分，結(jié)果是發(fā)散的。滾轉(zhuǎn)角估計方法類似。（1）俯仰角1.姿態(tài)估計2024/5/2819線性互補(bǔ)濾波器（2）偏航角如果偏航角可由GPS提供和電子羅盤提供。一種簡單的方法是定義測量的偏航角為兩者的加權(quán)和，即其中：是加權(quán)因子。因?yàn)殡娮恿_盤的采樣頻率和陀螺儀的采樣頻率高于GPS，所以可以采取以下方式獲取偏航角得到之后，可以得到偏航角估計為1.姿態(tài)估計2024/5/2820非線性互補(bǔ)濾波器非線性互補(bǔ)濾波器與線性互補(bǔ)濾波器的大體思想相似，都是利用加速度計和陀螺儀的互補(bǔ)優(yōu)勢，不同之處在于它們不是直接線性相加，而是遵從角速度和角度的非線性關(guān)系。根據(jù)非線性互補(bǔ)濾波器的原理，可以按照如下形式進(jìn)行濾波[3][3]MahonyR,HamelT,PflimlinJM.Nonlinearcomplementaryfiltersonthespecialorthogonalgroup[J].IEEETransactionsonAutomaticControl,2008,53(5):1203-1218.陀螺儀的角速度角度測量反饋增益定義表示互補(bǔ)濾波器估計輸出的姿態(tài)旋轉(zhuǎn)矩陣，代表由加速度傳感器和電子羅盤觀測到的姿態(tài)旋轉(zhuǎn)矩陣，而表示與之間的誤差，定義為1.姿態(tài)估計2024/5/2821卡爾曼濾波器非線性互補(bǔ)濾波器方法所用的狀態(tài)量高達(dá)12維，而且參數(shù)選取不能保證最優(yōu)。為降低方程的非線性化程度，可以考慮以下一種卡爾曼濾波器方法[4]。定義第三列提出來得到由于多旋翼在一般情況下，沿的加速度值很小，因此加速度觀測量可表示為其中。過程模型觀測模型[4].KangCW,ParkCG.AttitudeestimationwithaccelerometersandgyrosusingfuzzytunedKalmanfilter[C].ControlConference(ECC),2009European.IEEE,2009:3713-3718.多旋翼在飛行時，通過GPS或者視覺定位系統(tǒng)，可以獲得當(dāng)前的位置；聯(lián)合高度傳感器（一般為氣壓計），可以進(jìn)一步獲得當(dāng)前的更精確的多旋翼位置信息。定義絕對位置為。2.位置估計2024/5/2822基于GPS的位置估計過程模型可以表示為這里用到了加速度計等慣性測量單元，氣壓計和GPS，傳感器模型和參數(shù)請見第七講。觀測模型可以表示為如果考慮GPS的測量偏移？如果考慮GPS的高度測量作為觀測量？實(shí)際過程中，氣壓計和GPS同時測高度面臨問題，風(fēng)、GPS失效？SLAM(SimultaneousLocalizationandMapping)，也稱同步定位與建圖技術(shù)。SLAM問題可以描述為:機(jī)器人在未知環(huán)境中從一個未知位置開始移動，在移動過程中利用傳感器估計自身位置的同時建造周圍環(huán)境的增量式地圖，實(shí)現(xiàn)機(jī)器人的自主定位和導(dǎo)航[5][6]。2.位置估計2024/5/2823基于SLAM的位置估計視頻.AutonomousAerialNavigationinConfinedIndoorEnvironmentsFromhttps://youtu.be/IMSozUpFFkU[5].WhyteH,BalieyT.SimultaneousLocalizationandMapping(SLAM)Part1TheEssentialAlgorithms[J].IEEERobotics&AutomationMagazine,2006.[6].BaileyT,Durrant-WhyteH.Simultaneouslocalizationandmapping(SLAM):PartII[J].IEEERobotics&AutomationMagazine,2006,13(3):108-117.2.位置估計2024/5/2824表9.1開源SLAM算法作者描述網(wǎng)址CyrillStachniss,UdoFrese,GiorgioGrisetti一個提供給做SLAM研究的平臺，上面有很多公開的SLAM算法，同時包括一些數(shù)據(jù)。KaiArras提供了研究移動機(jī)器人SLAM的MATLAB仿真工具箱（CASRobotNavigationToolbox）http://www.cas.kth.se/toolboxTimBailey

提供了EKF-SLAM，F(xiàn)astSLAM1.0，F(xiàn)astSLAM2.0，UKF-SLAM的MATLAB仿真算法rmatik.uni-freiburg.de/bailey-slam.htmlMarkPaskin

提供了一些實(shí)現(xiàn)SLAM算法的Java類庫，包括卡爾曼濾波、信息濾波和MATLAB交互的細(xì)化結(jié)點(diǎn)樹濾波。/~paskin/slamAndrewDavison提供了一些用來建圖和定位的C++庫Scene，能夠?qū)崿F(xiàn)實(shí)時單目SLAM功能http://www.doc.ic.ac.uk/~ajd/Scene/index.htmlJosé

Neira提供了實(shí)現(xiàn)EKF-SLAM的MATLAB模擬器，證明了聯(lián)合相容分支定界算法的數(shù)據(jù)關(guān)聯(lián)http://webdiis.unizar.es/~neira/software/slam/slamsim.htmlDirkHahnel用C語言實(shí)現(xiàn)了基于網(wǎng)格的Fast-SLAMhttp://dblp.uni-trier.de/pers/hd/h/H=auml=hnel:Dirk.htmlDurrantWhyte,EduardoNebot,etal來自2002年在瑞典KTH舉辦的以SLAM為主題的暑期學(xué)校時候的MATLAB代碼http://www.cas.kth.se/SLAM/schedule.html2.位置估計2024/5/2825表9.2SLAM相關(guān)數(shù)據(jù)庫開發(fā)者描述網(wǎng)址AndrewHowardandNicholasRoy標(biāo)準(zhǔn)的機(jī)器人數(shù)據(jù)集，包括實(shí)際場景下激光和聲納數(shù)據(jù)；仿真環(huán)境下，不同傳感器數(shù)據(jù)；不同的地圖

JoseGuivant，JuanNietoandEduardoNebot大量的室外數(shù)據(jù)集，尤其包括著名的維多利亞公園的數(shù)據(jù).au/index.shtmlRadish（TheRoboticsDataSetRepository）大量豐富的室內(nèi)數(shù)據(jù)集，包括來自加利福利亞大學(xué)圖書館、西雅圖的Intel研究實(shí)驗(yàn)室、Edmonton會議中心的大片區(qū)域的數(shù)據(jù)IJRR（TheInternationalJournalofRoboticsResearch）IJRR對于每篇文章都有一個相關(guān)網(wǎng)頁，包含大量數(shù)據(jù)結(jié)果及影像資料2.位置估計2024/5/2826基于SLAM的位置估計（1）基于激光的SLAM圖9.6激光SLAM實(shí)驗(yàn)結(jié)果圖，其中圓點(diǎn)表示地圖地標(biāo)，線條表示運(yùn)動估計的軌跡利用現(xiàn)有的維多利亞公園數(shù)據(jù)，結(jié)合二維激光SLAM算法，進(jìn)行MATLAB仿真實(shí)驗(yàn)。實(shí)際SLAM實(shí)驗(yàn)結(jié)果如左圖(a)所示。而在實(shí)驗(yàn)仿真過程中，給出了慣導(dǎo)及激光掃描儀數(shù)據(jù)，直接利用這些數(shù)據(jù)代替實(shí)際的數(shù)據(jù)采集過程，不斷地與原有的地標(biāo)匹配和更新，這些特征地標(biāo)有助于跟蹤定位。最終仿真實(shí)驗(yàn)結(jié)果如圖(b)所示。2.位置估計2024/5/2827基于SLAM的位置估計（1）基于激光的SLAM目前大部分算法多基于二維平面，因此對于多旋翼這種在三維空間運(yùn)動的飛行器，我們要對算法的輸入稍作改變。首先，要求多旋翼定高飛行，這樣基本可以認(rèn)為多旋翼在二維平面上運(yùn)動。其次，認(rèn)為飛行器是在走廊類型的空間飛行，也就是空間的水平截面是相同的，至少在指定高度周邊是相同的。大部分室內(nèi)環(huán)境可以認(rèn)為滿足這個條件。視頻.[NUSUAV]Indoorautonomousquadrotorwithlaserscanner,https://youtu.be/bbtQy5g4tuc2.位置估計2024/5/2828基于SLAM的位置估計（2）基于單目視覺的SLAM在單目視覺SLAM框架中，有兩個核心步驟，一個是根據(jù)場景的結(jié)構(gòu)信息求取攝像機(jī)位姿，另一個是根據(jù)求取的攝像機(jī)位姿來重建場景的三維結(jié)構(gòu)。我們將第一個步驟稱為跟蹤(Tracking)，將第二個步驟稱為建圖(Mapping)。連續(xù)幀SLAM跟蹤和建圖的任務(wù)交替進(jìn)行，跟蹤依賴于建圖得到的場景結(jié)構(gòu)信息，而建圖反過來又依賴于跟蹤求取的攝像機(jī)位姿。關(guān)鍵幀SLAM將跟蹤和建圖兩個任務(wù)分離開，并且分別在兩個獨(dú)立的線程上運(yùn)行。這樣就可以在不影響相機(jī)跟蹤實(shí)時性的前提下，在建圖任務(wù)中采用時間效率低、精確性高的運(yùn)動求取結(jié)構(gòu)技術(shù)。2.位置估計2024/5/2829基于SLAM的位置估計（2）基于單目視覺的SLAM目前微小型無人機(jī)多采用關(guān)鍵幀SLAM或者說PTAM技術(shù)。由于SLAM方法大都采用單相機(jī)實(shí)現(xiàn)，因此得不到位置的尺度信息。但是可以由加速度計、氣壓計或超聲波模塊得到。接下來介紹如何將尺度信息恢復(fù)。建立過程模型如下：AchtelikM,AchtelikM,WeissS,etal.OnboardIMUandmonocularvisionbasedcontrolforMAVsinunknownin-andoutdoorenvironments[C]//Roboticsandautomation(ICRA),2011IEEEinternationalconferenceon.IEEE,2011:3056-3063.分別表示高度、高度方向上的速度、尺度因子和氣壓計偏移，而表示相應(yīng)的高斯噪聲。2.位置估計2024/5/2830基于SLAM的位置估計（2）基于單目視覺的SLAM[7]AchtelikM,AchtelikM,WeissS,etal.OnboardIMUandmonocularvisionbasedcontrolforMAVsinunknownin-andoutdoorenvironments[C]//Roboticsandautomation(ICRA),2011IEEEinternationalconferenceon.IEEE,2011:3056-3063.目前微小型無人機(jī)多采用關(guān)鍵幀SLAM或者說PTAM技術(shù)。由于SLAM方法大都采用單相機(jī)實(shí)現(xiàn)，因此得不到位置的尺度信息。但是可以由加速度計、氣壓計或超聲波模塊得到。接下來介紹如何將尺度信息恢復(fù)。建立觀測模型如下[7]：SLAM提供的高度信息氣壓計提供的高度信息3.速度估計2024/5/2831基于光流的速度估計方法光流是指圖像中所有像素點(diǎn)構(gòu)成的一種二維瞬時速度場，其中的二維速度矢量是景物中可見點(diǎn)的三維速度矢量在成像表面的投影。圖9.7光流示意圖（圖片來自JamesJeromeGibson的書籍《TheEcologicalApproachtoVisualPerception》）。A圖表示遠(yuǎn)離群山時的光流，B圖表示靠近群山時的光流（1）光流3.速度估計2024/5/2832基于光流的速度估計方法假定圖像點(diǎn)在時刻的灰度為，經(jīng)過時間間隔后，對應(yīng)點(diǎn)的變?yōu)椤．?dāng)時，可以認(rèn)為兩點(diǎn)的灰度保持不變，也就是假設(shè)圖像灰度是其位置和時間的連續(xù)變化函數(shù)，可以將上式的左邊進(jìn)行泰勒級數(shù)展開由于，忽略，可以得到光流約束方程：光流圖像灰度相對于

的偏導(dǎo)（1）光流3.速度估計2024/5/2833基于光流的速度估計方法開發(fā)者描述網(wǎng)址ComputerVisionSystemToolboxMATLABR2012a以及更高版本自帶的計算機(jī)視覺工具箱，將光流計算封裝成一個類vision.OpticalFlow/help/vision/index.htmlOpenCV開源計算機(jī)視覺庫，提供很多光流計算的API接口函數(shù)，有1.x,2.x,3.0等版本MachineVisionToolbox側(cè)重機(jī)器視覺、三維視覺方面的工具箱,MATLAB與現(xiàn)代計算機(jī)工作站結(jié)合/Machine_Vision_Toolbox.htmlVLFeat計算機(jī)視覺/圖像處理開源項(xiàng)目，使用C語言編寫，提供C語言和MATLAB兩種接口，實(shí)現(xiàn)大量計算機(jī)視覺算法/download.htmlPeterKovesi’sToolbox全部由MATLAB的m文件實(shí)現(xiàn)計算機(jī)視覺算法，無需要編譯安裝，支持Octave，輕量好用/matlabfns表.光流計算相關(guān)工具箱（1）光流3.速度估計2024/5/2834基于光流的速度估計方法如右圖所示的坐標(biāo)系，單目攝像機(jī)固連于飛行器質(zhì)心，鏡頭垂直于機(jī)身向下安裝。為了簡便，假設(shè)攝像機(jī)坐標(biāo)系與機(jī)體坐標(biāo)系重合，用表示，地面近似為平面，記為。圖9.8機(jī)體坐標(biāo)系與地球固連坐標(biāo)系下的點(diǎn)，其中表示攝像機(jī)中心距離地面點(diǎn)的距離，點(diǎn)所處的平面是（2）光流與機(jī)體速度關(guān)系3.速度估計2024/5/2835基于光流的速度估計方法（2）光流與機(jī)體速度關(guān)系地面P點(diǎn)的歸一化圖像坐標(biāo)表示為光流地面P點(diǎn)在慣性系和機(jī)體系關(guān)系如下地面P點(diǎn)靜止代入3.速度估計2024/5/2836基于光流的速度估計方法（2）光流與機(jī)體速度關(guān)系最終寫成抽象形式為對于地面P點(diǎn)，圖像點(diǎn)為，光流可以通過光流解算方法獲得（圖像處理方式）可以通過三軸陀螺儀測量得到，而可以通過超聲波測距模塊獲得3.速度估計2024/5/2837基于光流的速度估計方法（3）基于光流的速度估計如果有個可求光流的圖像點(diǎn)，我們有那么的估計可采用以下方式得到其中，為消除偏差后的測量角速度?；诠饬鞯脑O(shè)計存在著以下需要考慮的問題：（1）陀螺儀、光流測量、高度測量的周期是不相同的，因此需要考慮它們測量值的時間對準(zhǔn)；（2）焦距長度不確定以及鏡頭畸變；（3）地面不平整或背景在移動等；（4）光流誤匹配的處理。3.速度估計2024/5/2838基于氣動阻力模型的速度估計方法參見第六講多旋翼的運(yùn)動模型。引入由于螺旋槳揮舞產(chǎn)生的阻力，并簡化，我們可以得到多旋翼氣動阻力模型如下其中，表示相應(yīng)的噪聲。就是表示在機(jī)體坐標(biāo)系下的加速度。3.速度估計2024/5/2839基于氣動阻力模型的速度估計方法過程模型：角速度替換為:其中歐拉角變化率和角速度之間的關(guān)系觀測模型：三軸加速度計測量以上模型是非線性模型，需要采用擴(kuò)展卡爾曼濾波器。[8]AbeywardenaD,KodagodaS,DissanayakeG,etal.ImprovedStateEstimationinQuadrotorMAVs:ANovelDrift-FreeVelocityEstimator[J].Robotics&AutomationMagazine,IEEE,2013,20(4):32-39.3.速度估計2024/5/2840基于氣動阻力模型的速度估計方法多旋翼的小角度假設(shè)即，所以忽略高階小量可以得到[9]線性過程模型：建立線性觀測模型如下：以上模型是線性模型，采用卡爾曼濾波器即可。[9]LeishmanRC,MacdonaldJC,BeardRW,eta