STM32实现四驱小车（三）传感任务

本文接上一篇STM32实现四驱小车（二）通信任务——遥控器SBUS通信，在今天的文章中解决小车的姿态角测量问题，实现操作系统中的传感任务。对小车来说只需要测量航向角（YAW），方法就是使用姿态角传感器，通过串口直接读三轴角度，简单的不能再简单有木有。但是为了知识体系的完整性（也为了增加文章的逼格哈哈），本文会从传感器测量原理到姿态估计讲起，讲解标准对标多旋翼的姿态解算与状态估计。本文的结构是高高拿起，轻轻放下。众位看官如果只是想实现功能看最后二节代码部分就行了。

本文理论部分主要参考了全权老师的《多旋翼飞行器设计与控制》。

三轴加速度计是一种惯性传感器，能够测量物体的比力，即去掉重力后的整体加速度。这里我们需要记住，使用三轴加速度计的最重要的功能是测量角度，如图所示，弹簧压缩量由加速度计与地面的角度决定，比力能通过弹簧压缩长度来测量。因此在没有外力作用的情况下，加速度计能够准确的测量俯仰角和滚转角，且没有累计误差。 这句话很重要，它是实现互补滤波、卡尔曼滤波的基础。大家只需要知道加速度计的特性是低频特性好，高频特性差。 这里直接给出加速度计近似测量俯仰角和滚转角的公式： θ m = arcsin ⁡ ( a x b m g ) , ϕ m = − arcsin ⁡ ( a y b m g cos ⁡ θ m ) {{\theta }_{m}}=\arcsin \left( \frac{{{a}_{{{x}_{b}}m}}}{g} \right), {{\phi }_{m}}=-\arcsin \left( \frac{{{a}_{{{y}_{b}}m}}}{g\cos {{\theta }_{m}}} \right) θm​=arcsin(gaxb​m​​),ϕm​=−arcsin(gcosθm​ayb​m​​)

MEMS陀螺仪是基于科里奥利力工作的传感器，科里奥里力是对直线运动物体相对于旋转坐标系产生偏移的一种直观描述。因为科里奥利力正比于角速度，所以根据电容量的变化可以计算角速度。总之，大家知道这玩意可以测量三轴角速度就行了，三轴角速度积分就可以求得三轴角度。它的特点是高频特性好，低频特性差。

磁力计是提供导航及基于位置服务的重要组成部分，一般利用各向异性磁致电阻或者霍尔效应来检测空间中的磁感应强度。此外，基于洛伦兹力的磁力计在不断研究和发展。基于洛伦兹力原理，电磁场会激发电磁力，进而改变电路中的电容大小……

大家只需要知道这玩意是测量三轴磁场强度的就行了，其中最重要的是测量偏航角。磁力计测量的偏航角在滤波中将会作为观测值。

状态估计是控制与决策的基础，具有十分重要的作用。传感器的测量存在大量噪声，一方面，有些信息无法直接测量得到，需要被估计出来，另一方面，传感器的信息存在冗余。所以需要进行多传感器融合来提高状态估计的精确性和鲁棒性。对机器人来说，状态估计包括姿态估计、速度估计、位置估计等。在本文中我们只关注姿态估计。

姿态估计的主要目的是估计姿态角，姿态角的表示方法有欧拉角、旋转矩阵、四元数三种常见形式。姿态信息主要利用三轴加速度计、三轴陀螺仪、三轴磁力计的数据，通过互补滤波或卡尔曼滤波获得。三轴陀螺仪的动态响应快，测量精度高，但求解姿态角时需要对角速度积分，会产生累计误差（积分漂移）。 三轴加速度计和三轴磁力计可以得到稳定的姿态角，无累计误差，但动态响应特性差、测量噪声大。

这里先下一些定义，三轴姿态角向量表示为 s = [ ϕ , θ , ψ ] T s={{[\phi ,\theta ,\psi ]}^{T}} s=[ϕ,θ,ψ]T，分别表示滚转角、俯仰角、偏航角。机体旋转角速度为 b w = [ w x b , w y b , w z b ] T ^{\text{b}}w={{[{{w}_{{{x}_{b}}}},{{w}_{{{y}_{b}}}},{{w}_{{{z}_{b}}}}]}^{T}} bw=[wxb​​,wyb​​,wzb​​]T，在滚转角和俯仰角变化较小的情况下可以近似为 [ ϕ ˙ , θ ˙ , ψ ˙ ] T = [ w x b , w y b , w z b ] T {{[\dot{\phi },\dot{\theta },\dot{\psi }]}^{T}}={{[{{w}_{{{x}_{b}}}},{{w}_{{{y}_{b}}}},{{w}_{{{z}_{b}}}}]}^{T}} [ϕ˙​,θ˙,ψ˙​]T=[wxb​​,wyb​​,wzb​​]T

