Ardupilot四元数姿态控制函数attitude_controller_run_quat解析

在开始阅读本文之前，建议可以先看一下前面几篇博文，当然你基础好直接看也是没事的~
详解APM的开方控制器sqrt_controller
Ardupilot倾转分离函数thrust_heading_rotation_angles
Ardupilot前馈及平滑函数input_euler_angle_roll_pitch_yaw解析

 

源码解析

以n系简称NED坐标系，b系简称机体坐标系，tb表示期望姿态，cb表示当前姿态。

// 根据姿态误差计算得出期望角速率
void AC_AttitudeControl::attitude_controller_run_quat()
{
	// 检索四元数车辆姿态
	// 获取到当前姿态的四元数并保存到attitude_vehicle_quat
    Quaternion attitude_vehicle_quat;
    _ahrs.get_quat_body_to_ned(attitude_vehicle_quat);

	// 计算姿态误差
	// _attitude_target_quat：输入参数，在n系下期望姿态的四元数，旋转方向tb->n
	// attitude_vehicle_quat：输入参数，在n系下当前姿态的四元数，旋转方向cb->n
	// attitude_error_vector：传入传出参数，姿态误差向量
	// _thrust_error_angle：传入传出参数，期望z轴和当前z轴的误差（详见轴角分离），即推力方向上的偏差角
	// 该函数通过前两个输入的四元数参数计算得到各轴的姿态偏差
	// 经过此函数后得到的参数均在b系下
    Vector3f attitude_error_vector;
    thrust_heading_rotation_angles(_attitude_target_quat, attitude_vehicle_quat, attitude_error_vector, _thrust_error_angle);

	// P控制器，输入为姿态误差，输出为期望角速率
	// 函数内部根据是否开启了sqrt_controller来区分计算各通道的期望角速率_rate_target_ang_vel
	// 是，则使用开方调整过的P控制器
	// 否，则使用常规的Kp*error
    _rate_target_ang_vel = update_ang_vel_target_from_att_error(attitude_error_vector);

	// 对P控制器输出叠加前馈项，尝试确保侧倾和俯仰误差随车身框架而不是参考框架旋转
	// x轴的前馈项 = y轴的姿态误差（限幅后的）*陀螺仪获取的z轴角速度
	// y轴的前馈项 = x轴的姿态误差（限幅后的）*陀螺仪获取的z轴角速度
	// 注意此处并没有对z轴的期望角速率进行处理
    // todo: this should probably be a matrix that couples yaw as well.
    _rate_target_ang_vel.x += constrain_float(attitude_error_vector.y, -M_PI / 4, M_PI / 4) * _ahrs.get_gyro().z;
    _rate_target_ang_vel.y += -constrain_float(attitude_error_vector.x, -M_PI / 4, M_PI / 4) * _ahrs.get_gyro().z;

	// 期望角速率限幅
    ang_vel_limit(_rate_target_ang_vel, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));

	// 角速度前馈旋转到机体坐标中（对该处有疑问的详见本文后面）
	// _attitude_target_ang_vel是一个全局变量，表示期望姿态的期望角速率，是由input_euler_angle_roll_pitch_yaw等类似函数获取到的前馈速率
	// attitude_target_ang_vel_quat在此处表明是一个向量[0,v]
	Quaternion attitude_target_ang_vel_quat = Quaternion(0.0f, _attitude_target_ang_vel.x, _attitude_target_ang_vel.y, _attitude_target_ang_vel.z);
	// 运算过程注意四元数和旋转矩阵一样是可以上下标相互约去的，此处以Q(a)(b)形式表明上标为a，下标为b，表示b向a的旋转过程
	// attitude_vehicle_quat前面说过是表示cb->n旋转的单位四元数，求逆后表示n->cb
	// to_to_from_quat = Q(cb)(n)*Q(n)(tb)=Q(cb)(tb)
	// 表明获取的四元数to_to_from_quat表示tb->cb的旋转，计算公式也可视作期望姿态与当前姿态的作差
	Quaternion to_to_from_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat;
	// 将tb下的期望角速率旋转至cb下获取当前姿态的角速率期望前馈叠加量desired_ang_vel_quat
    Quaternion desired_ang_vel_quat = to_to_from_quat.inverse() * attitude_target_ang_vel_quat * to_to_from_quat;

	// 校正推力矢量并平稳添加前馈和偏航输入
	// 下述过程根据推力角误差大小分配在Z轴YAW角上的数学计算结果和传感器测量值之间的确信度来确定Z轴的期望角速度
	// 同时将前馈控制量添加到各轴
	// 当推力角误差特别大的时候，表明计算出来的期望值不可信，此时z轴期望角速度取陀螺仪测量值
    if (_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE * 2.0f) {
        _rate_target_ang_vel.z = _ahrs.get_gyro().z;
	} else if (_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE) {
		// 当推力角误差较大时，根据之前计算出来的推力角误差_thrust_error_angle计算出前馈因子
		// 对各轴的旋转角速度各自叠加前馈控制量
        float feedforward_scalar = (1.0f - (_thrust_error_angle - AC_ATTITUDE_THRUST_ERROR_ANGLE) / AC_ATTITUDE_THRUST_ERROR_ANGLE);
        _rate_target_ang_vel.x += desired_ang_vel_quat.q2 * feedforward_scalar;
        _rate_target_ang_vel.y += desired_ang_vel_quat.q3 * feedforward_scalar;
        _rate_target_ang_vel.z += desired_ang_vel_quat.q4;
		// 校正推力矢量，feedforward_scalar表示对计算出的数学结果的确信度，(1- feedforward_scalar)则表示对陀螺仪测得的角速度的确信度
		// 两者综合得到最后的期望角速率
        _rate_target_ang_vel.z = _ahrs.get_gyro().z * (1.0 - feedforward_scalar) + _rate_target_ang_vel.z * feedforward_scalar;
	} else {
		// 推力角误差较小时，表明当前数学计算得到的期望角速率可信度较高，直接进行前馈叠加
        _rate_target_ang_vel.x += desired_ang_vel_quat.q2;
        _rate_target_ang_vel.y += desired_ang_vel_quat.q3;
        _rate_target_ang_vel.z += desired_ang_vel_quat.q4;
    }

    if (_rate_bf_ff_enabled) {
        // 当开启机体速率前馈时，实时计算更新期望姿态四元数，每一次更新都需要将其单位化
        Quaternion attitude_target_update_quat;
        attitude_target_update_quat.from_axis_angle(Vector3f(_attitude_target_ang_vel.x * _dt, _attitude_target_ang_vel.y * _dt, _attitude_target_ang_vel.z * _dt));
        _attitude_target_quat = _attitude_target_quat * attitude_target_update_quat;
        _attitude_target_quat.normalize();
    }

    // 确保四元数已经被单位化
    _attitude_target_quat.normalize();

	// 记录姿态误差以处理EKF重置
	// 等式相当于将_attitude_target_quat表示的期望姿态减去attitude_vehicle_quat表示的当前姿态
    _attitude_ang_error = attitude_vehicle_quat.inverse() * _attitude_target_quat;
}

