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

Abstract

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

Description

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

技术领域

本发明涉及自主水下航行器（Autonomous Underwater Vehicle,AUV）导航仪器校准技术，属于海洋工程领域。

背景技术

自主水下航行器是无缆连接的水下无人自主航行器平台，在装载合适的声纳、水文、化学等传感器后可应用于水下环境监测、近海石油工程作业、水下搜索与测绘等领域。它具有控制灵活、航行面积广阔、价格低廉等特点，是近年来海洋工程领域研究的热点之一。

水下导航技术是实现水下航行器自主航行的关键，但是不同于传统导航系统，水下导航技术具有海洋环境复杂、可获取的外界信息少、系统功耗/体积制约等特点，具有极大的挑战性。常用的水下导航传感器有多普勒计程仪（DopplerVelocity Log，DVL）、电子罗盘、惯性导航系统、深度传感器等，DVL利用安装在AUV载体上的声学换能器阵发射声波，利用多普勒原理测量AUV对于水底或者水层/水面的相对速度，具有精度高的特点，是水下航行器导航系统的关键设备。

DVL测得的速度信息是相对于自身定义的仪器坐标系下的速度，在使用DVL测量的速度进行导航时，必须要经过坐标转换，将其转换到导航坐标系下，这一过程通常需利用姿态传感器测得的载体姿态信息。若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\left(\mathrm{\Π}\right)\·v^d---\left(1\right)$

其中，

为DVL测量得到的三维速度，

为载体速度，

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

$R_d^b\left(\mathrm{\Π}\right)=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\β}& -\sin \mathrm{\γ}\cos \mathrm{\α}+\cos \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}& \sin \mathrm{\γ}\sin \mathrm{\α}+\cos \mathrm{\γ}\cos \mathrm{\β}\sin \mathrm{\α}\\ \sin \mathrm{\γ}\cos \mathrm{\β}& \cos \mathrm{\γ}\cos \mathrm{\α}+\sin \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}& -\cos \mathrm{\γ}\sin \mathrm{\α}+\sin \mathrm{\γ}\sin \mathrm{\β}\cos \mathrm{\α}\\ -\sin \mathrm{\β}& \cos \mathrm{\β}\sin \mathrm{\φ}& \cos \mathrm{\β}\cos \mathrm{\α}\end{array}\right]---\left(2\right)$

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

$v^n=R_b^n\left(\mathrm{\Θ}\right)\·v^b---\left(3\right)$

$p^n\left(k\right)=p^n(k-1)+R_b^n\left(\mathrm{\Θ}(k-1)\right)\·R_d^b\·v^d(k-1)\·\mathrm{\Δt}---\left(4\right)$

其中，pⁿ(k)＝[xyz]^T为k时刻推算得到的三维位置信息，Δt为航位推算时间间隔，Θ=[φθψ]^T为姿态传感器测得的载体横滚、纵倾和航偏角信息，

为载体坐标系向导航坐标系转换的旋转矩阵，表示为：

$R_b^n\left(\mathrm{\Θ}\right)=\left[\begin{array}{ccc}\cos \mathrm{\ψ}\cos \mathrm{\θ}& -\sin \mathrm{\ψ}\cos \mathrm{\φ}+\cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}& \sin \mathrm{\ψ}\sin \mathrm{\φ}+\cos \mathrm{\ψ}\cos \mathrm{\θ}\sin \mathrm{\φ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\φ}+\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}& -\cos \mathrm{\ψ}\sin \mathrm{\φ}+\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \\ -\sin \mathrm{\θ}& \cos \mathrm{\θ}\sin \mathrm{\φ}& \cos \mathrm{\θ}\cos \mathrm{\φ}\end{array},\mathrm{\φ}\right]---\left(5\right)$

步骤2）中，所述的离散时间系统状态方程为：

x_k=f(x_k-1,u_k-1,n_k-1)      （6）

其中x_k-1=[xyzαβγ]^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(\mathrm{\Θ}\right)\·R_d^b\·v^d\·\mathrm{\Δt}---\left(7\right)$

