无人机—加速度计与光流数据融合

加速度计与光流数据融合

       无人机的定点定高飞行时实现目标识别追踪，航点规划，循迹巡线等下一个任务的基础，稳定的定高定点飞行才能执行其他高级算法。在室内没有GPS的情况下，要想实现这个目标，最最简单的就是利用光流传感器了。

计算光流线速度和位置

---

       市场的光流也都大同小异，有的带有超声波，有的不带，但是经过厂家的各种所谓的优化高深算法，输出的无非两个值最为关键，并且基本上所有的厂家都会有：

pixel_flow_x_sum;    // latest x flow measurement in pixels*10 [pixels]
pixel_flow_y_sum;  // latest y flow measurement in pixels*10  [pixels]

       即在X、Y轴方向一定时间内累计的像素点。为了保证精度，厂家一般在内部的程序中，该值都乘以了10000.0f
       因为需要的是X、Y的线速度，所以换算成线速度为

speed_x = (High * pixel_flow_x_sum/10000.0f / Timespan) * 100; //cm/s
speed_y = (High * pixel_flow_y_sum/10000.0f / Timespan) * 100; //cm/s

       其中，High是超声波或者气压计计算出来的高度，单位m。因为这里传感器输出的是角速度，需要乘以高度才变成线速度。Timespan是传感器更新一次数据需要的时间，单位s。乘以100就换算成cm/s。
       得到光流X、Y轴方向的速度之后，通过积分便可以得到位置

sum_x += speed_x * Time_period; //cm
sum_y += speed_y * Time_period; //cm

       其中speed_x和speed_y为上一步求取的线速度，单位cm/s。Time_period为运行的时间周期，单位为s。

无人机位置控制和速度控制

---

       得到了光流传感器X、Y轴的线速度，如果将光流传感器固定到无人机底部，那就可以得到近似无人机X、Y轴的线速度，如果假设X轴与ROLL方向重合，Y轴与PITCH轴重合，那就可以利用这两个方向的线速度控制无人机定点飞行。
位置控制PID(实际用P即可)
期望值 = 输入（默认为0）
反馈值 = 现在无人机偏离原点的位置（sum_x/sum_y）
误差 = 期望值 - 反馈值
输出 = Kp * 误差
速度控制PID（实际用PI即可）
期望值 = 输入（位置控制输出）
反馈值 = 光流传感器测量值（speed_x/speed_y）
误差 = 期望值 - 反馈值
输出 = Kp * 误差 + Ki * 误差积分
       最后需要控制无人机的姿态，则需要将速度控制的输出转换为姿态角度控制PID的输入即可。简单的可以进行线性变换过去，实际效果也挺好。通过调试，经过这一步，实现定点控制应该问题不大。但要实现更好的效果则需要光流数据与加速度计数据进行数据融合，让测量的数据更加准确。

加速度计数据与光流数据融合

---

       数据融合之前，需要陀螺仪对光流数据的旋转补偿，达到的效果就是飞机原地绕Pitch和Roll小幅度的旋转，光流数据基本不会太大的波动。因为飞机的位置没动，只是姿态有小范围的抖动，这个时候光流也会检测到数据变化，但事实这个是我们所不希望的。陀螺仪能够检测飞机的角速度，而光流也能够检测出飞机在Pitch和Roll方向的角速度，正好可以利用陀螺仪来进行补偿。

Flow_Decode(&Flow_Buffer[0]);	  /* 光流数据解析 */
TimePeriod(&flow_Delta); /*得到这段代码运行时间 ms */
Flow_Delta_T = Flow_Delta.Time_Delta/1000.0f;  /* 得到周期时间 */
//得到陀螺仪数据 角速度 单位为弧度
gyro_y = imu.gyroRaw[1]*180.0f/M_PI_F;    //gyro[1]  pitch  光流Y轴
gyro_x = imu.gyroRaw[0]*180.0f/M_PI_F;	  //gyro[0]  roll   光流X轴
//先给陀螺仪滤波
LPF_1_(8.0f,Flow_Delta_T ,gyro_y,gyro_lpf_y);     //gyro low pass filter (delay) for fitting flow data()
LPF_1_(8.0f,Flow_Delta_T ,gyro_x,gyro_lpf_x);   /*  8.0需要自己尝试，保证光流数值和陀螺仪数值相位相差不多 */

//对光流原始数据进行滤波
LPF_1_(30.0f,Flow_Delta_T ,flow_dat.x,flow_dat.x);  /*对光流数据进行简单滤波，去除一点毛刺 */   
LPF_1_(30.0f,Flow_Delta_T ,flow_dat.y,flow_dat.y);   
//速度计算方法：(H * flow_dat.x / T) * 100   H为高度，单位m; T为间隔时间,单位s，100是m->cm的转换
UPflow_speed_x =  flow_dat.x / Flow_Delta_T ;  //速度 rad/s 
UPflow_speed_y =  flow_dat.y / Flow_Delta_T ;  //速度 rad/s
/* --------------------利用陀螺仪对光流进行补偿，保证在原地旋转时，光流输出几乎为 0 -----------------*/
/* 1.105和1.101系数需要自己一个一个试，用来抵消低通滤波带来的幅值减小 */
UPflow_speed_x = US100_Distance * (UPflow_speed_x  -  1.105 * LIMIT(((gyro_lpf_x)/57.295779f),-flow_t2,flow_t2)) * 100.0f;   //速度 cm/s
UPflow_speed_y = US100_Distance * (UPflow_speed_y  +  1.101 * LIMIT(((gyro_lpf_y)/57.295779f),-flow_t1,flow_t1)) * 100.0f  ; 

       最后得到的UPflow_speed_x 和UPflow_speed_y是经过陀螺仪补偿之后的，数据会更加完美。自己调试的时候一定需要结合地面站来看波形调试数据，否则肯定不行的，相位和幅值对不好的，效果反而不好。
       其中的Flow_Decode、LPF_1_、LIMIT函数都是参考优象光流模块的资料，优象模块链接。

数据融合

---

       做完上面的步骤，就可以进行数据融合了。融合之前还需要注意坐标系的转换。加速度计测量到是机体坐标系的，是以飞机的Pitch、Roll为轴的坐标系，同时，这个Pitch和Roll是时时刻刻变化的，但是光流测量的是飞机相对大地的Pitch、Roll方向的。这是两个不同的坐标系，前者是机体坐标系，后者为航向坐标系（不知道准不准）。航向坐标系，Yaw = 0的坐标系，具体转换如下：

void Vector_From_BodyFrame2HeadFrame(Vector3f *bf,Vector3f *ef)
{
/*从机体坐标系转换到航向坐标系*/
ef->x= Cos_Pitch*bf->x + Sin_Pitch*Sin_Roll*bf->y + Sin_Pitch*Cos_Roll*bf->z;  
ef->y= 0 *bf->x + Cos_Roll*bf->y + (-1)* Sin_Roll*bf->z;
ef->z=(-1)*Sin_Pitch*bf->x + Cos_Pitch*Sin_Roll*bf->y+  Cos_Pitch*Cos_Roll*bf->z;
}

       通过坐标系转换的加速度计数据便可以与光流数据进行数据融合了。（未完待续 。。）