一种无人艇的多传感器辅助定位方法

技术领域

本发明涉及无人艇定位技术领域，尤其涉及一种无人艇的多传感器辅助定位方法。

背景技术

无人艇在水域上运行，最需要的是解算出自身的位姿，有了位姿无人艇方可进行定位与导航。现有的无人艇主要在应用卫星导航(GPS)进行定位，GPS的获取数据，经过解算算法得出X，Y，Z轴的数据，定位出无人艇的当前位置。GPS的定位方法可以全天候，全球性都很出色，但是缺点易容易受到电磁干扰并且GPS的更新频率在1hz-10hz，无人艇的水域运行得不到实时性的满足；

而另一种技术则是采用惯性测量单元(IMU)，惯性测量单元搭配在无人艇，通过获取惯性测量单元的角加速度和加速度，积分计算出惯导当前的速度与位置，从而获取无人艇的当前姿态，惯导的导航系统数据频率更新快，输出信息多，是缺点在于无人艇的运作是需要长时间的，IMU具有累积误差的致命缺点，无人艇无法准确定位，系统的误差大。

上述两种技术对于无人艇自身的位姿的计算求解均无法达到最佳，因此目前急需一种在解决上述现有技术的同时能够最优解算出无人艇自身位姿的方法。

发明内容

本发明的目的在于针对背景技术中的缺陷，提出一种无人艇的多传感器辅助定位方法，实现最优解算无人艇自身位姿的效果。

为达此目的，本发明采用以下技术方案：

一种无人艇的多传感器辅助定位方法，搭载于定位系统，所述定位系统包括GPS、IMU、双目相机和单目相机，所述定位方法具体步骤如下：

步骤一：判断所述定位系统是否初始化，若否，则返回所述定位系统的初始化阶段，循环等待至所述GPS、IMU、双目相机和单目相机的数据不为空；若是，则执行步骤二；

步骤二：提取ROU区域并读取IMU和GPS的数据，根据深度学习的结果动态确定滑窗窗口大小，具体包括：

双目相机线程进行粒子滤波实现跟踪目标，根据深度学习的分类结果进行动态投放粒子的数量；

根据粒子追踪的目标进行计算双目相机的位姿；

步骤三：将双目相机的位姿、IMU的位姿和GPS的位姿进行融合；

步骤四：构造所述定位系统的卡尔曼的量测方程；

步骤五：获取卡尔曼的增益，确定定位系统的误差状态并获得无人艇的位置、速度和姿态，更新卡尔曼的系统协方差。

优选的，在步骤三中，包括获取GPS的数据，具体步骤如下：

根据公式(1)-公式(3)确定卫星在轨道平面的位置；

x′

y′

z′

其中：

