CN115031726A - 一种数据融合导航定位方法 - Google Patents

一种数据融合导航定位方法 Download PDF

Info

Publication number
CN115031726A
CN115031726A CN202210324057.1A CN202210324057A CN115031726A CN 115031726 A CN115031726 A CN 115031726A CN 202210324057 A CN202210324057 A CN 202210324057A CN 115031726 A CN115031726 A CN 115031726A
Authority
CN
China
Prior art keywords
coordinate system
underwater robot
underwater
displacement
robot
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.)
Pending
Application number
CN202210324057.1A
Other languages
English (en)
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.)
Harbin Engineering University
Original Assignee
Harbin Engineering 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 Harbin Engineering University filed Critical Harbin Engineering University
Priority to CN202210324057.1A priority Critical patent/CN115031726A/zh
Publication of CN115031726A publication Critical patent/CN115031726A/zh
Pending legal-status Critical Current

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
    • 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
    • G01C21/1656—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 with passive imaging devices, e.g. cameras
    • 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/20—Instruments for performing navigational calculations
    • 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
    • Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
    • Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
    • Y02A90/30—Assessment of water resources

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)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种数据融合导航定位方法，步骤1：测量惯性坐标系下水下机器人相对于海底速度，对速度进行积分测得相对位移；步骤2：通过双目摄像机获取目标图片，通过双目测距分别获得相邻时刻水下机器人在双目坐标系下位置，然后得到相邻时刻在惯性坐标系下的位置，然后得到在惯性坐标系下的位移；步骤3：利用步骤1的相对位移和步骤2的在惯性坐标系下的位移值，采用变种NDT算法进行延时补偿，并对步骤2得到的位移进行补偿；步骤4：对补偿后的位移与相对位移进行融合，得出水下机器人抓取过程中的位置。本发明解决通过双目测距延时和频率低问题以及由于光照条件的变化无法连续测距的问题，满足水下机器人在抓取状态时导航定位要求。

Description

