一种双控制器的无人机姿态解算方法及系统与流程

本发明涉及一种无人机控制领域，特别是关于一种双控制器的无人机姿态解算方法及系统。

背景技术：

随着微机电系统技术(mems)和计算机技术的不断发展，低成本的捷联惯性测量单元(imu)广泛应用于在小型飞行器中，捷联惯性测量单元由三轴陀螺仪、三轴加速度计和电子罗盘组成。陀螺仪受温漂的影响。长时间运行漂移严重，并且积分运算会产生积累误差；加速度计容易受到载体震动和运动加速度的影响；电子罗盘容易受外部磁场的影响。因此，这三种传感器都不能单独用于飞行器姿态的测量。需要进行这三种传感器数据的融合。而目前被应用在飞行器姿态角上的数据融合算法有互补滤波、共轭梯度法滤波、扩展卡尔曼滤波以及无迹卡尔曼滤波。

互补滤波原理简单，能够滤去噪声和抑制漂移由于不容易确定合适的高通和低通截止频率，因此精度不高；共轭梯度法滤波和互补滤波相结合在姿态解算中的应用，存在迭代计算量和精度的平衡问题；ekf和ukf是卡尔曼滤波对非线性系统的滤波方法，ekf存在线性化误差和截断误差，对一般的非线性系统影响不大；ukf在收敛速度和估计精度中要高于ekf，但其计算量要比ekf大，并且它们的系统噪声和观测噪声不容易确定，解算的姿态角容易受到噪声干扰。同时ekf与ukf运算量较大会对姿态结算中要求的实时性有较大影响。

技术实现要素：

针对上述问题，本发明的目的是提供一种双控制器的无人机姿态解算方法及系统，其解算精度较高。

为实现上述目的，本发明采取以下技术方案：一种双控制器的无人机姿态解算方法，该方法通过互补滤波算法和ekf融合解算姿态，其包括以下步骤：1)设置一包括主机和从机的无人机姿态解算系统；2)在主机中，采用加速度计和电子罗盘解算的姿态角对陀螺仪解算的姿态角进行漂移补偿，并采用互补滤波算法把加速度计和电子罗盘解算的姿态角与陀螺仪解算的姿态角进行初步融合，滤去噪声并抑制姿态角漂移；3)将互补滤波后的姿态角传输给从机，互补滤波后的姿态角传输给从机，并采用ekf提高姿态角的解算精度，从机每50hz一个周期把解算出来的高精度姿态角传输给主机进行姿态补偿和控制；ekf为扩展卡尔曼滤波算法；4)当主机未接收到从机的姿态角信息时，则以主机互补滤波算法解算出来的姿态角进行姿态控制。

进一步，所述步骤2)中，采用加速度计和电子罗盘解算的姿态角对陀螺仪解算的姿态角进行漂移补偿的方法如下：通过地面坐标系到载体坐标系的转换矩阵将重力和磁场强度在地面坐标系的分量分别转换到载体坐标系中，然后分别与加速度计和电子磁力计单位化后的测量值[axayaz]^t和[mxmymz]^t进行交叉向量积运算，分别求出两向量之间的误差，然后将这两个误差分别经过pi调节器后加在一起，得到了补偿陀螺仪的温漂补偿量，进而消除陀螺仪数据的误差；其中，在地面坐标系中的重力和磁场强度的分量分别为[001]^t和[oxoyoz]^t；ax、ay、az分别对应单位化后载体坐标系下的x,y,z轴的加速度测量值，mx、my、mz分别对应单位化后载体坐标系下的x,y,z轴的磁场强度测量值，0x、0y、oz即当地的磁场强度在x,y,z的分量。

进一步，所述步骤2)中，与加速度计之间的误差计算过程为：首先当地面坐标系和载体坐标系重合时，机体的测量值等同于重力；而当机体转动时，地面坐标系的重力是保持不变的而机体测量值会发生变化；重力乘上转换矩阵得到旋转状态下载体坐标系的标准值三轴分量大小，将这个分量大小与单位化后的加速度计测量的三轴加速度分量进行对比，得到一个误差e1。

