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

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

Info

Publication number
CN109682377B
CN109682377B CN201910175278.5A CN201910175278A CN109682377B CN 109682377 B CN109682377 B CN 109682377B CN 201910175278 A CN201910175278 A CN 201910175278A CN 109682377 B CN109682377 B CN 109682377B
Authority
CN
China
Prior art keywords
quaternion
attitude
accelerometer
formula
gradient descent
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.)
Active
Application number
CN201910175278.5A
Other languages
English (en)
Other versions
CN109682377A (zh
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.)
Lanzhou Jiaotong University
Original Assignee
Lanzhou Jiaotong 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 Lanzhou Jiaotong University filed Critical Lanzhou Jiaotong University
Priority to CN201910175278.5A priority Critical patent/CN109682377B/zh
Publication of CN109682377A publication Critical patent/CN109682377A/zh
Application granted granted Critical
Publication of CN109682377B publication Critical patent/CN109682377B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)
  • Gyroscopes (AREA)

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)三轴的输出数据分别通过公式
Figure GDA0002657888930000011
进行平滑处理。
所述步骤B1中，加速度计信息经过动态步长梯度下降算法得到四元数，包括
将导航坐标系n中重力加速度g通过四元数法旋转到载体坐标系b中的值，然后减去当前加速度计的测量值作差，得到通过加速度计表征的旋转矩阵的误差函数；重力加速度g在导航坐标系n中的值标准化后为gn＝[0 0 0 1]T；载体坐标系中加速度计的各轴分量为ax、ay、az，即载体坐标系B中的加速度表示为ab＝[0 ax ay az]；重力加速度g从导航坐标系n旋转到载体坐标系b:
Figure GDA0002657888930000021
式中，gb为向量g在b系中的坐标；
Figure GDA0002657888930000022
为n系相对b系的四元数
Figure GDA0002657888930000023
Figure GDA0002657888930000024
为n系相对b系的复数共轭四元数；gn为向量g在n系中的坐标；
将标准化gn＝[0 0 0 1]T代入(1)式，得出重力加速度在载体坐标系下的值，将其与载体坐标系中的加速度计测量值相减得到误差函数
Figure GDA0002657888930000025
Figure GDA0002657888930000026
对误差函数
Figure GDA0002657888930000027
求导后，得出对应的雅可比矩阵：
Figure GDA0002657888930000028
通过雅可比矩阵得到误差函数的梯度值：
Figure GDA0002657888930000029
迭代公式为：
Figure GDA00026578889300000210
式中，
Figure GDA00026578889300000211
为梯度下降法所求的目标姿态四元数，
Figure GDA00026578889300000212
为迭代的上一次姿态四元数估计值，μ为梯度下降法的步长，
Figure GDA00026578889300000213
为梯度的归一化表示梯度的方向；
迭代公式(5)表示梯度下降法所求的姿态四元数是从初始时刻的姿态沿负梯度方向更新到误差函数的极值点而迭代出来的姿态；
给定初始姿态四元数为
Figure GDA0002657888930000031
将初始值代入(4)式计算得到初始梯度值
Figure GDA0002657888930000032
对应的迭代公式为：
Figure GDA0002657888930000033
式中，
Figure GDA0002657888930000034
为第一步迭代得到的四元数；
Figure GDA0002657888930000035
为初始姿态四元数；
Figure GDA0002657888930000036
为初始姿态四元数对应的梯度值；
将(6)式代入误差函数(2)式，得到一个关于步长μ的函数：
Figure GDA0002657888930000037
对(7)式求导得到其导数
Figure GDA0002657888930000038
令
Figure GDA0002657888930000039
即误差函数值最小时，求解出对应的步长μ1，用此步长和迭代公式求出下一个姿态四元数。与上相似，计算每一步姿态四元数值后，都用此值对步长作与上类似更新。以此来实现梯度下降中的动态步长调整。
所述步骤B2中，陀螺仪信息经过四元数微分方程得到四元数，包括：
四元数微分方程:
Figure GDA00026578889300000310
式中，
Figure GDA00026578889300000311
为b系相对于n系的四元数；
Figure GDA00026578889300000312
为
Figure GDA00026578889300000313
的导数；ω为载体的各轴角速度，即陀螺仪的输出量
Figure GDA00026578889300000314
Figure GDA00026578889300000315
为四元数乘法；
(8)式可写为
Figure GDA00026578889300000316
采用先离散后迭代的方法对四元数微分方程进行求解定义系统采样周期为Ts，离散化后四元数方程为:
Figure GDA00026578889300000317
式中：
Figure GDA00026578889300000318
为k时刻姿态四元数；
Figure GDA00026578889300000319
为k-1时刻姿态四元数；
Figure GDA00026578889300000320
为k时刻四元数导数。
所述步骤C中，具体算法为：
基于四元数微分方程和梯度下降的姿态融合算法为
Figure GDA00026578889300000321
式(11)中，
Figure GDA00026578889300000322
为梯度下降法求出的姿态四元数；qω(k)为基于陀螺仪的四元微分方程求出的姿态四元数；α和1-α分别为两种姿态四元数的权重；
式(11)取最优姿态的条件为
Figure GDA0002657888930000041
的收敛速度等于qω(k)的发散速度，即
Figure GDA0002657888930000042
其中，β为四元数微分方程求解姿态算法的收敛速度，当取得最优姿态时β也就等于梯度下降的步长μ；由于当载体高速运动时，载体姿态变化大，动态步长μ随之也变大，因此式(12)可近似为:
Figure GDA0002657888930000043
那么式(5)可重新定义为
Figure GDA0002657888930000044
将式(14)和式(10)代入式(11)得到最终动姿态融合算法为：
Figure GDA0002657888930000045
式中，qest(k)为k时刻的姿态四元数；qest(k-1)为k-1时刻的姿态四元数；
Figure GDA0002657888930000046
为陀螺仪的四元数微分；μ为梯度下降的步长；Ts为采样周期。
所用的惯性导航器件IMU为3DM-AHRS300A航姿参考系统。
本发明将采用动态步长梯度下降法进行姿态解算，用梯度下降法来寻求误差函数的最小值，使得算法简便且寻优速度更快；采用一维寻优法对梯度下降法的梯度步长进行动态处理，使得算法步长与载体的运动合角速度成正比，增强了姿态解算的动态性能；同时，用均值滤波对姿态估计前的加速度计数据作平滑处理，可以修正运动载体的运动噪声，消除运动加速度对运动姿态的不利影响。
附图说明
下面根据附图和实施例对本发明作进一步详细说明。
图1基于动态步长梯度下降的姿态估计方法结构图。
图2具体算法流程图。
图3仿真实验流程图。
具体实施方式
如图1-3所示，本发明实施例提供一种基于动态步长梯度下降的姿态估计方法，包括：
步骤A：惯性导航器件IMU有三轴加速度计和三轴陀螺仪，采集IMU输出的加速度计和陀螺仪数据(其中，加速度计输出三个轴的加速度数据；陀螺仪输出三个轴的角速度数据)；
步骤B1：为修正运动载体的运动噪声，对来自IMU的加速度计数据信息平滑处理；然后经过动态步长梯度下降算法得到一组四元数；
步骤B2：通过四元数的微分方程，使陀螺仪的输出数据求解出另外一组四元数；
步骤C：把步骤B1和步骤B2中的得到的两组数据作数据融合，得到所需要的姿态信息。
所述步骤A中，IMU输出的加速度计和陀螺仪数据包括：
加速度计的(xy，z)三轴加速度信息、陀螺仪的(x，y，z)三轴角速度信息。
所述步骤B1中，对加速度计的输出值用均值滤波法进行平滑处理，建立一个11位数组空间，前10位为采集到的加速度计输出数据，第11位为前10位数据的平均值，且前10位遵循先进先出(FIFO)原则；加速度计(x,y,z)三轴的输出数据分别通过公式
Figure GDA0002657888930000051
进行平滑处理。
所述步骤B1中，加速度计信息经过动态步长梯度下降算法得到四元数，包括
将导航坐标系n中重力加速度g通过四元数法旋转到载体坐标系b中的值，然后减去当前加速度计的测量值作差，得到通过加速度计表征的旋转矩阵的误差函数；重力加速度g在导航坐标系n中的值标准化后为gn＝[0 0 0 1]T；载体坐标系中加速度计的各轴分量为ax、ay、az，即载体坐标系B中的加速度表示为ab＝[0 ax ay az]；重力加速度g从导航坐标系n旋转到载体坐标系b:
Figure GDA0002657888930000052
式中，gb为向量g在b系中的坐标；
Figure GDA0002657888930000053
为n系相对b系的四元数
Figure GDA0002657888930000054
Figure GDA0002657888930000055
为n系相对b系的复数共轭四元数；gn为向量g在n系中的坐标；
将标准化gn＝[0 0 0 1]T代入(1)式，得出重力加速度在载体坐标系下的值，将其与载体坐标系中的加速度计测量值相减得到误差函数
Figure GDA0002657888930000056
Figure GDA0002657888930000057
对误差函数
Figure GDA0002657888930000058
求导后，得出对应的雅可比矩阵：
Figure GDA0002657888930000059
通过雅可比矩阵得到误差函数的梯度值：
Figure GDA00026578889300000510
迭代公式为：
Figure GDA0002657888930000061
式中，
Figure GDA0002657888930000062
为梯度下降法所求的目标姿态四元数，
Figure GDA0002657888930000063
为迭代的上一次姿态四元数估计值，μ为梯度下降法的步长，
Figure GDA0002657888930000064
为梯度的归一化表示梯度的方向；
迭代公式(5)表示梯度下降法所求的姿态四元数是从初始时刻的姿态沿负梯度方向更新到误差函数的极值点而迭代出来的姿态；
给定初始姿态四元数为
Figure GDA0002657888930000065
将初始值代入(4)式计算得到初始梯度值
Figure GDA0002657888930000066
对应的迭代公式为：
Figure GDA0002657888930000067
式中，
Figure GDA0002657888930000068
为第一步迭代得到的四元数；
Figure GDA0002657888930000069
为初始姿态四元数；
Figure GDA00026578889300000610
为初始姿态四元数对应的梯度值；
将(6)式代入误差函数(2)式，得到一个关于步长μ的函数：
Figure GDA00026578889300000611
对(7)式求导得到其导数
Figure GDA00026578889300000612
令
Figure GDA00026578889300000613
即误差函数值最小时，求解出对应的步长μ1，用此步长和迭代公式求出下一个姿态四元数。与上相似，计算每一步姿态四元数值后，都用此值对步长作与上类似更新。以此来实现梯度下降中的动态步长调整。
所述步骤B2中，陀螺仪信息经过四元数微分方程得到四元数，包括：
四元数微分方程:
Figure GDA00026578889300000614
式中，
Figure GDA00026578889300000615
为b系相对于n系的四元数；
Figure GDA00026578889300000616
为
Figure GDA00026578889300000617
的导数；ω为载体的各轴角速度，即陀螺仪的输出量
Figure GDA00026578889300000618
Figure GDA00026578889300000619
为四元数乘法；
(8)式可写为
Figure GDA00026578889300000620
采用先离散后迭代的方法对四元数微分方程进行求解定义系统采样周期为Ts，离散化后四元数方程为:
Figure GDA0002657888930000071
式中：
Figure GDA0002657888930000072
为k时刻姿态四元数；
Figure GDA0002657888930000073
为k-1时刻姿态四元数；
Figure GDA0002657888930000074
为k时刻四元数导数。
所述步骤C中，具体算法为：
基于四元数微分方程和梯度下降的姿态融合算法为
Figure GDA0002657888930000075
式(11)中，
Figure GDA0002657888930000076
为梯度下降法求出的姿态四元数；qω(k)为基于陀螺仪的四元微分方程求出的姿态四元数；α和1-α分别为两种姿态四元数的权重；
式(11)取最优姿态的条件为
Figure GDA0002657888930000077
的收敛速度等于qω(k)的发散速度，即
Figure GDA0002657888930000078
其中，β为四元数微分方程求解姿态算法的收敛速度，当取得最优姿态时β也就等于梯度下降的步长μ；由于当载体高速运动时，载体姿态变化大，动态步长μ随之也变大，因此式(12)可近似为:
Figure GDA0002657888930000079
那么式(5)可重新定义为
Figure GDA00026578889300000710
将式(14)和式(10)代入式(11)得到最终动姿态融合算法为：
Figure GDA00026578889300000711
式中，qest(k)为k时刻的姿态四元数；qest(k-1)为k-1时刻的姿态四元数；
Figure GDA00026578889300000712
为陀螺仪的四元数微分；μ为梯度下降的步长；Ts为采样周期。
所用的惯性导航器件IMU为3DM-AHRS300A航姿参考系统。
本发明将采用动态步长梯度下降法进行姿态解算，用梯度下降法来寻求误差函数的最小值，使得算法简便且寻优速度更快；采用一维寻优法对梯度下降法的梯度步长进行动态处理，使得算法步长与载体的运动合角速度成正比，增强了姿态解算的动态性能；同时，用均值滤波对姿态估计前的加速度计数据作平滑处理，可以修正运动载体的运动噪声，消除运动加速度对运动姿态的不利影响。
最后应说明的是：以上所述仅为发明的优选实施例而已，并不用于限制发明，尽管参照前述实施例对发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在发明的保护范围之内。

