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

digitech

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

PX4无人机飞控开发系列：
第1篇：仿真工具介绍
第2篇：02RTL返航模式优化
第3篇：室内定点之光流一：PID位置控制
第4篇：室内定点之光流二：EKF融合
第5篇：GAZEBO仿真
<hr/>前言

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

1.PX4Flow光流模块

一开始我们采夠了一个PX4Flow光流模块进行测试，首先进行桌面测试时就出现噪声大，数据跳变的问题，再进一步进行调焦校准等操作数据表现上稍微好一些进行装机测试，定点很不稳定。当然之前公司测试时用这个模块也有能实现定点的情况，这与这个模块厂家和运气有很大关系。超声标称是0.3-4.5m，但实际测试1.5m以上就出现很多噪声了。建议大家就不要浪费前和时间在这个经常被吐槽的模块上了。图片如下


2.优象光流模块

后续通过调研找到了一款体积小，功耗低，精度高，且价格非常便宜的优象光流模块，型号LC-302，顺便附上模块链接：
点击跳转->光流模块_优象科技有限公司



首先通过实现桌面测试，发现该模块精度果然很高，且数据非常稳定，基本上没有跳点和跳变的情况。安装到无人机上时需要确立好坐标，把光流相机OK或是看作画的小人图像对准飞机机头方向。



3.北醒激光雷达

一开使用是气压计定高，大家也知道，如果室外的话使用气压计高度是会飘的，且容易受到气流的扰动，一旦飞行高度出现波动，对于光流的计算就会有影响，导致室内定点水平精度差。调研到北醒一款小型的激光雷达有效距离能到12m高，于是买回来试试，亲测12m有效，注意地面反光会有过饱和现象测距不准，还和北醒的杨总成了朋友，报我的名字有优惠/偷笑/，链接：
点击跳转->北醒激光雷达


二、方案实现

1.激光雷达定高

北醒TFmini Plus 有两种通信方式I2C/USART，默认是I2C，可参考tfmini.cpp进行修改。

TF数据读取：

#define TFminiPlus_TAKE_RANGE_REG        0x51                /* Measure range Register */
#define TFminiPlus_SET_ADDRESS_1        0xAA                /* Change address 1 Register */
#define TFminiPlus_SET_ADDRESS_2        0xA5                /* Change address 2 Register */

/* Device limits */
#define TFminiPlus_MIN_DISTANCE         (0.20f)
#define TFminiPlus_MAX_DISTANCE         (11.00f)

#define TFminiPlus_CONVERSION_INTERVAL         100000 /* 60ms for one sonar */
#define TICKS_BETWEEN_SUCCESIVE_FIRES         100000 /* 30ms between each sonar measurement

UORB发布高度距离信息：

uint16_t distance_cm = val[2]  | val[3]<< 8;
float distance_m = float(distance_cm) * 1e-2f;

struct distance_sensor_s report;
report.timestamp = hrt_absolute_time();
report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
report.orientation = _rotation;
report.current_distance = distance_m;
report.min_distance = get_minimum_distance();
report.max_distance = get_maximum_distance();
report.variance = 0.0f;
report.signal_quality = val[4] | val[5]<< 8;
report.temp = (val[6] | val[7]<< 8) / 8 - 256;

/* TODO: set proper ID */
report.id = 0;

/* publish it, if we are the primary */
if (_distance_sensor_topic != nullptr) {
        orb_publish(ORB_ID(distance_sensor), _distance_sensor_topic, &report);
    }

高度信息即可订阅该消息用于精度较高的室内飞行了。

2.读取光流数据

找到帧头读取数据：

else if(flow_buff[read_finish+1] == 0X0A &&flow_buff[read_finish+13] == 0X55){
  memcpy(flow_data, flow_buff+read_finish,30);
  //printf("flow_data1:\n%s\n", flow_data);
  ground_distance = flow_data[6+2]+(flow_data[7+2]<<8);
  pixel_flow_x_integral = flow_data[0+2] + (flow_data[1+2]<<8);;
  pixel_flow_y_integral = flow_data[2+2] + (flow_data[3+2]<<8);
  integration_timespan = flow_data[4+2]+ (flow_data[5+2]<<8); //+ (flow_data[14+2]<<16) + (flow_data[15+2]<<24);
  gyro_temperature = flow_data[9+2];
  qual = flow_data[8+2];

PID控制：

/*悬停控制*/
static void PositionPID(float sum_x, float sum_y)
{
        static float lastVxErro,lastVyErro;
        static float pidVx_pOut,pidVx_dOut,pidVx_iOut;
        static float pidVy_pOut,pidVy_dOut,pidVy_iOut;
        static unsigned char flag_Y,flag_X;
        /***************X轴PID参数**ROLL************/
        float Vxkp=0.087f;//
        float Vxki=0.00052f;//0.001f;
        float Vxkd=0.0252f;//-0.000531;

        /***************Y轴PID参数*PITCH*************/
        float Vykp=0.088f;
        float Vyki=0.0005f;//0.001f;
        float Vykd=0.025f;

        /*X轴位移速度调整*/
        //float vxErro=(float)(0.0f-(-pixX*hight/100));
        float vxErro= sum_x;
        float vxErroDelta=(vxErro-lastVxErro)/0.016f;
        lastVxErro=vxErro;
        /*X轴积分分离处理*/
        if(vxErro <= 50.0f&&vxErro >= -50.0f)
        {
                flag_X = 0;
        }
        else
        {
                flag_X = 1;
        }
        pidVx_pOut=Vxkp * vxErro;
        pidVx_dOut=Vxkd * vxErroDelta;
        pidVx_iOut+=Vxki * vxErro;
        if(pidVx_iOut>2.5f)//1.5
                pidVx_iOut=2.5f;
        if(pidVx_iOut<-2.5f)
                pidVx_iOut=-2.5f;

        pidVx_value=pidVx_pOut+pidVx_dOut+flag_X*pidVx_iOut;

         /*** 归一化 ***/
        pidVx_value = pidVx_value * 22.0 /500.0 ;
        //pidVx_value = pidVx_value * 0.08f;
        pidVx_value = LIMIT(pidVx_value, -1.0f, 1.0f);

y轴类似以上代码。

总结

前期采用了位置误差作为反馈值进行了位置PID闭环处理，测试定点效果还可以，但出现扰动，无人机位置出现偏移后，该方法将会把新的位置作为基准进行闭环，这样无人机将不会回到最初想设定的位置，存在此不足，下一篇介绍光流融合的方式进行室内定点，并附上视频，那我们下期见啦！