CN109506646A - 一种双控制器的无人机姿态解算方法及系统 - Google Patents

一种双控制器的无人机姿态解算方法及系统 Download PDF

Info

Publication number
CN109506646A
CN109506646A CN201811385198.4A CN201811385198A CN109506646A CN 109506646 A CN109506646 A CN 109506646A CN 201811385198 A CN201811385198 A CN 201811385198A CN 109506646 A CN109506646 A CN 109506646A
Authority
CN
China
Prior art keywords
attitude angle
angle
host
coordinate system
attitude
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN201811385198.4A
Other languages
English (en)
Inventor
焦永刚
滕瑞
任彬
赵增旭
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shijiazhuang Tiedao University
Original Assignee
Shijiazhuang Tiedao University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shijiazhuang Tiedao University filed Critical Shijiazhuang Tiedao University
Priority to CN201811385198.4A priority Critical patent/CN109506646A/zh
Publication of CN109506646A publication Critical patent/CN109506646A/zh
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/18—Stabilised platforms, e.g. by gyroscope

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Environmental & Geological Engineering (AREA)
  • General Life Sciences & Earth Sciences (AREA)
  • Geology (AREA)
  • Navigation (AREA)
  • Gyroscopes (AREA)

Abstract

本发明涉及一种双控制器的无人机姿态解算方法及系统，其设置包括主机和从机的无人机姿态解算系统；在主机中，采用加速度计和电子罗盘解算的姿态角对陀螺仪解算的姿态角进行漂移补偿，并采用互补滤波算法把加速度计和电子罗盘解算的姿态角与陀螺仪解算的姿态角进行初步融合，滤去噪声并抑制姿态角漂移；将互补滤波后的姿态角传输给从机，互补滤波后的姿态角传输给从机，并采用EKF提高姿态角的解算精度，从机每50HZ一个周期把解算出来的高精度姿态角传输给主机进行姿态补偿和控制；EKF为扩展卡尔曼滤波算法；当主机未接收到从机的姿态角信息时，则以主机互补滤波算法解算出来的姿态角进行姿态控制。本发明提高了解算姿态的精确性，有很好的实用性。

Description

