CN109682377A - 一种基于动态步长梯度下降的姿态估计方法 - Google Patents

Abstract

一种基于动态步长梯度下降的姿态估计方法，包括：步骤A：惯性导航器件IMU有三轴加速度计和三轴陀螺仪，采集IMU输出的加速度计和陀螺仪数据；步骤B1：为修正运动载体的运动噪声，对来自IMU的加速度计数据信息平滑处理；然后经过动态步长梯度下降算法得到一组四元数；步骤B2：通过四元数的微分方程，使陀螺仪输出的数据求解出另外一组四元数；步骤C：把步骤B1和步骤B2中的得到的两组数据作数据融合，得到所需要的姿态信息。本发明将采用动态步长梯度下降法进行姿态解算，用梯度下降法来寻求误差函数的最小值，使得算法简便且寻优速度更快；采用一维寻优法对梯度下降法的梯度步长进行动态处理，增强了姿态解算的动态性能。

Description

一种基于动态步长梯度下降的姿态估计方法

技术领域

本发明涉及信号处理技术领域，具体的说是一种基于动态步长梯度下降的姿态估计方法。

背景技术

惯性导航系统(INS)以其全天候工作、不易受外界环境干扰等优势得到广泛应用，微机电系统(MEMS)由于其成本低、体积小、易于集成以及功耗低等优点，近年来成了惯性导航系统的首选器件，基于MEMS的惯性测量单元(IMU)能够测量载体运动的三轴陀螺信息和加速度信息，通过姿态解算就能够得到载体姿态，实现惯性导航。但是陀螺仪具有温度漂移特性，长时间运行漂移严重，并且积分运算会产生累积误差；加速度计易受载体震动和运动加速度的影响。当这几个传感器分别独立工作时，都会由于各自的局限性而导致较大的偏差，不能单独用于姿态的测量，因此如何将这传感器的数据进行融合，滤除外部干扰，得到高可靠性、高精度的姿态数据，是一项值得研究的工作。

针对姿态解算算法，国内外进行了广泛、深入的研究。针对上述问题，国内外学者相继研究出不同的姿态解算算法。如采用欧拉角法进行姿态解算，但是由于欧拉角存在奇异性问题，所以欧拉角法姿态解算无法应用于需要全姿态的运动载体上。采用方向余弦矩阵法进行姿态解算，有效地避免了欧拉角法存在的奇异问题，但旋转矩阵法所需方程的计算量大、姿态解算时效性低，无法应用于对姿态解算实时性要求较高的无人机系统。采用四元数法进行互补滤波姿态解算，减小了计算量，但是该算法由于滤波系数固定而无法实现自适应，在非重力加速度较大时解算精度迅速下降。基于梯度下降法的姿态解算法，其实质是对加速度计的测量值与重力加速度在机体坐标系的投影求差值得到误差函数，并对误差函数进行梯度下降使姿态角收敛，没有有效地消除非重力加速度所带来的影响，并且梯度下降的步长难以确定，导致姿态角收敛速度缓慢或者振荡。

发明内容

为了解决上述技术问题，本发明提供了一种基于动态步长梯度下降的姿态估计方法，解决了梯度下降的步长准确、姿态角收敛快速的问题。

为达到上述目的，本发明通过以下技术方案来具体实现：

一种基于动态步长梯度下降的姿态估计方法，包括：

步骤A：惯性导航器件IMU有三轴加速度计和三轴陀螺仪，采集IMU输出的加速度计和陀螺仪数据(其中，加速度计输出三个轴的加速度数据；陀螺仪输出三个轴的角速度数据)；

步骤B1：为修正运动载体的运动噪声，对来自IMU的加速度计数据信息平滑处理；然后经过动态步长梯度下降算法得到一组四元数；

步骤B2：通过四元数的微分方程，使陀螺仪的输出数据求解出另外一组四元数；

步骤C：把步骤B1和步骤B2中的得到的两组数据作数据融合，得到所需要的姿态信息。

所述步骤A中，IMU输出的加速度计和陀螺仪数据包括：

加速度计的(x,y,z)三轴角加速度信息、陀螺仪的(x,y,z)三轴角速度信息。

所述步骤B1中，对加速度计的输出值用均值滤波法进行平滑处理，建立一个11位数组空间，前10位为采集到的加速度计输出数据，第11位为前10位数据的平均值，且前10位遵循先进先出(FIFO)原则；加速度计(x,y,z)三轴的输出数据分别通过公式进行平滑处理。

所述步骤B1中，加速度计信息经过动态步长梯度下降算法得到四元数，包括