${\left[\begin{array}{ccc}\stackrel{\·}{\mathrm{\α}}& \stackrel{\·}{\mathrm{\β}}& \stackrel{\·}{\mathrm{\γ}}\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---\left(9\right)\end{array}$

其中，z_k=[xyz]^T为k时刻系统测量向量，东北向位置信息由GPS提供，GPS输出位置信息为经纬度信息，经转化为导航坐标系下的位置信息，深度信息由深度计提供，w_k为k时刻系统测量噪声向量，为零均值高斯白噪声，协方差矩阵为R_k。

步骤2）中，所述的改进的平方根容积卡尔曼滤波流程具体步骤如下：

1）设定初始参数

设定初始状态值x₀，初始协方差矩阵P₀；

2）时间更新

首先要计算基本的容积点和对应的权值，使用三阶容积原则获得基本容积点和对应权值为

${\mathrm{\ξ}}_i=\sqrt{m}{l}_i,{\mathrm{\ω}}_i=\frac{1}{2m},i=\mathrm{1,2},...,2m---\left(11\right)$

其中，m为系统状态向量的维数，状态向量维数为6；

$l_i\∈\{\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]\}---\left(12\right)$

假设 $p\left(x_{k-1}\right|z_{k-1})=N(x_{k-1}:{\hat{x}}_{k-1|k-1},P_{k-1|k-1})$ 已知，分解协方差矩阵

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}{\mathrm{\ξ}}_i+{\hat{x}}_{k-1|k-1},i=\mathrm{1,2},\·\·\·,2m---\left(14\right)$

通过状态方程传递容积点

$X_{i,k-1|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}{\mathrm{\Σ}}}{X}_{i,k-1|k-1}^{*}---\left(16\right)$

计算预测协方差的平方根

$S_{k|k-1}=\mathrm{Tria}\left(\right[{\mathrm{\χ}}_{k|k-1}^{*}{S}_{Q,k-1}\left]\right)---\left(17\right)$

其中

Tria(·)表示对矩阵进行三角化，采用QR分解，取R矩阵的下三角部分；