①俯仰角

俯仰角的拉氏变换表示为 θ ( s ) = 1 τ s + 1 θ ( s ) + τ s τ s + 1 θ ( s ) \theta (s)=\frac{1}{\tau s+1}\theta (s)+\frac{\tau s}{\tau s+1}\theta (s) θ(s)=τs+11​θ(s)+τs+1τs​θ(s) 其中 1 / ( τ s + 1 ) 1/(\tau s+1) 1/(τs+1) 表示低通滤波器的传递函数， τ ∈ R + \tau \in {{\mathbb{R}}_{\text{+}}} τ∈R+​表示时间常数； τ s / ( τ s + 1 ) \tau s/(\tau s+1) τs/(τs+1)表示高通滤波器的传递函数。考虑到加速度计测量得到的俯仰角无漂移，但噪声大，可将俯仰角测量值建模为(加速度计的测量值)： θ m = θ + n θ {{\theta }_{m}}=\theta +{{n}_{\theta }} θm​=θ+nθ​ 其中， n θ ∈ R n_{\theta} \in \mathbb{R} nθ​∈R 表示高频噪声， θ \theta θ 表示俯仰角真值。

考虑到角速度积分得到的俯仰角噪声小，但漂移大，可以建模为 w y b m ( s ) s = θ ( s ) + c 1 s \frac{{{w}_{{{y}_{b}}m}}(s)}{s}=\theta (s)+c\frac{1}{s} swyb​m​(s)​=θ(s)+cs1​ 其中， w y b m ( s ) / s {{w}_{{{y}_{b}}m}}(s)/s wyb​m​(s)/s 表示对角速率积分得到俯仰角的拉氏变换， c / s c/s c/s 表示常值漂移的拉氏变换， w y b m {{w}_{{{y}_{b}}m}} wyb​m​是陀螺仪测量值。

因此针对俯仰角，线性互补滤波器的标准形式表示为 θ ^ ( s ) = 1 τ s + 1 θ m ( s ) + τ s τ s + 1 ( 1 s w y b m ( s ) ) \hat{\theta }(s)=\frac{1}{\tau s+1}{{\theta }_{m}}(s)+\frac{\tau s}{\tau s+1}\left( \frac{1}{s}{{w}_{{{y}_{b}}m}}(s) \right) θ^(s)=τs+11​θm​(s)+τs+1τs​(s1​wyb​m​(s)) 下面将给出线性互补滤波器能够精确估计姿态角的解释。将上式整理为 θ ^ ( s ) = θ ( s ) + ( 1 τ s + 1 n θ ( s ) + τ s τ s + 1 c 1 s ) \hat{\theta }(s)=\theta (s)+\left( \frac{1}{\tau s+1}{{n}_{\theta }}(s)+\frac{\tau s}{\tau s+1}c\frac{1}{s} \right) θ^(s)=θ(s)+(τs+11​nθ​(s)+τs+1τs​cs1​) 可以看出，高频噪声 θ m ( s ) {{\theta }_{m}}(s) θm​(s) 通过低通滤波器 1 / ( τ s + 1 ) 1/(\tau s+1) 1/(τs+1)基本上衰减为0，而低频信号 c / s c/s c/s通过高通滤波器 τ s / ( τ s + 1 ) \tau s/(\tau s+1) τs/(τs+1) 也基本衰减为0。因此可以认为 θ ^ ( s ) ≈ θ ( s ) \hat{\theta }(s)\approx \theta (s) θ^(s)≈θ(s)。在整个过程中，低通滤波器将加速度计测量值无漂移的优势保留下来，而高通滤波器将陀螺仪噪声小的优势保留下来，互补滤波器实现了无偏估计。

