先进非线性控制方法 INDI 快速部署到PX4用于四旋翼控制（part2）

一、PX4 v11 的姿态控制解析

这一部分主要介绍PX4核心控制算法是怎么实现的。其实核心的控制算法部分很简单，大部分代码都是在进行各种分情况讨论，各种安全检查。下面简单介绍一下，希望大家看了之后有点收获，能给我点个赞，嘿嘿。

1. 角度环控制

这部分代码在 src/modules/mc_att_control里

主程序在 mc_att_control_main.cpp 里

简单介绍就是先生成姿态角指令（roll, pitch, yaw的指令，即attitude_setpoint）

姿态指令生成👆（在函数最后publish了）

然后根据这个指令计算姿态角速度指令（即_rates_sp）

计算姿态角速度指令👆（_attitude_control是AttitudeControl类的实例，具体实现在同文件夹下的AttitudeControl里）

然后把姿态角速度指令发布

发布姿态角速度指令👆

2. 角速度环控制

这部分代码在 src/modules/mc_rate_control里

即根据姿态角速度指令来计算得到期望力矩，这也是本文需要改的部分。

主程序在 MulticopterRateControl.cpp 里

简单介绍就是先订阅 角速度指令rates_sp 的消息

获取角速度指令👆

然后计算控制力矩指令

根据当前的角速度和角速度指令计算控制力矩指令（就是这个att_control）👆

接着发布这个att_control

发布控制力矩指令att_control👆

3. 控制分配

这部分内容也挺多的，主要是在 src/lib下的 mixer 和 mixer_module里，四旋翼的具体实现是在src/lib/mixer/MultirotorMixer/MultirotorMixer.cpp 里
直接看分配的主要函数：mix

多旋翼分配实现👆
其实就是先获取控制力矩指令，然后分情况调用函数进行分配。

比如在使用jmavsim仿真时调用的是 mix_airmode_disabled 函数

mix_airmode_disabled中分配的实现👆
这部分就是具体的实现过程了，首先是先不考虑 yaw 进行分配，然后最后再单独分配 yaw.

二、简易INDI如何部署到PX4

1. 获取角加速度 和 电机转速测量值

经过 part1 的推导之后我们知道，INDI需要得到角加速度的值来进行计算，猜怎么着？我惊奇的发现 PX4 v11 里这个角加速度值已经算出来了，之前的版本里都是没有这个量的，而且这个角加速度的量在现在的 PX4 里也没有用到，不知道他们是要搞什么。

（1）角加速度

接下来讲一讲这个角加速度的始末：

这个角加速度怎么来的呢？
在src/modules/sensors/vehicle_angular_velocity是用来计算角速度和角加速度的，其中的.cpp 文件里有计算的流程

计算角速度和角加速度的过程👆

简单来说就是：
  获取陀螺仪数据并矫正热误差
→   矫正运行中的偏差
→   应用陷波滤波器对上面得到的角速度值原始值进行滤波
→   对滤波后的角速度进行微分得到角加速度值
→   把角速度和角加速度存一下
→   对两个值应用低通滤波器进行平滑处理

处理完了就把两个量发布

发布角速度和角加速度量👆

于是我们就可以在msg里找到 vehicle_angular_velocity.msg 和 vehicle_angular_acceleration.msg 这两个文件了，也就是说可以通过 μORB 订阅到这两个topic了。另外，这个 vehicle_angular_acceleration.msg 目前还没有存到日志文件里，可以通过简单处理把他存到日志文件里，便于后面的分析。

（2）转速

经过 part1 的推导之后我们知道，INDI需要得到 电机转速 的值来进行计算，猜怎么着？我惊奇的发现 PX4 v11 里这个电机转速值已经估计出来了，之前的版本里都是没有这个量的，而且这个电机转速的量在现在的 PX4 里也没有用到，不知道他们是要搞什么。（他们不会要把姿态控制换成INDI吧。。）

这个电机转速测量值（估计值）我是先在msg文件夹下发现的，就是 rpm.msg 。但是这个东西我还妹有完全搞明白，不知道是根据什么估算的电机转速，有兴趣的朋友可以了解一下px4里这个 rpm 是怎么估计的。（我知道有的穿越机电调可以回传电机转速，这个就比较成熟了。还有篇文章是用类似鼠标里那个光栅编码器来测转速的，不过这个可能撞一下就坏了，不太好）

但是！要说但是了。
我试了下，这个转速估计值在jmavsim仿真里是没有的计算出来的，没试gazebo。

另外，用这个电机转速还有个问题在于，px4里实际输出的电机指令也不是电机的转速，而是分配之后，到每个电机头上的归一化的（可以认为是推力）值计算出的 pwm 值，所以考虑电机转速回传之后，会带来很多麻烦，我嫌麻烦还没有分析这个，所以暂且按下不表。

于是我的处理方法是，用类似的滞后环节去模拟电机的响应过程。也就是对电机进行建模，然后估计电机的当前转速。目前的简易做法是计算出力矩指令之后，加上一个滞后作为实际得到的控制力矩

2. 具体实现过程

（0）简要回顾与分析

