CN112197765A - 一种实现水下机器人精细导航的方法 - Google Patents

Abstract

本发明涉及水下机器人导航技术，旨在提供一种实现水下机器人精细导航的方法。包括：获得AUV的初始位姿；计算AUV在行进过程中的姿态、速度和位置；利用摄像机采集的水下图像信息，计算AUV的姿态和位置，并初始化AUV地图；对九轴传感器和摄像机采集的信息进行传感器信息融合，获得AUV最优位姿信息；根据AUV的最优位姿对地图进行更新；本发明利用九轴传感器的数据高更新率的特点，提高导航信息数据量；利用基于九轴的惯性导航提供每时刻的运动数据，为视觉导航提供尺度信息，同时解决视觉导航在水下匹配失真问题；利用视觉导航解决惯性导航的累积误差问题。本方法通过对AUV的位姿进行误差补偿，可以使AUV的位置误差，提高导航精度。

Description

一种实现水下机器人精细导航的方法

技术领域

本发明属于水下机器人导航技术领域，特别涉及一种针对自主水下机器人(Autonomous Underwater Vehicle，AUV)的精细导航方法。

背景技术

在近几年，人类开始广泛应用水下机器人对海底环境进行探索，采集深海海底生物、海底地形图像信息。水下机器人能够保证其自身安全并且正常完成一切任务的基础是：知道其自身的位置，即保证可靠的导航定位。随着深海科学调查的快速发展，海底调查任务需要构建小尺度的水下环境地图，因此对水下精细导航技术提出了迫切的需求。由于这些任务往往是利用水下机器人在某小片区域进行工作，相对于水下机器人整体活动区域尺度而言，工作区域是很小的，因此称为小微尺度。同时在工作区域中，AUV需要完成一系列操作，与特定位置环境进行互动，这就要求AUV的位置误差尽可能的小，数据更新率尽可能高，意味着需要对AUV进行高精度导航，这种高精度导航也被称为精细导航。目前水下导航过程中存在以下问题：1)在水下GPS设备无法使用；2)水下声学设备成本高、水听器布设麻烦等。

为了解决以上难题，研究人员提出利用水下地形作为参考，利用地形匹配***与惯性导航***结合为AUV提供导航，但这种水下导航仅使用于AUV大尺度航行，其误差为米级，不适用于小范围工作导航；同时研究人员针对部分水下作业(如水下装置充电、对准等)，提出利用单目摄像机寻找已知特征的水下标志物对AUV进行导航。这种导航方式的精度一般在厘米级，但极其依赖于水下先验的环境知识，而AUV在大部分工作时面对的是未知的水下环境。同时目前的水下导航方式均采用含有惯性导航方式的组合导航方式，受惯性导航平台所限，其数据更新率一般在秒级，数据刷新率低，难以精确地对AUV进行控制。

发明内容

本发明要解决的技术问题是，克服现有技术中的不足，提供一种实现水下机器人精细导航的方法。

为解决技术问题，本发明的解决方案是：

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

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

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

(2)利用加速度测量值计算静止状态下AUV的部分姿态：

由于在AUV静止时只受重力状态，静止状态有重力加速度，本发明通过静止状态下AUV的加速度来确定AUV的部分姿态。

假设AUV由加速度计测量所得的加速度测量值为

其中

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

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

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

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

由于水平面垂直于重力方向，因此无论AUV在水平面朝向哪个方向，其加速度计测量值均相同，因此无法通过加速度计确定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_n-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的初始特征地图。

本发明中，所述步骤三中优化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)删除所有不在扇面范围内的

匹配特征点对。

本发明中，九轴传感器可选MPU9250九轴传感器(换用其他九轴传感器也可)，光学摄像机可选定焦广角单目摄像机(简称为单目相机、相机或摄像机)。

发明原理描述：

本发明提出的水下机器人精细导航方法与装置，采用将单目视觉导航与基于九轴传感器的惯性导航进行结合，利用传感器信息融合方式提高导航准确率。本发明利用九轴传感器的数据高更新率的特点，提高导航信息数据量；利用基于九轴的惯性导航提供每时刻的运动数据，为视觉导航提供尺度信息，同时解决视觉导航在水下匹配失真问题；利用视觉导航解决惯性导航的累积误差问题。本方法通过卡尔曼滤波器将两种导航的优势相结合，通过对AUV的位姿进行误差补偿，可以使AUV的位置误差，提高导航精度。

