CN113503891B - 一种sinsdvl对准校正方法、系统、介质及设备 - Google Patents

一种sinsdvl对准校正方法、系统、介质及设备 Download PDF

Info

Publication number
CN113503891B
CN113503891B CN202110438136.0A CN202110438136A CN113503891B CN 113503891 B CN113503891 B CN 113503891B CN 202110438136 A CN202110438136 A CN 202110438136A CN 113503891 B CN113503891 B CN 113503891B
Authority
CN
China
Prior art keywords
error
sins
dvl
matrix
speed
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
CN202110438136.0A
Other languages
English (en)
Other versions
CN113503891A (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.)
Naval University of Engineering PLA
Original Assignee
Naval University of Engineering PLA
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 Naval University of Engineering PLA filed Critical Naval University of Engineering PLA
Priority to CN202110438136.0A priority Critical patent/CN113503891B/zh
Publication of CN113503891A publication Critical patent/CN113503891A/zh
Application granted granted Critical
Publication of CN113503891B publication Critical patent/CN113503891B/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
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
    • 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

本发明属于导航系统技术领域，公开了一种SINSDVL对准校正方法、系统、介质及设备，所述SINSDVL对准校正方法包括：将参考速度与DVL输出速度构建成两个点集，将校准问题转化为Wahba问题；将粒子群算法引入误差估计问题中，建立基于粒子群算法的估计模型；将SINS与DVL传感器间的安装误差、比例因子误差建立粒子群算法适应度函数，估计常值误差矩阵，进而估计得到SINS与DVL传感器间的比例因子与安装失准角。仿真实验与半物理实验结果表明，本发明的校准方法能够快速准确估计出SINS与DVL传感器间的比例因子与安装失准角，使得组合导航精度大幅提升，无需载体做特殊运动或匀速运动，简单且对准速度快。

Description

