CN117570976B - 基于逆向推算的超低速载体惯性定向方法 - Google Patents
基于逆向推算的超低速载体惯性定向方法 Download PDFInfo
- Publication number
- CN117570976B CN117570976B CN202410061054.2A CN202410061054A CN117570976B CN 117570976 B CN117570976 B CN 117570976B CN 202410061054 A CN202410061054 A CN 202410061054A CN 117570976 B CN117570976 B CN 117570976B
- Authority
- CN
- China
- Prior art keywords
- time
- matrix
- angle
- inertial
- navigation
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 59
- 238000001914 filtration Methods 0.000 claims abstract description 13
- 238000012937 correction Methods 0.000 claims abstract description 11
- 238000009499 grossing Methods 0.000 claims abstract description 7
- 238000007711 solidification Methods 0.000 claims abstract description 6
- 230000008023 solidification Effects 0.000 claims abstract description 6
- 239000011159 matrix material Substances 0.000 claims description 82
- 230000008569 process Effects 0.000 claims description 22
- 238000004364 calculation method Methods 0.000 claims description 19
- 230000008859 change Effects 0.000 claims description 18
- 238000005259 measurement Methods 0.000 claims description 17
- 239000013598 vector Substances 0.000 claims description 11
- 230000004069 differentiation Effects 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 6
- 238000006243 chemical reaction Methods 0.000 claims description 5
- 238000007781 pre-processing Methods 0.000 abstract description 3
- 238000010276 construction Methods 0.000 abstract description 2
- 230000001133 acceleration Effects 0.000 description 4
- 238000006467 substitution reaction Methods 0.000 description 3
- 239000003245 coal Substances 0.000 description 2
- 238000013500 data storage Methods 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 238000001228 spectrum Methods 0.000 description 2
- 238000012546 transfer Methods 0.000 description 2
- 230000004075 alteration Effects 0.000 description 1
- 238000009412 basement excavation Methods 0.000 description 1
- 238000005452 bending Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000005065 mining Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 230000009897 systematic effect Effects 0.000 description 1
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/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/183—Compensation of inertial measurements, e.g. for temperature effects
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T90/00—Enabling technologies or technologies with a potential or indirect contribution to GHG emissions mitigation
Abstract
本发明实施例公开了一种基于逆向推算的超低速载体惯性定向方法，包括：步骤1：采集惯性传感器数据，对惯性传感器数据进行预处理；步骤2：对预处理后的惯性传感器数据进行凝固惯性系粗对准，同时进行逆向导航解算，进行固定位置组合导航并通过反馈校正；步骤3：使用零速修正和输出平滑的导航方法进行导航解算，解算完成后输出角度。本发明可以有效的消除加速度计内杆臂带来的速度误差，可通过FIR滤波器可以隔离工程机械施工时内部电机抖动产生的有害振动，减小外界干扰对输出角度的影响；同时可以节约初始对准时间，本发明可以在极低速运动条件下，通过卡尔曼滤波组合导航的方式对角度值进行最优化估计，使系统角度误差最小。
Description
技术领域
本发明涉及超低速运动定向技术领域，尤其涉及一种基于逆向推算的超低速载体惯性定向方法。
背景技术
目前，现有技术中针对在露天煤矿的连续挖掘机，井下煤矿的连续采煤机和滚筒采煤机、地下挖掘的掘进机等大型机械设备的方向确定的方案较少。现有技术在超低速运动（平均运动速度不大于0.01m/s）载体的定向精度较低。
发明内容
本发明实施例所要解决的技术问题在于，提供一种基于逆向推算的超低速载体惯性定向方法，以提升超低速运动载体的定向精度。
为了解决上述技术问题，本发明实施例提出了一种基于逆向推算的超低速载体惯性定向方法，包括：
步骤1：采集惯性传感器数据，所述惯性传感器包括陀螺和加速度计，所述惯性传感器数据包括通过陀螺采集的角运动数据和通过加速度计采集的线运动数据，对惯性传感器数据进行预处理，先滤除振动干扰，再补偿加速度计内杆臂带来的测量误差，得到处理后的惯性传感器数据；
步骤2：对处理后的惯性传感器数据进行凝固惯性系粗对准，得到T1时刻的角度值，同时存储粗对准后的惯性传感器数据，根据存储的惯性传感器数据进行逆向导航解算，得到对准初始时刻T0的角度值，再进行固定位置组合导航并通过反馈校正；得到对准结束时刻T2的三维角度值，完成整个初始对准过程；
步骤3：使用零速修正和输出平滑的导航方法进行导航解算，解算完成后输出实时角度值。
进一步地，步骤1中，采用FIR低通数字滤波器滤除振动干扰，并根据下式对加速度计测量的比力进行简化的内杆臂误差补偿：
式中是补偿后的加速度计比力输出，/>是加速度计的原始比力输出，/>为惯性传感器的三轴角速度向量，/>为内杆臂向量。
进一步地，步骤2中，根据矩阵链式乘法，惯性系解析粗自对准算法将时变的初始姿态矩阵的求解拆分为/>、/>和/>三个方向余弦阵的计算，
；
式中，参考位置矩阵只与载体所在纬度和对准时长有关，其解析表达式为：
；
式中：是指地球自转角速率，/>为对准时间，符号n代表导航坐标系，符号b代表载体坐标系，符号n0代表初始对准初始时刻导航坐标系，符号b0代表初始对准初始时刻载体坐标系，/>表示t时刻姿态矩阵，/>表示t0时刻姿态矩阵，/>表示t-1到t时刻的角位置变化矩阵；
角位置变化矩阵通过陀螺输出直接在惯性系/>下实时姿态更新获得，具体的微分方程为：/>；
其中，表示角位置变化矩阵的微分，/>表示角位置变化矩阵，/>表示角度变化值；
其中，，且/>由系的定义可知，初始/>时刻，/>，且，通过解微分方程的形式就可以计算出/>；
其次，定义为以下积分：
；
定义为以下积分：
；
式中表示陀螺感知的载体的角运动，/>表示加速度计的测量输出，L为当地纬度，/>是指地球自转角速率，t表示实时的对准时间；
分别选取对准过程中两个中间时刻,/>,/>，并分别计算对应得到的参考矢量/>、/>和/>、/>，有如下投影关系成立：
；
通过矩阵基本运算，简化上述方程可得：
；
其中，符号、符号/>表示为：
；
通过矩阵迭代计算出，再通过矩阵连乘公式就得到/>；
经过以上计算得到T1时刻的姿态转换矩阵，通过姿态矩阵到姿态角的转换关系，进而得到T1时刻角度值。
进一步地，步骤2中，以得到的为初值，从存储数据的数组中从后向前依次取出惯性器件数据，再根据以下公式进行姿态角逆向导航跟踪，得到初始时刻的姿态矩阵：
；
式中，表示k-1时刻的方向余弦矩阵，/>表示k时刻的方向余弦矩阵，/>表示为采样间隔时间，/>代表是从K时刻到k-1时刻转过的平均角度，/>代表是从K时刻到k-1时刻转动的平均角速度，/>代表是k-1时刻陀螺的角增量输出，表示在当前纬度条件下的地球自转各分量，L0表示当前的纬度。
进一步地，步骤3中，以得到的为角度初始值，建立离散线性卡尔曼滤波基本方程、惯性导航系统的误差传播方程，再建立以速度量测为零的量测方程，通过估计的姿态失准角实时反馈校正得到导航过程中实时的/>。
进一步地，步骤3中，再通过滑窗的方法，对经过卡尔曼滤波输出校正后的姿态角进行滑窗平滑，得到最终的输出角度值，输出结果与滑窗的基本关系如下：
；
式中，为输出姿态角，包括俯仰角、横滚角、航向角，n为滑窗的深度，/>为当前滑窗内的姿态角度值。
本发明的有益效果为：本发明根据特定工程机械的超低速运动特点，结合目前常规的导航计算方法，进行最优化的总结和应用；本发明通过惯性传感器的数据预处理可以有效的消除加速度计内杆臂带来的速度测量误差，同时通过FIR滤波器可以隔离工程机械施工时内部电机抖动产生的有害振动，减小外界干扰对输出角度的影响；同时可以节约初始对准时间，本发明可以将初始对准时间缩短为3分钟；本发明可以在极低速运动条件下，通过卡尔曼滤波组合导航的方式对角度值进行最优化估计，使系统角度误差最小。
附图说明
图1是本发明实施例的载体的干扰信号功率谱示意图。
图2是本发明实施例的基于逆向推算的超低速载体惯性定向方法的流程示意图。
具体实施方式
需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互结合，下面结合附图和具体实施例对本发明作进一步详细说明。
本发明实施例中若有方向性指示（诸如上、下、左、右、前、后……）仅用于解释在某一特定姿态（如附图所示）下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。
另外，在本发明中若涉及“第一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。
请参照图2，本发明实施例的基于逆向推算的超低速载体惯性定向方法包括步骤1～步骤3。
步骤1：采集惯性传感器数据，所述惯性传感器包括陀螺和加速度计，所述惯性传感器数据包括通过陀螺采集的角运动数据和通过加速度计采集的线运动数据，对惯性传感器数据进行预处理，隔离振动干扰并补偿加速度计内杆臂带来的速度误差。
本发明实施例根据载体在移动条件下的干扰振动频率进行FIR低通数字滤波，FIR数字滤波的3dB通带截止频率一般低于干扰振动频率的0.5倍，一般的地面低速运动的载体的通带截止频率设置在20Hz~40Hz范围，阻带截止频率一般设定为干扰振动频率的1倍，处理的数据是高速采样的惯性器件原始数据，采样频率不低于200Hz。
根据某种采掘设备实际工况，首先使用2000Hz的采样频率，采集加速度计数据，分析出惯性传感器感知到载体的干扰信号功率谱如图1所示。从图1中看出干扰频率集中在100Hz~120Hz之间，所以设计以下参数的FIR低通滤波器，其中采样频率为200Hz，通带截止频率为30Hz，通带为3dB，阻带截止频率为100Hz，阻带为20dB。
根据捷联惯性导航系统（简称INS）的角速度和加速度计的内杆臂，对加速度计测量的比力进行简化的内杆臂误差补偿，补偿方法如下：
式中是补偿后的加速度计比力输出，/>是加速度计的原始比力输出，/>为惯性传感器的三轴角速度向量，/>为内杆臂向量，该值一般是结构设计完成后直接测量得到。
经过步骤1就得到了用于角度计算的惯性传感器信息，包括角速率信息和线加速度信息/>。
步骤2：对预处理后的惯性传感器数据进行凝固惯性系粗对准，同时存储粗对准过程中的惯性传感器数据，根据存储的惯性传感器数据进行逆向导航解算，得到对准初始时刻的三维角度值，根据得到的初始对准初始时刻姿态角进行固定位置组合导航并通过反馈校正。
步骤2包括：
（1）凝固惯性系粗对准
在进行凝固惯性系粗对准时同时存储惯性器件的数据，计算得到。
根据矩阵链式乘法，惯性系解析粗自对准算法将时变的初始姿态矩阵的求解拆分为/>、/>和/>三个方向余弦阵的计算，
式中，参考位置矩阵只与载体所在纬度/>和对准时长/>有关，其解析表达式为：
；
公式中：是指地球自转角速率，/>为对准时间，符号n代表导航坐标系，本发明是地球地面上导航，取地理坐标系为导航坐标系，符号b代表载体坐标系，本发明取本体右前上坐标系为载体坐标系。符号n0代表初始对准初始时刻导航坐标系。符号b0代表初始对准初始时刻载体坐标系，/>表示t0时刻姿态矩阵，/>表示t-1到t时刻的角位置变化矩阵。
角位置变化矩阵描述了载体由于基座晃动和地球自转共同引起的系统相对于对准开始时刻的角位置变化，该角运动可通过陀螺仪跟踪，并通过陀螺输出直接在惯性系/>下实时姿态更新获得，具体的微分方程为：/>；
其中，表示角位置变化矩阵的微分，/>表示角位置变化矩阵，/>表示角度变化值。
其中，，且由/>系的定义可知，初始/>时刻，/>，且，通过解微分方程的形式就可以计算出/>。
常值矩阵描述了两个特定惯性坐标系之间的角位置关系，其求解是惯性系解析粗自对准的关键，下文对其计算方法进行展开分析。
在理想静基座条件下，时刻/>系下加速度计比力输出/>与重力矢量/>之间关系为：
；
式中，在b系下的比力输出，/>表示在n系下的重力加速度输出，/>表示姿态矩阵。
两边同时映射至系，得
；
式中，表示角位置变化矩阵，/>在b系下的比力输出，/>表示在n系下的重力加速度输出，/>参考位置矩阵的转置矩阵，/>表示0时刻的姿态矩阵。
对上式两边分别积分，可得
若以符号替换/>，且用/>替换/> ，即
得所描述的积分矢量解析解为
；
式中，只是一个记号，/>只是另外一个记号，/>表示角位置变化矩阵，/>在b系下的比力输出，g 为当地重力加速度，t为初始对准时间，L为当地纬度，/>是指地球自转角速率。
分别选取对准过程中两个中间时刻,/>,/>，并分别计算对应得到的参考矢量/>、/>和/>、/>，有如下投影关系成立：
简化可得：
;
其中，符号，符号/>表示为：
。
以上公式中只有是未知量通过矩阵迭代就可以准确计算出/>，再通过矩阵连乘公式就得到/>。
经过以上步骤的迭代就得到了T1时刻的姿态矩阵，通过姿态矩阵到姿态角的转换关系，进而得到T1时刻角度值。
（2）数据存储逆向导航
以T1时刻的姿态矩阵为初值，以前一阶段存储的惯性器数据进行逆向导航解算。首先从存储数据的数组中从后向前依次取出惯性器件数据，再根据以下公式（逆向导航解算公式）进行姿态角逆向导航跟踪：
;
式中表示k-1时刻的方向余弦矩阵，/>表示k时刻的方向余弦矩阵，/>表示为采样间隔时间，/>代表是从K时刻到k-1时刻转过的平均角度，/>代表是从K时刻到k-1时刻转动的平均角速度，/>代表是k-1时刻陀螺的角增量输出，表示在当前纬度条件下的地球自转各分量。
经过以上步骤的迭代计算就得到了T0时刻的姿态矩阵。
（3）数据存储固定位置组合导航
根据前一阶段得到的问姿态矩阵初值进行固定位置组合导航并通过失准角反馈校正角度值。
首先说明线性离散时间系统基本方程
设时刻的被估计状态/>受系统噪声序列/>驱动，驱动机理由下述状态方程描述：
对的量测满足线性关系，量测方程为：
上两式中，为/>时刻至/>时刻的一步转移矩阵；/>为系统噪声驱动阵；为量测阵；/>为量测噪声序列；/>为系统激励噪声序列。同时，/>和/>满足：
式中，为系统噪声序列的方差阵，假设为非负定阵；/>为量测噪声序列的方差阵，假设为正定阵；/>为/>函数。
再次说明离散卡尔曼滤波基本方程
；
其中， 为k时刻状态预测，/> 为k-1时刻的状态估计，/> 为k时刻的状态估计， /> 为k时刻均方误差阵的预测，/> 为k时刻均方误差阵的估计，/> 为k-1时刻均方误差阵的估计，/>为/>时刻至/>时刻的一步转移矩阵；/>为k-1时刻系统噪声序列的方差阵, />为k-1时刻系统噪声驱动阵；/>为k时刻系统量测阵，/>为k时刻量测噪声序列的方差阵，I为单位矩阵，/> 为滤波增益。
然后说明惯性导航系统的误差方程。
(1) 姿态误差方程
式中，为导航计算机实际建立的导航坐标系/>相对于理想导航坐标系系/>的偏差角，称之为姿态失准角，/>为姿态失准角的微分，/>为速度误差，/>为位置误差，/>为姿态矩阵，/>为b系下的陀螺等效零偏，/>为n系向对于i系旋转值。
(2)速度误差方程
；
式中，为速度误差的微分，/>为速度误差，/>为速度，/>为n系下的比力输出，/>为姿态失准角，/>地球自转角速率，/>为惯性系在地球表面移动因为地球表面弯曲引起的n系旋转，/>为位置误差，/>为姿态矩阵，/>为b系下的陀螺等效零偏。
(3)位置误差方程
；
式中为位置误差的微分，/>为位置误差，/>为速度误差。
式中
；
；
。
以上式中，是指地球自转角速率，/>是指东向速度，/>是指北向速度，/>是指天向速度，/>是指地球子午圈半径，/>是指地球卯酉圈半径，L是指当地纬度，h是指高度。
最后说明系统方程的建立
取系统的状态量为：；
式中，为失准角误差，/>为速度误差，/>为位置误差，/>为等效陀螺零偏误差，/>为等效加速度计零偏误差。
系统的量测量为：，系统的量测矩阵为：
式中，为惯性计算的位置，/>为固定位置值。
其中，量测方程的量测误差标准差设计为：Rk=0.05米。
经过卡尔曼滤波器迭代之后，估计的失准角误差为，再通过矩阵叉乘的形式将失准角误差迭代到姿态角中，完成姿态角的最小方差估计。最终得到对准结束时刻T2时刻的姿态矩阵/>。
步骤3：使用零速修正和输出平滑的导航方法进行导航解算，解算完成后输出角度。
步骤3包括：
（1）零速修正
根据以T2时刻的姿态矩阵初值，使用当前时刻采集得到的惯性传感器数据，进行零速修正组合导航，得到实时的tk时刻的姿态矩阵/>。
线性离散时间系统基本方程、卡尔曼滤波基本方程、惯性导航系统的误差方程同上。
取系统的状态量为：；
式中，为失准角误差，/>为速度误差，/>为位置误差，/>为等效陀螺零偏误差，/>为等效加速度计零偏误差。
系统的量测量为：，系统的量测矩阵为：/>
其中，卡尔曼滤波的量测标准差矩阵的取值最为关键，本发明优选使用与载体实际平均运动速度的2倍值。
经过卡尔曼滤波迭代之后，实时计算出tk时刻的姿态矩阵。
（2）输出平滑
本发明实施例通过滑窗的方法，对输出的姿态角结果进行滑窗平滑，滑窗深度n为本发明所适用的产品输出的频率，一般应不小于10，，输出结果与滑窗的基本关系如下：
；
式中，为输出姿态角，包括俯仰角、横滚角、航向角。n为滑窗的深度，/>为当前滑窗内的姿态角度值。
经过滑窗平滑后输出载体导航状态下实时的姿态角，即完成定向测量。
尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同范围限定。
Claims (4)
1.一种基于逆向推算的超低速载体惯性定向方法，其特征在于，包括：
步骤1：采集惯性传感器数据，所述惯性传感器包括陀螺和加速度计，所述惯性传感器数据包括通过陀螺采集的角运动数据和通过加速度计采集的线运动数据，对惯性传感器数据进行预处理，先滤除振动干扰，再补偿加速度计内杆臂带来的测量误差，得到处理后的惯性传感器数据；
步骤2：对处理后的惯性传感器数据进行凝固惯性系粗对准，得到T1时刻的角度值，同时存储粗对准后的惯性传感器数据，根据存储的惯性传感器数据进行逆向导航解算，得到对准初始时刻T0的角度值，再进行固定位置组合导航并通过反馈校正；得到对准结束时刻T2的三维角度值，完成整个初始对准过程；
步骤3：使用零速修正和输出平滑的导航方法进行导航解算，解算完成后输出实时角度值；
步骤1中，采用FIR低通数字滤波器滤除振动干扰，并根据下式对加速度计测量的比力进行简化的内杆臂误差补偿：
；
式中是补偿后的加速度计比力输出，/>是加速度计的原始比力输出，/>为惯性传感器的三轴角速度向量，/>为内杆臂向量；
步骤2中，根据矩阵链式乘法，惯性系解析粗自对准算法将时变的初始姿态矩阵的求解拆分为/>、/>和/>三个方向余弦阵的计算，
；
式中，参考位置矩阵只与载体所在纬度和对准时长有关，其解析表达式为：
；
式中：是指地球自转角速率，/>为对准时间，符号n代表导航坐标系，符号b代表载体坐标系，符号n0代表初始对准初始时刻导航坐标系，符号b0代表初始对准初始时刻载体坐标系，/>表示t时刻姿态矩阵，/>表示t0时刻姿态矩阵，/>表示t-1到t时刻的角位置变化矩阵；
角位置变化矩阵通过陀螺输出直接在惯性系/>下实时姿态更新获得，具体的微分方程为：/>；
其中，表示角位置变化矩阵的微分，/>表示角位置变化矩阵，/>表示角度变化值；
其中，，且/>由系的定义可知，初始/>时刻，/>，且，通过解微分方程的形式就可以计算出/>；
其次，定义为以下积分：
；
定义为以下积分：
；
式中表示陀螺感知的载体的角运动，/>表示加速度计的测量输出，L为当地纬度，/>是指地球自转角速率，t表示实时的对准时间；
分别选取对准过程中两个中间时刻,/>,/>，并分别计算对应得到的参考矢量/>、/>和/>、/>，有如下投影关系成立：
；
通过矩阵基本运算，简化上述方程可得：
；
其中，符号、符号/>表示为：
；
通过矩阵迭代计算出，再通过矩阵连乘公式就得到/>；
经过以上计算得到T1时刻的姿态转换矩阵，通过姿态矩阵到姿态角的转换关系，进而得到T1时刻角度值。
2.如权利要求1所述的基于逆向推算的超低速载体惯性定向方法，其特征在于，步骤2中，以得到的为初值，从存储数据的数组中从后向前依次取出惯性器件数据，再根据以下公式进行姿态角逆向导航跟踪，得到初始时刻的姿态矩阵/>：
；
式中，表示k-1时刻的方向余弦矩阵，/>表示k时刻的方向余弦矩阵，/>表示为采样间隔时间，/>代表是从K时刻到k-1时刻转过的平均角度，/>代表是从K时刻到k-1时刻转动的平均角速度，/>代表是k-1时刻陀螺的角增量输出，/>表示在当前纬度条件下的地球自转各分量，L0表示当前的纬度。
3.如权利要求2所述的基于逆向推算的超低速载体惯性定向方法，其特征在于，步骤3中，以得到的为角度初始值，建立离散线性卡尔曼滤波基本方程、惯性导航系统的误差传播方程，再建立以速度量测为零的量测方程，通过估计的姿态失准角实时反馈校正得到导航过程中实时的/>。
4.如权利要求3所述的基于逆向推算的超低速载体惯性定向方法，其特征在于，步骤3中，再通过滑窗的方法，对经过卡尔曼滤波输出校正后的姿态角进行滑窗平滑，得到最终的输出角度值，输出结果与滑窗的基本关系如下：
；
式中，为输出姿态角，包括俯仰角、横滚角、航向角，n为滑窗的深度，/>为当前滑窗内的姿态角度值。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410061054.2A CN117570976B (zh) | 2024-01-16 | 2024-01-16 | 基于逆向推算的超低速载体惯性定向方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410061054.2A CN117570976B (zh) | 2024-01-16 | 2024-01-16 | 基于逆向推算的超低速载体惯性定向方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117570976A CN117570976A (zh) | 2024-02-20 |
| CN117570976B true CN117570976B (zh) | 2024-05-03 |
Family
ID=89864805
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410061054.2A Active CN117570976B (zh) | 2024-01-16 | 2024-01-16 | 基于逆向推算的超低速载体惯性定向方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117570976B (zh) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106052715A (zh) * | 2016-05-23 | 2016-10-26 | 西北工业大学 | 单轴旋转捷联惯导系统回溯式自对准方法 |
| CN106482749A (zh) * | 2016-12-07 | 2017-03-08 | 西北工业大学 | 基于逆向导航算法的捷联惯导与转速计组合对准方法 |
| CN112697141A (zh) * | 2020-12-16 | 2021-04-23 | 北京航空航天大学 | 基于逆向导航的惯导/里程计动基座姿态与位置对准方法 |
| CN115265588A (zh) * | 2022-07-15 | 2022-11-01 | 北京航空航天大学 | 陆用捷联惯导基于逆向导航的零速修正在线平滑方法 |
| CN116499493A (zh) * | 2023-03-15 | 2023-07-28 | 中国船舶集团有限公司第七〇七研究所 | 一种基于低频逆向滤波的惯导快速对准方法 |
-
2024
- 2024-01-16 CN CN202410061054.2A patent/CN117570976B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106052715A (zh) * | 2016-05-23 | 2016-10-26 | 西北工业大学 | 单轴旋转捷联惯导系统回溯式自对准方法 |
| CN106482749A (zh) * | 2016-12-07 | 2017-03-08 | 西北工业大学 | 基于逆向导航算法的捷联惯导与转速计组合对准方法 |
| CN112697141A (zh) * | 2020-12-16 | 2021-04-23 | 北京航空航天大学 | 基于逆向导航的惯导/里程计动基座姿态与位置对准方法 |
| CN115265588A (zh) * | 2022-07-15 | 2022-11-01 | 北京航空航天大学 | 陆用捷联惯导基于逆向导航的零速修正在线平滑方法 |
| CN116499493A (zh) * | 2023-03-15 | 2023-07-28 | 中国船舶集团有限公司第七〇七研究所 | 一种基于低频逆向滤波的惯导快速对准方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117570976A (zh) | 2024-02-20 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108731670A (zh) | 基于量测模型优化的惯性/视觉里程计组合导航定位方法 | |
| CN110886606B (zh) | 一种随钻特征量辅助的惯性测斜方法及装置 | |
| CN110792430B (zh) | 一种基于多传感器数据融合的随钻测斜方法及装置 | |
| CN109596144B (zh) | Gnss位置辅助sins行进间初始对准方法 | |
| US20210348924A1 (en) | Attitude measurement method | |
| CN109141475B (zh) | 一种dvl辅助sins鲁棒行进间初始对准方法 | |
| CN110231029B (zh) | 一种水下机器人多传感器融合数据处理方法 | |
| CN107270893A (zh) | 面向不动产测量的杆臂、时间不同步误差估计与补偿方法 | |
| CN109540135B (zh) | 水田拖拉机位姿检测和偏航角提取的方法及装置 | |
| CN110595503B (zh) | 基于李群最优估计的sins捷联惯性导航系统晃动基座自对准方法 | |
| CN111102993A (zh) | 一种旋转调制型捷联惯导系统晃动基座初始对准方法 | |
| CN103364817B (zh) | 一种基于r-t-s平滑的pos系统双捷联解算后处理方法 | |
| CN113175933A (zh) | 一种基于高精度惯性预积分的因子图组合导航方法 | |
| CN110672095A (zh) | 一种基于微惯导的行人室内自主定位算法 | |
| CN112212862A (zh) | 一种改进粒子滤波的gps/ins组合导航方法 | |
| CN113566850B (zh) | 惯性测量单元的安装角度标定方法、装置和计算机设备 | |
| CN111722295A (zh) | 一种水下捷联式重力测量数据处理方法 | |
| CN110926499A (zh) | 基于李群最优估计的sins捷联惯性导航系统晃动基座自对准方法 | |
| CN113008229A (zh) | 一种基于低成本车载传感器的分布式自主组合导航方法 | |
| CN117570976B (zh) | 基于逆向推算的超低速载体惯性定向方法 | |
| CN112729283A (zh) | 一种基于深度相机/mems惯导/里程计组合的导航方法 | |
| CN109945857B (zh) | 一种面向不动产实地测量的车载惯性定位方法及其装置 | |
| CN111141283A (zh) | 一种通过地磁数据判断行进方向的方法 | |
| CN113551669B (zh) | 基于短基线的组合导航定位方法及装置 | |
| CN113834502A (zh) | 捷联惯导初始对准方法及纬度计算方法 |
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 |