無人機系列之飛控算法

https://blog.csdn.net/ad3600/article/details/54947073

一.? 無人機的分類

按飛行平臺構型分類：無人機可分為固定翼無人機，旋翼無人機，無人飛艇，傘翼無人機，撲翼無人機等。

圖1 無人機平臺構型

多軸飛行器multirotor是一種具有三個以上旋翼軸的特殊的直升機。

旋翼的總距固定而不像一般直升機那樣可變。

通過改變不同旋翼相對轉速可以改變單軸推進力的大小，從而控制飛行器的運行軌跡。

圖2 多軸飛行器

圖3 各類變模態(tài)平臺

二.? 無人機的系統(tǒng)架構

圖4 無人機系統(tǒng)架構

三. 飛控系統(tǒng)簡介

導航飛控系統(tǒng)之導航子系統(tǒng)功能：向無人機提供位置，速度，飛行姿態(tài)，引導無人
機沿指定航線安全，準時，準確的飛行。
????????? 獲得必要的導航要素：高度，速度，姿態(tài)，航向
????????? 給出定位信息：經(jīng)度，緯度，相對位移
????????? 引導飛機沿規(guī)定計劃飛行
????????? 接收控制站的命令并執(zhí)行
????????? 配合其它系統(tǒng)完成各種任務

1. 飛控系統(tǒng)功能:

導航飛控之飛控子系統(tǒng)功能：完成起飛，空中飛行，執(zhí)行任務，返航等整個飛行過
程的核心系統(tǒng)，對無人機實現(xiàn)全權控制與管理，是無人機的大腦。
??????????無人機姿態(tài)穩(wěn)定與控制
????????? 與導航子系統(tǒng)協(xié)調完成航跡控制
????????? 起飛與返航控制
??????????無人機飛行管理
????????? 無人機任務管理與控制
??????????應急控制

2.飛控系統(tǒng)--傳感器:

??????????飛控系統(tǒng)常用的傳感器包括：
??????????角速率傳感器陀螺儀

圖5 陀螺儀

??????????加速度傳感器

圖5 加速計

??????????氣壓計和超聲波

圖5 聲納與氣壓二合一

??????????GPS

圖6 GPS示意圖

??????光流

從二維圖像序列中檢測物體運動、提取運動參數(shù)并且分析物體運動的相關規(guī)律

????????? 光流是空間運動物體在觀測成像平面上的像素運動的“瞬時速度”
????????? 用于飛行器的動態(tài)定位和輔助慣性導航
??????????Lucas Kanade算法
這個算法是最常見，最流行的。它計算兩幀在時間t到t + δt之間每個像素點位置的移動

圖7 光流算法示意圖

??????????地磁傳感器

圖8 磁力計

四. 飛控系統(tǒng)的關鍵算法

1. 關鍵算法流程框圖

圖9 關鍵算法流程框圖

2.姿態(tài)解算

1. init函數(shù)初始化，建立3x3矩陣R。

2. 磁力計修正，得到誤差corr：先計算得到誤差角度mag_er，再用_wrap_pi函數(shù)做約束，再計算corr值，相當于機體坐標系繞地理坐標系N軸（Z軸）轉動arctan（mag_earth(1), mag_earth(0)）度。

3. 加速度計修正更新誤差corr：將陀螺儀計算得到的矩陣第三行（即重力加速度部分）轉換到b系，再將加速度測得重力加速度（_accel - 機體加速度）的數(shù)據(jù)歸一化（本身屬于b系），將這兩個的值進行叉乘即測得誤差。

   具體過程：歸一化的n系重力加速度通過旋轉矩陣R左乘旋轉到b系，即k為歸一化的旋轉矩陣R（b-e）的第三行，總的受到合力的方向（_accel）減去機體加速度方向（_pos_acc）得到g的方向，即總加速度（加速度獲?。p去機體運動加速度（第五部分）獲取重力加速度，然后姿態(tài)矩陣的不是行就是列來與純重力加速度來做叉積，算出誤差。

   
   

   因為運動加速度是有害的干擾，必須減掉。

   
   

   算法的理論基礎是[0,0,1]與姿態(tài)矩陣相乘。該差值獲取的重力加速度的方向是導航坐標系下的z軸，加上運動加速度之后，總加速度的方向就不是與導航坐標系的天或地平行了，所以要消除這個誤差，即“_accel-_pos_acc”。然后叉乘z軸向量得到誤差，進行校準 。

4. 對誤差corr進行PI控制器中的I（積分），得到_gyro_bias，再對_gyro_bias做約束處理。

5. 使用修正的數(shù)據(jù)更新四元數(shù)，并把_rates和_gyro_bias置零便于下次調用時使用。

圖10 姿態(tài)解算

3.姿態(tài)控制

3.1.姿態(tài)角度控制

1. ?獲取目標姿態(tài)target，并構建目標姿態(tài)旋轉矩陣。

2. ?通過控制四元數(shù)獲取當前狀態(tài)的旋轉矩陣DCM。

3. ?取兩個矩陣中的Z軸向量，即YAW-axis。

4. 計算roll，pitch誤差，得到誤差值e_R：通過R_z%R_sp_z叉乘當前姿態(tài)的z軸和目標姿態(tài)的z軸的誤差大?。葱枰D的角度）并旋轉到b系（即先對齊Z軸）。

5. ?計算yaw的權重。

6. ?構造e_R_cp反對稱矩陣，通過羅德里格公式旋轉得到roll，pitch旋轉后的矩陣 R_rp。

7. ?計算yaw的誤差，進一步更新誤差值e_R：roll_pitch旋轉后的矩陣的x軸

1. 目標姿態(tài)旋轉矩陣獲取四元數(shù)。

2. 對四元數(shù)的虛部取出賦值給e_R_d，再對其進行歸一化處理。

3. 對四元數(shù)的實部取出。

4. 計算e_R_d：通過虛部與實部的一系列計算得來。

3.2.姿態(tài)角速度控制

1. 獲取當前角速度值rates：通過_ctrl_state數(shù)據(jù)結構（當前姿態(tài)信息）把需要的有效數(shù)據(jù)賦值給rates。

2. 獲取目前角速度值_rates_sp。

3. 計算得到角速度差rates_err。

4. 對角速度差rates_err進行PD控制，還需要加一個前饋。

5. 發(fā)布控制量_att_control。

圖11 姿態(tài)控制