将导航坐标系n中重力加速度g通过四元数法旋转到载体坐标系b中的值，然后减去当前加速度计的测量值作差，得到通过加速度计表征的旋转矩阵的误差函数；重力加速度g在导航坐标系n中的值标准化后为gⁿ＝[0 0 0 1]^T；载体坐标系中加速度计的各轴分量为a_x、a_y、a_z，即载体坐标系B中的加速度表示为a^b＝[0 a_x a_y a_z]；重力加速度g从导航坐标系n旋转到载体坐标系b:

式中，g^b为向量g在b系中的坐标；为n系相对b系的四元数 为n系相对b系的复数共轭四元数；gⁿ为向量g在n系中的坐标；

将标准化gⁿ＝[0 0 0 1]^T代入(1)式，得出重力加速度在载体坐标系下的值，将其与载体坐标系中的加速度计测量值相减得到误差函数

对误差函数求导后，得出对应的雅可比矩阵：

通过雅可比矩阵得到误差函数的梯度值：

迭代公式为：

式中，为梯度下降法所求的目标姿态四元数，为迭代的上一次姿态四元数估计值，μ为梯度下降法的步长，为梯度的归一化表示梯度的方向；

迭代公式(5)表示梯度下降法所求的姿态四元数是从初始时刻的姿态沿负梯度方向更新到误差函数的极值点而迭代出来的姿态；

给定初始姿态四元数为将初始值代入(4)式计算得到初始梯度值对应的迭代公式为：

式中，为第一步迭代得到的四元数；为初始姿态四元数；为初始姿态四元数对应的梯度值；

将(6)式代入误差函数(2)式，得到一个关于步长μ的函数：

对(7)式求导得到其导数令即误差函数值最小时，求解出对应的步长μ₁，用此步长和迭代公式求出下一个姿态四元数。与上相似，计算每一步姿态四元数值后，都用此值对步长作与上类似更新。以此来实现梯度下降中的动态步长调整。

所述步骤B2中，陀螺仪信息经过四元数微分方程得到四元数，包括：

四元数微分方程:

式中，为b系相对于n系的四元数；为的导数；ω为载体的各轴角速度，即陀螺仪的输出量 为四元数乘法；

(8)式可写为

采用先离散后迭代的方法对四元数微分方程进行求解定义系统采样周期为T_s，离散化后四元数方程为:

式中：为k时刻姿态四元数；为k-1时刻姿态四元数；为k时刻四元数导数。

所述步骤C中，具体算法为：

基于四元数微分方程和梯度下降的姿态融合算法为

式(11)中，为梯度下降法求出的姿态四元数；q_ω(k)为基于陀螺仪的四元微分方程求出的姿态四元数；α和1-α分别为两种姿态四元数的权重；

式(11)取最优姿态的条件为的收敛速度等于q_ω(k)的发散速度，即

其中，β为四元数微分方程求解姿态算法的收敛速度，当取得最优姿态时β也就等于梯度下降的步长μ；由于当载体高速运动时，载体姿态变化大，动态步长μ随之也变大，因此式(12)可近似为:

那么式(5)可重新定义为

将式(14)和式(10)代入式(11)得到最终动姿态融合算法为：

式中，q_est(k)为k时刻的姿态四元数；q_est(k-1)为k-1时刻的姿态四元数；为陀螺仪的四元数微分；μ为梯度下降的步长；T_s为采样周期。

所用的惯性导航器件IMU为3DM-AHRS300A航姿参考系统。

本发明将采用动态步长梯度下降法进行姿态解算，用梯度下降法来寻求误差函数的最小值，使得算法简便且寻优速度更快；采用一维寻优法对梯度下降法的梯度步长进行动态处理，使得算法步长与载体的运动合角速度成正比，增强了姿态解算的动态性能；同时，用均值滤波对姿态估计前的加速度计数据作平滑处理，可以修正运动载体的运动噪声，消除运动加速度对运动姿态的不利影响。

附图说明

下面根据附图和实施例对本发明作进一步详细说明。

图1基于动态步长梯度下降的姿态估计方法结构图。

图2具体算法流程图。

图3仿真实验流程图。

具体实施方式

如图1-3所示，本发明实施例提供一种基于动态步长梯度下降的姿态估计方法，包括：

步骤A：惯性导航器件IMU有三轴加速度计和三轴陀螺仪，采集IMU输出的加速度计和陀螺仪数据(其中，加速度计输出三个轴的加速度数据；陀螺仪输出三个轴的角速度数据)；

步骤B1：为修正运动载体的运动噪声，对来自IMU的加速度计数据信息平滑处理；然后经过动态步长梯度下降算法得到一组四元数；

步骤B2：通过四元数的微分方程，使陀螺仪的输出数据求解出另外一组四元数；

步骤C：把步骤B1和步骤B2中的得到的两组数据作数据融合，得到所需要的姿态信息。

所述步骤A中，IMU输出的加速度计和陀螺仪数据包括：

加速度计的(x,y,z)三轴角加速度信息、陀螺仪的(x,y,z)三轴角速度信息。