${\mathrm{\χ}}_{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_{\mathrm{xz},k|k-1}=P_{k|k-1}\·H^T---\left(21\right)$

S_zz,k|k-1=Tria([H·S_k|k-1S_R,k])      （22）

其中 $S_{R,k}{S}_{R,k}^T=R_k;$

计算卡尔曼增益

$K_k=(P_{\mathrm{xz},k|k-1}/S_{\mathrm{zz},k|k-1}^T)/S_{\mathrm{zz},k|k-1}---\left(23\right)$

得到估计值，完成平方根更新

${\hat{x}}_{k|k-1}={\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）。

步骤1）中，所述的DVL采集到的所述速度信息包括前进速度信息、横向速度信息、垂直速度信息中的一种或多种。

步骤1）中，所述的姿态信息包括航偏角信息、横滚角信息、纵倾角信息中的一种或几种。

步骤1）中，所述的东北向位置信息来自于经度信息和纬度信息。

本发明的有益效果是：操作简单，无需外部设备辅助，可有效估计出DVL的三维安装偏差角，从而消除DVL安装误差对AUV导航精度的影响，具有很强的实用价值。

附图说明

图1是水下航行器载体坐标系和DVL仪器坐标系的偏差示意图；

图2是本发明流程图；

图3是改进的平方根容积卡尔曼滤波算法流程图；

图4是GPS轨迹、粗校正后航位推算结果以及应用本发明校正后航位推算结果的轨迹对比图，其中实线1是GPS实时记录的位置信息，虚线2是粗校正后的航位推算结果，虚线3是应用本发明校正后的航位推算结果，导航数据来源于AUV湖上试验；

图5是本发明安装偏角估计结果，导航数据来源于AUV湖上试验，与图4采用数据为同一组；

图6是粗校正后航位推算误差结果和本发明校正后航位推算误差结果的对比图，其中虚线7表示粗校正后的误差结果，实线8表示应用本发明校正后的误差结果；

图7是GPS轨迹、粗校正后航位推算结果以及应用本发明校正后航位推算结果的轨迹对比图，其中实线9是GPS实时记录的位置信息，虚线10是应用本发明校正后的航位推算结果，虚线11是粗校正后的航位推算结果，点12为由GPS得到的AUV最终位置信息，点13为本发明校正后推算得到的AUV最终位置信息，点14为粗校正后推算得到的AUV最终位置信息；AUV运动轨迹为先在水面上跑一段路程，然后进行下潜，在水下进行定深5m的运动，最后重新浮出水面沿直线航行一段时间。

具体实施方式

以下结合附图和实施例对本发明做进一步的说明。

本发明用于对安装在自主水下航行器中的多普勒计程仪安装误差进行校正，从而提高水下航行器自主导航的精度。

本发明的流程如图2所示。

首先对本发明采用的几种坐标系进行说明，以下公式中上标b代表载体坐标系，上标d为DVL仪器坐标系，上标n代表导航坐标系，本发明选用东北天地理坐标系为导航坐标系。

DVL是测量水下航行器移动速度的传感器，工作时其换能器阵向水底/水面发送固定频率的声波。根据多普勒频移原理，载体相对于回波反射物的移动会造成接收信号频率的变化。DVL通过测量返回声波的频率，并通过接收与发送信号间频率的关系计算出载体的三维移动速度。

DVL测得的三轴速度信息是相对于自身定义的仪器坐标系下的速度，在使用DVL速度测量值进行航位推算时，通过载体三维姿态信息将测得速度投影到导航坐标系，投影后可得AUV东西方向的速度，南北方向的速度和天向的速度，这些速度测量信息通过导航算法可实现AUV定位，其中AUV的深度信息一般由深度计直接获取。实际工程应用中，由于机械加工等误差，DVL存在一定量的安装误差，导致仪器坐标系与载体坐标系不重合，如图1所示，两者存在一定的安装偏角，需要对安装误差进行校正再进行航位推算，以消除安装误差对系统整体导航精度的影响。

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

$v^b=R_d^b\left(\mathrm{\Π}\right)\·v^d---\left(1\right)$

其中，

为DVL测量得到的三维速度，

为载体速度，

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

为：

$R_d^b\left(\mathrm{\Π}\right)=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\β}& -\sin \mathrm{\γ}\cos \mathrm{\α}+\cos \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}& \sin \mathrm{\γ}\sin \mathrm{\α}+\cos \mathrm{\γ}\cos \mathrm{\β}\sin \mathrm{\α}\\ \sin \mathrm{\γ}\cos \mathrm{\β}& \cos \mathrm{\γ}\cos \mathrm{\α}+\sin \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}& -\cos \mathrm{\γ}\sin \mathrm{\α}+\sin \mathrm{\γ}\sin \mathrm{\β}\cos \mathrm{\α}\\ -\sin \mathrm{\β}& \cos \mathrm{\β}\sin \mathrm{\φ}& \cos \mathrm{\β}\cos \mathrm{\α}\end{array}\right]---\left(2\right)$

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

$v^n=R_b^n\left(\mathrm{\Θ}\right)\·v^b---\left(3\right)$

$p^n\left(k\right)=p^n(k-1)+R_b^n\left(\mathrm{\Θ}(k-1)\right)\·R_d^b\·v^d(k-1)\·\mathrm{\Δt}---\left(4\right)$

其中，pⁿ(k)＝[xyz]^T为k时刻推算得到的三维位置信息，Δt为航位推算时间间隔，Θ=[φθψ]^T为姿态传感器测得的载体横滚、纵倾和航偏角信息，

为载体坐标系向导航坐标系转换的旋转矩阵，表示为：

$R_b^n\left(\mathrm{\Θ}\right)=\left[\begin{array}{ccc}\cos \mathrm{\ψ}\cos \mathrm{\θ}& -\sin \mathrm{\ψ}\cos \mathrm{\φ}+\cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}& \sin \mathrm{\ψ}\sin \mathrm{\φ}+\cos \mathrm{\ψ}\cos \mathrm{\θ}\sin \mathrm{\φ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\φ}+\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}& -\cos \mathrm{\ψ}\sin \mathrm{\φ}+\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \\ -\sin \mathrm{\θ}& \cos \mathrm{\θ}\sin \mathrm{\φ}& \cos \mathrm{\θ}\cos \mathrm{\φ}\end{array},\mathrm{\φ}\right]---\left(5\right)$

