基于梯度下降的四旋翼无人机姿态航向参考系统解算方法.pdf

1、(10)申请公布号 CN 103363992 A (43)申请公布日 2013.10.23 CN 103363992 A *CN103363992A* (21)申请号 201310272266.7 (22)申请日 2013.06.29 G01C 21/20(2006.01) (71)申请人 天津大学 地址 300072 天津市南开区卫津路 92 号 (72)发明人 鲜斌 宋英麟 茹滨超 (74)专利代理机构 天津市北洋有限责任专利代 理事务所 12201 代理人 杜文茹 (54) 发明名称 基于梯度下降的四旋翼无人机姿态航向参考 系统解算方法 (57) 摘要 一种基于梯度下降的四旋翼无人机姿态

2、航向 参考系统解算方法， 包括 ： 通过陀螺仪、 加速度传 感器、 地磁传感器采集微型四旋翼无人机姿态信 息， 其中， 陀螺仪测量微型四旋翼无人机的角速 率， 加速度传感器测量微型四旋翼无人机的线加 速度， 地磁传感器测量微型四旋翼无人机所处位 置的磁场信息 ； 应用基于梯度下降的四元数更新 算法将加速度传感器、 地磁传感器所到的数据与 陀螺仪所采集的数据进行融合， 修正陀螺仪的漂 移 ； 应用四元数法解算微型四旋翼无人机姿态角 信息。本发明采用基于梯度下降法的数据融合方 法， 充分利用加速度传感器和地磁传感器数据， 对 陀螺仪数据的漂移进行实时修正， 输出三维姿态 角信息， 从而减少系统误差

3、， 有效地提高了姿态测 量精度和稳定性。 (51)Int.Cl. 权利要求书 2 页 说明书 6 页 附图 3 页 (19)中华人民共和国国家知识产权局 (12)发明专利申请 权利要求书2页 说明书6页 附图3页 (10)申请公布号 CN 103363992 A CN 103363992 A *CN103363992A* 1/2 页 2 1. 一种基于梯度下降的四旋翼无人机姿态航向参考系统解算方法， 其特征在于， 包括 如下步骤 ： 1） 通过陀螺仪、 加速度传感器、 地磁传感器采集微型四旋翼无人机姿态信息， 其中， 陀 螺仪测量微型四旋翼无人机的角速率， 加速度传感器测量微型四旋翼无人机的线

4、加速度， 地磁传感器测量微型四旋翼无人机所处位置的磁场信息 ； 2） 应用基于梯度下降的四元数更新算法将加速度传感器、 地磁传感器所到的数据与陀 螺仪所采集的数据进行融合， 修正陀螺仪的漂移 ； 3） 应用四元数法解算微型四旋翼无人机姿态角信息。 2. 根据权利要求 1 所述的基于梯度下降的四旋翼无人机姿态航向参考系统解算方法， 其特征在于， 步骤 2） 所述的修正陀螺仪的漂移是 ： 设传感器在刚体坐标系下的测量值为 d=0 dx dy dz， 设在大地坐标系下的实际值 为 s=0 sx sy sz， 为使误差最小， 需求解函数 f(Q,d,s) 的最小值 ： minf(Q,d,s)， 其中，

5、 Q=q1 q2 q3 q4为刚体坐标系对大地坐标系的旋转四元数， Q*为共轭四元数， 将 d=0 dx dy dz、 s=0 sx sy sz、 Q=q1 q2 q3 q4 代入式 得 ： 根据梯度下降算法， 该 f(Q,d,s) 函数负梯度搜索的单位向量为 ： 其中， J(Q,d) 为 f(Q,d,s) 的雅克比矩阵， JT(Q,d) 为 f(Q,d,s) 的雅克比矩阵的转置 ； 通过以上推导， 得出带有梯度下降补偿算法的四元数更新方程为 ： 权 利 要 求 书 CN 103363992 A 2 2/2 页 3 其中 Qk+1为当前时刻对陀螺仪漂移修正后的四元数， 为步长， k 为大于等于