为了用计算机（单片机）实现滤波器算法，需要将滤波器转换为离散时间差分形式。通过一阶向后差分法，将 s s s表示为 s = ( 1 − z − 1 ) / T s s=(1-{{z}^{-1}})/{{T}_{s}} s=(1−z−1)/Ts​，上面的互补滤波器标准形式转化为 ϕ ^ ( k ) = τ τ + T s ( ϕ ^ ( k − 1 ) + T s w x b m ( k ) ) + T s τ + T s ϕ m ( k ) \hat{\phi }(k)=\frac{\tau }{\tau +{{T}_{s}}}\left( \hat{\phi }(k-1)+{{T}_{s}}{{w}_{{{x}_{b}}m}}(k) \right)+\frac{{{T}_{s}}}{\tau +{{T}_{s}}}{{\phi }_{m}}(k) ϕ^​(k)=τ+Ts​τ​(ϕ^​(k−1)+Ts​wxb​m​(k))+τ+Ts​Ts​​ϕm​(k) 取 τ / ( τ + T s ) = 0.95 \tau /(\tau +{{T}_{s}})=0.95 τ/(τ+Ts​)=0.95 ，则 T s / ( τ + T s ) = 0.05 {{T}_{s}}/(\tau +{{T}_{s}})=0.05 Ts​/(τ+Ts​)=0.05 ，控制器实现俯仰角解算的滤波算法为： θ ^ ( k ) = 0.95 ( θ ^ ( k − 1 ) + T s w y b m ( k ) ) + 0.05 θ m ( k ) \hat{\theta }(k)=0.95\left( \hat{\theta }(k-1)+{{T}_{s}}{{w}_{{{y}_{b}}m}}(k) \right)+0.05{{\theta }_{m}}(k) θ^(k)=0.95(θ^(k−1)+Ts​wyb​m​(k))+0.05θm​(k)

到这里单片机实现互补滤波的算法就出来了，Ts是采样周期， w y b m ( k ) {{w}_{{{y}_{b}}m}}(k) wyb​m​(k)与 θ m ( k ) {{\theta }_{m}}(k) θm​(k)分别是第k次测量得到的陀螺仪俯仰角速度测量值和加速度计俯仰角测量值。

② 滚转角

滚转角的原理和俯仰角的原理是一样的，这里就不推导了，直接给出结果。 ϕ ^ ( k ) = τ τ + T s ( ϕ ^ ( k − 1 ) + T s w x b m ( k ) ) + T s τ + T s ϕ m ( k ) \hat{\phi }(k)=\frac{\tau }{\tau +{{T}_{s}}}\left( \hat{\phi }(k-1)+{{T}_{s}}{{w}_{{{x}_{b}}m}}(k) \right)+\frac{{{T}_{s}}}{\tau +{{T}_{s}}}{{\phi }_{m}}(k) ϕ^​(k)=τ+Ts​τ​(ϕ^​(k−1)+Ts​wxb​m​(k))+τ+Ts​Ts​​ϕm​(k) 取 τ / ( τ + T s ) = 0.95 \tau /(\tau +{{T}_{s}})=0.95 τ/(τ+Ts​)=0.95 ，则 T s / ( τ + T s ) = 0.05 {{T}_{s}}/(\tau +{{T}_{s}})=0.05 Ts​/(τ+Ts​)=0.05 ，控制器实现滚转角解算的滤波算法为： ϕ ^ ( k ) = 0.95 ( ϕ ^ ( k − 1 ) + T s w x b m ( k ) ) + 0.05 ϕ m ( k ) \hat{\phi }(k)=0.95\left( \hat{\phi }(k-1)+{{T}_{s}}{{w}_{{{x}_{b}}m}}(k) \right)+0.05{{\phi }_{m}}(k) ϕ^​(k)=0.95(ϕ^​(k−1)+Ts​wxb​m​(k))+0.05ϕm​(k)

③ 偏航角

航向角的互补滤波与滚转角、俯仰角不一样。俯仰角和滚转角的观测值来自于加速度计，而加速度计无法测量航向角，所以需要另外寻找航向角的观测值。三轴磁力计可以提供准确的偏航角，此外GPS接收机也可以提供偏航角。将两者测量得到的偏航角分别表示为 ψ GPS {{\psi }_{\text{GPS}}} ψGPS​和 ψ mag {{\psi }_{\text{mag}}} ψmag​。一种方法是定义偏航角为两者的加权和。即 ψ m = ( 1 − α ψ ) ψ G P S + α ψ ψ m a g \psi_{\mathrm{m}}=\left(1-\alpha_{\psi}\right) \psi_{\mathrm{GPS}}+\alpha_{\psi} \psi_{\mathrm{mag}} ψm​=(1−αψ​)ψGPS​+αψ​ψmag​ α ψ ∈ [ 0 , 1 ] {{\alpha }_{\psi }}\in [0,1] αψ​∈[0,1]是加权因子，它决定了是更相信GPS还是更相信磁罗盘。

