Pixhawk学习10.1——多旋翼位置控制

1.整体流程

位置控制部分主要以control_auto为主进行学习。因为自动控制部分也包含了手动控制的内容。多旋翼位置控制主要分为几个步骤：
根据航线计算期望位置 ==> 根据期望位置计算期望速度 ==> 根据期望速度计算期望推力矢量 ==> 根据期望推力矢量计算期望姿态。
因此，多旋翼位置控制部分也根据上述步骤进行学习，由于该部分的内容比较多，可能会分几期进行。

2.多旋翼位置控制的特点

对照前述固定翼位置控制部分的内容，多旋翼位置控制具有一些特点：
1）多旋翼的高度可以直上直下，因此在高度通道控制上只牵扯到推力的问题，而不像固定翼那样要进行高度控制，就得综合考虑推力和俯仰角两部分；
2）多旋翼的水平位移可以不用管航向，即多旋翼可以前飞、倒飞、侧飞，而可以不考虑机头的朝向。

3.期望位置点计算

多旋翼自动模式飞行时会根据提前预设好的航线或者实时给定的航向飞行，而使用者的目的是想让飞机压着航线飞行。由于多旋翼的速度是有范围的，其最大速度受到动力能力、安全系数等因素的限制，因此在压航线飞行时，飞机的期望位置点并不是预设航线中的位置点，而是根据这些航线点和飞机性能不断计算的最优期望位置点。
这里需要明确一点：多旋翼的位置控制第一宗旨是“以最快的速度回到航线上！！！”

3.1 位置缩放

这里先简单介绍一下位置缩放。
前面说了，多旋翼飞机是有最大速度限制的。即单位时间的运动距离是有上限的。以最大速度5m/s计算，如果位置到速度的比例参数P=1，那么位置误差的最大值需要被限制在5m。那么5m就要作为一个控制单位1。
PIX中将该部分进行了归一化：

/* scaled space: 1 == position error resulting max allowed speed */

		math::Vector<3> cruising_speed = _params.vel_cruise;

		if (PX4_ISFINITE(_pos_sp_triplet.current.cruising_speed) &&
		    _pos_sp_triplet.current.cruising_speed > 0.1f) {
			cruising_speed(0) = _pos_sp_triplet.current.cruising_speed;
			cruising_speed(1) = _pos_sp_triplet.current.cruising_speed;
		}

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

		/* convert current setpoint to scaled space */
		math::Vector<3> curr_sp_s = curr_sp.emult(scale);

		/* by default use current setpoint as is */
		math::Vector<3> pos_sp_s = curr_sp_s;

3.2 期望位置点计算

由于最大速度的限制，导致期望位置点的计算有以下几种情况。这几种情况的分类方法如下：
以当前无人机位置C点为圆心，最大速度归一化后的位置误差1为半径（如果是上述最大速度5m/s，位置控制的P为1，那么该处的误差1就代表位置5m），画圆后看与航线的位置关系，来判定期望位置点的计算方法。
如下图，A为上一个航线点，B为下一个航线点，C为当前无人机位置，D为当前无人机点离AB航线的最近距离点，即CD⊥AB，E为圆与AB相交、且离B点较近的点。

- 圆与AB相交
1、D点位于B点右侧

如上图，此时表示无人机已经过了B点，这种情况下，以B点作为期望位置点，目的是让无人机回到下一个航线点上。
2、D点位于B点左侧（包含D点位于A点左侧的情况）

表示无人机还未超过B点，那么此时的运动目的有两个：
1）要回到AB航向上；
2）要以最大速度回到航线上。
那么此时的期望位置点为E，即既能回到AB航线上，又是以最大速度飞行。

- 圆与AB不相交
1、D点位于AB中间

此时的期望位置点为D，目的是让无人机尽快回到AB航线上。
2、D点位于A点左侧

此时的期望位置点为A。此时没有必要让飞机回到AB的左侧延长线上，而是要以最大速度尽快飞到A点。
3、D点位于B点右侧

此时的期望位置点为B。同上，此时没有必要让飞机回到AB的右侧延长线上，而是要以最大速度尽快飞回到B点。

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();

	if (sphere_r > cd_len) {
		/* we have triangle CDX with known CD and CX = R, find DX */
		float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len);

		if ((sphere_c - line_b) * ab_norm > 0.0f) {
			/* target waypoint is already behind us */
			res = line_b;

		} else {
			/* target is in front of us */
			res = d + ab_norm * dx_len; // vector A->B on line
		}

		return true;

	} else {
		/* have no roots, return D */
		res = d; /* go directly to line */

		/* previous waypoint is still in front of us */
		if ((sphere_c - line_a) * ab_norm < 0.0f) {
			res = line_a;
		}

		/* target waypoint is already behind us */
		if ((sphere_c - line_b) * ab_norm > 0.0f) {
			res = line_b;
		}

		return false;
	}
}