6、零的整 数， 为陀螺仪测得的角速度， 为四元数的乘法运算， Qk为上一时刻对陀螺仪漂移修正后 的四元数。 3. 根据权利要求 1 所述的基于梯度下降的四旋翼无人机姿态航向参考系统解算方法， 其特征在于， 步骤 3） 所述的解算微型四旋翼无人机姿态角信息是 ： 设微型四旋翼无人机的偏航角为 ， 俯仰角为 ， 滚转角为 ， 取大地坐标系为导航 坐标系， 并规定xg、 yg、 zg分别指向东、 北、 天， 则刚体坐标系能够通过三次基本旋转坐标变换 转化为导航坐标系， 通过下式就能够解算出载体姿态角信息 权 利 要 求 书 CN 103363992 A 3 1/6 页 4 基于梯度下降的四旋翼无人机姿

7、态航向参考系统解算方法 技术领域 0001 本发明涉及一种四旋翼无人机姿态航向数据测量。 特别是涉及一种基于梯度下降 的四旋翼无人机姿态航向参考系统解算方法。 背景技术 0002 姿态航向参考系统是四旋翼无人机机载电子系统中的一个核心设备， 其担负着为 无人机飞行控制系统提供实时、 可靠的飞行状态测量数据的重任。目前常见的四旋翼无人 机机体尺寸较小， 载荷较轻， 对惯性导航单元重量和尺寸限制较为严格， 因此多采用基于 MEMS( 微机电系统 ) 技术的微型姿态导航系统提供飞行状态测量数据。 0003 目前， 国内与国外有多种航向姿态测量系统的组成方案和应用。法国通用机械电 器公司的 Crist