一种双控制器的无人机姿态解算方法及系统
技术领域
本发明涉及一种无人机控制领域，特别是关于一种双控制器的无人机姿态解算方法及系统。
背景技术
随着微机电系统技术(MEMS)和计算机技术的不断发展，低成本的捷联惯性测量单元(IMU)广泛应用于在小型飞行器中，捷联惯性测量单元由三轴陀螺仪、三轴加速度计和电子罗盘组成。陀螺仪受温漂的影响。长时间运行漂移严重，并且积分运算会产生积累误差；加速度计容易受到载体震动和运动加速度的影响；电子罗盘容易受外部磁场的影响。因此，这三种传感器都不能单独用于飞行器姿态的测量。需要进行这三种传感器数据的融合。而目前被应用在飞行器姿态角上的数据融合算法有互补滤波、共轭梯度法滤波、扩展卡尔曼滤波以及无迹卡尔曼滤波。
互补滤波原理简单，能够滤去噪声和抑制漂移由于不容易确定合适的高通和低通截止频率，因此精度不高；共轭梯度法滤波和互补滤波相结合在姿态解算中的应用，存在迭代计算量和精度的平衡问题；EKF和UKF是卡尔曼滤波对非线性系统的滤波方法，EKF存在线性化误差和截断误差，对一般的非线性系统影响不大；UKF在收敛速度和估计精度中要高于EKF，但其计算量要比EKF大，并且它们的系统噪声和观测噪声不容易确定，解算的姿态角容易受到噪声干扰。同时EKF 与UKF运算量较大会对姿态结算中要求的实时性有较大影响。
发明内容
针对上述问题，本发明的目的是提供一种双控制器的无人机姿态解算方法及系统，其解算精度较高。
为实现上述目的，本发明采取以下技术方案：一种双控制器的无人机姿态解算方法，该方法通过互补滤波算法和EKF融合解算姿态，其包括以下步骤：1)设置一包括主机和从机的无人机姿态解算系统；2)在主机中，采用加速度计和电子罗盘解算的姿态角对陀螺仪解算的姿态角进行漂移补偿，并采用互补滤波算法把加速度计和电子罗盘解算的姿态角与陀螺仪解算的姿态角进行初步融合，滤去噪声并抑制姿态角漂移；3)将互补滤波后的姿态角传输给从机，互补滤波后的姿态角传输给从机，并采用EKF提高姿态角的解算精度，从机每50HZ一个周期把解算出来的高精度姿态角传输给主机进行姿态补偿和控制；EKF为扩展卡尔曼滤波算法；4)当主机未接收到从机的姿态角信息时，则以主机互补滤波算法解算出来的姿态角进行姿态控制。
进一步，所述步骤2)中，采用加速度计和电子罗盘解算的姿态角对陀螺仪解算的姿态角进行漂移补偿的方法如下：通过地面坐标系到载体坐标系的转换矩阵将重力和磁场强度在地面坐标系的分量分别转换到载体坐标系中，然后分别与加速度计和电子磁力计单位化后的测量值[ax ay az]T和[mx my mz]T进行交叉向量积运算，分别求出两向量之间的误差，然后将这两个误差分别经过PI调节器后加在一起，得到了补偿陀螺仪的温漂补偿量，进而消除陀螺仪数据的误差；其中，在地面坐标系中的重力和磁场强度的分量分别为[0 01]T和[Ox Oy Oz]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＝[1 0 0,0 1 0,0 0 1]T；3.4)状态更新：其中，代表的是本时刻卡尔曼滤波的最终输出值，Z(K)选取的是本时刻经过互补滤波后的姿态角数据；Z(K)＝[Xam Yam Zam]T+Vk，Vk＝[Vx Vy Vz]T为观测噪声向量，Xam为互补滤波后得到的翻滚角，Yam为互补滤波后得到的俯仰角，Zam为互补滤波后得到的偏航角；X(K)选取陀螺仪结算出来的角度累加作为状态变量,X(K)＝X(K-1)+W*T，W为陀螺仪输出的三轴角速度,T为运算步长；Hk＝[1 0 0,0 10,0 0 1]T；3.5)状态协方差矩阵更新：P(K)＝(I-K(k)Hk)P(K/K-1)；其中，I 为[1 1 1,1 11,1 1 1]T。
一种用于实现上述方法的双控制器的无人机姿态解算系统，其包括主机、从机、加速度计、陀螺仪、磁力计、电子调速器和无刷电机；所述加速度计、陀螺仪和磁力计将采集到的信息传输至所述主机，所述主机与所述从机进行信息交互，由所述主机将接收到的信息传输给所述从机进行互补滤波处理，所述从机将互补滤波处理后的信息传输给所述主机；所述主机向所述电子调速器传输控制信号，所述电子调速器根据接收到的控制信号调节所述无刷电机的转速。
进一步，所述系统还包括接收机；外部遥控器发送的期望姿态角度通过天线接收的电磁信号传入所述接收机，由所述接收机将期望姿态角度信息传输至所述主机，进而由所述主机控制所述电子调速器工作。
本发明由于采取以上技术方案，其具有以下优点：本发明在不影响解算姿态实时性的前提下，大幅度的提高了解算姿态的精确性，同时有很好的实用性。
附图说明
图1是本发明的系统结构示意；
图2是本发明的解算方法流程示意；
图3a是本发明实施例中静态下用互补滤波得到的俯仰角示意图；
图3b是本发明实施例中静态下用EKF得到的俯仰角示意图；
图3c是本发明实施例中互补滤波、EKF结合算法得到的俯仰角示意图；
图4a是本发明实施例中静态下用互补滤波得到的翻滚角示意图；
图4b是本发明实施例中静态下用EKF得到的翻滚角示意图；
图4c是本发明实施例中互补滤波、EKF结合算法得到的翻滚角示意图。
具体实施方式
下面结合附和实施例对本发明进行详细的描述。
如图1所示，本发明提供一种双控制器的无人机姿态解算系统，其包括主机、从机、加速度计、陀螺仪、磁力计、电子调速器和无刷电机。加速度计、陀螺仪和磁力计将采集到的信息传输至主机，主机与从机进行信息交互，由主机将接收到的信息传输给从机进行互补滤波处理，从机将互补滤波处理后的信息传输给主机。主机向电子调速器传输控制信号，电子调速器根据接收到的控制信号调节无刷电机的转速。
上述实施例中，本发明的系统还包括接收机。外部遥控器发送的期望姿态角度通过天线接收的电磁信号传入接收机，由接收机将期望姿态角度信息传输至主机，进而由主机控制电子调速器工作。
上述各实施例中，主机和从机均采用STM32F407XXXX系列的单片机。
上述各实施例中，加速度计采用三轴加速度计。
如图2所示，本发明还提供一种双控制器的无人机姿态解算方法，该方法通过互补滤波算法和EKF融合解算姿态，其包括以下步骤：
1)设置一包括主机和从机的无人机姿态解算系统；
2)在主机中，采用加速度计和电子罗盘解算的姿态角对陀螺仪解算的姿态角进行漂移补偿，并采用互补滤波算法把加速度计和电子罗盘解算的姿态角与陀螺仪解算的姿态角进行初步融合，滤去噪声并抑制姿态角漂移；
如果单独用陀螺仪解算姿态角，由于陀螺仪长时间运行温漂严重，会造成解算出来的姿态角也发生漂移。本实施例中采用加速度计和电子罗盘解算的姿态角对陀螺仪解算的姿态角进行漂移补偿的方法如下：
通过地面坐标系到载体坐标系的转换矩阵将重力和磁场强度在地面坐标系的分量分别转换到载体坐标系中，然后分别与加速度计和电子磁力计(即电子罗盘)单位化后的测量值[ax ay az]T和[mx my mz]T进行交叉向量积运算，分别求出两向量之间的误差，然后将这两个误差分别经过PI调节器后加在一起，得到了补偿陀螺仪的温漂补偿量，进而消除陀螺仪数据的误差，也就是在消除旋转矩阵的误差。其中，在地面坐标系中的重力和磁场强度的分量分别为[0 0 1]T和[Ox Oy Oz]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＝[1 0 0,0 1 0,0 0 1]T；
3.4)状态更新：
其中，代表的是本时刻卡尔曼滤波的最终输出值，Z(K)选取的是本时刻经过互补滤波后的姿态角数据。
Z(k)为观测方程，可以选取加速度计、电子罗盘联合直接解算出来的姿态角。在本实施例中，Z(k)由互补滤波后得到的姿态角进行更新，而以往的卡尔曼滤波算法的观测方程都是由加速度计、电子罗盘联合直接解算出来的姿态角度。但是这样的话由于机体震动和外力加速度的影响会使观测方程产生很大误差。本发明通过互补滤波给卡尔曼滤波的观测方程提供了误差较小的平稳数据，使得卡尔曼滤波精度得到大幅度提升。
Z(K)＝[Xam Yam Zam]T+Vk，Vk＝[Vx Vy Vz]T为观测噪声向量，X(K)可以选取陀螺仪结算出来的角度累加作为状态变量,X(K)＝X(K-1)+W*T,W为陀螺仪输出的三轴角速度,T为运算步长。其中Hk＝[1 0 0,0 1 0,0 0 1]T。Xam为互补滤波后得到的翻滚角，Yam为互补滤波后得到的俯仰角，Zam为互补滤波后得到的偏航角；
3.5)状态协方差矩阵更新：
P(K)＝(I-K(k)Hk)P(K/K-1)
其中，I为[1 1 1,1 1 1,1 1 1]T；
EKF的预测更新过程：初值为加速度计和电子磁力计给出也可以给0， P(0)初值为单位矩阵即可，假设系统和观测噪声为高斯白噪声，各自的协方差矩阵为Q和R。
实施例：
采用两块STM32F407系列单片机，一块单片机作为主机进行陀螺仪、加速度计和磁力计的数据获取，并进行互补滤波解算姿态角度，另一块作为从机进行EKF 解算，两块单片机之间用USART通信，主机解算过后的姿态角度传输至从机，从机结算后再把姿态信息反馈至主机，用来修正误差。同时两者的数据通过串口上传到电脑，用MATLAB画可以得到如下。
静态实验：将开发板放置在水平台面上，俯仰角的理想值在蓝色曲线附近。如图3a～图3c所示分别为静态下用互补滤波、EKF、两者结合算法得到的俯仰角。可以观察出来，在卡尔曼滤波中，观测量对于其的重要性，直接选择加速度计和磁力计联合解算出来的姿态角作为观测量，机体的震动和外力加速度会给加速度计解算出的姿态角有很大的误差。单独的互补滤波解算后的姿态角有一定得漂移，由于不无法准确估计出观测量的噪声会导致EKF受噪声影响较严重。两者结合后能够很好的抑制偏差。
动态实验：利用三轴转台带动两块核心板进行姿态变化运动，用上述滤波得到翻滚角，如图4a～图4c所示分别为静态下用互补滤波、EKF、两者结合算法得到的翻滚角。可以观察出互补滤波算法能够准确跟踪到姿态角的快速变化，但是有一定得超调量。EKF的姿态角动态响应较慢，具有明显的滞后性。两者结合后能够有效滤除噪声，同时在姿态角快速变化时能够快速追踪，精度有极大提高。
综上所述，本发明采用互补滤波和EKF两种算法的结合能够对姿态角的漂移和噪声有明显的抑制作用。同时在姿态角快速变化时，能够实现快速最终，同时用两块单片机内核分别运行两种算法，在忽略USART通信时间的情况下，只需要花费平时互补滤波算法的时间即可将EKF算法一起融合，这样的话能够保证姿态的精准性，同时还能留下充裕的时间让飞控进行电机的控制，使无人机更加平稳。
上述各实施例仅用于说明本发明，各部件的结构、设置位置及步骤都是可以有所变化的，在本发明技术方案的基础上，凡根据本发明原理对个别部件及步骤进行的改进和等同变换，均不应排除在本发明的保护范围之外。