一种数据融合导航定位方法
技术领域
本发明涉及一种数据融合导航定位方法，尤其涉及一种水下机器人抓取运动过程的数据融合导航定位方法，属于水下导航定位技术领域。
背景技术
由于水下作业技术的困难，特别是对海参，海胆等水下目标的抓取，而且人类的潜水深度有限，水下机器人的水下作业技术的发展受到越来越广泛的关注。水下机器人抓取过程中的导航定位的精度与频率直接决定其能否抓取成功。不同的传感器在水下抓取过程中满足不同的定位要求，因此非常有必要进行数据融合，使其为水下机器人控制提供准确而且频率高的相对定位数据，使得水下机器人能够成功的对水下目标的进行抓取。
虽然国内将视觉信息与传统意义上的数据融合导航定位的方法已经进行了大量的研究，但原理和方法各有不同：本发明的实质是提出了一种水下机器人抓取运动过程的数据融合导航定位方法。与以往的融合导航定位方法有着明显的区别。一种基于多传感器融合的精确定位系统(CN201711242930.8)主要讨论了激光雷达、单天线GPS模块与九轴IMU进行联合定位，该研究方法并不适用于海洋环境。一种多传感器融合的视觉定位方法(CN202110797407.1)，该发明使用的算法对初始化有较高的要求，在低纹理等比较恶劣的环境进行初始化时会导致后面定位漂移变大，使得导航定位数据定位失真。一种视觉与惯性联合定位方法(CN201711122305.X)直接将视觉定位结果与惯性导航结果进行融合，该融合方法缺乏对视觉定位结果的可靠性分析，一旦视觉定位结果误差过大，将导致此后若干时间内融合定位精度下降。一种基于激光及视觉融合的机器人定位方法(CN202011593913.0)，该发明研究对象为激光和视觉，其方法是利用卡尔曼滤波进行数据融合，该方法中激光容易受到光源，高反射材质，低反光率物体，水下颗粒的影响，但发明中并未考虑到这些情况。
发明内容
针对上述现有技术，本发明要解决的技术问题是提供一种水下机器人抓取运动过程的数据融合导航定位方法，解决水下机器人在目标抓取过程中，通过双目测距的延时和频率低问题，传统的船位推算的累计误差，以及在抓取过程中由于光照条件的变化无法连续测距的问题，满足水下机器人在抓取状态时的导航定位要求。
为解决上述技术问题，本发明的一种数据融合导航定位方法，包括：
步骤1：测量惯性坐标系下水下机器人相对于海底的速度，利用多普勒测速仪对速度进行积分测得水下机器人的相对位移；
步骤2：通过双目摄像机获取目标图片，通过双目测距分别获得相邻时刻水下机器人在双目坐标系下位置，然后转换到目标坐标系下，再转换到惯性坐标系下，分别得到相邻时刻水下机器人在惯性坐标系下的位置，然后得到在惯性坐标系下的位移；
步骤3：利用步骤1得到的水下机器人相对位移和步骤2得到的水下机器人在惯性坐标系下的位移值，采用变种NDT算法进行延时补偿，并利用延时补偿对步骤2得到的位移进行补偿；
步骤4：对步骤3补偿后的位移与步骤1得到的相对位移进行融合，得出水下机器人抓取过程中的位置。
进一步的，步骤1具体为：
步骤1.1：利用磁罗经测得水下机器人的姿态，利用多普勒测速仪测得在载体坐标系下水下机器人相对与海底的速度
Figure BDA0003571089520000021
步骤1.2：使用扩展卡尔曼滤波对
Figure BDA0003571089520000022
进行滤波；
步骤1.3：将
Figure BDA0003571089520000023
转换到惯性坐标系下，得到在惯性坐标系下水下机器人相对于海底的速度：
Figure BDA0003571089520000024
其中，
Figure BDA0003571089520000025
为在惯性坐标系下水下机器人相对于海底的速度，
Figure BDA0003571089520000026
为从水下机器人坐标系转换到惯性坐标系下的转换矩阵；
步骤1.4：利用DVL对
Figure BDA0003571089520000027
进行积分测得水下机器人的相对位移。
进一步的，步骤3具体为：
步骤3.1：将水下机器人相对位移看作是参考点云，将参考点云所占空间划分成网格，并计算每个网格的正态分布参数均值和协方差矩阵：
Figure BDA0003571089520000028
Figure BDA0003571089520000029
其中，n表示网格中点云数量，Xi表示参考点云的数值；
步骤3.2：对变换参数p＝(tx ty φ)进行初始化设定，φ设定为零，tx,ty设定为延时时间TD，且tx＝ty；
步骤3.3：利用延时时间TD对步骤2得到的位移进行补偿：
xb＝xa+v×TD
其中，v为DVL测得水下机器人速度，xa为步骤2得到的位移值，xb为延时补偿后得到水下机器人的位移值；
步骤3.4：计算每个网格的概率密度：
Figure BDA0003571089520000031
计算NDT配准得分：
Figure BDA0003571089520000032
步骤3.5：score为最大值时结束计算，此时TD为双目匹配的延迟时间，xb为补偿后的位移。
进一步的，步骤4具体为：
步骤4.1：依据上一时刻的位置预测水下机器人在当前时刻的状态，假定水下目标是静止的，操纵水下机器人趋近目标；
预测阶段的系统状态方程
Figure BDA0003571089520000033
为：
Figure BDA0003571089520000034
其中
Figure BDA0003571089520000035
为由DVL、磁罗经测得的水下机器人在载体坐标系下x,y以及转角φ的变化，
Figure BDA0003571089520000036
为由水下机器人在惯性坐标系下上一时刻的位置与艏向角，水下目标在惯性坐标系下的坐标为
Figure BDA0003571089520000037
状态量的协方差为：
Figure BDA0003571089520000038
其中，雅可比矩阵J和Q为：
Figure BDA0003571089520000039
Figure BDA00035710895200000310
雅可比矩阵Jv和Qv为：
Figure BDA00035710895200000311
Figure BDA00035710895200000312
Figure BDA0003571089520000041
步骤4.2：将由延时补偿得出水下目标在双目坐标系下的位置，转换到载体坐标系下(xb,yb)，然后转换到水下机器人极坐标系下：
Figure BDA0003571089520000042
获得观测值为：
Figure BDA0003571089520000043
Figure BDA0003571089520000044
其中，R为观测协方差，设定为
Figure BDA0003571089520000045
将水下目标在惯性坐标系下的位置
Figure BDA0003571089520000046
转换到水下机器人极坐标系下为：
Figure BDA0003571089520000047
扩展卡尔曼滤波的增益Kn为：
Figure BDA0003571089520000048
Figure BDA0003571089520000049
Figure BDA00035710895200000410
上式中的矩阵Hn为：
Figure BDA00035710895200000411
式中Δx,Δy的值为：
Figure BDA00035710895200000412
更新后的状态方程
Figure BDA00035710895200000413
即机器人的位置，为：
Figure BDA00035710895200000414
Figure BDA00035710895200000415
其中：
Figure BDA0003571089520000051
本发明的有益效果：本发明通过对不同传感器的数据进行融合将满足水下抓取时的定位要求，使其既能为保证传统的导航定位要求，也能为水下机器人控制提供准确而且频率较高的相对定位数据，使得水下机器人能够成功的对水下目标的进行抓取。
本发明针对UVMS进行目标抓取时的定位问题，提出了基于变种的NDT算法的延时补偿技术。首先使用迭代扩展卡尔曼滤波技术对多普勒测速仪测得的机器人坐标系下的速度进行滤波，并对滤波以后的速度进行坐标转换，船位推算后将满足UVMS导航定位要求。但是为进行目标抓取任务，需要利用双目测距技术成功获取机器人相对于目标的距离，然后为解决单纯依靠双目测距定位存在频率较低和数据延迟，或单纯依靠船位推算精度低的问题，提出了基于变种的NDT算法利用船位推算结果对双目测距值进行延时补偿的方案，最后利用算法进行数据融合的解决方案。
本发明得优势在于利用变种的NDT算法解决水下机器人在目标抓取过程中，通过双目测距的延时和频率低问题。利用延时补偿后的算法解决传统意义上的导航定位技术存在较大误差以及无法满足水下机器人进行目标抓取式定位的要求的问题。延时补偿后的算法，既可以利用双目测距值修正传统导航定位的累计误差，又可以利用传统导航定位数据对双目测距值进行延时补偿，将满足抓取时的定位要求。
附图说明
图1是系统结构图；
图2是双目测距流程图；
图3是双目匹配结果图；
图4是双目测距结果图；
图5是结果分析图；
具体实施方式
下面结合附图与具体实施方式对本发明作进一步详细描述。
结合图1和图2，本发明包括：
步骤一：通过测量与计算得到惯性坐标系下水下机器人相对于海底的速度，利用DVL对速度进行积分测得机器人的相对位移：
1.利用磁罗经测得水下机器人的姿态，利用多普勒测速仪(DVL)作为水下导航的设备测得在机器人坐标系下水下机器人相对与海底的速度
Figure BDA0003571089520000052
2.针对水下机器人的横纵摇过大，或者水底有淤泥，以及水下机器人在经过鱼群等时，由于DVL发出的波束在以上情况下无法完全反射回来等情况，基于这些情况以及误差和高斯白噪声等，使用扩展卡尔曼滤波技术对测量的速度进行滤波，使速度在短时间内精度满足要求；
3.然后将其转换到惯性坐标系下，最后得到在惯性坐标系下水下机器人相对于海底的速度：
Figure BDA0003571089520000061
其中
Figure BDA0003571089520000062
为在惯性坐标系下水下机器人相对于海底的速度。
Figure BDA0003571089520000063
为从机器人坐标系转换到惯性坐标系下的转换矩阵，
Figure BDA0003571089520000064
为DVL测得的在机器人坐标系下水下机器人相对与海底的速度。
4.将
Figure BDA0003571089520000065
值利用DVL进行积分(传统船位推算方法)测得机器人在惯性坐标系下的相对位移；
步骤二：通过双目测距以及坐标转换得到机器人在惯性坐标系下的位移值:
1.通过双目摄像机获取目标图片；
2.进行摄像机标定，得到摄像机的内部参数以及外部参数；
3.对图像进行预处理，然后进行立体校正；
4.对校正过后的图像进行立体匹配，获得视差图；
5.通过视差图获得水下目标在双目坐标系下的位置，然后将其转换到水下目标坐标系下，得到水下机器人相对于水下目标的位置，然后再将水下机器人相对于水下目标的位置转换到惯性坐标系下，得到机器人在惯性坐标系下的位置；
6.重复步骤5获得两相邻时刻机器人在惯性坐标系下的位置，然后通过计算获得机器人相邻时刻在惯性坐标系下的位移。
步骤三：提出变种的NDT算法，利用DVL的积分结果与双目测距的结果进行延时补偿，解决双目测距由于匹配所带来的延时问题；
1.将利用DVL进行积分测得的机器人相对位移看作是参考点云，将参考点云所占空间划分成指定大小的网格，并将计算每个网格的正态分布参数均值和协方差矩阵：
Figure BDA0003571089520000066
Figure BDA0003571089520000067
其中，n表示网格中点云数量，Xi表示参考点云的数值。
2.对于延时补偿，将变换参数p进行初始化，利用p＝(tx ty φ)进行设定，在延时补偿中φ不需要进行设定，所以将其设定为零。将tx,ty(其中tx＝ty)设定为TD，作为双目由于左右匹配计算量大而产生的延时时间；
双目测距得到的机器人相对于目标的位置后，两次结果值之间做差与坐标转换后得到机器人的位移值，其可以由设定的延时时间进行补偿，双目测距延时补偿可以将双目测距得到的位移值看成是时间轴上的平移：
xb＝xa+v×TD
其中，v为DVL测得机器人速度，xa为双目测距得到机器人的位移值，xb为延时补偿后双目测距得到机器人的位移值。
3.根据DVL滤波与坐标转换后的积分值所求得的均值和协方差矩阵，与双目测距得到机器人的位移值，计算其每个网格的概率密度：
Figure BDA0003571089520000071
NDT配准得分由每个网格计算出的概率密度相加获得：
Figure BDA0003571089520000072
4.寻找合适的延迟时间TD，若使得上式中的score值最大，则结束计算，此时TD为双目匹配的延迟时间，经过步骤2计算后将得到延时补偿优化后的机器人的位移值；
步骤四：对延时补偿后的双目测距结果与DVL在发现目标以后的积分结果进行融合，得出精确的UVMS抓取过程中的位置；
1.预测阶段:
该过程是依据上一时刻的位置预测机器人在当前时刻的状态，在基于双目测距对水下目标的抓取时，假定水下目标是静止不动的，只是操纵水下机器人趋近目标；
预测阶段的系统状态方程
Figure BDA0003571089520000073
由下式计算给出：
Figure BDA0003571089520000074
其中
Figure BDA0003571089520000075
为由DVL、磁罗经测得的机器人在机器人坐标系下x,y以及转角φ的变化，
Figure BDA0003571089520000076
为由机器人在惯性坐标系下上一时刻的位置与艏向角，由于本发明是对于水下目标的抓取，所以假设只是对一个目标进行连续的观测，所以水下目标在惯性坐标系下的坐标为
Figure BDA0003571089520000077
状态量的协方差为：
Figure BDA0003571089520000081
其中雅可比矩阵J和Q为：
Figure BDA0003571089520000082
Figure BDA0003571089520000083
雅可比矩阵Jv和Qv为：
Figure BDA0003571089520000084
Figure BDA0003571089520000085
Figure BDA0003571089520000086
2.由延时补偿得出水下目标在双目坐标系下的位置，将其转换到机器人坐标系下为(xb,yb)，然后通过下式进行坐标变换(将其转换在机器人极坐标系下)：
Figure BDA0003571089520000087
由此可获得观测值为
Figure BDA0003571089520000088
Figure BDA0003571089520000089
R为观测协方差。
预测和观测过程均存在偏差，偏差是均值为零、方差为1的正态分布随机数，所以将观测协方差R设定为：
Figure BDA00035710895200000810
将水下目标在惯性坐标系下的位置
Figure BDA00035710895200000811
转换到机器人极坐标系下，则水下目标在机器人极坐标系下的坐标为：
Figure BDA00035710895200000812
由于在抓取过程中只是对一个目标，所以扩展卡尔曼滤波的增益Kn由下式计算可得：
Figure BDA0003571089520000091
Figure BDA0003571089520000092
Figure BDA0003571089520000093
上式中的矩阵Hn为
Figure BDA0003571089520000094
式中Δx,Δy的值为
Figure BDA0003571089520000095
更新后的状态方程
Figure BDA0003571089520000096
(机器人的位置)为
Figure BDA0003571089520000097
Figure BDA0003571089520000098
其中
Figure BDA0003571089520000099
图1是系统结构图，也就是导航定位流程图，利用普勒测速仪(DVL)与磁罗经作为水下导航的设备测得在机器人坐标系下水下机器人相对与海底的速度和姿态，使用迭代扩展卡尔曼滤波技术进行滤波，使速度在短时间内满足精度要求，然后将其装换到惯性坐标系下，最后得到在大地坐标系下水下机器人相对于海底的速度,将速度值利用DVL的积分可以测得机器人的相对位移。图2是双目测距流程图，通过双目相机可以获得水下生物图片，然后经过双目测距流程图的具体操作，可以获得双目匹配结果图3，经过匹配测量可以得到视差图，通过视差图可以得到机器人到水下生物的距离，对数据(每一时刻机器人到水下生物的距离)进行整理最终获得双目测距结果图4，通过图4可以得到机器人到水下生物的距离，两次测距值之间做差，然后转换到惯性坐标系下后，得到机器人在惯性坐标系下的位移值。然后利用上述中的DVL的积分与双目测距的结果进行延时补偿(提出变种的NDT算法)，最后基于算法对双目测距结果与DVL在发现目标以后的积分结果进行融合，得到更为精确的机器人相对于抓取目标的位置(图5)。