8、al100 航向姿态参考系统采用环形激光陀螺仪和石英加速度计， 系统中采 用卡尔曼滤波器， 便于系统在地面、 船上和飞行中进行校准。Cristal100 航向姿态参考系 统可用于固定翼或旋转翼飞机。美国 Crossbow 公司的 NAV440 姿态参考系统综合利用了基 于 MEMS 的惯性器件和 GPS 技术， 体现了 MEMS 基础上的最新型的测量和控制技术。广泛应 用在航空、 陆地、 海洋导航， 跟踪控制， 平台稳定， ROV/AGV 控制， UAV/RPV 控制， 精准耕种等 领域。(Crossbowtechnology.NAV440GPS-AIDED MEMS Inertial Nav

9、igation SystemEB/ OL.http:/ 0004 在国内， 微型姿态航向测量系统相关研究工作的发展较晚一些。微型姿态航向测 量系统研究工作发展的核心力量主要集中在一些科研机构和高等院校。 北京航空航天大学 研制的微型 iFLY-G2 组合导航系统是小型六自由度组合导航系统。它以 MEMS 陀螺仪和加 速度传感器为核心器件， 系统依靠内部 DSP 处理器解算出航向姿态信息， 可向机载控制器 提供实时的欧拉角、 三维角速率等信息。 iFLY-G2为各种飞行器的稳定和控制提供精确和全 面的测量信息。iFLY-G2 提供 AHRS/DR 和 GPS/INS 两种组合导航模式， 在 GP

10、S 信息有效时， 自动切换到 GPS/INS 组合导航模式， 提供 50HZ 的位置、 速度、 姿态等信息 ； 在 GPS 信息无效 时， 自动切换到 AHRS/DR 模式 ( 包含航位推算的 AHRS 模式 )， 保证飞行的安全。 0005 近年来， 随着 MEMS 技术的发展， 微型姿态航向测量系统也随之出现。它一般由基 于 MEMS 技术的传感器件、 专用集成电路和嵌入式微型控制器组成， 可以为运动载体提供实 时的姿态等相关的导航信息。微型姿态导航系统具有体积小、 成本低、 自主性、 实时、 抗干 扰能力强等特点 ( 于俊杰 . 微惯性组合系统设计与研究 D. 哈尔滨工程大学， 2007

11、.)( 刘 危.基于MEMS的低成本MIMU的应用研究D.国防科学技术大学， 2004.)， 但其精度主要取 决于惯性器件的精度， 惯性器件的测量精度单从硬件结构设计和工艺方面很难有大幅度的 提高， 传感器漂移较大， 并且系统误差随时间积累， 不适用于长时间载体姿态确定的应用。 0006 姿态航向参考系统由多个轴向传感器组成， 能够为飞行器提供航向， 滚转和俯仰 信息， 这类系统可为飞行器提供准确可靠的姿态与航行信息。为提高无人机在飞行过程中 的稳定性， 姿态航向参考系统需要实时提供无人机当前姿态角， 供机载控制器使用。然而， 通常情况下低成本传感器漂移较大， 并且系统误差随时间积累， 不适用

12、于长时间载体姿态 说 明 书 CN 103363992 A 4 2/6 页 5 确定的使用， 这在一定程度上限制了航向姿态测量系统的应用。 0007 分析已有文献的技术报道可以看出， 传感器漂移较大、 精度低是目前微型四旋翼 无人机姿态航向参考系统主要存在的问题。 0008 微型姿态导航系统具有体积小、 成本低、 自主性、 实时、 抗干扰力强等特点， 但其精 度主要取决于惯性传感器件的精度， 惯性传感器件的测量精度单从硬件结构设计和工艺方 面很难有大幅度的提高， 传感器数据漂移较大， 并且系统误差随时间积累， 不适用于长时间 载体姿态确定的应用。 0009 随着捷联式惯性导航技术的发展， 为了

13、更简便地描述载体的姿态信息， 采用了四 元数这个数学工具， 用它来弥补通常描述刚体角运动的 3 个欧拉角参数在设计控制系统时 的不足。四元数可以描述一个坐标系或一个矢量相对某一个坐标系的旋转， 四元数的标量 部分表示 1/2 转角的余弦值， 而其矢量部分则表示瞬时转轴的方向、 瞬时转动轴与参考坐 标系轴间的方向余弦值。因此， 一个四元数既表示了转轴的方向， 又表示了转角的大小， 往 往称其为转动四元数。( 张斌 . 基于 MIMU 的捷联惯性导航系统数据采集与处理技术研究 D. 哈尔滨 ： 哈尔滨工程大学， 2010) 通过陀螺仪采集载体角速率数据， 使用四元数更新方 法对数据进行解算， 可得

14、到载体姿态信息。 发明内容 0010 本发明所要解决的技术问题是， 提供一种能够获取精度较高的姿态角测量值， 实 现微型四旋翼无人机姿态信息估计， 提高微型四旋翼无人机姿态和位置控制精度的基于梯 度下降的四旋翼无人机姿态航向参考系统解算方法。 0011 本发明所采用的技术方案是 ： 一种基于梯度下降的四旋翼无人机姿态航向参考系 统解算方法， 包括如下步骤 ： 0012 1) 通过陀螺仪、 加速度传感器、 地磁传感器采集微型四旋翼无人机姿态信息， 其 中， 陀螺仪测量微型四旋翼无人机的角速率， 加速度传感器测量微型四旋翼无人机的线加 速度， 地磁传感器测量微型四旋翼无人机所处位置的磁场信息 ；

15、0013 2) 应用基于梯度下降的四元数更新算法将加速度传感器、 地磁传感器所到的数据 与陀螺仪所采集的数据进行融合， 修正陀螺仪的漂移 ； 0014 3) 应用四元数法解算微型四旋翼无人机姿态角信息。 0015 步骤 2) 所述的修正陀螺仪的漂移是 ： 0016 设传感器在刚体坐标系下的测量值为， 设在大地坐标系下的 实际值为， 为使误差最小， 需求解函数 f(Q， d， s) 的最小值 ： min f(Q， d， s)， 0017 0018 其中，为刚体坐标系对大地坐标系的旋转四元数， Q*为共轭四 元数， 0019 将、代 入 式 得 ： 说 明 书 CN 103363992 A 5 3