Claims (6)

1.一种基于动态步长梯度下降的姿态估计方法，包括：
步骤A：惯性导航器件IMU有三轴加速度计和三轴陀螺仪，采集IMU输出的加速度计和陀螺仪数据；
步骤B1：为修正运动载体的运动噪声，对来自IMU的加速度计数据信息平滑处理；然后经过动态步长梯度下降算法得到一组四元数；
步骤B2：通过四元数的微分方程，使陀螺仪输出的数据求解出另外一组四元数；
步骤C：把步骤B1和步骤B2中的得到的两组数据作数据融合，得到所需要的姿态信息；
所述步骤B1中，加速度计信息经过动态步长梯度下降算法得到四元数，包括
将导航坐标系n中重力加速度g通过四元数法旋转到载体坐标系b中的值，然后减去当前加速度计的测量值作差，得到通过加速度计表征的旋转矩阵的误差函数；重力加速度g在导航坐标系n中的值标准化后为gn＝[0 0 0 1]T；载体坐标系中加速度计的各轴分量为ax、ay、az，即载体坐标系B中的加速度表示为ab＝[0ax ay az]；重力加速度g从导航坐标系n旋转到载体坐标系b:
Figure FDA0002657888920000011
式中，gb为向量g在b系中的坐标；
Figure FDA0002657888920000012
为n系相对b系的四元数
Figure FDA0002657888920000013
Figure FDA0002657888920000014
为n系相对b系的复数共轭四元数；gn为向量g在n系中的坐标；
将标准化gn＝[0 0 0 1]T代入(1)式，得出重力加速度在载体坐标系下的值，将其与载体坐标系中的加速度计测量值相减得到误差函数
Figure FDA0002657888920000015
Figure FDA0002657888920000016
对误差函数
Figure FDA0002657888920000017
求导后，得出对应的雅可比矩阵：
Figure FDA0002657888920000018
通过雅可比矩阵得到误差函数的梯度值：
Figure FDA0002657888920000019
迭代公式为：
Figure FDA0002657888920000021
式中，
Figure FDA0002657888920000022
为梯度下降法所求的目标姿态四元数，
Figure FDA0002657888920000023
为迭代的上一次姿态四元数估计值，μ为梯度下降法的步长，
Figure FDA0002657888920000024
为梯度的归一化表示梯度的方向；
迭代公式(5)表示梯度下降法所求的姿态四元数是从初始时刻的姿态沿负梯度方向更新到误差函数的极值点而迭代出来的姿态；
给定初始姿态四元数为
Figure FDA0002657888920000025
将初始值代入(4)式计算得到初始梯度值
Figure FDA0002657888920000026
对应的迭代公式为：
Figure FDA0002657888920000027
式中，
Figure FDA0002657888920000028
为第一步迭代得到的四元数；
Figure FDA0002657888920000029
为初始姿态四元数；
Figure FDA00026578889200000210
为初始姿态四元数对应的梯度值；
将(6)式代入误差函数(2)式，得到一个关于步长μ的函数：
Figure FDA00026578889200000211
对(7)式求导得到其导数
Figure FDA00026578889200000212
令
Figure FDA00026578889200000213
即误差函数值最小时，求解出对应的步长μ1，用此步长和迭代公式求出下一个姿态四元数；与上相似，计算每一步姿态四元数值后，都用此值对步长作与上类似更新；以此来实现梯度下降中的动态步长调整。
2.如权利要求1所述的方法，其特征在于，
所述步骤A中，IMU输出的加速度计和陀螺仪数据包括：
加速度计的(x,y,z)三轴加速度信息、陀螺仪的(x,y,z)三轴角速度信息。
3.如权利要求1所述的方法，其特征在于，
所述步骤B1中，对加速度计的输出值用均值滤波法进行平滑处理，建立一个11位数组空间，前10位为采集到的加速度计输出数据，第11位为前10位数据的平均值，且前10位遵循先进先出(FIFO)原则；加速度计(x,y,z)三轴的输出数据分别通过公式
Figure FDA00026578889200000214
进行平滑处理。
4.如权利要求1所述的方法，其特征在于，
所述步骤B2中，陀螺仪信息经过四元数微分方程得到四元数，包括：
四元数微分方程:
Figure FDA00026578889200000215
式中，
Figure FDA0002657888920000031
为b系相对于n系的四元数；
Figure FDA0002657888920000032
为
Figure FDA0002657888920000033
的导数；ω为载体的各轴角速度，即陀螺仪的输出量
Figure FDA0002657888920000034
Figure FDA0002657888920000035
为四元数乘法；
(8)式可写为
Figure FDA0002657888920000036
采用先离散后迭代的方法对四元数微分方程进行求解定义系统采样周期为Ts，离散化后四元数方程为:
Figure FDA0002657888920000037
式中：
Figure FDA0002657888920000038
为k时刻姿态四元数；
Figure FDA0002657888920000039
为k-1时刻姿态四元数；
Figure FDA00026578889200000310
为k时刻四元数导数。
5.如权利要求4所述的方法，其特征在于，
所述步骤C中，具体算法为：
基于四元数微分方程和梯度下降的姿态融合算法为
Figure FDA00026578889200000311
式(11)中，
Figure FDA00026578889200000312
为梯度下降法求出的姿态四元数；qω(k)为基于陀螺仪的四元微分方程求出的姿态四元数；α和1-α分别为两种姿态四元数的权重；
式(11)取最优姿态的条件为
Figure FDA00026578889200000313
的收敛速度等于qω(k)的发散速度，即
Figure FDA00026578889200000314
其中，β为四元数微分方程求解姿态算法的收敛速度，当取得最优姿态时β也就等于梯度下降的步长μ；由于当载体高速运动时，载体姿态变化大，动态步长μ随之也变大，因此式(12)可近似为:
Figure FDA00026578889200000315
那么式(5)可重新定义为
Figure FDA00026578889200000316
将式(14)和式(10)代入式(11)得到最终动姿态融合算法为：
Figure FDA00026578889200000317
式中，qest(k)为k时刻的姿态四元数；qest(k-1)为k-1时刻的姿态四元数；
Figure FDA00026578889200000318
为陀螺仪的四元数微分；μ为梯度下降的步长；Ts为采样周期。
6.如权利要求1所述的方法，其特征在于，
所用的惯性导航器件IMU为3DM-AHRS300A航姿参考系统。
CN201910175278.5A 2019-03-08 2019-03-08 一种基于动态步长梯度下降的姿态估计方法 Active CN109682377B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910175278.5A CN109682377B (zh) 2019-03-08 2019-03-08 一种基于动态步长梯度下降的姿态估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910175278.5A CN109682377B (zh) 2019-03-08 2019-03-08 一种基于动态步长梯度下降的姿态估计方法

