PX4无人机飞控开发——第4篇：室内定点之光流二

PX4无人机飞控开发——第4篇：室内定点之光流二

大家好我是Jones,写博客记录一下工作的痕迹，同时也对工作做一个总结，才疏学浅，难免会有很多纰漏，还请大家批评指正!

PX4无人机飞控开发系列

第1篇：仿真工具介绍
第2篇：02RTL返航模式优化
第3篇：室内定点之光流一：PID位置控制
第4篇：室内定点之光流二：EKF融合
第5篇：GAZEBO仿真

前言

无人机在室外飞行时通常我们采用GPS定位，一旦我们飞入室内等GPS有遮挡的环境时，GPS信号就受到了干扰，更新位置会失效。我们想办法让无人机在室内也能够“撒手”飞行，我们今天使用光流的采集无人机移动的速度，进行位移估算，通过速度和位移计算从而达到定点。

一、模块对比

省略，见第3篇：室内定点之光流一

二、方案实现

1.激光雷达定高

省略，见第3篇：室内定点之光流一

2.读取光流数据

订阅TF高度信息：

...
struct distance_sensor_s tfmini;
orb_copy(ORB_ID(distance_sensor), _distance_sensor_sub , &tfmini);
  ...

速度计算方法：(1.0*flow_dat.y/0.04)*100 1.0为高度，单位m; 0.04为间隔时间,单位s，100是m->cm的转换：

...
speed_x = flow_x * 2500.0f * ((int)tfmini.current_distance < 6 ? ((int)tfmini.current_distance <4 ? 2 : 2.5):3);//m-->cm  //1.0f*flow_x/(integration_timespan* 1e-6f);  2m
             speed_y = flow_y * 2500.0f * ((int)tfmini.current_distance < 6 ? ((int)tfmini.current_distance <4 ? 2 : 2.5):3);//m-->cm  //1.0f*flow_y/(integration_timespan* 1e-6f);  2m
 //对速度进行限幅
 speed_x = LIMIT(speed_x, -62.0f, 62.0f);//速度限制在-62cm/s到62cm/s之间
 speed_y = LIMIT(speed_y, -62.0f, 62.0f);

 sum_flow_x += speed_x*0.04f; //位移 cm  //speed_x *(integration_timespan *1e-6f);
 sum_flow_y += speed_y*0.04f; //位移 cm  //speed_y *(integration_timespan *1e-6f);

if(sum_flow_x > POS_CONTROL_LIMIT_MAX|| sum_flow_x < POS_CONTROL_LIMIT_MIN || qual < 245)
        sum_flow_x = 0.0f;
if(sum_flow_y > POS_CONTROL_LIMIT_MAX|| sum_flow_y < POS_CONTROL_LIMIT_MIN || qual < 245)
        sum_flow_y = 0.0f;
  ...

UORB发布光流消息

...
struct optical_flow_s flow;
memset(&flow, 0, sizeof(flow));

flow.timestamp = hrt_absolute_time();
flow.pixel_flow_x_integral = -(float)pixel_flow_y_integral/10000;//对齐方向
flow.pixel_flow_y_integral = (float)pixel_flow_x_integral/10000;

if(flow_pub == NULL)
	{
	   flow_pub = orb_advertise(ORB_ID(optical_flow), &flow);
	}
else
	{
	   orb_publish(ORB_ID(optical_flow), flow_pub, &flow);
	}
..

接着我们在参数设置里将FLOW_EN = 1使能，我们就可以用到EKF融合的位置，进行室内定点飞行了，“撒手”解放双手，提高飞行的安全。且在遥控器断链后依然可以安全进行降落。

三、飞行测试

1.室内光流定点测试

点击跳转->B站：PX4无人机飞控开发_03室内光流定点测试

2.室内遥控器断链自动降落测试

点击跳转->B站：PX4无人机飞控开发_04室内光流定点遥控器断链

3.室内光流定点，系留线对光流的影响测试

点击跳转->B站：PX4无人机飞控开发_05测试线缆对光流定点的影响

总结

第3篇：室内定点之光流一，采用了位置误差作为反馈值进行了位置PID闭环处理，测试定点效果还可以，但出现扰动，无人机位置出现偏移后，该方法将会把新的位置作为基准进行闭环，这样无人机将不会回到最初想设定的位置，存在此不足。
该篇章介绍光流融合的方式进行室内定点，我们采用了取巧的方式，将优象光流数据替换了PX4FLOW是光流数据，EKF融合直接订阅即可，不用修改EKF的融合代码，用最小的修改实现最大的飞行需求，且避免bug,并附上飞行视频，那我们下期见啦！