16、/6 页 6 0020 0021 根据梯度下降算法， 该 f(Q， d， s) 函数负梯度搜索的单位向量为 ： 0022 0023 0024 其中， J(Q， d) 为 f(Q， d， s) 的雅克比矩阵， JT(Q， d) 为 f(Q， d， s) 的雅克比矩阵的 转置 ； 0025 0026 0027 通过以上推导， 得出带有梯度下降补偿算法的四元数更新方程为 ： 0028 0029 其中 Qk+1为当前时刻对陀螺仪漂移修正后的四元数， 为步长， k 为大于等于零的 整数， 为陀螺仪测得的角速度， 为四元数的乘法运算， Qk为上一时刻对陀螺仪漂移修正 后的四元数。 0030 步骤 3) 所

17、述的解算微型四旋翼无人机姿态角信息是 ： 0031 设微型四旋翼无人机的偏航角为 ， 俯仰角为 ， 滚转角为 ， 取大地坐标系为 导航坐标系， 并规定xg、 yg、 zg分别指向东、 北、 天， 则刚体坐标系能够通过三次基本旋转坐标 变换转化为导航坐标系， 通过下式就能够解算出载体姿态角信息 0032 0033 本发明的基于梯度下降的微型四旋翼无人机姿态航向参考系统数据解算方法， 采 用基于梯度下降法的数据融合方法， 充分利用加速度传感器和地磁传感器数据， 对陀螺仪 数据的漂移进行实时修正， 输出三维姿态角信息， 从而减少系统误差， 有效地提高了姿态测 量精度和稳定性。本发明具有如下特点 ：

18、0034 1、 解算过程计算量小。 传统的基于卡尔曼滤波器的姿态航向参考系统数据解算方 法一般需要进行大量的矩阵运算， 在实际应用中对处理器运算速度要求高， 提高了姿态航 说 明 书 CN 103363992 A 6 4/6 页 7 向参考系统成本。 而本发明提出的解算方法计算量小， 对处理器运算速度要求低， 适用于微 型无人机低成本姿态航向参考系统。 0035 2.精度及稳定性高。 本发明提出的数据解算方法融合了加速度传感器与地磁传感 器数据信息， 克服了陀螺仪数据漂移较大的问题， 使用加速度计和磁力计数据对陀螺仪漂 移进行补偿， 修正了陀螺仪漂移产生的系统误差， 提高了姿态航向参考系统测量

19、精度。 0036 3.本发明采用低成本MEMS传感器为基础， 以低成本ARM数据处理器为核心的硬件 架构。因此， 与现有的姿态航向参考系统相比， 本发明具有体积小、 重量轻、 功耗低等优点， 并大大降低了微型姿态导航系统的成本。 附图说明 0037 图 1 是基于梯度下降的四元数补偿算法流程图 ； 0038 图 2a 是数据处理前滚转角实验结果 ； 0039 图 2b 是数据处理后滚转角实验结果 ； 0040 图 3a 是数据处理前俯仰角实验结果 ； 0041 图 3b 是数据处理后俯仰角实验结果 ； 0042 图 4a 是数据处理前偏航角实验结果 ； 0043 图 4b 是数据处理后偏航角实

20、验结果。 具体实施方式 0044 下面结合实施例和附图对本发明的基于梯度下降的四旋翼无人机姿态航向参考 系统解算方法做出详细说明。 0045 如图 1 所示， 本发明的基于梯度下降的四旋翼无人机姿态航向参考系统解算方 法， 包括如下步骤 ： 0046 1) 通过陀螺仪、 加速度传感器、 地磁传感器采集微型四旋翼无人机姿态信息， 其 中， 陀螺仪测量微型四旋翼无人机的角速率， 加速度传感器测量微型四旋翼无人机的线加 速度， 地磁传感器测量微型四旋翼无人机所处位置的磁场信息 ； 0047 2) 应用基于梯度下降的四元数更新算法将加速度传感器、 地磁传感器所到的数据 与陀螺仪所采集的数据进行融合，

