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

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

权利要求书
1.  一种基于梯度下降的四旋翼无人机姿态航向参考系统解算方法，其特征在于，包括如下步骤：
1）通过陀螺仪、加速度传感器、地磁传感器采集微型四旋翼无人机姿态信息，其中，陀螺仪测量微型四旋翼无人机的角速率，加速度传感器测量微型四旋翼无人机的线加速度，地磁传感器测量微型四旋翼无人机所处位置的磁场信息；
2）应用基于梯度下降的四元数更新算法将加速度传感器、地磁传感器所到的数据与陀螺仪所采集的数据进行融合，修正陀螺仪的漂移；
3）应用四元数法解算微型四旋翼无人机姿态角信息。

2.  根据权利要求1所述的基于梯度下降的四旋翼无人机姿态航向参考系统解算方法，其特征在于，步骤2）所述的修正陀螺仪的漂移是：
设传感器在刚体坐标系下的测量值为d=[0  dx  dy  dz]，设在大地坐标系下的实际值为s=[0  sx  sy  sz]，为使误差最小，需求解函数f(Q,d,s)的最小值：minf(Q,d,s)，
f(Q,d,s)=Q*⊗d⊗Q-s]]>
其中，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)=Q*⊗d⊗Q-s]]>得：
f(Q,d,s)=2dx(12-q32-q42)+2dy(q1q4+q2q3)+2dz(q2q4-q1q3)-sx2dx(q2q3-q1q4)+2dy(12-q22-q42)+2dz(q1q2+q3q4)-sy2dx(q1q3+q2q4)+2dz(12-q22-q32)+2dy(q3q4-q1q2)-sz]]>
根据梯度下降算法，该f(Q,d,s)函数负梯度搜索的单位向量为：
sk=-▿f(Q,d,s)||▿f(Q,d,s)||]]>
▿f(Q,d,s)=JT(Q,d)f(Q,d,s)]]>
其中，J(Q,d)为f(Q,d,s)的雅克比矩阵，JT(Q,d)为f(Q,d,s)的雅克比矩阵的转置；
J(Q,d)=2dyq4-2dzq32dyq3+2dzq4-2dxq4+2dzq22dxq3-4dyq2+2dzq12dxq3-2dyq22dxq4-2dyq1-4dzq2]]>
-4dxq3+2dyq2-2dzq1-4dxq4+2dyq1+2dzq22dxq2+2dzq4-2dxq1-4dyq4+2dzq32dxq1+2dyq4-4dzq32dxq2+2dyq3]]>
通过以上推导，得出带有梯度下降补偿算法的四元数更新方程为：
Qk+1=Qk+12Qk⊗ω-μ▿f(Qk,d,s)||▿f(Qk,d,s)||]]>
其中Qk+1为当前时刻对陀螺仪漂移修正后的四元数，μ为步长，k为大于等于零的整数，ω为陀螺仪测得的角速度，为四元数的乘法运算，Qk为上一时刻对陀螺仪漂移修正后的四元数。

3.  根据权利要求1所述的基于梯度下降的四旋翼无人机姿态航向参考系统解算方法，其特征在于，步骤3）所述的解算微型四旋翼无人机姿态角信息是：
设微型四旋翼无人机的偏航角为Ψ，俯仰角为θ，滚转角为γ，取大地坐标系为导航坐标系，并规定xg、yg、zg分别指向东、北、天，则刚体坐标系能够通过三次基本旋转坐标变换转化为导航坐标系，通过下式就能够解算出载体姿态角信息
θ=arcsin[2(q3q4+q1q2)]γ=arctan[-2(q2q4-q1q3)/q42-q32-q22+q12]ψ=arctan[2(q2q3-q1q4)/q32-q42-q12+q22].]]>