根据式（4）得到的带误差校正的DVL航位推算模型，取AUV三维位置信息及DVL三维安装偏角为系统状态向量，DVL测得速度信息及姿态传感器测得姿态信息为系统输入向量，得到离散化的系统状态方程为：

x_k=f(x_k-1,u_k-1,n_k-1)      （6）其中x_k-1=[xyzαβγ]^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(\mathrm{\Θ}\right)\·R_d^b\·v^d\·\mathrm{\Δt}---\left(7\right)$

${\left[\begin{array}{ccc}\stackrel{\·}{\mathrm{\α}}& \stackrel{\·}{\mathrm{\β}}& \stackrel{\·}{\mathrm{\γ}}\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---\left(9\right)\end{array}$

其中，z_k=[xyz]^T为k时刻系统测量向量，东北向位置信息由GPS提供，GPS输出位置信息为经纬度信息，经转化为导航坐标系下的位置信息。深度信息由深度计提供。w_k为k时刻系统测量噪声向量，为零均值高斯白噪声，协方差矩阵为R_k。

系统状态方程为非线性方程，为克服系统状态方程线性化而产生的误差同时考虑测量方程线性的特点，本发明采用一种改进的平方根容积卡尔曼滤波进行滤波估计。

对于本发明的非线性过程，其状态方程和测量方程由如下方程描述：

$\left\{\begin{array}{c}{x}_k=f(x_{k-1},u_{k-1},n_{k-1})\\ z_k=H\·x_k+w_k\end{array}\right.---\left(10\right)$

其中，参数同上文定义。

改进的平方根容积卡尔曼滤波流程如图3所示，具体步骤如下：

1）设定初始参数

设定初始状态值x₀，初始协方差矩阵P₀

2）时间更新

首先要计算基本的容积点和对应的权值。使用三阶容积原则获得基本容积点和对应权值为

${\mathrm{\ξ}}_i=\sqrt{m}{l}_i,{\mathrm{\ω}}_i=\frac{1}{2m},i=\mathrm{1,2},...,2m---\left(11\right)$

其中，m为系统状态向量的维数，本发明中状态向量维数为6。

$l_i\∈\{\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]\}---\left(12\right)$

假设 $p\left(x_{k-1}\right|z_{k-1})=N(x_{k-1}:{\hat{x}}_{k-1|k-1},P_{k-1|k-1})$ 已知，分解协方差矩阵

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}{\mathrm{\ξ}}_i+{\hat{x}}_{k-1|k-1},i=\mathrm{1,2},\·\·\·,2m---\left(14\right)$

通过状态方程传递容积点

$X_{i,k-1|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}{\mathrm{\Σ}}}{X}_{i,k-1|k-1}^{*}---\left(16\right)$

计算预测协方差的平方根

$S_{k|k-1}=\mathrm{Tria}\left(\right[{\mathrm{\χ}}_{k|k-1}^{*}{S}_{Q,k-1}\left]\right)---\left(17\right)$

其中

Tria(·)表示对矩阵进行三角化，采用QR分解，取R矩阵的下三角部分。

