pixhawk position_estimator_inav.cpp再分析

此篇blog前已经分析inav的数据处理流程(详情请看pixhawk position_estimator_inav.cpp思路整理及数据流)，但是没有仔细分析里面的程序思想，感觉里面还是有很多地方值得咀嚼，看懂部分，还有些地方不懂，先记下了。

1.矫正思想---类卡尔曼

更正：inav可理解为观测量反馈算法，与加速度计观测反馈陀螺仪以得到准确姿态信息类似，其流程如下图(2017.03.27)

此处权值的求取的流程是

光流

float flow_q = flow.quality / 255.0f;
float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min);
w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist);
params.w_xy_flow * w_flow

GPS

//权值的产生
//gps.cpp发布
orb_publish_auto(ORB_ID(vehicle_gps_position), &_report_gps_pos_pub, &_report_gps_pos, &_gps_orb_instance,ORB_PRIO_DEFAULT);
//inav.cpp处理
orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph);
w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv);
w_xy_gps_p = params.w_xy_gps_p * w_gps_xy;
w_xy_gps_v = params.w_xy_gps_v * w_gps_xy;

eph、epv的处理(这个不大清楚)

static const float min_eph_epv = 2.0f;  // min EPH/EPV, used for weight calculation
static const float max_eph_epv = 20.0f;    // max EPH/EPV acceptable for estimation
float eph = max_eph_epv;
float epv = 1.0f;
if (gps.eph > max_eph_epv || gps.epv > max_eph_epv || gps.fix_type < 3)
{mavlink_log_info(&mavlink_log_pub, "[inav] GPS signal lost");}
if (gps.eph < max_eph_epv * 0.7f && gps.epv < max_eph_epv * 0.7f && gps.fix_type >= 3)
mavlink_log_info(&mavlink_log_pub, "[inav] GPS signal found");
w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph);
w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv);
/* increase EPH/EPV on each step */
if (eph < 0.000001f) { //get case where eph is 0 -> would stay 0eph = 0.001;
}
if (eph < max_eph_epv) {eph *= 1.0f + dt;
}
if (epv < 0.000001f) { //get case where epv is 0 -> would stay 0epv = 0.001;
}
if (epv < max_eph_epv) {epv += 0.005f * dt;    // add 1m to EPV each 200s (baro drift)
}
epv = fminf(epv, gps.epv);
eph = fminf(eph, gps.eph);
local_pos.eph = eph;
local_pos.epv = epv;
global_pos.eph = eph;
global_pos.epv = epv;

