旋翼無人機(jī)的建模、規(guī)劃與控制目錄TOC\h\h第1章位置、姿態(tài)描述與坐標(biāo)系變換\h1.1概述\h1.1.1右手定則\h1.1.2地球表面慣性坐標(biāo)系與機(jī)體坐標(biāo)系\h1.1.3歐拉角\h1.2位置、姿態(tài)\h1.2.1位置描述（移動）\h1.2.2姿態(tài)描述（旋轉(zhuǎn)）\h1.2.3坐標(biāo)系繞坐標(biāo)軸轉(zhuǎn)動\h1.3坐標(biāo)系變換\h1.3.1旋轉(zhuǎn)矩陣的用法\h1.3.2連續(xù)轉(zhuǎn)動\h1.3.3齊次變換矩陣\h1.3.4剛體狀態(tài)的表達(dá)\h1.4姿態(tài)的其他描述方法\h1.4.1X-Y-Z固定角坐標(biāo)系\h1.4.2等效軸角坐標(biāo)系表示法\h1.4.3歐拉參數(shù)法\h第2章旋翼無人機(jī)的建模分析\h2.1旋翼無人機(jī)系統(tǒng)的坐標(biāo)系介紹\h2.2旋翼無人機(jī)的運(yùn)動分析\h2.2.1平移運(yùn)動分析\h2.2.2旋轉(zhuǎn)運(yùn)動分析\h2.3旋翼的陀螺效應(yīng)\h2.4旋翼的空氣動力學(xué)效應(yīng)\h2.5空氣阻力\h2.6旋翼無人機(jī)模型的建立\h2.6.1受力在機(jī)體系和導(dǎo)航系之間的轉(zhuǎn)換\h2.6.2歐拉角速度與機(jī)體角速度的轉(zhuǎn)換\h2.6.3旋翼無人機(jī)的動力學(xué)模型\h第3章旋翼無人機(jī)的運(yùn)動信息估計(jì)\h3.1旋翼無人機(jī)系統(tǒng)中傳感器簡介\h3.1.1九軸慣性測量單元\h3.1.2GPS\h3.1.3光流\h3.1.4氣壓計(jì)\h3.2系統(tǒng)可觀性分析\h3.3姿態(tài)估計(jì)\h3.3.1互補(bǔ)濾波\h3.3.2梯度下降法\h3.3.3卡爾曼濾波\h3.4位置估計(jì)\h3.4.1加速度計(jì)積分\h3.4.2組合導(dǎo)航（GPS與IMU）\h3.4.3光流（室內(nèi)）定位\h第4章旋翼無人機(jī)視覺定位方法及系統(tǒng)\h4.1視覺定位方法簡介\h4.2相機(jī)標(biāo)定\h4.3Kinect外部視覺定位\h4.4單目視覺系統(tǒng)\h4.5雙目視覺系統(tǒng)\h4.5.1雙目標(biāo)定\h4.5.2雙目定位\h第5章旋翼無人機(jī)的路徑規(guī)劃\h5.1旋翼無人機(jī)航跡規(guī)劃的基本原理\h5.1.1旋翼無人機(jī)航跡規(guī)劃的定義\h5.1.2旋翼無人機(jī)航跡規(guī)劃系統(tǒng)\h5.1.3旋翼無人機(jī)航跡規(guī)劃的應(yīng)用原理\h5.2旋翼無人機(jī)航跡規(guī)劃方法綜述\h5.2.1環(huán)境建模方法\h5.2.2航跡規(guī)劃搜索算法\h5.3SLAM算法\h5.3.1SLAM的一般過程\h5.3.2SLAM問題的一般模型\h5.3.3基于EKF的SLAM研究\h5.4A*算法\h5.4.1A*算法搜索步驟\h5.4.2稀疏A*算法\h5.5人工勢場法\h5.5.1基本人工勢場法的路徑規(guī)劃\h5.5.2三維環(huán)境下使路徑平滑的人工勢場法\h5.5.3基于人工勢場法的路徑規(guī)劃仿真\h5.6生物啟發(fā)神經(jīng)動力學(xué)模型算法\h5.7航跡規(guī)劃的融合算法\h5.7.1稀疏A*與神經(jīng)動力學(xué)融合算法\h5.7.2A*與人工勢場融合算法\h第6章旋翼無人機(jī)控制器的設(shè)計(jì)\h6.1旋翼無人機(jī)控制框架\h6.1.1位置控制\h6.1.2姿態(tài)控制\h6.1.3控制分配\h6.1.4電機(jī)控制\h6.2常用的控制算法及控制器的實(shí)現(xiàn)\h6.2.1PID控制\h6.2.2反步法控制\h6.2.3滑模變結(jié)構(gòu)控制\h6.2.4神經(jīng)網(wǎng)絡(luò)控制\h第7章機(jī)載操作臂\h7.1機(jī)載操作臂簡介\h7.1.1背景\h7.1.2機(jī)載操作臂的機(jī)構(gòu)與控制\h7.2機(jī)載操作臂運(yùn)動學(xué)\h7.2.1連桿坐標(biāo)系的描述和規(guī)定\h7.2.2關(guān)節(jié)空間和笛卡爾空間\h7.2.3機(jī)載操作臂運(yùn)動學(xué)\h7.2.4機(jī)載操作臂逆運(yùn)動學(xué)\h7.3速度和靜力\h7.3.1剛體的線速度和角速度\h7.3.2連桿間的速度傳遞\h7.3.3雅可比矩陣\h7.3.4作用在操作臂上的靜力\h7.3.5速度和靜力的笛卡爾變換\h7.4機(jī)載操作臂動力學(xué)\h7.4.1加速度\h7.4.2牛頓-歐拉迭代動力學(xué)方程\h7.4.3迭代形式與封閉形式的動力學(xué)方程\h7.4.4機(jī)載操作臂動力學(xué)的拉格朗日公式\h7.5機(jī)載操作臂的末端軌跡生成\h7.5.1關(guān)節(jié)空間規(guī)劃方法\h7.5.2笛卡爾空間規(guī)劃方法\h7.5.3路徑的實(shí)時生成\h7.5.4無碰撞路徑規(guī)劃\h7.6機(jī)載操作臂的控制\h7.6.1單關(guān)節(jié)的建模和控制\h7.6.2多輸入多輸出控制系統(tǒng)\h7.6.3李雅普諾夫穩(wěn)定性分析\h7.6.4基于笛卡爾空間的控制系統(tǒng)\h第8章帶臂旋翼無人機(jī)\h8.1帶臂旋翼無人機(jī)簡介\h8.1.1概述\h8.1.2帶臂旋翼無人機(jī)的結(jié)構(gòu)\h8.1.3帶臂旋翼無人機(jī)的特點(diǎn)\h8.2帶臂旋翼無人機(jī)的聯(lián)合建模\h8.2.1帶臂旋翼無人機(jī)的運(yùn)動學(xué)模型\h8.2.2帶臂旋翼無人機(jī)的動力學(xué)模型\h8.2.3帶臂旋翼無人機(jī)的協(xié)調(diào)控制\h8.3帶臂旋翼無人機(jī)的視覺伺服控制\h8.3.1視覺伺服控制\h8.3.2基于位置的視覺伺服控制\h8.3.3基于圖像的視覺伺服控制\h8.3.4基于視覺伺服控制的帶臂旋翼無人機(jī)\h8.4帶臂旋翼無人機(jī)的應(yīng)用第1章位置、姿態(tài)描述與坐標(biāo)系變換1.1概述1.1.1右手定則右手的拇指指向x軸的正方向，食指指向y軸的正方向，中指所指示的方向即是z軸的正方向。進(jìn)一步，如圖1.1所示，要確定軸的正旋轉(zhuǎn)方向，用右手的大拇指指向軸的正方向，彎曲手指，那么彎曲手指所指示的方向即是軸的正旋轉(zhuǎn)方向。本章采用的坐標(biāo)系和后面定義的角度正方向都是沿用右手定則。圖1.1右手定則下的坐標(biāo)軸和旋轉(zhuǎn)正方向1.1.2地球表面慣性坐標(biāo)系與機(jī)體坐標(biāo)系地球表面慣性坐標(biāo)系（簡稱慣性坐標(biāo)系）用于研究多旋翼飛行器（多旋翼飛行器指四旋翼飛行器以上的飛行器，本文主要討論四旋翼）相對于地面的運(yùn)動狀態(tài)，確定機(jī)體的空間位置坐標(biāo)（見圖1.2）。它忽略了地球曲率，即假設(shè)地球表面是一個平面。在地面上選一點(diǎn)作為多旋翼飛行器起飛位置，z軸垂直于地面向下，并使x軸或y軸在水平面內(nèi)指向某一方向，則慣性坐標(biāo)系確定。然后，按右手定則確定x軸、y軸。機(jī)體坐標(biāo)系，其原點(diǎn)取在多旋翼飛行器的重心上，坐標(biāo)系與多旋翼飛行器固連（見圖1.2）。z軸在多旋翼飛行器對稱平面內(nèi)指向機(jī)頭（機(jī)頭方向與多旋翼飛行器十字形或X字形相關(guān)）。z軸在飛行器對稱平面內(nèi)，垂直軸向下。然后，按右手定則確定x軸，y軸。其中定義三個單位向量：圖1.2地球表面慣性坐標(biāo)系與機(jī)體坐標(biāo)系在地球表面慣性坐標(biāo)系中，沿著軸xe，ye，ze（右下標(biāo)e表示earth）的單位向量可以表示為｛e1，e2，e3｝。在機(jī)體坐標(biāo)系下，沿著xb，yb，zb（右下標(biāo)b表示body）軸的單位向量滿足bb1＝e1，bb2＝e2，bb3＝e3。（左上標(biāo)b表示向量在機(jī)體坐標(biāo)系）。在地球表面慣性坐標(biāo)系中，沿著軸xb，yb，zb軸的單位向量表示為｛eb1，eb2，eb3｝（左上標(biāo)e表示向量在地球表面慣性坐標(biāo)系）。1.1.3歐拉角歐拉角的定義（見圖1.3）：圖1.3歐拉角直觀表示示意圖機(jī)體坐標(biāo)系與地面慣性坐標(biāo)系之間的夾角就是飛行器的姿態(tài)角，又稱歐拉角（見圖1.4）。俯仰角θ：機(jī)體軸與地平面（水平面）之間的夾角，飛行器抬頭為正。偏航角（方位角）ψ：機(jī)體軸在水平面上的投影與地軸之間的夾角，以機(jī)頭左偏為正。滾轉(zhuǎn)角（傾斜角）φ：飛行器對稱面繞機(jī)體軸轉(zhuǎn)過的角度，右滾為正（見圖1.5）。圖1.4飛機(jī)歐拉角示意圖圖1.5正的俯仰角θ，負(fù)的滾轉(zhuǎn)角φ，正的偏航角Ψ1.2位置、姿態(tài)1.2.1位置描述（移動）概念：位置描述指以向量P來描述｛B｝的原點(diǎn)相對于｛W｝的狀態(tài)。例如圖1.6用向量P描述坐標(biāo)系｛B｝原相對坐標(biāo)系｛W｝其中｛W｝為世界坐標(biāo)系，｛B｝為機(jī)體坐標(biāo)系。兩個空間坐標(biāo)系的位置關(guān)系：1.2.2姿態(tài)描述（旋轉(zhuǎn)）概念：姿態(tài)描述表示兩個空間坐標(biāo)系坐標(biāo)軸的方向關(guān)系（見圖1.7）。圖1.7坐標(biāo)系姿態(tài)描述圖1.8姿態(tài)的求解解：（見圖1.8）因此，｛B｝相對于｛A｝的姿態(tài)：旋轉(zhuǎn)矩陣的性質(zhì)（見圖1.9）：圖1.9旋轉(zhuǎn)矩陣性質(zhì)正交矩陣的性質(zhì)：AAT＝ATA＝I。因此旋轉(zhuǎn)矩陣為3×3的正交矩陣，同樣旋轉(zhuǎn)矩陣R滿足正交矩陣的性質(zhì)。3）坐標(biāo)系矩陣R有9個數(shù)字，但上列兩個條件置入了6個限制條件，所以R只有3個自由度，與空間中轉(zhuǎn)動具有3個自由度相符。1.2.3坐標(biāo)系繞坐標(biāo)軸轉(zhuǎn)動其中θ為旋轉(zhuǎn)角度。圖1.10坐標(biāo)軸旋轉(zhuǎn)1.3坐標(biāo)系變換1.3.1旋轉(zhuǎn)矩陣的用法旋轉(zhuǎn)矩陣R（見圖1.11）有三種用法（見圖1.12）。1）描述一個坐標(biāo)系相對于另一個坐標(biāo)系的姿態(tài)。圖1.11旋轉(zhuǎn)矩陣2）對于一個點(diǎn)P，由其在坐標(biāo)系｛B｝中的表達(dá)轉(zhuǎn)換到坐標(biāo)系｛A｝中的表達(dá)。其中坐標(biāo)系｛A｝與坐標(biāo)系｛B｝僅有相對轉(zhuǎn)動。AP＝（見圖1.12）推導(dǎo)證明：在初始坐標(biāo)系｛B｝中，點(diǎn)P的坐標(biāo)：在新坐標(biāo)系｛A｝中，點(diǎn)P的坐標(biāo)：圖1.12｛B｝坐標(biāo)系轉(zhuǎn)換到｛A｝坐標(biāo)結(jié)果如圖1.13所示。3）點(diǎn)P在繞著坐標(biāo)系的某一坐標(biāo)軸旋轉(zhuǎn)，得到新的點(diǎn)P′（見圖1.14）。AP′＝R（θ）AP圖1.13上視圖驗(yàn)證圖1.14點(diǎn)P在同一坐標(biāo)中轉(zhuǎn)動圖1.15AP求解1.3.2連續(xù)轉(zhuǎn)動空間中的旋轉(zhuǎn)矩陣有3個自由度（見圖1.16），要表達(dá)一般的旋轉(zhuǎn)矩陣，可拆解成3次旋轉(zhuǎn)角度，以對應(yīng)到空間中的3個自由度。Z-Y-X歐拉角：圖1.16旋轉(zhuǎn)矩陣3個自由度1）將坐標(biāo)系｛A｝與坐標(biāo)系｛B｝的原點(diǎn)重合。2）將坐標(biāo)系｛A｝繞著軸旋轉(zhuǎn)α角度，并把新的坐標(biāo)系記為｛B′｝。3）將坐標(biāo)系｛B′｝繞著軸旋轉(zhuǎn)β角度，并把新的坐標(biāo)系記為｛B″｝。4）將坐標(biāo)系｛B″｝繞著軸旋轉(zhuǎn)γ角度，新的坐標(biāo)系記為｛B?｝，｛B?｝與｛B｝重合。如果把這些旋轉(zhuǎn)看成是坐標(biāo)系的描述，就可寫出：根據(jù)Z-Y-X歐拉角的定義，每個旋轉(zhuǎn)矩陣都是繞著自身坐標(biāo)系旋轉(zhuǎn)的：例1-4：以歐拉角方法旋轉(zhuǎn)，先對X軸旋轉(zhuǎn)60°，后對Y軸旋轉(zhuǎn)30°，和先對Y軸旋轉(zhuǎn)30°，后對X軸旋轉(zhuǎn)60°各自的相同么？解：①先對X軸旋轉(zhuǎn)60°，后對Y軸旋轉(zhuǎn)30°：②先對Y軸旋轉(zhuǎn)30°，后對X軸旋轉(zhuǎn)60°：因此可發(fā)現(xiàn)兩個旋轉(zhuǎn)矩陣并不相同。這里要注意坐標(biāo)軸旋轉(zhuǎn)的次序，不同次序組合的歐拉角，將得到不同的旋轉(zhuǎn)矩陣。旋轉(zhuǎn)矩陣反推旋轉(zhuǎn)角度：若β≠±90°，可得到：Z-Y-Z歐拉角：1.3.3齊次變換矩陣對于一個點(diǎn)P，由其在坐標(biāo)系｛B｝中的表達(dá)轉(zhuǎn)換到坐標(biāo)系｛A｝中的表達(dá)。例1-7：則AP為多少？圖1.17求解AP示意圖解：（參考圖1.17）將點(diǎn)P1在同一坐標(biāo)系中進(jìn)行轉(zhuǎn)動和移動到P1－2位置（見圖1.18），那么P1與P2之間可表達(dá)為：圖1.18點(diǎn)P在坐標(biāo)系中的移動和轉(zhuǎn)動圖齊次變換矩陣的連續(xù)運(yùn)算，見圖1.19。圖1.19齊次變換矩陣的連續(xù)運(yùn)算連續(xù)運(yùn)算，求未知的相對關(guān)系。求矩陣T的反矩陣。解：（參考圖1.20）圖1.20求解矩陣T的反矩陣1.3.4剛體狀態(tài)的表達(dá)剛體的狀態(tài)由建立在剛體上的坐標(biāo)系來表示，坐標(biāo)系原點(diǎn)常建立在重心上。而剛體狀態(tài)的變化，由坐標(biāo)系的移動與旋轉(zhuǎn)變化來表示（見圖1.21）。移動：由機(jī)體坐標(biāo)系｛B｝的原點(diǎn)位置判定。旋轉(zhuǎn)：由機(jī)體坐標(biāo)系｛B｝的姿態(tài)判定。圖1.21剛體狀態(tài)表達(dá)為了便于連續(xù)運(yùn)算，通過一個特殊形式的齊次變換，將移動和旋轉(zhuǎn)整合起來。即為齊次變換矩陣。齊次變換矩陣有三種用法：1）描述一個坐標(biāo)系相對于另一個坐標(biāo)系的空間狀態(tài)[見圖1.22（a）]。2）對于一個點(diǎn)P，由其在坐標(biāo)系｛B｝中的表達(dá)轉(zhuǎn)換到坐標(biāo)系｛A｝中的表達(dá)[見圖1.22（b）]。3）將點(diǎn)P在同一坐標(biāo)系中進(jìn)行移動和轉(zhuǎn)動[見圖1.22（c）]。圖1.22齊次變換矩陣1.4姿態(tài)的其他描述方法1.4.1X-Y-Z固定角坐標(biāo)系X-Y-Z固定角坐標(biāo)系，為描述坐標(biāo)系的另一種方式（見圖1.23）。1）將坐標(biāo)系｛A｝與坐標(biāo)系｛B｝的原點(diǎn)重合。圖1.23描述坐標(biāo)系的三種不同方式不同于前面介紹的歐拉角，這里每個旋轉(zhuǎn)都是繞著固定參考坐標(biāo)系｛A｝的軸。這里可直接推導(dǎo)等價旋轉(zhuǎn)矩陣：注意這個結(jié)果與以Z-Y-X歐拉角旋轉(zhuǎn)得到的結(jié)果完全相同?？梢钥偨Y(jié)，繞固定軸旋轉(zhuǎn)3次的最終姿態(tài)和以相反順序繞運(yùn)動坐標(biāo)軸旋轉(zhuǎn)3次的最終姿態(tài)相同。因?yàn)楸磉_(dá)式相同，所以X-Y-Z固定角的反算也與之前介紹過的Z-Y-X歐拉角的反算完全相同，這里不做贅述。1.4.2等效軸角坐標(biāo)系表示法對于空間中兩個任意姿態(tài)的坐標(biāo)系，總可以在空間里找到某個軸，使其中一個坐標(biāo)系繞該軸旋轉(zhuǎn)一個角度就能與另一個坐標(biāo)系姿態(tài)重合。此時所說的軸是一般方向的軸（不是之前所說的主軸）。式中vθ＝1－cosθ。上式將等效軸角坐標(biāo)系表示轉(zhuǎn)變成旋轉(zhuǎn)矩陣表示。反算：對于給定的旋轉(zhuǎn)矩陣，求與θ。若θ角在0°和180°之間（θ≠0°或180°），則：1.4.3歐拉參數(shù)法另一種姿態(tài)表示法是通過4個數(shù)值來表示的，稱為歐拉參數(shù)。根據(jù)等效旋轉(zhuǎn)軸＝（kx，ky，kz）T和等效旋轉(zhuǎn)角θ得到歐拉參數(shù)如下：這4個參數(shù)不是獨(dú)立的：這個關(guān)系式是恒定的。4個參數(shù)存在這一個限制條件，同樣與空間姿態(tài)有3個自由度相符。四元數(shù)的4個參數(shù)，可以看作是四維空間中單位超球面上的一點(diǎn)。用歐拉參數(shù)組表示旋轉(zhuǎn)矩陣Rε：用已知的旋轉(zhuǎn)矩陣Rε得到對應(yīng)的歐拉參數(shù)：這里注意：當(dāng)θ角等于180°時，ε4＝0，式子將失去意義。歐拉參數(shù)法的應(yīng)用：對剛體進(jìn)行旋轉(zhuǎn)時，如果對兩個正交矩陣復(fù)合需要進(jìn)行27次乘法和18次加法；而通過歐拉參數(shù)的形式只需要16次乘法和12次加法，這對嵌入式系統(tǒng)尤其重要。第2章旋翼無人機(jī)的建模分析2.1旋翼無人機(jī)系統(tǒng)的坐標(biāo)系介紹在對飛行器進(jìn)行姿態(tài)分析時，要依靠參考坐標(biāo)系，因此首先建立兩個參考坐標(biāo)系：一個是機(jī)體坐標(biāo)系Ob（xb，yb，zb），而另一個是地球表面慣性坐標(biāo)系Oe（xe，ye，ze），對于定義的兩個直角參考坐標(biāo)系，它們的坐標(biāo)軸都符合右手定則。如圖2.1所示，機(jī)體坐標(biāo)系的坐標(biāo)原點(diǎn)Ob位于四旋翼飛行器的重心，機(jī)體坐標(biāo)系的zb軸位于垂直于飛行器平面上的垂直線上。xb軸和飛行器的一根軸上的機(jī)架在同一條直線上，前方指向xb軸的正方向。yb軸則和飛行器上的另一根軸上的機(jī)架在同一條直線上，右方為xb軸正方向。地球表面慣性坐標(biāo)系的原點(diǎn)Oe固定在地面的某一點(diǎn)，先讓xe軸在水平面內(nèi)指向某一方向，ze軸垂直于地面向下，根據(jù)右手定則可確定軸ye。圖2.1地球表面慣性坐標(biāo)系與機(jī)體坐標(biāo)系2.2旋翼無人機(jī)的運(yùn)動分析四旋翼飛行器通過調(diào)節(jié)4個電機(jī)轉(zhuǎn)速來改變旋翼轉(zhuǎn)速，實(shí)現(xiàn)升力的變化，從而控制飛行器的姿態(tài)和位置。四旋翼飛行器是一種6自由度的垂直升降機(jī)，雖然只有4個輸入力，同時卻有6個狀態(tài)輸出，所以它又是一種欠驅(qū)動系統(tǒng)?？裳豿，y，z軸平移運(yùn)動，以及沿著x，y，z軸旋轉(zhuǎn)運(yùn)動。基本運(yùn)動狀態(tài)分別是：升降運(yùn)動、滾轉(zhuǎn)運(yùn)動、俯仰運(yùn)動、偏航運(yùn)動和水平運(yùn)動，分別對應(yīng)沿z軸平移、繞x軸旋轉(zhuǎn)、繞y軸旋轉(zhuǎn)、繞z軸旋轉(zhuǎn)、沿x或y軸平移運(yùn)動。2.2.1平移運(yùn)動分析1）升降運(yùn)動。如圖2.2所示，升降運(yùn)動是指飛行器沿z軸的正方向或負(fù)方向運(yùn)動。同時等量地增大或減小四個電機(jī)的轉(zhuǎn)速，即可實(shí)現(xiàn)增大或減小螺旋槳沿z軸方向的升力（如圖2.2所示，分別為T1，T2，T3，T4）。當(dāng)垂直方向的升力大于重力（見圖2.2中的G）和阻力的和力時，即可實(shí)現(xiàn)沿z軸負(fù)方向運(yùn)動；當(dāng)重力大于垂直方向的升力和阻力的和力時，即可實(shí)現(xiàn)沿重力方向運(yùn)動；當(dāng)垂直方向的升力等于重力時，即可實(shí)現(xiàn)飛行器的定高飛行。因?yàn)?個電機(jī)的升力及轉(zhuǎn)速是等量增減的，所以升降運(yùn)動是獨(dú)立的，不會影響其他維度的運(yùn)動。經(jīng)過簡單分析可知，當(dāng)電機(jī)轉(zhuǎn)速改變量相同時，X字形結(jié)構(gòu)的飛行器所產(chǎn)生的轉(zhuǎn)動力矩大小大約是十字形結(jié)構(gòu)飛行器的倍。所以X字形結(jié)構(gòu)的無人機(jī)的機(jī)動性要好于十字形結(jié)構(gòu)的無人機(jī)。圖2.2升降運(yùn)動2）水平運(yùn)動。水平運(yùn)動是指飛行器沿x軸或y軸方向運(yùn)動。如圖2.2升降運(yùn)動所示的飛行器，由于阻力的存在，飛行器保持水平姿態(tài)實(shí)現(xiàn)水平運(yùn)動是不可能的，必須依靠飛行器的升力在水平面上的分力提供動力克服阻力，才能實(shí)現(xiàn)飛行器的水平運(yùn)動。因此必須先進(jìn)行相應(yīng)的滾轉(zhuǎn)運(yùn)動及俯仰運(yùn)動方可實(shí)現(xiàn)水平運(yùn)動，且進(jìn)行水平運(yùn)動時飛行器必須保持某一姿態(tài)才可克服阻力保持飛行。因此水平運(yùn)動與俯仰運(yùn)動及滾轉(zhuǎn)運(yùn)動是耦合的，飛行器只有4個獨(dú)立的自由度。由于飛行器的水平運(yùn)動與俯仰運(yùn)動及滾轉(zhuǎn)運(yùn)動有關(guān)，俯仰運(yùn)動及滾轉(zhuǎn)運(yùn)動的靈敏度直接影響到水平運(yùn)動的靈敏度。2.2.2旋轉(zhuǎn)運(yùn)動分析1）滾轉(zhuǎn)運(yùn)動。滾轉(zhuǎn)運(yùn)動是指飛行器繞x軸轉(zhuǎn)動，使飛行器在y軸方向產(chǎn)生角度變化。如圖2.3所示，當(dāng)同時等量增加y軸負(fù)方向的二號、三號兩電機(jī)轉(zhuǎn)速，等量減小y軸正方向一號、四號兩電機(jī)轉(zhuǎn)速，即可實(shí)現(xiàn)四個電機(jī)升力對y軸方向力矩大小的改變。由于x軸正方向力矩增大，飛行器將會繞x軸正方向轉(zhuǎn)動，進(jìn)而實(shí)現(xiàn)滾轉(zhuǎn)運(yùn)動。若要實(shí)現(xiàn)沿x軸負(fù)方向轉(zhuǎn)動，只需等量增加一號、四號電機(jī)轉(zhuǎn)速，等量減小二號、三號電機(jī)轉(zhuǎn)速即可。由于增大的升力與減小的升力相同，因此升力的合力在z軸方向的大小不會改變，即滾轉(zhuǎn)運(yùn)動不會影響升降運(yùn)動。但由于滾轉(zhuǎn)運(yùn)動使姿態(tài)角發(fā)生改變，升力的合力會在y軸方向上產(chǎn)生分力，進(jìn)而發(fā)生沿y軸的水平運(yùn)動，所以滾轉(zhuǎn)運(yùn)動與沿y軸的運(yùn)動是耦合的。滾轉(zhuǎn)運(yùn)動必然伴隨沿y軸的水平運(yùn)動。圖2.3滾轉(zhuǎn)運(yùn)動2）俯仰運(yùn)動。同理，俯仰運(yùn)動是指飛行器繞y軸轉(zhuǎn)動，使飛行器在x軸方向產(chǎn)生角度變化。如圖2.4所示，當(dāng)同時等量增加x軸正方向一號、二號兩電機(jī)轉(zhuǎn)速，等量減小x軸負(fù)方向三號、四號兩電機(jī)轉(zhuǎn)速，即可實(shí)現(xiàn)四個電機(jī)升力對y軸力矩大小的改變。由于y軸正方向力矩增大，飛行器將會繞y軸正方向轉(zhuǎn)動，進(jìn)而實(shí)現(xiàn)俯仰運(yùn)動。若要實(shí)現(xiàn)沿y軸負(fù)方向轉(zhuǎn)動，只需等量增加三號、四號電機(jī)轉(zhuǎn)速，等量減小一號、二號電機(jī)轉(zhuǎn)速即可。與滾轉(zhuǎn)運(yùn)動情況相似，升力的合力在z軸方向的大小不會改變，俯仰運(yùn)動不會影響升降運(yùn)動。俯仰運(yùn)動使姿態(tài)角發(fā)生改變，升力的合力會在x軸方向上產(chǎn)生分力，進(jìn)而發(fā)生沿x軸的水平運(yùn)動，所以俯仰運(yùn)動與沿x軸的運(yùn)動是耦合的。俯仰運(yùn)動必然伴隨沿x軸的水平運(yùn)動。2.4俯仰運(yùn)動3）偏航運(yùn)動。偏航運(yùn)動是指飛行器繞z軸方向轉(zhuǎn)動，偏航運(yùn)動可以改變飛行器機(jī)頭方向。如圖2.5所示，當(dāng)?shù)攘吭龃髮且惶?、三號電機(jī)轉(zhuǎn)速，等量減小對角二號、四號電機(jī)轉(zhuǎn)速，即可實(shí)現(xiàn)飛行器在z軸正方向上角動量的不平衡，進(jìn)而使飛行器發(fā)生繞z軸正方向的轉(zhuǎn)動。同理，若要實(shí)現(xiàn)飛行器繞z軸負(fù)方向的轉(zhuǎn)動只需等量增大二號、四號電機(jī)轉(zhuǎn)速，等量減小一號、三號電機(jī)的轉(zhuǎn)速即可。圖2.5偏航運(yùn)動在偏航運(yùn)動中，由于4個升力的大小是等量增減的，故不會對z軸方向上升力的合力產(chǎn)生影響，即偏航運(yùn)動與升降運(yùn)動是獨(dú)立的。偏航運(yùn)動時傾斜姿態(tài)角未發(fā)生變化，不會在x軸及y軸上產(chǎn)生力矩，同樣也不會在水平面產(chǎn)生分力，所以偏航運(yùn)動與水平運(yùn)動、滾轉(zhuǎn)運(yùn)動、俯仰運(yùn)動都是相互獨(dú)立的。2.3旋翼的陀螺效應(yīng)陀螺效應(yīng)是指旋轉(zhuǎn)中的物體在旋轉(zhuǎn)過程中保持其動量矩矢量方向不變的特性。這種特性有兩個特點(diǎn)：定軸性和進(jìn)動性。定軸性是指當(dāng)旋轉(zhuǎn)的物體沒有受到外力矩作用時，其動量矢量在慣性坐標(biāo)系中的方向維持與初始方向不變的性質(zhì)。進(jìn)動性是指當(dāng)旋轉(zhuǎn)的物體受到外力矩M作用時，不會像靜止的物體沿著受力方向傾倒，而是使旋轉(zhuǎn)的物體動量矩矢量H端點(diǎn)繞著轉(zhuǎn)軸以一定角速度ω做進(jìn)動運(yùn)動，如圖2.6所示。根據(jù)賴柴定理，對于旋轉(zhuǎn)的物體，動量矩相當(dāng)于給此物體施加了一個約束，其約束是一對力偶，稱為陀螺力矩。這對力矩表明對于高速旋轉(zhuǎn)的物體，在空間方向上必須有一外力矩克服其產(chǎn)生的陀螺力矩。由以上分析可知要想迫使力矩改變，陀螺效應(yīng)產(chǎn)生有兩個前提條件：①物體必須繞著一個極軸旋轉(zhuǎn)（產(chǎn)生動量矩）；②物體必須有一個外力矩克服陀螺力矩，使其動量矩的方向發(fā)生改變。旋翼繞zb軸方向做高速轉(zhuǎn)動時，若zb軸發(fā)生轉(zhuǎn)動會形成阻抗力矩，該力矩稱為旋翼的陀螺效應(yīng)力矩。由陀螺力學(xué)可知，陀螺效應(yīng)力矩的表達(dá)式如式（2-1）所示，其中J∑為電機(jī)與槳葉轉(zhuǎn)動慣量的和，ω為機(jī)體坐標(biāo)系的角速度，?為槳葉角速度。由式（2-1）可知，當(dāng)機(jī)體坐標(biāo)系發(fā)生轉(zhuǎn)動時，會在z軸及z軸方向形成陀螺力矩。設(shè)機(jī)體坐標(biāo)系的角速度為ω＝（pqr）T，則在機(jī)體坐標(biāo)下4個槳葉的陀螺效應(yīng)力矩可表示為式（2-2）的形式。圖2.6剛體的進(jìn)動2.4旋翼的空氣動力學(xué)效應(yīng)當(dāng)飛行器具有前飛的速度時，旋翼不僅會受到沿著構(gòu)造旋轉(zhuǎn)軸的升力F和反扭矩Q的作用，同時還會受到阻力D和側(cè)傾力矩L的作用。同時，升力F，反扭矩Q，阻力D以及側(cè)傾力矩L均與旋翼轉(zhuǎn)速ω的平方成比例關(guān)系，具體的表述見式（2-3）：其中，CF為旋翼的拉力系數(shù)，CQ為旋翼的扭矩系數(shù)，CD為旋翼的阻力系數(shù)，CL為旋翼的側(cè)傾力矩系數(shù)，ρ，R，A分別表示空氣密度、旋翼槳葉半徑以及槳盤的面積?？衫脛恿坷碚摵腿~素理論分別對相對氣流和旋翼的幾何特性進(jìn)行分析，以準(zhǔn)確求取旋翼的空氣動力學(xué)系數(shù)。1）誘導(dǎo)速度的計(jì)算。誘導(dǎo)速度，可以反映出旋翼與其所處周圍空氣的相互作用，能夠引起速度大小和方向變化。在旋翼改變空氣的動量和動能的同時，氣流對槳盤產(chǎn)生的反作用就形成了空氣動力。以飛行器處于懸停狀態(tài)為例，推導(dǎo)誘導(dǎo)速度。如圖2.7所示，飛行器處于懸停狀態(tài)，旋翼的滑流截面積為上面大下面小的形狀，其中V0代表旋翼處的空氣速度，v1代表旋翼槳盤處的誘導(dǎo)速度且與該處的空氣速度V1相等，v2代表旋翼下方很遠(yuǎn)處的誘導(dǎo)速度且與該處的空氣的速度V2相等。根據(jù)動量定理，針對旋翼滑流，有：其中，m′為單位時間內(nèi)流過滑流任一截面的空氣質(zhì)量，F(xiàn)′為滑流所受到的合外力。根據(jù)質(zhì)量守恒定律，可得：其中ρ為空氣密度，V0，V1，V2為圖2.7所示各區(qū)域的空氣速度，S0，S1，S2為對應(yīng)的區(qū)域截面積，C為正常數(shù)。因此，氣流對旋翼槳盤產(chǎn)生的反作用力即為旋翼產(chǎn)生的升力F：根據(jù)能量守恒定律和旋翼滑流的動能變化率可確定旋翼消耗的功率：圖2.7誘導(dǎo)速度根據(jù)式（2-6）（2-7）及（2-8）可得旋翼槳盤下方的誘導(dǎo)速度等于槳盤處的誘導(dǎo)速度的2倍，則：因此式（2-6）表達(dá)的旋翼產(chǎn)生的升力可重新表達(dá)為：其中，ρ為空氣密度，A為旋翼槳的槳盤面積。根據(jù)式（2-10）可得飛行器處于懸停狀態(tài)下的旋翼誘導(dǎo)速度為：同理，利用動量理論和能量定理，可推導(dǎo)出飛行器以速度v飛行時的誘導(dǎo)速度：2）葉素理論。葉素理論是假設(shè)旋翼氣流無滑流收縮，將旋翼槳葉分成很多小的小段，即葉素，根據(jù)旋翼翼型可以對每個葉素上的空氣動力進(jìn)行計(jì)算，然后沿著徑向進(jìn)行積分求得旋翼的總的空氣動力。葉素理論考慮了旋翼的幾何特性和運(yùn)動特性。首先，取槳葉上徑向位置為r，寬度為Δr，弦長為c的葉素，對其進(jìn)行受力分析，如圖2.8所示。其中θs為槳葉葉素幾何安裝角，φs為來流角，αs為迎角，V0為飛行器的飛行速度。圖2.8葉素受力分析根據(jù)圖2.8可知，槳葉葉素的安裝角θs、迎角αs與來流角φs之間有如下關(guān)系：其中，Up，Ut為來流在豎直和水平方向上的兩個速度分量。和誘導(dǎo)速度v1的關(guān)系為：其中，v1為飛行器以某一速度V0飛行時旋翼的誘導(dǎo)速度，λ為流入比系數(shù)，μ為前進(jìn)比系數(shù)。加載在葉素上的升力E和阻力D為：其中，ΔE稱為基元升力，ΔD稱為基元后向力，q為動壓，Ct為葉素的基元升力系數(shù)，Cd為葉素的基元后向力系數(shù)。將基元后向力ΔD和基元升力ΔE在葉素坐標(biāo)系上投影，便可得到如下基元阻力ΔFx和基元拉力ΔFy：因此根據(jù)式（2-16）可得ΔFx≈－ΔD，ΔFy≈ΔE。整個旋翼產(chǎn)生的升力F為：根據(jù)上述求取過程可得旋翼的反扭矩Q、阻力D以及側(cè)傾力矩L，其表達(dá)式如下：將式（2-17）～（2-20）和式（2-3）對比，可分別得到拉力系數(shù)CF，扭矩系數(shù)CQ，阻力系數(shù)CD，側(cè)傾力矩系數(shù)CL的表達(dá)式如下：其中，?為旋翼的實(shí)度。2.5空氣阻力1）槳葉揮舞。槳葉揮舞是由沿著槳葉氣流入射速度分布不均勻引起的運(yùn)動。前行和后行槳葉所受的升力有差異會導(dǎo)致升力不均勻，同時葉片非完全剛體則會引起槳尖掃過的平面發(fā)生傾斜，從而產(chǎn)生槳葉揮舞現(xiàn)象即槳葉的上下運(yùn)動。如圖2.9（a）所示，前行槳葉逆風(fēng)前進(jìn)，因此獲得更大的相對速度。這時作用在螺旋槳的拉力增加，產(chǎn)生向上揮舞速度。如圖2.9（c）所示，螺旋槳的向上揮舞速度會使攻角減小，進(jìn)而減少拉力。圖2.9槳葉揮舞2）多旋翼氣動阻力模型。如圖2.10（a）所示，多旋翼向右飛行，螺旋槳逆時針旋轉(zhuǎn)，在A點(diǎn)處產(chǎn)生最大的相對速度。然而，螺旋槳最大的上偏位置出現(xiàn)在B點(diǎn)，其中A點(diǎn)比B點(diǎn)滯后π/2。這個道理如同正弦曲線運(yùn)動規(guī)律一般[如圖2.10（b）]。因此，如圖2.10（c）所示，螺旋槳槳盤下方是前行螺旋槳（advancingblade）區(qū)，上方是后撤螺旋槳（retreatingblade）區(qū)。因?yàn)槲恢脺笏俣?，所以前面半?yún)^(qū)是螺旋槳上偏區(qū)，而后面半?yún)^(qū)是螺旋槳下偏區(qū)。螺旋槳的揮舞改變了槳盤的方向，從而進(jìn)一步改變了拉力的方向。從圖2.11可以看出，拉力不再與多旋翼的機(jī)體軸平行，而是在xb負(fù)方向上產(chǎn)生分量，即誘導(dǎo)阻力。該阻力是多旋翼阻力的主要組成部分，不容忽視。多旋翼氣動阻力模型將以此為依據(jù)。多旋翼在機(jī)體軸xb，yb上的速度為，，則：阻力表示為：圖2.10螺旋槳揮舞速度和偏移位置圖2.11多旋翼飛行器升力阻力示意圖其中fx，fy分別表示機(jī)體軸xb，yb上的阻力，而kdrαg∈R＋表示阻力系數(shù)，多旋翼的空氣阻力模型如式（2-29），式（2-30）。2.6旋翼無人機(jī)模型的建立2.6.1受力在機(jī)體系和導(dǎo)航系之間的轉(zhuǎn)換在建立四旋翼飛行器的動力學(xué)模型之前，先定義以下變量。飛行器的位置向量P＝（x，y，z）T∈R3是飛行器重心相對于慣性坐標(biāo)系Oe（Xe，Ye，Ze）的坐標(biāo)變量，飛行器的線速度向量v＝（v1，v2，v3）T∈R3是飛行器相對于慣性坐標(biāo)Oe（Xe，Ye，Ze）的線速度，飛行器的角速度向量ω＝（ω1，ω2，ω3）T∈R3是飛行器相對于慣性坐標(biāo)系Oe（Xe，Ye，Ze）的角速度，飛行器的姿態(tài)向量Θ＝（φ，θ，ψ）T∈R3是飛行器相對于慣性坐標(biāo)系Oe（Xe，Ye，Ze）的姿態(tài)角變量，其中φ，θ，ψ分別表示飛行器的滾轉(zhuǎn)角、俯仰角、偏航角，如圖2.12所示。圖2.12四旋翼飛行器姿態(tài)角表示其中，滾轉(zhuǎn)角φ是機(jī)體坐標(biāo)系Ob（Xb，Yb，Zb）相對慣性坐標(biāo)系Oe（Xe，Ye，Ze）沿X軸的變化的角度，一般定義飛行器向右滾轉(zhuǎn)時的角度為正。由機(jī)體坐標(biāo)系Ob（Xb，Yb，Zb）到慣性坐標(biāo)系Oe（Xe，Ye，Ze）沿X軸旋轉(zhuǎn)的轉(zhuǎn)換矩陣為：俯仰角θ是機(jī)體坐標(biāo)系Ob（Xb，Yb，Zb）相對于慣性坐標(biāo)系Oe（Xe，Ye，Ze）沿Y軸的變化的角度，一般定義飛行器頭部上揚(yáng)時為正。由機(jī)體坐標(biāo)系Ob（Xb，Yb，Zb）到慣性坐標(biāo)系Oe（Xe，Ye，Ze）沿Y軸旋轉(zhuǎn)的轉(zhuǎn)換矩陣為：偏航角ψ是機(jī)體坐標(biāo)系Ob（Xb，Yb，Zb）相對于慣性坐標(biāo)系Oe（Xe，Ye，Ze）沿Z軸的變化的角度，一般定義飛行器向右偏航時為正。由機(jī)體坐標(biāo)系Ob（Xb，Yb，Zb）到慣性坐標(biāo)系Oe（Xe，Ye，Ze）沿Z軸旋轉(zhuǎn)的轉(zhuǎn)換矩陣為：2.6.2歐拉角速度與機(jī)體角速度的轉(zhuǎn)換機(jī)體坐標(biāo)系Ob（Xb，Yb，Zb）到慣性坐標(biāo)系Oe（Xe，Ye，Ze）的線速度轉(zhuǎn)換矩陣R（Θ）∈R3×3是按照Zg，Yg，Xg的順序?qū)C(jī)體坐標(biāo)系Ob（Xb，Yb，Zb）旋轉(zhuǎn)得到的。其數(shù)學(xué)表達(dá)式為：角速度轉(zhuǎn)換矩陣RT（Θ）的一階導(dǎo)數(shù)滿足以下性質(zhì)：式（2-37）中矩陣S（ε）∈R3×3為一個反對稱矩陣，其具體形式為：2.6.3旋翼無人機(jī)的動力學(xué)模型四旋翼飛行器控制器設(shè)計(jì)的基礎(chǔ)是飛行器的動力學(xué)模型，前文我們已經(jīng)闡述過可以把四旋翼飛行器看作剛體來進(jìn)行研究，且飛行器的重心與機(jī)體坐標(biāo)系的原點(diǎn)重合，忽略飛行器運(yùn)動時由螺旋槳和電機(jī)產(chǎn)生的振動，以及飛行器受地面效應(yīng)的影響。在上述假設(shè)下，四旋翼飛行器的運(yùn)動可分為：重心沿慣性坐標(biāo)系X，Y，Z三個坐標(biāo)軸的線運(yùn)動，以及繞慣性坐標(biāo)系三個坐標(biāo)軸的轉(zhuǎn)動，即滾轉(zhuǎn)、俯仰、偏航。對于四旋翼飛行器，大多數(shù)研究人員都是依據(jù)角動量守恒定律和牛頓第二定律進(jìn)行飛行器的動力學(xué)建模。根據(jù)牛頓第二定律可知，建立四旋翼飛行器的動力學(xué)方程如式（2-39）所示：其中，m為整個飛行器的總質(zhì)量，包括供電設(shè)備（鏗電池）和機(jī)架、螺旋槳電機(jī)等的質(zhì)量，因此飛行器的質(zhì)量不會發(fā)生改變；F為飛行器運(yùn)動過程中所受的合外力。飛行器所受的外力包括重力、阻力、升力。其中ˉG＝（0，0，g）T∈R3為重力向量（g＝9.8m/s2，為重力加速度）。在機(jī)體坐標(biāo)系中，單個螺旋槳所產(chǎn)生的升力為：式（2-41）是在慣性坐標(biāo)下建立的物理方程式，因此要把兩個物理方程式統(tǒng)一在同一坐標(biāo)系中，利用線速度轉(zhuǎn)換矩陣R（Θ）∈R3×3，統(tǒng)一到慣性坐標(biāo)系中，如式（2-42）：飛行器在運(yùn)動的過程中受到的阻力為：化簡整理得：式（2-45）為四旋翼飛行器的線性位移的運(yùn)動數(shù)學(xué)模型。根據(jù)角動量守恒定律，慣性坐標(biāo)系下飛行器的轉(zhuǎn)動方程式如式（2-46）所示。飛行器的角速度向量和飛行器的姿態(tài)向量有如下關(guān)系：則式（2-47）化簡整理可得：其中，MB為機(jī)體坐標(biāo)系下的合外力矩。四旋翼飛行器在運(yùn)動過程中，螺旋槳的旋轉(zhuǎn)除了給飛行器提供一個向上的升力外，每個螺旋槳還產(chǎn)生一個扭矩。設(shè)旋翼到重心的距離大小為l，且四個旋翼與重心的距離都相等，則每個旋翼所產(chǎn)生的力矩大小為：由四旋翼飛行器的飛行原理可知，俯仰力矩大小為：滾轉(zhuǎn)力矩大小為：偏航力矩大小為：其中，λ為比例系數(shù)，其大小由旋翼半徑、空氣密度、葉片數(shù)等決定。在四旋翼飛行器運(yùn)動過程中，當(dāng)飛行器的飛行姿態(tài)發(fā)生變化時，對于高速旋轉(zhuǎn)的螺旋槳和直流無刷電機(jī)的轉(zhuǎn)子，由于陀螺效應(yīng)會產(chǎn)生一個附加的無法避免的陀螺力矩，其數(shù)學(xué)表達(dá)為：其中Hi為總動量矩，定義Jr為繞z軸轉(zhuǎn)動的慣量，動量矩的數(shù)學(xué)表達(dá)式為：將式（2-54）代入式（2-53）化簡整理得：則四旋翼飛行器的總合外力矩為：將式（2-49）（2-50）（2-51）（2-56）代入式（2-48）化簡整理得：由式（2-47）得到機(jī)體角速度與姿態(tài)的方程如下：綜上所述，四旋翼飛行器的運(yùn)動學(xué)方程為：第3章旋翼無人機(jī)的運(yùn)動信息估計(jì)3.1旋翼無人機(jī)系統(tǒng)中傳感器簡介3.1.1九軸慣性測量單元1）慣性測量單元。慣性測量單元（inertialmeasurementunit，簡稱IMU）是測量物體三軸姿態(tài)角（或角速率）以及加速度的裝置。IMU大多用在需要進(jìn)行運(yùn)動控制的設(shè)備上，如汽車和機(jī)器人；也被用在需要用姿態(tài)進(jìn)行精密位移推算的設(shè)備，如潛艇、飛機(jī)、導(dǎo)彈（見圖3.1）和航天器的慣性導(dǎo)航設(shè)備等。圖3.1導(dǎo)彈上的慣性測量元件MEMS三軸加速度計(jì)2）技術(shù)背景。傳統(tǒng)的MEMS慣性傳感設(shè)備的成本非常低，體積極小，質(zhì)量很輕，輸出穩(wěn)定且不易受到干擾，對外部環(huán)境條件的適應(yīng)性極強(qiáng)，而且現(xiàn)在幾乎隨處都能買到。應(yīng)用于旋翼無人機(jī)控制的九軸慣性測量傳感器包括三軸陀螺儀、三軸加速傳感器、三軸磁感應(yīng)傳感器。三軸陀螺儀最大的作用就是測量角速度，以判別物體的運(yùn)動狀態(tài)，所以也稱為運(yùn)動傳感器。加速度計(jì)是慣性導(dǎo)航和慣性制導(dǎo)系統(tǒng)的基本測量元件之一，其本質(zhì)上是一個振蕩系統(tǒng)，可以用來測量載體的運(yùn)動加速度。磁力儀中的磁力計(jì)采用垂直的結(jié)構(gòu)，每個磁力計(jì)軸上的傳感器檢測在該方向上的地磁場強(qiáng)度。3）慣性測量裝置IMU的工作原理。三軸加速度計(jì)是一個微機(jī)電系統(tǒng)（MEMS）元件，如圖3.2所示。MEMS三軸加速度計(jì)是采用壓阻式、壓電式和電容式工作原理，產(chǎn)生的加速度（壓力或者位移）分別正比于電阻、電壓和電容的變化。這些變化可以進(jìn)行放大處理之后通過濾波電路進(jìn)行采集。該傳感器的缺點(diǎn)是受振動影響較大。由于大多數(shù)MEMS傳感器是以半導(dǎo)體材料作為基礎(chǔ)加工的，而半導(dǎo)體對溫度很敏感，再加上安裝、電路設(shè)計(jì)等影響因素，零點(diǎn)漂移和溫度漂移不可避免，但可以通過濾波算法得到更準(zhǔn)確的數(shù)值?；贛EMS技術(shù)的三軸加速傳感器配合陀螺儀或電子羅盤等元件可創(chuàng)建方位推算系統(tǒng)，對定位系統(tǒng)實(shí)現(xiàn)互補(bǔ)性應(yīng)用。圖3.2MEMS加速度測量原理陀螺儀的英文名為gyroscope，是一種用來傳感和維持方向的裝置設(shè)備。陀螺儀是由一個位于軸心且可旋轉(zhuǎn)的輪子構(gòu)成。陀螺儀一旦開始旋轉(zhuǎn)，由于輪子的角動量，陀螺儀有抗拒方向改變的趨向。陀螺儀可分為單軸陀螺儀和三軸陀螺儀兩種。單軸陀螺儀只能測量一個方向，系統(tǒng)測試三維空間就需要三個。而三軸陀螺儀就可以同時測量6個方向。三軸陀螺儀多用于航海、航天等導(dǎo)航、定位系統(tǒng)，它能夠精確地確定運(yùn)動物體的方位，其物理模型如圖3.3（a）所示。MEMS陀螺原理如圖3.3（c）所示，兩個質(zhì)量塊運(yùn)動速度方向相反，而大小相同。它們產(chǎn)生的科里奧利力相反，從而壓迫兩塊對應(yīng)的電容板移動，產(chǎn)生電容差分變化。電容的變化正比于旋轉(zhuǎn)角速度。因?yàn)榧铀俣茸兓荒軐?dǎo)致兩個質(zhì)量塊朝相同方向移動，不會帶來電容的差分變化。三軸陀螺儀在實(shí)際生產(chǎn)和安裝過程中，總會出現(xiàn)一些偏差。因此，實(shí)際應(yīng)用時需要建立標(biāo)定前和標(biāo)定后的三軸角速度的關(guān)系。圖3.3（b）所示為三個互相垂直的磁力計(jì)，每個軸向上的傳感器檢測在該方向上的地磁場強(qiáng)度。這種具有晶體結(jié)構(gòu)的合金材料對外界的磁場很敏感，磁場的強(qiáng)弱變化會導(dǎo)致AMR（anisotropicmagneto-resistance，各向異性磁致電阻，見圖3.4）自身電阻值發(fā)生變化。另外還可以采用洛倫茲力原理，電流流過磁場產(chǎn)生力，從而驅(qū)動電容等變化。圖3.3陀螺儀及MEMS原理圖3.4磁致電阻當(dāng)有外界磁場Hα?xí)r，AMR上主磁域方向就會發(fā)生變化而不再是初始的方向了，那么磁場方向和電流的夾角θ也會發(fā)生變化，如圖3.5所示。對于AMR材料來說，θ角的變化會引起AMR自身阻值的變化，并且呈線性關(guān)系，如圖3.5所示。圖3.5磁致電阻作用原理三軸電子羅盤在實(shí)際生產(chǎn)和安裝過程中，總會出現(xiàn)一些偏差，不僅如此，周邊的原件（飛控芯片、供電電路、電機(jī)）也會對其產(chǎn)生影響。要想減小誤差也需要相應(yīng)的模型分析和校正方法。3.1.2GPS定位系統(tǒng)（見圖3.6）是指利用衛(wèi)星，在全球范圍內(nèi)實(shí)時進(jìn)行定位、導(dǎo)航的系統(tǒng)，簡稱GPS（globalpositioningsystem）。例如美國的GPS、中國的北斗星、俄羅斯的GLONASS、歐洲的伽利略都屬于衛(wèi)星定位系統(tǒng)。圖3.6定位系統(tǒng)雖然慣性導(dǎo)航系統(tǒng)能夠連續(xù)工作并有效地提供姿態(tài)信息、位置信息和速度信息，但由于慣性傳感器誤差的積累，慣性導(dǎo)航系統(tǒng)的工作精度會隨時間下降。GPS可以提供長時間的誤差為米級的高精度位置輸出且用戶設(shè)備成本較低，但由于GPS信號會被遮擋或干擾，因此不能僅依賴GPS提供連續(xù)導(dǎo)航參數(shù)（見圖3.7）。鑒于INS（inertialnavigationsystem，慣性導(dǎo)航系統(tǒng)）和GPS系統(tǒng)的優(yōu)缺點(diǎn)互補(bǔ)，飛控系統(tǒng)中經(jīng)常將二者組合在一起，綜合兩系統(tǒng)的優(yōu)勢，提供有效、長時、高精度、完整的導(dǎo)航參數(shù)。圖3.7無人機(jī)搭載的GPS接收機(jī)3.1.3光流點(diǎn)在像平面的運(yùn)動稱為光流。光流是測速算法，并不能直接定位。簡單理解，光流就是通過檢測圖像中光點(diǎn)和暗點(diǎn)的移動，來判斷圖像中像素點(diǎn)相對于飛行器的移動速度。如果地面是靜止的，自然就可以得到飛行器相對于地面的移動速度。光流定位其實(shí)是利用光流測速（見圖3.8）再積分定位。圖3.8光流測速3.1.4氣壓計(jì)多旋翼多采用壓電式氣壓計(jì)（見圖3.9），氣壓計(jì)也是一種壓力傳感器，其原理是許多晶體（如石英、碧璽、酒石酸鉀鈉等）按一定軸向受壓時，會在表面上產(chǎn)生電荷（壓電現(xiàn)象），電荷的量與所受壓力成正比。新一代的氣壓計(jì)既可以測量氣壓和高度，又可以測量溫度。大氣壓隨高度的增加而減小，氣壓計(jì)正是通過測量大氣壓來估計(jì)高度。由于大氣壓分布不是均勻的而且氣壓計(jì)對氣流的影響很敏感（有風(fēng)情況測量不準(zhǔn)），因此氣壓計(jì)只能得到飛行高度的近似值。圖3.9氣壓計(jì)外觀圖3.2系統(tǒng)可觀性分析圖3.10系統(tǒng)狀態(tài)圖可觀測性表示輸出可以完全反映系統(tǒng)狀態(tài)的特性，因此不僅要考慮系統(tǒng)的狀態(tài)方程還要考慮輸出方程。以考慮連續(xù)時間線性時變系統(tǒng)∑為例，其狀態(tài)方程和輸出方程分別為：其中t，t0∈J，x為n維狀態(tài)，u為p維輸入，y為q維輸出，t0為初始時刻，A（t）為n×n維的系統(tǒng)矩陣，B（t）為n×p維的輸入矩陣，C（t）為q×n維的輸出矩陣，D（t）為q×p維的傳輸矩陣，A（t）的元在J上為絕對可積，B（t）的元在J上為平方可積。對系統(tǒng)的運(yùn)動進(jìn)行分析可知，系統(tǒng)∑狀態(tài)解的表達(dá)式為：其中φ（t，τ）為系統(tǒng)的狀態(tài)轉(zhuǎn)移矩陣，δ（t）為單位沖激函數(shù)。在許多實(shí)際問題中，控制系統(tǒng)的狀態(tài)變量不是由直接測量得到的，而是通過某種觀測方法得到的，某種觀測系統(tǒng)所得到的結(jié)果對系統(tǒng)狀態(tài)真實(shí)性的反映就是系統(tǒng)的可觀測性。可觀性反映系統(tǒng)外部可直接或間接測量的輸出y（t）和輸入u（t）來確定或識別系統(tǒng)狀態(tài)的能力。如果系統(tǒng)的任何內(nèi)部運(yùn)動狀態(tài)變化都可由系統(tǒng)的外部輸出和輸入唯一地確定，那么稱系統(tǒng)是能觀的，或者更確切地說，是狀態(tài)能觀的。否則，就稱系統(tǒng)為狀態(tài)不完全能觀的。下面通過例子來說明可觀性的意義。例3-1：給出兩個傳感器GPS和加速度計(jì)，那么哪一個能夠穩(wěn)定地估計(jì)出多旋翼速度？為了簡化起見，我們用GPS觀測位置，一般用如下模型：其中x，v∈R分別表示位置狀態(tài)和速度狀態(tài)，ε∈R表示已知統(tǒng)計(jì)特性的噪聲。為了簡化起見，用加速度計(jì)估計(jì)速度，一般用如下模型：其中v，α∈R分別表示速度大小和加速度大小，ε∈R表示已知統(tǒng)計(jì)特性的噪聲。3.3姿態(tài)估計(jì)3.3.1互補(bǔ)濾波1）互補(bǔ)濾波基本概念。對無人機(jī)上的慣性測量單元來說，加速度計(jì)對四軸的加速度比較敏感，取瞬時值計(jì)算傾角誤差比較大；而陀螺儀積分得到的角度不受加速度的影響，但是隨著時間的增加積分漂移和溫度漂移帶來的誤差比較大。所以這兩個傳感器正好可以相互彌補(bǔ)缺點(diǎn)。短時間內(nèi)用陀螺儀比較準(zhǔn)確，以它為主；長時間用加速度計(jì)比較準(zhǔn)確，這時候加大它的比重，這就是互補(bǔ)了。而濾波指的是加速度計(jì)要濾掉高頻信號，陀螺儀要濾掉低頻信號，互補(bǔ)濾波器就是根據(jù)傳感器特性不同，通過不同的濾波器（高通或低通，互補(bǔ)的），然后再相加得到整個頻帶的信號。例如，加速度計(jì)測傾角，其動態(tài)響應(yīng)較慢，在高頻時信號不可用，所以可通過低通抑制高頻；陀螺儀響應(yīng)快，積分后可測傾角，不過由于零漂等，在低頻段信號不好，通過高通濾波可抑制低頻噪聲。將兩者結(jié)合，就將陀螺和加速度計(jì)的優(yōu)點(diǎn)融合起來，得到在高頻和低頻都較好的信號，互補(bǔ)濾波需要選擇切換的頻率點(diǎn)，即高通和低通的頻率。2）用于姿態(tài)解算的互補(bǔ)濾波原理及應(yīng)用。圖3.11準(zhǔn)確地闡述了互補(bǔ)濾波的過程。正常情況下用陀螺儀的數(shù)據(jù)就可以進(jìn)行姿態(tài)的更新，但是由于陀螺儀的積分誤差，這里用acc（加速度）和mag（地磁信息）去校正，求出他們的誤差用D去彌補(bǔ)。注意看看PID的公式和作用，PID是作用于誤差（實(shí)際與期望之間的差值），最終反復(fù)調(diào)節(jié)，讓實(shí)際值等于期望值。圖3.11互補(bǔ)濾波流程圖3.3.2梯度下降法梯度下降法（gradientdescent）是一個一階最優(yōu)化算法，通常也稱為最速下降法。要使用梯度下降法找到一個函數(shù)的局部極小值，必須沿函數(shù)當(dāng)前點(diǎn)對應(yīng)梯度（或者是近似梯度）的反方向按規(guī)定步長進(jìn)行迭代搜索。如果相反地向梯度正方向進(jìn)行迭代搜索，則會接近函數(shù)的局部極大值點(diǎn)，這個過程則被稱為梯度上升法。在互補(bǔ)濾波法中，我們使用加速度計(jì)來補(bǔ)償陀螺儀，將兩者的數(shù)據(jù)融合解算出四元數(shù)下的姿態(tài)。在梯度下降法中，依然是通過加速度計(jì)的數(shù)據(jù)來與陀螺儀的數(shù)據(jù)進(jìn)行融合。而不同點(diǎn)是：互補(bǔ)濾波法中，對載體坐標(biāo)系b下的加速度做叉積以求出誤差，補(bǔ)償給陀螺儀以校正誤差；梯度下降法中，通過加速度計(jì)的數(shù)據(jù)求出一組四元數(shù)q，然后通過四元數(shù)的微分方程，并使用陀螺儀得到的數(shù)據(jù)求解出另外一組四元數(shù)q?。因?yàn)樵诟咚龠\(yùn)動狀態(tài)下，陀螺儀數(shù)據(jù)更可靠；而在低速運(yùn)動狀態(tài)下，加速度計(jì)數(shù)據(jù)更可靠，所以兩組四元數(shù)分別乘以權(quán)重再相加，就得到了期望的輸出結(jié)果。使用梯度下降法求解姿態(tài)的算法介紹：假設(shè)一個誤差函數(shù)ef（x），我們希望其滿足：ef（x）＝0，即誤差為0。接著，我們要求解這個方程，得到x的值。此時x值對應(yīng)最優(yōu)解。使用梯度下降法求解，首先我們就需要先求出它的導(dǎo)數(shù)將x換成四元數(shù)Q＝（q0，q1，q2，q3）。函數(shù)也相應(yīng)變化，其導(dǎo)數(shù)變?yōu)槿缦滦问剑阂驗(yàn)槲覀兦蠼獾淖藨B(tài)是三維姿態(tài)，而上式的因變量只對應(yīng)了一維的情況。所以將因變量推廣到xyz軸下，變成一個多元向量函數(shù)：求它的導(dǎo)數(shù)：當(dāng)這個誤差函數(shù)為0（即最小值）時，我們認(rèn)為旋轉(zhuǎn)矩陣沒有誤差，姿態(tài)是精確的。那么，接下來的任務(wù)就是使用前面介紹的梯度下降法相關(guān)公式來求解了。用前面闡述的方法求這個多元向量函數(shù)的導(dǎo)數(shù)（即它的雅可比矩陣）：代入公式求出梯度：通過離散疊加替代積分可以求解出一組四元數(shù)xn，將其乘以權(quán)重與陀螺儀數(shù)據(jù)通過四元數(shù)微分方程求解得到的另一組四元數(shù)q?。最終的公式為：其中qg為最終輸出的四元數(shù)，α為權(quán)重，q為加速度計(jì)的數(shù)據(jù)通過梯度下降法得到的四元數(shù)，q?為陀螺儀的數(shù)據(jù)通過四元數(shù)微分方程求解得到的四元數(shù)。3.3.3卡爾曼濾波1）卡爾曼濾波算法的基本思想?？柭鼮V波（Kalmanfiltering），一種利用線性系統(tǒng)狀態(tài)方程，通過系統(tǒng)輸入輸出觀測數(shù)據(jù)，對系統(tǒng)狀態(tài)進(jìn)行最優(yōu)估計(jì)的算法。因?yàn)橛^測數(shù)據(jù)中包括系統(tǒng)中的噪聲和干擾的影響，所以最優(yōu)估計(jì)也可看作濾波過程。算法的核心思想是，根據(jù)當(dāng)前的儀器測量值和上一刻的預(yù)測量和誤差，計(jì)算得到當(dāng)前的最優(yōu)量，再預(yù)測下一刻的量。里面比較突出的是觀點(diǎn)是把誤差納入計(jì)算，而且分為預(yù)測誤差和測量誤差兩種，統(tǒng)稱為噪聲。還有一個非常大的特點(diǎn)是誤差獨(dú)立存在，始終不受測量數(shù)據(jù)的影響。2）卡爾曼濾波算法的原理及推導(dǎo)公式。我們先要引入一個離散控制過程的系統(tǒng)。該系統(tǒng)可用一個線性隨機(jī)微分方程來描述：再加上系統(tǒng)的測量值：上兩式子中，X（k）是k時刻的系統(tǒng)狀態(tài)；U（k）是k時刻對系統(tǒng)的控制量；A和B是系統(tǒng)參數(shù)，對于多模型系統(tǒng)，它們?yōu)榫仃嚕籞（k）是k時刻的測量值；H是測量系統(tǒng)的參數(shù)，對于多測量系統(tǒng)，H為矩陣；W（k）和V（k）分別表示過程和測量的噪聲。它們被假設(shè)成高斯白噪聲（whiteGaussiannoise），它們的協(xié)方差分別是Q，R（這里假設(shè)它們不隨系統(tǒng)狀態(tài)變化而變化）。當(dāng)滿足上面的條件時（線性隨機(jī)微分系統(tǒng)中產(chǎn)生的噪聲都是高斯白噪聲），卡爾曼濾波器是最優(yōu)的信息處理器。下面用它們結(jié)合協(xié)方差來估算系統(tǒng)的最優(yōu)化輸出。首先，我們要利用系統(tǒng)的過程模型，來預(yù)測下一狀態(tài)的系統(tǒng)。假設(shè)現(xiàn)在的系統(tǒng)狀態(tài)是k，根據(jù)系統(tǒng)的模型，可以基于系統(tǒng)的上一狀態(tài)而預(yù)測出現(xiàn)在狀態(tài)：式（3-1）中，X（k∣k－1）是利用上一狀態(tài)預(yù)測的結(jié)果，X（k－1∣k－1）是上一狀態(tài)最優(yōu)的結(jié)果，U（k）為現(xiàn)在狀態(tài)的控制量，如果沒有控制量，它可以為0。到現(xiàn)在為止，系統(tǒng)結(jié)果已經(jīng)更新了，可是對應(yīng)于X（k∣k－1）的協(xié)方差還沒更新。用P表示協(xié)方差：式（3-2）中，P（k∣k－1）是X（k∣k－1）對應(yīng)的協(xié)方差，P（k－1∣k－1）是X（k－1∣k－1）對應(yīng)的協(xié)方差，A′表示A的轉(zhuǎn)置矩陣，Q是系統(tǒng)過程的協(xié)方差。式（3-1）（3-2）就是卡爾曼濾波器5個公式當(dāng)中的前兩個，也就是對系統(tǒng)的預(yù)測。有了現(xiàn)在狀態(tài)的預(yù)測結(jié)果，再收集現(xiàn)在狀態(tài)的測量值。結(jié)合預(yù)測值和測量值，可以得到現(xiàn)在狀態(tài)（k）的最優(yōu)化估算值X（k∣k）：其中g(shù)K為卡爾曼增益（Kalmangain）：到現(xiàn)在為止，已經(jīng)得到了（k）狀態(tài)下最優(yōu)的估算值X（k∣k）。但是為了讓卡爾曼濾波器不斷地運(yùn)行下去直到系統(tǒng)過程結(jié)束，還要更新（k）狀態(tài)下X（k∣k）的協(xié)方差：其中I為1的矩陣，對于單模型單測量，I＝1。當(dāng)系統(tǒng)進(jìn)入（k＋1）狀態(tài)時，P（k∣k）就是式（3-2）的P（k－1∣k－1）。這樣，算法就可以自回歸地運(yùn)算下去。這就是卡爾曼濾波器原理的基本描述。式（3-1）（3-2）（3-3）（3-4）和式（3-5）就是他的5個基本公式。根據(jù)這5個公式，可以很容易地實(shí)現(xiàn)計(jì)算機(jī)的程序。3.4位置估計(jì)3.4.1加速度計(jì)積分加速度是一個對象速度的變化速率，速度是一個對象位置的變化速率。換句話說，速度是位置的導(dǎo)數(shù)，加速度是速度的導(dǎo)數(shù)，因此有如下公式：積分和導(dǎo)數(shù)相反。如果一個物體的加速度已知，那么我們能夠利用二重積分獲得物體的位置。假設(shè)初始條件為0，那么有如下公式：一個理解這個公式的方法是將積分定義成曲線下面包圍的區(qū)域，積分運(yùn)算結(jié)果是極小區(qū)域的總和，區(qū)域的寬度趨近于0。換句話說，積分的和表示了一個物理變量的大?。ㄋ俣龋?。利用曲線下方區(qū)域的概念，我們能得出一個結(jié)論：對一個信號采樣，得到該信號大小的瞬時值，這樣就能夠在兩次采樣之間得到一個小的區(qū)域。為了獲得連貫的值，采樣時間必須相同。采樣時間代表這塊區(qū)域的寬，同時采樣得到的值代表區(qū)域的高。設(shè)加速度函數(shù)為x（t），則速度函數(shù)y（t）與它的關(guān)系如式（3-6）所示，x（t）拉氏變換函數(shù)為x（s），y（t）拉氏變換為y（s），則將式（3-6）進(jìn)行拉氏變換后如式（3-7）所示：所以加速度積分的模型如圖（3.12）所示，圖中x（s）為加速度的S域函數(shù)，y（s）為積分后的速度S域函數(shù)。由1/s的Z變換為1/（1－1/z）可以將式（3-7）轉(zhuǎn)換到Z域，如下式：由上式可以進(jìn)一步得到加速度和積分后的速度的差分方程關(guān)系：其中y（k）是速度的第k個采樣點(diǎn)，y（k－1）是速度的第k－1個采樣點(diǎn)，x（k）是加速度的第k個采樣點(diǎn)。圖3.12加速度積分積分會對誤差積累，隨著時間的推移積分出的誤差就會越來越明顯。因此實(shí)際測得的加速度零點(diǎn)可能偏上或者偏下，這樣就會導(dǎo)致積分出的速度整體以一定的斜率向上或者向下漂移，需要對計(jì)算出的速度進(jìn)行誤差補(bǔ)償。3.4.2組合導(dǎo)航（GPS與IMU）組合導(dǎo)航技術(shù)結(jié)合GPS、慣性測量元件、地磁指南針和氣壓計(jì)各自的優(yōu)缺點(diǎn)，使用電子信號處理領(lǐng)域的很多技術(shù)，融合多種傳感器的測量值，獲得較為準(zhǔn)確的飛行器15個狀態(tài)量的測量。慣性測量元件的測量容易發(fā)散，這個發(fā)散可以通過GPS來抑制。GPS可以獲得三維位置也可以獲得三維速度，慣性測量元件可以獲得三維加速度，加速度的積分也是速度。在通過地磁指南針獲得航向的基礎(chǔ)上，兩種速度的觀測就可以融合起來，通過GPS的測量值來發(fā)現(xiàn)并抑制慣性測量元件的發(fā)散。慣性測量元件的發(fā)散被抑制住之后，它也可以更準(zhǔn)確地測量三維角度和三維加速度。因此GPS和慣性測量元件在這些情況中互相取長補(bǔ)短。除此之外，氣壓計(jì)和GPS互相提高了高度測量的精度，地磁指南針、GPS和慣性測量元件一同提高了航向測量的精度，它們都是利用了相同的融合、互補(bǔ)的思想。組合導(dǎo)航技術(shù)中傳感器互補(bǔ)的原理直接源于1948年誕生的信息論。克勞德·香農(nóng)總結(jié)歸納出的信息論提出了信息的概念以及如何從數(shù)學(xué)上度量信息，信息論可以說是現(xiàn)代人類文明的基石之一。解釋清楚信息的本質(zhì)之后，人們才能夠用數(shù)學(xué)表示一個樸素而又深刻的原理：信息可以用來估計(jì)狀態(tài)，信息越多，狀態(tài)量被估計(jì)得越準(zhǔn)。GPS是一種基于衛(wèi)星的無線電導(dǎo)航系統(tǒng)。GPS可為各類用戶連續(xù)地提供動態(tài)的三維位置、三維速度和時間信息，實(shí)現(xiàn)全球、全天候的連續(xù)實(shí)時導(dǎo)航、定位和授時。加之GPS設(shè)備成本低、體積小等優(yōu)點(diǎn)，GPS技術(shù)在導(dǎo)航和測量各領(lǐng)域得到了廣泛的應(yīng)用。慣性測量單元（inertialmeasurementunit，IMU）具有自主性強(qiáng)、隱蔽性好和信息完備的特點(diǎn)。而系統(tǒng)定位誤差隨時間持續(xù)積累，即使是高精度的INS系統(tǒng)長期工作誤差也會越來越大，必須進(jìn)行修正。GPS技術(shù)和慣性技術(shù)各有優(yōu)勢，但在誤差傳播性能上正好是互補(bǔ)的。INS的誤差會隨時間積累而越來越大；而GPS的誤差不隨時間積累。INS短期精度好，尤其在高動態(tài)環(huán)境下精度仍然較高；而GPS在長時間工作時表現(xiàn)出色，但不適用于高動態(tài)環(huán)境。高精度的GPS信息，作為外部量測輸入，在運(yùn)動過程中頻繁修正INS，以控制其誤差隨時間的積累；短時間高精度的INS定位結(jié)果，可以很好地解決GPS動態(tài)環(huán)境中的信號失鎖和周跳問題，而且INS還可以輔助GPS接收機(jī)增強(qiáng)其抗干擾能力，提高捕獲和跟蹤衛(wèi)星信號的能力。因此把這兩項(xiàng)技術(shù)結(jié)合起來可以得到高精度的導(dǎo)航系統(tǒng)。1）組合導(dǎo)航系統(tǒng)的卡爾曼濾波器。MEMS-IMU/GPS組合導(dǎo)航系統(tǒng)的基本構(gòu)成為：MEMS-IMU、GPS接收機(jī)和導(dǎo)航計(jì)算機(jī)。導(dǎo)航系采用東北天地理坐標(biāo)系，各傳感器信息通過最優(yōu)卡爾曼濾波器進(jìn)行融合，系統(tǒng)的工作原理如圖3.13所示。圖3.13組合導(dǎo)航系統(tǒng)的工作原理考慮陀螺儀隨機(jī)誤差為一階馬氏過程和白噪聲，加速度計(jì)隨機(jī)誤差為白噪聲，系統(tǒng)的誤差狀態(tài)變量定義為：其中，φE，φN，φU為東向、北向和天向的數(shù)學(xué)平臺誤差角；δvE，δvN，δvU為東向、北向和天向速度誤差；δφ，δλ，δh為緯度、經(jīng)度和高度誤差；εrx，εry，εrz為陀螺儀一階馬氏過程誤差。列寫狀態(tài)方程如下：圖3.14所示的系統(tǒng)中，采用常規(guī)松組合的方式進(jìn)行MEMS-IMU和GPS的速度、位置組合。由于MEMS-IMU的精度非常低，航向角發(fā)散速度相當(dāng)快，GPS的航跡角雖然不是真航向，但在很大程度上也能反映出載體的航向變化，故將GPS航跡角與MEMS-IMU的航向進(jìn)行組合，從而維持系統(tǒng)航向的有效輸出。為了增加系統(tǒng)姿態(tài)信息的冗余度，特別是在GPS信號失效時保持系統(tǒng)的姿態(tài)穩(wěn)定，利用MEMS-IMU中加速度計(jì)的比力信息間隔性地估計(jì)姿態(tài)角（滾轉(zhuǎn)角和俯仰角），并與MEMS-IMU輸出的姿態(tài)角進(jìn)行組合。加速度計(jì)估計(jì)的姿態(tài)角可表示為：姿態(tài)觀測方程可表示為：其中，Hα為姿態(tài)觀測矩陣；Vα為觀測噪聲；Zα為觀測量，由MEMS-IMU解算的姿態(tài)（y1，θ1）和加速度計(jì)估計(jì)的姿態(tài)（yD，θD）之差構(gòu)成，即：由于系統(tǒng)狀態(tài)方程中的誤差角為數(shù)學(xué)平臺誤差角，它描述了數(shù)學(xué)平臺和地理坐標(biāo)系之間的關(guān)系，而姿態(tài)觀測方程中的姿態(tài)誤差角描述了載體和地理坐標(biāo)系之間的關(guān)系，因此，不能將姿態(tài)觀測矩陣簡單定義為單位矩陣。通過推導(dǎo)可得姿態(tài)誤差角與數(shù)學(xué)平臺誤差角之間存在如下的轉(zhuǎn)換關(guān)系：綜上所述，MEMS-IMU/GPS組合導(dǎo)航系統(tǒng)的觀測方程可表示為：其中，H為觀測矩陣，V為觀測噪聲，Z為觀測量。2）信息融合策略。MEMS-IMU/GPS組合導(dǎo)航系統(tǒng)的信息融合策略表述為：①系統(tǒng)在工作過程中，MEMS-IMU每隔ΔT＝0.01s采樣一次，GPS每隔TG＝0.1s輸出載體速度、位置和航跡角信息，對各測量值剔除野值和粗大誤差。②MEMS-IMU通過級聯(lián)解算獲得載體的姿態(tài)、速度和位置信息，加速度計(jì)每隔TA＝0.1s計(jì)算滾轉(zhuǎn)角和俯仰角。③當(dāng)系統(tǒng)導(dǎo)航時間t為TG（或TA）的整數(shù)倍時，采用最優(yōu)卡爾曼濾波進(jìn)行姿態(tài)、速度和位置組合。④當(dāng)觀測信息有效時，利用卡爾曼濾波器的估計(jì)值更新系統(tǒng)的姿態(tài)、速度和位置，并對系統(tǒng)進(jìn)行反饋校正；當(dāng)觀測信息無效時，卡爾曼濾波器只進(jìn)行時間更新，并直接輸出MEMS-IMU的導(dǎo)航結(jié)果。3.4.3光流（室內(nèi)）定位在無人機(jī)上光流定位通常是借助于無人機(jī)底部的一個攝像頭采集圖像數(shù)據(jù)，然后采用光流算法計(jì)算兩幀圖像的位移，進(jìn)而實(shí)現(xiàn)對無人機(jī)的定位，這種定位手段配合GPS可以在室外實(shí)現(xiàn)對無人機(jī)的精準(zhǔn)控制，并且在室內(nèi)沒有GPS信號的時候，也可以實(shí)現(xiàn)對無人機(jī)的高精度的定位，實(shí)現(xiàn)更加平穩(wěn)的控制。在光流理論中，前提是下面兩個假設(shè)成立：①攝像頭采集到的兩幀圖像之間的像素灰度不變。②相鄰的兩幀圖像的像素具有相對運(yùn)動。根據(jù)第一個假設(shè)，如果兩幀圖像的灰度值不變，那么以下關(guān)系式成立：其中I（x，y，t）表示在時間dt后移動到第二幀圖像（x＋dx，y＋dy）的位置，采用泰勒級數(shù)對兩邊進(jìn)行展開，消去相同的項(xiàng)，就可以得到如下方程：其中：以上就是光流方程，其中fx和fy表示圖像的梯度，ft表示時間梯度。但是上述方法是無法得到（u，v）T，因?yàn)橐粋€等式無法求解兩個未知數(shù)。為了解決這個問題，我們可以采用經(jīng)典的Lucas-Kanade方法來進(jìn)行求解。在Lucas-Kanade方法中，我們需要用到第二個假設(shè)，即在目標(biāo)點(diǎn)的鄰域內(nèi)所有的點(diǎn)都具有相似的運(yùn)動，這就是Lucas-Kanade方法的核心，基于該假設(shè)，其利用一個3×3鄰域中的9個點(diǎn)具有相同運(yùn)動得到9個光流方程，然后采用最小二乘法進(jìn)行擬合求解，最終得到（u，v）T為：以上就是光流法計(jì)算像素點(diǎn)的移動速度的方法，在使用的時候，我們只需要對圖像中的一些點(diǎn)進(jìn)行跟蹤，采用上面的方法就可以計(jì)算得到光流向量，根據(jù)得到的光流向量，就可以進(jìn)一步優(yōu)化無人機(jī)的姿態(tài)控制，實(shí)現(xiàn)更加準(zhǔn)確的控制。第4章旋翼無人機(jī)視覺定位方法及系統(tǒng)4.1視覺定位方法簡介自主導(dǎo)航是移動機(jī)器人領(lǐng)域中的關(guān)鍵技術(shù)之一。近年來，隨著機(jī)器人系統(tǒng)運(yùn)算能力和圖像處理等相關(guān)技術(shù)的發(fā)展，人們開始越來越多地研究不依賴于GPS系統(tǒng)的自主導(dǎo)航技術(shù)，這是因?yàn)镚PS系統(tǒng)存在精度不夠高、更新速度慢以及數(shù)據(jù)容易跳變等缺陷；而且在室內(nèi)環(huán)境中，往往無法獲得GPS信號；天氣條件、云層太厚也會影響GPS信號的接收?；谝曈X的飛行器位姿測量是當(dāng)前旋翼飛行器導(dǎo)航的研究熱點(diǎn)，常用的飛行器視覺位姿測量方法可以分為外部視覺方法和內(nèi)部視覺方法兩種。內(nèi)部視覺方法即通過機(jī)載的圖像傳感器來獲得位姿信息，又叫機(jī)載視覺定位，它不依賴于外部信號強(qiáng)度，具有抗干擾能力強(qiáng)、適應(yīng)性好的特點(diǎn)。機(jī)載視覺系統(tǒng)按照視覺傳感器的種類可以分為雙目及多目視覺系統(tǒng)、單目視覺系統(tǒng)兩種。雙目及多目視覺系統(tǒng)，以Bumblebee2為例，該物理系統(tǒng)主要由兩個單目攝像機(jī)組成，其相互之間標(biāo)定工藝復(fù)雜，價格昂貴，但是適用范圍廣，精度十分高。和雙目及多目視覺系統(tǒng)相比，單目視覺系統(tǒng)不需要攝像機(jī)之間相對標(biāo)定，且不受攝像機(jī)安裝誤差的影響，價格便宜，傳感器質(zhì)量小，運(yùn)算量小，但是需要目標(biāo)點(diǎn)之間有幾何約束關(guān)系，且需要至少4個目標(biāo)點(diǎn)成功匹配才能獲取無人機(jī)精確的位姿，因此應(yīng)用場景有限制。外部視覺以Kinect為例，在獲得彩色圖像的同時，通過對紅外數(shù)據(jù)的處理獲得圖像中每個點(diǎn)的深度數(shù)據(jù)，從而精確獲取四旋翼飛行器的位置和姿態(tài)信息，但是它的缺點(diǎn)也是顯然的，當(dāng)飛行器到室外或是一些無法安置攝像頭的地方，就無法實(shí)現(xiàn)視覺定位。4.2相機(jī)標(biāo)定相機(jī)的精確標(biāo)定是無人機(jī)位姿精確求解的必要條件。對于單目視覺來說，相機(jī)標(biāo)定就是獲取相機(jī)內(nèi)參以及畸變參數(shù)；對于雙目視覺來說，既要獲取內(nèi)參也要獲取外參。當(dāng)然，相機(jī)標(biāo)定在OPENCV或MATLAB中都有現(xiàn)成的函數(shù)，只要知道如何調(diào)用就可以了。但是之所以單獨(dú)列出一節(jié)，是為了讓讀者可以透過函數(shù)看到外表下的內(nèi)涵。在了解標(biāo)定過程之前，我們需要了解到圖像坐標(biāo)系是如何與世界坐標(biāo)系聯(lián)系在一起的，并且坐標(biāo)系之間的關(guān)系不僅僅關(guān)乎標(biāo)定過程，還是視覺定位的原理及其基礎(chǔ)。這個過程需要一步一步地分析，我們試圖通過攝像機(jī)坐標(biāo)系來搭建一個橋梁。在此之前，為了方便讀者理解，先列出一些名詞的解釋。世界坐標(biāo)系（xw，yw，zw）：是目標(biāo)物體位置的參考系。除了無窮遠(yuǎn)，世界坐標(biāo)可以根據(jù)運(yùn)算方便與否自由放置。相機(jī)坐標(biāo)系（xc，yc，zc）：是攝像機(jī)站在自己角度上衡量的物體的坐標(biāo)系。攝像機(jī)坐標(biāo)系的原點(diǎn)在攝像機(jī)的光心上，z軸與攝像機(jī)光軸平行。圖像坐標(biāo)系（x，y）/（u，v）：是以攝像機(jī)拍攝的二維照片為基準(zhǔn)建立的坐標(biāo)系。讀者也許已經(jīng)發(fā)現(xiàn)了，如圖4.1所示相機(jī)拍攝出來的圖片都是以像素的形式儲存的，由于存儲順序的原因，拍攝出來的坐標(biāo)系的原點(diǎn)并不在圖像的中心，這并不方便我們后續(xù)的建模，所以我們需要將坐標(biāo)原點(diǎn)在左上角的（u，v）形式的圖像坐標(biāo)系轉(zhuǎn)換為坐標(biāo)原點(diǎn)在中心的（x，y）形式的圖像坐標(biāo)系。圖4.1坐標(biāo)系之間的關(guān)系顯然，（x，y）與（u，v）存在如下轉(zhuǎn)換關(guān)系：其中，dx代表x軸方向一個像素的寬度，dy代表y軸方向上一個像素的寬度。dx，dy為攝像機(jī)的內(nèi)參數(shù)。（u0，v0）是圖像平面的主點(diǎn)，位于攝像機(jī)光軸與成像平面的焦點(diǎn)O′處，也是攝像機(jī)的內(nèi)參數(shù)。根據(jù)式（4-1）和（4-2），我們可以寫出式（4-3）這個矩陣形式：接著我們試圖走出圖像坐標(biāo)系，將圖像坐標(biāo)系與相機(jī)坐標(biāo)系連接起來。在這里，我們先假設(shè)主點(diǎn)就是成像儀的中心（實(shí)際上芯片的中心通常不在光軸上，這是因?yàn)槲覀儫o法對成像儀進(jìn)行微米級別的精度安裝，在后面的內(nèi)容中我們將考慮這個誤差），然后我們可以利用小孔成像的經(jīng)典模型進(jìn)行建模，如圖4.2所示。圖4.2小孔成像模型這里為了將模型簡化，將成像平面放在了小孔前面，形成了如圖4.3的模型。在數(shù)學(xué)上這兩個模型是等價的，并且后者更方便我們建模。圖4.3透視投影模型不妨設(shè)在相機(jī)坐標(biāo)系上的一點(diǎn)為（xc，yc，zc），圖像平面上所對應(yīng)的一點(diǎn)為（x，y），我們可以很容易地寫出如下公式：顯然，我們可以根據(jù)式（4-4）和（4-5）寫出式（4-6）這個矩陣形式：至此也就完成了圖像坐標(biāo)系與相機(jī)坐標(biāo)系的聯(lián)系。接下來，只剩下最后一步就可以將圖像坐標(biāo)系與世界坐標(biāo)系聯(lián)系起來。如圖4.1所示，我們可以看到相機(jī)坐標(biāo)系與世界坐標(biāo)系只差一個旋轉(zhuǎn)和平移的過程?；趧傮w變換的模型，我們寫出式（4-7）：基于上面講的內(nèi)容，我們可以得出如圖4.4的結(jié)果。圖像坐標(biāo)系就是這樣與世界坐標(biāo)系聯(lián)系在一起的。圖4.4圖像坐標(biāo)系與世界坐標(biāo)系之前的內(nèi)容我們提到過主點(diǎn)并不是成像儀的中心，盡管可能誤差并不大，但是我們?nèi)匀恍枰茖W(xué)嚴(yán)謹(jǐn)?shù)孛鎸@些問題。其實(shí)只需在式（4-4）和（4-5）上作出一些修改，如式（4-8）和（4-9）所示。其中cx和cy代表主點(diǎn)與成像儀中心的可能的偏移，寫成矩陣形式如下：盡管這樣的偏移是客觀存在的，但是由于這只是建模的過程，這樣的誤差是可以容忍的。綜合式（4-3）（4-6）和（4-7），我們可以寫出式（4-11）。因?yàn)闃?biāo)定物是平面，所以我們可以把世界坐標(biāo)系構(gòu)造在zw＝0的平面上。那么式（4-11）可以改寫成式（4-12）。得出上述式子我們就可以開始標(biāo)定的過程了。從式（4-14），我們可以得出如下結(jié)論：我們只需要已知四個點(diǎn)在圖像坐標(biāo)系和世界坐標(biāo)系的位置，就可以求出單應(yīng)性矩陣H。為了方便后續(xù)推導(dǎo)作以下運(yùn)算：定義：那么，我們有其中結(jié)合式（4-16）和式（4-17），我們得出：根據(jù)式（4-23），我們發(fā)現(xiàn)只需要三幅圖，就可以求出B矩陣，再使用Cholesky分解，就可以輕松地得到攝像機(jī)的內(nèi)參矩陣A。內(nèi)參矩陣求出來了，外參矩陣自然也求出來了，這是由于A是可逆矩陣。盡管只需要三幅圖與四個點(diǎn)我們就可以求出內(nèi)參與外參矩陣，但是為了讓求出的結(jié)果更魯棒，我們更傾向于使用棋盤格的標(biāo)定板，在這上面將會有許多可以檢測到的特征點(diǎn)，并且我們會多次移動相機(jī)或標(biāo)定板，每移動一次獲取一幅新的圖像。最后通過最小化來獲取內(nèi)參矩陣和外參矩陣，通過這樣的方法我們可以使得求出的結(jié)果不再只是拘泥于物理模型，而是具有了真正的實(shí)際物理意義。4.3Kinect外部視覺定位Kinect是微軟公司開發(fā)的3D體感攝像機(jī)，開始用于體感游戲，目前廣泛用于機(jī)器人視覺系統(tǒng)。Kinect外觀如圖4.5所示。圖4.5Kinect外觀RGB彩色攝像頭用于彩色圖像的攝取，深度圖像信息采用的是主動獲取方式，紅外發(fā)射器發(fā)射紅外線，紅外傳感器對反射回來的紅外線進(jìn)行接收，通過對反射信息的處理，就能獲得每個像素的深度值。Kinect是目前最便宜的三維深度攝像頭。為了精確獲取四旋翼飛行器的位置信息，設(shè)計(jì)了一套基于Kinect的旋翼飛行機(jī)器人視覺定位系統(tǒng)。Kinect是一款RGB-D傳感器，即在獲得彩色圖像的同時，通過對紅外數(shù)據(jù)的處理獲得圖像中每個點(diǎn)的深度數(shù)據(jù)，配備了1080P的高清攝像頭，采用專用的USB3.0端口傳輸數(shù)據(jù)，數(shù)據(jù)幀率最高可達(dá)50幀/秒。基于Kinect的外部視覺定位系統(tǒng)結(jié)構(gòu)示意圖如圖4.6所示，在3m×3m×3m的室內(nèi)空間中，搭建了實(shí)驗(yàn)平臺，其中Kinect位于正中心的上方。Kinect利用光編碼技術(shù)計(jì)算深度信息，通過USB3.0與主機(jī)相連，四旋翼飛行器通過藍(lán)牙與主機(jī)通信，接收主機(jī)的控制指令和主機(jī)所計(jì)算的坐標(biāo)數(shù)據(jù)。地面主機(jī)上運(yùn)行使用跟蹤定位軟件，軟件可以通過Camshift等算法跟蹤安裝在飛行器表面的彩色標(biāo)記以及向飛行器發(fā)送彩色目標(biāo)所在的位置信息，利用該視覺定位系統(tǒng)可以控制飛行器飛行到目標(biāo)附近，并將目標(biāo)所在的三維坐標(biāo)信息輸出給機(jī)械臂，操縱機(jī)械臂完成目標(biāo)抓取作業(yè)。Kinect所發(fā)出的紅外散斑具有高度的隨機(jī)性，而且會隨著距離的不同變換圖案。也就是說，空間中任意兩處的散斑圖案都是不同的。因此，根據(jù)物體上面的散斑圖案，就可以計(jì)算這個物體距離Kinect平面的距離。第二代Kinect配置了1080P的2D彩色攝像頭，水平視野達(dá)到70°，垂直視野達(dá)到60°。Kinect坐標(biāo)系的坐標(biāo)原點(diǎn)位于彩色相機(jī)鏡頭的中心（見圖4.7）。設(shè)彩色圖像處理計(jì)算得到的物體重心在Kinect彩色圖像中的坐標(biāo)為（x，y），圖像坐標(biāo)的原點(diǎn)在圖像的左上角，必須首先將圖像坐標(biāo)轉(zhuǎn)換成以圖像中心點(diǎn)為原點(diǎn)的坐標(biāo)值（x1，y1），計(jì)算公式為：圖4.6基于Kinect的外部視覺定位系統(tǒng)結(jié)構(gòu)示意圖其中，H和W分別是圖像分辨率的高和寬，H＝920dpi，W＝1080dpi。由于Kinect彩色攝像頭的視野范圍（FOV）為垂直60°和水平70°，由此可以求出FOV與像素的關(guān)系約是0.065°/pixel。進(jìn)而可以求得圖4.7中的水平轉(zhuǎn)角θh和豎直轉(zhuǎn)角θv如下。圖4.7Kinect坐標(biāo)系的側(cè)視圖與俯視圖然后根據(jù)Kinect深度信息zw，可以求得在Kinect坐標(biāo)系中的目標(biāo)坐標(biāo)（xw，yw，zw），下標(biāo)w表示世界坐標(biāo)。由此就完成了基于Kinect視覺系統(tǒng)的圖像坐標(biāo)系到世界坐標(biāo)系的映射。再通過Camshift或其他檢測追蹤算法實(shí)時捕捉到無人機(jī)的圖像，就可以基于此圖像實(shí)時獲取無人機(jī)在世界坐標(biāo)系下的位置。該方法的好處是簡單易懂，花費(fèi)不高，精度準(zhǔn)確，但局限性也是一目了然的，因?yàn)橛性S多地方并不適合安裝外部視覺的攝像頭。4.4單目視覺系統(tǒng)顧名思義，單目視覺系統(tǒng)指的是僅僅通過一個攝像頭來實(shí)現(xiàn)無人機(jī)位姿的測量。通過n個世界點(diǎn)和與之對應(yīng)的圖像坐標(biāo)點(diǎn)以及攝像機(jī)的內(nèi)部參數(shù)來計(jì)算攝像機(jī)方位的問題稱為n點(diǎn)透視問題，簡稱為PnP問題。在這里，我們以EPnP算法為例講解飛行器的位姿估計(jì)過程。值得注意的是，內(nèi)部參數(shù)已經(jīng)在之前的相機(jī)標(biāo)定過程中得以求解。接下來我將首先講解一下EPnP算法的原理，盡管這個算法與其他算法一樣都已經(jīng)有現(xiàn)成的函數(shù)，但是我們依舊希望能從前人的智慧中窺見一些對未來的展望，這不僅僅是有助于思維的擴(kuò)展或是對前人貢獻(xiàn)的致敬，更是為日后方便讀者基于這些理論知識進(jìn)行創(chuàng)新。在開始介紹算法之前，出于方便，我們先給出一些符號的定義：：世界坐標(biāo)系的參考點(diǎn)坐標(biāo)；：對應(yīng)于世界坐標(biāo)系下參考點(diǎn)在相機(jī)坐標(biāo)系下的坐標(biāo)；cj：控制點(diǎn)坐標(biāo)。假設(shè)我們已知n個參考點(diǎn)的三維坐標(biāo)以及它們各自對應(yīng)的圖像坐標(biāo)點(diǎn)。對于一般情況，即n個參考點(diǎn)不在一個平面，我們可以給定4個不共面的控制點(diǎn)的不同的線性組合來分別表示這n個參考點(diǎn)；對于n個參考點(diǎn)在同一個平面，我們只需要3個控制點(diǎn)就可以表示這n個參考點(diǎn)，但是這只是上述情況的特殊情況，其實(shí)是包含在原問題里的。我們用4個給定的控制點(diǎn)來表示參考點(diǎn)坐標(biāo)，如式（4-30）所示。由于世界參考點(diǎn)與控制點(diǎn)的坐標(biāo)都是已知的，所以αij也容易求得。同理，我們可以寫出相機(jī)坐標(biāo)系下的關(guān)系式（4-31）：值得注意的是式（4-30）中的αij與式（4-31）中的是同一個，這個原因也是顯然的，這無非是用不同的坐標(biāo)系來描述同一件事情，因此它們的線性組合系數(shù)是不變的。結(jié)合式（4-11）和內(nèi)參矩陣A易推得式（4-32）：在這里，我們忽略γ對后續(xù)結(jié)果的影響，將式（4-32）改寫成式（4-33）：將式（4-34）和式（4-35）寫成矩陣形式并簡寫如下：其中M是一個2n×12的矩陣，且容易發(fā)現(xiàn)矩陣M中的系數(shù)全是已知量，看上去我們應(yīng)該直接求解這個線性方程組，這樣就可以獲取4個控制點(diǎn)在相機(jī)坐標(biāo)系的坐標(biāo)，然而這樣的計(jì)算量無疑是很大的，在這里我們采用了巧妙的方法求解該問題。眾所周知，M