大家好我是Jones,写博客记录一下工作的痕迹，同时也对工作做一个总结，才疏学浅，难免会有很多纰漏，还请大家批评指正!
第1篇：仿真工具介绍
第2篇：02RTL返航模式优化
第3篇：室内定点之光流一：PID位置控制
第4篇：室内定点之光流二：EKF融合
第5篇：GAZEBO仿真
无人机在室外飞行时通常我们采用GPS定位，一旦我们飞入室内等GPS有遮挡的环境时，GPS信号就受到了干扰，更新位置会失效。我们想办法让无人机在室内也能够“撒手”飞行，我们今天使用光流的采集无人机移动的速度，进行位移估算，通过速度和位移从而达到定点。
一开始我们采夠了一个PX4Flow光流模块进行测试，首先进行桌面测试时就出现噪声大，数据跳变的问题，再进一步进行调焦校准等操作数据表现上稍微好一些进行装机测试，定点很不稳定。当然之前公司测试时用这个模块也有能实现定点的情况，这与这个模块厂家和运气有很大关系。超声标称是0.3-4.5m，但实际测试1.5m以上就出现很多噪声了。建议大家就不要浪费前和时间在这个经常被吐槽的模块上了。图片如下
后续通过调研找到了一款体积小，功耗低，精度高，且价格非常便宜的优象光流模块，型号LC-302，顺便附上模块链接：http://www.upixels.com/hnyx2019/vip_doc/15917119.html
点击跳转->优象光流
首先通过实现桌面测试，发现该模块精度果然很高，且数据非常稳定，基本上没有跳点和跳变的情况。安装到无人机上时需要确立好坐标，把光流相机OK或是看作画的小人图像对准飞机机头方向。
一开使用是气压计定高，大家也知道，如果室外的话使用气压计高度是会飘的，且容易受到气流的扰动，一旦飞行高度出现波动，对于光流的计算就会有影响，导致室内定点水平精度差。调研到北醒一款小型的激光雷达有效距离能到12m高，于是买回来试试，亲测12m有效，注意地面反光会有过饱和现象测距不准，还和北醒的杨总成了朋友，报我的名字有优惠/偷笑/，链接：http://www.benewake.com/product/detail/5c345cd0e5b3a844c472329b.html
点击跳转->北醒激光雷达
北醒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);
}
...
高度信息即可订阅该消息用于精度较高的室内飞行了。
找到帧头读取数据：
...
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闭环处理，测试定点效果还可以，但出现扰动，无人机位置出现偏移后，该方法将会把新的位置作为基准进行闭环，这样无人机将不会回到最初想设定的位置，存在此不足，下一篇介绍光流融合的方式进行室内定点，并附上视频，那我们下期见啦！