2.光流模块传来信息的处理

         /* optical flow */orb_check(optical_flow_sub, &updated);if (updated && lidar_valid) {orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow);flow_time = t;float flow_q = flow.quality / 255.0f;//0<flow.quality<255//qual = (uint8_t)(meancount * 255 / (NUM_BLOCKS*NUM_BLOCKS));//每2帧图像匹配块的数量float dist_bottom = lidar.current_distance;//超声波测得的距离//离地面距离>20cm&&flow.quality>某个值&&PX4_R(att.R, 2, 2) > 0.7f(不知道什么意思)if (dist_bottom > flow_min_dist && flow_q > params.flow_q_min && PX4_R(att.R, 2, 2) > 0.7f) {/* distance to surface *///float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); //use this if using sonarfloat flow_dist = dist_bottom; //use this if using lidar/* check if flow if too large for accurate measurements *//* calculate estimated velocity in body frame */float body_v_est[2] = { 0.0f, 0.0f };for (int i = 0; i < 2; i++) {body_v_est[i] = PX4_R(att.R, 0, i) * x_est[1] + PX4_R(att.R, 1, i) * y_est[1] + PX4_R(att.R, 2, i) * z_est[1];/* 旋转矩阵第1,2列*北东地xyz轴的速度=机体xy轴方向的速度* x_est[]是矫正之后的速度*/}/* set this flag if flow should be accurate according to current velocity and attitude rate estimate */flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow &&fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow;/* flow_accurate判断光流精度能否使用* xy轴机体速度/距离=机体角速度* 机体角速度-飞控测得的角速度<某个阈值，表明精度可用*//*calculate offset of flow-gyro using already calibrated gyro from autopilot*/flow_gyrospeed[0] = flow.gyro_x_rate_integral / (float)flow.integration_timespan * 1000000.0f;flow_gyrospeed[1] = flow.gyro_y_rate_integral / (float)flow.integration_timespan * 1000000.0f;flow_gyrospeed[2] = flow.gyro_z_rate_integral / (float)flow.integration_timespan * 1000000.0f;//moving average//滑动均值滤波if (n_flow >= 100) {gyro_offset_filtered[0] = flow_gyrospeed_filtered[0] - att_gyrospeed_filtered[0];gyro_offset_filtered[1] = flow_gyrospeed_filtered[1] - att_gyrospeed_filtered[1];gyro_offset_filtered[2] = flow_gyrospeed_filtered[2] - att_gyrospeed_filtered[2];n_flow = 0;flow_gyrospeed_filtered[0] = 0.0f;flow_gyrospeed_filtered[1] = 0.0f;flow_gyrospeed_filtered[2] = 0.0f;att_gyrospeed_filtered[0] = 0.0f;att_gyrospeed_filtered[1] = 0.0f;att_gyrospeed_filtered[2] = 0.0f;} else {flow_gyrospeed_filtered[0] = (flow_gyrospeed[0] + n_flow * flow_gyrospeed_filtered[0]) / (n_flow + 1);flow_gyrospeed_filtered[1] = (flow_gyrospeed[1] + n_flow * flow_gyrospeed_filtered[1]) / (n_flow + 1);flow_gyrospeed_filtered[2] = (flow_gyrospeed[2] + n_flow * flow_gyrospeed_filtered[2]) / (n_flow + 1);att_gyrospeed_filtered[0] = (att.pitchspeed + n_flow * att_gyrospeed_filtered[0]) / (n_flow + 1);att_gyrospeed_filtered[1] = (att.rollspeed + n_flow * att_gyrospeed_filtered[1]) / (n_flow + 1);att_gyrospeed_filtered[2] = (att.yawspeed + n_flow * att_gyrospeed_filtered[2]) / (n_flow + 1);n_flow++;}/*yaw compensation (flow sensor is not in center of rotation) -> params in QGC*///yaw方向补偿yaw_comp[0] = - params.flow_module_offset_y * (flow_gyrospeed[2] - gyro_offset_filtered[2]);yaw_comp[1] = params.flow_module_offset_x * (flow_gyrospeed[2] - gyro_offset_filtered[2]);/* flow_gyrospeed[2]光流模块上测得的z轴角速度* gyro_offset_filtered[2]光流模块上测得的z轴角速度平均值-飞控测得的z轴角速度平均值* flow_gyrospeed[2]-gyro_offset_filtered[2]=飞控的z轴角速度*//* convert raw flow to angular flow (rad/s) *///将光流转换成弧度float flow_ang[2];/* check for vehicle rates setpoint - it below threshold -> dont subtract -> better hover */orb_check(vehicle_rate_sp_sub, &updated);if (updated)orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rate_sp_sub, &rates_setpoint);double rate_threshold = 0.15f;//pitch方向的角速度<某个阈值，不用pitch角度的补偿if (fabs(rates_setpoint.pitch) < rate_threshold) {//warnx("[inav] test ohne comp");flow_ang[0] = (flow.pixel_flow_x_integral / (float)flow.integration_timespan * 1000000.0f) * params.flow_k;//for now the flow has to be scaled (to small)}//pitch方向的角速度>某个阈值，需要pitch角度的补偿else {//warnx("[inav] test mit comp");//calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro)flow_ang[0] = ((flow.pixel_flow_x_integral - flow.gyro_x_rate_integral) / (float)flow.integration_timespan * 1000000.0f+ gyro_offset_filtered[0]) * params.flow_k;//for now the flow has to be scaled (to small)}if (fabs(rates_setpoint.roll) < rate_threshold) {flow_ang[1] = (flow.pixel_flow_y_integral / (float)flow.integration_timespan * 1000000.0f) * params.flow_k;//for now the flow has to be scaled (to small)}/* flow.integration_timespan* 每2帧图像拍摄时间差的积分，积分时间段是I2C读取光流的时间* accumulation timespan in microseconds* pixflow main.c中* uint32_t deltatime = (get_boot_time_us() - lasttime);* integration_timespan += deltatime;*//* flow.pixel_flow_y_integral * I2C读取光流的时间段内每2帧图像间的光流和 * accumulated optical flow in radians around y axis* pixflow main.c中* accumulated_flow_x += pixel_flow_y/focal_length_px * 1.0f; //rad axis swapped to align x flow around y axis*                       x移动距离(图片)/焦距=弧度*//* flow_ang[]是[rad/s]，光流位移转变成弧度再除以时间*/     else {//calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro)flow_ang[1] = ((flow.pixel_flow_y_integral - flow.gyro_y_rate_integral) / (float)flow.integration_timespan * 1000000.0f+ gyro_offset_filtered[1]) * params.flow_k;//for now the flow has to be scaled (to small)}/* flow measurements vector */float flow_m[3];if (fabs(rates_setpoint.yaw) < rate_threshold) {flow_m[0] = -flow_ang[0] * flow_dist;//角速度*距离=位移速度flow_m[1] = -flow_ang[1] * flow_dist;} else {flow_m[0] = -flow_ang[0] * flow_dist - yaw_comp[0] * params.flow_k;flow_m[1] = -flow_ang[1] * flow_dist - yaw_comp[1] * params.flow_k;}flow_m[2] = z_est[1];/* velocity in NED */float flow_v[2] = { 0.0f, 0.0f };/* project measurements vector to NED basis, skip Z component */for (int i = 0; i < 2; i++) {for (int j = 0; j < 3; j++) {flow_v[i] += PX4_R(att.R, i, j) * flow_m[j];//从机体坐标转换成北东地坐标，旋转矩阵的列分别为xyz的旋转，i=0,1，正好跳过z轴}}/* velocity correction */corr_flow[0] = flow_v[0] - x_est[1];corr_flow[1] = flow_v[1] - y_est[1];/* adjust correction weight */float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min);//(光流匹配数量-最少达标光流匹配数量)/(1-最少达标光流匹配数量)w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist);//X4_R(att.R, 2, 2)单独拿出来乘不知道是什么意思/* if flow is not accurate, reduce weight for it */// TODO make this more fuzzyif (!flow_accurate) {//精度不可用w_flow *= 0.05f;}/* under ideal conditions, on 1m distance assume EPH = 10cm */eph_flow = 0.1f / w_flow;flow_valid = true;} else {w_flow = 0.0f;flow_valid = false;}flow_updates++;}

