[发明专利]自主水下航行器多普勒计程仪安装误差的校正方法

权利要求书

1.一种自主水下航行器多普勒计程仪安装误差的校正方法，其特征在于，步骤如下：

1）数据采集：利用全球定位系统获取AUV在水面时的东北向位置信息，深度计获取AUV的深度信息，利用DVL采集AUV的速度信息，利用姿态传感器获取姿态信息；

2）滤波估计：根据带误差校正的DVL航位推算模型，取AUV三维位置信息及DVL三维安装偏角为系统状态向量，DVL测得速度信息及姿态传感器测得姿态信息为系统输入向量，取AUV三维位置信息为测量向量，建立离散时间系统状态方程和测量方程；采用改进的平方根容积卡尔曼滤波进行滤波估计，得到DVL安装偏角的估计值；

3）误差校正：根据安装偏角的估计值得到DVL安装误差校正矩阵，用于AUV导航系统校准，提高AUV导航精度。

2.根据权利要求1所述的校正方法，其特征在于，步骤2）中，所述的带误差校正的DVL航位推算模型推导如下：

根据载体坐标系与DVL仪器坐标系的转换关系，存在安装偏差时，DVL测量速度的校正公式可表示为：

vb=Rdb(Π)·vd---(1)]]>

其中，为DVL测量得到的三维速度，为载体速度，为安装误差校正矩阵，即DVL仪器坐标系向载体坐标系转换的旋转矩阵，Π=[α β γ]^T为仪器坐标系相对于载体坐标系的横滚、纵倾和航偏三维偏角，当DVL和姿态传感器安装固定后，三维偏角为固定值，安装误差校正矩阵为：

Rdb(Π)=cosγcosβ-sinγcosα+cosγsinβsinαsinγsinα+cosγcosβsinαsinγcosβcosγcosα+sinγsinβsinα-cosγsinα+sinγsinβcosα-sinβcosβsinφcosβcosα---(2)]]>

根据三维姿态信息，将载体移动速度投影到导航坐标系，在前一刻推算得到的位置信息上进行累加得到下一时刻的推算位置信息，得到带误差校正的DVL航位推算模型数学表示为：

vn=Rbn(Θ)·vb---(3)]]>

pn(k)=pn(k-1)+Rbn(Θ(k-1))·Rdb·vd(k-1)·Δt---(4)]]>

其中，pⁿ(k)＝[x y z]^T为k时刻推算得到的三维位置信息，Δt为航位推算时间间隔，Θ=[φ θ ψ]^T为姿态传感器测得的载体横滚、纵倾和航偏角信息，为载体坐标系向导航坐标系转换的旋转矩阵，表示为：

Rbn(Θ)=cosψcosθ-sinψcosφ+cosψsinθsinφsinψsinφ+cosψcosθsinφsinψcosθcosψcosφ+sinψsinθsinφ-cosψsinφ+sinψsinθcos-sinθcosθsinφcosθcosφφ---(5)]]>