PX4位置控制的姿态角限制实现

PX4中位置控制时的姿态角限制

PX4中进行位置控制时，经过位置控制的PID计算，会得到在NED坐标系下的期望推力矢量，期望推力矢量最终会映射到期望姿态上

去，姿态角限制是在计算期望推力矢量时完成的，图中Zb和T的方向是相反的：

其中针对姿态角限制的代码如下：

void limitTilt(Vector3f &body_unit, const Vector3f &world_unit, const float max_angle)
{
	// determine tilt
	const float dot_product_unit = body_unit.dot(world_unit);
	float angle = acosf(dot_product_unit);
	// limit tilt
	angle = math::min(angle, max_angle);
	Vector3f rejection = body_unit - (dot_product_unit * world_unit);

	// corner case exactly parallel vectors
	if (rejection.norm_squared() < FLT_EPSILON) {
		rejection(0) = 1.f;
	}

	body_unit = cosf(angle) * world_unit + sinf(angle) * rejection.unit();
}

px4中并没有分别对俯仰角和横滚角进行限制，而是限制了倾斜角tilt，即限制的是机体系Z轴和NED坐标系Z轴之间的夹角

限制过程

经过位置控制PID计算会得到NED坐标系下3个轴的加速度设定值，同时假设垂向的加速度为1个g，以此来计算在该水平期望加速度下，负的总推力矢量（推力矢量在机体坐标系下总是和Z轴平行的），单位化之后，在NED坐标系下的坐标，记改矢量为$body_z$：

1. 首先计算，当前水平推力设定下，需要的姿态角angle_raw，计算方式如下：

   $$angle_{raw} = arccos(body_z .* [0,0,1]^T)$$

2. 当$angle_{raw}$大于我们设定的角度限制时，即$angle_{raw}>angle_{limit}$时，直接以$angle_{limit}$计算新的body_z：

   1. 先计算一个向量$rejection$，计算方式如下：

      $$rejection = body_z - (body_z * [0,0,1]^T)*[0,0,1]^T$$

   2. 然后作特殊处理，当rejection模值特别小时，body_z和[0,0,1]，向量是平行的，同时当rejection模式很小时，需要作特殊处理，因为之后会用到rejection的单位向量rejection.unit，若不作处理在计算rejection的单位向量rejection.unit时可能会出现除零的情况，处理如下：

      // corner case exactly parallel vectors
      	if (rejection.norm_squared() < FLT_EPSILON) {
      		rejection(0) = 1.f;
      	}

1. 根据限制的倾角，反算新的body_z，即body_z_limit：

   1. 首先计算向量A：

      $$A = cos(angle)*[0,0,1]^T$$

   2. 然后计算向量B：

      $$B=sin(angle)*rejection.unit$$

2. 最后计算新的body_z即body_z_limit:

   $$body_{zlimit} = A + B$$

   至此，期望的机体Z轴方向限制完成，间接限制了最终的姿态设定值

赏

谢谢你请我吃糖果

支付宝

微信