Claims (4)

1.一种数据融合导航定位方法，其特征在于，包括：
步骤1：测量惯性坐标系下水下机器人相对于海底的速度，利用多普勒测速仪对速度进行积分测得水下机器人的相对位移；
步骤2：通过双目摄像机获取目标图片，通过双目测距分别获得相邻时刻水下机器人在双目坐标系下位置，然后转换到目标坐标系下，再转换到惯性坐标系下，分别得到相邻时刻水下机器人在惯性坐标系下的位置，然后得到在惯性坐标系下的位移；
步骤3：利用步骤1得到的水下机器人相对位移和步骤2得到的水下机器人在惯性坐标系下的位移值，采用变种NDT算法进行延时补偿，并利用延时补偿对步骤2得到的位移进行补偿；
步骤4：对步骤3补偿后的位移与步骤1得到的相对位移进行融合，得出水下机器人抓取过程中的位置。
2.根据权利要求1所述的一种数据融合导航定位方法，其特征在于：步骤1具体为：
步骤1.1：利用磁罗经测得水下机器人的姿态，利用多普勒测速仪测得在载体坐标系下水下机器人相对与海底的速度
Figure FDA0003571089510000011
步骤1.2：使用扩展卡尔曼滤波对
Figure FDA0003571089510000012
进行滤波；
步骤1.3：将
Figure FDA0003571089510000013
转换到惯性坐标系下，得到在惯性坐标系下水下机器人相对于海底的速度：
Figure FDA0003571089510000014
其中，
Figure FDA0003571089510000015
为在惯性坐标系下水下机器人相对于海底的速度，
Figure FDA0003571089510000016
为从水下机器人坐标系转换到惯性坐标系下的转换矩阵；
步骤1.4：利用DVL对
Figure FDA0003571089510000017
进行积分测得水下机器人的相对位移。
3.根据权利要求1所述的一种数据融合导航定位方法，其特征在于：步骤3具体为：
步骤3.1：将水下机器人相对位移看作是参考点云，将参考点云所占空间划分成网格，并计算每个网格的正态分布参数均值和协方差矩阵：
Figure FDA0003571089510000018
Figure FDA0003571089510000019
其中，n表示网格中点云数量，Xi表示参考点云的数值；
步骤3.2：对变换参数p＝(tx ty φ)进行初始化设定，φ设定为零，tx,ty设定为延时时间TD，且tx＝ty；
步骤3.3：利用延时时间TD对步骤2得到的位移进行补偿：
xb＝xa+v×TD
其中，v为DVL测得水下机器人速度，xa为步骤2得到的位移值，xb为延时补偿后得到水下机器人的位移值；
步骤3.4：计算每个网格的概率密度：
Figure FDA0003571089510000021
计算NDT配准得分：
Figure FDA0003571089510000022
步骤3.5：score为最大值时结束计算，此时TD为双目匹配的延迟时间，xb为补偿后的位移。
4.根据权利要求1所述的一种数据融合导航定位方法，其特征在于：步骤4具体为：
步骤4.1：依据上一时刻的位置预测水下机器人在当前时刻的状态，假定水下目标是静止的，操纵水下机器人趋近目标；
预测阶段的系统状态方程
Figure FDA0003571089510000023
为：
Figure FDA0003571089510000024
其中
Figure FDA0003571089510000025
为由DVL、磁罗经测得的水下机器人在载体坐标系下x,y以及转角φ的变化，
Figure FDA0003571089510000026
为由水下机器人在惯性坐标系下上一时刻的位置与艏向角，水下目标在惯性坐标系下的坐标为
Figure FDA0003571089510000027
状态量的协方差为：
Figure FDA0003571089510000028
其中，雅可比矩阵J和Q为：
Figure FDA0003571089510000029
Figure FDA00035710895100000210
雅可比矩阵Jv和Qv为：
Figure FDA00035710895100000211
Figure FDA0003571089510000031
Figure FDA0003571089510000032
步骤4.2：将由延时补偿得出水下目标在双目坐标系下的位置，转换到载体坐标系下(xb,yb)，然后转换到水下机器人极坐标系下：
Figure FDA0003571089510000033
获得观测值为：
Figure FDA0003571089510000034
Figure FDA0003571089510000035
其中，R为观测协方差，设定为
Figure FDA0003571089510000036
将水下目标在惯性坐标系下的位置
Figure FDA0003571089510000037
转换到水下机器人极坐标系下为：
Figure FDA0003571089510000038
扩展卡尔曼滤波的增益Kn为：
Figure FDA0003571089510000039
Figure FDA00035710895100000310
Figure FDA00035710895100000311
上式中的矩阵Hn为：
Figure FDA00035710895100000312
式中Δx,Δy的值为：
Figure FDA00035710895100000313
更新后的状态方程
Figure FDA00035710895100000314
即机器人的位置，为：
Figure FDA00035710895100000315
Figure FDA0003571089510000041
其中：
Figure FDA0003571089510000042
CN202210324057.1A 2022-03-29 2022-03-29 一种数据融合导航定位方法 Pending CN115031726A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210324057.1A CN115031726A (zh) 2022-03-29 2022-03-29 一种数据融合导航定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210324057.1A CN115031726A (zh) 2022-03-29 2022-03-29 一种数据融合导航定位方法