得到 ψ m {{\psi }_{\text{m}}} ψm​之后，得到偏航角估计为 ψ ^ ( k ) = τ τ + T s ( ψ ^ ( k − 1 ) + T s w z b m ( k ) ) + T τ + T s ψ m ( k ) \hat{\psi }(k)=\frac{\tau }{\tau +{{T}_{s}}}\left( \hat{\psi }(k-1)+{{T}_{s}}{{w}_{{{z}_{b}}m}}(k) \right)+\frac{T}{\tau +{{T}_{s}}}{{\psi }_{m}}(k) ψ^​(k)=τ+Ts​τ​(ψ^​(k−1)+Ts​wzb​m​(k))+τ+Ts​T​ψm​(k) 取 τ / ( τ + T s ) = 0.95 \tau /(\tau +{{T}_{s}})=0.95 τ/(τ+Ts​)=0.95 ，则 T s / ( τ + T s ) = 0.05 {{T}_{s}}/(\tau +{{T}_{s}})=0.05 Ts​/(τ+Ts​)=0.05 ，控制器实现偏航角解算的滤波算法为： ψ ^ ( k ) = 0.95 ( ψ ^ ( k − 1 ) + T s w z b m ( k ) ) + 0.05 ψ m ( k ) \hat{\psi }(k)=0.95\left( \hat{\psi }(k-1)+{{T}_{s}}{{w}_{{{z}_{b}}m}}(k) \right)+0.05{{\psi }_{m}}(k) ψ^​(k)=0.95(ψ^​(k−1)+Ts​wzb​m​(k))+0.05ψm​(k)

略

留待以后分解……

打公式太累了……

华丽丽的的略……以后分解

依然是华丽丽的的略……以后分解

小伙伴们不要觉得我敷衍，这些内容都涉及较深的理论含量，写起来费劲、读起来也费劲，咱们稳扎稳打、细水长流。此外这篇文章本来就是写姿态角解算啊啊啊！！目的已经达到了。下面回到我们的小车，我们的目的是测量小车的航向角，从而控制小车走直线和机动转弯。 以下是代码实现。

以上的理论推导没看懂没有任何关系，丝毫不影响咱们实现航向角测量，因为我直接简单粗暴的用了姿态角传感器，其内部就是用的卡尔曼滤波算法，直接输出姿态角，控制器根本就不用做姿态解算算法了哈哈（感觉好不要脸啊）。反正实用为王，事实上模块化就是趋势，人的精力是有限的，怎么快怎么来。不过从知识体系的完整性来说，建议还是把理论部分弄通。

姿态角传感器用的维特智能的JY901B和十轴惯导JY-GPSIMU，价格分别为100多和500多，这两个器件都能测气压、高度，十轴惯导还带GPS， 后面写四旋翼系列我们依然用这两个器件。使用两个传感器是冗余设计，有的要求高的场合用三个四个的都有。至于怎么去使用多个，是只用一个还是多个数据取平均都可以灵活调整。

JY901B的数据输出是IIC和UART两种方式，十轴惯导只有UART输出，本文分别用IIC和UART2读两个器件的角度数据。另外JY901B最好焊在板子上，或者用引脚对插，总之注意惯性器件安装一定要稳固。

创建JY901_IIC.h和JY901_IIC.c两个文件，用于IIC驱动。下面把这两个文件的内容附上，使用的是软件模拟IIC（几乎所有IIC的驱动都都是这样的，我用过的很多板子、器件在这一部分都是一样的）

JY901_IIC.h内容如下，实现函数声明和宏定义

建立一个文件HWT905.c，在这里定义一些全局变量，这些变量在串口中断函数里面要用到。

然后在uart.c文件中添加一个CopeSerial2Data函数，在串口2的中断函数里面调用CopeSerial2Data函数就可以一次将所有数据读到内存中了。

进过以上步骤实现了IIC和UART读取两个器件数据的功能了。但是这还不够，虽然是直接读角度，但也得滤个波好吧，这里用常见的滑动平均滤波，也叫环形滤波器、也叫有限冲击响应滤波……

创建一个sensor.h和sensor.h文件，专门用来处理传感器数据的读取和滤波。sensor.h文件内容如下：

sensor.c内容如下：

这段代码是不是超级好读懂（自我感觉良好~），我所有的命名都是字面意思。在while循环里面一直调用sensorReadAngle(Gyro, Angle)读取角度和角速度（已经经过了滤波处理），然后用读到的值更新当前状态，state是一个结构体，包含了小车的所有状态量。setstate是期望状态的结构体，包括姿态角、速度等，这个值会根据遥控器的摇杆 开关位置来更新。

到这里就实现了UCOS-III操作系统里面的传感任务了，读取到了较为可靠的三轴角度和三轴角速度。这两组值将会在StabilizationTask（姿态控制任务）中大显身手。

这里做下说明，本系列文章笔者重在分享思想、算法，在讲解上会弱化一些基本知识（比如单片机各个外设的原理、单片机编程的基本知识等），在代码的粘贴上会忽视一些底层的驱动代码和无关紧要的部分，事实上上面的代码我都经过删减了，只留下了干货。所以可以说面向的是中高级选手，拿来主义者可以打道回府了，本系列文章不开源，不提供源码，请见谅。