与现有技术相比，本发明的有益效果在于：

(1)本发明采用光学图像采集外界信息，不需要先验的环境知识；

(2)本发明采用低成本九轴传感器，现对于传统惯性导航平台，成本低；

(3)本发明提出的导航算法导航精确度高，输出更新率高。

附图说明

图1是卡尔曼滤波器框架；

图2是本发明方法的实现流程图；

图3是本发明的装置框图。

具体实施方式

本发明所述实现水下机器人精细导航的方法，其具体内容与说明书发明内容部分的记载相同，此处不再赘述。

下面以一个装有单目摄像机与九轴传感器的水下机器人在未知工作环境中进行导航的例子，来说明本发明的方法(如图2所示)和装置(如图3所示)。

该装置包括水下机器人，按常规方式装配摄像机、九轴传感器和处理器。摄像机用于获取图像信息，并将数据传送至处理器，处理器包括但不限于计算机、树莓派、DSP 等形式。九轴传感器用于获取AUV的运动信息，并将数据传送给处理器进行处理。

该方法的实现流程如图2所示。

(1)首先对相机和九轴传感器进行校准。

(2)导入初始参数：九轴传感器数据：f^b、

m_b；图像集数据：I (I＝{I₁，I₂，…，I_m})(图像集数据是AUV开始导航后，相机拍摄到的图像序列)，初始速度：V₀；初始位置：L₀、λ₀、h₀；相机内参矩阵：K；地球自转角速率：ω_ie；重力加速度标准值：g₀；地球半长轴：R_e；地球扁率：e；九轴校准参数：ε^a、K^a、ε^w、ε^s、K^s；相机字典；更新频率：Δt＝30Hz。

(3)将传感器数据进行对齐：

采用就近方式对传感器数据进行对齐，以相机时间戳为一组以30Hz为频率的时间节点t₁，t₂，…，t_k，…，t_m，其他传感器数据在时间

时的测量值为

在

中寻找时间点

使得

为最小值，将

视为在时间点t_k下产生的测量数据。

(4)利用九轴传感器校准参数对九轴传感器进行校准。

(5)根据步骤一所述，计算AUV初始角度γ₀，θ₀，ψ₀。

(6)获得初始位姿，姿态：

位置：D₀←[L₀ λ₀ H₀]^T，初始状态：

初始误差：

(7)利用步骤二中，姿态更新算法计算AUV姿态Q_k+1，下标k表示时间。

(8)利用姿态四元数Q_k+1求旋转矩阵获得姿态矩阵

(9)利用姿态矩阵

求AUV姿态角

(10)利用步骤二中，速度更新算法计算AUV速度V_k+1。

(11)利用步骤二中，位置更新算法计算AUV位置

(12)对图像集数据进行图像灰度化，获得I_k。

(13)利用步骤三中，ORB方法生成orb特征点与描述子orb_I_k。

(14)利用特征点匹配获得匹配后特征fea，并利用步骤三中提到的优化方式对fea进行优化。

(15)如果地图没有初始化，且fea中匹配对数大于50，且orb_I_k＞100， orb_I_k-1＞100。

a)利用步骤三中的单应矩阵计算相机位姿

b)步骤五计算地图特征map。

c)计算地图尺度

d)利用光束平差法对地图进行优化，获得优化后的地图map。

e)完成地图初始化。

(16)如果地图完成初始化，则利用图1所示的卡尔曼滤波器进行处理

a)利用步骤三中的PnP方法计算相机位姿

b)利用步骤四中的(3)，计算AUV速度

c)利用步骤二中的(3)，计算经纬度

d)利用步骤四中的(2)，计算卡尔曼滤波器状态量。

e)利用步骤四中的(4)，计算卡尔曼滤波器观测量。

f)利用步骤四中的(5)，算最优位置误差估计获得

g)进行误差补偿

h)利用步骤五计算地图特征map。

i)利用光束平差法对地图进行优化，获得优化后的地图map。

(17)重复(7)～(16)直至导航任务停止。

Claims (2)

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)删除所有不在扇面范围内的

匹配特征点对。

Images