如何使用優象光流芯片U30實現四軸無人機懸停

在沒有GPS的環境下，比如室內環境，四軸無人機在水平方向會不斷漂移。如何讓無人機實現穩定的自主懸停呢？光流芯片可以感知無人機在水平方向的運動信息（速度、位移等），將該運動信息反饋給飛控系統，再結合無人機高度數據，無人機便可以實現高精度的自主懸停。湖南優象的光流芯片U30是專門為感知無人機在水平方向的運動而開發的一款ASIC芯片，四軸無人機獲取U30的數據，調整PID參數就可以實現穩定的懸停。以前的光流視覺懸停技術對地面紋理、天氣、白天/夜晚有特殊要求，導致無人機難以懸停在亮光瓷磚等光滑地面、柏油路等簡單紋理地面上，光線不足或夜晚也無法懸停。U30采用先進的智能感知算法徹底解決了這些問題，可懸停于幾乎所有的地面上，晚上在城市環境也能懸停，而且體積小、功耗低、成本低。

無人機的飛控端從U30接收到光流數據后，接下來就是如何實現對于飛行的控制，即估計飛機真實的水平運動速度。以光流懸停為例，光流是如何抑制飛機懸停時的漂移的呢？一般來說，飛機在無光流時產生隨機漂移的原因是姿態角度的誤差造成的，使得飛機在自身有傾角時而錯誤的認為自身是水平而導致的，由于多旋翼是欠驅動系統，有傾角從而產生該方向上的漂移速度，因此光流就是估計這個誤差角度或漂移速度，并反饋給控制系統進行控制(一般是串級的PID控制)，控制輸出的結果一般是飛機的傾斜角度，即期望角度。

在實際使用中，用光流實現懸停時主要有兩部分的工作，一是光流數據的處理，對U30輸出的數據進行加工處理，可以是光流的自身濾波，或與加速度計進行融合濾波，以獲取飛機的位移與速度信息；二是用處理后獲得的估計位置與速度數據進行PID控制，控制飛機位置與速度調整，實現懸停。其基本流程如圖1所示。

U30輸出數據的加工處理
U30輸出原始的光流數據并不是直接輸出給PID使用，這些數據需要進行加工處理，才能供飛控端使用。對U30輸出數據的處理分兩種情況：一種是只使用純光流數據，經過濾波處理得到飛機的位移與速度信息；二是將光流數據與加速度計進行融合，以得到飛控端PID控制所需要的位移和速度信息。

只使用純光流數據進行控制，平滑性與抗環境干擾性會差一些，但好處是處理簡單，需要調試的參數少，且在懸停時與進行加速度融合控制效果也差不了多少，建議初次使用時先采用該方式，先跑通整個控制框架；將光流與IMU融合，如果IMU加速度計數據處理得好，數據各個方面的性能會好很多，如果處理不好或加速度計輸出數據比較差反而會適得其反，且處理相對比較復雜，細節較多，調試更困難，因此在單純用光流效果不理想，且調試一段時間后還沒有改善，建議采用本方式。

只使用光流純數據的處理方法
給出的光流數據經解碼與處理后(參考附錄中的up_flow.c)得到的x、y方向的光流flow_dat.x，flow_dat.y。因此光流純數據的處理也是基于該兩個變量。

1.1 速度計算方法：

speed_x = (h*flow_dat.x/ (integration_timespan*0.000001))*100

speed_y = (h*flow_dat.y/(integration_timespan*0.000001))*100

其中h為測量高度，單位m; integration_timespan為相鄰兩個光流輸出的間隔時間,單位us，100是m->cm的轉換。

1.2 位移計算方法：

sum_flow_x += speed_x*(integration_timespan*0.000001);

sum_flow_y += speed_y*(integration_timespan*0.000001);

該位移默認開始懸停點為零，計算的是后續累積時間內，飛機距離該初始位置點的水平位移，單位為cm。

圖1 光流數據處理與控制流程圖

1.3 備注：

1.3.1 由于計算的速度幀率不是很高，且幀間的速度信息可能差別比較大，因此可以根據控制需要，選擇對計算的速度進行低通濾波，但不宜引入過大的延時。