说明书基于梯度下降的四旋翼无人机姿态航向参考系统解算方法
技术领域
本发明涉及一种四旋翼无人机姿态航向数据测量。特别是涉及一种基于梯度下降的四旋翼无人机姿态航向参考系统解算方法。
背景技术
姿态航向参考系统是四旋翼无人机机载电子系统中的一个核心设备，其担负着为无人机飞行控制系统提供实时、可靠的飞行状态测量数据的重任。目前常见的四旋翼无人机机体尺寸较小，载荷较轻，对惯性导航单元重量和尺寸限制较为严格，因此多采用基于MEMS(微机电系统)技术的微型姿态导航系统提供飞行状态测量数据。
目前，国内与国外有多种航向姿态测量系统的组成方案和应用。法国通用机械电器公司的Cristal100航向姿态参考系统采用环形激光陀螺仪和石英加速度计，系统中采用卡尔曼滤波器，便于系统在地面、船上和飞行中进行校准。Cristal100航向姿态参考系统可用于固定翼或旋转翼飞机。美国Crossbow公司的NAV440姿态参考系统综合利用了基于MEMS的惯性器件和GPS技术，体现了MEMS基础上的最新型的测量和控制技术。广泛应用在航空、陆地、海洋导航，跟踪控制，平台稳定，ROV/AGV控制，UAV/RPV控制，精准耕种等领域。(Crossbowtechnology.NAV440GPS-AIDED MEMS Inertial Navigation System[EB/OL].http://www.xbow.com.cn/.)
在国内，微型姿态航向测量系统相关研究工作的发展较晚一些。微型姿态航向测量系统研究工作发展的核心力量主要集中在一些科研机构和高等院校。北京航空航天大学研制的微型iFLY-G2组合导航系统是小型六自由度组合导航系统。它以MEMS陀螺仪和加速度传感器为核心器件，系统依靠内部DSP处理器解算出航向姿态信息，可向机载控制器提供实时的欧拉角、三维角速率等信息。iFLY-G2为各种飞行器的稳定和控制提供精确和全面的测量信息。iFLY-G2提供AHRS/DR和GPS/INS两种组合导航模式，在GPS信息有效时，自动切换到GPS/INS组合导航模式，提供50HZ的位置、速度、姿态等信息；在GPS信息无效时，自动切换到AHRS/DR模式(包含航位推算的AHRS模式)，保证飞行的安全。
近年来，随着MEMS技术的发展，微型姿态航向测量系统也随之出现。它一般由基于MEMS技术的传感器件、专用集成电路和嵌入式微型控制器组成，可以为运动载体提供实时的姿态等相关的导航信息。微型姿态导航系统具有体积小、成本低、自主性、实时、抗干扰能力强等特点(于俊杰.微惯性组合系统设计与研究[D].哈尔滨工程大学，2007.)(刘危.基于MEMS的低成本MIMU的应用研究[D].国防科学技术大学，2004.)，但其精度主要取决于惯性器件的精度，惯性器件的测量精度单从硬件结构设计和工艺方面很难有大幅度的提高，传感器漂移较大，并且系统误差随时间积累，不适用于长时间载体姿态确定的应用。
姿态航向参考系统由多个轴向传感器组成，能够为飞行器提供航向，滚转和俯仰信息，这类系统可为飞行器提供准确可靠的姿态与航行信息。为提高无人机在飞行过程中的稳定 性，姿态航向参考系统需要实时提供无人机当前姿态角，供机载控制器使用。然而，通常情况下低成本传感器漂移较大，并且系统误差随时间积累，不适用于长时间载体姿态确定的使用，这在一定程度上限制了航向姿态测量系统的应用。
分析已有文献的技术报道可以看出，传感器漂移较大、精度低是目前微型四旋翼无人机姿态航向参考系统主要存在的问题。
微型姿态导航系统具有体积小、成本低、自主性、实时、抗干扰力强等特点，但其精度主要取决于惯性传感器件的精度，惯性传感器件的测量精度单从硬件结构设计和工艺方面很难有大幅度的提高，传感器数据漂移较大，并且系统误差随时间积累，不适用于长时间载体姿态确定的应用。
随着捷联式惯性导航技术的发展，为了更简便地描述载体的姿态信息，采用了四元数这个数学工具，用它来弥补通常描述刚体角运动的3个欧拉角参数在设计控制系统时的不足。四元数可以描述一个坐标系或一个矢量相对某一个坐标系的旋转，四元数的标量部分表示1/2转角的余弦值，而其矢量部分则表示瞬时转轴的方向、瞬时转动轴与参考坐标系轴间的方向余弦值。因此，一个四元数既表示了转轴的方向，又表示了转角的大小，往往称其为转动四元数。(张斌.基于MIMU的捷联惯性导航系统数据采集与处理技术研究[D].哈尔滨：哈尔滨工程大学，2010)通过陀螺仪采集载体角速率数据，使用四元数更新方法对数据进行解算，可得到载体姿态信息。
发明内容
本发明所要解决的技术问题是，提供一种能够获取精度较高的姿态角测量值，实现微型四旋翼无人机姿态信息估计，提高微型四旋翼无人机姿态和位置控制精度的基于梯度下降的四旋翼无人机姿态航向参考系统解算方法。
本发明所采用的技术方案是：一种基于梯度下降的四旋翼无人机姿态航向参考系统解算方法，包括如下步骤：
1)通过陀螺仪、加速度传感器、地磁传感器采集微型四旋翼无人机姿态信息，其中，陀螺仪测量微型四旋翼无人机的角速率，加速度传感器测量微型四旋翼无人机的线加速度，地磁传感器测量微型四旋翼无人机所处位置的磁场信息；
2)应用基于梯度下降的四元数更新算法将加速度传感器、地磁传感器所到的数据与陀螺仪所采集的数据进行融合，修正陀螺仪的漂移；
3)应用四元数法解算微型四旋翼无人机姿态角信息。
步骤2)所述的修正陀螺仪的漂移是：
设传感器在刚体坐标系下的测量值为d=0dxdydz]]>，设在大地坐标系下的实际值为s=0sxsysz]]>，为使误差最小，需求解函数f(Q，d，s)的最小值：min f(Q，d，s)，
f(Q,d,s)=Q*⊗d⊗Q-s]]>
其中，Q=q1q2q3q4]]>为刚体坐标系对大地坐标系的旋转四元数，Q*为共轭四元数，
将d=0dxdydz,]]>、s=0sxsysz,]]>、Q=q1q2q3q4]]>代入式f(Q,d,s)=Q*⊗d⊗Q-s]]>得：
f(Q,d,s)=2dx(12-q32-q42)+2dy(q1q4+q2q3)+2dz(q2q4-q1q3)-sx2dx(q2q3-q1q4)+2dy(12-q22-q42)+2dz(q1q2+q3q4)-sy2dx(q1q3+q2q4)+2dz(12-q22-q32)+2dy(q3q4-q1q2)-sz]]>
根据梯度下降算法，该f(Q，d，s)函数负梯度搜索的单位向量为：
sk=-▿f(Q,d,s)||▿f(Q,d,s)||]]>
▿f(Q,d,s)=JT(Q,d)f(Q,d,s)]]>
其中，J(Q，d)为f(Q，d，s)的雅克比矩阵，JT(Q，d)为f(Q，d，s)的雅克比矩阵的转置；
J(Q,d)=2dyq4-2dzq32dyq3+2dzq4-2dxq4+2dzq22dxq3-4dyq2+2dzq12dxq3-2dyq22dxq4-2dyq1-4dzq2]]>
-4dxq3+2dyq2-2dzq1-4dxq4+2dyq1+2dzq22dxq2+2dzq4-dxq1-4dyq4+2dzq32dxq1+2dyq4-4dzq32dxq2+2dyq3]]>
通过以上推导，得出带有梯度下降补偿算法的四元数更新方程为：
Qk+1=Qk+12Qk⊗ω-μ▿f(Qk,d,s)||▿f(Qk,d,s)||]]>
其中Qk+1为当前时刻对陀螺仪漂移修正后的四元数，μ为步长，k为大于等于零的整数，ω为陀螺仪测得的角速度，为四元数的乘法运算，Qk为上一时刻对陀螺仪漂移修正后的四元数。
步骤3)所述的解算微型四旋翼无人机姿态角信息是：
设微型四旋翼无人机的偏航角为ψ，俯仰角为θ，滚转角为γ，取大地坐标系为导航坐标系，并规定xg、yg、zg分别指向东、北、天，则刚体坐标系能够通过三次基本旋转坐标变换转化为导航坐标系，通过下式就能够解算出载体姿态角信息
θ=arcsin[2(q3q4+q1q2)]γ=arctan[-2(q2q4-q1q3)/q42-q32-q22+q12]ψ=arctan[2(q2q3-q1q4)/q32-q42-q12+q22].]]>
本发明的基于梯度下降的微型四旋翼无人机姿态航向参考系统数据解算方法，采用基于梯度下降法的数据融合方法，充分利用加速度传感器和地磁传感器数据，对陀螺仪数据的漂移进行实时修正，输出三维姿态角信息，从而减少系统误差，有效地提高了姿态测量精度和稳定性。本发明具有如下特点：
1、解算过程计算量小。传统的基于卡尔曼滤波器的姿态航向参考系统数据解算方法一般 需要进行大量的矩阵运算，在实际应用中对处理器运算速度要求高，提高了姿态航向参考系统成本。而本发明提出的解算方法计算量小，对处理器运算速度要求低，适用于微型无人机低成本姿态航向参考系统。
2.精度及稳定性高。本发明提出的数据解算方法融合了加速度传感器与地磁传感器数据信息，克服了陀螺仪数据漂移较大的问题，使用加速度计和磁力计数据对陀螺仪漂移进行补偿，修正了陀螺仪漂移产生的系统误差，提高了姿态航向参考系统测量精度。
3.本发明采用低成本MEMS传感器为基础，以低成本ARM数据处理器为核心的硬件架构。因此，与现有的姿态航向参考系统相比，本发明具有体积小、重量轻、功耗低等优点，并大大降低了微型姿态导航系统的成本。
附图说明
图1是基于梯度下降的四元数补偿算法流程图；
图2a是数据处理前滚转角实验结果；
图2b是数据处理后滚转角实验结果；
图3a是数据处理前俯仰角实验结果；
图3b是数据处理后俯仰角实验结果；
图4a是数据处理前偏航角实验结果；
图4b是数据处理后偏航角实验结果。
具体实施方式
下面结合实施例和附图对本发明的基于梯度下降的四旋翼无人机姿态航向参考系统解算方法做出详细说明。
如图1所示，本发明的基于梯度下降的四旋翼无人机姿态航向参考系统解算方法，包括如下步骤：
1)通过陀螺仪、加速度传感器、地磁传感器采集微型四旋翼无人机姿态信息，其中，陀螺仪测量微型四旋翼无人机的角速率，加速度传感器测量微型四旋翼无人机的线加速度，地磁传感器测量微型四旋翼无人机所处位置的磁场信息；
2)应用基于梯度下降的四元数更新算法将加速度传感器、地磁传感器所到的数据与陀螺仪所采集的数据进行融合，修正陀螺仪的漂移；
所述的修正陀螺仪的漂移是：
设传感器在刚体坐标系下的测量值为d=0dxdydz]]>，设在大地坐标系下的实际值为s=0sxsysz]]>，为使误差最小，需求解函数f(Q，d，s)的最小值：min f(Q，d，s)，
f(Q,d,s)=Q*⊗d⊗Q-s---(1)]]>
其中，Q=q1q2q3q4]]>为刚体坐标系对大地坐标系的旋转四元数，Q*为共轭四元数，
将d=0dxdydz,]]>s=0sxsysz,]]>Q=q1q2q3q4]]>代入式(1)得：
f(Q,d,s)=2dx(12-q32-q42)+2dy(q1q4+q2q3)+2dz(q2q4-q1q3)-sx2dx(q2q3-q1q4)+2dy(12-q22-q42)+2dz(q1q2+q3q4)-sy2dx(q1q3+q2q4)+2dz(12-q22-q32)+2dy(q3q4-q1q2)-sz---(2)]]>
根据梯度下降算法，该f(Q，d，s)函数负梯度搜索的单位向量为：
sk=-▿f(Q,d,s)||▿f(Q,d,s)||,---(3)]]>
▿f(Q,d,s)=JT(Q,d)f(Q,d,s)---(4)]]>
其中，J(Q，d)为f(Q，d，s)的雅克比矩阵，JT(Q，d)为f(Q，d，s)的雅克比矩阵的转置；
J(Q,d)=2dyq4-2dzq32dyq3+2dzq4-2dxq4+2dzq22dxq3-4dyq2+2dzq12dxq3-2dyq22dxq4-2dyq1-4dzq2]]>
                                             (5)