所述步骤B1中，对加速度计的输出值用均值滤波法进行平滑处理，建立一个11位数组空间，前10位为采集到的加速度计输出数据，第11位为前10位数据的平均值，且前10位遵循先进先出(FIFO)原则；加速度计(x,y,z)三轴的输出数据分别通过公式进行平滑处理。

所述步骤B1中，加速度计信息经过动态步长梯度下降算法得到四元数，包括

将导航坐标系n中重力加速度g通过四元数法旋转到载体坐标系b中的值，然后减去当前加速度计的测量值作差，得到通过加速度计表征的旋转矩阵的误差函数；重力加速度g在导航坐标系n中的值标准化后为gⁿ＝[0 0 0 1]^T；载体坐标系中加速度计的各轴分量为a_x、a_y、a_z，即载体坐标系B中的加速度表示为a^b＝[0 a_x a_y a_z]；重力加速度g从导航坐标系n旋转到载体坐标系b:

式中，g^b为向量g在b系中的坐标；为n系相对b系的四元数 为n系相对b系的复数共轭四元数；gⁿ为向量g在n系中的坐标；

将标准化gⁿ＝[0 0 0 1]^T代入(1)式，得出重力加速度在载体坐标系下的值，将其与载体坐标系中的加速度计测量值相减得到误差函数

对误差函数求导后，得出对应的雅可比矩阵：

通过雅可比矩阵得到误差函数的梯度值：

迭代公式为：

式中，为梯度下降法所求的目标姿态四元数，为迭代的上一次姿态四元数估计值，μ为梯度下降法的步长，为梯度的归一化表示梯度的方向；

迭代公式(5)表示梯度下降法所求的姿态四元数是从初始时刻的姿态沿负梯度方向更新到误差函数的极值点而迭代出来的姿态；

给定初始姿态四元数为将初始值代入(4)式计算得到初始梯度值对应的迭代公式为：

式中，为第一步迭代得到的四元数；为初始姿态四元数；为初始姿态四元数对应的梯度值；

将(6)式代入误差函数(2)式，得到一个关于步长μ的函数：

对(7)式求导得到其导数令即误差函数值最小时，求解出对应的步长μ₁，用此步长和迭代公式求出下一个姿态四元数。与上相似，计算每一步姿态四元数值后，都用此值对步长作与上类似更新。以此来实现梯度下降中的动态步长调整。

所述步骤B2中，陀螺仪信息经过四元数微分方程得到四元数，包括：

四元数微分方程:

式中，为b系相对于n系的四元数；为的导数；ω为载体的各轴角速度，即陀螺仪的输出量 为四元数乘法；

(8)式可写为

采用先离散后迭代的方法对四元数微分方程进行求解定义系统采样周期为T_s，离散化后四元数方程为:

式中：为k时刻姿态四元数；为k-1时刻姿态四元数；为k时刻四元数导数。

所述步骤C中，具体算法为：

基于四元数微分方程和梯度下降的姿态融合算法为

式(11)中，为梯度下降法求出的姿态四元数；q_ω(k)为基于陀螺仪的四元微分方程求出的姿态四元数；α和1-α分别为两种姿态四元数的权重；

式(11)取最优姿态的条件为的收敛速度等于q_ω(k)的发散速度，即

其中，β为四元数微分方程求解姿态算法的收敛速度，当取得最优姿态时β也就等于梯度下降的步长μ；由于当载体高速运动时，载体姿态变化大，动态步长μ随之也变大，因此式(12)可近似为:

那么式(5)可重新定义为

将式(14)和式(10)代入式(11)得到最终动姿态融合算法为：

式中，q_est(k)为k时刻的姿态四元数；q_est(k-1)为k-1时刻的姿态四元数；为陀螺仪的四元数微分；μ为梯度下降的步长；T_s为采样周期。

所用的惯性导航器件IMU为3DM-AHRS300A航姿参考系统。

本发明将采用动态步长梯度下降法进行姿态解算，用梯度下降法来寻求误差函数的最小值，使得算法简便且寻优速度更快；采用一维寻优法对梯度下降法的梯度步长进行动态处理，使得算法步长与载体的运动合角速度成正比，增强了姿态解算的动态性能；同时，用均值滤波对姿态估计前的加速度计数据作平滑处理，可以修正运动载体的运动噪声，消除运动加速度对运动姿态的不利影响。

最后应说明的是：以上所述仅为发明的优选实施例而已，并不用于限制发明，尽管参照前述实施例对发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在发明的保护范围之内。

Claims (7)

1.一种基于动态步长梯度下降的姿态估计方法，包括：

步骤A：惯性导航器件IMU有三轴加速度计和三轴陀螺仪，采集IMU输出的加速度计和陀螺仪数据；