Publications (2)

Publication Number Publication Date
CN109682377A CN109682377A (zh) 2019-04-26
CN109682377B true CN109682377B (zh) 2020-10-20

Family

ID=66184302

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910175278.5A Active CN109682377B (zh) 2019-03-08 2019-03-08 一种基于动态步长梯度下降的姿态估计方法

Country Status (1)

Country Link
CN (1) CN109682377B (zh)

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110207696A (zh) * 2019-06-06 2019-09-06 南京理工大学 一种基于拟牛顿法的九轴运动传感器姿态测量方法
CN110440746A (zh) * 2019-08-05 2019-11-12 桂林电子科技大学 一种基于四元数梯度下降的非开挖地下钻头姿态融合方法
CN110531613A (zh) * 2019-09-05 2019-12-03 广东工业大学 二轴无人机控制方法、装置、设备及计算机可读存储介质
CN111964676A (zh) * 2020-08-26 2020-11-20 三一机器人科技有限公司 航姿解算方法、装置、电子设备及计算机可读存储介质
CN112051868A (zh) * 2020-09-10 2020-12-08 湖北咿呀医疗投资管理股份有限公司 智能电动牙刷轨迹的定位方法及系统
CN112179342B (zh) * 2020-09-21 2021-08-27 中国安全生产科学研究院 一种估计无人机姿态的方法及系统
CN112665574B (zh) * 2020-11-26 2024-02-27 江苏科技大学 基于动量梯度下降法的水下机器人姿态采集方法
CN113390437B (zh) * 2021-05-06 2023-04-07 上海奥欧智能科技有限公司 一种基于imu的计步定位的步长修正系统及方法
CN113672137B (zh) * 2021-08-17 2024-08-16 北京字节跳动网络技术有限公司 光标位置更新方法、装置和电子设备
CN114459475A (zh) * 2022-02-28 2022-05-10 宁波赛福汽车制动有限公司 摩托车的车身姿态检测方法、系统、摩托车及存储介质

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104648685B (zh) * 2015-02-12 2016-08-24 武汉科技大学 基于智能手机的四旋翼飞行器指定路径航拍系统和方法
CN107664498A (zh) * 2017-08-25 2018-02-06 广州新维感信息技术有限公司 一种姿态融合解算方法及系统
CN108037658A (zh) * 2017-11-15 2018-05-15 东莞市松迪智能机器人科技有限公司 一种基于导航系统的机器人运行误差的纠偏方法

