[发明专利]一种实现水下机器人精细导航的方法

权利要求书

1.一种实现水下机器人精细导航的方法，其特征在于，是利用九轴传感器获取AUV的运动信息，同时利用光学摄像机采集水底环境特征；在AUV运行时拍摄水底图像，通过环境图像建立水下环境地图，并通过传感器信息融合获得水下机器人自身位置以用于精细导航；具体包括以下步骤：

步骤一：获得AUV的初始位姿；

(1)利用陀螺仪和加速度计获取运动信息，利用磁力计测量周围磁场状态；

(2)利用加速度计测量静止状态下AUV的重力加速度，进而确定AUV的部分姿态；

假设AUV的加速度测量值为：

式中：表示AUV在x、y、z方向的加速度，下标代表轴方向；上标b表示载体坐标系，载体坐标系以载体朝向为y轴，右手方为x轴，上方为z轴；T表示矩阵转置；

通过向量关系得到AUV的俯仰角和横滚角：

式中，θ为俯仰角，以东方向为旋转轴的右手螺旋方向为正；γ为横滚角，以北方向为旋转轴的右手螺旋方向为正，|f^b|表示载体加速度值；

(3)利用磁力计确定AUV的航向：

采用磁力计对AUV航向进行确定；设AUV由磁力计测得地球磁场强度为其中表示AUV在x、y、z方向的磁场强度；

通过向量关系得到AUV的艏向角：

其中ψ为AUV艏向角，其以北偏东为正；

步骤二：计算AUV在行进过程中的姿态、速度和位置；

(1)AUV姿态更新

假设AUV在x、y、z方向的角速度为则计算得到载体姿态四元数为：

其中，Q(t_k)表示t＝k时刻时AUV的姿态；Q(t_k+1)表示t＝k+1时刻时AUV的姿态；Q＝[q₀,q₁,q₂,q₃]为姿态四元数，q_i,i＝0～3是四元数参数；I表示单位阵，Δt表示陀螺仪采样间隔，ΔΘ为无特别含义的中间变量；

对进行分解，将其分解为AUV自身运动的角速度与地球自转引起的角速度：

式中，为陀螺测量值，反映AUV自身运动的角速度；由位置速率和地球自转速率ω_ie经坐标转换后而得；

其中：

Vⁿ＝[V_E V_N V_U]^T，表示AUV在东、北、天方向的速度；上标n表示惯性坐标系，以东方向为x轴，北方向为y轴，天为z轴；R_M为AUV所在点子午圈曲率半径，R_N为AUV所在点卯酉圈曲率半径；是AUV姿态矩阵表示载体坐标系到惯性坐标系的转换关系，由姿态四元数直接得到；L表示AUV的纬度，h表示AUV相对于水平面的高度；

其中：

式中为地球扁率，R_e＝6387824米，R_p＝6356803米；

(2)AUV速度更新

AUV速度表示为：

式中，为t＝m时的速度，为t＝m-1时的速度；Δt为陀螺仪采样时间间隔，为t＝m-1时刻加速度计测量值，gⁿ表示重力加速度；

(3)AUV位置更新

AUV位置矩阵表示为：

式中表示t＝m时的位置矩阵；

其中：

同时：

式中I为单位阵，ξ_m为中间变量；

F(t)为中间变量，为AUV在笛卡尔坐标系下的位置增量；

通过位置矩阵获得AUV经纬度，获得方法如下；

L＝arcsin P₃₃

步骤三：利用摄像机采集的水下图像信息，计算AUV的姿态和位置，并初始化AUV地图；

(1)利用摄像机进行光学图像采集，得到光学图像；

(2)利用ORB算法提取相邻图像特征点，并对其进行匹配；定义匹配完成的特征集为m_t，其中t是当前时刻；

(3)优化ORB特征匹配；

(4)利用本质矩阵、单应矩阵或PnP方法，求解AUV位姿；

步骤四：对九轴传感器和摄像机采集的信息进行传感器信息融合，获得AUV最优位姿信息；

采用卡尔曼滤波器以九轴惯性导航误差方程(姿态、速度、位置)为状态方程；以九轴惯性导航误差方程的解算值与步骤三的解算值之差作为观测值进行滤波；

(1)计算九轴惯性导航误差方程：

(1.1)速度误差方程：

式中δVⁿ表示速度的误差量，表示速度的误差量的导数，φ_E、φ_N、φ_U是姿态误差角，f^b是加速度计测量值，是当前时刻姿态矩阵，表示位置速率的误差量，δh表示高度误差，δL表示纬度误差，表示地球自转速率的误差量，

(1.2)位置误差方程：

式中表示纬度误差导数，表示经度误差的导数，δh为高度误差，δL为纬度误差；

(1.3)姿态误差方程：

式中为姿态误差角的导数，为陀螺测量值，

(2)将误差方程的解算值作为卡尔曼滤波方程的状态量

利用步骤二获得AUV的姿态、位置、速度，令

式中X_k表示k时刻AUV的状态，表示k时刻AUV的姿态，表示k时刻AUV的速度，下标E、N、U表示东、北、天方向，D_k＝[L_k λ_k h_k]^T表示k时刻AUV所处的经度、纬度、高度；

利用卡尔曼滤波器对误差大小进行量化，对导航结果进行补偿；

式中表示k时刻误差补偿后AUV的状态，表示AUV状态误差的最优估计，表示k时刻角度误差，表示k时刻速度误差，δL表示经度误差，δλ表示纬度误差，δh表示高度误差；

(3)根据摄像机的位置解算值计算AUV速度

将步骤三计算出的AUV的姿态和位置，记为：与

其中为AUV速度；

(4)计算卡尔曼滤波器观测量

获得AUV状态的观测量Z_k：

其中：

(5)进行卡尔曼滤波

将Z_k与x_k导入卡尔曼滤波器中，获得AUV状态误差的最优估计进而计算出k时刻误差补偿后AUV的状态

步骤五：根据AUV的最优位姿对地图进行更新；

利用三角测量计算特征点位置，将特征点位置的集合作为AUV的初始特征地图。

2.根据权利要求1所述的方法，其特征在于，所述步骤三中，优化ORB特征匹配的方法是：

在AUV速度超过5cm/s时，对图像匹配结果进行优化；当满足优化条件时，根据2帧前的AUV位置x_k-2和最近3秒的速度矢量v_k-2，v_k-1，v_k估计下一时刻AUV的位置x_k+1；具体优化方式如下：

(1)在x_k-2上生成100个点

(2)将这些点的位置根据速度矢量进行转移：其中w_k-2为误差项，w_k-2服从正态分布，标准差取0.025；t为每帧间隔；其中w_k-1服从正态分布，标准差取0.05；其中w_k服从正态分布，标准差取0.1；

(3)寻找顶点为x_k的扇面，使其能够包络所有点；设扇面小圆半径为r，大圆半径为R，角度为θ；

(4)图像匹配时将两张图像I_k,I_k+1叠加，根据匹配结果，将匹配的特征点对进行连线，表示I_k图像上匹配上的第j对特征点，获得

(5)删除所有不在扇面范围内的匹配特征点对。