Claims (10)

1.一种双控制器的无人机姿态解算方法，该方法通过互补滤波算法和EKF融合解算姿态，其特征在于包括以下步骤：
1)设置一包括主机和从机的无人机姿态解算系统；
2)在主机中，采用加速度计和电子罗盘解算的姿态角对陀螺仪解算的姿态角进行漂移补偿，并采用互补滤波算法把加速度计和电子罗盘解算的姿态角与陀螺仪解算的姿态角进行初步融合，滤去噪声并抑制姿态角漂移；
3)将互补滤波后的姿态角传输给从机，互补滤波后的姿态角传输给从机，并采用EKF提高姿态角的解算精度，从机每50HZ一个周期把解算出来的高精度姿态角传输给主机进行姿态补偿和控制；EKF为扩展卡尔曼滤波算法；
4)当主机未接收到从机的姿态角信息时，则以主机互补滤波算法解算出来的姿态角进行姿态控制。
2.如权利要求1所述方法，其特征在于：所述步骤2)中，采用加速度计和电子罗盘解算的姿态角对陀螺仪解算的姿态角进行漂移补偿的方法如下：通过地面坐标系到载体坐标系的转换矩阵将重力和磁场强度在地面坐标系的分量分别转换到载体坐标系中，然后分别与加速度计和电子磁力计单位化后的测量值[ax ay az]T和[mx my mz]T进行交叉向量积运算，分别求出两向量之间的误差，然后将这两个误差分别经过PI调节器后加在一起，得到了补偿陀螺仪的温漂补偿量，进而消除陀螺仪数据的误差；其中，在地面坐标系中的重力和磁场强度的分量分别为[0 0 1]T和[Ox Oy Oz]T；ax、ay、az分别对应单位化后载体坐标系下的x,y,z轴的加速度测量值，mx、my、mz分别对应单位化后载体坐标系下的x,y,z轴的磁场强度测量值，0x、0y、Oz即当地的磁场强度在x,y,z的分量。
3.如权利要求1所述方法，其特征在于：所述步骤2)中，与加速度计之间的误差计算过程为：首先当地面坐标系和载体坐标系重合时，机体的测量值等同于重力；而当机体转动时，地面坐标系的重力是保持不变的而机体测量值会发生变化；重力乘上转换矩阵得到旋转状态下载体坐标系的标准值三轴分量大小，将这个分量大小与单位化后的加速度计测量的三轴加速度分量进行对比，得到一个误差E1。
4.如权利要求3所述方法，其特征在于：所述步骤2)中，与电子磁力计之间的误差计算过程为：载体坐标系下的磁力计测出的数据通过转换矩阵转换到地面坐标系下的分量，而在地面坐标系中，要对这个分量进行标准化处理；标准化后的数据通过旋转矩阵得到在载体下的分量，与磁力计的测量值进行比较得到误差E2。
5.如权利要求4所述方法，其特征在于：所述步骤2)中，总的误差E＝E1+E2，陀螺仪通过PI调节减去这个误差，然后更新旋转矩阵；旋转矩阵是由陀螺仪数据算出，所以消除陀螺仪数据的误差也就是在消除旋转矩阵的误差。
6.如权利要求1至5任一项所述方法，其特征在于：所述步骤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列。
7.如权利要求6所述方法，其特征在于：所述步骤2.2)中，四元数与姿态角之间的转换有如下对应转换关系：
则得到四元数表示的转换矩阵分别为：
8.如权利要求1至5任一项所述方法，其特征在于：所述步骤3)中，EKF包括以下步骤：
3.1)状态预测：
为状态变量的先验估计，为上一时刻(K-1)陀螺仪数据累加得到姿态角；W＝[ωx(K-1) ωy(K-1) ωz(K-1)]T是陀螺仪在上一时刻(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＝[1 0 0,0 1 0,0 0 1]T；
3.4)状态更新：
其中，代表的是本时刻卡尔曼滤波的最终输出值，Z(K)选取的是本时刻经过互补滤波后的姿态角数据；Z(K)＝[Xam Yam Zam]T+Vk，Vk＝[Vx Vy Vz]T为观测噪声向量，Xam为互补滤波后得到的翻滚角，Yam为互补滤波后得到的俯仰角，Zam为互补滤波后得到的偏航角；X(K)选取陀螺仪结算出来的角度累加作为状态变量,X(K)＝X(K-1)+W*T，W为陀螺仪输出的三轴角速度,T为运算步长；Hk＝[1 0 0,0 1 0,0 0 1]T；
3.5)状态协方差矩阵更新：
P(K)＝(I-K(k)Hk)P(K/K-1)
其中，I为[1 1 1,1 1 1,1 1 1]T。
9.一种用于实现如权利要求1至8任一项所述方法的双控制器的无人机姿态解算系统，其特征在于：包括主机、从机、加速度计、陀螺仪、磁力计、电子调速器和无刷电机；所述加速度计、陀螺仪和磁力计将采集到的信息传输至所述主机，所述主机与所述从机进行信息交互，由所述主机将接收到的信息传输给所述从机进行互补滤波处理，所述从机将互补滤波处理后的信息传输给所述主机；所述主机向所述电子调速器传输控制信号，所述电子调速器根据接收到的控制信号调节所述无刷电机的转速。
10.如权利要求9所述系统，其特征在于：所述系统还包括接收机；外部遥控器发送的期望姿态角度通过天线接收的电磁信号传入所述接收机，由所述接收机将期望姿态角度信息传输至所述主机，进而由所述主机控制所述电子调速器工作。
CN201811385198.4A 2018-11-20 2018-11-20 一种双控制器的无人机姿态解算方法及系统 Pending CN109506646A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811385198.4A CN109506646A (zh) 2018-11-20 2018-11-20 一种双控制器的无人机姿态解算方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811385198.4A CN109506646A (zh) 2018-11-20 2018-11-20 一种双控制器的无人机姿态解算方法及系统

