PX4多旋翼航线算法代码解析

前言

最近研究了一下PX4对于多旋翼和固定翼的控制，其实网上对于这一部分的算法上的解释已经很多了，这里仅仅是对于PX4关键代码的个人理解个记录。今天先记录下PX4对于多旋翼航线规划的代码理解，后续会陆续更新多旋翼个固定翼的姿态控制、位置控制、L1、TECS等相关代码的解析

---

一、航线算法

其实PX4控多旋翼跑航线这块实质上还是控位置，走的位置环，然后经过速度环和内环实现航线规划，这跟传统的侧偏距算法有点区别，侧偏距PD算法直接将侧偏距转换为期望的纠偏速度，其实是生成的地理坐标系下期望的速度，然后经过速度环和内环实现航线规划。这两种算法可以都仿一下试试哪一种效果好

二、代码解析

这里只解读下void MulticopterPositionControl::control_auto(float dt)这个函数里关键函数的内容

1.GPS经纬度转地理系下坐标

__EXPORT int map_projection_reproject(const struct map_projection_reference_s *ref, float x, float y, double *lat,
				      double *lon)
{
	if (!map_projection_initialized(ref)) {
		return -1;
	}

	double x_rad = x / CONSTANTS_RADIUS_OF_EARTH;
	double y_rad = y / CONSTANTS_RADIUS_OF_EARTH;
	double c = sqrtf(x_rad * x_rad + y_rad * y_rad);
	double sin_c = sin(c);
	double cos_c = cos(c);

	double lat_rad;
	double lon_rad;

	if (fabs(c) > DBL_EPSILON) {
		lat_rad = asin(cos_c * ref->sin_lat + (x_rad * sin_c * ref->cos_lat) / c);
		lon_rad = (ref->lon_rad + atan2(y_rad * sin_c, c * ref->cos_lat * cos_c - x_rad * ref->sin_lat * sin_c));

	} else {
		lat_rad = ref->lat_rad;
		lon_rad = ref->lon_rad;
	}

	*lat = lat_rad * 180.0 / M_PI;
	*lon = lon_rad * 180.0 / M_PI;

	return 0;
}

这里主要是把期望位置和当前位置坐标转到原点为第一次上电的点（第一次接受到GPS数据点）的坐标系。

2.scale值的计算

math::Vector<3> scale = _params.pos_p.edivide(cruising_speed);

在整个航线算法中，scale这个值扮演这及其重要的作用，scale这个值把位置信息转到了一个特殊的坐标系，姑且称为单位圆坐标系，用这个特殊坐标系的意义在于，如果在这个坐标系中，两个坐标点的位置差如果等于1，则飞机在一个单位周期内可以巡航速度飞过这一段位置，避免无意义的加减速，也可以说保持速度，这是这个scale值的意义。

---

3.位置坐标转换

math::Vector<3> pos_s = _pos.emult(scale);
math::Vector<3> prev_sp_s = prev_sp.emult(scale);
math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s;
math::Vector<3> curr_pos_s = pos_s - curr_sp_s;
float curr_pos_s_len = curr_pos_s.length();

注意：其中带后缀_s的都是转过坐标系的

4.飞机当前位置距离目标位置误差小于一个单位1

