此篇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_flowGPS
//权值的产生
//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;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 0
eph = 0.001;
}
if (eph < max_eph_epv) {
eph *= 1.0f + dt;
}
if (epv < 0.000001f) { //get case where epv is 0 -> would stay 0
epv = 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 sonar
float 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 fuzzy
if (!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 print
warnx("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主题有何用？先看到这里，以后再找答案。。
如果您觉得此文对您的发展有用，请随意打赏。
您的鼓励将是笔者书写高质量文章的最大动力^_^!!
扫一扫
4702
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