(x′

u

r

根据公式(4)获取升交点赤经；

其中：

Ω

Ω

t

t

根据公式(5)-公式(7)将卫星在轨道直角坐标系中的坐标位置(x′

x

y

z

其中：i

基于上述坐标位置获取无人艇的当前位置。

优选的，在步骤三中，包括获取IMU数据，具体步骤如下：

获取无人艇的位置p(t)和速度v(t)，包括：

其中：

a(t)表示利用传感器读取的加速度；

a

g表示重力；

使用4阶Runge-Kutta积分对速度和位置进行更新；

更新速度包括：

其中：

k

k

k

k

a

Δt表示相隔的两个时刻时间Δt＝t

v

更新位置包括：

K

K

K

K

其中：

K

K

K

K

p

优选的，在步骤三中，将IMU和GPS的位姿融合形成无人艇第一个融合位姿，具体步骤如下：

Z

其中：

Z

K

P

P

δn

δn

δp表示P

δv表示V

8w表示设定噪声0.0047。

优选的，包括获取双目相机的位姿，具体步骤如下：

提取ROI区域特征点，包括：

获取一帧图片，选择感兴趣区域，以一个像素点P为原点，P点的亮度为l

l

遍历感兴趣区域，提取所有满足条件的特征点；

获取特征点后得到特征点的领域像素梯度分布，根据梯度公式划分领域窗口的大小，获得特征描述子。

优选的，获取双目相机的位姿还包括如下步骤：

粒子算法的跟踪，包括：

初始化：获取到目标模板的特征描述子，在以该目标模板的位置为中心开始向全图随机撒M个粒子，粒子集为Ψ

通过匹配点对集合(P

重采样：将粒子不断更新移动速度来改变粒子的位置的方式修改为按照粒子相似度来重新随机撒粒子，随机选取相似度最高的n个粒子，根据公式(22)获得以每个粒子为中心的一定范围内需要重新投放的粒子数，总共投放的粒子数应等于N个；

其中：

g表示n个粒子的相似度之和；

C

粒子滤波跟踪：第t时刻目标的位置是粒子集Ψ

其中：

优选的，获取双目相机的位姿还包括：

特征点更新：基于Body-frame的姿态约束特征点更新，所述双目相机构造三维相机坐标系，为卡尔曼滤波求解最有位姿提供三维信息，具体包括：

H＝KRk′

x

其中，H表示预测点矩阵；

k′表示标定参数；

R表示Body-frame坐标系的位姿矩阵；

x

优选的，构造量测方程包括：

其中：

r

n

将公式(17)投影到左零空间V获得：

其中：

进行EKF滤波，获得最优位姿，包括：

对左零空间对状态向量雅克比

其中：

r′表示矩阵分解后的结果；

T

n′表示噪声；

n′的噪声协方差R为：

R＝σ

其中：

σ

I表示单位矩阵；

获取卡尔曼的增益K，包括：

其中：

P表示状态协方差；

根据卡尔曼的增益K和对左零空间对状态向量雅克比

Δx＝Kr′--公式(31)；

恢复定位系统的状态x

x

其中：x

更新定位系统的状态协方差矩阵P

P

其中：

K

优选的，所述深度学习包括：

通过预先训练好的联级回归模型的多尺度卷积神经网络，预测待定场景的类别，其中，每一个精化层包括特征提取器、特征融合器和回归器；

在训练好的模型中输入图片得出分类结果，依据特征提取器的数据，归一化特征点获取滑窗大小因子。

本发明相对于现有技术来说所达到的有益效果：

本发明相对于传统的定位解算算法，融合了多个传感器的数据，弥补了各个传感器的缺点实现1+1＞2的效果，利用IMU的更新频率快的特点实现载体的实时性要求，融合GPS后弥补IMU本身具有发散的缺点，同时补上GPS的更新频率慢，相机的特征点提取采用粒子滤波的思想，实现更好的持续捕捉并且追踪特征点，相机的特征点预测加入了载体坐标系的相对旋转进行约束。现更好的特征点更新，弥补上相机的尺度变化问题，使得鲁棒性更佳。深度学习的计算结果，对不同的场景应用不同的初始参数，以便更好解算位姿。

本发明可以在GPS的无信号的环境，GPS的数据紊乱，融合时候，系统更考虑IMU的数据与视觉可靠性。对于长时间的运作IMU数据错误也可以计算出好的无人艇状态，也可以在视觉捕捉的数据过差也能精准定位无人艇的当前状态。同时对于传感器的异常例如：传感器的损坏等因素造成数据错误。具有一定的纠正错误能力，即在单一传感器损坏依然可以正常定位无人艇的状态。在数据完整的场景，无人艇系统可以提高系统的稳定性，快速性，准确性。以深度学习反馈的结果为参数因子决定系统投放的粒子数和滑窗大小以及常见的初始化参数。动态调节系统的计算。数据残缺可以加大上一次的数据计算完成下一个时刻的估计定位状态。完成定位无人艇系统的状态。

本发明的精度较高，使用滤波的方法使得定位出无人艇的当前专利使用的专利使用方法较快解算出当前的无人艇数据，相比传统的方法具有使用场景约束较小状态更具有高精度性。

附图说明

图1是本发明的一个实施例的定位系统硬件结构示意图；

图2是本发明的一个实施例的定位系统数据传输示意图；

图3是本发明的一个实施例的定位系统运行框架图；

图4是本发明的一个实施例的定位系统的数学模型图；

图5是本发明的一个实施例的IMU与GPS的时间差关系图；

图6是本发明的一个实施例的GPS与IMU的运算关系图；

图7是本发明的一个实施例的相机追踪示意图；

图8是本发明的一个实施例的传感器融合顺序图；

图9是本发明的一个实施例的深度学习的使用流程图；

图10是本发明的一个实施例的神经卷积网络训练图；

图11是本发明的一个实施例的分类结果的无人艇应用图。

具体实施方式

下面结合附图并通过具体实施方式来进一步说明本发明的技术方案。

本发明的一种无人艇的多传感器辅助定位方法，搭载于定位系统，本发明为无人艇的定位提供了一种新的思路，系统由相机，IMU，GPS来组成，利用IMU读取的位姿和GPS读取的位姿进行数据融合，对两个数据进行权值融合，相机的特征点识别使用粒子滤波提取特征进行持续追踪，载体系融合后的数据与相机构成的三维坐标构造残差量测模型，对模型进行卡尔曼的算法定位出当前无人艇的位姿，以便于后续的工作。

对于水域上面的场景具有单一化的情况使用深度学习计算出场景，对不同的场景加载不同的设定数据以便于更精确的解算位姿。不同场景由深度学习反馈的参数为滑窗大小因子决定粒子滤波的投放粒子数目与滑窗的窗口大小。特征点少，双目相机投放的粒子多去捕捉特征点以及滑窗的窗口变大，对老帧的计算更多确定的无人艇状态更为精准。特征点多，双目相机投放的粒子少去捕捉特征点以及滑窗的窗口小，对新帧的计算更多确定的无人艇状态更为精准。因此，深度学习的分类结果反馈的滑窗大小因子可以动态的变化滑窗窗口大小

本发明提出滤波的视觉与GPS和IMU的融合，以及加以深度学习的辅助的方法去定位出无人艇的当前状态包括位置，速度，姿态。

如图1和图2所示，所述定位系统包括GPS、IMU、双目相机和单目相机；

如图3所示，所述定位方法具体步骤如下：

步骤一：判断所述定位系统是否初始化，若否，则返回所述定位系统的初始化阶段，循环等待至所述GPS、IMU、双目相机和单目相机的数据不为空；若是，则执行步骤二；

步骤二：提取ROU区域并读取IMU和GPS的数据，根据深度学习的结果动态确定滑窗窗口大小，具体包括：

双目相机线程进行粒子滤波实现跟踪目标，根据深度学习的分类结果进行动态投放粒子的数量；

根据粒子追踪的目标进行计算双目相机的位姿；

步骤三：将双目相机的位姿、IMU的位姿和GPS的位姿进行融合；

步骤四：构造所述定位系统的卡尔曼的量测方程；

步骤五：获取卡尔曼的增益，确定定位系统的误差状态并获得无人艇的位置、速度和姿态，更新卡尔曼的系统协方差。

本发明通过解算出系统的当前的位置，速度，姿态数据，在系统初始化后把深度学习的分类结果反馈出来的滑窗大小因子加入解算之前，然后将预设的参数加入无人艇初始系统中，无人艇的相机手动提取ROI区域，IMU，GPS读取数据。系统的相机部分采用粒子滤波实现持续追踪特征点并且左右相机求解相机位姿，多传感器融合存在时间差问题，因此根据各个传感器时间差问题，IMU的数据处理一直处理，当有相机数据，GPS数据来临时就进行多传感器融合。对于融合后，构造相机重投影误差量测模型，进行计算卡尔曼的增益，最终达到解算出最优的位置，速度，姿态。

本发明的数学模型如图4所示：

World表示世界坐标系，Body Frame表示载体坐标系，Cam表示相机坐标系。组合传感器将多个传感器的优势集合在一起，形成一个约束条件解算出当前的姿态，位置，速度；

在图4中，o

系统由一个拥有左右侧相机的双目立体相机，单目的相机以及IMU和GPS传感器组合而成。系统的工作主要双目相中的左右侧相机立体匹配后由，双目相机进行粒子滤波，计算相机位姿。此时，IMU与GPS的位姿也进行一个加权融合，产生一个IMU与GPS的融合位姿与相机的位姿再进行传感器融合优化。系统的更新主要由视觉更新完成，由观测特征点确定量测方程，对量测方程简化。经过卡尔曼计算估计出最优的姿态，速度，位置。单目相机的工作主要是深度学习计算当前特征点数目判断应用场景是否是环境复杂场景还是单一的空旷环境场景。返回数据可以决定投放粒子数目和滑动窗口的大小。

优选的，在步骤三中，包括获取GPS的数据，具体步骤如下：

GPS可以提供无人艇的当前位置，获取无人艇的当前位置与速度。那么极坐标表示升交点角距和卫星矢径长度即可确定卫星在轨道平面的位置(x′

根据公式(1)-公式(3)确定卫星在轨道平面的位置；

x′

y′

z′

其中：

(x′

u

r

根据公式(4)获取升交点赤经；

其中：

Ω

Ω

t

t

根据公式(5)-公式(7)将卫星在轨道直角坐标系中的坐标位置(x′

x

y

z

其中：i

基于上述坐标位置获取无人艇的当前位置。

优选的，在步骤三中，包括获取IMU数据，具体步骤如下：

IMU的更新频率达到200Hz，IMU得到原始数据。无人艇需要计算重力因素和误差，为了求出零偏，比例因子，交轴耦合等误差，本系统采用Kalibr工具箱标定，通过旋转3个轴，针对加速度计与陀螺仪同时标定。得到标定后数据，这里的加速度积分得到速度，速度积分得到位置。积分过程选择了RK4的积分方法。积分的原理是近似的办法，利用多个斜率加权近似整段函数的斜率；

获取无人艇的位置p(t)和速度v(t)，包括：

其中：

a(t)表示利用传感器读取的加速度；

a

g表示重力；

使用4阶Runge-Kutta积分对速度和位置进行更新；

更新速度包括：

其中：

k

k

k

k

a

Δt表示相隔的两个时刻时间Δt＝t

v

更新位置包括：

K

K

K

K

其中：

K

K

K

K

p

利用RK4的迭代计算出IMU的当前位置，速度，位置以及偏差。

优选的，如图5和图6所示，无人艇单一的传感器对于定位是极其不利的，首先我们进行GPS和IMU的传感器数据，融合主要利用惯导的误差方程作为系统的方程，利用惯导的输出的位置，速度以及卫星系统的输出位置，速度做差作为系统的量测量，以便进行后续的卡尔曼滤波。

惯导的频率是要比GPS的频率快的，因此出现了一个时间差。在组合中，在有INS的信息就更新INS信息存储在缓冲数组中，当GPS的信息到来才可以进行处理做差计算；

在步骤三中，将IMU和GPS的位姿融合形成无人艇第一个融合位姿，具体步骤如下：

Z

其中：

Z

K

P

P

δn

δn

δp表示P

δv表示V

δw表示设定噪声0.0047。

GPS和IMU的融合形成无人艇的第一个融合位姿信息，实现较为可靠的数据。

优选的，如图7所示，系统不仅仅有惯导和卫星的约束估计位姿，还有相机的约束，做出最优估计。本系统采用Intel RealSense T265带IMU的追踪鱼眼相机，可以利用粒子滤波优化左眼追踪特征点，采用自适应Ransac算法剔除误匹配点在左右眼匹配成优化后的三维特征点，利用IMU与GPS的融合结果的相对转动变量R估计下一个特征点的位置；

包括获取双目相机的位姿，具体步骤如下：

提取ROI区域特征点，包括：

获取一帧图片，选择感兴趣区域，以一个像素点P为原点，P点的亮度为l

l

遍历感兴趣区域，提取所有满足条件的特征点；

获取特征点后得到特征点的领域像素梯度分布，根据梯度公式划分领域窗口的大小，获得特征描述子。

特征点匹配通常会发生误匹配点，利用自适应Rahsac算法剔除误匹配点。特征点的过多导致不可以使用最小二乘法拟合特征点，使用Ransac将特征点分为局外点与局内点，降低计算矩阵的维度。

优选的，获取双目相机的位姿还包括如下步骤：

粒子算法的跟踪，包括：

初始化：获取到目标模板的特征描述子，在以该目标模板的位置为中心开始向全图随机撒M个粒子，粒子集为Ψ

通过匹配点对集合(P

重采样：为了提高程序响应速度，将粒子不断更新移动速度来改变粒子的位置的方式修改为按照粒子相似度来重新随机撒粒子，随机选取相似度最高的n个粒子，根据公式(22)获得以每个粒子为中心的一定范围内需要重新投放的粒子数，总共投放的粒子数应等于N个；

其中：

g表示n个粒子的相似度之和；

C

粒子滤波跟踪：第t时刻目标的位置是粒子集Ψ

其中：

优选的，获取双目相机的位姿还包括：

特征点更新：粒子算法不具备尺度更新与感兴趣区域内的特征点更新，在长时间的追踪容易发生形变导致匹配错误，采用Body-frame的姿态约束特征点更新，并且双目相机可以构造出三维的相机坐标系，可以为后续的卡尔曼滤波求解最优位姿提供三维信息；

基于Body-frame的姿态约束特征点更新，所述双目相机构造三维相机坐标系，为卡尔曼滤波求解最有位姿提供三维信息，具体包括：

H＝KRk′

x

其中，H表示预测点矩阵；

k′表示标定参数；

R表示Body-frame坐标系的位姿矩阵；

x

优选的，如图8所示，载体系下的坐标是经过GPS和IMU的数据融合，在相机数据没有到来之前，一直更新IMU，相机额更新频率比GPS快，系统先和相机融合再以GPS的信息矫正IMU的累积误差，系统状态矩阵增广进行了两次；对载体估计要以误差作为状态向量，这样的目的在于系统处于强非线性化的情况下，偏差接近0值可以进行线性化误差比较小，根据上述的GPS/INS解算与相机的特征点追踪进行量测模型构造；

构造量测方程包括：

其中：

r

n

将公式(17)投影到左零空间V获得：

其中：

进行EKF滤波，获得最优位姿，包括：

对左零空间对状态向量雅克比

其中：

r′表示矩阵分解后的结果；

T

n′表示噪声；

n′的噪声协方差R为：

R＝σ

其中：

σ

I表示单位矩阵；

获取卡尔曼的增益K，包括：

其中：

P表示状态协方差；

根据卡尔曼的增益K和对左零空间对状态向量雅克比

Δx＝Kr′--公式(31)；

恢复定位系统的状态x

x

其中：x

更新定位系统的状态协方差矩阵P

P

其中：

K

优选的，无人艇的场景比较特殊，水域上的情况，相机大多数的情况可能捕捉空旷的水面，双目特征点的统计办法不能区分不同的场景，设定参数单一化使得无人艇在水域上的定位运行效果不佳，及时的切换里程计的参数是十分必须的，因此使用经过训练的神经网络对大部分场景进行识别；

本系统采用级联回归的模型的多尺度卷积神经网络的方法进行场景预测，深度学习的方法流程如图9所示；

所述深度学习包括：

通过预先训练好的联级回归模型的多尺度卷积神经网络，预测待定场景的类别，其中，每一个精化层包括特征提取器、特征融合器和回归器；

在训练好的模型中输入图片得出分类结果，依据特征提取器的数据，归一化特征点获取滑窗大小因子。

如图10所示：

Cov1_1：第一个网络分支的第一个卷积层；

Cov3_1：第一个网络分支的第三个卷积层；

Cov3_2：第二个网络分支的第三个卷积层；

Cov3_3：第三个网络分支的第三个卷积层；

Fc_1：第一个全连接层；

深度学习捕捉的场景反馈滑窗大小因子参数，及时调制位姿计算，提升计算速度以及精准度。在分类结果为空旷区域即特征点少，就滑窗的窗口变大，计算的无人艇位姿更为准确。相反，特征点少，计算滑窗大小变小，提升计算速度与精准度。无人艇在深度学习的卷积网络作用如图11所示。

以上结合具体实施例描述了本发明的技术原理。这些描述只是为了解释本发明的原理，而不能以任何方式解释为对本发明保护范围的限制。基于此处的解释，本领域的技术人员不需要付出创造性的劳动即可联想到本发明的其它具体实施方式，这些方式都将落入本发明的保护范围之内。