if (curr_pos_s_len < 1.0f) {
					/* copter is closer to waypoint than unit radius */
					/* check next waypoint and use it to avoid slowing down when passing via waypoint */
					// 检查下一个航点是否有效并改变航点值避免无效减速
					if (_pos_sp_triplet.next.valid) {
						math::Vector<3> next_sp;
						//映射下一个位置设定值到当前坐标系
						map_projection_project(&_ref_pos,
								       _pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon,
								       &next_sp.data[0], &next_sp.data[1]);
						next_sp(2) = -(_pos_sp_triplet.next.alt - _ref_alt);//下一个航点高度设定值

						//如果下一个设定值和当前设定值之间的距离小于最小距离，注意这里是真实世界的位置
						if ((next_sp - curr_sp).length() > MIN_DIST) {
							//转坐标
							math::Vector<3> next_sp_s = next_sp.emult(scale);
							/* calculate angle prev - curr - next */
							math::Vector<3> curr_next_s = next_sp_s - curr_sp_s;
							//当前航点到上一个航点向量归一化
							math::Vector<3> prev_curr_s_norm = prev_curr_s.normalized();
							/* cos(a) * curr_next, a = angle between current and next trajectory segments */
							//a为当前轨迹和下一个轨迹之间的夹角
							//a为pre-->curr和curr-->next的夹角
							float cos_a_curr_next = prev_curr_s_norm * curr_next_s;

							/* cos(b) = 点乘结果/模值*/
							float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len;

							//如果a为锐角，并且b也为锐角
							if (cos_a_curr_next > 0.0f && cos_b > 0.0f) {
								float curr_next_s_len = curr_next_s.length();

								/* if curr - next distance is larger than unit radius, limit it */
								//如果curr与next的距离大于单位半径，则限制它
								if (curr_next_s_len > 1.0f) {
									cos_a_curr_next /= curr_next_s_len;
								}

								/* feed forward position setpoint offset */
								//修改后的期望位置
								//如果角a和角b很小，则加的很小
								math::Vector<3> pos_ff = prev_curr_s_norm *
											 cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) *
											 (1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f));
								pos_sp_s += pos_ff;
							}
						}
					}

				}

5.飞机当前位置距离目标位置误差大于等于一个单位1

这里是整个航线算法的关键函数near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s);

bool
MulticopterPositionControl::cross_sphere_line(const math::Vector<3> &sphere_c, float sphere_r,
		const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3> &res)
{
	/* project center of sphere on line */
	/* normalized AB */
	math::Vector<3> ab_norm = line_b - line_a;
	ab_norm.normalize();
	//这里可以理解为原点到垂足点的向量，也就是是垂足点的坐标值
	math::Vector<3> d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm);
	float cd_len = (sphere_c - d).length();

	/* we have triangle CDX with known CD and CX = R, find DX */
	//单位圆跟航线有交点，下一个期望位置在靠近目标点的交点上
	if (sphere_r > cd_len) {
		/* have two roots, select one in A->B direction from D */
		float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len);
		res = d + ab_norm * dx_len;
		return true;

	} 
	//单位圆与航线没有交点，下一个期望位置在垂足上。这个距离大于一个单位1，后面的程序会归一化到单位1
	else {
		/* have no roots, return D */
		res = d;
		return false;
	}
}

这里就是简单的向量加减法不再进行赘述，需要注意的就是cd_len 的计算sphere_c - d可以当（sphere_c -line_a） - ab_norm * ((sphere_c - line_a) * ab_norm)这么理解。程序里那样写是为了方便计算垂足点坐标。

6.新计算出的期望位置归一化到单位圆坐标

在这里处理下计算出的期望位置就发布了

/* move setpoint not faster than max allowed speed */
math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale);

/* difference between current and desired position setpoints, 1 = max speed */
//这里将scale = _params.pos_p.edivide(cruising_speed)带入，得出的d_pos_m_len 是一个时间量纲
math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p);
float d_pos_m_len = d_pos_m.length();
//大于这个dt证明飞机在一个dt时间内以巡航速度不能到达目标点，所有要缩小到单位1
if (d_pos_m_len > dt) {
	pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p);
}
//将单位圆坐标系坐标点转到地理系下
/* scale result back to normal space */
_pos_sp = pos_sp_s.edivide(scale);

7.计算期望的航向角

if (_pos_sp_triplet.current.yawspeed_valid
	 && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_FOLLOW_TARGET) {
	_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
	} else if (PX4_ISFINITE(_pos_sp_triplet.current.yaw)) {
		_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
	}

总结

上述为笔者对航线代码的一点解读，有问题大家可以一块讨论。后续有时间会记录对PX4其他代码的理解解析。