【飞控】姿态控制-开方控制器和倾斜角转加速度函数

函数名:sqrt_controller(开方控制器)
位置:libraries\AC_AttitudeControl\AC_AttitudeControl.cpp(1047行)
用途:经过改良的kp控制,当kp过大时,会呈现根号曲线,使响应变软

_vel_target.z = AC_AttitudeControl::sqrt_controller(_pos_error.z, _p_pos_z.kP(), _accel_z_cms, _dt);

原函数

float AC_AttitudeControl::sqrt_controller(float error, float p, float second_ord_lim, float dt)
{                                       
    float correction_rate; 
    if (is_negative(second_ord_lim) || is_zero(second_ord_lim)) {
        //二阶限制为零或负数 在这为 _accel_z_cms
        correction_rate = error*p;  
    } else if (is_zero(p)) {
        //p为0 在这_p_pos_z.kP()=1
        if (is_positive(error)) {
            correction_rate = safe_sqrt(2.0f*second_ord_lim*(error));
        } else if (is_negative(error)) {
            correction_rate = -safe_sqrt(2.0f*second_ord_lim*(-error));
        } else {
            correction_rate = 0.0f;
        }
    } else {//zing-note:正常进这个函数
        float linear_dist = second_ord_lim/sq(p);
        if (error > linear_dist) {//误差非常大
            correction_rate = safe_sqrt(2.0f*second_ord_lim*(error-(linear_dist/2.0f)));
        } else if (error < -linear_dist) {//误差非常小
            correction_rate = -safe_sqrt(2.0f*second_ord_lim*(-error-(linear_dist/2.0f)));
        } else {//正常状态
            correction_rate = error*p;
        }
    }
    if (!is_zero(dt)) {
        return constrain_float(correction_rate, -fabsf(error)/dt, fabsf(error)/dt);
    } else {
        return correction_rate;
    }
}

可以看出这个函数是在KP控制上加了一些限制,如果超过限制就做一些处理,如果没有超过限制,这个控制的效果就是普通的KP,
_vel_target.z=_pos_error.z*_p_pos_z.kP()
在物理上可以理解为这个KP=1/dt,所以这个算法可以认为是位置误差微分得到速度,这个思路在APM中被广泛应用。

那么经过限制后会有一个什么效果呢?最直观的方式就是把函数图像画出来,假设误差是从[-10,10]匀速变化,经过这个控制器在不同p值下的输出曲线。

可以很明显的看出,在没有达到限制时,输出曲线斜率随着p值增大而变大,慢慢的限制要求达到,曲线慢慢逼近一条曲线,p越大输出越接近这条曲线,这条曲线类似于根号函数的曲线,所以这个函数取名为开方控制器。

这个控制器被广泛应用在APM的控制过程中,甚至是用在处理遥控器的输入信号,来控制遥控器的软硬程度,看的代码多了就发现,大家思路都差不多,区别就是这些不起眼的限制条件,先学习框架在学习细节,比如这个误差乘kp得到微分的思路你学会了吗?

函数名:lean_angles_to_accel
位置:libraries\AC_AttitudeControl\AC_PosControl.cpp
用途:倾斜角转加速度
原函数:

void AC_PosControl::lean_angles_to_accel(float& accel_x_cmss, float& accel_y_cmss) const
{   
    accel_x_cmss = (GRAVITY_MSS * 100) * (-_ahrs.cos_yaw() * _ahrs.sin_pitch() * _ahrs.cos_roll() - _ahrs.sin_yaw() * _ahrs.sin_roll()) / MAX(_ahrs.cos_roll()*_ahrs.cos_pitch(), 0.5f);
    accel_y_cmss = (GRAVITY_MSS * 100) * (-_ahrs.sin_yaw() * _ahrs.sin_pitch() * _ahrs.cos_roll() + _ahrs.cos_yaw() * _ahrs.sin_roll()) / MAX(_ahrs.cos_roll()*_ahrs.cos_pitch(), 0.5f);
}

上次讲了加速度转倾斜角,不知道有没有人去思考倾斜角转加速度呢?
我们已知倾斜角,也就是已知旋转矩阵,使用该旋转矩阵,将机体坐标系下的加速度转换到地理坐标系下即可。
倾斜角转加速度的思路是一样的,假设当前飞机只受重力和旋翼产生的支持力,保持z轴平衡状态。
所以有:

$$R_B^NF_b=\left[\begin{matrix} \times \\\times \\-mg \end{matrix}\right]$$

其中$R_B^N$是机体坐标系转NED坐标系的旋转矩阵。
可得:

$$\left[\begin{matrix} \times & \times & \cos\psi\sin\theta\cos\phi+\sin\psi\sin\phi \\ \times & \times & \sin\psi\sin\theta\cos\phi-\cos\psi\sin\phi \\ \times & \times & \cos\psi\sin\theta \end{matrix} \right]\left[\begin{matrix} 0 \\0 \\f \end{matrix}\right] =\left[\begin{matrix} \times \\\times \\-mg \end{matrix}\right]$$

所以有:

$$F_b=\left[\begin{matrix} 0 \\0 \\\frac{-mg}{\cos\theta\cos\phi} \end{matrix}\right]$$

因为$F_B$产生在NED坐标系下的加速度有:

$$\left[\begin{matrix} F_Nx \\F_Ny \\F_Nz \end{matrix}\right] =\left[\begin{matrix} \times & \times & \cos\psi\sin\theta\cos\phi+\sin\psi\sin\phi \\ \times & \times & \sin\psi\sin\theta\cos\phi-\cos\psi\sin\phi \\ \times & \times & \times \end{matrix}\right] \left[\begin{matrix} 0 \\0 \\\frac{-mg}{\cos\theta\cos\phi} \end{matrix}\right]$$

所以:

$$\begin{matrix} F_Nx=(-mg)*(\sin\theta\cos\psi/\cos\theta+\sin\psi\sin\phi/\cos\theta\cos\phi)\\ F_Ny=(-mg)*(\sin\theta\sin\psi/\cos\theta-\cos\psi\sin\phi/\cos\theta\cos\phi) \end{matrix}$$

至于最后为什么有个MAX,作者是这样说的

3 is dependent on there being a large lean angle and angle boost running. If we are in ACRO and at a large lean angle then you are probably holding altitude and the Fbz is probably closer to -mg/(cos_theta * cos_rho). Using the result of 3 will also have the effect of the aircraft going through the unconstrained acceleration part of the Fbz curve on it's way to the low angle causing a large acceleration as it levels. So 3 is not a good option in general.

1 and 2 are reasonable and both will result in a good starting point. 2 will result in a twitch as you say but due to the acceleration limiting in most navigation modes 1 will to. In any case we have to limit the angle to something to prevent the cos terms going to zero so we need some version of 2.

For now I will leave it using the 2 version until we update the output of the navigation to a full 3D acceleration controller.

其实这个限制有三种情况,最终选择了第二种限制方法得到了现在的函数。
这个函数的讨论的全过程在
https://github.com/ArduPilot/ardupilot/issues/7267
可以看看这个最终函数是如何修改得到的。

看完姿态控制觉得姿态部分真的是代码的精华所在,以前只是走马观花的了解了大概真是浪费,之后的文章后把姿态控制进行详细的分析,来感受一下精华的魅力。

关注微信公众号,回复【姿态】即可获取画sqrt_controller函数图的python源码,同时欢迎加我的微信交流飞控心得,如果能转发分享那就更棒棒了。