${\mathrm{\χ}}_{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）

其中 $S_{R,k}{S}_{R,k}^T=R_k.$

计算卡尔曼增益

$K_k=(P_{\mathrm{xz},k|k-1}/S_{\mathrm{zz},k|k-1}^T)/S_{\mathrm{zz},k|k-1}---\left(23\right)$

得到估计值，完成平方根更新

${\hat{x}}_{k|k-1}={\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）

通过上述非线性滤波，可以得到安装偏角的估计值，选取其收敛值，得到安装误差校正矩阵

本发明采用带DVL安装误差校正的航位推算模型作为状态方程，选取三维位置信息作为测量向量，通过改进的平方根容积卡尔曼滤波对三维安装偏角进行估计。本发明复杂性较低，操作简单，可有效校正DVL安装误差，提高AUV导航精度。

实施例

现场水面校正试验中，通过姿态传感器获取横滚、纵倾和航偏角信息，利用DVL采集AUV前进速度和横向速度信息，上述信息可建立校正算法的观测方程。在评价导航性能时，利用AUV上安装的GPS得到AUV的东北向位置信息，作为AUV位置参考真值，用于评价AUV航位导航算法的定位精度。由于AUV的深度信息一般由深度计直接测量，准确度较高，通常不需要导航算法估计。

AUV姿态传感器模块以及DVL模块均由手工安装，加工误差及安装误差难以避免，同时安装DVL换能器时由于机械安装需要等原因可能存在某一固定偏置，如本实验AUV系统的旋转角为45°，因而必须进行角度校正，考虑两种校正方式：1）粗校正，即根据已知条件人为设定校正矩阵，根据实际安装情况，设定安装偏角为[α β γ]＝[0° 0° -45°]，得到粗校正矩阵；2）本发明提出的校正方法。

图4中，AUV在水面上做校正航行，平均航行速度为2m/s，实线1为AUV在水面由GPS得到的位置信息。图5给出了本发明对安装偏角的估计结果，取滤波稳定后安装偏角估计值均值得到校正矩阵，再对DVL安装误差进行校正。图4中，虚线2为粗校正后DVL推算得到的AUV每一时刻的位置信息，虚线3为本发明校正后DVL推算得到的AUV每一时刻的位置信息。图6中，虚线7为粗校正后的位置误差，实线8为本发明校正后的位置误差，对比可以发现，本发明提出的校正方法可以有效提高导航精度。

由于仪器安装固定后，安装偏角为固定值，对于校正轨迹得到的校正矩阵采用另一航次进行有效性验证。图7中，AUV在水面运行一段时间后进行下潜，平均航行速度为1.5m/s，定深5m航行。实线9为AUV在水面由GPS得到的位置信息，图中路径中间部分AUV处于水下，因此没有GPS信息。虚线10为本发明校正后DVL推算得到的AUV每一时刻的位置信息，虚线11为粗校正后DVL推算得到的AUV每一时刻的位置信息。可以看出利用本发明进行校正得到的推算位置信息更为接近GPS的位置信息，进一步证明了本发明的可行性。

上述实施例表明利用本发明可有效对DVL安装误差进行校正，从而提高AUV导航系统的定位精度。

Claims (7)

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

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

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

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

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

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

$v^b=R_d^b\left(\mathrm{\Π}\right)\·v^d---\left(1\right)$

其中，

为DVL测量得到的三维速度，

为载体速度，

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

$R_d^b\left(\mathrm{\Π}\right)=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\β}& -\sin \mathrm{\γ}\cos \mathrm{\α}+\cos \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}& \sin \mathrm{\γ}\sin \mathrm{\α}+\cos \mathrm{\γ}\cos \mathrm{\β}\sin \mathrm{\α}\\ \sin \mathrm{\γ}\cos \mathrm{\β}& \cos \mathrm{\γ}\cos \mathrm{\α}+\sin \mathrm{\γ}\sin \mathrm{\β}\sin \mathrm{\α}& -\cos \mathrm{\γ}\sin \mathrm{\α}+\sin \mathrm{\γ}\sin \mathrm{\β}\cos \mathrm{\α}\\ -\sin \mathrm{\β}& \cos \mathrm{\β}\sin \mathrm{\φ}& \cos \mathrm{\β}\cos \mathrm{\α}\end{array}\right]---\left(2\right)$

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

$v^n=R_b^n\left(\mathrm{\Θ}\right)\·v^b---\left(3\right)$

$p^n\left(k\right)=p^n(k-1)+R_b^n\left(\mathrm{\Θ}(k-1)\right)\·R_d^b\·v^d(k-1)\·\mathrm{\Δt}---\left(4\right)$

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

为载体坐标系向导航坐标系转换的旋转矩阵，表示为：