进一步，所述步骤2)中，与电子磁力计之间的误差计算过程为：载体坐标系下的磁力计测出的数据通过转换矩阵转换到地面坐标系下的分量，而在地面坐标系中，要对这个分量进行标准化处理；标准化后的数据通过旋转矩阵得到在载体下的分量，与磁力计的测量值进行比较得到误差e2。

进一步，所述步骤2)中，总的误差e＝e1+e2，陀螺仪通过pi调节减去这个误差，然后更新旋转矩阵；旋转矩阵是由陀螺仪数据算出，所以消除陀螺仪数据的误差也就是在消除旋转矩阵的误差。

进一步，所述步骤2)中，互补滤波算法包括以下步骤：2.1)对该无人机进行建模，首先建立两个基本坐标系：惯性坐标系e(oxyz)和载体坐标系b(oxyz),并分别定义欧拉角如下：偏航角ψ：ox在oxy平面投影与x轴夹角；俯仰角θ：oz在oxz平面投影与z轴夹角；翻滚角φ：oy在oyz平面与y轴夹角；2.2)采用四元数来表示转换矩阵为载体坐标系到地面坐标系的转换矩阵，为地面坐标系到载体坐标系的转换矩阵；2.3)根据旋转矩阵得到对应的姿态解算为：偏航角ψ＝arctan(r23/r33)；俯仰角θ＝-arcsin(r13)；翻滚角φ＝arctan(r12/r11)；其中，r23表示地面坐标系到载体坐标系的转换矩阵的第2行第3列；r33表示地面坐标系到载体坐标系的转换矩阵的第3行第3列；r13表示地面坐标系到载体坐标系的转换矩阵的第1行第3列；r12表示地面坐标系到载体坐标系的转换矩阵的第1行第2列；r11表示地面坐标系到载体坐标系的转换矩阵的第1行第1列。

进一步，所述步骤2.2)中，四元数与姿态角之间的转换有如下对应转换关系：

则得到四元数表示的转换矩阵分别为：

进一步，所述步骤3)中，ekf包括以下步骤：3.1)状态预测：为状态变量的先验估计，为上一时刻(k-1)陀螺仪数据累加得到姿态角；是陀螺仪在上一时刻(k-1)测出的三轴角速度值，ωx、ωy、ωz分别为陀螺仪输出的x轴、y轴和z轴角速度,t为运算步长；基于上一时刻的数据预测出的本时刻k的姿态角数据；ak/k-1为状态进一步转移矩阵，为单位矩阵；3.2)获取状态预测协方差矩阵：其中，p(k/k-1)为状态协方差p(k)的先验估计,q为系统噪声协方差矩阵；3.3)进行卡尔曼增益更新：其中，r为观测噪声协方差矩阵；hk＝[100,010,001]^t；3.4)状态更新：其中，代表的是本时刻卡尔曼滤波的最终输出值，z(k)选取的是本时刻经过互补滤波后的姿态角数据；z(k)＝[xamyamzam]^t+vk，vk＝[vxvyvz]^t为观测噪声向量，xam为互补滤波后得到的翻滚角，yam为互补滤波后得到的俯仰角，zam为互补滤波后得到的偏航角；x(k)选取陀螺仪结算出来的角度累加作为状态变量,x(k)＝x(k-1)+w*t，w为陀螺仪输出的三轴角速度,t为运算步长；hk＝[100,010,001]^t；3.5)状态协方差矩阵更新：p(k)＝(i-k(k)hk)p(k/k-1)；其中，i为[111,111,111]^t。

一种用于实现上述方法的双控制器的无人机姿态解算系统，其包括主机、从机、加速度计、陀螺仪、磁力计、电子调速器和无刷电机；所述加速度计、陀螺仪和磁力计将采集到的信息传输至所述主机，所述主机与所述从机进行信息交互，由所述主机将接收到的信息传输给所述从机进行互补滤波处理，所述从机将互补滤波处理后的信息传输给所述主机；所述主机向所述电子调速器传输控制信号，所述电子调速器根据接收到的控制信号调节所述无刷电机的转速。