总结：attitude_controller_run_quat()函数在内部通过调用thrust_heading_rotation_angles()函数计算出姿态误差，然后根据姿态误差调用了P控制器得到期望角速率，并进行了前馈叠加及平滑操作。

 

细节讲解

thrust_heading_rotation_angles()

这个函数主要的作用就是根据输入的期望姿态和当前姿态计算出姿态误差，详细介绍请看我之前的博文：Ardupilot倾转分离函数thrust_heading_rotation_angles

update_ang_vel_target_from_att_error()

这个函数内部代码比较简单，但是还是说一下。

主要就是P控制器的作用，将输入的姿态误差处理之后转换为期望角速率输出。

可以很清楚看到内部会根据是否启用_use_sqrt_controller来进行不同的处理，如果_use_sqrt_controller = 1，那么会调用整定后的P控制器sqrt_controller进行计算；而如果_use_sqrt_controller = 0，那么就会按照传统计算方式 Kp*error 进行计算。

关于sqrt_controller详见：详解APM的开方控制器sqrt_controller

// Update rate_target_ang_vel using attitude_error_rot_vec_rad
Vector3f AC_AttitudeControl::update_ang_vel_target_from_att_error(const Vector3f &attitude_error_rot_vec_rad)
{
    Vector3f rate_target_ang_vel;
    // Compute the roll angular velocity demand from the roll angle error
    if (_use_sqrt_controller) {
        rate_target_ang_vel.x = sqrt_controller(attitude_error_rot_vec_rad.x, _p_angle_roll.kP(), constrain_float(get_accel_roll_max_radss() / 2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS), _dt);
    } else {
        rate_target_ang_vel.x = _p_angle_roll.kP() * attitude_error_rot_vec_rad.x;
    }
    // todo: Add Angular Velocity Limit

    // Compute the pitch angular velocity demand from the pitch angle error
    if (_use_sqrt_controller) {
        rate_target_ang_vel.y = sqrt_controller(attitude_error_rot_vec_rad.y, _p_angle_pitch.kP(), constrain_float(get_accel_pitch_max_radss() / 2.0f, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS), _dt);
    } else {
        rate_target_ang_vel.y = _p_angle_pitch.kP() * attitude_error_rot_vec_rad.y;
    }
    // todo: Add Angular Velocity Limit

    // Compute the yaw angular velocity demand from the yaw angle error
    if (_use_sqrt_controller) {
        rate_target_ang_vel.z = sqrt_controller(attitude_error_rot_vec_rad.z, _p_angle_yaw.kP(), constrain_float(get_accel_yaw_max_radss() / 2.0f, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS, AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS), _dt);
    } else {
        rate_target_ang_vel.z = _p_angle_yaw.kP() * attitude_error_rot_vec_rad.z;
    }
    // todo: Add Angular Velocity Limit
    return rate_target_ang_vel;
}