一种SINSDVL对准校正方法、系统、介质及设备
技术领域
本发明属于导航系统技术领域，尤其涉及一种SINSDVL对准校正方法、系统、介质及设备。
背景技术
目前，自主式水下航行器(AUV)在执行任务中，导航系统不可或缺。AUV导航系统为其提供实时的导航信息，包括速度、位置、姿态等，精确的导航信息是AUV成功完成任务的保证之一。在水下环境，AUV的主导航设备为捷联式惯性导航系统(SINS)。SINS虽然在短时间内能够提供高精度的导航信息，但是由于它是一种基于牛顿惯性定律的积分推算系统，需要对惯性测量单元(IMU)，即陀螺仪及加速度传感器的输出进行积分运算，因此随时间迅速增长的误差不可避免。目前提供导航系统的精度的方法主要是将两种或两种以上的导航系统进行组合。AUV组合导航系统常用的导航子系统或传感器包括：全球定位系统、多普勒速度计程仪、深度计等。由于海水屏蔽GPS，所以只有在AUV接收天线露出水面时才能接收到GPS信号。多普勒速度计程仪是一种基于多普勒频移测量速度的传感器。利用信息融合手段，可以利用速度观测得到精确的导航信息。
在理想情况下，SINS/DVL组合导航系统可以得到高精度的导航信息。但是实际上，SINS/DVL组合导航的精度取决于传感器对准校准的精度。由于各种误差的影响，导航结果并不理想。比如比例因子误差、安装误差等这些误差由于工艺的限制不可避免，为了减小这些误差对导航结果的影响，研究者们提出了许多方法来校正安装误差。
在现有文献中，校正对准方法只考虑到航向失准角的影响，但是在只考虑航向失准角的情况下仍然存在较大的误差，不能满足导航精度的要求。随后，现有文献将校正对准问题扩展到三维失准角，使得导航进度得到提升。在现有文献中，在辅助传感器LBL的提供精确位置信息的前提下，可以估计出安装失准角。但是其使用受到局限性。现有文献利用卡尔曼滤波技术来估计失准角，但是其需要长时间不间断的GPS信号提供导航信息使估计值收敛，这在实际运用中受到限制。现有文献通过使用来自INS和DVL的加速度，提出一种基于加速度校正对准的方法。其优点是不需要使用外部辅助传感器，但是AUV必须进行复杂的操纵才能获得变化的加速度。现有文献通过分析组合系统的可观测性，利用两点即可求出DVL比例因子及失准矩阵，此方法不需要外部传感器的辅助，但需要有特定轨迹与机动。在现有文献中，将校正对准问题转化为两点序列之间的对准参数估计问题，基于奇异值分解的最小二乘估计方法用于对准。这种估计方法便捷快速且精度较高，但是其仅在小失准角且载体匀速运动时效果较高。现有文献提出利用利用遗传算法及支持向量机来构造用于积分标定和合并补偿的回归预测因子，该方法能在模型训练完成时补偿存在的失准角及误差，但是要求训练集与测试集满足一致性要求，有局限性。
通过上述分析，现有技术存在的问题及缺陷为：
(1)SINS/DVL组合导航的精度取决于传感器对准校准的精度，但由于比例因子误差、安装误差等各种误差的影响，导航结果并不理想。
(2)现有校正对准方法只考虑到航向失准角的影响，但是在只考虑航向失准角的情况下仍然存在较大的误差，不能满足导航精度的要求。
(3)现有校正对准方法需要长时间不间断的GPS信号提供导航信息使估计值收敛，这在实际运用中受到限制。
(4)现有校正对准方法不需要使用外部辅助传感器，但是AUV必须进行复杂的操纵才能获得变化的加速度。
(5)现有校正对准方法需要有特定轨迹与机动，仅在小失准角且载体匀速运动时效果较高，且要求训练集与测试集满足一致性要求，有局限性。
解决以上问题及缺陷的难度为：现有技术的问题与缺陷可以总结为校正对准方法或需要特定机动、或需要长时间外部不间断辅助信号以达到误差估计收敛。这在实际应用中，对于民用等要求不高的场合可适用。但是在军用方面需要快速性以及准确性。为实现快速校正对准，解决以上问题，需要重新建立SINS与DVL间的误差模型，并利用不同于传统卡尔曼滤波等方式估计误差。
解决以上问题及缺陷的意义为：高精度的水下导航定位技术是海洋资源勘探、开发，海洋工程及军事作战和精确打击等国民和国防应用领域必不可少的关键技术。SINS与DVL组合导航是目前普遍的水下航行器导航方案。然而SINS与DVL间的安装失准角、比例因子等误差对组合导航的精度影响非常大，在要求高精度导航信息的水下航行器中，由于SINS与DVL间的误差带来的组合导航误差是无法忍受的。精确地校准SINS与DVL间的安装失准角、比例因子等误差对SINS/DVL组合导航精度提升明显。同时，快速校准SINS与DVL间的安装失准角对水下航行器、水下作战部队的快速反应与应急启动有重要意义。
发明内容
针对现有技术存在的问题，本发明提供了一种SINSDVL对准校正方法、系统、介质及设备，尤其涉及一种基于粒子群算法寻优的SINSDVL对准校正方法、系统、介质及设备。
本发明是这样实现的，一种SINSDVL对准校正方法，所述SINSDVL对准校正方法包括：
将参考速度与DVL输出速度构建成两个点集，将校准问题转化为Wahba问题；将粒子群算法引入误差估计问题中，建立基于粒子群算法的估计模型；将SINS与DVL传感器之间的安装误差、比例因子误差建立粒子群算法适应度函数，估计出常值误差矩阵，进而估计得到SINS与DVL传感器间的比例因子与安装失准角。
进一步，所述SINSDVL对准校正方法包括以下步骤：
步骤一，进行传感器间的误差分析；
步骤二，基于粒子群算法的误差校正模型的构建；
步骤三，利用粒子群寻优算法求解出最优旋转矩阵，实现对准校正。
进一步，步骤一中，所述进行传感器间的误差分析，包括：
选取东北天地理坐标系作为捷联惯导系统的导航参考坐标系，记为n系。地心惯性坐标系记为i系，载体坐标系记为b系，DVL测量坐标系为d系。在理想情况下，b系与d系的应该是对齐的，但是由于工艺及安装问题，b系与d系存在失准角。两个坐标系的关系中，ob-xbybzb为b系，od-xdydzd为d系。
Figure BDA0003033865700000031
是SINS与DVL间的杆臂误差。b系与d系之间的失准角与比例因子对组合导航系统的精度有很大的影响。
设
Figure BDA0003033865700000032
为b系和d系之间的旋转矩阵，通过
Figure BDA0003033865700000033
可以将d系速度vd转化为b系下的速度vb。理想情况下，
Figure BDA0003033865700000034
由于失准角不可忽略，则
Figure BDA0003033865700000035
φ＝[x,y,z]T是三个失准角组成的向量，[·×]是向量[·]的反对称矩阵。DVL量测的n系下的速度可以表示为：
Figure BDA0003033865700000036
式中，
Figure BDA0003033865700000037
为姿态转移矩阵，s为比例因子。
进一步，步骤二中，所述粒子群算法，包括：
PSO算法是一种以种群为基础的随机搜索方法，以鸟群觅食行为为模型。PSO算法通过建立一大群个体，称为粒子；其中每个粒子代表一个候选解决方案，通过粒子个体的简单行为，群体内的信息交互实现问题求解的智能性。
每个粒子可视为N维搜索空间中的一个搜索个体，粒子的当前位置即为对应优化问题的一个候选解，粒子的飞行过程即为该个体的搜索过程。粒子的飞行速度可根据粒子历史最优位置和种群历史最优位置进行动态调整。粒子仅具有两个属性：速度和位置，速度代表移动的快慢，位置代表移动的方向。每个粒子单独搜寻的最优解叫做个体极值，粒子群中最优的个体极值作为当前全局最优解。不断迭代，更新速度和位置。最终得到满足终止条件的最优解。
算法流程如下：
Vid＝ωVid+C1r(0,1)(Pid-Xid)+C2r(0,1)(Pgd-Xid)；
Xid＝Xid+Vid；
式中，ω称为惯性因子，通过调节ω的大小，可以对全局寻优性能和局部寻优性能进行调整。C1和C2称为加速度常数，前者为每个粒子的个体学习因子，后者为每个粒子的社会学习因子。r(0,1)表示区间[0,1]上的随机数，Pid表示第i个变量的个体极值的第d维，Pgd表示全局最优解的第d维。
进一步，步骤二中，所述基于PSO算法的对准校正，包括：
SINS与DVL传感器间的误差主要包括比例因子及失准角误差，所述误差矩阵如下式所示：
Figure BDA0003033865700000041
式中，s为比例因子。φ＝[x,y,z]T是安装失准角，φ×是失准角的反对称矩阵,为均值为0的高斯白噪声。
通过SINS/GPS组合导航，得到参考速度：
Figure BDA0003033865700000042
将SINS/GPS组合导航得到的参考速度用于校准问题的参考点集，可得：
Figure BDA0003033865700000043
根据上式，比例因子s和失准角旋转矩阵
Figure BDA0003033865700000044
即可求出。令
Figure BDA0003033865700000045
IMU与DVL间的失准角旋转矩阵
Figure BDA0003033865700000046
不会随时间变化，比例因子s虽然会随水体属性的变化而变化，但是在执行任务期间的变化幅度基本可以忽略，因此认为P是固定不变的，故两个传感器间的校准问题可以转化为估计两个点集之间的旋转矩阵问题。定义
Figure BDA0003033865700000047
和vb分别为两个点集，通过PSO算法来估计两个点集间的最优旋转矩阵。
构建PSO算法的粒子
Figure BDA0003033865700000048
每一个粒子中的
Figure BDA0003033865700000049
和
Figure BDA00030338657000000410
分别代表三个方向的失准角，
Figure BDA00030338657000000411
为比例因子，i代表为第i个粒子。
PSO算法通过建立适应度函数计算适应度来评价粒子的优劣，粒子通过不断更新逐步提高自身的适应度值，最终所有粒子都要趋向于最优粒子。建立适应度函数是PSO算法进行SINS/DVL对准校正的关键，为了寻找到最优的比例因子及失准角，建立代价函数如下所示：
Figure BDA00030338657000000412
式中，t为点集的长度，也就是采集的速度的时间长度，||X||F为矩阵X的Frobenius范数，
Figure BDA00030338657000000413
和
Figure BDA00030338657000000414
分别为PSO算法的粒子表示的比例因子
Figure BDA00030338657000000415
和失准角
Figure BDA00030338657000000416
优化的目标为求得代价函数的最小值，故定义适应度函数为F＝1/J。
进一步，步骤三中，所述利用粒子群寻优算法求解出最优旋转矩阵，粒子群寻优化算法的框架如下所示
Figure BDA00030338657000000417
Figure BDA0003033865700000051
利用粒子群寻优算法求解出最优旋转矩阵后，对DVL输出的速度进行修正，然后利用修正后的速度进行组合导航的模型；具体包括：
根据SINS误差模型，选取位置误差δP、速度误差δv、姿态误差
Figure BDA0003033865700000052
陀螺仪误差εb和加速度计误差
Figure BDA0003033865700000053
作为状态量。SINS状态向量为：
Figure BDA0003033865700000054
式中，δL和δλ分别代表纬度误差与经度误差，δvE和δvN分别代表东向和北向速度误差，
Figure BDA0003033865700000055
代表俯仰角误差，
Figure BDA0003033865700000056
代表横滚角误差，
Figure BDA0003033865700000057
代表航向角误差。
建立SINS系统状态方程为：
Figure BDA0003033865700000058
式中，FSINS为状态转移矩阵，WSINS为代表零均值的高斯白噪声。
状态转移矩阵FSINS为：
Figure BDA0003033865700000059
式中，F为7×7矩阵，F中的非零元素如下：
F1，4＝1/Re，
F2，1＝(vE/Re)tanLsecL，
F2,3＝secL/Re，
F3，3＝(vN/Re)tanL，
F3，4＝2ωie sinL+(vE/Re)tanL，
F3，6＝-fU，
F3，7＝fN，
Figure BDA0003033865700000061
F4,3＝-[2ωiesinL+(vE/Re)tanL]，
F4，5＝fU，
F4，7＝-fE，
F5，4＝-1/Re，
F5，6＝ωiesinL+(vE/Re)tanL，
F5，7＝-[ωiecosL+(vE/Re)]，
F6，1＝-ωiesinL，
F6，3＝1/Re，
F6,5＝-[ωiesinL+(vE/Re)tanL]，
F6，7＝-vE/Re，
F7,1＝ωiecosL+(vE/Re)(secL)2，
F7，3＝tanL/Re，
F7，5＝[ωiecosL+(vE/Re)]，
F7，6＝vN/Re；
其中，
Figure BDA0003033865700000062
G为6×6矩阵，其中具体形式如下：
Figure BDA0003033865700000071
式中，
Figure BDA0003033865700000072
表示姿态矩阵
Figure BDA0003033865700000073
的前两行。
将SINS解算得到的速度与DVL测量的b系下速度在n系上投影之差作为观测量，则SINS/DVL子系统的量测方程为：
Figure BDA0003033865700000074
式中，Hv为量测矩阵，量测噪声Vv～N(0,Rv),Rv为量测噪声阵。选取东向速度误差与北向速度误差作为观测量，则有：
Figure BDA0003033865700000075
则量测矩阵Hv为：
Hv＝[02×2,I2×2,02×9]；
KF滤波过程如下：
k时刻离散KF的时间更新方程和量测更新方程如下：
(a)时间更新方程：
Figure BDA0003033865700000076
Figure BDA0003033865700000077
(b)量测更新方程：
Figure BDA0003033865700000078
Figure BDA0003033865700000079
Figure BDA00030338657000000710
Figure BDA00030338657000000711
利用SINS与GPS组合导航得到参考速度，b系下的参考速度如下所示：
Figure BDA0003033865700000081
式中，下标SINS/GPS代表通过SINS/GPS组合导航得到的姿态转移矩阵及速度。
本发明的另一目的在于提供一种应用所述的SINSDVL对准校正方法的SINSDVL对准校正系统，所述SINSDVL对准校正系统包括：
误差分析模块，用于进行传感器间的误差分析；
问题转化模块，用于将参考速度与DVL输出速度构建成两个点集，将校准问题转化为Wahba问题；
模型构建模块，用于进行基于粒子群算法的误差校正模型的构建；
对准校正模块，用于利用粒子群寻优算法求解出最优旋转矩阵，估计得到SINS与DVL传感器间的比例因子与安装失准角。
本发明的另一目的在于提供一种信息数据处理终端，包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行如下步骤：
将参考速度与DVL输出速度构建成两个点集，将校准问题转化为Wahba问题；将粒子群算法引入误差估计问题中，建立基于粒子群算法的估计模型；
将SINS与DVL传感器之间的安装误差、比例因子误差建立粒子群算法适应度函数，估计出常值误差矩阵，进而估计得到SINS与DVL传感器间的比例因子与安装失准角。
本发明的另一目的在于提供一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行如下步骤：
将参考速度与DVL输出速度构建成两个点集，将校准问题转化为Wahba问题；将粒子群算法引入误差估计问题中，建立基于粒子群算法的估计模型；
将SINS与DVL传感器之间的安装误差、比例因子误差建立粒子群算法适应度函数，估计出常值误差矩阵，进而估计得到SINS与DVL传感器间的比例因子与安装失准角。
本发明的另一目的在于提供一种自主式水下航行器，所述自主式水下航行器搭载所述的SINSDVL对准校正系统，并实施所述的SINSDVL对准校正方法。
结合上述的所有技术方案，本发明所具备的优点及积极效果为：本发明提供的SINSDVL对准校正方法，用粒子群优化算法的传感器校准算法。将参考速度与DVL输出速度构建成两个点集，从而将校准问题转化为Wahba问题，并利用粒子群寻优算法求解出最优旋转矩阵。仿真实验与半物理实验结果表明，本发明提出的校准方法能够快速准确地估计出SINS与DVL传感器间的比例因子与安装失准角，使得组合导航精度得到大幅提升。相对于传统的校准方法，本算法不要求AUV做特殊机动或匀速运动，可以任何运动状态下进行校准。
本发明提出了一种新的SINS/DVL组合导航校正校准方法，将粒子群算法引入误差估计问题中。建立了基于粒子群算法的估计模型。将SINS与DVL传感器之间的安装误差、比例因子误差建立粒子群算法适应度函数，估计出常值误差矩阵。与传统的校正方法相比，该方法的优点或贡献可以概括为：1)该方法不需要载体做特殊运动或匀速运动；2)该方法简单且对准速度快。
SINS与DVL间的安装误差以及比例因子误差对SINS/DVL组合导航的精度有极大的影响。针对SINS/DVL对准校正问题，提出了一种利用粒子群算法来估计比例因子误差及失准角。仿真和实验结果证明了提出的方法的有效性，并且相对于传统方法有着一下几个优点：1)校准过程不局限于载体运动状态，适合在任何运动状态下校准；2)该方法能够可靠地估计比例因子误差及失准角；3)该方法简单且对准校正速度快。
附图说明
为了更清楚地说明本发明实施例的技术方案，下面将对本发明实施例中所需要使用的附图做简单的介绍，显而易见地，下面所描述的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下还可以根据这些附图获得其他的附图。
图1是本发明实施例提供的SINSDVL对准校正方法流程图。
图2是本发明实施例提供的SINSDVL对准校正系统结构框图；
图中：1、误差分析模块；2、问题转化模块；3、模型构建模块；4、对准校正模块。
图3是本发明实施例提供的两个坐标系的关系示意图。
图4(a)是本发明实施例提供的SINS/DVL组合导航结构示意图。图4(b)是本发明实施例提供的SINS/GPS组合系统结构图所示。
图5(a)-图5(d)是本发明实施例提供的匀速运动的蒙特卡洛实验结果示意图。
图6是本发明实施例提供的代价函数得到的值J的变化过程示意图。
图7(a)-图7(c)是本发明实施例提供的不同方法校正的载体速度对比图。
图8是本发明实施例提供的实验轨迹图。
图9是本发明实施例提供的位置误差示意图。
图10(a)-图10(d)是本发明实施例提供的变速运动的蒙特卡洛实验结果图。
图11是本发明实施例提供的PSO代价函数值变化过程示意图。
图12-图13是本发明实施例提供的不同方法校正的载体速度对比图。
图14是本发明实施例提供的轨迹对比图。
图15是本发明实施例提供的组合导航位置误差示意图。
图16是本发明实施例提供的实验轨迹示意图。
图17(a)-图17(c)是本发明实施例提供的载体速度校正示意图。
图18是本发明实施例提供的轨迹对比示意图。
图19是本发明实施例提供的位置误差示意图。
图20是本发明实施例提供的蒙特卡洛实验结果示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
针对现有技术存在的问题，本发明提供了一种SINSDVL对准校正方法、系统、介质及设备，下面结合附图对本发明作详细的描述。
如图1所示，本发明实施例提供的SINSDVL对准校正方法包括以下步骤：
S101，进行传感器间的误差分析；
S102，基于粒子群算法的误差校正模型的构建；
S103，利用粒子群寻优算法求解出最优旋转矩阵，实现对准校正。
如图2所示，本发明实施例提供的SINSDVL对准校正系统包括：
误差分析模块1，用于进行传感器间的误差分析；
问题转化模块2，用于将参考速度与DVL输出速度构建成两个点集，将校准问题转化为Wahba问题；
模型构建模块3，用于进行基于粒子群算法的误差校正模型的构建；
对准校正模块4，用于利用粒子群寻优算法求解出最优旋转矩阵，并估计得到SINS与DVL传感器间的比例因子与安装失准角。
下面结合实施例对本发明的技术方案作进一步描述。
1、发明概述
在水下SINS/DVL组合导航系统中，SINS与DVL传感器间的校准问题是影响组合导航精度的重要因素之一。传统校准方法通常需要长时间的滤波估计或者要求AUV进行特定的机动，具有局限性。本发明提出一种利用粒子群优化算法的传感器校准算法。将参考速度与DVL输出速度构建成两个点集，从而将校准问题转化为Wahba问题，并利用粒子群寻优算法求解出最优旋转矩阵。仿真实验与半物理实验结果表明，本发明提出的校准方法能够快速准确地估计出SINS与DVL传感器间的比例因子与安装失准角，使得组合导航精度得到大幅提升。相对于传统的校准方法，本算法不要求AUV做特殊机动或匀速运动，可以任何运动状态下进行校准。
为了解决上述文献所提到的方法的不足，本发明提出了一种新的SINS/DVL组合导航校正校准方法，将粒子群算法引入误差估计问题中。建立了基于粒子群算法的估计模型。将SINS与DVL传感器之间的安装误差、比例因子误差建立粒子群算法适应度函数，估计出常值误差矩阵。与传统的校正方法相比，该方法的优点或贡献可以概括为：1)该方法不需要载体做特殊运动或匀速运动；2)该方法简单且对准速度快。
本发明的结构如下：第二节分析了传感器间的误差并提出了基于粒子群算法的误差校正模型；第三节分析了SINS/DVL组合导航的框架；第四节利用仿真实验验证了本发明提出的方法在不同情况下的性能；在第五节中利用船载实测数据进行半物理仿真并验证了所提的方法。在第六节中对实验结果进行分析与总结。
2、对准校正算法
2.1误差分析
选取东北天地理坐标系作为捷联惯导系统的导航参考坐标系，记为n系。地心惯性坐标系记为i系，载体坐标系记为b系，DVL测量坐标系为d系。在理想情况下，b系与d系的应该是对齐的，但是由于工艺及安装问题，b系与d系存在失准角。两个坐标系的关系如图3所示，图中ob-xbybzb为b系，od-xdydzd为d系。
Figure BDA0003033865700000111
是SINS与DVL间的杆臂误差。b系与d系之间的失准角与比例因子对组合导航系统的精度有很大的影响。
设
Figure BDA0003033865700000112
为b系和d系之间的旋转矩阵，通过
Figure BDA0003033865700000113
可以将d系速度vd转化为b系下的速度vb。理想情况下，
Figure BDA0003033865700000114
然而由于失准角不可忽略，则
Figure BDA0003033865700000115
φ＝[x,y,z]T是三个失准角组成的向量，[·×]是向量[·]的反对称矩阵。DVL量测的n系下的速度可以表示为：
Figure BDA0003033865700000116
式中，
Figure BDA0003033865700000117
为姿态转移矩阵，s为比例因子。
2.2粒子群算法
PSO算法是一种以种群为基础的随机搜索方法，以鸟群觅食行为为模型。PSO算法通过建立一大群个体(称为粒子)，其中每个粒子代表一个候选解决方案，通过粒子个体的简单行为，群体内的信息交互实现问题求解的智能性。
每个粒子可视为N维搜索空间中的一个搜索个体，粒子的当前位置即为对应优化问题的一个候选解，粒子的飞行过程即为该个体的搜索过程.粒子的飞行速度可根据粒子历史最优位置和种群历史最优位置进行动态调整.粒子仅具有两个属性：速度和位置，速度代表移动的快慢，位置代表移动的方向。每个粒子单独搜寻的最优解叫做个体极值，粒子群中最优的个体极值作为当前全局最优解。不断迭代，更新速度和位置。最终得到满足终止条件的最优解。
算法流程如下：
Vid＝ωVid+C1r(0,1)(Pid-Xid)+C2r(0,1)(Pgd-Xid)；
Xid＝Xid+Vid；
式中，ω称为惯性因子，通过调节ω的大小，可以对全局寻优性能和局部寻优性能进行调整。C1和C2称为加速度常数，前者为每个粒子的个体学习因子，后者为每个粒子的社会学习因子。r(0,1)表示区间[0,1]上的随机数，Pid表示第i个变量的个体极值的第d维，Pgd表示全局最优解的第d维。
2.3基于PSO算法的对准校正方法
SINS与DVL传感器间的误差主要包括比例因子及失准角误差。误差矩阵如式x所示。
Figure BDA0003033865700000121
式中，s为比例因子。φ＝[x,y,z]T是安装失准角，φ×是失准角的反对称矩阵,为均值为0的高斯白噪声。在实际中，通常还包括一个非常小的常值误差，在这里不考虑它的影响。
通过SINS/GPS组合导航，可以得到参考速度：
Figure BDA0003033865700000122
将式x代入公式x，可得：
Figure BDA0003033865700000123
根据上式，比例因子s和失准角旋转矩阵
Figure BDA0003033865700000124
即可求出。令
Figure BDA0003033865700000125
IMU与DVL间的失准角旋转矩阵
Figure BDA0003033865700000126
不会随时间变化，比例因子s虽然会随水体属性的变化而变化，但是在执行任务期间的变化幅度基本可以忽略，因此认为P是固定不变的。因此可以认为两个传感器间的校准问题可以转化为估计两个点集之间的旋转矩阵问题。定义
Figure BDA0003033865700000127
和vb分别为两个点集，通过PSO算法来估计两个点集间的最优旋转矩阵。
构建PSO算法的粒子
Figure BDA0003033865700000128
每一个粒子中的
Figure BDA0003033865700000129
和
Figure BDA00030338657000001210
分别代表三个方向的失准角，
Figure BDA00030338657000001211
为比例因子，i代表为第i个粒子。
PSO算法通过建立适应度函数计算适应度来评价粒子的优劣，粒子通过不断更新逐步提高自身的适应度值，最终所有粒子都要趋向于最优粒子。建立适应度函数是本文提出PSO算法进行SINS/DVL对准校正的关键，为了寻找到最优的比例因子及失准角，建立代价函数如下所示：
Figure BDA00030338657000001212
式中，t为点集的长度，也就是采集的速度的时间长度，||X||F为矩阵X的Frobenius范数，
Figure BDA00030338657000001213
和
Figure BDA00030338657000001214
分别为PSO算法的粒子表示的比例因子
Figure BDA00030338657000001215
和失准角
Figure BDA00030338657000001216
为了寻找最优参数，优化的目标为求得代价函数的最小值，故定义适应度函数为F＝1/J。
3、组合导航系统
3.1SINS/DVL组合导航框架：
SINS/DVL组合导航结构如图4(a)所示。
从图4(a)中可以看出SINS/DVL组合导航是利用SINS与DVL的速度残差通过Kalman滤波进行组合导航。Kalman滤波实时反馈误差给SINS得到精确的导航信息。
根据现有文献中所述SINS误差模型，选取位置误差δP、速度误差δv、姿态误差
Figure BDA0003033865700000131
陀螺仪误差εb和加速度计误差
Figure BDA0003033865700000132
作为状态量。SINS状态向量为：
Figure BDA0003033865700000133
式中，δL和δλ分别代表纬度误差与经度误差，δvE和δvN分别代表东向和北向速度误差，
Figure BDA0003033865700000134
代表俯仰角误差，
Figure BDA0003033865700000135
代表横滚角误差，
Figure BDA0003033865700000136
代表航向角误差。
建立SINS系统状态方程为：
Figure BDA0003033865700000137
式中，FSINS为状态转移矩阵，WSINS为代表零均值的高斯白噪声。
状态转移矩阵FSINS为：
Figure BDA0003033865700000138
式中，F为7×7矩阵，F中的非零元素如下：
F1，4＝1/Re，
F2，1＝(vE/Re)tanLsecL，
F2,3＝secL/Re，
F3，3＝(vN/Re)tanL，
F3，4＝2ωie sinL+(vE/Re)tanL，
F3，6＝-fU，
F3，7＝fN，
Figure BDA0003033865700000139
F4,3＝-[2ωiesinL+(vE/Re)tanL]，
F4，5＝fU，
F4，7＝-fE
F5，4＝-1/Re,
F5，6＝ωiesinL+(vE/Re)tanL,
F5，7＝-[ωiecosL+(vE/Re)],
F6，1＝-ωiesinL,
F6，3＝1/Re,
F6,5＝-[ωiesinL+(vE/Re)tanL],
F6，7＝-vE/Re,
F7,1＝ωiecosL+(vE/Re)(secL)2
F7，3＝tanL/Re,
F7，5＝[ωiecosL+(vE/Re)]，
F7，6＝vN/Re
其中，
Figure BDA0003033865700000141
G为6×6矩阵，其中具体形式如下：
Figure BDA0003033865700000142
式中，
Figure BDA0003033865700000143
表示姿态矩阵
Figure BDA0003033865700000144
的前两行。
将SINS解算得到的速度与DVL测量的b系下速度在n系上投影之差作为观测量，则SINS/DVL子系统的量测方程为：
Figure BDA0003033865700000145
式中，Hv为量测矩阵，量测噪声Vv～N(0,Rv),Rv为量测噪声阵。选取东向速度误差与北向速度误差作为观测量，则有：
Figure BDA0003033865700000151
则量测矩阵Hv为：
Hv＝[02×2,I2×2,02×9]；
KF滤波过程如下：
k时刻离散KF的时间更新方程和量测更新方程如下：
(a)时间更新方程
Figure BDA0003033865700000152
Figure BDA0003033865700000153
(b)量测更新方程
Figure BDA0003033865700000154
Figure BDA0003033865700000155
Figure BDA0003033865700000156
Pk|k＝(I-KkHk)Pk|k-1 (2.3.6)
3.2SINS/GPS组合导航框架
为了对准校正SINS与DVL传感器间的失准角、比例因子等误差，设计SINS/GPS组合导航系统用于提供参考值。
图4(b)是本发明实施例提供的SINS/GPS组合系统结构图所示。
利用SINS与GPS组合导航得到参考速度，b系下的参考速度如下所示：
Figure BDA0003033865700000157
式中下标SINS/GPS代表通过SINS/GPS组合导航得到的姿态转移矩阵及速度。
4、仿真实验
在本节中，通过仿真验证了所提方法的性能。利用捷联惯导反演算法，先设置好载体运动轨迹，则可以模拟生成惯性传感器信息。同时，对设置的载体运动轨迹添加一个高斯噪声可以模拟为GPS位置信息。将仿真程序真实的n系下速度与真实的姿态进行转换可以得到真实的b系速度。若在真实姿态中添加一个失准角，并加入比例因子与高斯噪声，则可以认为得到的b系下的速度是未经校正对准的DVL输出速度。仿真实验IMU的传感器误差设置为：陀螺仪的常值零偏为0.02°/h，角度随机游走为
Figure BDA0003033865700000161
加速度计的常值零偏为50μg，加速度计的随机游走为1μg。IMU的更新频率为200Hz。GPS位置量测噪声设置为[10m,10m,10m]，速度量测噪声设置为[0.5m/s，0.5m/s，0.5m/s]。更新频率为1Hz。
在仿真中，选择了三种方案来验证所提出的校正对准方法的性能：
(I)SINS与DVL传感器之间人为设置失准角为[0.1°，-0.12°,1°]，且载体做匀速运动；
(II)SINS与DVL传感器之间人为设置失准角为[0.1°，0.12°,1°]，载体做变速运动；
在上述仿真方案中，使用传统校正对准方案进行性能的比较：现有技术文献中使用基于SVD的最小二乘法估计方法。
4.1匀速运动
本仿真实验的初始位置为[34°,108°,0m],初始速度和初始姿态分别为[-5,0，0](m/s)和[0,0,90°]，AUV保持匀速行驶。人为添加失准角为[0.1°,-0.12°,1°]，比例因子为1。PSO的参数按经验分配如下：c1＝2，c2＝2,w＝0.7,vmax＝5。公式()中的适应度函数的参数为：p1＝1,p2＝1,p3＝1.设置粒子群规模为100，最大迭代次数为1000。为了验证估计算法的有效性和可重复性，使用相同的模拟数据与参数进行50次蒙特卡洛实验。结果如图5所示，其统计量列在表1。
表1统计量
估计值 Mean Std 估计精度 参考值
ε<sub>p</sub> 0.10018 3.9336e-11 99.81％ 0.1
ε<sub>r</sub> -0.11209 2.4300e-09 93.41％ -0.12
ε<sub>y</sub> 1.00009 5.2353e-09 99.99％ 1
ε<sub>s</sub> 0.99985 4.5004e-12 99.98％ <sup>1</sup>
以第一个测试为例，代价函数得到的值J的变化过程如图6所示。从图6及表1可以看出，50次蒙特卡洛实验的估计结果之间的差异很小且估计精度较高，这表明利用PSO方法具有良好的稳定性和可重复性。
仿真实验的时间设置为3600(s)。这里前面600(s)为校正对准阶段，利用参考文献的传统方法和基于粒子群寻优的对准校正方法各自估计出误差矩阵。当估计出误差矩阵，分别对DVL速度进行校正，再分别利用校正后的DVL速度进行组合导航，并对误差进行对比。
图7为不同方法校正的载体速度对比图，图7中曲线KF代表的是利用仿真得到的GPS速度与SINS组合导航的结果，此结果可以当作速度参考值。Uncalibrated曲线代表的是有存在失准角的DVL输出与SINS组合导航得到的结果，SVD曲线为利用文献中的方法得到的结果，绿色PSO曲线为利用本文提出方法得到的结果。从图中可以看出，在未校正情况下的组合导航结果与真实值相差较多，利用SVD方法得到的速度接近参考速度，且对结果有平滑效果。利用PSO方法寻找出误差矩阵并补偿的结果与参考值十分接近，效果较好。图8为实验轨迹图，可以看出利用未校正的速度组合的轨迹与真实轨迹偏差较远。在匀速运动下，利用SVD与PSO方法校正得到的组合导航轨迹接近真实值。图9为位置误差，从图中看出，在匀速运动下，利用SVD与PSO方法校正的组合导航位置误差在3600(s)内均小于20m，而不校正的位置误差大于100m。所以可以看出SINS/DVL传感器间的安装误差对导航精度影响非常大，对准校正对于提升组合导航精度是十分重要的。
4.2变速运动
本仿真实验的初始位置为[34°,108°,0m]，初始速度和初始姿态分别为[0,0，0](m/s)和[0,0,90°]，AUV进行变速运动。人为添加失准角为[0.1°,0.12°,1°]，比例因子为1。PSO的参数按经验分配如下：c1＝2，c2＝2,w＝0.7,vmax＝5.公式()中的适应度函数的参数为：p1＝1,p2＝1,p3＝1.设置粒子群规模为100，最大迭代次数为1000。为了验证估计算法的有效性和可重复性，使用相同的模拟数据与参数进行50次蒙特卡洛实验。结果如图10所示，其统计量列在表2。
表2统计量
估计值 Mean Std 估计精度 参考值
ε<sub>p</sub> 0.10022 9.1875e-10 99.77％ 0.1
ε<sub>r</sub> 0.13027 5.7216e-8 91.44％ 0.12
ε<sub>y</sub> 1.00008 1.0205e-10 99.99％ 1
ε<sub>s</sub> 0.99985 3.1342e-15 99.98％ 1
以第一个测试为例，代价函数得到的值J的变化过程如图11所示。从图11及表2可以看出，50次蒙特卡洛实验的估计结果之间的差异很小，这表明利用PSO方法在变速运动情况下也具有良好的稳定性和可重复性。
仿真实验的时间设置为3600(s)。这里前600(s)为校正对准阶段，利用传统方法和基于粒子群寻优的对准校正方法各自估计出误差矩阵。当估计出误差矩阵，分别对DVL速度进行校正，再分别利用校正后的DVL速度进行组合导航，并对误差进行对比。
图12-13为不同方法校正的载体速度对比图。可以看到，在载体速度变化时，SVD方法校正得到的速度与参考速度误差较大，而PSO方法校正得到的速度在变速运动下依旧有很好的精度。图14为轨迹对比图，可以看出利用SVD方法校正得到的组合导航轨迹严重恶化，说明SVD方法只适用于匀速运动。利用PSO方法校正的组合导航轨迹与真实值十分接近，效果与匀速运动下的基本相同。组合导航位置误差如图15所示，在3600(s)组合导航后，利用PSO方法校正的位置误差小于50m，表现出良好的性能。基于SVD方法的位置误差非常大，完全没有参考作用。
5、半物理仿真实验
在本节中，利用船载实验系统收集的IMU数据与DVL传感器数据进行半物理仿真实验。船载实验系统包括SINS与DVL传感器，及一套单天线GPS接收设备，设备参数如表3所示。
表设备参数
Figure BDA0003033865700000171
Figure BDA0003033865700000181
本实验在扬子江进行，选取其中一段3600(s)数据用于验证本方法。初始位置为[30.8511°，110.9750°]，实验轨迹如图16所示。
实验共3600(s)，利用前1000(s)数据分别使用SVD方法与PSO方法估计比例因子与失准矩阵。利用估计得到的误差矩阵补偿DVL输出，再重新进行组合导航来做性能对比验证。通过补偿后的DVL输出如图17所示。组合导航的结果及位置误差对比如图18-19所示。图中，深色曲线代表DVL输出的原始速度，没有经过任何校正与补偿，浅色色曲线为利用GPS速度得到的b系下的参考速度，蓝色曲线和紫色曲线分别为利用PSO方法与SVD方法得到的校正后的DVL速度。可以看出，利用PSO方法得到的速度相比于SVD得到的速度在变化趋势上更接近参考速度。利用SVD方法校正得到的速度会极大损失载体的动态变化量，对于匀速运动的载体能够适用。但是在实际应用中，在环境较严苛的情况下，载体不一定能做匀速运动，因此PSO方法更适用于SINS与DVL传感器间的对准校正。图18为利用校正后的速度进行组合导航的轨迹，可以看出利用PSO方法校正后，轨迹与参考轨迹十分接近。利用DVL输出的原始速度组合导航得到的轨迹为红色曲线，可以看到位置发散较严重。利用SVD方法校正后的组合导航轨迹效果也不太好。图19为各校正方法得到的位置与参考位置间的误差，原始速度与SVD方法校正的速度组合得到的位置误差都很大。利用PSO方法校正后的速度进行3600(s)组合导航得到的误差小于50m。
为了验证所提方法的稳定性和可重复性，进行50次独立的蒙特卡洛实验进行验证。将位置的RMSE作为性能度量值，RMSE的定义如下所示：
Figure BDA0003033865700000182
式中ek为位置误差。
蒙特卡洛实验结果如图20所示，可以看出在50次独立实验中，RMSE始终稳定在(15.355，15.415)(m)这个区间。每次实验间的结果变化幅度不到0.1m，说明提出的方法有着优秀的稳定性与可重复性。
6、
SINS与DVL间的安装误差以及比例因子误差对SINS/DVL组合导航的精度有极大的影响。针对SINS/DVL对准校正问题，提出了一种利用粒子群算法来估计比例因子误差及失准角。仿真和实验结果证明了提出的方法的有效性，并且相对于传统方法有着一下几个优点：1)校准过程不局限于载体运动状态，适合在任何运动状态下校准；2)该方法能够可靠地估计比例因子误差及失准角；3)该方法简单且对准校正速度快。
在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上；术语“上”、“下”、“左”、“右”、“内”、“外”、“前端”、“后端”、“头部”、“尾部”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”、“第三”等仅用于描述目的，而不能理解为指示或暗示相对重要性。
在上述实施例中，可以全部或部分地通过软件、硬件、固件或者其任意组合来实现。当使用全部或部分地以计算机程序产品的形式实现，所述计算机程序产品包括一个或多个计算机指令。在计算机上加载或执行所述计算机程序指令时，全部或部分地产生按照本发明实施例所述的流程或功能。所述计算机可以是通用计算机、专用计算机、计算机网络、或者其他可编程装置。所述计算机指令可以存储在计算机可读存储介质中，或者从一个计算机可读存储介质向另一个计算机可读存储介质传输，例如，所述计算机指令可以从一个网站站点、计算机、服务器或数据中心通过有线(例如同轴电缆、光纤、数字用户线(DSL)或无线(例如红外、无线、微波等)方式向另一个网站站点、计算机、服务器或数据中心进行传输)。所述计算机可读取存储介质可以是计算机能够存取的任何可用介质或者是包含一个或多个可用介质集成的服务器、数据中心等数据存储设备。所述可用介质可以是磁性介质，(例如，软盘、硬盘、磁带)、光介质(例如，DVD)、或者半导体介质(例如固态硬盘SolidState Disk(SSD))等。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，都应涵盖在本发明的保护范围之内。