进一步，所述系统还包括接收机；外部遥控器发送的期望姿态角度通过天线接收的电磁信号传入所述接收机，由所述接收机将期望姿态角度信息传输至所述主机，进而由所述主机控制所述电子调速器工作。

本发明由于采取以上技术方案，其具有以下优点：本发明在不影响解算姿态实时性的前提下，大幅度的提高了解算姿态的精确性，同时有很好的实用性。

附图说明

图1是本发明的系统结构示意；

图2是本发明的解算方法流程示意；

图3a是本发明实施例中静态下用互补滤波得到的俯仰角示意图；

图3b是本发明实施例中静态下用ekf得到的俯仰角示意图；

图3c是本发明实施例中互补滤波、ekf结合算法得到的俯仰角示意图；

图4a是本发明实施例中静态下用互补滤波得到的翻滚角示意图；

图4b是本发明实施例中静态下用ekf得到的翻滚角示意图；

图4c是本发明实施例中互补滤波、ekf结合算法得到的翻滚角示意图。

具体实施方式

下面结合附和实施例对本发明进行详细的描述。

如图1所示，本发明提供一种双控制器的无人机姿态解算系统，其包括主机、从机、加速度计、陀螺仪、磁力计、电子调速器和无刷电机。加速度计、陀螺仪和磁力计将采集到的信息传输至主机，主机与从机进行信息交互，由主机将接收到的信息传输给从机进行互补滤波处理，从机将互补滤波处理后的信息传输给主机。主机向电子调速器传输控制信号，电子调速器根据接收到的控制信号调节无刷电机的转速。

上述实施例中，本发明的系统还包括接收机。外部遥控器发送的期望姿态角度通过天线接收的电磁信号传入接收机，由接收机将期望姿态角度信息传输至主机，进而由主机控制电子调速器工作。

上述各实施例中，主机和从机均采用stm32f407xxxx系列的单片机。

上述各实施例中，加速度计采用三轴加速度计。

如图2所示，本发明还提供一种双控制器的无人机姿态解算方法，该方法通过互补滤波算法和ekf融合解算姿态，其包括以下步骤：

1)设置一包括主机和从机的无人机姿态解算系统；

2)在主机中，采用加速度计和电子罗盘解算的姿态角对陀螺仪解算的姿态角进行漂移补偿，并采用互补滤波算法把加速度计和电子罗盘解算的姿态角与陀螺仪解算的姿态角进行初步融合，滤去噪声并抑制姿态角漂移；

如果单独用陀螺仪解算姿态角，由于陀螺仪长时间运行温漂严重，会造成解算出来的姿态角也发生漂移。本实施例中采用加速度计和电子罗盘解算的姿态角对陀螺仪解算的姿态角进行漂移补偿的方法如下：

通过地面坐标系到载体坐标系的转换矩阵将重力和磁场强度在地面坐标系的分量分别转换到载体坐标系中，然后分别与加速度计和电子磁力计(即电子罗盘)单位化后的测量值[axayaz]^t和[mxmymz]^t进行交叉向量积运算，分别求出两向量之间的误差，然后将这两个误差分别经过pi调节器后加在一起，得到了补偿陀螺仪的温漂补偿量，进而消除陀螺仪数据的误差，也就是在消除旋转矩阵的误差。其中，在地面坐标系中的重力和磁场强度的分量分别为[001]^t和[oxoyoz]^t；ax、ay、az分别对应单位化后载体坐标系下的x,y,z轴的加速度测量值，mx、my、mz分别对应单位化后载体坐标系下的x,y,z轴的磁场强度测量值，0x、0y、oz即当地的磁场强度在x,y,z的分量。

与加速度计之间的误差计算过程为：首先当地面坐标系和载体坐标系重合时(即机体无任何转动)，机体的测量值等同于重力；而当机体转动时，地面坐标系的重力是保持不变的而机体测量值会发生变化，所以重力是一个标准值。由于转换矩阵包含了旋转量即两个坐标系之间的角度值，重力乘上转换矩阵可以得到旋转状态下载体坐标系的标准值三轴分量大小，将这个分量大小与单位化后的加速度计测量的三轴加速度分量进行对比，可以得到一个误差e1。

