[发明专利]一种AUV推位导航系统杆臂误差标校方法

权利要求书

1.一种AUV推位导航系统杆臂误差标校方法，其特征在于，包括以下步骤：

步骤1、确定AUV的运动方式为回转运动；

步骤1.1、AUV上DVL位于艏部，AUV载体系b的原点o_b与AUV质心重合，x_b轴指向艇艏方向，y_b轴垂直于x_b轴，指向右舷方向，z_b轴垂直于x_b-y_b平面，满足右手定则向下，R_CA为载体系下DVL安装点距质心距离，杆臂长度矢量为存在杆臂误差；

步骤1.2、AUV在水下航行时，会产生角运动，为A点在载体坐标系下DVL的输出值，V_bx是在A点线速度沿载体坐标系x_b轴的分量，V_by是在A点线速度沿载体坐标系y_b轴的分量，ω_bx，ω_by，ω_bz分别是在A点的角速度在载体坐标系的投影分量；

步骤1.3、方位随动水平坐标系b'的原点o_b′与载体系b原点重合，x_b′轴方向与AUV艏向一致，且与y_b′构成当地水平坐标系，z_b′轴铅垂向下，三者满足右手定则；

步骤1.4、R为在方位随动坐标系AUV做回转运动的半径，AUV回转运动方向与z_b′轴同向顺时针旋转为正，回转半径在方位随动水平坐标系下的坐标为与z_b′轴反向逆时针旋转时为负，回转半径在方位随动水平坐标系下的坐标为γ为AUV运动时的横滚角，θ为AUV运动时的纵倾角；

步骤2、根据AUV回旋运动的特点，对AUV回转运动的系统进行分析，构建适用于卡尔曼滤波算法的杆臂误差模型；

步骤2.1、对回转运动系统进行分析，当AUV不存在横摇和纵摇运动时，在载体坐标系C点处的线速度为：而所以在载体坐标系下载体系下，沿AUV的载体系横滚轴正向的杆臂坐标为则A点处载体系下的线速度误差为

步骤2.2、对回转运动系统进行分析，当AUV做回旋运动时存在纵摇和横摇运动，所以载体坐标系b下AUV的回旋线速度为其中设在方位随动水平坐标系下R_DC的坐标为则在载体系下，AUV的回旋线速度为又因此DVL的输出值，即A点速度表示为：又载体系b下R_CA的坐标为即得假设得

步骤3、确定系统状态量、观测量，构建状态方程和观测方程，根据卡尔曼滤波对AUV的杆臂长度进行最优估计；

步骤4、根据AUV杆臂长度的最优估计，标校由杆臂效应引起的推位导航系统定位误差。

2.根据权利要求1所述一种AUV推位导航系统杆臂误差标校方法，其特征在于，所述步骤3具体包括以下步骤：

步骤3.1、杆臂长度以作为系统的状态变量建立卡尔曼滤波方程，则状态方程为：w为满足零均值正态分布的高斯白噪声，观测方程为v为满足零均值正态分布的高斯白噪声；

步骤3.2、根据卡尔曼滤波对AUV推位导航系统中的杆臂长度R_CA进行最优估计。

3.根据权利要求2所述一种AUV推位导航系统杆臂误差标校方法，其特征在于，所述步骤4具体包括以下步骤：

步骤4.1、由步骤3得杆臂长度R_CA的最优估计值，得A点处的速度误差

步骤4.2、根据得到A点的速度通过补偿A点处的速度误差得到C点线速度