-4dxq3+2dyq2-2dzq1-4dxq4+2dyq1+2dzq22dxq2+2dzq4-dxq1-4dyq4+2dzq32dxq1+2dyq4-4dzq32dxq2+2dyq3]]>
通过以上推导，得出带有梯度下降补偿算法的四元数更新方程为：
Qk+1=Qk+12Qk⊗ω-μ▿f(Qk,d,s)||▿f(Qk,d,s)||---(6)]]>
其中Qk+1为当前时刻对陀螺仪漂移修正后的四元数，μ为步长，k为大于等于零的整数，ω为陀螺仪测得的角速度，为四元数的乘法运算，Qk为上一时刻对陀螺仪漂移修正后的四元数。
3)应用四元数法解算微型四旋翼无人机姿态角信息。
所述的解算微型四旋翼无人机姿态角信息是：
设微型四旋翼无人机的偏航角为ψ，俯仰角为θ，滚转角为γ，取大地坐标系为导航坐标系，并规定xg、yg、zg分别指向东、北、天，则刚体坐标系能够通过三次基本旋转坐标变换转化为导航坐标系，通过下式就能够解算出载体姿态角信息
θ=arcsin[2(q3q4+q1q2)]γ=arctan[-2(q2q4-q1q3)/q42-q32-q22+q12]ψ=arctan[2(q2q3-q1q4)/q32-q42-q12+q22]---(7).]]>
综上可见，本发明的基于梯度下降的四旋翼无人机姿态航向参考系统解算方法，利用多传感器数据融合，对低成本传感器漂移较大的问题进行了修正，提高了微型姿态航向系统测量精度。
下面通过实验结果来对本发明的基于梯度下降的四旋翼无人机姿态航向参考系统解算方法的综合性能进行验证和说明。图2a是数据处理前滚转角实验结果；图2b是数据处理后滚 转角实验结果；图3a是数据处理前俯仰角实验结果；图3b是数据处理后俯仰角实验结果，横坐标为时间(单位：毫秒)，纵坐标为俯仰/滚转角(单位：度)，处理前角度漂移约10度/分。采用本发明的基于梯度下降的微型四旋翼无人机姿态航向参考系统数据解算方法进行数据解算，经过处理后，角度漂移约0.3度/分。图4a是数据处理前偏航角实验结果；图4b是数据处理后偏航角实验结果，采用本发明的基于梯度下降的四旋翼无人机姿态航向参考系统解算方法进行数据解算，经过处理后，角度漂移约2度/分。结果分析表明，本发明的基于梯度下降的四旋翼无人机姿态航向参考系统解算方法可以有效地降低角度漂移，提高微型姿态航向系统的稳定性和准确性，可有效地应用于微型四旋翼无人机系统的姿态和位置控制。