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

無(wú)人機(jī)的飛控端從U30接收到光流數(shù)據(jù)后，接下來(lái)就是如何實(shí)現(xiàn)對(duì)于飛行的控制，即估計(jì)飛機(jī)真實(shí)的水平運(yùn)動(dòng)速度。以光流懸停為例，光流是如何抑制飛機(jī)懸停時(shí)的漂移的呢？一般來(lái)說(shuō)，飛機(jī)在無(wú)光流時(shí)產(chǎn)生隨機(jī)漂移的原因是姿態(tài)角度的誤差造成的，使得飛機(jī)在自身有傾角時(shí)而錯(cuò)誤的認(rèn)為自身是水平而導(dǎo)致的，由于多旋翼是欠驅(qū)動(dòng)系統(tǒng)，有傾角從而產(chǎn)生該方向上的漂移速度，因此光流就是估計(jì)這個(gè)誤差角度或漂移速度，并反饋給控制系統(tǒng)進(jìn)行控制(一般是串級(jí)的PID控制)，控制輸出的結(jié)果一般是飛機(jī)的傾斜角度，即期望角度。

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

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

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

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

1.1 速度計(jì)算方法：

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

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

其中h為測(cè)量高度，單位m; integration_timespan為相鄰兩個(gè)光流輸出的間隔時(shí)間,單位us，100是m->cm的轉(zhuǎn)換。

1.2 位移計(jì)算方法：

sum_flow_x += speed_x*(integration_timespan*0.000001);

sum_flow_y += speed_y*(integration_timespan*0.000001);

該位移默認(rèn)開(kāi)始懸停點(diǎn)為零，計(jì)算的是后續(xù)累積時(shí)間內(nèi)，飛機(jī)距離該初始位置點(diǎn)的水平位移，單位為cm。

圖1 光流數(shù)據(jù)處理與控制流程圖

1.3 備注：

1.3.1 由于計(jì)算的速度幀率不是很高，且?guī)g的速度信息可能差別比較大，因此可以根據(jù)控制需要，選擇對(duì)計(jì)算的速度進(jìn)行低通濾波，但不宜引入過(guò)大的延時(shí)。

1.3.2 理論上在速度計(jì)算之前需要進(jìn)行旋轉(zhuǎn)補(bǔ)償，但在初始測(cè)試時(shí)，可以暫時(shí)先不考慮，先跑通整個(gè)框架。如果懸停效果可以接受也不用進(jìn)行旋轉(zhuǎn)補(bǔ)償；若懸停時(shí)低頻震蕩且PID參數(shù)怎么也調(diào)試不好，就可能需要進(jìn)行補(bǔ)償，具體的旋轉(zhuǎn)補(bǔ)償方法在2.2節(jié)中再做詳細(xì)敘述。

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

1.3.4 在只使用氣壓計(jì)進(jìn)行定高而沒(méi)有超聲波時(shí)，由于氣壓計(jì)會(huì)有很大漂移，因此輸出的高度要做限制。首先限制高度要非負(fù)，另外在較低高度時(shí)可以使用固定高度，我們?cè)跍y(cè)試時(shí)一般將高度寫死固定為1m，實(shí)際測(cè)試發(fā)現(xiàn)在實(shí)際高度5m以下，使用該寫死的高度也能有較好的懸停效果；當(dāng)實(shí)際高度比較高時(shí)，可以適當(dāng)放大該固定高度比例。

光流與加速度計(jì)融合的處理方法
一般單獨(dú)使用光流的數(shù)據(jù)也能夠獲取較好的靜態(tài)懸停效果，如果對(duì)光流的動(dòng)態(tài)性能也有較高要求，比如實(shí)現(xiàn)更好的抗風(fēng)、位移控制、目標(biāo)跟隨等高級(jí)功能，則需要進(jìn)行光流與加速度計(jì)數(shù)據(jù)的融合以獲取更加精確、平滑與實(shí)時(shí)的數(shù)據(jù)。具體實(shí)現(xiàn)可以參考，下面對(duì)該文件進(jìn)行簡(jiǎn)單梳理，并給出計(jì)算過(guò)程。

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

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

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

補(bǔ)償公式為：

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方向的補(bǔ)償為例，其中，fx_gyro_fix為旋轉(zhuǎn)補(bǔ)償后的輸出，單位是mm/s;fx為光流角速度；(10 *use_height)為使用的高度值，單位為mm。

