[发明专利]一种水下管道位置坐标测量方法

权利要求书

1.一种水下管道位置坐标测量方法，其特征在于，包括：

设置在水下机器人(2)上的定位获取装置(1)首先获取水面上第一位置(3)的第一坐标位置数据；

水下机器人(2)从第一位置(3)下潜至水中第二位置(4)，同时设置在所述水下机器人(2)上的惯导系统采集第一路径(7)的第一运动数据,包括水下机器人(2)的姿态角信息和线速度信息；水下机器人(2)到达第二位置(4)后，根据公式(3)至公式(14)结合所述第一坐标位置数据和所述第一运动数据得出管道坐标P1，包括经度、维度和高程；其中，所述第二位置4在所述被测管道正上方河床上；

水下机器人(2)从第二位置(4)上浮至水面第三位置(5)，并通过所述定位获取装置(1)获取所述第三位置(5)的第二位置信息；所述水下机器人(2)从第二位置(4)上浮至水面第三位置(5)的过程中，惯导系统采集第二路径(8)的第二运动数据；根据公式(3)至公式(14)结合所述第二位置信息和所述第二运动数据得出管道坐标P2；

根据公式(15)对第一坐标数据和第二坐标数据求平均值，得出所述管道的位置坐标；

具体计算如下：

惯性器件敏感载体的运动信息，其中加表输出的是载体在b系中的线加速度；陀螺输出的是载体在惯性空间的角速度；则需要获取载体在n系中的运动路径，必须将线加速度数据与角速度数据转换成n系下的表达；根据矢量旋转前后的坐标表达，可用该矢量在坐标系中的投影与其旋转角度构成的三角函数表达，矢量r跟随坐标系XOY旋转X′OY′坐标系，矢量r的模不变，但是r’在坐标系XOY的表达发生变化；设这个旋转关系是由n系旋转至b系，则有：

式中：r_n为在n系下的一个矢量，r_b为b系下的一个矢量；

为矢量r_b在n系下的投影转换矩阵；

C_Z、C_Y、C_X分别是载体绕Z轴、Y轴、X轴旋转时的转换矩阵；

根据公式(1)、(2)可以推导出方向余弦的微分方程，考虑到高精度惯导组的采样频率高达数百赫兹以上，并且水下机器人(ROV)在水中的角机动并不激烈；不可交换误差极其微小，姿态更新解算可以直接使用罗德里格旋转公式，即为：

ω_e＝[0 ω_ecosL ω_esinL]^T    (7)

式中，m与m-1为姿态更新的时间间隔t中的区间端点；

(θ_m×)为θ_m构成的反对称矩阵；

ω_i为陀螺输出的角速度，ω_b为载体相对于b系的角速度，ω_e为地球自转分量，ω_n为载体运动相对于地球的角速度；

V_N、V_E分别是载体在n系中的北向速度与东向速度；

R₁为载体在其位置的子午圈曲率半径，R₀为载体在其位置的卯酉圈曲率半径；

H为高程；

根据公式(1)至公式(8)，可以实时更新水下机器人(ROV)的姿态信息；

载体速度解算方法

从哥氏加速度推导出载体在地球表面运动时线加速度的比力方程为：

理论上，微分方程求数值解的精度比方程解要低，但是考虑到积分步长足够小以及

水下机器人(ROV)的运动速度不大，(2ω_e+ω_n)V_n可以忽略不计，且在高精度的惯导组中，使用数值解算也能满足精度要求；速度更新算法为：

式中：V_n为载体在n系中的线速度；

g为重力加速度，可以使用达郎贝尔方程获得；

f为加速度计的测量值；

载体位置更新

分析可以得出经纬度变化为：

式中：λ₀为初始经度，L₀为初始纬度，H₀为初始高程；在第一位置3或第二位置4通过定位获取装置获取的位置信息；

λ_BDS、L_BDS、H_BDS分别是定位获取装置1获取北斗卫星系统提供的坐标数据，分别是经度、纬度、高程，此处的坐标数据为水下机器人在路径7或路径8中运动过程中获取的坐标数据(经度、纬度、高程)；

V_U为载体在n系中的向天速度；

公式(14)含义是经纬、纬度、高程的初始值是从北斗卫星系统获取；

式中：P为测量点的最终坐标信息；

P₁为ROV从水面3下潜到水底4后，对水底4的位置解算结果；

P₂为ROV从水底4到水面5过程后，对水底4的位置解算结果。