$R_b^n\left(\mathrm{\Θ}\right)=\left[\begin{array}{ccc}\cos \mathrm{\ψ}\cos \mathrm{\θ}& -\sin \mathrm{\ψ}\cos \mathrm{\φ}+\cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}& \sin \mathrm{\ψ}\sin \mathrm{\φ}+\cos \mathrm{\ψ}\cos \mathrm{\θ}\sin \mathrm{\φ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\cos \mathrm{\φ}+\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\φ}& -\cos \mathrm{\ψ}\sin \mathrm{\φ}+\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \\ -\sin \mathrm{\θ}& \cos \mathrm{\θ}\sin \mathrm{\φ}& \cos \mathrm{\θ}\cos \mathrm{\φ}\end{array},\mathrm{\φ}\right]---\left(5\right)$

3.根据权利要求1所述的校正方法，其特征在于，步骤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(\mathrm{\Θ}\right)\·R_d^b\·v^d\·\mathrm{\Δt}---\left(7\right)$

${\left[\begin{array}{ccc}\stackrel{\·}{\mathrm{\α}}& \stackrel{\·}{\mathrm{\β}}& \stackrel{\·}{\mathrm{\γ}}\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---\left(9\right)\end{array}$

其中，z_k=[x y z]^T为k时刻系统测量向量，东北向位置信息由GPS提供，GPS输出位置信息为经纬度信息，经转化为导航坐标系下的位置信息，深度信息由深度计提供，w_k为k时刻系统测量噪声向量，为零均值高斯白噪声，协方差矩阵为R_k。

4.根据权利要求1所述的校正方法，其特征在于，步骤2）中，所述的改进的平方根容积卡尔曼滤波流程具体步骤如下：

1）设定初始参数

设定初始状态值x₀，初始协方差矩阵P₀；

2）时间更新

首先要计算基本的容积点和对应的权值，使用三阶容积原则获得基本容积点和对应权值为

${\mathrm{\ξ}}_i=\sqrt{m}{l}_i,{\mathrm{\ω}}_i=\frac{1}{2m},i=\mathrm{1,2},...,2m---\left(11\right)$

其中，m为系统状态向量的维数，状态向量维数为6；

$l_i\∈\{\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]\}---\left(12\right)$

假设 $p\left(x_{k-1}\right|z_{k-1})=N(x_{k-1}:{\hat{x}}_{k-1|k-1},P_{k-1|k-1})$ 已知，分解协方差矩阵

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}{\mathrm{\ξ}}_i+{\hat{x}}_{k-1|k-1},i=\mathrm{1,2},\·\·\·,2m---\left(14\right)$

通过状态方程传递容积点

$X_{i,k-1|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}{\mathrm{\Σ}}}{X}_{i,k-1|k-1}^{*}---\left(16\right)$

计算预测协方差的平方根

$S_{k|k-1}=\mathrm{Tria}\left(\right[{\mathrm{\χ}}_{k|k-1}^{*}{S}_{Q,k-1}\left]\right)---\left(17\right)$

其中

Tria(·)表示对矩阵进行三角化，采用QR分解，取R矩阵的下三角部分；

${\mathrm{\χ}}_{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）

其中 $S_{R,k}{S}_{R,k}^T=R_k;$

计算卡尔曼增益

$K_k=(P_{\mathrm{xz},k|k-1}/S_{\mathrm{zz},k|k-1}^T)/S_{\mathrm{zz},k|k-1}---\left(23\right)$

得到估计值，完成平方根更新

${\hat{x}}_{k|k-1}={\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）。

5.根据权利要求1所述的校正方法，其特征在于，步骤1）中，所述的DVL采集到的所述速度信息包括前进速度信息、横向速度信息、垂直速度信息中的一种或多种。

6.根据权利要求1所述的校正方法，其特征在于，步骤1）中，所述的姿态信息包括航偏角信息、横滚角信息、纵倾角信息中的一种或几种。

7.根据权利要求1所述的校正方法，其特征在于，步骤1）中，所述的东北向位置信息来自于经度信息和纬度信息。

Images