[发明专利]一种惯性/DVL/USBL安装误差标定方法

权利要求书

1.一种惯性/DVL/USBL安装误差标定方法，其特征在于，包括如下步骤：

首先，定义坐标系：

a)导航坐标系n：O_XnYnZn，惯导质心_北天东，原点在惯导质心上；

b)载体坐标系b：O_XbYbZb，惯导质心_前上右；

c)DVL设备坐标系s：原点在DVL质心，x_s沿DVL纵轴方向，z_s与DVL横轴一致，y_s沿DVL竖轴向上，x_sy_sz_s构成右手坐标系；

d)USBL基阵坐标系a：原点在USBL基阵中心，x_a沿USBL基阵纵轴方向，z_a与USBL基阵横轴一致，y_a沿USBL基阵竖轴向上，x_ay_az_a构成右手坐标系；

然后，进行误差标定，包括如下步骤：

步骤1、惯导/DVL安装误差标定；

首先，沿着指定的第一航段、GPS速度、DVL速度采集，然后，将惯导/DVL绕垂向轴旋转90°，沿指定的第二航段采集GPS速度、DVL速度；利用两次采集的数据计算DVL坐标系至载体系转换矩阵；

步骤2、惯导/USBL安装误差标定；

首先，沿指定的第一环形航迹绕USBL水下应答器航行，并且采集GPS位置、USBL定位数据，然后，船体反向航行，沿与所述第一环形航迹航迹形状相同、方向相反的第二环形航迹，绕水下应答器采集GPS位置、USBL定位数据；利用两次采集的数据计算USBL基阵坐标系至载体系转换矩阵。

2.如权利要求1所述的一种惯性/DVL/USBL安装误差标定方法，其特征在于，

在步骤1中，将GPS速度Vⁿ投影至载体系b：

其中为惯导解算姿态阵；

载体系至DVL坐标系转换矩阵则DVL速度和GPS速度在载体系投影有如下关系：

惯导与DVL刚性固定连接，二者相对位置关系不变，待求量是常值矩阵；

沿指定的第一航段采集GPS速度、DVL速度，将GPS速度向载体系b投影，计算二者速度均值

其中：

k为离散时间点；

n为速度采样总数；

将惯导/DVL绕垂向轴旋转90°，沿指定的第二航段采集GPS速度、DVL速度，将GPS速度向载体系b投影，计算二者速度均值

其中：

k为离散时间点；

m为速度采样总数；

式(3)、(4)速度均值满足如下关系：

构造单位矢量i_b，j_b，k_b和i_s，j_s，k_s：

则DVL坐标系至载体系转换矩阵可按下式计算：

如此，得到了DVL安装误差；

在步骤2中，设USBL应答器在基阵内的相对位置测量值为L^a，基阵坐标系中心和应答器的大地坐标分别为P、P_t，为惯导解算姿态阵，为基阵坐标系到载体系的转换矩阵，由于惯导与USBL基阵固联，为常值矩阵；根据以上条件可以计算应答器相对基阵中心的位置增量：

ΔP＝P_t-P (9)

将位置增量转换为导航系n内的距离Lⁿ：

其中L为当地纬度；

R_M为子午圈曲率半径；

R_N卯酉圈曲率半径；

通过位置增量计算得出的距离相对地球半径很小，对上述距离作圆弧—直线近似，则Lⁿ可认为是应答器在当地地理坐标系内的坐标，该坐标与应答器在基阵坐标系内的坐标有如下关系：

进而

其中L^b是应答器在载体系b内坐标；

船载GPS可提供精确的位置测量，补偿掉USBL相对GPS的杆臂位置后可作为USBL安装误差标定的位置基准；

沿指定的第一环形航迹绕水下应答器采集GPS位置、USBL定位数据，计算及其均值

其中：

k为离散时间点；

n为位置采样总数；

船体反向航行，沿与所述第一环形航迹航迹形状相同、方向相反的第二环形航迹绕水下应答器采集GPS位置、USBL定位数据，计算及其均值

其中：

k为离散时间点；

m为位置采样总数；

式(13)、(14)位置均值满足如下关系：

构造单位矢量i_b，j_b，k_b和i_a，j_a，k_a：

则基阵坐标系到载体系转换矩阵可按下式计算：

如此，得到了USBL基阵安装误差。