多旋翼飛行器：從原理到實(shí)踐 課件 ch06自駕儀校準(zhǔn)和姿態(tài)控制調(diào)試

自駕儀校準(zhǔn)和姿態(tài)控制調(diào)試“工業(yè)和信息化部“十四五”規(guī)劃教材多旋翼飛行器:從原理到實(shí)踐第六章01傳感器原理和校準(zhǔn)1.總體介紹自駕儀(或稱飛控)是自動(dòng)控制多旋翼姿態(tài)、位置和軌跡的設(shè)備，要想控制多旋翼首先需要得到多旋翼的當(dāng)前狀態(tài)。載人飛行器的姿態(tài)測(cè)量一般使用機(jī)械陀螺儀或者光纖陀螺儀，但是由于這些陀螺儀體積龐大，價(jià)格昂貴，因此無(wú)法在小型和微型無(wú)人機(jī)上使用。隨著計(jì)算機(jī)和微電子的發(fā)展，捷聯(lián)慣導(dǎo)(StrapdownInertialNavigationSystem)技術(shù)開(kāi)始得到廣泛應(yīng)用，它利用加速度和角速率來(lái)對(duì)飛行器的傾斜姿態(tài)、位置和速度進(jìn)行動(dòng)態(tài)的推算。此外，加速度和角速率的測(cè)量設(shè)備也是比較容易進(jìn)行微型化生產(chǎn)的，因此這類測(cè)量設(shè)備稱為MEMS(Micro-Electro-MechanicalSystem，微機(jī)電系統(tǒng))，簡(jiǎn)稱微機(jī)械機(jī)械陀螺儀與ST多軸MEMS陀螺儀大小的比較如圖6.1所示。1.總體介紹空間有X、Y、Z三個(gè)相互垂直的方向，還有繞這三個(gè)軸的旋轉(zhuǎn)方向，一共六個(gè)自由度(DegreeofFreedom，DOF)，見(jiàn)3.1節(jié)。如果每個(gè)自由度運(yùn)動(dòng)的測(cè)量都需要一組相關(guān)的傳感器，那么進(jìn)行完整的測(cè)量需要三個(gè)方向的陀螺儀測(cè)量角速率以及三個(gè)方向的加速度計(jì)測(cè)量加速度。1.總體介紹這樣的系統(tǒng)稱為6DOF系統(tǒng)，能夠提供6DOF姿態(tài)信息的設(shè)備稱為IMU(InertialMeasurementUnit，慣性測(cè)量單元)。使用IMU輸出的原始數(shù)據(jù)，再結(jié)合特定算法解算出姿態(tài)位置信息，這樣的系統(tǒng)稱為慣性導(dǎo)航系統(tǒng)(InertialNavigationSystem，INS)。IMU提供的是傳感器輸出的原始數(shù)據(jù)，而INS結(jié)合算法和IMU原始數(shù)據(jù)計(jì)算出更準(zhǔn)確的信息。1.總體介紹隨著技術(shù)的不斷進(jìn)步，微機(jī)械產(chǎn)品又發(fā)展出氣壓傳感器，用于測(cè)量氣壓高度和空速;磁場(chǎng)傳感器，也稱為磁羅盤、磁力計(jì)，用于測(cè)量地磁方向等。IMU再加入三軸磁力計(jì)一同解算，就稱為AHRS(AttitudeandHeadingReferenceSystem，航姿參考系統(tǒng))，其相關(guān)產(chǎn)品如圖6.2所示。有了航姿參考系統(tǒng)，再加上氣壓高度計(jì)，飛行器便能夠感知自身飛行時(shí)所處的高度和姿態(tài)。自駕儀系統(tǒng)中常見(jiàn)的MEMS傳感器分為兩種，一種是成品傳感器設(shè)備(如圖6.3所一種是MEMS芯片(如圖6.4所示)。成品傳感器是帶有外殼和連接器的完整設(shè)備，內(nèi)部有電源轉(zhuǎn)換、濾波和解算能力能夠輸出高精度的姿態(tài)、加速度或角速率信息，有的還能夠連接衛(wèi)星導(dǎo)航設(shè)備進(jìn)行融合解算，并且整體擁有一定的防霉菌、防潮濕和防鹽霧的三防能力。然而，成品傳感器往往價(jià)格高昂，常見(jiàn)的成品傳感器品牌包括MIT、BEI、MEAS、SiliconSensing等。而MEMS芯片主打更加低廉的成本和更高的集成度，早期甚至使用模擬信號(hào)進(jìn)行輸出，還需要用戶對(duì)溫度曲線進(jìn)行校準(zhǔn)。隨著技術(shù)發(fā)展，當(dāng)前常見(jiàn)的芯片級(jí)MEMS傳感器全部集成了解算算法，能夠自己進(jìn)行溫度補(bǔ)償甚至濾波計(jì)算。例如，InvenSense的產(chǎn)品MPU6000能夠直接輸出旋轉(zhuǎn)矩陣、四元數(shù)和歐拉角的融合演算數(shù)據(jù)，這大大減輕了自駕儀主處理器的負(fù)擔(dān)，因此其成為了開(kāi)源飛控的首選傳感器。2.三軸加速度計(jì)三軸加速度計(jì)是一種慣性傳感器,用于測(cè)量運(yùn)動(dòng)中的X、Y、Z三個(gè)空間軸方向的比力,即去掉重力后的整體加速度或者單位質(zhì)量上作用的非引力。當(dāng)加速度計(jì)相對(duì)地球保持靜止時(shí)加速度計(jì)能夠感知重力加速度，因此整體加速度的矢量合力為朝向地心的18加速度;而在自由落體運(yùn)動(dòng)中，三軸加速度計(jì)的輸出為零。MEMS加速度計(jì)原理如圖6.5所示。2.三軸加速度計(jì)飛行器所用的加速度計(jì)通?；贛EMS技術(shù)，因此其尺寸非常小，可以做到幾個(gè)毫米見(jiàn)方，而且只有毫瓦級(jí)的功耗。MEMS技術(shù)可以基于壓阻效應(yīng)、壓電效應(yīng)和電容原理，其中，電容效應(yīng)應(yīng)用比較廣泛。該類型的MEMS芯片內(nèi)部設(shè)計(jì)有一個(gè)帶有極板的重塊。隨著加速度改變，重塊的相對(duì)位置發(fā)生變化，極板之間的相對(duì)位置也發(fā)生變化通過(guò)測(cè)量極板之間的電容來(lái)得到加速度信號(hào)。進(jìn)一步，信號(hào)接入單片機(jī)，通過(guò)放大濾波等處理，最終輸出數(shù)字信號(hào)。該類傳感器的缺點(diǎn)是受振動(dòng)的影響大。當(dāng)?shù)刂亓Φ拇笮〔浑S加速度計(jì)姿態(tài)的改變而改變，一般情況下為g=9.81m/s2。加速度計(jì)正是基于此原理完成校準(zhǔn)，分別將三軸加速度計(jì)的六個(gè)面保持水平，在靜止過(guò)程中采集傳感器的測(cè)量值并進(jìn)行平均計(jì)算。之后，將平均值大小與真實(shí)加速度大小進(jìn)行比較，用于校準(zhǔn)參數(shù)。使用MissionPlanner校準(zhǔn)自駕儀加速度計(jì)的實(shí)驗(yàn)詳見(jiàn)6.4.1節(jié)。2.三軸加速度計(jì)評(píng)價(jià)加速度計(jì)的常見(jiàn)指標(biāo)包括量程、溫漂、零漂、非線性和噪聲等。量程是指這款傳感器測(cè)量值的加速度范圍，對(duì)于多旋翼來(lái)說(shuō)一般+16g就足夠使用;溫漂是關(guān)鍵參數(shù)指的是在不同溫度情況下，傳感器測(cè)量的變化，也就是說(shuō)在相同加速度的情況下不同溫度環(huán)境帶來(lái)的測(cè)量差異;非線性指的是在加速度計(jì)輸入與輸出偏離線性關(guān)系的系數(shù);噪聲指的是在加速度計(jì)輸入不變的情況下，輸出數(shù)據(jù)的波動(dòng)大小，輸出數(shù)據(jù)波動(dòng)越小，噪聲則越小。常見(jiàn)加速度參數(shù)對(duì)比如圖6.6所示。3.三軸角速率陀螺儀需要注意的是，三軸陀螺儀通常測(cè)量的是角速率，而不是角度。MEMS陀螺儀的原理是加速度計(jì)的擴(kuò)展，通過(guò)高頻電流使陀螺儀當(dāng)中的重塊進(jìn)行振蕩，當(dāng)傳感器振蕩的過(guò)程中，就會(huì)產(chǎn)生垂直于速度方向的科里奧利力，該力正比于角速度。MEMS陀螺儀通常包含兩個(gè)質(zhì)量塊，兩者運(yùn)動(dòng)速度的方向相反，大小相等。在旋轉(zhuǎn)過(guò)程中，它們會(huì)產(chǎn)生不同方向的力，因而產(chǎn)生相反的電容，該電容差正比于角速度。而加速度使兩個(gè)質(zhì)量塊所產(chǎn)生的電容變化相等，因此不會(huì)影響角速率測(cè)量。MEMS陀螺儀原理圖如圖6.7所示。3.三軸角速率陀螺儀陀螺儀的校準(zhǔn)通常是在靜止?fàn)顟B(tài)下完成，即角速率為零。此時(shí)靜置傳感器，保持采樣一段時(shí)間，在該時(shí)間段中選取多個(gè)采樣值，然后計(jì)算它們的平均值。若平均值不為零則需要減去它來(lái)完成校準(zhǔn)。評(píng)價(jià)陀螺儀的常見(jiàn)指標(biāo)同樣包括量程、溫漂、零漂、非線性和噪聲等。量程通常為±2000°/s就足夠使用。常見(jiàn)陀螺儀參數(shù)對(duì)比如圖6.8所示。由于三軸加速度計(jì)和三軸角速率陀螺儀信號(hào)能夠互相補(bǔ)償，因此，市場(chǎng)上大部分MEMS芯片都集成了這兩種傳感器于一體，以減小體積，降低成本。這是推算運(yùn)動(dòng)相關(guān)的核心傳感器，也可稱為6DOF傳感器，理論上單芯片就可支撐INS系統(tǒng)的運(yùn)算。4.三軸磁力計(jì)無(wú)人機(jī)使用的三軸磁力計(jì)一般用來(lái)探測(cè)地球磁場(chǎng)，進(jìn)而推算出飛行器的三維磁航向。磁力計(jì)測(cè)量值一般是利用鐵磁體的各向異性磁阻來(lái)進(jìn)行測(cè)量，該測(cè)量效果實(shí)時(shí)性較好，但需要充磁的過(guò)程和充磁器件，因此精度并不高。磁力計(jì)在飛行使用當(dāng)中用途比較單一主要用于測(cè)量磁場(chǎng)來(lái)推算地球磁極相對(duì)于傳感器的方向，因此也可以直接稱為“磁羅盤”。常見(jiàn)的一種磁力計(jì)(HMC5883L磁力計(jì)模塊)如圖6.9所示。磁力計(jì)的校準(zhǔn)過(guò)程相對(duì)比較復(fù)雜，需要分別繞三個(gè)軸進(jìn)行圓周運(yùn)動(dòng)。在實(shí)際校準(zhǔn)過(guò)程中大多為手動(dòng)操作，將傳感器各個(gè)軸豎直向下放置，繞豎直軸旋轉(zhuǎn)一周以取得各個(gè)角度測(cè)量的地磁場(chǎng)強(qiáng)度。因?yàn)樵谕坏攸c(diǎn)地磁場(chǎng)強(qiáng)度始終保持不變，所以用這一原理校準(zhǔn)磁力計(jì)。使用MissionPlanner校準(zhǔn)磁力計(jì)的實(shí)驗(yàn)詳見(jiàn)6.42節(jié)。4.三軸磁力計(jì)評(píng)價(jià)磁力計(jì)的常見(jiàn)指標(biāo)包括量程、溫漂、精度、線性度和噪聲等。由于地磁強(qiáng)度約為500至600毫高斯，磁力計(jì)的磁場(chǎng)量程為±8高斯(Gauss)即可。常見(jiàn)磁力計(jì)參數(shù)對(duì)比如圖6.10所示。磁力計(jì)在計(jì)算運(yùn)動(dòng)時(shí)，可以給6DOF傳感器提供一個(gè)絕對(duì)方位的參考，以此提高準(zhǔn)確度并減小漂移。因此，磁力計(jì)常與三軸陀螺儀和三軸加速度計(jì)整合為一個(gè)完整9DOF芯片。理論上這種單芯片就可以支撐AHRS航姿運(yùn)算系統(tǒng)。5.氣壓計(jì)氣壓計(jì)分為絕壓氣壓計(jì)和差壓氣壓計(jì)兩種，絕壓氣壓計(jì)測(cè)量的是絕對(duì)壓力，而差壓氣壓計(jì)測(cè)量的是兩個(gè)測(cè)量點(diǎn)之間的壓力差。因此，常用絕壓氣壓計(jì)來(lái)測(cè)量氣壓絕對(duì)數(shù)值，然后換算為國(guó)際標(biāo)準(zhǔn)氣壓海拔高度，而差壓氣壓計(jì)的兩個(gè)測(cè)量點(diǎn)，分別接通空速管的總壓與靜壓，通過(guò)壓力差換算空速。這兩種氣壓計(jì)也分別稱為“氣壓高度計(jì)”和“空速傳感器”，如圖6.11所示。5.氣壓計(jì)MEMS氣壓計(jì)的原理與其他工業(yè)氣壓計(jì)并無(wú)本質(zhì)區(qū)別。首先使用真空膜盒感受差壓或絕壓，然后通過(guò)形變驅(qū)動(dòng)周圍器件的電容發(fā)生變化。測(cè)量膜盒的形變先得到輸出再經(jīng)過(guò)內(nèi)置電路的放大，最后通過(guò)單片機(jī)的濾波和數(shù)字化而得到輸出。因?yàn)楝F(xiàn)實(shí)中相同位置的大氣壓在不斷變化，所以氣壓計(jì)的校準(zhǔn)情況比較特殊。一般清況下，都是在飛行器即將離地飛行的瞬間進(jìn)行自動(dòng)校準(zhǔn)，其方法依然是采集多個(gè)數(shù)值并取平均，把靜置啟動(dòng)時(shí)刻的當(dāng)前數(shù)值當(dāng)作相對(duì)零高度和相對(duì)零空速。而海拔氣壓高度則直接使用出廠校準(zhǔn)的數(shù)據(jù)進(jìn)行計(jì)算，無(wú)須使用校準(zhǔn)。5.氣壓計(jì)由于空速傳感器只在固定翼飛行器上使用，因此在多旋翼內(nèi)容當(dāng)中不再討論。評(píng)價(jià)氣壓計(jì)傳感器的常見(jiàn)指標(biāo)包括量程、溫漂、零漂、分辨率和噪聲等。因?yàn)槎嘈矶际窃诖髿鈱又酗w行，測(cè)量的是大氣壓的變化，所以量程在一個(gè)大氣壓附近即可，對(duì)于多旋翼來(lái)說(shuō)一般300~1100hPa即可滿足需求。常見(jiàn)氣壓高度計(jì)參數(shù)如圖6.12所示。6.超聲波測(cè)距儀超聲波是一種頻率超出人耳聽(tīng)覺(jué)頻率上限的聲波(頻率大于20kHz)。由于具有良好的方向性和強(qiáng)大的穿透力，超聲波廣泛應(yīng)用于測(cè)速、測(cè)距等方面。超聲波信號(hào)由超聲波傳感器發(fā)出，經(jīng)由障礙反射，被接收器所接收，并利用這個(gè)時(shí)間差計(jì)算出到障礙的距離?？紤]到不同溫度的影響，空氣中聲波的傳播速度不同，因此，測(cè)量結(jié)果需要根據(jù)溫度相應(yīng)地調(diào)整。超聲波測(cè)距儀存在一些不足:它的測(cè)量范圍小，而且柔軟的物體或者與傳感器成特定角度的物體可能導(dǎo)致反射的聲波較小，甚至導(dǎo)致測(cè)距儀接收不到反射波(如圖6.13所示)。超聲波測(cè)距儀的運(yùn)行相對(duì)穩(wěn)定，無(wú)須特別校準(zhǔn)，但對(duì)旋翼氣流比較敏感，測(cè)量穩(wěn)定性會(huì)受到影響，因此，在多旋翼上安裝時(shí)需要遠(yuǎn)離旋翼下方。需要注意的是，由于多旋翼的運(yùn)動(dòng)會(huì)導(dǎo)致機(jī)身傾斜，從而使超聲波測(cè)距儀指向的方向不垂直于地面，進(jìn)而影響測(cè)量高度的準(zhǔn)確性。因此，在使用超聲波測(cè)距時(shí)，需要全程根據(jù)飛行器姿態(tài)進(jìn)行幾何校正。7.雷達(dá)與上述超聲波測(cè)距儀原理類似的測(cè)距傳感器還有激光雷達(dá)和無(wú)線電雷達(dá)(如圖6.14所示)。激光雷達(dá)發(fā)射單束激光并通過(guò)測(cè)量光的反射時(shí)間得到單點(diǎn)距離，而無(wú)線電雷達(dá)則是通過(guò)發(fā)射高頻無(wú)線電波來(lái)實(shí)現(xiàn)測(cè)距。激光雷達(dá)優(yōu)點(diǎn)是對(duì)目標(biāo)介質(zhì)兼容性好(目標(biāo)反射性好)，缺點(diǎn)是波束窄，難以搜索和捕獲目標(biāo);雖然無(wú)線電雷達(dá)的也有著介質(zhì)兼容性好的優(yōu)點(diǎn)，但是設(shè)備體積較大，只能用于較大的飛行器。7.雷達(dá)360度激光雷達(dá)是一種二維激光測(cè)距儀，在旋轉(zhuǎn)過(guò)程中，不斷發(fā)射激光束來(lái)測(cè)距，輸出周邊障礙的二維點(diǎn)云。激光雷達(dá)通常用于測(cè)高、避障和實(shí)時(shí)生成周圍地圖。激光雷達(dá)測(cè)量原理如圖6.15所示，通過(guò)測(cè)量角度和測(cè)量距離可以計(jì)算出雷達(dá)距離地面的高度。進(jìn)一步，三維激光雷達(dá)可以得到更為細(xì)致的三維立體圖像，常用領(lǐng)域包括自動(dòng)駕駛感知、地形地貌測(cè)繪和文物精細(xì)重建等，如圖6.16所示。8.全球衛(wèi)星導(dǎo)航系統(tǒng)全球衛(wèi)星導(dǎo)航系統(tǒng)(GlobalNavigationSatelliteSystem，GNSS)是一種通過(guò)接收衛(wèi)星信號(hào)，并進(jìn)行幾何運(yùn)算，來(lái)確定GNSS接收機(jī)天線的地理位置的全球定位設(shè)備，一般至少需要四顆衛(wèi)星實(shí)現(xiàn)定位，GNSS系統(tǒng)原理圖如圖6.17所示。GNSS系統(tǒng)中的衛(wèi)星均帶有與其他衛(wèi)星保持同步的原子時(shí)鐘，而且這些原子時(shí)鐘每天都會(huì)根據(jù)地面真實(shí)時(shí)間進(jìn)行修正。同時(shí)，地面也會(huì)監(jiān)控衛(wèi)星的精確位置。當(dāng)衛(wèi)星實(shí)時(shí)廣播它們的當(dāng)前位置和時(shí)間時(shí)，接收機(jī)可以接收多組衛(wèi)星數(shù)據(jù)，然后利用冗余算法來(lái)消除誤差。8.全球衛(wèi)星導(dǎo)航系統(tǒng)該系統(tǒng)的測(cè)量用于位置控制。雖然GNSS系統(tǒng)無(wú)須校準(zhǔn)，但是需要等待足夠多的衛(wèi)星與接收機(jī)連接，進(jìn)入穩(wěn)定的定位狀態(tài)后才可以使用。通過(guò)地面站可以觀察多旋冀GNSS與衛(wèi)星通信的狀態(tài)，星數(shù)表示接收到的衛(wèi)星數(shù)量。在民用導(dǎo)航中，常用水平精度因子(HDOP)和垂直精度因子(VDOP)表示衛(wèi)星導(dǎo)航系統(tǒng)的定位精度，其數(shù)值越小說(shuō)明定位精度越高。常見(jiàn)的單點(diǎn)GNSS系統(tǒng)的定位狀態(tài)包括“nofix”——無(wú)法定位“2D——2D定位(輸出經(jīng)緯度)以及“3Dfx”——3D定位(輸出經(jīng)緯度和海拔)。目前全球有GPS、Glonass、北斗、伽利略等衛(wèi)星定位系統(tǒng)，還有一些區(qū)域性衛(wèi)星定位增強(qiáng)系統(tǒng)。另外，每種衛(wèi)星定位系統(tǒng)還會(huì)有多個(gè)頻率的信號(hào)，用于冗余校正誤差。對(duì)于只支持一種衛(wèi)星定位系統(tǒng)的接收機(jī)設(shè)備，通常直接使用衛(wèi)星定位系統(tǒng)名，支持美國(guó)的GPS系統(tǒng)就被稱為“GPS”，而支持中國(guó)北斗衛(wèi)星系統(tǒng)的接收機(jī)被稱為“北斗”;同時(shí)支持兩種衛(wèi)星系統(tǒng)的接收機(jī)被稱為雙模衛(wèi)星定位設(shè)備;支持三個(gè)系統(tǒng)以上多個(gè)頻率的接收機(jī)設(shè)備被稱為多系統(tǒng)多頻衛(wèi)星定位設(shè)備。8.全球衛(wèi)星導(dǎo)航系統(tǒng)GNSS的絕對(duì)精度一般是米級(jí)，其觀測(cè)主要收到以下因素影響:01衛(wèi)星相關(guān)誤差，包括軌道誤差和衛(wèi)星鐘差;02傳播誤差，包括電離層誤差、電流層誤差和多路徑誤差；03接收機(jī)誤差，包括接收機(jī)時(shí)鐘差和觀測(cè)誤差。8.全球衛(wèi)星導(dǎo)航系統(tǒng)從接收機(jī)到衛(wèi)星的距離可以由GNSS接收機(jī)接收到的測(cè)距碼獲得。由于存在衛(wèi)星相關(guān)誤差、傳播誤差，因此這個(gè)距離也稱為偽距。為提高GNSS的測(cè)量精度，差分定位技術(shù)將一臺(tái)接收機(jī)放置在固定的地面基準(zhǔn)參考站內(nèi)。由于基準(zhǔn)參考站本身的位置是實(shí)際已知的，因此，站內(nèi)的接收機(jī)通過(guò)GNSS系統(tǒng)得到自己的位置測(cè)量值后，將位置的測(cè)量值與實(shí)際值相減，就可以得到GNSS測(cè)出的定位與實(shí)際定位的誤差。之后，基準(zhǔn)參考站將該誤差實(shí)時(shí)發(fā)送出去，附近的移動(dòng)接收機(jī)就可以接收這個(gè)誤差來(lái)修正自己的定位。雖然差分技術(shù)可以消除GNSS公共誤差，但是接收機(jī)的內(nèi)部誤差不能被消除?；谳d波相位差分技術(shù)(RealTimeKinematic，RTK)采用相同的基礎(chǔ)理論，但是使用了衛(wèi)星信號(hào)載波相位作為測(cè)量信息。在RTK系統(tǒng)中，基準(zhǔn)站不斷地將觀測(cè)到的載波相位發(fā)送出去移動(dòng)接收機(jī)在接收基準(zhǔn)站信息后，比較其自身觀測(cè)的相位與基準(zhǔn)站接收的相位，實(shí)時(shí)計(jì)算出移動(dòng)接收機(jī)的位置。9.攝像機(jī)攝像機(jī)(又稱相機(jī)、攝像頭)在無(wú)人機(jī)當(dāng)中應(yīng)用很廣泛，可以進(jìn)行目標(biāo)識(shí)別和跟蹤，可以進(jìn)行目標(biāo)測(cè)量和避障，也可以進(jìn)行光流定位，還可以進(jìn)行視覺(jué)計(jì)里以輔助導(dǎo)航等。攝像機(jī)將三維場(chǎng)景投射到二維圖像平面，采集的圖像以標(biāo)準(zhǔn)電視信號(hào)的形式經(jīng)高速采集系統(tǒng)傳輸?shù)接?jì)算機(jī)中并進(jìn)行數(shù)字化，數(shù)字圖像在計(jì)算機(jī)中以二維數(shù)組存儲(chǔ)和處理。攝像機(jī)通過(guò)成像透射將三維場(chǎng)景投射到二維像平面的投影關(guān)系稱為攝像機(jī)成像模型，該模型能將三維空間中的點(diǎn)與圖像平面中的點(diǎn)對(duì)應(yīng)起來(lái)。9.攝像機(jī)三維場(chǎng)景中的直線投射到二維像平面上也是直線，但未標(biāo)定的攝像機(jī)對(duì)直線進(jìn)行拍攝后所得到的畫面未必是直線，這種現(xiàn)象稱為光學(xué)畸變。因此，攝像機(jī)標(biāo)定就是確定攝像機(jī)的內(nèi)部參數(shù)和外部參數(shù)，通過(guò)內(nèi)外參數(shù)就可以對(duì)圖像進(jìn)行修正，從而減少光學(xué)畸變的影響。如圖6.18所示，攝像機(jī)標(biāo)定通常使用標(biāo)定板標(biāo)定板上有排列整齊的特征角點(diǎn)，后續(xù)操作就是求解合適的內(nèi)外參數(shù)使得二維圖像平面上的特征角點(diǎn)也是排列整齊的。攝像機(jī)外部參數(shù)標(biāo)定包括攝像機(jī)之間的位置和姿態(tài)，以及與飛行器中心的相對(duì)位姿。多旋翼可以同時(shí)裝配多個(gè)攝像機(jī)，當(dāng)只有一個(gè)攝像機(jī)工作時(shí)稱為單目視覺(jué)，兩個(gè)攝像機(jī)工作時(shí)稱為雙目視覺(jué)，三個(gè)及三個(gè)以上攝像機(jī)工作則稱為多目視覺(jué)。單目視覺(jué)只能獲得二維圖像信息，而雙目視覺(jué)能夠得到場(chǎng)景的深度信息，因?yàn)閮蓚€(gè)攝像機(jī)拍攝的角度不同，所以拍攝到的同一物體在二維圖像中的坐標(biāo)也就不同，再根據(jù)三角測(cè)距原理就可以得到物體的深度信息。攝像機(jī)越多，場(chǎng)景深度信息的誤差也就越小，但計(jì)算量會(huì)大大增加。9.攝像機(jī)通過(guò)攝像機(jī)、計(jì)算芯片和算法的高度集成，可以進(jìn)一步得到智能相機(jī)，并把很多需要解決的工作進(jìn)行模塊化封裝，以及接口標(biāo)準(zhǔn)化。例如，一體化深度相機(jī)越來(lái)越普遍它無(wú)須用戶自行設(shè)計(jì)算法，便可以輕松獲取立體圖像。投影式深度相機(jī)由常規(guī)相機(jī)、紅外線激光投影儀和紅外相機(jī)組成。紅外線投影儀投影紅外光，進(jìn)而通過(guò)分析紅外相機(jī)接收的反射光得到深度信息。也有僅采用雙目方案的深度相機(jī)，這對(duì)算法要求很高，目前性能與前面所述投影式方案有些差距。近年來(lái)，智能相機(jī)可以用于同步定位與建圖功能，這樣能夠進(jìn)行室內(nèi)外自主定位如圖6.19所示，英特爾RealSense深度攝像頭D435i包含一個(gè)RGB相機(jī)、兩個(gè)紅外相機(jī)、一個(gè)紅外點(diǎn)陣投射器和一個(gè)IMU，其中，RGB相機(jī)用于采集彩色圖片，紅外點(diǎn)陣投射器用于發(fā)射紅外線，紅外相機(jī)用于接收紅外線。英特爾RealSense追蹤攝像頭T265包含兩個(gè)魚眼鏡頭傳感器、一個(gè)IMU和一個(gè)英特爾MovidiusMyriad2VPU，可直接輸出三維坐標(biāo)，滿足機(jī)器人、無(wú)人機(jī)和增強(qiáng)現(xiàn)實(shí)快速原型制作需求。9.攝像機(jī)除此之外，還有一類光流傳感器，如圖6.20所示。光流是視野中由于攝像機(jī)和場(chǎng)景的相對(duì)運(yùn)動(dòng)導(dǎo)致觀測(cè)目標(biāo)、表面或邊緣的表觀運(yùn)動(dòng)模式。通常比較同一個(gè)實(shí)際目標(biāo)點(diǎn)在兩幀圖像中對(duì)應(yīng)的像素點(diǎn)，根據(jù)該像素點(diǎn)在兩幀圖像中的位置之差和兩幀圖像的時(shí)間差，來(lái)計(jì)算出像素點(diǎn)在圖像中的運(yùn)動(dòng)速度，這需要假設(shè)目標(biāo)點(diǎn)的灰度不變。根據(jù)所有或部分關(guān)鍵像素點(diǎn)的速度，可以推導(dǎo)出多旋翼的移動(dòng)速度，即通過(guò)圖像序列來(lái)對(duì)瞬時(shí)圖像速度或離散圖像位移進(jìn)行估計(jì)。02狀態(tài)估計(jì)1.濾波原理傳感器測(cè)量出的數(shù)據(jù)往往具有一定的偏差，并不能直接使用。例如，如果直接使用陀螺儀測(cè)量的角速度來(lái)計(jì)算多旋翼的姿態(tài)角，那么角速度的誤差會(huì)不斷積累，導(dǎo)致測(cè)算出的姿態(tài)角與真實(shí)值之間的誤差很大。因此，需要使用濾波器來(lái)處理傳感器數(shù)據(jù)，以得到較為精確的測(cè)量值。濾波是對(duì)信號(hào)中的噪聲進(jìn)行消除或抑制。以低通濾波器在加速度計(jì)測(cè)量時(shí)輸出的電壓為例，低通濾波器允許頻率低的信號(hào)通過(guò)，而頻率高的信號(hào)則不能通過(guò)。電機(jī)和螺旋槳的振動(dòng)頻率相對(duì)較高，會(huì)導(dǎo)致加速度計(jì)發(fā)生信號(hào)畸變，即測(cè)量出來(lái)的電壓振動(dòng)頻率高，幅度較大，而實(shí)際的多旋翼姿態(tài)變化的振動(dòng)頻率較低，其波動(dòng)較小且變化連續(xù)，理論上加速度計(jì)輸出電壓的振動(dòng)頻率低且幅度小。1.濾波原理因此，為了抑制信號(hào)畸變的影響，將加速度計(jì)上一時(shí)刻的輸出電壓視為式(6.1)中的“上一時(shí)刻電壓”，當(dāng)前時(shí)刻的輸出電壓為式(6.1)中的“當(dāng)前電壓測(cè)量值”。顯然直接使用當(dāng)前電壓測(cè)量值代替上一時(shí)刻電壓進(jìn)行迭代是不行的。因此，考慮給兩者加上權(quán)值。例如給上一時(shí)刻電壓95%的權(quán)值，當(dāng)前電壓測(cè)量值5%的權(quán)值，則加速度計(jì)輸出電壓的更新公式如式(6.1)所示當(dāng)前時(shí)刻電壓=0.95x上一時(shí)刻電壓+0.05x當(dāng)前電壓測(cè)量值其中，當(dāng)進(jìn)行到下一時(shí)刻，將“當(dāng)前時(shí)刻電壓”作為“上一時(shí)刻電壓”進(jìn)行迭代。設(shè)置權(quán)值后，如果某一個(gè)時(shí)刻產(chǎn)生畸變信號(hào)，它只有5%被接受，那么當(dāng)前時(shí)刻電壓不會(huì)受到太大影響，這樣便能極大地降低畸變信號(hào)的影響。正常情況下，多旋翼姿態(tài)發(fā)生變化后，當(dāng)前電壓測(cè)量值會(huì)不斷地通過(guò)權(quán)重來(lái)影響當(dāng)前時(shí)刻電壓，使其發(fā)生變化若增加上一時(shí)刻電壓的權(quán)重，則可以降低信號(hào)畸變的影響，但會(huì)使得計(jì)算出的電壓延遲增大。1.濾波原理另外一種常用的濾波器為卡爾曼濾波器(KalmanFilter，KF)?？柭鼮V波也稱為線性二次型估計(jì)，能夠從一系列不完全且包含噪聲不確定性的觀測(cè)量中估計(jì)系統(tǒng)的未知狀態(tài)，其估計(jì)精度往往比單純的基于單一觀測(cè)量的方法更高?？柭鼮V波只適用于線性系統(tǒng)，該系統(tǒng)的輸入與輸出成比例(即齊次性)，且多個(gè)輸入相加之后作用在系統(tǒng)上所得到的輸出等于分別作用在系統(tǒng)上所得輸出之和(即疊加性)。而多旋翼的GNSS/INS狀態(tài)方程和量測(cè)方程是非線性的，因此，通常使用擴(kuò)展卡爾曼濾波器(ExtendedKalmanFilter，EKF)，將非線性問(wèn)題轉(zhuǎn)化為線性問(wèn)題O,1?？柭鼮V波器與前面經(jīng)驗(yàn)數(shù)值公式中不同之處在于可以處理多維狀態(tài)估計(jì)，此外，其權(quán)值可以根據(jù)系統(tǒng)參數(shù)在線自動(dòng)調(diào)整而不是一個(gè)固定的權(quán)值。自駕儀濾波器調(diào)參的實(shí)驗(yàn)詳見(jiàn)6.4.3節(jié)。2.姿態(tài)估計(jì)姿態(tài)估計(jì)是對(duì)姿態(tài)角進(jìn)行估計(jì)，通常利用三軸加速度計(jì)、三軸陀螺儀和三軸磁力計(jì)的數(shù)據(jù)，通過(guò)線性互補(bǔ)濾波器、非線性互補(bǔ)濾波器或卡爾曼濾波器處理來(lái)得到姿態(tài)角估計(jì)值。三軸加速度計(jì)固連在多旋翼機(jī)體，其三軸與機(jī)體坐標(biāo)系一致。因此，低頻的俯仰角θm和滾轉(zhuǎn)角Φm觀測(cè)量可以由加速度計(jì)測(cè)量值bam近似得到，即其中，bam=[axbm

aybm

azbm]加度計(jì)測(cè)量值在機(jī)體的Obxb、Obyb和Obzb軸的分量。3.位置估計(jì)01基于GNSS的位置估計(jì)基于GNSS的位置估計(jì)常使用IMU、GNSS接收機(jī)和氣壓計(jì)?？柭鼮V波器結(jié)合多旋翼運(yùn)動(dòng)學(xué)模型，融合不同傳感器的信息來(lái)估計(jì)位置。GNSS接收機(jī)能夠提供二維位置信息，氣壓計(jì)能夠提供高度信息，則傳感器觀測(cè)模型為3.位置估計(jì)02基于SLAM的位置估計(jì)SLAM(SimultaneousLocalizationAndMapping)稱為同步定位與建圖技術(shù)。SLAM問(wèn)題可以描述為機(jī)器人在估計(jì)自身位置的同時(shí)建造未知環(huán)境的增量式地圖，實(shí)現(xiàn)自主定位和導(dǎo)航。相關(guān)文獻(xiàn)可以參考[116]和117。SLAM技術(shù)可以基于不同類型的傳感器包括距離傳感器，如超聲波測(cè)距儀和激光掃描測(cè)距儀，也可以基于方位傳感器，如攝像機(jī)等;也有一些傳感器融合的方案，如融合距離傳感器和三維攝像機(jī)等方位傳感器。表6.1和表6.2給出了一些SLAM系統(tǒng)和相關(guān)數(shù)據(jù)庫(kù)，以幫助讀者更好地理解。下面以基于激光的SLAM和基于單目視覺(jué)的SLAM為例展開(kāi)介紹。3.位置估計(jì)02基于SLAM的位置估計(jì)激光SLAM是利用激光測(cè)距來(lái)進(jìn)行定位與建圖的。二維SLAM算法要求多旋翼定高飛行，特別是在走廊類型的空間中飛行，即要求空間的水平截面相同。如果多旋翼在復(fù)雜的三維空間飛行，那么需要采用三維SLAM算法。隨著飛行空間增大，需要重建地圖，存儲(chǔ)的地圖坐標(biāo)會(huì)更多，計(jì)算量變得更復(fù)雜。相比全自主飛行，定點(diǎn)懸停飛行的難度要小得多，因?yàn)榇鎯?chǔ)的地圖坐標(biāo)較少。谷歌提供了名為Cartographer的SLAM算法可以用于二維或三維的定位與建圖，圖6.24顯示了官方例程的運(yùn)行效果。3.位置估計(jì)02基于SLAM的位置估計(jì)視覺(jué)SLAM因相機(jī)種類的不同可以分為三類:單目相機(jī)、雙目相機(jī)和RGB-D相機(jī)。其中，單目相機(jī)有一個(gè)攝像頭，雙目相機(jī)有兩個(gè)攝像頭，RGB-D相機(jī)可以額外地測(cè)出圖像中每個(gè)像素與相機(jī)之間的距離。視覺(jué)SLAM包括兩個(gè)核心步驟:定位和建圖。在定位過(guò)程中，根據(jù)場(chǎng)景的結(jié)構(gòu)信息求取攝像機(jī)的位置與姿態(tài):而在建圖過(guò)程中，根據(jù)求取的攝像機(jī)位置與姿態(tài)重建場(chǎng)景的三維結(jié)構(gòu)。跟蹤與建圖任務(wù)交替進(jìn)行，跟蹤依賴于建圖得到的場(chǎng)景結(jié)構(gòu)信息，建圖反過(guò)來(lái)依賴于跟蹤求取的運(yùn)動(dòng)信息。3.位置估計(jì)02基于SLAM的位置估計(jì)一種常用的地圖形式為八叉樹(shù)地圖。它把三維空間分成多個(gè)小方塊，每個(gè)小方塊存儲(chǔ)了該方塊在實(shí)際空間中是否存在物體的信息。如果需要更高的地圖精度，那么可以將方塊拆分成等大的八個(gè)小方塊，再分別表示各空間里面是否存在物體。當(dāng)八個(gè)小方塊中都包含物體或都不包含物體時(shí)，則不需要將原先的方塊進(jìn)行拆分，以此節(jié)省存儲(chǔ)空間。圖6.25為視覺(jué)SLAM過(guò)程中建立的八叉樹(shù)地圖，隨著時(shí)間增加，地圖越建越大。03姿態(tài)控制和調(diào)試1.動(dòng)態(tài)系統(tǒng)的基本描述多旋翼底層飛行控制的閉環(huán)結(jié)構(gòu)如圖6.26所示。多旋翼是一個(gè)欠驅(qū)動(dòng)系統(tǒng)，即獨(dú)立控制量個(gè)數(shù)小于系統(tǒng)自由度個(gè)數(shù)的系統(tǒng)，它雖然有六個(gè)自由度(三個(gè)自由度位置、三個(gè)自由度姿態(tài))，但是只有四個(gè)獨(dú)立輸入(四個(gè)電機(jī)轉(zhuǎn)速)。因此，多旋翼只能跟蹤四個(gè)期望指令(三個(gè)自由度位置、偏航角)，剩余的變量由期望指令確定。1.動(dòng)態(tài)系統(tǒng)的基本描述多旋翼采用內(nèi)外環(huán)控制。外環(huán)控制器為位置控制器，內(nèi)環(huán)控制器為姿態(tài)控制器，即先控制好多旋翼的姿態(tài)，再控制多旋翼的位置，動(dòng)態(tài)系統(tǒng)的運(yùn)行順序如圖6.26所示。先給出期望位置和期望偏航角，再通過(guò)期望位置與實(shí)際位置的偏差可以計(jì)算出期望俯仰角、期望滾轉(zhuǎn)角和期望拉力:之后，根據(jù)期望姿態(tài)角和實(shí)際姿態(tài)角的偏差計(jì)算出期望力矩，然后結(jié)合期望拉力，通過(guò)控制分配器計(jì)算出期望螺旋槳轉(zhuǎn)速;最后，利用期望螺旋槳轉(zhuǎn)速計(jì)算出期望油門指令。計(jì)算完所有的期望值之后，多旋翼給電機(jī)控制器輸出期望的控制信號(hào)，從而完成多旋翼的控制。1.動(dòng)態(tài)系統(tǒng)的基本描述關(guān)于控制分配器，如果多旋翼是四旋翼，那么力和力矩四個(gè)獨(dú)立輸入將換算成四個(gè)螺旋槳的控制，通常期望這些控制作用到真實(shí)的四旋翼也能產(chǎn)生同樣的力和力矩。如果多旋翼是十八旋翼，那么也只有力和力矩四個(gè)獨(dú)立輸入，并將其換算成十八個(gè)螺旋槳的控制。通常期望這些控制作用到真實(shí)的十八個(gè)旋翼也能產(chǎn)生同樣的力和力矩。這樣一來(lái)，控制器設(shè)計(jì)與多旋翼的旋翼個(gè)數(shù)及安裝位置就解耦了。以下PID原理和調(diào)試步驟僅針對(duì)位置控制器和姿態(tài)控制器，通過(guò)控制器的設(shè)計(jì)，根據(jù)實(shí)時(shí)反饋，得到動(dòng)態(tài)變化的力和力矩的期望。2.PID原理和調(diào)試步驟01PID控制原理PID算法是比例P(Proportion)、積分I(Integration)、微分D(Differentiation)組成的一種控制律算法。飛行器在空中飛行一共需要控制六個(gè)自由度，分別是高度、經(jīng)度和緯度三個(gè)平移自由度，以及偏航、俯仰和滾轉(zhuǎn)三個(gè)旋轉(zhuǎn)自由度。那么，PID控制就是將這六個(gè)自由度變量偏差的比例、積分和微分之和作為控制器的輸入，計(jì)算輸出值的一種控制方法。常規(guī)PID控制系統(tǒng)結(jié)構(gòu)框圖如圖6.27所示。2.PID原理和調(diào)試步驟01PID控制原理如圖6.27所示，“p”參數(shù)可以理解為糾正偏差的力度，P參數(shù)越大，系統(tǒng)動(dòng)作越快;“I”參數(shù)可以理解為要達(dá)到的精度，I參數(shù)越大態(tài)跟蹤越精確;“D”參數(shù)可以防止力度太大造成的超調(diào)效果，可以理解為快要到達(dá)指定目標(biāo)時(shí)提前減速的效果。PID算法已經(jīng)有很長(zhǎng)的歷史，是目前應(yīng)用最廣泛的控制律算法。其因結(jié)構(gòu)簡(jiǎn)單、穩(wěn)定性好、工作可靠、調(diào)節(jié)方便而成為工業(yè)控制的主要技術(shù)之一。尤其是當(dāng)被控對(duì)象的結(jié)構(gòu)和參數(shù)不能被完全掌握，或得不到精確的數(shù)學(xué)模型時(shí)，系統(tǒng)控制器的結(jié)構(gòu)和參數(shù)必須依靠經(jīng)驗(yàn)和現(xiàn)場(chǎng)調(diào)試來(lái)確定。這時(shí)應(yīng)用PID控制技術(shù)最為方便，多旋翼就是這樣的典型。由于四旋翼種類不同，布局和重量也將不同，因此很難找到一個(gè)通用的數(shù)學(xué)模型所以在控制多旋翼的算法中，PID控制算法是目前使用次數(shù)最多的。2.PID原理和調(diào)試步驟02PID調(diào)試步驟PID調(diào)試是對(duì)PID控制器的比例系數(shù)、積分系數(shù)和微分系數(shù)參數(shù)進(jìn)行調(diào)整，專業(yè)術(shù)語(yǔ)稱為整定，又稱為調(diào)參。只有合適的系數(shù)才能充分發(fā)揮控制器的性能，使被控對(duì)象得到精確的控制。而四旋翼的PID調(diào)參方法一般是按照臨界比例度法的思路，結(jié)合試湊和經(jīng)驗(yàn)進(jìn)行邊飛邊調(diào)。常見(jiàn)的飛控調(diào)參界面如圖6.28所示。2.PID原理和調(diào)試步驟02PID調(diào)試步驟以姿態(tài)控制器調(diào)參為例，分為內(nèi)環(huán)和外環(huán)。內(nèi)環(huán)調(diào)參目標(biāo)是速率(對(duì)于姿態(tài)來(lái)說(shuō)一般指角速率)，其相關(guān)的是飛行器的動(dòng)態(tài)特性是否靈活，是否快速跟蹤給定的姿態(tài)角速率指令，俗稱“跟手”:而外環(huán)為姿態(tài)環(huán)(自穩(wěn))，調(diào)參目標(biāo)是角度，外環(huán)相關(guān)的更多是飛行器的靜態(tài)特性，如是否穩(wěn)定及抗風(fēng)性如何。需要注意的是，要完整地進(jìn)行四旋翼的試湊調(diào)參，需要具備較高的飛行操作能力，至少能夠在模擬器當(dāng)中完成普通四旋翼增穩(wěn)模式下的懸停飛行(見(jiàn)8