一种用于管道潜航机器人的DVL标定方法

本发明涉及一种用于管道潜航机器人的dvl标定方法，属于管道潜航机器人导航定位检测领域。

背景技术：

1、地下管网是城市赖以生存和发展的“生命线”，其保证了我们日常生活和工业等方面的正常运转。随着城市基础建设越来越完善，地下管网规模也越来越庞大复杂，排水、排污、运输石油天然气等各种管道纵横交错。当地下管道发生缺陷(裂纹、腐蚀、凹陷等)时往往伴随着巨大事故的发生，这不仅造成一定经济损失，还可能危害人身安全，因此定期对管道进行缺陷检测尤其重要，以确保管道的安全稳定地使用。由于城市管道大多都深埋于地下且管网的分布愈加复杂，使用人工方式对管道维护和管道缺陷的检测定位较为困难，而管道潜航机器人可以进入到管道内部进行检测，大大提高了检测效率，并且潜航机器人具有在水下连续工作时间长、工作效率稳定的特点，完美地实现管道检测工作的自主性并代替人完成一部分的作业任务。为了保障管道潜航机器人精准到达缺陷位置进行检测作业，使其潜航器具备高精度导航是很有必要的。

2、管道潜航机器人通常在有水的管道环境下作业，大多采用捷联惯性导航系统(sins)和多普勒计程仪(dvl)组合导航的方式进行导航定位，sins具有良好的独立性、抗干扰性和隐蔽性等优点，但其导航解算结果会随时间的增长而累积误差，而dvl的测量速度精度高和无误差累积的特点可以很好地弥补抑制sins的导航误差，使得管道潜航机器人的导航定位精度大幅度提升。然而dvl的测速误差会影响sins/dvl组合导航精度，其误差来源主要是dvl安装时存在安装角偏差以及自身的比例因子误差，因此在组合导航前需要对dvl进行误差标定以提高导航的精度。现存的部分标定方法采用sins/dvl/gnss(全球导航卫星系统)组合导航方式，当gnss信号有效时，以gnss的输出信息作为观测量，利用sins/gnss组合导航之后的姿态、速度和位置信息并基于卡尔曼滤波器对dvl误差进行在线迭代标定，但由于引入了dvl的量测噪声对标定结果产生影响，使得标定结果呈现噪声特性，不能得到很好的标定精度。

技术实现思路

1、本发明的目的在于克服现有技术中的不足，提供一种用于管道潜航机器人的dvl标定方法，使管道潜航机器人在进行sins/dvl组合导航前快速精确标定出dvl的比例因子误差和安装误差角，以此提高机器人导航精度，从而能够准确高效定位到管道缺陷位置进行检测工作，确保管道检测工作顺利高效率完成，同时解决了卡尔曼滤波在线标定方法存在的问题。

2、为达到上述目的，本发明是采用下述技术方案实现的：

3、第一方面，本发明提供了一种用于管道潜航机器人的dvl标定方法，包括：

4、获取步骤，包括：获取潜航机器人完成初始对准后，开始运动航行时的导航信息；

5、根据获得的导航信息，建立速度或位置比例因子误差模型，再通过误差模型构建速度或位置观测方程；

6、根据速度或位置观测方程构建姿态估计目标函数，采用lm算法对dvl进行误差标定，如果标定时间满足所设定的标定时间，则完成管道潜航机器人的dvl误差标定，否则返回获取步骤更新迭代数据。

7、进一步的，所述导航信息包括dvl速度信息输出以及捷联惯性导航系统/全球导航卫星系统sins/gnss组合导航系统的速度姿态和位置信息，若潜航机器人位于满水管道时，接收不到gnss信号，则采集sins的速度、姿态、位置信息和dvl速度信息。

8、进一步的，将所述导航信息经过卡尔曼滤波器融合估计得到更为精确的导航数据，并将导航坐标系下速度转换至载体坐标系，从而得到组合导航系统在载体系下的速度并求解姿态矩阵表示导航坐标系到载体坐标系的姿态转化矩阵，b表示载体系，n表示导航系，sins/gnss组合导航系统载体坐标系下速度为：

