• 導航系與載體坐標系的關(guān)系

?

導航坐標系（簡稱N系）

?

?

載體坐標系（簡稱B系）

????????取右手笛卡爾直角坐標系為載體坐標系，繞X軸逆時針轉(zhuǎn)動為俯仰運動正方向，繞Y軸逆時針轉(zhuǎn)動為橫滾運動正方向，繞Z軸逆時針轉(zhuǎn)動為偏航運動正方向，即途中、、分別表示俯仰角、橫滾角、偏航角。

????????萌新新手容易將三個角度弄混，這里根據(jù)字面意思直觀的理解記憶：

• 俯——形容身體或者頭向下彎曲，從高處俯看；仰——形容面朝上，仰望、仰視，俯仰就是指的飛機相對地平面向上仰視或者向下俯視

• 橫滾——指的是飛機相對縱軸（機頭到機位所在軸）左右發(fā)生旋轉(zhuǎn)后與水平面投影之間的夾角

• ?偏航——航向指的是飛機機頭的朝向，注意偏航角并不是指的實際飛機運動方向。

????????下面以MPU6050為例，來展示加速度計、陀螺儀數(shù)據(jù)三軸數(shù)據(jù)XYZ方向與飛控中B系對應關(guān)系。

?

????????上圖中帶直線箭頭沿坐標軸方向標注+X、+Y、+Z指示的是三軸加速度計原始數(shù)據(jù)正方向，繞軸旋轉(zhuǎn)帶環(huán)狀箭頭+X、+Y、+Z為三軸角速度的正方向。三軸加速度計XYZ正方向分別與載體坐標系的XYZ一致，故繞傳感器自身X、Y、Z軸旋轉(zhuǎn)分別表示俯仰、橫滾、偏航角速度，其中逆時針旋轉(zhuǎn)為正方向。

?

• 載體坐標系XY平面在導航系EN水平面方向的二維投影坐標系（簡稱RP坐標系）

?

????????當飛機在三維空間內(nèi)發(fā)生俯仰、橫滾、橫滾角度運動時，導航坐標系和載體系統(tǒng)不再重合，其中由于偏航運動造成的載體XY平面在導航系上投影方向隨之游動。

????????在無人機進行位置、速度控制時，需要將EN方向的期望位置、速度、加速度轉(zhuǎn)換到RP坐標系上來，最后將期望的加速度映射成期望姿態(tài)角度。姿態(tài)控制環(huán)（角度、角速度）在這里可以理解成為外層控制回路的執(zhí)行機構(gòu)。

????????在這里需要理解的是RP坐標系只是作為系統(tǒng)中間坐標系，用于轉(zhuǎn)換導航EN方向與載體XY方向關(guān)系，坐標軸R(roll)、P(pitch)相對導航系統(tǒng)EN方向來講，相差一個偏航方向運動。

?

• 光流水平位置控制坐標系

????????光流傳感器原始數(shù)據(jù)輸出的是采樣間隔時間內(nèi)相對地面的像素移動，通過相機參數(shù)與旋轉(zhuǎn)補償后可以得到速度觀測量。由于光流是直接固定在飛機上，故光流得到的觀測速度是在RP坐標系下。通過B系到N系的旋轉(zhuǎn)矩陣得到的是飛機相對于地理正北（SLAM開始建圖時刻朝向、無磁力計情況下的開機初始機頭模擬正北）的慣導加速度，所以要想將二者進行融合有兩種途徑：

①　將RP坐標系下的觀測速度，通過偏航角旋轉(zhuǎn)關(guān)系將坐標基底從RP映射到EN方向上來，然后與EN方向的慣導加速度進行融合；

②　將EN坐標系下的慣導加速度，通過偏航角旋轉(zhuǎn)關(guān)系從EN方向映射到RP方向上來，然后與RP方向的觀測速度進行融合；

????????采用方式1時，在無人機進行位置、速度控制時，又需要將EN方向的期望加速度通過偏航映射又需要重新轉(zhuǎn)到RP方向。為了簡化運算流程，飛控代碼在前期采用的是方式2，慣導融合直接在RP坐標系下融合，位置、速度控制也是在RP方向，減少了運算流程。

????????在這里需要注意的是前期飛控光流方向XY方向與RP方向在X方向上剛好反向，部分舊版本的SDK函數(shù)為了兼容老客戶，X方向相反這部分一直有保留下來，下面給出光流控制坐標系與RP坐標系關(guān)系。

?

?

?

????????根據(jù)上面圖示可知：當希望控制飛機向前移動時，可以直接在光流速度控制中，給定Y方向期望速度為+即可，同理希望左側(cè)移動時，給定X方向速度期望為+，速度控制相關(guān)函數(shù)在后面教程中給出接收。

?

• SLAM定位下的坐標系

????????激光雷達SLAM定位能輸出飛機在地圖中的位置與航向角度，注意這里的位置和航向是相對SLAM建圖初始時刻飛機的位置與朝向為基準的，直觀上來理解就是飛機初始時刻朝向哪里，那個方位就是SLAM得到航向角的“正北”，此時SLAM航向角度為0。SLAM輸出的位置原點也是基于初始時刻飛機的坐標位置的。我們將SLAM的“正北”稱為模擬正北(SN),垂直右側(cè)的坐標軸稱為模擬正東(SN)，初始時刻的位置為導航原點O。

?

????????上圖中SE-O-SN為SLAM定位下的等效導航坐標系（EN），O’(x,y)和Yaw分別表示SLAM建圖輸出的位置與航向信息。細心的同學肯定可以發(fā)現(xiàn)，當我們把SN、SE當做導航坐標系下的East、North等效來看時，二者在導航融合和控制方法上是一致的，算法在處理是只需要把原來由磁力計或者GPS運動航向得到的偏航角度信息替換為由SLAM定位算法輸出的航向信息就可以了。需要注意的是由于SLAM定位數(shù)據(jù)有延時、數(shù)據(jù)精度不夠，SLAM定位的航向角度信息同樣需要和飛控IMU融合才能使用。

????????在SLAM定位模式下，配套有專用的高效搭積木式二次開發(fā)教程，針對新手用戶控制無人機飛行有專用的API函數(shù)取實現(xiàn)，可以直接控制無人機位置、速度、角度、角速度等接口函數(shù)，初學者可以無需了解具體內(nèi)部實現(xiàn)（也有配套教程講解實現(xiàn)原理），只需要理解飛行器的基本控制流程，API函數(shù)功能、接口參數(shù)定義等。每個API函數(shù)都有配套的精簡demo例程讓客戶逐步學習來加深理解，直到融會貫通。

?

????????用戶二次開發(fā)時根據(jù)已經(jīng)編寫好的API函數(shù)，按實際比賽要求配合視覺傳感器，編寫按一定流程動作飛行的API指令即可高效率的完成比賽內(nèi)容，同時新增Subtask_Demo.c里面有提供2021年國賽植保無人機G題基礎(chǔ)題、附加題完整競賽學習方案。以往競賽常規(guī)賽題元素例如自主循跡、移動追蹤、繞桿避障、AprilTag定位等demo都有提供。