CN112197789B - 一种基于quest的ins/dvl安装误差标定方法 - Google Patents
一种基于quest的ins/dvl安装误差标定方法 Download PDFInfo
- Publication number
- CN112197789B CN112197789B CN202010817669.5A CN202010817669A CN112197789B CN 112197789 B CN112197789 B CN 112197789B CN 202010817669 A CN202010817669 A CN 202010817669A CN 112197789 B CN112197789 B CN 112197789B
- Authority
- CN
- China
- Prior art keywords
- dvl
- ins
- installation error
- speed
- quest
- 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
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
-
- 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
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Navigation (AREA)
Abstract
本发明公开一种基于QUEST的INS/DVL安装误差标定方法,针对INS/DVL安装误差标定时的大角度非线性问题及安装误差与DVL标度因数误差耦合的问题,首先进行INS体坐标系下的INS、DVL速度矢量预处理,然后基于QUEST的最优四元数求解,最后进行INS与DVL之间安装误差角计算。本发明方法不受DVL标度因数误差及安装误差角度大小影响,可以对惯导***与DVL间的安装误差进行在线标定。可以适用于各种角度情况下的DVL安装误差标定。
Description
技术领域
本发明属于误差的标定技术领域,具体涉及惯性导航***与DVL之间大角度安装误差的标定。
背景技术
惯导***与DVL安装误差的标定结果是影响SINS/DVL组合导航精度的关键因素,其直接影响SINS/DVL组合导航的定位精度。传统的最小二乘法标定及卡尔曼滤波标定,一是只能对小角度的安装误差进行标定,难以对大角度安装误差进行高精度标定;二是安装误差和DVL标度因数误差耦合在一起,误差难以分离,受载体运动环境影响较大。本文提出了一种SINS/DVL安装误差标定方法,不受DVL标度因数误差及安装误差角度大小影响,可以对惯导***与DVL间的安装误差进行在线标定。
发明内容
本发明提供了一种基于QUEST的INS/DVL安装误差标定方法,不受DVL标度因数误差及安装误差角度大小影响,可以对惯导***与DVL间的安装误差进行在线标定。
本发明一种基于QUEST的INS/DVL安装误差标定方法,包括如下步骤:
步骤一,INS体坐标系下的INS、DVL速度矢量预处理;
步骤二,基于QUEST的最优四元数求解;
步骤三,INS与DVL之间安装误差角计算。
进一步地,
所述步骤一中INS体坐标系下的INS、DVL速度矢量预处理包括:
1)根据INS的角速度和DVL相对于INS的杆臂,对DVL速度矢量进行杆臂误差的补偿,补偿方式如下:
式中:为DVL输出的原始速度矢量,/>为INS的角速度,/>为DVL相对于INS的杆臂;
2)计算INS和DVL的合速度,计算方法如下:
式中,为INS的体坐标系速度矢量,Vins为INS的合速度,Vdvl为DVL的合速度;
3)对INS和DVL的速度进行判断,当且仅当INS合速度大于1m/s,DVL合速度大于1m/s,INS航向角速度小于1°/s,INS合速度和DVL合速度之间差值小于1m/s时,进行DVL安装误差角估计的后续步骤计算;
4)对DVL速度矢量和INS与GNSS组合导航得到的体坐标下的速度进行归一化处理,计算方法如下:
式中,为INS体坐标系速度,/>为归一化后的DVL速度矢量,/>为归一化后的INS体坐标系速度矢量。
进一步地,
所述步骤二中基于QUEST的最优四元数求解包括:
1)通过以下公式,计算各中间变量
σ=trB (8)
S=B+BT (9)
其中,n表示n组速度矢量测量值;
2)根据式(6)至(9),计算如下损失函数构造矩阵
3)λmax为矩阵K的最大特征值,使用1作为初值,进行迭代计算求取λmax;将λmax,0=1带入式(11)进行迭代,得:
λmax,i+1=λmax,i-p(λi)/p′(λi),i=0,1,... (11)
p(λi)计算公式如下:
p(λ)=λ4-(a+b)λ2-cλ+(ab+cσ-d) (12)
其中,a=σ2-κ,κ为S伴随矩阵的迹;
4)根据计算出来的特征值λmax,计算对应的K的特征向量即为对应的最优四元数。
所述步骤三中INS与DVL之间安装误差角计算包括:
1)得到最优估计的四元数后,将该四元数转化为安装误差矩阵
其中,qi为四元数中的元素;
2)再根据安装误差矩阵即可计算得到DVL安装误差角
其中,Cij为中第i行,第j列元素。
本发明针对INS/DVL安装误差标定时的大角度非线性问题及安装误差与DVL标度因数误差耦合的问题,提出了一种利用惯导***体坐标系速度矢量和DVL速度矢量进行安装误差估计的安装误差标定方法,可以适用于各种角度情况下的DVL安装误差标定。
附图说明
图1DVL安装误差角估计结果
图2DVL安装误差角估计结果
具体实施方式
下面结合附图对本发明作进一步详细说明。
标定DVL安装误差时,惯导***处于INS/GNSS组合导航工作模式,DVL速度不参与组合导航。在INS/GNSS组合导航过程中,当惯导***接收到DVL速度时,在该导航周期进行DVL安装误差估计的迭代计算。各周期标定算法实现过程如下:
1)根据惯导***的角速度和DVL相对于惯导***的杆臂,对DVL速度矢量进行杆臂误差的补偿。补偿方式如下:
式中:为DVL输出的原始速度矢量,/>为惯导***的角速度,/>为DVL相对于惯导***的杆臂。
2)计算惯导***和DVL的和速度,计算方法如下:
式中,为惯导***的体坐标系速度矢量,Vins为惯导***的合速度,Vdvl为DVL的合速度。
3)对惯导***和DVL的速度进行判断,当且仅当惯导***合速度大于1m/s,DVL合速度大于1m/s,惯导***航向角速度小于1°/s,惯导***合速度和DVL合速度之间差值小于1m/s时,进行DVL安装误差角估计的后续步骤计算。
4)对DVL速度矢量和惯导***与GNSS组合导航得到的体坐标下的速度进行归一化处理。计算方法如下:
式中,为惯导***体坐标系速度,/>为归一化后的DVL速度矢量,/>为归一化后的惯导***体坐标系速度矢量。
5)取αi为权重系数,且共n个系数,对应n组速度矢量测量值
6)通过以下公式,计算各中间变量。
S=B+BT (9)
7)根据式(6)至(9),计算如下损失函数构造矩阵
8)计算K的特征值λi和特征向量qi,其中i=1,2,3,4。
9)K的所有特征值中最大的一个λmax。λmax理论值为1,因此可使用1作为初值,进行迭代计算求取λmax。将λmax,0=1带入式(11)进行迭代,得:
λmax,i+1=λmax,i-p(λi)/p′(λi),i=0,1,... (11)
其中
10)为保证迭代收敛性,令:
[(λ+σ)I-S]-1=γ-1(αI+βS+S2) (13)
可得α=λ2-σ2+κ,β=λ-σ,γ=α(λ+σ)-Δ。
将α、β和γ代入式(12)可得:
p(λ)=λ4-(a+b)λ2-cλ+(ab+cσ-d)=0 (14)
其中a=σ2-κ,
由此可得:
由此可得两速度矢量间安装误差最优解为:
其中:
11)安装误差角余弦矩阵A与四元数字的转换关系为:
其中:
为实现姿态对最优估计,定义如下损失函数:
则有:
g(A)=1-L(A)=tr[ABT] (20)
为实现损失函数最小,只需满足g(A)最大即可。
将(17)带入(20)可得:
四元数的元素具有唯一约束
为求取式(21)在式(22)约束条件下对最大值,重新构建方程:
令可得
由上式分析可知,λ是K的一个特征根,则是对应的特征向量,因此(16)的结果是一种最优估计。
12)得到最优估计的四元数后,将该四元数转化为安装误差矩阵,再根据安装误差矩阵即可计算得到DVL安装误差角。
采用某次海试试验数据以此方法进行SINS/DVL标定,标定过程中安装误差角收敛过程如图1所示:
由于直线航行过程中只有前向速度,横滚角可观测度较低,需要进行转弯机动产生侧向速度,从而使估计结果收敛。图中3600s处存在转弯机动,该机动完成后,安装误差角估计值迅速收敛,得到稳定的估计结果。
在此数据基础上,将DVL原始速度加入45°航向安装误差角(DVL换能器斜45°装),再次进行标定,两次结果比较如图2所示:
两次安装误差角估计结果统计如下:
从估计结果可以看出,此标定方法可以对DVL安装误差角进行有效标定。
上述具体实施方式仅限于解释和说明本发明的技术方案,但并不能构成对权利要求保护范围的限定。本领域技术人员应当清楚,在本发明的技术方案的基础上做任何简单的变形或替换而得到的新的技术方案,均落入本发明的保护范围内。
Claims (3)
1.一种基于QUEST的INS/DVL安装误差标定方法,其特征在于,包括如下步骤:
步骤一,INS体坐标系下的INS、DVL速度矢量预处理;
步骤二,基于QUEST的最优四元数求解;
步骤三,INS与DVL之间安装误差角计算;
所述步骤一中INS体坐标系下的INS、DVL速度矢量预处理包括:
1)根据INS的角速度和DVL相对于INS的杆臂,对DVL速度矢量进行杆臂误差的补偿,补偿方式如下:
式中:为DVL输出的原始速度矢量,/>为INS的角速度,/>为DVL相对于INS的杆臂;
2)计算INS和DVL的合速度,计算方法如下:
式中,为INS的体坐标系速度矢量,Vins为INS的合速度,Vdvl为DVL的合速度;
3)对INS和DVL的速度进行判断,当且仅当INS合速度大于1m/s,DVL合速度大于1m/s,INS航向角速度小于1°/s,INS合速度和DVL合速度之间差值小于1m/s时,进行DVL安装误差角估计的后续步骤计算;
4)对DVL速度矢量和INS与GNSS组合导航得到的体坐标下的速度进行归一化处理,计算方法如下:
式中,为INS体坐标系速度,/>为归一化后的DVL速度矢量,/>为归一化后的INS体坐标系速度矢量。
2.根据权利要求1所述的一种基于QUEST的INS/DVL安装误差标定方法,其特征在于,
所述步骤二中基于QUEST的最优四元数求解包括:
1)通过以下公式,计算各中间变量
σ=trB (8)
S=B+BT (9)
其中,n表示n组速度矢量测量值;
2)根据式(6)至(9),计算如下损失函数构造矩阵
3)λmax为矩阵K的最大特征值,使用1作为初值,进行迭代计算求取λmax;将λmax,0=1带入式(11)进行迭代,得:
λmax,i+1=λmax,i-p(λi)/p′(λi),i=0,1,... (11)
p(λi)计算公式如下:
p(λ)=λ4-(a+b)λ2-cλ+(ab+cσ-d) (12)
其中,a=σ2-κ,κ为S伴随矩阵的迹;
4)根据计算出来的特征值λmax,计算对应的K的特征向量即为对应的最优四元数。
3.根据权利要求1所述的一种基于QUEST的INS/DVL安装误差标定方法,其特征在于,
所述步骤三中INS与DVL之间安装误差角计算包括:
1)得到最优估计的四元数后,将该四元数转化为安装误差矩阵
其中,qi为四元数中的元素;
2)再根据安装误差矩阵即可计算得到DVL安装误差角
其中,Cij为中第i行,第j列元素。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010817669.5A CN112197789B (zh) | 2020-08-14 | 2020-08-14 | 一种基于quest的ins/dvl安装误差标定方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010817669.5A CN112197789B (zh) | 2020-08-14 | 2020-08-14 | 一种基于quest的ins/dvl安装误差标定方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112197789A CN112197789A (zh) | 2021-01-08 |
CN112197789B true CN112197789B (zh) | 2023-09-12 |
Family
ID=74006122
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010817669.5A Active CN112197789B (zh) | 2020-08-14 | 2020-08-14 | 一种基于quest的ins/dvl安装误差标定方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112197789B (zh) |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113703018B (zh) * | 2021-07-12 | 2022-08-05 | 山东大学 | 一种dvl内外误差标定方法 |
CN114252089B (zh) * | 2021-09-30 | 2024-05-31 | 河北汉光重工有限责任公司 | 一种dvl测速误差的组合标定方法 |
CN114543795B (zh) * | 2021-12-31 | 2024-01-02 | 文远苏行(江苏)科技有限公司 | 双天线航向角的安装误差估计方法和调整方法及相关设备 |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105509765A (zh) * | 2014-09-23 | 2016-04-20 | 北京自动化控制设备研究所 | 一种惯性/dvl/usbl安装误差标定方法 |
CN108592952A (zh) * | 2018-06-01 | 2018-09-28 | 北京航空航天大学 | 基于杆臂补偿与正反倍速率同时标定多mimu误差的方法 |
CN109724599A (zh) * | 2019-03-12 | 2019-05-07 | 哈尔滨工程大学 | 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法 |
CN110221333A (zh) * | 2019-04-11 | 2019-09-10 | 同济大学 | 一种车载ins/od组合导航***的量测误差补偿方法 |
CN110501024A (zh) * | 2019-04-11 | 2019-11-26 | 同济大学 | 一种车载ins/激光雷达组合导航***的量测误差补偿方法 |
CN110608756A (zh) * | 2019-09-27 | 2019-12-24 | 中国船舶重工集团公司第七0七研究所 | Sins/dvl组合导航***安装误差结构补偿方法 |
CN110646012A (zh) * | 2018-06-27 | 2020-01-03 | 北京自动化控制设备研究所 | 一种惯导***单位置初始对准最优方法 |
CN110986925A (zh) * | 2019-12-02 | 2020-04-10 | 北京自动化控制设备研究所 | 初始姿态最优估计方法 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US7979231B2 (en) * | 2008-11-13 | 2011-07-12 | Honeywell International Inc. | Method and system for estimation of inertial sensor errors in remote inertial measurement unit |
-
2020
- 2020-08-14 CN CN202010817669.5A patent/CN112197789B/zh active Active
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105509765A (zh) * | 2014-09-23 | 2016-04-20 | 北京自动化控制设备研究所 | 一种惯性/dvl/usbl安装误差标定方法 |
CN108592952A (zh) * | 2018-06-01 | 2018-09-28 | 北京航空航天大学 | 基于杆臂补偿与正反倍速率同时标定多mimu误差的方法 |
CN110646012A (zh) * | 2018-06-27 | 2020-01-03 | 北京自动化控制设备研究所 | 一种惯导***单位置初始对准最优方法 |
CN109724599A (zh) * | 2019-03-12 | 2019-05-07 | 哈尔滨工程大学 | 一种抗野值的鲁棒卡尔曼滤波sins/dvl组合导航方法 |
CN110221333A (zh) * | 2019-04-11 | 2019-09-10 | 同济大学 | 一种车载ins/od组合导航***的量测误差补偿方法 |
CN110501024A (zh) * | 2019-04-11 | 2019-11-26 | 同济大学 | 一种车载ins/激光雷达组合导航***的量测误差补偿方法 |
CN110608756A (zh) * | 2019-09-27 | 2019-12-24 | 中国船舶重工集团公司第七0七研究所 | Sins/dvl组合导航***安装误差结构补偿方法 |
CN110986925A (zh) * | 2019-12-02 | 2020-04-10 | 北京自动化控制设备研究所 | 初始姿态最优估计方法 |
Non-Patent Citations (1)
Title |
---|
张亚文 ; 莫明岗 ; 马小艳 ; 江薇 ; .一种基于集中滤波的SINS/DVL/USBL水下组合导航算法.导航定位与授时.2017,(第01期),25-31. * |
Also Published As
Publication number | Publication date |
---|---|
CN112197789A (zh) | 2021-01-08 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112197789B (zh) | 一种基于quest的ins/dvl安装误差标定方法 | |
CN108827310B (zh) | 一种船用星敏感器辅助陀螺仪在线标定方法 | |
CN112504298B (zh) | 一种gnss辅助的dvl误差标定方法 | |
CN111380518B (zh) | 一种引入径向速度的sins/usbl紧组合导航定位方法 | |
CN114076610B (zh) | Gnss/mems车载组合导航***的误差标定、导航方法及其装置 | |
CN113252041B (zh) | 一种适用于小型水下机器人的组合导航方法 | |
CN116358566B (zh) | 一种基于抗差自适应因子的粗差探测组合导航方法 | |
CN112798021A (zh) | 基于激光多普勒测速仪的惯导***行进间初始对准方法 | |
CN116147624B (zh) | 一种基于低成本mems航姿参考***的船舶运动姿态解算方法 | |
CN115307643A (zh) | 一种双应答器辅助的sins/usbl组合导航方法 | |
CN110632636B (zh) | 一种基于Elman神经网络的载体姿态估计方法 | |
CN115979253A (zh) | 一种基于抗差滤波的水下机器人多传感器紧组合导航方法 | |
CN110703205B (zh) | 基于自适应无迹卡尔曼滤波的超短基线定位方法 | |
CN111238485A (zh) | 一种基于导航敏感器拍摄火星图像的***误差自补偿方法 | |
CN113434806B (zh) | 一种抗差自适应多模型滤波方法 | |
CN113688562A (zh) | 一种基于ga算法的浅水多波束褶皱误差自动改正方法 | |
CN111707292B (zh) | 一种自适应滤波的快速传递对准方法 | |
CN110672127B (zh) | 阵列式mems磁传感器实时标定方法 | |
CN113503891B (zh) | 一种sinsdvl对准校正方法、***、介质及设备 | |
CN114111840B (zh) | 一种基于组合导航的dvl误差参数在线标定方法 | |
CN113447018B (zh) | 一种水下惯性导航***的姿态实时估计方法 | |
CN107228683B (zh) | 一种多星敏感器间慢变误差实时在轨修正方法 | |
CN113252029B (zh) | 一种基于光学陀螺量测信息的天文导航姿态传递方法 | |
CN115031729A (zh) | Sins/dvl/usbl水下紧组合导航方法及装置、水下载体控制设备 | |
CN115790669B (zh) | 基于最大熵平滑变结构滤波的dvl误差自适应标定方法 |
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 |