9、进一步的，所述dvl速度信息由dvl测速原理得出，当潜航机器人进行导航运动时，dvl测量得到的速度为：其中为dvl的输出速度，ζ为dvl比例因子误差，为安装误差角方向余弦矩阵，为dvl与sins之间杠杆臂在b系下的投影，为b系相对于n系的旋转角速度在b系上的投影。

10、进一步的，所述速度或位置比例因子误差模型，具体为：

11、速度比例因子误差模型为：其模型是对dvl测速方程两边取模运算，利用速度观测得到；

12、位置观测的比例因子误差为：式中dvl的位置信息根据dvl测速方程对dvl进行位置计算得到，管道潜航机器人位置利用dvl采样间隔内sins/gnss组合导航解算出n个数据得到，其中δtd为dvl采样时间间隔；为k-1时刻到k时刻sins/gnss在b系下测量的第i个速度输出；n为sins/gnss组合导航在δtd时间内解算出的数据个数。

13、进一步的，所述速度或位置观测方程为当管道潜航机器人在满水环境下构建基于速度观测方程其中，否则构建基于位置观测方程其中

14、进一步的，所述姿态估计目标函数由基于levenberg-marquardt四元数估计理论的姿态确定算法得出，其目标函数为其中表示b系到d系的转换四元数，表示的共轭四元数。

15、进一步的，所述采用lm算法对dvl进行误差标定为基于lm算法求解目标函数来计算安装角误差，通过更新xk+1＝xk+hlm使得满足目标函数minf(xk+1)，其中f(xk+1)为具体步骤为：

16、(1)确定目标函数minf(xk+1)，给定初值x0，标定时间kmax和数值ε1,ε2,τ，得到信赖域半径μ＝τ·max{aii}。其中aii为j(x0)tj(x0)，j(x)是函数f(x)的雅可比矩阵函数；

17、(2)如果||g||∞≤ε1，则跳转到步骤(6)，否则继续执行下一步，其中g＝j(x)f(x)；

18、(3)根据增量方程(j(x)j(x)t+μi)hlm＝-g求解hlm；

19、(4)如果||hlm||≤ε2(||x||+ε2)，则跳转到步骤(6)，否则迭代xk+1＝xk+hlm并计算近似指标

20、(5)如果ρ>0.75,μ＝2·μ；如果ρ<0.25，μ＝μ/2，回到步骤(2)；否则执行下一步；

21、(6)判断标定时间是否满足所设定的标定时间kmax，若是则完成dvl标定过程，否则回到步骤(2)更新数据继续执行。

22、第二方面，本发明提供一种用于管道潜航机器人的dvl标定系统，包括：

23、获取模块，用于获取潜航机器人完成初始对准后，开始运动航行时的导航信息；

24、模型建立模块，用于根据获得的导航信息，建立速度或位置比例因子误差模型，再通过误差模型构建速度或位置观测方程；

25、误差标定模块，用于根据速度或位置观测方程构建姿态估计目标函数，采用lm算法对dvl进行误差标定，如果标定时间满足所设定的标定时间，则完成管道潜航机器人的dvl误差标定，否则返回获取模块更新迭代数据。

26、第三方面，本发明提供一种用于管道潜航机器人的dvl标定装置，包括处理器及存储介质；

27、所述存储介质用于存储指令；

28、所述处理器用于根据所述指令进行操作以执行根据前述任一项所述方法的步骤。

29、与现有技术相比，本发明所达到的有益效果：

30、本发明提供一种用于管道潜航机器人的dvl标定方法，在sins/gnss组合导航系统经过kf滤波后，得到的更为准确的数据，有利于减少其他传感器误差对dvl误差标定结果的影响；采用的lm算法具有梯度下降法和牛顿法的优点，能够更加快速精确得到标定结果，而且构建的位置观测方程能够减少dvl速度量测噪声的影响，从而提高标定结果的精度。