1.3.2 理論上在速度計算之前需要進行旋轉補償，但在初始測試時，可以暫時先不考慮，先跑通整個框架。如果懸停效果可以接受也不用進行旋轉補償；若懸停時低頻震蕩且PID參數怎么也調試不好，就可能需要進行補償，具體的旋轉補償方法在2.2節中再做詳細敘述。

1.3.3 光流位移量的處理有些細節需要注意，在某些情況下需要清除該累積位移與控制積分量。情況一，打搖桿后需要清零，否則松搖桿后飛機還會往前進的反方向后退一段距離；情況二，起飛前或正在起飛時需要清零，否則飛機會斜著起飛，即起飛上升時飛機往一邊嚴重漂移。情況三，懸停時，當累積距離大于一定范圍。

1.3.4 在只使用氣壓計進行定高而沒有超聲波時，由于氣壓計會有很大漂移，因此輸出的高度要做限制。首先限制高度要非負，另外在較低高度時可以使用固定高度，我們在測試時一般將高度寫死固定為1m，實際測試發現在實際高度5m以下，使用該寫死的高度也能有較好的懸停效果；當實際高度比較高時，可以適當放大該固定高度比例。

光流與加速度計融合的處理方法
一般單獨使用光流的數據也能夠獲取較好的靜態懸停效果，如果對光流的動態性能也有較高要求，比如實現更好的抗風、位移控制、目標跟隨等高級功能，則需要進行光流與加速度計數據的融合以獲取更加精確、平滑與實時的數據。具體實現可以參考，下面對該文件進行簡單梳理，并給出計算過程。

第一步：確定使用高度。在只使用氣壓計高度時，高度不是很準(原因參考2.1.3備注中第四點)，因此根據氣壓計的輸出值，對使用的高度進行了分段處理，實際調試中可以根據飛機不同高度是否震蕩進行調整，使飛機不震蕩即可。

第二步：光流角速度的計算。在2.1.2中speed_x為線速度，若speed_x不乘高度即為光流角速度，表示為fx = flow_dat.x/ (integration_timespan*0.000001))，同理可求fy，單位為rad/s。

第三步：光流旋轉補償。該步驟是提升性能核心，否則不能夠支持比較大的PID控制參數，表現為加大控制參數飛機震蕩?；舅枷胧鞘褂?a href="http://www.qd573.com/tags/陀螺儀/" target="_blank">陀螺儀角速度對光流進行旋轉補償，目的是使得飛機在只有旋轉沒有平移時光流最終輸出為零。

補償公式為：

fx_gyro_fix = ((fx + LIMIT(((gyro_lpf_y)/57.3f),-flow_t1,flow_t1)) *10 *use_height ) ; //rotation compensation

fy_gyro_fix = ((fy - LIMIT(((gyro_lpf_x)/57.3f),-flow_t2,flow_t2)) *10 *use_height ) ;

以X方向的補償為例，其中，fx_gyro_fix為旋轉補償后的輸出，單位是mm/s;fx為光流角速度；(10 *use_height)為使用的高度值，單位為mm。

(gyro_lpf_y)/57.3為陀螺儀輸出角速度，單位rad/s，flow_t1為對陀螺儀輸出的限幅，值的大小由fx的輸出最大值決定，因為陀螺儀的輸出是遠遠大于光流的輸出范圍的，限幅避免過度補償。gyro_lpf_y為原始的陀螺儀角速度(度/秒)，經低通濾波后所得：LPF_1_(3.0f,dT,gyro_x,gyro_lpf_x)，目的是使得光流輸出的相位與陀螺儀相位一致。

第四步：求水平航向坐標系下加速度值a(n)(該坐標系與俯仰和滾轉角為零時的機體坐標系重合)。由姿態角可以獲取姿態旋轉矩陣，該姿態旋轉矩陣在yaw=0時得到的旋轉矩陣即為所求R，再使用a(n) = R*a(b)求得目標向量值，包括xyz三個方向的加速度值，其中a(b)為機體加速度計測量向量，即傳感器原始輸出值，單位是mm/(s*s)。

第五步：加速度積分獲取速度，并與光流進行互補濾波,獲取初步的融合速度f1_fx.out。

第六步：融合速度修正，由于加速度計積分有漂移，因此可能造成融合的速度在懸?；蜻\動中會漂移真實值，而光流的計算是沒有漂移及累積誤差的，因此以光流數據做參考，對f1_fx.out做一個二次修正，得到最終融合速度輸出f_out_x。