与电子磁力计之间的误差计算过程为：载体坐标系下的磁力计测出的数据通过转换矩阵转换到地面坐标系下的分量，而在地面坐标系中，磁场强度方向应该是指向北和天两个方向的，所以要对这个分量进行标准化处理。此时标准化后的数据通过旋转矩阵得到在载体下的分量，与磁力计的测量值进行比较得到误差e2。

总的误差e＝e1+e2，陀螺仪通过pi调节减去这个误差。然后更新旋转矩阵。

而旋转矩阵里面包含了两个坐标系之间的角度偏差以及机身的姿态角数据，只要旋转矩阵正常，解算出来的姿态角度就正常，而旋转矩阵是由陀螺仪数据算出，所以消除陀螺仪数据的误差也就是在消除旋转矩阵的误差。

3)将互补滤波后的姿态角传输给从机，互补滤波后的姿态角传输给从机，并采用扩展卡尔曼滤波算法(ekf)提高姿态角的解算精度，从机每50hz一个周期把解算出来的高精度姿态角传输给主机进行姿态补偿和控制。

4)当主机未接收到从机的姿态角信息时，则以主机互补滤波算法解算出来的姿态角进行姿态控制。

上述步骤2)中，以微小型四旋翼无人机为例，互补滤波算法包括以下步骤：

2.1)在对微小型四旋翼无人机进行控制之前，对该四旋翼无人机进行建模，首先建立两个基本坐标系：惯性坐标系e(oxyz)和载体坐标系b(oxyz),惯性坐标系即地面坐标系，方向为东北天(0xyz)；飞行坐标系即载体坐标系。并分别定义欧拉角如下：

偏航角ψ：ox在oxy平面投影与x轴夹角；

俯仰角θ：oz在oxz平面投影与z轴夹角；

翻滚角φ：oy在oyz平面与y轴夹角；

为了使问题简化，不考虑地球自转的影响，将地面近似的看做一个平面，则地面坐标系n系的xn,yn,zn轴分别按照北东天的顺序方向选取，载体坐标系的xb,yb,zb分别按照载体的前、右、下顺序方向选取。载体中心为两坐标系的原点o，如5所示。小写三轴为地面坐标系，大写三轴为载体坐标系。

2.2)采用四元数来表示转换矩阵

载体坐标系到地面坐标系的转换矩阵为：

式中，表示地面坐标系到载体坐标系的转换矩阵，表示载体坐标系到地面坐标系的转换矩阵，b表示的载体坐标系,n为地面坐标系。两者互为转置矩阵。

为避免大量的三角函数运算，采用四元数来表示转换矩阵为：

由于四元数与姿态角之间的转换有如下对应转换关系：

则得到四元数表示的转换矩阵分别为：

2.3)根据旋转矩阵得到对应的姿态解算为：

偏航角ψ＝arctan(r23/r33)；

俯仰角θ＝-arcsin(r13)；

翻滚角φ＝arctan(r12/r11)；

其中，r23表示地面坐标系到载体坐标系的转换矩阵的第2行第3列；r33表示地面坐标系到载体坐标系的转换矩阵的第3行第3列；r13表示地面坐标系到载体坐标系的转换矩阵的第1行第3列；r12表示地面坐标系到载体坐标系的转换矩阵的第1行第2列；r11表示地面坐标系到载体坐标系的转换矩阵的第1行第1列。

上述步骤3)中，卡尔曼滤波算法是一种线性最小方差估计。在飞行器的姿态融合中，有着很高的解算精度。ekf包括以下步骤：

3.1)状态预测：

为状态变量的先验估计，为上一时刻(k-1)陀螺仪数据累加得到姿态角；是陀螺仪在上一时刻(k-1)测出的三轴角速度值，ωx、ωy、ωz分别为陀螺仪输出的x轴、y轴和z轴角速度,t为运算步长；基于上一时刻的数据预测出的本时刻k的姿态角数据；ak/k-1为状态进一步转移矩阵，此处转移矩阵可以选取为单位矩阵；