Publications (1)

Publication Number Publication Date
CN109506646A true CN109506646A (zh) 2019-03-22

Family

ID=65749339

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811385198.4A Pending CN109506646A (zh) 2018-11-20 2018-11-20 一种双控制器的无人机姿态解算方法及系统

Country Status (1)

Country Link
CN (1) CN109506646A (zh)

Cited By (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109990776A (zh) * 2019-04-12 2019-07-09 武汉深海蓝科技有限公司 一种姿态测量方法及装置
CN110377058A (zh) * 2019-08-30 2019-10-25 深圳市道通智能航空技术有限公司 一种飞行器的偏航角修正方法、装置及飞行器
CN110514228A (zh) * 2019-09-02 2019-11-29 哈尔滨工业大学 微小型无人机航姿测量系统动态综合性能测试装置及方法
CN110674888A (zh) * 2019-10-11 2020-01-10 中国人民解放军海军航空大学青岛校区 一种基于数据融合的头部姿态识别方法
CN110702113A (zh) * 2019-05-09 2020-01-17 中科探海(苏州)海洋科技有限责任公司 基于mems传感器的捷联惯导系统数据预处理和姿态解算的方法
CN111141283A (zh) * 2020-01-19 2020-05-12 杭州十域科技有限公司 一种通过地磁数据判断行进方向的方法
CN111207734A (zh) * 2020-01-16 2020-05-29 西安因诺航空科技有限公司 一种基于ekf的无人机组合导航方法
CN111290424A (zh) * 2020-03-26 2020-06-16 南方医科大学南方医院 用于医院血液样本运输的无人机姿态控制方法及无人机
CN111831962A (zh) * 2020-07-14 2020-10-27 河北科技大学 四旋翼无人机姿态解算方法、装置及终端设备
CN112611380A (zh) * 2020-12-03 2021-04-06 燕山大学 基于多imu融合的姿态检测方法及其姿态检测装置
CN112649001A (zh) * 2020-12-01 2021-04-13 中国航空工业集团公司沈阳飞机设计研究所 一种小型无人机姿态与位置解算方法
CN113163247A (zh) * 2021-04-02 2021-07-23 深圳创维－Rgb电子有限公司 设备控制方法、遥控器及计算机可读存储介质
CN114234971A (zh) * 2022-02-28 2022-03-25 西安星通通信科技有限公司 一种可降低噪声全姿态imu姿态解算方法
CN114279446A (zh) * 2021-12-22 2022-04-05 广东汇天航空航天科技有限公司 飞行汽车航姿测量方法、装置及飞行汽车
CN115145157A (zh) * 2022-09-07 2022-10-04 北京航天自动控制研究所 一种基于截止频率映射的姿态控制增益整定方法
CN115167495A (zh) * 2022-08-10 2022-10-11 青岛理工大学 一种基于32单片机的双闭环串级pid四旋翼飞行器

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104914874A (zh) * 2015-06-09 2015-09-16 长安大学 一种基于自适应互补融合的无人机姿态控制系统及方法
CN105588567A (zh) * 2016-01-25 2016-05-18 北京航空航天大学 一种自动电子罗盘校准辅助式的航姿参考系统及方法
CN106643737A (zh) * 2017-02-07 2017-05-10 大连大学 风力干扰环境下四旋翼飞行器姿态解算方法
CN107063262A (zh) * 2017-04-07 2017-08-18 武汉理工大学 一种用于无人机姿态解算的互补滤波方法
CN207008411U (zh) * 2017-06-27 2018-02-13 上海拓攻机器人有限公司 多传感器冗余备份的双余度飞行控制系统及含其的无人机
CN108398128A (zh) * 2018-01-22 2018-08-14 北京大学深圳研究生院 一种姿态角的融合解算方法和装置
CN108827299A (zh) * 2018-03-29 2018-11-16 南京航空航天大学 一种基于改进四元数二阶互补滤波的飞行器姿态解算方法

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104914874A (zh) * 2015-06-09 2015-09-16 长安大学 一种基于自适应互补融合的无人机姿态控制系统及方法
CN105588567A (zh) * 2016-01-25 2016-05-18 北京航空航天大学 一种自动电子罗盘校准辅助式的航姿参考系统及方法
CN106643737A (zh) * 2017-02-07 2017-05-10 大连大学 风力干扰环境下四旋翼飞行器姿态解算方法
CN107063262A (zh) * 2017-04-07 2017-08-18 武汉理工大学 一种用于无人机姿态解算的互补滤波方法
CN207008411U (zh) * 2017-06-27 2018-02-13 上海拓攻机器人有限公司 多传感器冗余备份的双余度飞行控制系统及含其的无人机
CN108398128A (zh) * 2018-01-22 2018-08-14 北京大学深圳研究生院 一种姿态角的融合解算方法和装置
CN108827299A (zh) * 2018-03-29 2018-11-16 南京航空航天大学 一种基于改进四元数二阶互补滤波的飞行器姿态解算方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
TAREK DAKHLALLAH著: "《多传感器数据融合系统——EKF及模糊决策应用分析》", 30 September 2016, 国防工业出版社 *

Cited By (26)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109990776A (zh) * 2019-04-12 2019-07-09 武汉深海蓝科技有限公司 一种姿态测量方法及装置
CN109990776B (zh) * 2019-04-12 2021-09-24 武汉深海蓝科技有限公司 一种姿态测量方法及装置
CN110702113A (zh) * 2019-05-09 2020-01-17 中科探海(苏州)海洋科技有限责任公司 基于mems传感器的捷联惯导系统数据预处理和姿态解算的方法
CN110702113B (zh) * 2019-05-09 2022-12-27 中科探海(苏州)海洋科技有限责任公司 基于mems传感器的捷联惯导系统数据预处理和姿态解算的方法
CN110377058B (zh) * 2019-08-30 2021-11-09 深圳市道通智能航空技术股份有限公司 一种飞行器的偏航角修正方法、装置及飞行器
CN110377058A (zh) * 2019-08-30 2019-10-25 深圳市道通智能航空技术有限公司 一种飞行器的偏航角修正方法、装置及飞行器
CN110514228A (zh) * 2019-09-02 2019-11-29 哈尔滨工业大学 微小型无人机航姿测量系统动态综合性能测试装置及方法
CN110674888A (zh) * 2019-10-11 2020-01-10 中国人民解放军海军航空大学青岛校区 一种基于数据融合的头部姿态识别方法
CN110674888B (zh) * 2019-10-11 2022-04-05 中国人民解放军海军航空大学青岛校区 一种基于数据融合的头部姿态识别方法
CN111207734A (zh) * 2020-01-16 2020-05-29 西安因诺航空科技有限公司 一种基于ekf的无人机组合导航方法
CN111207734B (zh) * 2020-01-16 2022-01-07 西安因诺航空科技有限公司 一种基于ekf的无人机组合导航方法
CN111141283A (zh) * 2020-01-19 2020-05-12 杭州十域科技有限公司 一种通过地磁数据判断行进方向的方法
CN111290424A (zh) * 2020-03-26 2020-06-16 南方医科大学南方医院 用于医院血液样本运输的无人机姿态控制方法及无人机
CN111290424B (zh) * 2020-03-26 2020-11-06 南方医科大学南方医院 用于医院血液样本运输的无人机姿态控制方法及无人机
CN111831962A (zh) * 2020-07-14 2020-10-27 河北科技大学 四旋翼无人机姿态解算方法、装置及终端设备
CN111831962B (zh) * 2020-07-14 2023-02-03 河北科技大学 四旋翼无人机姿态解算方法、装置及终端设备
CN112649001A (zh) * 2020-12-01 2021-04-13 中国航空工业集团公司沈阳飞机设计研究所 一种小型无人机姿态与位置解算方法
CN112649001B (zh) * 2020-12-01 2023-08-22 中国航空工业集团公司沈阳飞机设计研究所 一种小型无人机姿态与位置解算方法
CN112611380A (zh) * 2020-12-03 2021-04-06 燕山大学 基于多imu融合的姿态检测方法及其姿态检测装置
CN113163247A (zh) * 2021-04-02 2021-07-23 深圳创维－Rgb电子有限公司 设备控制方法、遥控器及计算机可读存储介质
CN114279446A (zh) * 2021-12-22 2022-04-05 广东汇天航空航天科技有限公司 飞行汽车航姿测量方法、装置及飞行汽车
CN114279446B (zh) * 2021-12-22 2023-11-03 广东汇天航空航天科技有限公司 飞行汽车航姿测量方法、装置及飞行汽车
CN114234971A (zh) * 2022-02-28 2022-03-25 西安星通通信科技有限公司 一种可降低噪声全姿态imu姿态解算方法
CN115167495A (zh) * 2022-08-10 2022-10-11 青岛理工大学 一种基于32单片机的双闭环串级pid四旋翼飞行器
CN115145157B (zh) * 2022-09-07 2022-12-13 北京航天自动控制研究所 一种基于截止频率映射的姿态控制增益整定方法
CN115145157A (zh) * 2022-09-07 2022-10-04 北京航天自动控制研究所 一种基于截止频率映射的姿态控制增益整定方法

Similar Documents

Publication Publication Date Title
CN109506646A (zh) 一种双控制器的无人机姿态解算方法及系统
CN107314718B (zh) 基于磁测滚转角速率信息的高速旋转弹姿态估计方法
CN105203098B (zh) 基于九轴mems传感器的农业机械全姿态角更新方法
CN106708066A (zh) 基于视觉/惯导的无人机自主着陆方法
CN105929836B (zh) 用于四旋翼飞行器的控制方法
CN107389099B (zh) 捷联惯导系统空中快速对准装置及方法
CN105737854B (zh) 一种捷联惯导系统在线标定方法
CN111207745B (zh) 一种适用于大机动无人机垂直陀螺仪的惯性测量方法
CN110017837A (zh) 一种姿态抗磁干扰的组合导航方法
JP2011227017A (ja) 慣性センサ，磁気センサおよび速度計を用いた移動体の姿勢推定装置および姿勢推定方法
CN111895988A (zh) 无人机导航信息更新方法及装置
GB2378765A (en) Error compensation in an inertial navigation system
CN104215244B (zh) 基于发射惯性坐标系的空天飞行器组合导航鲁棒滤波方法
CN106352897B (zh) 一种基于单目视觉传感器的硅mems陀螺误差估计与校正方法
CN110285815A (zh) 一种可在轨全程应用的微纳卫星多源信息姿态确定方法
Wu et al. Attitude and gyro bias estimation by the rotation of an inertial measurement unit
CN110243362A (zh) 一种中高空超声速靶标导航方法
CN108759814A (zh) 一种四旋翼飞行器横滚轴角速度和俯仰轴角速度估计方法
Gu et al. Design and implementation of attitude and heading reference system with extended Kalman filter based on MEMS multi-sensor fusion
Weibel et al. Small unmanned aerial system attitude estimation for flight in wind
CN113932803B (zh) 适用于高动态飞行器的惯性/地磁/卫星组合导航系统
CN110286688B (zh) 一种带有机械臂的水下四旋翼无人机控制方法
CN207487689U (zh) 一种导航终端
Huang et al. Integration of MEMS inertial sensor-based GNC of a UAV
Zhang et al. An improved Kalman filter for attitude determination of multi-rotor UAVs based on low-cost MEMS sensors

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20190322