Publications (1)

Publication Number Publication Date
CN115031726A true CN115031726A (zh) 2022-09-09

Family

ID=83119298

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210324057.1A Pending CN115031726A (zh) 2022-03-29 2022-03-29 一种数据融合导航定位方法

Country Status (1)

Country Link
CN (1) CN115031726A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116295020A (zh) * 2023-05-22 2023-06-23 山东高速工程检测有限公司 一种桥梁病害定位方法及定位装置

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103968838A (zh) * 2014-05-09 2014-08-06 哈尔滨工程大学 一种基于极坐标系的auv曲线运动状态下的协同定位方法
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN112197765A (zh) * 2020-09-14 2021-01-08 浙江大学 一种实现水下机器人精细导航的方法
CN113074725A (zh) * 2021-05-11 2021-07-06 哈尔滨工程大学 一种基于多源信息融合的小型水下多机器人协同定位方法及系统

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103968838A (zh) * 2014-05-09 2014-08-06 哈尔滨工程大学 一种基于极坐标系的auv曲线运动状态下的协同定位方法
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN112197765A (zh) * 2020-09-14 2021-01-08 浙江大学 一种实现水下机器人精细导航的方法
CN113074725A (zh) * 2021-05-11 2021-07-06 哈尔滨工程大学 一种基于多源信息融合的小型水下多机器人协同定位方法及系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
李加强: "基于视觉辅助定位的水下机器人目标抓取控制" *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116295020A (zh) * 2023-05-22 2023-06-23 山东高速工程检测有限公司 一种桥梁病害定位方法及定位装置
CN116295020B (zh) * 2023-05-22 2023-08-08 山东高速工程检测有限公司 一种桥梁病害定位方法及定位装置