(gyro_lpf_y)/57.3為陀螺儀輸出角速度，單位rad/s，flow_t1為對(duì)陀螺儀輸出的限幅，值的大小由fx的輸出最大值決定，因?yàn)橥勇輧x的輸出是遠(yuǎn)遠(yuǎn)大于光流的輸出范圍的，限幅避免過(guò)度補(bǔ)償。gyro_lpf_y為原始的陀螺儀角速度(度/秒)，經(jīng)低通濾波后所得：LPF_1_(3.0f,dT,gyro_x,gyro_lpf_x)，目的是使得光流輸出的相位與陀螺儀相位一致。

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

第五步：加速度積分獲取速度，并與光流進(jìn)行互補(bǔ)濾波,獲取初步的融合速度f(wàn)1_fx.out。

第六步：融合速度修正，由于加速度計(jì)積分有漂移，因此可能造成融合的速度在懸?；蜻\(yùn)動(dòng)中會(huì)漂移真實(shí)值，而光流的計(jì)算是沒(méi)有漂移及累積誤差的，因此以光流數(shù)據(jù)做參考，對(duì)f1_fx.out做一個(gè)二次修正，得到最終融合速度輸出f_out_x。

備注：

本算法中只對(duì)融合速度進(jìn)行了處理，而沒(méi)有融合高度的處理。因?yàn)樵跍y(cè)試中發(fā)現(xiàn)使用融合速度積分得到位移精度反而不如單純的光流累積計(jì)算精度高，因此建議位移的獲取仍采用2.1.2中位移的獲取方法。

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

2.1 對(duì)于位置環(huán)，期望位置為零，反饋值為光流測(cè)量位移，輸出為期望速度。

2.2 對(duì)于速度環(huán)，期望值為位置環(huán)的輸出，反饋值為光流測(cè)量速度，速度環(huán)輸出為期望角度或期望加速度。理論上，輸出應(yīng)為期望加速度，并通過(guò)非線性轉(zhuǎn)換為期望角度，從而傳遞給姿態(tài)環(huán)。在demo中我們的輸出直接是期望角度，這是因?yàn)樵谛〗嵌葧r(shí)，我們認(rèn)為期望加速度與角度是線性相關(guān)的。

2.3 定點(diǎn)控制中比較重要的一點(diǎn)，要確保外環(huán)的執(zhí)行頻率要低于內(nèi)環(huán)的執(zhí)行頻率，否則會(huì)造成系統(tǒng)的不穩(wěn)定。比如，我們位置環(huán)的控制周期為40ms,則速度環(huán)的控制周期為20ms等等。

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

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

3.2 光流模塊的安裝。在飛機(jī)上安裝光流模塊時(shí)，盡量保證鏡頭的水平，不要有傾斜角度，光流模塊與機(jī)體坐標(biāo)系不要有偏航方向的夾角且在機(jī)架的旋轉(zhuǎn)中心，即光流模塊的軸與機(jī)體坐標(biāo)系的軸應(yīng)盡量保持重合。特別注意一旦調(diào)試通過(guò)，進(jìn)入批量生產(chǎn)時(shí)，在其他飛機(jī)上都需要以該方向固定安裝，不可隨意改變安裝方向。

3.3初始調(diào)試時(shí)，接上光流后，使用上位機(jī)觀察是否有輸出，驗(yàn)證輸出值是否與圖2.所述的光流坐標(biāo)系定義一致，比如往左水平運(yùn)動(dòng)光流Y是否為正；水平向右運(yùn)動(dòng)光流Y是否為負(fù)，且輸出值是否在合理范圍等。

3.4 確保光流的輸出與機(jī)體控制坐標(biāo)系的一致性，即實(shí)飛測(cè)試前需要驗(yàn)證光流的控制輸出值與期望給飛機(jī)的控制指令是否一致(手拿著飛機(jī)水平移動(dòng)，看輸出是否符合預(yù)期)，避免造成控制指令的不匹配導(dǎo)致的飛機(jī)亂飛現(xiàn)象。

3.5 初始調(diào)試時(shí)應(yīng)盡量選擇光照明亮，地面紋理豐富且不反光，無(wú)風(fēng)的環(huán)境進(jìn)行，避免引入環(huán)境的干擾因素。

附錄 與
// 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) //動(dòng)態(tài)調(diào)整濾波截止頻率的一階濾波

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

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

f1->e_nr = LIMIT(safe_div(my_pow(f1->a),((f1->b) + my_pow(f1->a)),0),0,1); //變化量的有效率，LIMIT 將該數(shù)限制在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，都無(wú)效：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); //機(jī)體坐標(biāo)系下加速度到水平機(jī)體航向坐標(biāo)系(將機(jī)體水平放置，沒(méi)有偏航角，

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

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) ?//光流效果不好時(shí)

{
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; //融合速度二次修正，最終輸出結(jié)果

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;

}

}

審核編輯 黃宇