Claims (8)

1.一种SINSDVL对准校正方法，其特征在于，所述SINSDVL对准校正方法包括：
将参考速度与DVL输出速度构建为两个点集，将校准问题转化为Wahba问题；
将粒子群算法引入误差估计问题中，建立基于粒子群算法的估计模型；
将SINS与DVL传感器之间的安装误差、比例因子误差建立粒子群算法适应度函数，估计出常值误差矩阵，并得到SINS与DVL传感器间的比例因子与安装失准角；
所述SINSDVL对准校正方法具体包括以下步骤：
步骤一，进行传感器间的误差分析；
步骤二，基于粒子群算法的误差校正模型的构建；
步骤三，利用粒子群寻优算法求解出最优旋转矩阵，实现对准校正；
步骤二中，基于PSO算法的对准校正，包括：
SINS与DVL传感器间的误差包括比例因子及失准角误差，所述误差矩阵如下式所示：
Figure FDA0003861713460000011
式中，s为比例因子；φ＝[x,y,z]T是安装失准角，φ×是失准角的反对称矩阵，为均值为0的高斯白噪声；
通过SINS/GPS组合导航，得到参考速度：
Figure FDA0003861713460000012
由此得：
Figure FDA0003861713460000013
根据上式，求出比例因子s和失准角旋转矩阵
Figure FDA0003861713460000014
令
Figure FDA0003861713460000015
IMU与DVL间的失准角旋转矩阵
Figure FDA0003861713460000016
不会随时间变化，忽略比例因子s在执行任务期间的变化幅度，因此认为P是固定不变的，两个传感器间的校准问题转化为估计两个点集之间的旋转矩阵问题；定义
Figure FDA0003861713460000017
和vb分别为两个点集，通过PSO算法来估计两个点集间的最优旋转矩阵；
构建PSO算法的粒子
Figure FDA0003861713460000018
每一个粒子中的
Figure FDA0003861713460000019
和
Figure FDA00038617134600000110
分别代表三个方向的失准角，
Figure FDA00038617134600000111
为比例因子，i代表为第i个粒子；
PSO算法通过建立适应度函数计算适应度来评价粒子的优劣，粒子通过不断更新逐步提高自身的适应度值，最终所有粒子都要趋向于最优粒子；建立适应度函数是PSO算法进行SINS/DVL对准校正的关键，为了寻找到最优的比例因子及失准角，建立代价函数如下所示：
Figure FDA0003861713460000021
式中，t为点集的长度，也就是采集的速度的时间长度，||X||F为矩阵X的Frobenius范数，
Figure FDA0003861713460000022
和
Figure FDA0003861713460000023
分别为PSO算法的粒子表示的比例因子
Figure FDA0003861713460000024
和失准角
Figure FDA0003861713460000025
优化的目标为求得代价函数的最小值，故定义适应度函数为F＝1/J。
2.如权利要求1所述的SINSDVL对准校正方法，其特征在于，步骤一中，所述进行传感器间的误差分析，包括：
选取东北天地理坐标系作为捷联惯导系统的导航参考坐标系，记为n系；地心惯性坐标系记为i系，载体坐标系记为b系，DVL测量坐标系为d系；在理想情况下，b系与d系是对齐的，但是由于工艺及安装问题，b系与d系存在失准角；两个坐标系的关系中，ob-xbybzb为b系，od-xdydzd为d系；
Figure FDA0003861713460000026
是SINS与DVL间的杆臂误差；
设
Figure FDA0003861713460000027
为b系和d系之间的旋转矩阵，通过
Figure FDA0003861713460000028
将d系速度vd转化为b系下的速度vb；理想情况下，
Figure FDA0003861713460000029
由于失准角不可忽略，则
Figure FDA00038617134600000210
φ＝[x,y,z]T是三个失准角组成的向量，[(×]是向量[·]的反对称矩阵；DVL量测的n系下的速度表示为：
Figure FDA00038617134600000211
式中，
Figure FDA00038617134600000212
为姿态转移矩阵，s为比例因子。
3.如权利要求1所述的SINSDVL对准校正方法，其特征在于，步骤二中，所述粒子群算法，包括：
Vid＝ωVid+C1r(0,1)(Pid-Xid)+C2r(0,1)(Pgd-Xid)；
Xid＝Xid+Vid；
式中，ω称为惯性因子，通过调节ω的大小，对全局寻优性能和局部寻优性能进行调整；C1和C2称为加速度常数，前者为每个粒子的个体学习因子，后者为每个粒子的社会学习因子；r(0,1)表示区间[0,1]上的随机数，Pid表示第i个变量的个体极值的第d维，Pgd表示全局最优解的第d维。
4.如权利要求1所述的SINSDVL对准校正方法，其特征在于，所述步骤三中，粒子群寻优算法如下：
(1)Input:N维的粒子群
(2)Output:全局最优解gbest
(3)随机初始化一组N维粒子群，并评估所有粒子的适合度值
(4)更新个体极值pbest以及全局最优解gbest
(5)设置c1＝c2＝2,ω＝0.6,Vmax＝10,随机初始化粒子飞行速度；
(6)While没有达到停止条件do；
(7)根据个体极值的适应度值从最坏到最好对粒子进行排序；
(8)for每一个粒子Xi,i＝1to N do
(9)评估所有粒子的适合度值，对粒子进行排序
(10)更新所有粒子的个体极值pbest以及全局最优解gbest
(11)end for
(12)for每一个粒子Xi,i＝1to N do
(13)
Figure FDA0003861713460000031
(14)|Vi|≤Vmax
(15)Xi(t+1)＝Xi(t+1)+Vi(t+1)
(16)end for
(17)更新全局最优解gbest；
(18)end while
(19)Return gbest；
利用粒子群寻优算法求解出最优旋转矩阵后，对DVL输出的速度进行修正，然后利用修正后的速度进行组合导航的模型；具体包括：
根据SINS误差模型，选取位置误差δP、速度误差δv、姿态误差
Figure FDA0003861713460000032
陀螺仪误差εb和加速度计误差
Figure FDA0003861713460000033
作为状态量，SINS状态向量为：
Figure FDA0003861713460000034
式中，δL和δλ分别代表纬度误差与经度误差，δvE和δvN分别代表东向和北向速度误差，
Figure FDA0003861713460000035
代表俯仰角误差，
Figure FDA0003861713460000036
代表横滚角误差，
Figure FDA0003861713460000037
代表航向角误差；
建立SINS系统状态方程为：
Figure FDA0003861713460000038
式中，FSINS为状态转移矩阵，WSINS为代表零均值的高斯白噪声；
状态转移矩阵FSINS为：
Figure FDA0003861713460000039
式中，F为7×7矩阵，F中的非零元素如下：
F1，4＝1/Re，
F2，1＝(vE/Re)tan L sec L，
F2,3＝secL/Re，
F3，3＝(vN/Re)tan L，
F3，4＝2ωiesin L+(vE/Re)tan L，
F3，6＝-fU，
F3，7＝fN，
Figure FDA0003861713460000041
F4,3＝-[2ωiesin L+(vE/Re)tan L]，
F4，5＝fU，
F4，7＝-fE，
F5，4＝-1/Re，
F5，6＝ωiesin L+(vE/Re)tan L，
F5，7＝-[ωiecos L+(vE/Re)]，
F6，1＝-ωiesin L，
F6，3＝1/Re，
F6,5＝-[ωiesin L+(vE/Re)tan L]，
F6，7＝-vE/Re，
F7,1＝ωiecos L+(vE/Re)(sec L)2，
F7，3＝tanL/Re，
F7，5＝[ωiecosL+(vE/Re)]，
F7，6＝vN/Re；
其中，
Figure FDA00038617134600000513
Figure FDA0003861713460000051
G为6×6矩阵，其中具体形式如下：
Figure FDA0003861713460000052
式中，
Figure FDA0003861713460000053
Figure FDA0003861713460000054
表示姿态矩阵
Figure FDA0003861713460000055
的前两行；
将SINS解算得到的速度与DVL测量的b系下速度在n系上投影之差作为观测量，则SINS/DVL子系统的量测方程为：
Figure FDA0003861713460000056
式中，Hv为量测矩阵，量测噪声Vv～N(0,Rv),Rv为量测噪声阵；选取东向速度误差与北向速度误差作为观测量，则有：
Figure FDA0003861713460000057
则量测矩阵Hv为：
Hv＝[02×2,I2×2,02×9]；
KF滤波过程如下：
k时刻离散KF的时间更新方程和量测更新方程如下：
(a)时间更新方程：
Figure FDA0003861713460000058
Figure FDA0003861713460000059
(b)量测更新方程：
Figure FDA00038617134600000510
Figure FDA00038617134600000511
Figure FDA00038617134600000512
Pk|k＝(I-KkHk)Pk|k-1；
利用SINS与GPS组合导航得到参考速度，b系下的参考速度如下所示：
Figure FDA0003861713460000061
式中，下标SINS/GPS代表通过SINS/GPS组合导航得到的姿态转移矩阵及速度。
5.一种应用如权利要求1～4任意一项所述的SINSDVL对准校正方法的SINSDVL对准校正系统，其特征在于，所述SINSDVL对准校正系统包括：
误差分析模块，用于进行传感器间的误差分析；
问题转化模块，用于将参考速度与DVL输出速度构建成两个点集，将校准问题转化为Wahba问题；
模型构建模块，用于进行基于粒子群算法的误差校正模型的构建；
对准校正模块，用于利用粒子群寻优算法求解出最优旋转矩阵，估计得到SINS与DVL传感器间的比例因子与安装失准角。
6.一种信息数据处理终端，其特征在于，所述信息数据处理终端包括存储器和处理器，所述存储器存储有计算机程序，所述计算机程序被所述处理器执行时，使得所述处理器执行如权利要求1～4任意一项所述的SINSDVL对准校正方法，包括如下步骤：
将参考速度与DVL输出速度构建成两个点集，将校准问题转化为Wahba问题；将粒子群算法引入误差估计问题中，建立基于粒子群算法的估计模型；
将SINS与DVL传感器之间的安装误差、比例因子误差建立粒子群算法适应度函数，估计出常值误差矩阵，进而估计得到SINS与DVL传感器间的比例因子与安装失准角。
7.一种计算机可读存储介质，存储有计算机程序，所述计算机程序被处理器执行时，使得所述处理器执行如权利要求1～4任意一项所述的SINSDVL对准校正方法，包括如下步骤：
将参考速度与DVL输出速度构建成两个点集，将校准问题转化为Wahba问题；将粒子群算法引入误差估计问题中，建立基于粒子群算法的估计模型；
将SINS与DVL传感器之间的安装误差、比例因子误差建立粒子群算法适应度函数，估计出常值误差矩阵，进而估计得到SINS与DVL传感器间的比例因子与安装失准角。
8.一种自主式水下航行器，其特征在于，所述自主式水下航行器搭载权利要求5所述的SINSDVL对准校正系统，并实施权利要求1～4任意一项所述的SINSDVL对准校正方法。
CN202110438136.0A 2021-04-22 2021-04-22 一种sinsdvl对准校正方法、系统、介质及设备 Active CN113503891B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110438136.0A CN113503891B (zh) 2021-04-22 2021-04-22 一种sinsdvl对准校正方法、系统、介质及设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110438136.0A CN113503891B (zh) 2021-04-22 2021-04-22 一种sinsdvl对准校正方法、系统、介质及设备

Publications (2)

Publication Number Publication Date
CN113503891A CN113503891A (zh) 2021-10-15
CN113503891B true CN113503891B (zh) 2022-11-29

Family

ID=78008313

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110438136.0A Active CN113503891B (zh) 2021-04-22 2021-04-22 一种sinsdvl对准校正方法、系统、介质及设备

Country Status (1)

Country Link
CN (1) CN113503891B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114459501B (zh) * 2021-12-10 2023-08-25 华中光电技术研究所（中国船舶重工集团公司第七一七研究所） 一种惯导安装误差自动标定方法及装置

Family Cites Families (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2013059360A1 (en) * 2011-10-17 2013-04-25 Prodyne Corporation Ultrasonic measurement of particle size distribution
CN102735268B (zh) * 2012-07-10 2014-11-26 中国人民解放军国防科学技术大学 一种基于姿态优化激励的捷联三轴磁力仪标定方法
CN103697910B (zh) * 2013-12-14 2016-06-01 浙江大学 自主水下航行器多普勒计程仪安装误差的校正方法
CN103759742B (zh) * 2014-01-22 2017-04-05 东南大学 基于模糊自适应控制技术的捷联惯导非线性对准方法
CN104215259B (zh) * 2014-08-22 2018-04-24 哈尔滨工程大学 一种基于地磁模量梯度和粒子滤波的惯导误差校正方法
CN105004351A (zh) * 2015-05-14 2015-10-28 东南大学 基于自适应upf的sins大方位失准角初始对准方法
CN109141436A (zh) * 2018-09-30 2019-01-04 东南大学 改进的无迹卡尔曼滤波算法在水下组合导航中的应用方法
CN110398257B (zh) * 2019-07-17 2022-08-02 哈尔滨工程大学 Gps辅助的sins系统快速动基座初始对准方法
CN111323049B (zh) * 2020-03-19 2021-04-20 哈尔滨工程大学 一种粒子群算法的粗对准方法
CN111964689A (zh) * 2020-07-08 2020-11-20 河北汉光重工有限责任公司 深海惯导方位快速校正的方法
CN112504298B (zh) * 2020-11-25 2024-03-15 东南大学 一种gnss辅助的dvl误差标定方法

Also Published As

Publication number Publication date
CN113503891A (zh) 2021-10-15

Similar Documents

Publication Publication Date Title
CN109211276B (zh) 基于gpr与改进的srckf的sins初始对准方法
CN108827310B (zh) 一种船用星敏感器辅助陀螺仪在线标定方法
CN111156987B (zh) 基于残差补偿多速率ckf的惯性/天文组合导航方法
Mu et al. A practical INS/GPS/DVL/PS integrated navigation algorithm and its application on Autonomous Underwater Vehicle
CN111536967B (zh) 一种基于ekf的多传感器融合温室巡检机器人跟踪方法
CN109556631B (zh) 一种基于最小二乘的ins/gnss/偏振/地磁组合导航系统对准方法
CN112146655B (zh) 一种BeiDou/SINS紧组合导航系统弹性模型设计方法
CN111380518B (zh) 一种引入径向速度的sins/usbl紧组合导航定位方法
CN108458709B (zh) 基于视觉辅助测量的机载分布式pos数据融合方法和装置
CN112798021B (zh) 基于激光多普勒测速仪的惯导系统行进间初始对准方法
CN106802143B (zh) 一种基于惯性仪器和迭代滤波算法的船体形变角测量方法
CN114777812B (zh) 一种水下组合导航系统行进间对准与姿态估计方法
Zhang et al. Application of modified ekf based on intelligent data fusion in auv navigation
CN112083457B (zh) 一种神经网络优化的imm卫星定位导航方法
Liu et al. Tightly coupled modeling and reliable fusion strategy for polarization-based attitude and heading reference system
Zhang et al. A calibration method of ultra-short baseline installation error with large misalignment based on variational Bayesian unscented Kalman filter
CN116642482A (zh) 基于固态激光雷达和惯性导航的定位方法、设备和介质
CN115265532A (zh) 一种用于船用组合导航中的辅助滤波方法
CN113503891B (zh) 一种sinsdvl对准校正方法、系统、介质及设备
CN111982126B (zh) 一种全源BeiDou/SINS弹性状态观测器模型设计方法
CN111707292B (zh) 一种自适应滤波的快速传递对准方法
CN116907503A (zh) 一种基于抗野值鲁棒定位算法的遥感编队卫星定位方法及系统
CN114111840B (zh) 一种基于组合导航的dvl误差参数在线标定方法
CN113689501B (zh) 一种基于收敛点的双机协同目标机定位跟踪控制方法
Zhang et al. Calibration method of DVL based on position observation information

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