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 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主题有何用？先看到这里，以后再找答案。。

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