Also Published As

Publication number Publication date
CN109682377A (zh) 2019-04-26

Similar Documents

Publication Publication Date Title
CN109682377B (zh) 一种基于动态步长梯度下降的姿态估计方法
CN109696183B (zh) 惯性测量单元的标定方法及装置
WO2020220729A1 (zh) 基于角加速度计/陀螺/加速度计的惯性导航解算方法
CN110125936B (zh) 一种空间机器人的地面实验验证系统
CN104898681B (zh) 一种采用三阶近似毕卡四元数的四旋翼飞行器姿态获取方法
CN107560613B (zh) 基于九轴惯性传感器的机器人室内轨迹跟踪系统及方法
CN112630813B (zh) 基于捷联惯导和北斗卫星导航系统的无人机姿态测量方法
CN107063262A (zh) 一种用于无人机姿态解算的互补滤波方法
CN110174121A (zh) 一种基于地磁场自适应修正的航姿系统姿态解算方法
CN108731676B (zh) 一种基于惯性导航技术的姿态融合增强测量方法及系统
CN107402007A (zh) 一种提高微型ahrs模块精度的方法和微型ahrs模块
CN108981694A (zh) 基于小波神经网络与ekf的姿态解算方法及系统
CN105890593A (zh) 一种mems惯性导航系统及基于该系统的轨迹重构方法
CN1664506A (zh) 一种载体姿态测量方法及其系统
CN112665574B (zh) 基于动量梯度下降法的水下机器人姿态采集方法
Liu et al. A complementary filter based on multi-sample rotation vector for attitude estimation
CN108871319B (zh) 一种基于地球重力场与地磁场序贯修正的姿态解算方法
CN111189474A (zh) 基于mems的marg传感器的自主校准方法
CN112055804A (zh) 信息处理装置、信息处理方法和程序
CN108444468B (zh) 一种融合下视视觉与惯导信息的定向罗盘
CN109916399B (zh) 一种阴影下的载体姿态估计方法
CN111121820B (zh) 基于卡尔曼滤波的mems惯性传感器阵列融合方法
CN103557866A (zh) 一种基于地磁技术的虚拟陀螺仪及算法
CN109506674B (zh) 一种加速度的校正方法及装置
Zhe et al. Adaptive complementary filtering algorithm for imu based on mems

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
GR01 Patent grant
GR01 Patent grant