備注：

本算法中只對融合速度進行了處理，而沒有融合高度的處理。因為在測試中發現使用融合速度積分得到位移精度反而不如單純的光流累積計算精度高，因此建議位移的獲取仍采用2.1.2中位移的獲取方法。

飛控端PID控制方法
由光流獲取飛機水平漂移的速度與位移后，即可以根據這些信息進行控制。以懸停為例，則期望的位移為零，反饋的即光流的測量值，一般采用的是串級的PID控制，即位置環+速度環控制，輸出為期望角度，給姿態控制環。具體可以參考中的控制部分。由于該部分比較簡單，在這里只對其中一些需要注意的細節進行描述。

2.1 對于位置環，期望位置為零，反饋值為光流測量位移，輸出為期望速度。

2.2 對于速度環，期望值為位置環的輸出，反饋值為光流測量速度，速度環輸出為期望角度或期望加速度。理論上，輸出應為期望加速度，并通過非線性轉換為期望角度，從而傳遞給姿態環。在demo中我們的輸出直接是期望角度，這是因為在小角度時，我們認為期望加速度與角度是線性相關的。

2.3 定點控制中比較重要的一點，要確保外環的執行頻率要低于內環的執行頻率，否則會造成系統的不穩定。比如，我們位置環的控制周期為40ms,則速度環的控制周期為20ms等等。

2.4 控制量在某些情況下需要清零，特別是控制積分量。比如打搖桿、起飛時或一鍵翻滾時。

光流使用中需要注意的細節
3.1 光流模塊的檢查。拿到光流芯片后，首先需要檢查鏡頭表面是否有塑料薄膜、水珠、劃痕等，確保鏡頭表面干凈，器件或連線無松動等影響鏡頭成像效果的問題。

3.2 光流模塊的安裝。在飛機上安裝光流模塊時，盡量保證鏡頭的水平，不要有傾斜角度，光流模塊與機體坐標系不要有偏航方向的夾角且在機架的旋轉中心，即光流模塊的軸與機體坐標系的軸應盡量保持重合。特別注意一旦調試通過，進入批量生產時，在其他飛機上都需要以該方向固定安裝，不可隨意改變安裝方向。

3.3初始調試時，接上光流后，使用上位機觀察是否有輸出，驗證輸出值是否與圖2.所述的光流坐標系定義一致，比如往左水平運動光流Y是否為正；水平向右運動光流Y是否為負，且輸出值是否在合理范圍等。

3.4 確保光流的輸出與機體控制坐標系的一致性，即實飛測試前需要驗證光流的控制輸出值與期望給飛機的控制指令是否一致(手拿著飛機水平移動，看輸出是否符合預期)，避免造成控制指令的不匹配導致的飛機亂飛現象。

3.5 初始調試時應盡量選擇光照明亮，地面紋理豐富且不反光，無風的環境進行，避免引入環境的干擾因素。

附錄 與
// up_flow.h

#ifndef _up_flow_H

#define _up_flow_H

#include "stm32f4xx.h"

#include "ms5611.h"

void Flow_Init(void);

void Flow_Duty(float dT);

void Flow_Get(u8);

extern struct flow_float flow_dat;

extern float exp_rol_flow,exp_pit_flow;

struct flow_integral_frame {
unsigned short frame_count_since_last_readout;

signed short pixel_flow_x_integral;

signed short pixel_flow_y_integral;

signed short gyro_x_rate_integral;

signed short gyro_y_rate_integral;

signed short gyro_z_rate_integral;

unsigned int integration_timespan;

unsigned int sonar_timestamp;

unsigned short ground_distance;

signed short gyro_temperature;

unsigned char qual;

} ;

struct flow_float{
float x;

float y;

unsigned short dt;

unsigned char qual;

unsigned char update;

};#endif

// up_flow.c

#define LIMIT( x,min,max ) ( ((x) < (min)) ? (min) : ( ((x) > (max))? (max) : (x) ) )

#define LPF_1_(hz,t,in,out) ((out) += ( 1 / ( 1 + 1 / ( (hz) *3.14f *(t) ) ) ) *( (in) - (out) )) //一階低通濾波

#define safe_div(numerator,denominator,safe_value) ( (denominator == 0)? (safe_value) : ((numerator)/(denominator)) )