INDI 的原理在 part1 中已经介绍过，下面简要回顾并分析下需要修改的地方：

对于角速度控制环来说，令指令跟踪误差为$e_{\Omega }=\Omega -{\Omega }_r$
则角速度跟踪误差的动态方程可以写成
${\dot{e}}_{\Omega }={\dot{\Omega }}_0-{\dot{\Omega }}_r+g(x_{states})\Delta {\omega }^2$

那么根据 INDI 的原理可以得到（电机引起的）力矩的增量
$G_M\Delta {\omega }^2=-J({\dot{\Omega }}_0-{\dot{\Omega }}_r+K_p{e}_{\Omega })$

然后可以得到力矩的指令：
$M_c=G_M\cdot {\omega }_0^2+G_M\cdot \Delta {\omega }^2$
其中${\omega }_0^2$是估计出来的或者测量回来的转速。

或者写成$M_c=M_{c_0}+G_M\cdot \Delta {\omega }^2$
其中$M_{c_0}$是计算或者估计出来的当前控制力矩

也就是说，需要改的或者添加的就是 (1)获取角加速度，(2)获取指令微分，(3)修改控制律公式，(4)获取$M_{c_0}$。（ (2)可选，因为这个东西我看有的控制方法里用，有的不用，比如在PID的误差 $e$ 微分里就包含这个指令微分，但是如果用测量值 $y$ 计算微分量的话就不包含这个指令微分）

---

需要修改的代码都在 src/modules/mc_rate_control/Multicopter

---

（1）获取角加速度

角加速度已经发布了，所以直接订阅就行了
怎么订阅，有个简单的方法，直接按照角速度订阅的方式来一遍就行了：

订阅角速度👆，照例订阅一下角加速度即可

（2）获取指令微分

直接对订阅得到的 rates_sp 进行微分即可，不过这一步我省略了…

（3）修改控制律公式

角速率环的控制律公式在src/modules/mc_ate_control/RateControl/RateControl.cpp里，就一行：

角速度PID控制律👆

这是经典的PID控制，这里我们如果要改成INDI只需要：

角度环INDI控制律👆

这两个式子就是在实现
        $G_M\Delta {\omega }^2=-J({\dot{\Omega }}_0+K_p{e}_{\Omega })$
     和 $M_c=M_{c_0}+G_M\cdot \Delta {\omega }^2$

（$e_{\Omega }$的符号不对是因为我的误差的定义和PX4正好相反）
另外解释下，其中我在yaw通道加了个微分误差factor，因为仿真的时候发现yaw通道老震荡，所以多加了微分项抑制一下。按理说是不需要微分项的，后面可能要看一下为什么会出现震荡情况。

另外，在本文中这个$G_M\Delta {\omega }^2=-J({\dot{\Omega }}_0+K_p{e}_{\Omega })$公式里的$J$就相当于一个可整定的参数。原因有四点：（1）我这里没有修改Mixer里的$G_M$，（2）操纵导数本来就比较难获得（需要螺旋桨的力系数和力矩系数），（3）INDI里的操纵导数本来就是一个可以用来整定的量，（4）操纵力矩分配之后映射到的是输出给电机的PWM信号，而PWM信号和电机转速之间的关系也是个黑箱

（4）获取 $M_{c_0}$

$M_{c_0}$ 也就是上面 角度环INDI控制律 图里的torque_prev.
这里我是直接用一个低通滤波器作为控制力矩响应模型的。
意思就是说，真实的控制力矩响应比控制力矩指令就差了一个低通滤波器过程。即：
$M_{c_0}=M_c\cdot G_{lpf}$

实现起来就是这么一句：

torque_prev = _lp_filters_rate.apply(torque1);

由于懒，我这里用的 filter 还是px4自带的low pass filter。

！！！！！这里需要注意的一点是：由于之前获取角加速度的时候，也用了低通滤波器和陷波滤波器，导致角加速度与当前的真实角加速度有滞后，所以为了让测量或者估计的真实力矩响应与角速度同步，需要在测量或者估计的真实力矩上 再 加上同样的滞后。

OK，这四部分就介绍完了，剩下的就是调调参数的事情。我大概调了半天，才调到和之前的 PID 效果差不多的样子。

结语

目前我改的这个 PX4 v11 不是一个稳定的版本，是开发中的版本（很神奇地在里面发现了角加速度估计值和电机转速估计值，不知道开发人员是在憋什么大招）。但是要修改其他版本其实也是大同小异，毕竟v11里只是把角速度控制单独拿出来放到一个文件夹下了而已。

INDI的好处是鲁棒性和容错性比较强，在模型失配程度比较高的情况下也可以得到比较好的控制效果，另外在出现故障或者外部扰动的时候也能快速恢复稳定。这实际上得益于INDI能够实时将系统动态通过角加速度反馈到控制器中，比如当外部有强风吹的时候，PID需要有积分项才能重新实现在强风影响下的稳定，相当于是一个对强风影响的估计，这个过程是比较慢的，但是反馈角加速度可以快速把这个影响反馈到控制器中。大概就是这个意思，如果要深入理解可能要结合仿真比较好。

就介绍到这里，如果大家看了之后有什么想法，欢迎交流。