3.2)获取状态预测协方差矩阵：

其中，p(k/k-1)为状态协方差p(k)的先验估计,q为系统噪声协方差矩阵；

3.3)进行卡尔曼增益更新：

其中，r为观测噪声协方差矩阵；hk＝[100,010,001]^t；

3.4)状态更新：

其中，代表的是本时刻卡尔曼滤波的最终输出值，z(k)选取的是本时刻经过互补滤波后的姿态角数据。

z(k)为观测方程，可以选取加速度计、电子罗盘联合直接解算出来的姿态角。在本实施例中，z(k)由互补滤波后得到的姿态角进行更新，而以往的卡尔曼滤波算法的观测方程都是由加速度计、电子罗盘联合直接解算出来的姿态角度。但是这样的话由于机体震动和外力加速度的影响会使观测方程产生很大误差。本发明通过互补滤波给卡尔曼滤波的观测方程提供了误差较小的平稳数据，使得卡尔曼滤波精度得到大幅度提升。

z(k)＝[xamyamzam]^t+vk，vk＝[vxvyvz]^t为观测噪声向量，x(k)可以选取陀螺仪结算出来的角度累加作为状态变量,x(k)＝x(k-1)+w*t,w为陀螺仪输出的三轴角速度,t为运算步长。其中hk＝[100,010,001]^t。xam为互补滤波后得到的翻滚角，yam为互补滤波后得到的俯仰角，zam为互补滤波后得到的偏航角；

3.5)状态协方差矩阵更新：

p(k)＝(i-k(k)hk)p(k/k-1)

其中，i为[111,111,111]^t；

ekf的预测更新过程：初值为加速度计和电子磁力计给出也可以给0，p(0)初值为单位矩阵即可，假设系统和观测噪声为高斯白噪声，各自的协方差矩阵为q和r。

实施例：

采用两块stm32f407系列单片机，一块单片机作为主机进行陀螺仪、加速度计和磁力计的数据获取，并进行互补滤波解算姿态角度，另一块作为从机进行ekf解算，两块单片机之间用usart通信，主机解算过后的姿态角度传输至从机，从机结算后再把姿态信息反馈至主机，用来修正误差。同时两者的数据通过串口上传到电脑，用matlab画可以得到如下。

静态实验：将开发板放置在水平台面上，俯仰角的理想值在蓝色曲线附近。如图3a～图3c所示分别为静态下用互补滤波、ekf、两者结合算法得到的俯仰角。可以观察出来，在卡尔曼滤波中，观测量对于其的重要性，直接选择加速度计和磁力计联合解算出来的姿态角作为观测量，机体的震动和外力加速度会给加速度计解算出的姿态角有很大的误差。单独的互补滤波解算后的姿态角有一定得漂移，由于不无法准确估计出观测量的噪声会导致ekf受噪声影响较严重。两者结合后能够很好的抑制偏差。

动态实验：利用三轴转台带动两块核心板进行姿态变化运动，用上述滤波得到翻滚角，如图4a～图4c所示分别为静态下用互补滤波、ekf、两者结合算法得到的翻滚角。可以观察出互补滤波算法能够准确跟踪到姿态角的快速变化，但是有一定得超调量。ekf的姿态角动态响应较慢，具有明显的滞后性。两者结合后能够有效滤除噪声，同时在姿态角快速变化时能够快速追踪，精度有极大提高。

综上所述，本发明采用互补滤波和ekf两种算法的结合能够对姿态角的漂移和噪声有明显的抑制作用。同时在姿态角快速变化时，能够实现快速最终，同时用两块单片机内核分别运行两种算法，在忽略usart通信时间的情况下，只需要花费平时互补滤波算法的时间即可将ekf算法一起融合，这样的话能够保证姿态的精准性，同时还能留下充裕的时间让飞控进行电机的控制，使无人机更加平稳。

上述各实施例仅用于说明本发明，各部件的结构、设置位置及步骤都是可以有所变化的，在本发明技术方案的基础上，凡根据本发明原理对个别部件及步骤进行的改进和等同变换，均不应排除在本发明的保护范围之外。