步骤B1：为修正运动载体的运动噪声，对来自IMU的加速度计数据信息平滑处理；然后经过动态步长梯度下降算法得到一组四元数；

步骤B2：通过四元数的微分方程，使陀螺仪输出的数据求解出另外一组四元数；

步骤C：把步骤B1和步骤B2中的得到的两组数据作数据融合，得到所需要的姿态信息。

2.如权利要求1所述的方法，其特征在于，

所述步骤A中，IMU输出的加速度计和陀螺仪数据包括：

加速度计的(x,y,z)三轴角加速度信息、陀螺仪的(x,y,z)三轴角速度信息。

3.如权利要求1所述的方法，其特征在于，

所述步骤B1中，对加速度计的输出值用均值滤波法进行平滑处理，建立一个11位数组空间，前10位为采集到的加速度计输出数据，第11位为前10位数据的平均值，且前10位遵循先进先出(FIFO)原则；加速度计(x,y,z)三轴的输出数据分别通过公式进行平滑处理。

4.如权利要求1所述的方法，其特征在于，

所述步骤B1中，加速度计信息经过动态步长梯度下降算法得到四元数，包括

将导航坐标系n中重力加速度g通过四元数法旋转到载体坐标系b中的值，然后减去当前加速度计的测量值作差，得到通过加速度计表征的旋转矩阵的误差函数；重力加速度g在导航坐标系n中的值标准化后为gⁿ＝[0 0 0 1]^T；载体坐标系中加速度计的各轴分量为a_x、a_y、a_z，即载体坐标系B中的加速度表示为a^b＝[0 a_x a_y a_z]；重力加速度g从导航坐标系n旋转到载体坐标系b:

式中，g^b为向量g在b系中的坐标；为n系相对b系的四元数 为n系相对b系的复数共轭四元数；gⁿ为向量g在n系中的坐标；

将标准化gⁿ＝[0 0 0 1]^T代入(1)式，得出重力加速度在载体坐标系下的值，将其与载体坐标系中的加速度计测量值相减得到误差函数

对误差函数求导后，得出对应的雅可比矩阵：

通过雅可比矩阵得到误差函数的梯度值：

迭代公式为：

式中，为梯度下降法所求的目标姿态四元数，为迭代的上一次姿态四元数估计值，μ为梯度下降法的步长，为梯度的归一化表示梯度的方向；

迭代公式(5)表示梯度下降法所求的姿态四元数是从初始时刻的姿态沿负梯度方向更新到误差函数的极值点而迭代出来的姿态；

给定初始姿态四元数为将初始值代入(4)式计算得到初始梯度值对应的迭代公式为：

式中，为第一步迭代得到的四元数；为初始姿态四元数；为初始姿态四元数对应的梯度值；

将(6)式代入误差函数(2)式，得到一个关于步长μ的函数：

对(7)式求导得到其导数令即误差函数值最小时，求解出对应的步长μ₁，用此步长和迭代公式求出下一个姿态四元数。与上相似，计算每一步姿态四元数值后，都用此值对步长作与上类似更新。以此来实现梯度下降中的动态步长调整。

5.如权利要求4所述的方法，其特征在于，

所述步骤B2中，陀螺仪信息经过四元数微分方程得到四元数，包括：

四元数微分方程:

式中，为b系相对于n系的四元数；为的导数；ω为载体的各轴角速度，即陀螺仪的输出量 为四元数乘法；

(8)式可写为

采用先离散后迭代的方法对四元数微分方程进行求解定义系统采样周期为T_s，离散化后四元数方程为:

式中：为k时刻姿态四元数；为k-1时刻姿态四元数；为k时刻四元数导数。

6.如权利要求5所述的方法，其特征在于，

所述步骤C中，具体算法为：

基于四元数微分方程和梯度下降的姿态融合算法为

q_est(k)＝αq_▽(k)+(1-α)q_ω(k) (11)

式(11)中，q_▽(k)为梯度下降法求出的姿态四元数；q_ω(k)为基于陀螺仪的四元微分方程求出的姿态四元数；α和1-α分别为两种姿态四元数的权重；

式(11)取最优姿态的条件为q_▽(k)的收敛速度等于q_ω(k)的发散速度，即

其中，β为四元数微分方程求解姿态算法的收敛速度，当取得最优姿态时β也就等于梯度下降的步长μ；由于当载体高速运动时，载体姿态变化大，动态步长μ随之也变大，因此式(12)可近似为:

那么式(5)可重新定义为

将式(14)和式(10)代入式(11)得到最终动姿态融合算法为：

式中，q_est(k)为k时刻的姿态四元数；q_est(k-1)为k-1时刻的姿态四元数；为陀螺仪的四元数微分；μ为梯度下降的步长；T_s为采样周期。

7.如权利要求1所述的方法，其特征在于，

所用的惯性导航器件IMU为3DM-AHRS300A航姿参考系统。