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

10.1中介绍了目标位置点的计算逻辑，知道下一时刻的目标位置后，飞控需要根据当前位置进行计算，依次得到期望速度，期望拉力矢量，期望姿态。至此就完成了多旋翼的位置控制。

1、期望速度计算

上篇计算得到期望位置之后，根据飞机当前位置，可知道位置差矢量。在位置速度串级PID中，位置环只采用了比例，速度环采用了PID。

	/* run position & altitude controllers, if enabled (otherwise use already computed velocity setpoints) */
				if (_run_pos_control) {
					_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);
					_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
				}

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

得到期望速度之后，主要进行了以下三步操作，用来规范真正的期望速度：

#限制最大水平速度
确保水平速度矢量和大小在限制范围内，对水平两个速度进行等比例缩小；

/* make sure velocity setpoint is saturated in xy*/
				float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) +
							  _vel_sp(1) * _vel_sp(1));

				if (vel_norm_xy > _params.vel_max(0)) {
					/* note assumes vel_max(0) == vel_max(1) */
					_vel_sp(0) = _vel_sp(0) * _params.vel_max(0) / vel_norm_xy;
					_vel_sp(1) = _vel_sp(1) * _params.vel_max(1) / vel_norm_xy;
				}

#限制最大垂直速度
确保垂直速度在限制范围内，对垂向速度进行限制。

/* make sure velocity setpoint is saturated in z*/
				if (_vel_sp(2) < -1.0f * _params.vel_max_up) {
					_vel_sp(2) = -1.0f * _params.vel_max_up;
				}

				if (_vel_sp(2) >  _params.vel_max_down) {
					_vel_sp(2) = _params.vel_max_down;
				}

#限制期望速度变化量（加速度）
对当前和之前的期望速度做差，限制其变化量，即限制加速度值。

// limit total horizontal acceleration
				math::Vector<2> acc_hor;
				acc_hor(0) = (_vel_sp(0) - _vel_sp_prev(0)) / dt;
				acc_hor(1) = (_vel_sp(1) - _vel_sp_prev(1)) / dt;

				if ((acc_hor.length() > _params.acc_hor_max) & !_reset_pos_sp) {
					acc_hor.normalize();
					acc_hor *= _params.acc_hor_max;
					math::Vector<2> vel_sp_hor_prev(_vel_sp_prev(0), _vel_sp_prev(1));
					math::Vector<2> vel_sp_hor = acc_hor * dt + vel_sp_hor_prev;
					_vel_sp(0) = vel_sp_hor(0);
					_vel_sp(1) = vel_sp_hor(1);
				}

				// limit vertical acceleration

				float acc_v = (_vel_sp(2) - _vel_sp_prev(2)) / dt;

				if ((fabsf(acc_v) > 2 * _params.acc_hor_max) & !_reset_alt_sp) {
					acc_v /= fabsf(acc_v);
					_vel_sp(2) = acc_v * 2 * _params.acc_hor_max * dt + _vel_sp_prev(2);
				}

				_vel_sp_prev = _vel_sp;

				_global_vel_sp.vx = _vel_sp(0);
				_global_vel_sp.vy = _vel_sp(1);
				_global_vel_sp.vz = _vel_sp(2);

2.期望拉力矢量计算

拉力矢量由期望速度和当前速度的差进行PID计算得到。

thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) + thrust_int;

得到期望拉力矢量之后，又需要进行以下三种操作得到最终的实际期望拉力矢量：
#确保升力大于最小值
确保拉力矢量的幅值不能小于最小拉力值，不至于飞机掉下去。

	/* limit min lift */
					if (-thrust_sp(2) < thr_min) {
						thrust_sp(2) = -thr_min;
						saturation_z = true;
					}

#保证飞机不超过最大倾斜角
限制最大倾斜角。利用Z向的拉力值乘以最大倾斜角的正切，得到最大水平速度。并根据实际的水平和速度的幅值等比例缩小水平两个速度。

/* limit max tilt */
						if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) {
							/* absolute horizontal thrust */
							float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();

							if (thrust_sp_xy_len > 0.01f) {
								/* max horizontal thrust for given vertical thrust*/
								float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max);

								if (thrust_sp_xy_len > thrust_xy_max) {
									float k = thrust_xy_max / thrust_sp_xy_len;
									thrust_sp(0) *= k;
									thrust_sp(1) *= k;
									saturation_xy = true;
								}
							}
						}

#限制最大推力
如果拉力矢量的幅值大于最大拉力，那么让水平期望拉力为零，Z向拉力为拉力。

		if (-thrust_sp(2) > thr_max) {
								/* thrust Z component is too large, limit it */
								thrust_sp(0) = 0.0f;
								thrust_sp(1) = 0.0f;
								thrust_sp(2) = -thr_max;
								saturation_xy = true;
								saturation_z = true;

							} else {
								/* preserve thrust Z component and lower XY, keeping altitude is more important than position */
								float thrust_xy_max = sqrtf(thr_max * thr_max - thrust_sp(2) * thrust_sp(2));
								float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
								float k = thrust_xy_max / thrust_xy_abs;
								thrust_sp(0) *= k;
								thrust_sp(1) *= k;
								saturation_xy = true;
							}

3.期望姿态计算

PIX中的期望姿态由期望拉力矢量和期望航向来计算。期望航向已经在路径规划时得到。
期望拉力矢量其实是指定了机体的期望Z轴，期望航向是指定了机体的期望Z轴。
即，将拉力矢量归一化，就是往姿态矩阵 的第三列。

/* desired body_z axis = -normalize(thrust_vector) */
						math::Vector<3> body_x;
						math::Vector<3> body_y;
						math::Vector<3> body_z;

						if (thrust_abs > SIGMA) {
							body_z = -thrust_sp / thrust_abs;

						} else {
							/* no thrust, set Z axis to safe value */
							body_z.zero();
							body_z(2) = 1.0f;
						}

将期望航向角通过三角函数变换在于姿态矩阵R_sp 的第三列叉乘就能得R_sp 的第一列。

/* vector of desired yaw direction in XY plane, rotated by PI/2 */
						math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f);

						if (fabsf(body_z(2)) > SIGMA) {
							/* desired body_x axis, orthogonal to body_z */
							body_x = y_C % body_z;

							/* keep nose to front while inverted upside down */
							if (body_z(2) < 0.0f) {
								body_x = -body_x;
							}

							body_x.normalize();

再通过叉乘R_sp 的第三列和第一列，就能得到完整的姿态矩阵。

		/* desired body_y axis */
						body_y = body_z % body_x;

						/* fill rotation matrix */
						for (int i = 0; i < 3; i++) {
							R(i, 0) = body_x(i);
							R(i, 1) = body_y(i);
							R(i, 2) = body_z(i);
						}

至此，就得到了期望的姿态矩阵，可将R转换为四元数，或者直接求解水平期望姿态角。

/* copy quaternion setpoint to attitude setpoint topic */
						matrix::Quatf q_sp = R;
						memcpy(&_att_sp.q_d[0], q_sp.data(), sizeof(_att_sp.q_d));
						_att_sp.q_d_valid = true;

						/* calculate euler angles, for logging only, must not be used for control */
						matrix::Eulerf euler = R;
						_att_sp.roll_body = euler(0);
						_att_sp.pitch_body = euler(1);