void filter_1(float base_hz,float gain_hz,float dT,float in,_filter_1_st *f1) //動態調整濾波截止頻率的一階濾波

{
LPF_1_(gain_hz,dT,(in - f1->out),f1->a); //低通后的變化量

f1->b = my_pow(in - f1->out); //求一個數平方函數

f1->e_nr = LIMIT(safe_div(my_pow(f1->a),((f1->b) + my_pow(f1->a)),0),0,1); //變化量的有效率，LIMIT 將該數限制在0-1之間，safe_div為安全除法

LPF_1_(base_hz *f1->e_nr,dT,in,f1->out); //低通跟蹤

}

_xyz_f_st heading_coordinate_acc;

_xyz_f_st heading_coordinate_speed_fus;

_xyz_f_st heading_coordinate_speed_err_i;

float fx_gyro_fix,fy_gyro_fix;

float fx_o,fy_o;

float f_out_x,f_out_y;

float gyro_lpf_x,gyro_lpf_y;

_filter_1_st f1_fx;

_filter_1_st f1_fy;

float f1_b,f1_g;

//flow_dat.qual：x，y方向光流都有效：255，x有效：2，y有效：1，都無效：0；

float gyro_x,gyro_y;

void flow_fusion(float dT,float fx,float fy,s32 flow_height,u8 pos_hold)

{
float flow_t1 = 1.0,flow_t2 = 1.0;

fx_o = fx *10 *flow_height; //fx，fy（rad/s）-->flow speed (mm/s)

fy_o = fy *10 *flow_height;

u32 use_height=100;

if(flow_height<200) ? ?//input height (cm/s)

{
use_height = 100;

}

else if(flow_height<300)

{
use_height = 150;

}

else if(flow_height<400)

{
use_height = 200;

}

else if(flow_height<500)

{
use_height = 250;

}

else if(flow_height<600)

{
use_height = 300;

}

else if(flow_height<1000)

{
use_height = 350;

}

else

{
use_height = 400;

}

if( pos_hold == 1) //in pose hold mode

{
gyro_y = (((s16)sensor.Gyro_deg.y/2 )*2 ); //degree per second

gyro_x = (((s16)sensor.Gyro_deg.x/2 )*2 );

LPF_1_(3.0f,dT,gyro_y,gyro_lpf_y); //gyro low pass filter (delay) for fitting flow data()

LPF_1_(3.0f,dT,gyro_x,gyro_lpf_x);

fx_gyro_fix = ((fx + LIMIT(((gyro_lpf_y)/57.3f),-flow_t1,flow_t1)) *10 *use_height ) ; //rotation compensation

fy_gyro_fix = ((fy - LIMIT(((gyro_lpf_x)/57.3f),-flow_t2,flow_t2)) *10 *use_height ) ;

vec_3d_transition(&imu_data.z_vec, &imu_data.a_acc, &heading_coordinate_acc); //機體坐標系下加速度到水平機體航向坐標系(將機體水平放置，沒有偏航角，

//保持加速度與光流沒有偏航方向的影響)

f1_fx.out += ((s32)heading_coordinate_acc.x/10 *10 ) *dT; //integrated acceleration for speed

f1_fy.out += ((s32)heading_coordinate_acc.y/10 *10 ) *dT;

f1_g = 2.5f;

if(flow_dat.qual <3) ?//光流效果不好時

{
f1_b = 0.5f;

}

else

{
if(f1_b<1.2f)

{
f1_b += 0.02f;

}

}

filter_1(f1_b,f1_g,dT,fx_gyro_fix,&f1_fx); //flow_data with acc integrated data complementary filtering

filter_1(f1_b,f1_g,dT,fy_gyro_fix,&f1_fy);

heading_coordinate_speed_fus.x = f1_fx.out; //融合速度

heading_coordinate_speed_fus.y = f1_fy.out;

f_out_x = heading_coordinate_speed_fus.x + 0.1f *heading_coordinate_speed_err_i.x; //融合速度二次修正，最終輸出結果

f_out_y = heading_coordinate_speed_fus.y + 0.1f *heading_coordinate_speed_err_i.y;

heading_coordinate_speed_err_i.x += (fx_gyro_fix - f_out_x) *dT;

heading_coordinate_speed_err_i.y += (fy_gyro_fix - f_out_y) *dT;

}

}

審核編輯 黃宇