3.气压定高---手动切定高模式和航点定高

(1)手动切定高模式

通过实验发现，local_pos.z代表的数没有实际意义，因为在同一个地方会显示不同的数，而且经常是负几十到正十几之间，但是相对的local_pos.z是准确的，也就是上移动产生的local_pos.z差值是和实际一一对应的

上面蓝色字体的理解应该是错的，local_pos.z=加速度的二次积分(从开机就开始运行积分了，那么当时的高度就是零)+气压计矫正corr_baro = baro_offset - sensor.baro_alt_meter[0]- z_est[0]气压也减去了开机时的气压值了，那刚开始的气压高度就是0)，所以local_pos.z就是起飞地的高度，只是不知道为何它有时会偏移这么多，气压计没有稳定，刚上电温度不够稳定？

下面这就是笔者先手拿在一个固定的高度，一段时间后再上下移动0.5m得到的波形

那么笔者疑虑这是如何定高的？！

在mc_pos_control.cpp的manual()中，当切定高模式时会记下此时高度，作为高度设定值(如有疑惑，请看pixhawk mc_pos_control.cpp思路整理)

if (!_alt_hold_engaged) {_run_alt_control = false; /* request velocity setpoint to be used, instead of altitude setpoint */_vel_sp(2) = req_vel_sp_scaled(2);_pos_sp(2) = _pos(2);
}

接下来，这个高度就是期望高度，用于控制的一直就是高度差了，和上述实验非常吻合

if (_run_alt_control) {_vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2);
}

(2)航点定高

考虑的问题有2个：

航点定高用到的高度是绝对高度还是高度差，即是利用高度还是高度差定到对应航点的高度？因为不能像手动那样直接飞到一个高度，切定高模式，记录当时的高度，再用新的得到高度与之比较来调节。

定高的过程是什么？

先看获取的高度是什么。