Similar Documents

Publication Publication Date Title
Wu et al. Survey of underwater robot positioning navigation
CN113340295B (zh) 一种多个测距传感器的无人艇近岸实时定位与建图方法
CN107314768B (zh) 水下地形匹配辅助惯性导航定位方法及其定位系统
Roman et al. Application of structured light imaging for high resolution mapping of underwater archaeological sites
CN109633664B (zh) 基于rgb-d与激光里程计的联合定位方法
Teixeira et al. Underwater inspection using sonar-based volumetric submaps
CN109239709B (zh) 一种无人船的局部环境地图自主构建方法
Quist et al. Radar odometry with recursive-RANSAC
Meduna et al. Low-cost terrain relative navigation for long-range AUVs
CN111273298B (zh) 基于波浪滑翔器组网技术的水下声学目标定位与跟踪方法
CN109579850B (zh) 基于对水速度辅助惯导的深水智能导航方法
CN111829512A (zh) 一种基于多传感器数据融合的auv导航定位方法及系统
CN112556697A (zh) 一种基于联邦结构的浅耦合数据融合导航方法
CN111090283A (zh) 一种无人艇组合定位定向方法和系统
Han et al. GPS-less coastal navigation using marine radar for USV operation
CN114061565B (zh) 一种无人船舶slam及其应用方法
CN114077249B (zh) 一种作业方法、作业设备、装置、存储介质
CN115031726A (zh) 一种数据融合导航定位方法
Inzartsev et al. Underwater pipeline inspection method for AUV based on laser line recognition: Simulation results
CN112611376A (zh) 一种RGI-Lidar/SINS紧耦合AUV水下导航定位方法与系统
CN115930948A (zh) 一种果园机器人融合定位方法
Zeng et al. Integrating monocular vision and laser point for indoor UAV SLAM
CN115031718A (zh) 一种多传感器融合的无人船同步定位与建图方法(slam)及系统
RU2684710C1 (ru) Система коррекции ошибок инс летательного аппарата по дорожной карте местности
Ma et al. A review: The survey of attitude estimation in autonomous uav navigation

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
RJ01 Rejection of invention patent application after publication
RJ01 Rejection of invention patent application after publication

Application publication date: 20220909