21、修正陀螺仪的漂移 ； 0048 所述的修正陀螺仪的漂移是 ： 0049 设传感器在刚体坐标系下的测量值为， 设在大地坐标系下的 实际值为， 为使误差最小， 需求解函数 f(Q， d， s) 的最小值 ： min f(Q， d， s)， 0050 0051 其中，为刚体坐标系对大地坐标系的旋转四元数， Q*为共轭四 元数， 0052 将代入式 (1) 得 ： 说 明 书 CN 103363992 A 7 5/6 页 8 0053 0054 根据梯度下降算法， 该 f(Q， d， s) 函数负梯度搜索的单位向量为 ： 0055 0056 0057 其中， J(Q， d) 为 f(Q， d， s)

22、的雅克比矩阵， JT(Q， d) 为 f(Q， d， s) 的雅克比矩阵的 转置 ； 0058 0059 (5) 0060 0061 通过以上推导， 得出带有梯度下降补偿算法的四元数更新方程为 ： 0062 0063 其中 Qk+1为当前时刻对陀螺仪漂移修正后的四元数， 为步长， k 为大于等于零的 整数， 为陀螺仪测得的角速度， 为四元数的乘法运算， Qk为上一时刻对陀螺仪漂移修正 后的四元数。 0064 3) 应用四元数法解算微型四旋翼无人机姿态角信息。 0065 所述的解算微型四旋翼无人机姿态角信息是 ： 0066 设微型四旋翼无人机的偏航角为 ， 俯仰角为 ， 滚转角为 ， 取大地坐标

23、系为 导航坐标系， 并规定xg、 yg、 zg分别指向东、 北、 天， 则刚体坐标系能够通过三次基本旋转坐标 变换转化为导航坐标系， 通过下式就能够解算出载体姿态角信息 0067 0068 综上可见， 本发明的基于梯度下降的四旋翼无人机姿态航向参考系统解算方法， 利用多传感器数据融合， 对低成本传感器漂移较大的问题进行了修正， 提高了微型姿态航 向系统测量精度。 0069 下面通过实验结果来对本发明的基于梯度下降的四旋翼无人机姿态航向参考系 说 明 书 CN 103363992 A 8 6/6 页 9 统解算方法的综合性能进行验证和说明。图 2a 是数据处理前滚转角实验结果 ； 图 2b 是数

24、 据处理后滚转角实验结果 ； 图 3a 是数据处理前俯仰角实验结果 ； 图 3b 是数据处理后俯仰 角实验结果， 横坐标为时间(单位 ： 毫秒)， 纵坐标为俯仰/滚转角(单位 ： 度)， 处理前角度 漂移约 10 度 / 分。采用本发明的基于梯度下降的微型四旋翼无人机姿态航向参考系统数 据解算方法进行数据解算， 经过处理后， 角度漂移约 0.3 度 / 分。图 4a 是数据处理前偏航 角实验结果 ； 图 4b 是数据处理后偏航角实验结果， 采用本发明的基于梯度下降的四旋翼无 人机姿态航向参考系统解算方法进行数据解算， 经过处理后， 角度漂移约 2 度 / 分。结果分 析表明， 本发明的基于梯度下降的四旋翼无人机姿态航向参考系统解算方法可以有效地降 低角度漂移， 提高微型姿态航向系统的稳定性和准确性， 可有效地应用于微型四旋翼无人 机系统的姿态和位置控制。 说 明 书 CN 103363992 A 9 1/3 页 10 图 1 图 2a 说 明 书 附 图 CN 103363992 A 10 2/3 页 11 图 2b 图 3a 图 3b 说 明 书 附 图 CN 103363992 A 11 3/3 页 12 图 4a 图 4b 说 明 书 附 图 CN 103363992 A 12