自主水下航行器多普勒计程仪安装误差的校正方法

摘要

本发明公开自主水下航行器多普勒计程仪安装误差的校正方法。1）数据采集：获取AUV在水面时的东北向位置信息、深度信息、速度信息、姿态信息；2）滤波估计：根据带误差校正的DVL航位推算模型，取AUV三维位置信息及DVL三维安装偏角为系统状态向量，DVL测得速度信息及姿态传感器测得姿态信息为系统输入向量，取AUV三维位置信息为测量向量，建立离散时间系统状态方程和测量方程；采用改进的平方根容积卡尔曼滤波进行滤波估计，得到DVL安装偏角的估计值；3）误差校正：根据安装偏角的估计值得到DVL安装误差校正矩阵。本发明操作简单，无需外部设备辅助，可有效估计出DVL的三维安装偏差角，从而消除DVL安装误差对AUV导航精度的影响，具有很强的实用价值。

主权项

一种自主水下航行器多普勒计程仪安装误差的校正方法，其特征在于，步骤如下：1)数据采集：利用全球定位系统获取AUV在水面时的东北向位置信息，深度计获取AUV的深度信息，利用DVL采集AUV的速度信息，利用姿态传感器获取姿态信息；2)滤波估计：根据带误差校正的DVL航位推算模型，取AUV三维位置信息及DVL三维安装偏角为系统状态向量，DVL测得速度信息及姿态传感器测得姿态信息为系统输入向量，取AUV三维位置信息为测量向量，建立离散时间系统状态方程和测量方程；采用改进的平方根容积卡尔曼滤波进行滤波估计，得到DVL安装偏角的估计值；3)误差校正：根据安装偏角的估计值得到DVL安装误差校正矩阵，用于AUV导航系统校准，提高AUV导航精度；步骤2)中，所述的带误差校正的DVL航位推算模型推导如下：根据载体坐标系与DVL仪器坐标系的转换关系，存在安装偏差时，DVL测量速度的校正公式可表示为：$v^b=R_d^b(\Pi )·v^d---\left(1\right)$其中，为DVL测量得到的三维速度，为载体速度，为安装误差校正矩阵，即DVL仪器坐标系向载体坐标系转换的旋转矩阵，Π＝[α β γ]^T为仪器坐标系相对于载体坐标系的横滚、纵倾和航偏三维偏角，当DVL和姿态传感器安装固定后，三维偏角为固定值，安装误差校正矩阵为：$R_d^b\left(\Pi \right)=\left[\begin{array}{ccc}\cos \gamma \cos \beta & -\sin \gamma \cos \alpha +\cos \gamma \sin \beta \sin \alpha & \sin \gamma \sin \alpha +\cos \gamma \cos \beta \sin \alpha \\ \sin \gamma \cos \beta & \cos \gamma \cos \alpha +\sin \gamma \sin \beta \sin \alpha & -\cos \gamma \sin \alpha +\sin \gamma \sin \beta \cos \alpha \\ -\sin \beta & \cos \beta \sin \phi & \cos \beta \cos \alpha \end{array}\right]---\left(2\right)$根据三维姿态信息，将载体移动速度投影到导航坐标系，在前一刻推算得到的位置信息上进行累加得到下一时刻的推算位置信息，得到带误差校正的DVL航位推算模型数学表示为：$v^n=R_b^n\left(\Theta \right)·v^b---\left(3\right)$$p^n\left(k\right)=p^n(k-1)+R_b^n\left(\Theta \right(k-1\left)\right)·R_d^b·v^d(k-1)·\Delta t---\left(4\right)$其中，pⁿ(k)＝[x y z]^T为k时刻推算得到的三维位置信息，Δt为航位推算时间间隔，Θ＝[φ θ ψ]^T为姿态传感器测得的载体横滚、纵倾和航偏角信息，为载体坐标系向导航坐标系转换的旋转矩阵，表示为：$R_b^n\left(\Theta \right)=\left[\begin{array}{ccc}\cos \psi \cos \theta & -\sin \psi \cos \phi +\cos \psi \sin \theta \sin \phi & \sin \psi \sin \phi +\cos \psi \cos \theta \sin \phi \\ \sin \psi \cos \theta & \cos \psi \cos \phi +\sin \psi \sin \theta \sin \phi & -\cos \psi \sin \phi +\sin \psi \sin \theta \cos \phi \\ -\sin \theta & \cos \theta \sin \phi & \cos \theta \cos \alpha \end{array}\right]---\left(5\right);$步骤2)中，所述的离散时间系统状态方程为：x_k＝f(x_k‑1,u_k‑1,n_k‑1)   (6)其中x_k‑1＝[x y z α β γ]^T为k‑1时刻系统状态向量，为k‑1时刻系统输入向量，n_k‑1为k‑1时刻系统噪声向量，为零均值不相关高斯白噪声，协方差矩阵为Q_k‑1，对应的系统状态变量误差方程为：${\left[\begin{array}{ccc}\stackrel{·}{x}& \stackrel{·}{y}& \stackrel{·}{z}\end{array}\right]}^T=R_b^n\left(\Theta \right)·R_d^b·v^d·\Delta t---\left(7\right)$${\left[\begin{array}{ccc}\stackrel{·}{\alpha }& \stackrel{·}{\beta }& \stackrel{·}{\gamma }\end{array}\right]}^T={\left[\begin{array}{ccc}0& 0& 0\end{array}\right]}^T---\left(8\right)$选取AUV三维位置信息为测量向量，则离散化的系统测量方程为：$\begin{array}{c}{z}_k=H·x_k+w_k\\ =\left[\begin{array}{cccccc}1& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0\end{array}\right]·x_k+w_k\end{array}---\left(9\right)$其中，z_k＝[x y z]^T为k时刻系统测量向量，东北向位置信息由GPS提供，GPS输出位置信息为经纬度信息，经转化为导航坐标系下的位置信息，深度信息由深度计提供，w_k为k时刻系统测量噪声向量，为零均值高斯白噪声，协方差矩阵为R_k；步骤2)中，所述的改进的平方根容积卡尔曼滤波流程具体步骤如下：1)设定初始参数设定初始状态值x₀，初始协方差矩阵P₀；2)时间更新首先要计算基本的容积点和对应的权值，使用三阶容积原则获得基本容积点和对应权值为${\xi }_i=\sqrt{m}{l}_i,{\omega }_i=\frac{1}{2m},i=1,2,...,2m---\left(11\right)$其中，m为系统状态向量的维数，状态向量维数为6；$l_i\in \left\{\begin{array}{cccccccc}\left[\begin{array}{c}1\\ 0\\ ·\\ ·\\ ·\\ 0\end{array}\right],& \left[\begin{array}{c}0\\ 1\\ ·\\ ·\\ ·\\ 0\end{array}\right],& ...& \left[\begin{array}{c}0\\ 0\\ ·\\ ·\\ ·\\ 1\end{array}\right],& \left[\begin{array}{c}-1\\ 0\\ ·\\ ·\\ ·\\ 0\end{array}\right],& \left[\begin{array}{c}0\\ -1\\ ·\\ ·\\ ·\\ 0\end{array}\right],& ...& \left[\begin{array}{c}0\\ 0\\ ·\\ ·\\ ·\\ -1\end{array}\right]\end{array}\right\}---\left(12\right)$假设已知，分解协方差矩阵S_k‑1|k‑1＝chol(P_k‑1|k‑1)   (13)其中，chol(·)表示Cholesky分解；计算容积点$X_{i,k-1|k-1}=S_{k-1|k-1}{\xi }_i+{\hat{x}}_{k-1|k-1},i=1,2,...,2m---\left(14\right)$通过状态方程传递容积点$X_{i,k|k-1}^{*}=f\left(X_{i,k-1|k-1}\right)---\left(15\right)$得到k时刻状态预测值${\hat{x}}_{k|k-1}=\frac{1}{2m}\underset{i=1}{\overset{2m}{\Sigma }}{X}_{i,k|k-1}^{*}---\left(16\right)$计算预测协方差的平方根$S_{k|k-1}=Tria\left(\left[\begin{array}{cc}{\chi }_{k|k-1}^{*}& S_{Q,k-1}\end{array}\right]\right)---\left(17\right)$其中Tria(·)表示对矩阵进行三角化，采用QR分解，取R矩阵的下三角部分；${\chi }_{k|k-1}^{*}=\frac{1}{\sqrt{2m}}\left[\begin{array}{cccc}{X}_{1,k|k-1}^{*}-{\hat{x}}_{k|k-1}& X_{2,k|k-1}^{*}-{\hat{x}}_{k|k-1}& ...& X_{2n,k|k-1}^{*}-{\hat{x}}_{k|k-1}\end{array}\right]---\left(18\right)$3)测量更新测量更新时，考虑到测量方程的线性特性，无需通过传播容积点来获得测量估计值和协方差，直接采取平方根滤波方式；得到k时刻测量预测值${\hat{z}}_{k|k-1}=H·{\hat{x}}_{k|k-1}---\left(19\right)$以及预测协方差矩阵平方根$P_{k|k-1}=S_{k|k-1}{S}_{k|k-1}^T---\left(20\right)$P_xz,k|k‑1＝P_k|k‑1·H^T   (21)S_zz,k|k‑1＝Tria([H·S_k|k‑1S_R,k])   (22)其中计算卡尔曼增益$K_k=(P_{xz,k|k-1}/S_{zz,k|k-1}^T)/S_{zz,k|k-1}---\left(23\right)$得到估计值，完成平方根更新${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1})---\left(24\right)$S_k|k＝Tria([(I‑K·H)·S_k|k‑1K·S_R,k])   (25)。