角度前馈量旋转解释

有人可能会对这段代码的旋转过程有所疑惑，然而实际上在我上一篇博文（Ardupilot前馈及平滑函数input_euler_angle_roll_pitch_yaw解析）已经对这个问题进行了解释，这边再细说一下。

	// 角速度前馈旋转到机体坐标中（对该处有疑问的详见本文后面）
	// _attitude_target_ang_vel是一个全局变量，表示期望姿态的期望角速率，是由input_euler_angle_roll_pitch_yaw等类似函数获取到的前馈速率
	// attitude_target_ang_vel_quat在此处表明是一个向量[0,v]
	Quaternion attitude_target_ang_vel_quat = Quaternion(0.0f, _attitude_target_ang_vel.x, _attitude_target_ang_vel.y, _attitude_target_ang_vel.z);
	// 运算过程注意四元数和旋转矩阵一样是可以上下标相互约去的，此处以Q(a)(b)形式表明上标为a，下标为b，表示b向a的旋转过程
	// attitude_vehicle_quat前面说过是表示cb->n旋转的单位四元数，求逆后表示n->cb
	// to_to_from_quat = Q(cb)(n)*Q(n)(tb)=Q(cb)(tb)
	// 表明获取的四元数to_to_from_quat表示tb->cb的旋转，计算公式也可视作期望姿态与当前姿态的作差
	Quaternion to_to_from_quat = attitude_vehicle_quat.inverse() * _attitude_target_quat;
	// 将tb下的期望角速率旋转至cb下获取当前姿态的角速率期望前馈叠加量desired_ang_vel_quat
    Quaternion desired_ang_vel_quat = to_to_from_quat.inverse() * attitude_target_ang_vel_quat * to_to_from_quat;

可以看到这段代码将前馈期望速度从期望姿态转换到了当前姿态下，那么问题就是，为什么这个期望速度是在期望姿态下而不是原本就在当前姿态的呢？

因为在之前的input_euler_angle_roll_pitch_yaw()函数（其他类似）中，通过对本次输入的期望姿态和自驾仪中当前保存的未更新前的期望姿态作差求出姿态误差，然后再得到了_attitude_target_ang_vel这个期望角速率。

而在现在我们这个函数中，我们是通过期望姿态与当前姿态作差得到的姿态误差来求取期望角速率_rate_target_ang_vel。

因此_attitude_target_ang_vel是在期望姿态tb下的期望角速率，而_rate_target_ang_vel则是在当前姿态cb下的期望角速率，叠加前需要将_attitude_target_ang_vel从tb转换到cb下。

姿态更新部分

    if (_rate_bf_ff_enabled) {
        // 当开启机体速率前馈时，实时计算更新期望姿态四元数，每一次更新都需要将其单位化
        Quaternion attitude_target_update_quat;
        attitude_target_update_quat.from_axis_angle(Vector3f(_attitude_target_ang_vel.x * _dt, _attitude_target_ang_vel.y * _dt, _attitude_target_ang_vel.z * _dt));
        _attitude_target_quat = _attitude_target_quat * attitude_target_update_quat;
        _attitude_target_quat.normalize();
    }

该部分仍然跟之前讲解的input_euler_angle_roll_pitch_yaw()及类似函数有关，在input_euler_angle_roll_pitch_yaw()中如果开启了前馈，那么会计算出前馈速率，但是并没有将本次输入的期望姿态更新到当前保存的期望姿态中；而如果没有开启前馈，则会将本次输入的期望姿态更新到当前期望姿态，然后将前馈速率置0。

因此，此处解决了我们之前说过的如果开启前馈，姿态更新将在attitude_controller_run_quat()中更新的疑惑。

 

参考资料：

如何用四元数表示反向旋转，类似于旋转矩阵的转置表示反向旋转？

[飞控]姿态误差(三)-四元数和轴角求误差

 

如有错误请及时留言告知。