[飞控]APM姿态控制—两个常用的函数

Yannick

文 / zinghd

【作者简介】

作者还没有想好怎么描述自己

APM的姿态控制部分有大量的用于变换的函数，最近分析了两个常用的函数，以前看代码只看框架，觉得不就是姿态吗，不就是欧拉角，四元数，旋转矩阵吗？现在看过算法细节才知道每个算法的实现背后都是严谨的物理假设和数学推导，基础知识太重要，还有太多需要学习。
函数名：AC_PosControl::accel_to_lean_angles

位置：

libraries\AC_AttitudeControl\AC_PosControl.cpp

用途：将NED坐标系下的期望加速度转换成机体坐标系下的倾斜角度(pitch,roll)

源程序：
void AC_PosControl::accel_to_lean_angles(float accel_x_cmss, float accel_y_cmss, float& roll_target, float& pitch_target) const
{
    float accel_right, accel_forward;

    // rotate accelerations into body forward-right frame
    accel_forward = accel_x_cmss*_ahrs.cos_yaw() + accel_y_cmss*_ahrs.sin_yaw();
    accel_right = -accel_x_cmss*_ahrs.sin_yaw() + accel_y_cmss*_ahrs.cos_yaw();

    // update angle targets that will be passed to stabilize controller
    pitch_target = atanf(-accel_forward/(GRAVITY_MSS * 100.0f))*(18000.0f/M_PI);
    float cos_pitch_target = cosf(pitch_target*M_PI/18000.0f);
    roll_target = atanf(accel_right*cos_pitch_target/(GRAVITY_MSS * 100.0f))*(18000.0f/M_PI);
}

数学推导：

假设飞行器z轴受力平衡，即在z轴上保持静止。

已知NED坐标系下期望加速度accel_x_cmss，accel_y_cmss(融合之后的加速度数据为NED坐标系下)和遥控输入的yaw，求机体坐标系下pitch角和roll角。

1.需要先将yaw角进行补偿，即只剩下倾斜角。

将NED坐标系下的水平加速度做绕z旋转，得到没有旋转只有倾斜的中间状态,该状态下的x轴方向加速度为accel_forward，y轴加速度为accel_righ

NED坐标系转机体坐标系绕z轴旋转矩阵为：

2.通过accel_forward，accel_righ，计算pitch，roll。

应为飞机在z轴上受力平衡，所以机体坐标系下飞机受到的合外力将在z轴方向上产生一个-G的力才能使飞机在z轴受力平衡。

如图



支持飞机在NED坐标系z轴保持静止需要-G，而且需要x轴上产生accel_forward，y轴上产生accel_righ，这些都是由机体坐标系下的合外力-F提供的。

所以有：



其中

是NED坐标系转到机体坐标系的旋转矩阵(之前已经进行过yaw方向的旋转了所以这个矩阵里的yaw为0)。



可得：



所以有：



所以：





函数名：

void Quaternion::to_axis_angle

位置：libraries\AP_Math\quaternion.cpp
功能：将四元数转换成轴角形式
原函数：
void Quaternion::to_axis_angle(Vector3f &v)
{
    float l = sqrtf(sq(q2)+sq(q3)+sq(q4));
    v = Vector3f(q2,q3,q4);
    if (!is_zero(l)) {
        v /= l;
        v *= wrap_PI(2.0f * atan2f(l,q1));
    }
}

单位四元素可以表示为：



单位四元数有：



又因为：



可以得到：



所以：



但是函数里的四元素不是单位四元素所以要进行单位化



令:



所以：



以上两个函数反复运用在姿态控制部分，深入理解能够帮助你了解姿态的本质，和姿态控制的思想。

附上我画的姿态控制流程图可以帮助理解：



关注微信公众号，回复【姿态】即可获取结构图Visio文件，同时欢迎加作者微信进行交流。