if (gps_valid) {double lat = gps.lat * 1e-7;double lon = gps.lon * 1e-7;float alt = gps.alt * 1e-3;/* initialize reference position if needed */if (!ref_inited) {if (ref_init_start == 0) {ref_init_start = t;} else if (t > ref_init_start + ref_init_delay) {ref_inited = true;/* set position estimate to (0, 0, 0), use GPS velocity for XY */x_est[0] = 0.0f;x_est[1] = gps.vel_n_m_s;y_est[0] = 0.0f;y_est[1] = gps.vel_e_m_s;local_pos.ref_lat = lat;local_pos.ref_lon = lon;local_pos.ref_alt = alt + z_est[0];local_pos.ref_timestamp = t;/* initialize projection */map_projection_init(&ref, lat, lon);// XXX replace this printwarnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt);mavlink_log_info(&mavlink_log_pub, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt);}}

刚开始获取参考高度：local_pos.ref_alt = alt + z_est[0];

float alt= gps.alt * 1e-3;//GPS获取的高度

z_est[0]是离起点的高度

正式运行飞行器处理高度：corr_gps[2][0] = local_pos.ref_alt - alt -est_buf[est_i][2][0];

这个类似气压计，也就是说local_pos.ref_alt等价于baro_offset，所以之后用到的高度都是距离起点的高度

需要注意的是：

GPS也用上气压计

if (use_gps_z) {float offs_corr = corr_gps[2][0] * w_z_gps_p * dt; baro_offset += offs_corr; corr_baro += offs_corr;
}

GPS最后得到高度并且用于控制的其实是local_pos.z

if (use_gps_z) {epv = fminf(epv, gps.epv);inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v);
}

local_pos.z = z_est[0];

而全球高度global_pos.alt = local_pos.ref_alt -local_pos.z;因为地理坐标系前又下对应着xyz，所以高度是参考高度减去而不是加上local_pos.z

既然GPS最后得到的高度是local_pos.z，那么控制就都是用相对高度来实现定高，而且mc_pos_control.cpp就没有定阅global_pos的主题，所以位置控制都是用local_pos主题的数据。

那么问题又来了global_pos主题有何用？先看到这里，以后再找答案。。

如果您觉得此文对您的发展有用，请随意打赏。
您的鼓励将是笔者书写高质量文章的最大动力^_^!!

pixhawk position_estimator_inav.cpp再分析相关推荐

pixhawk position_estimator_inav.cpp思路整理及数据流
写在前面: 这篇blog主要参考pixhawk的高度解算算法解读,并且加以扩展,扩展到其他传感器,其实里面处理好多只是记录了流程,至于里面实际物理意义并不是很清楚,也希望大牛能够指导一下. 概述: 整 ...
综述 | 现在是蛋白质组学数据共享和再分析的黄金时间？
导读同更为成熟的基因组学.转录组学一样,基于质谱的蛋白质组学数据共享在科研实践中越来越常见.本文作者为我们强调了该领域这一空前的现状,为一些数据科学家带来了无限机遇.本文主要通过三个层面为我们阐述了 ...
功能强大的TCGA再分析平台
TCGA是研究肿瘤相关机制的重要资源.在线数据库更新改版都比较快,使用时需要参照最新的线上数据教程.不过癌症相关的数据库操作起来也都比较类似,输入一个或多个关注的目的基因,查看基因的功能注释,基因在哪 ...
【MATLAB深度学习工具箱】学习笔记--体脂估计算例再分析：拟合神经网络fitnet里面的函数】
介绍上一篇 [MATLAB深度学习工具箱]学习笔记--体脂估计算例再分析:拟合神经网络fitnet里面的数据结构]_bear_miao的博客-CSDN博客原文链接如下[MATLAB深度学习工具箱]学 ...
超越“虚拟的美丽”——云计算实践再分析
超越"虚拟的美丽"--云计算实践再分析 2012-05-31 16:02 | 3491次阅读 | [已有11条评论]发表评论来源:CSDN | 作者:李德毅 | 收藏到我的网摘 ...
NCEP再分析数据（FNL）Python下载
NCEP再分析数据(FNL)Python下载楼主第一次接触WRF模型,最近在摸索该如何使用WRF和相关数据如何下载,简单记录一下. 数据网址: https://rda.ucar.edu/datase ...
用NCL将GRIB/GRIB2文件转成nc文件（批量转），JRA-55再分析为例
背景: 答主最近需要用到JRA-55再分析数据,但在面对GRIB数据格式时犯难了,便想把GRIB格式转成nc文件再做分析.因为nc文件很容易用python或Matlab处理尝试了好些方法,发现先安装 ...
sklearn 读取csv_气象数据再分析数据的读取方式（GRIB格式为例）
在对全球环境进行研究中,我们总会遇到欧洲中期预报中(European Centre for Medium-Range Weather Forecasts)等发布的气象再分析数据,例如,我在ESA官网上 ...
python爬虫网易云音乐评论再分析_Scrapy爬取网易云音乐和评论（一、思路分析）...
目录: 前提: scrapy这个框架很多人用过,网上教程也很多,但大多就是爬爬小说这种比较简单且有规律的.尤其大多网站它是可以通过点击下一页的方式爬取下一页,我看到的教程也都是这样的.而网易云的按钮光 ...
linux网卡断流测试,Windows XP SP2操作系统下网络非完全断流的再分析(转)
Windows XP SP2操作系统下网络非完全断流的再分析(转)[@more@] 在升级到WindowsSP2系统后,本人的电脑经常出现这种怪毛病,具体情况为:在上网时网关能够PING通,用Bitc ...

pixhawk position_estimator_inav.cpp再分析

pixhawk position_estimator_inav.cpp再分析相关推荐

最新文章

热门文章