CN111426320B - 一种基于图像匹配/惯导/里程计的车辆自主导航方法 - Google Patents

Abstract

本发明提供了一种基于图像匹配/惯导/里程计的车辆自主导航方法。在需要定位的地面车辆上配置一架装备了对地拍照相机的无人机；启动车载惯导/里程计组合导航***，车辆在运动条件下由驾驶人员判断无人机是否具备起飞条件；如果具备起飞条件则UAV自主起飞并自主跟踪车辆，同时对地拍照并利用拍摄的实时图像和提前制作的基准图匹配，实现地面车辆的定位；得到定位结果后发送给车辆导航计算机进行数据融合；得到车辆定位结果后，无人机自主降落返回车辆。本发明提出了一种新的导航方案，利用无人机搭载航拍相机，将实时航拍图像与基准图库进行模板匹配，从而确定实时航拍图中车辆的地理坐标，将匹配定位结果修正惯导/里程计的组合导航误差。

Description

一种基于图像匹配/惯导/里程计的车辆自主导航方法

技术领域

本发明涉及定位导航技术领域，具体涉及一种基于图像匹配/惯导/里程计的车辆自主导航方法。

背景技术

车辆定位常用的方法是基于卫星导航(GNSS)和惯性导航(INS)融合的导航方法。这种导航方法的缺陷是在GNSS信号受到干扰、遮挡或欺骗的情况下，组合导航***的导航性能会显著下降，无法满足车辆导航需求。随着智能技术的发展，对不依赖外部信息的自主导航技术的需求不断增加，典型的应用包括无人车和军用特种车辆等。可以预计，未来无人车将广泛应用于物流、人员运输、巡逻和测绘等领域，在这些应用中，无人车的行驶区域复杂多变，由于GNSS信号的干扰、遮挡和惯性导航***的误差累积等原因，如果采用传统的GNSS/INS组合导航则无法实现无人车在所有行驶区域的可靠导航，无人车与军用车辆的可靠导航有赖于自主导航***的发展。自主导航***，是指不依赖于***外部的信号交互即可实现持续可靠导航的***。

车辆定位技术用于精确确定车辆所在位置的地理坐标，目前常用的车辆自主定位方法有惯性导航和里程计的组合导航，惯性导航、里程计和路标点的组合导航，以及惯导和视觉SLAM的组合导航等方法。下面分别讨论其在车辆自主导航中的应用情况。

惯性导航和里程计的组合一般通过卡尔曼滤波实现，按照观测量的不同可分为以下三种方法。第一种方法通过里程计进行位置解算，并以里程计位置与惯性导航解算位置的差值作为卡尔曼滤波器的观测量。第二种方法以里程计解算得到的速度作为参考速度，并利用里程计速度和惯性导航解算的速度的差值作为卡尔曼滤波的观测量。第三种方法使用在特定的采样间隔内里程计解算的位置增量和惯导解算的位置增量的差值作为滤波观测量进行融合，避免了以速度误差作为观测量时可能存在随机误差放大的问题。通过里程计和惯性导航的融合，可以显著提升导航***的性能，减缓导航误差的累积,但是由于里程计的测量信息中不包含绝对位置信息，组合导航***的误差会由于里程计的测量误差和惯性传感器的漂移等问题随时间积累，同时考虑到车辆的打滑和运动耦合误差，***的导航误差最终仍然会随时间发散。

为了抑制惯性导航和里程计组合导航误差的发散，惯导、里程计和路标点的组合导航方案被提出，该方案通过车载相机、雷达等测距测角传感器测量点和线的几何关系建立角度和位置测量模型，并基于多状态约束卡尔曼滤波对惯导和里程计组合导航的积累误差进行修正。此方案的合理使用可以消除前面讨论的惯导和里程计组合导航的累积误差，但该方案的缺点是需要预先在特定位置设置路标点，路标点的建设和维护工作量巨大，并带来成本增加，难以推广应用。

惯性导航和视觉SLAM组合导航***通过视觉传感器对周边的环境进行感知来解算车辆的位置和姿态信息，并将解算的位置和姿态信息与惯导进行融合优化获得车辆最优的导航解。由车载相机获取周边物体的影像信息，通过多帧照片之间的匹配计算车辆相对于周边环境的位置和姿态，并与惯导***通过滤波进行融合。基于滤波的数据融合方法鲁棒性高，计算复杂度低，但是无法用到全局信息对导航结果进行优化。基于此一种全局优化算法通过关键帧匹配技术，用一个统一的代价函数同时优化视觉和惯导的位姿信息，算法具有较高的精度和实时性。由于视觉SLAM导航是通过光学成像原理计算得到车辆与周围物体的相对位置，而非车辆的绝对地理坐标，所以视觉SLAM和惯导/里程计的融合导航会存在持续的误差累积。如果车辆的轨迹是闭合的，通过闭环检测可以消除这种累积误差，但是闭环检测的条件对车辆的运动轨迹进行了约束，对车辆行驶轨迹进行约束这在通常的车辆运动中是无法保证的。

综上所述，急需一种基于图像匹配/惯导/里程计的车辆自主导航方法以解决现有技术中存在的问题。

发明内容

本发明目的在于提供一种基于图像匹配/惯导/里程计的车辆自主导航方法，旨在解决车辆自主导航常见方法精度较低，应用场景受限等问题。

为实现上述目的，本发明提供了一种基于图像匹配/惯导/里程计的车辆自主导航方法，包括以下步骤：

步骤一：在需要定位的地面车辆上配置一架装备了对地拍照相机的无人机；

步骤二：启动车载惯导/里程计组合导航***，车辆在运动条件下由驾驶人员判断无人机是否具备起飞条件；

步骤三：如果具备起飞条件则UAV自主起飞并自主跟踪车辆，同时对地拍照并利用拍摄的实时图像和提前制作的基准图匹配，实现地面车辆的定位；

步骤四：得到定位结果后发送给车辆导航计算机进行数据融合；

步骤五：得到车辆定位结果后，无人机自主降落返回车辆。

进一步地，步骤三中无人机对车辆自主跟踪控制的具体方法是：在UAV距离车辆大于10米时，采用基于UWB的技术对UAV进行控制；在UAV距离车辆距离小于10米时，采用基于嵌套二维码识别的技术对UAV进行控制。

进一步地，基于嵌套二维码的UAV定位过程包括：

A、在车顶部配置二维码图像，并标定出图像中特征点的位置坐标，

B、通过UAV上的机载摄像机实时检测降落平台上的二维码标志获取UAV的相对位置和姿态信息；

C、利用P3P算法进行定位解算，已知条件为二维码特征点A,B,C,D在二维码局部坐标系下的坐标，通过小孔成像原理和几何约束求解得到特征点A,B,C,D在相机坐标系X_C,Y_C,Z_C,下的坐标，构成了二维码坐标系与相机坐标系之间的转换，得到无人机相对车辆的位置和姿态。

进一步地，无人机拍摄的实时图像与基准图像的匹配采用基于结构张量的异源图像快速匹配方法实现，包括张量提取、张量分析和张量匹配三个步骤。

进一步地，所述张量提取的具体方法是：设原图像I大小为m×m像素，灰度矩阵为I(x,y),f是高斯滤波器，通过公式(2)计算得到图像水平x方向梯度g_x和垂直y方向梯度g_y；

根据公式(3)计算图像结构张量T；

对结构张量进行分解：

λ_i和e_i分别是二维矩阵T的特征值和特征向量，λ₁-λ₂代表张量的曲线显著性。

进一步地，所述张量分析的具体方法是：将图像分为平坦区域R_f、平滑结构区域R_s和非平滑结构区域R_c；

所述平坦区域对应图像中无纹理或纹理较弱的区域，计算过程如下：

R_f＝{P|Trace(T)＜th,P∈I} (5)

式中，th为设定阈值，I为图像所有像素；

所述平滑结构区域对应图像的边缘部分，计算过程如下：

R_s＝{P|λ₁-λ₂＞λ₂,P∈I-R_f} (6)

非平滑结构区域对应图像的角点或噪声严重的部分，计算过程如下：

R_c＝{P|λ₁-λ₂≤λ₂,P∈I-R_f}。 (7)

进一步地，所述张量匹配的具体方法是：用L₁范数计算结构张量之间的差别来对基于结构张量的本征图像进行匹配；L₁范数就是差的绝对值之和，计算公式如下：

t_i和t_i′代表两幅图像上每个像素对应的结构张量的第i个元素，将实时图选出的模板在基准图中滑动，计算所有滑动匹配区域内所有像素点的L₁并进行归一化，L₁数值越小代表匹配程度越高，0代表区域最为相似，1代表最不相似区域。

进一步地，数据融合采用卡尔曼滤波结构，卡尔曼滤波的***方程为：

其中，X表示***的状态变量，U表示过程噪声变量，F表示***矩阵，具体定义如下：

式中，δrⁿ,δvⁿ,ψⁿ分别表示投影到导航坐标系n的位置误差、速度误差和姿态误差；

εⁿ表示投影到导航坐标系n的加速度计零偏和陀螺仪零偏；δd表示里程计误差；

u_r,u_v,u_ψ,

u_ε,u_d分别表示位置误差的驱动噪声、速度误差的驱动噪声、姿态误差的驱动噪声、加速度计零偏的驱动噪声、陀螺零偏的驱动噪声、里程计误差的驱动噪声；

***矩阵F可表示为：

进一步地，式(19)中的非零矩阵F_ij具体如下：

式中，i表示惯性坐标系，n表示导航坐标系，选择当地水平的北东地坐标系为导航坐标系；e表示地球坐标系，坐标原点在地心，X轴在赤道平面内指向本初子午线，Z轴沿着地球自转轴指向北极点；b表示载体坐标系，坐标原点位于惯导设备的中心，X轴指向前，Z轴指向下；

fⁿ表示投影到导航坐标系n的由加速度计测量得到的比力测量值；

表示导航坐标系n相对于惯性坐标系i的角速度投影到导航坐标系n中；

表示由载体坐标系到导航坐标系的姿态变换矩阵；下标N、E、D表示导航坐标系变量在北、东、地方向的分量。

进一步地，所述惯导里程计组合导航的观测方程：

其中，δVⁿ表示惯导的速度误差矢量，ψⁿ表示惯导的姿态误差矢量，δd表示里程计的安装误差矢量；K_D表示里程计的比例因子；P_D表示里程计输出的原始脉冲数；I_A表示矩阵

惯导与视觉定位组合导航的***观测方程为：

Z_B＝δrⁿ+w_rB (25)

其中δrⁿ，w_rB分别为观测变量和噪声变量。

应用本发明的技术方案，具有以下有益效果：

(1)本发明提出了一种新的导航方案，利用无人机搭载航拍相机，将实时航拍图像与基准图库进行模板匹配，从而确定实时航拍图中车辆的地理坐标，将匹配定位结果修正惯导/里程计的组合导航误差。

(2)本发明中，惯导、里程计、相机三种传感器的数据融合通过一种双速卡尔曼滤波结构进行最优估计，能够克服传统卡尔曼滤波方法不利于解决数据接收不同步，数据频率不同的问题。

(3)本发明采用一种无人机视觉辅助惯导/里程计的地面车辆组合导航方案，能够克服传统导航手段受天气环境、行驶路线限制和误差累积等问题。作为一个高精度的自主导航方法，由于传感器的独立性，专利所提出的方案可以广泛应用于军用和民用领域上，具有抗干扰性强和精度高的特点。并且所采用的卡尔曼滤波方法能够克服数据源不同步和数据源的数据频率不同步问题，为图像匹配、惯导和里程计的数据融合提供便利。

除了上面所描述的目的、特征和优点之外，本发明还有其它的目的、特征和优点。下面将参照图，对本发明作进一步详细的说明。

附图说明

构成本申请的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1是无人机辅助车辆定位流程图；

图2是UWB定位示意图；

图3是二维码定位示意图；

图4是组合导航卡尔曼滤波示意图；

具体实施方式

以下结合附图对本发明的实施例进行详细说明，但是本发明可以根据权利要求限定和覆盖的多种不同方式实施。

实施例1：

参见图1～图4，一种基于图像匹配/惯导/里程计的车辆自主导航方法，包括以下步骤：

步骤一：在需要定位的地面车辆上配置一架装备了对地拍照相机的无人机；

步骤二：启动车载惯导/里程计组合导航***，车辆在运动条件下由驾驶人员判断无人机是否具备起飞条件；

步骤三：如果具备起飞条件则UAV(无人机)自主起飞并自主跟踪车辆，同时对地拍照并利用拍摄的实时图像和提前制作的基准图匹配，实现地面车辆的定位；

步骤四：得到定位结果后发送给车辆导航计算机进行数据融合；

步骤五：得到车辆定位结果后，无人机自主降落返回车辆。

由图1可以看到，在无人机视觉辅助车辆定位的流程中，首先需要解决的是无人机对车辆的自主跟踪控制，UAV拍照的飞行高度大概在100米左右，考虑到定位自主性和飞行高度跨度较大。为了提高控制的可靠性，本实施例采取在UAV距离车辆较远(＞10米)时采用基于UWB(超宽带)的技术对UAV进行控制；在UAV距离车辆距离较近(＜10米)时采用基于嵌套二维码识别的技术对UAV进行控制。示意图如图2、图3所示：

如图2所示，远距离时通过UWB进行定位控制，原理与GPS类似可以通过已知坐标的基站发射超宽带确定无人机的位置。已知基站s₁，s₂，s₃，s₄的坐标为(x₁,y₁)、(x₂,y₂)、(x₃,y₃)、(x₄,y₄)，可求出UAV到基站的距离，根据方程组实时算出UAV的位置。

获得UAV相对于车辆的距离后，可基于此距离信息实现UAV对车辆的跟踪控制。基于UWB技术的定位***能够满足在UAV距离车辆大于10米时的跟踪控制需求。通过UWB无线测距模块，设计组网模型，进行基站配置优化，实现UAV的位置解算，能够满足UAV自主跟踪车辆的需求。

随着UAV与车辆的距离逐渐减小，此时采用基于嵌套二维码的定位用于UAV控制比较可靠。如上图3所示，通过近距离高分辨的视觉图像进行定位，能显著提高UAV的定位精度，同时可以获得UAV相对于车辆的姿态信息，有利于UAV自主降落的控制实现。

基于嵌套二维码的UAV定位过程如下。首先在车顶部配置二维码图像，并标定出图像中特征点的位置坐标，再通过UAV上的机载摄像机实时检测降落平台上的二维码标志获取UAV的相对位置和姿态信息。利用P3P算法进行定位解算，已知条件为二维码特征点A,B,C,D在二维码局部坐标系下的坐标，通过小孔成像原理和几何约束求解得到特征点_A,B,C,D在相机坐标系X_c,Y_c,Z_c,下的坐标，构成了二维码坐标系与相机坐标系之间的转换，得到无人机相对车辆的位置和姿态。

PNP算法是通过相机获得照片上的特征点的像素坐标以及已知的世界坐标系下同名特征点的世界坐标，解算对应拍摄时刻下的相机位置和姿态。

图像匹配定位是通过对影像内容、特征、结构、关系、纹理及灰度等的对应关系，相似性和一致性的分析，寻求相似影像目标的方法。在无人机能够掌握自主跟踪降落技术之后，需要对地面车辆进行图像匹配定位，即实现UAV(无人机)拍摄的实时图像与基准图像的匹配，完成匹配后，根据基准图中包含的坐标信息，计算出车辆的坐标位置，实现车辆定位。为了保证定位的实时性，需要快速实现实时图像和基准图像的匹配，同时考虑到实时图和基准图的异源特性，本发明专利采用基于结构张量的异源图像快速匹配方法进行实时图和基准图的匹配，主要分为张量提取、张量分析和张量匹配三个步骤。

首先是张量提取。设原图像I大小为m×m像素，灰度矩阵为I(x,y),f是高斯滤波器，通过公式(2)计算得到图像水平x方向梯度g_x和垂直y方向梯度g_y。

之后按照公式(3)计算图像结构张量。

结构张量可以按照如下公式进行分解：

λ_i和e_i分别是二维矩阵T的特征值和特征向量。λ₁-λ₂代表张量的曲线显著性，λ₁-λ₂越大说明张量对应的像素越有可能在曲线结构上，对二维图像，曲线结构通常对应边缘特征。

其次是张量分析。通过张量分析可以将图像分为平坦区域R_f、平滑结构区域R_s和非平滑结构区域R_c三个组成部分。其中平坦区域对应图像中无纹理或纹理较弱的区域，其结构张量的特点是张量矩阵的迹较小，因此可以根据张量矩阵的迹划分出平坦区域，纹理不明显不参与图像匹配，计算过程如下，th为设定阈值，I为图像所有像素。

R_f＝{P|Trace(T)＜th,P∈I} (5)

平滑结构区域往往对应图像的边缘部分，其结构张量的特点是曲线显著性突出，选择平滑区域作为模板匹配区域可以提高匹配的精度，计算如下：

R_s＝{P|λ₁-λ₂＞λ₂,P∈I-R_f} (6)

模板匹配是将模板通过变换映射到基准图像的某个窗口上，然后对两幅图像进行逐像素地比较，并根据差别或相似性来选择一个对应窗口作为匹配结果。

非平滑结构区域往往对应图像的角点或噪声严重的部分，其结构张量的特点是曲线显著性不突出，且特征值较大，计算如下：

R_c＝{P|λ₁-λ₂≤λ₂,P∈I-R_f} (7)

最后是张量匹配。结构张量的匹配用L₁范数计算结构张量之间的差别来对基于结构张量的本征图像进行匹配。L₁范数就是差的绝对值之和，计算公式如下：

t_i和t_i′代表两幅图像上每个像素对应的结构张量的第i个元素，将实时图选出的模板在基准图中滑动，计算所有滑动匹配区域内所有像素点的L₁并进行归一化，L₁数值越小代表匹配程度越高，0代表区域最为相似，1代表最不相似区域。通过上述方法可以对实时航拍图与基准图进行匹配从而确定实时图中车辆的地理坐标。

在完成UAV视觉定位流程后，通过UAV获得地面车辆的绝对地理坐标，需要将视觉定位结果与惯导/里程计数据进行融合。数据融合过程中考虑到数据获取频率特征差异较大，数据无法同步，如惯导为200HZ，而图像匹配定位频率则相对较低(取决于无人机起飞拍照频率)，采取了新的卡尔曼滤波结构对数据进行融合，具体的滤波方法如图4所示：

卡尔曼滤波是一种利用线性***状态方程，通过***输入输出观测数据，对***状态进行最优估计的算法。根据Kalman(卡尔曼)滤波的原理，为了实现上述滤波，首先要确定滤波的***方程和观测方程：

Z＝HX+W (10)

方程(9)是卡尔曼滤波的***方程，描述***状态量X的变化规律，其中X表示***的状态变量，U表示过程噪声变量，F表示***矩阵。方程(10)是卡尔曼滤波的观测方程，描述***观测量Z与***状态量X的变化关系，其中W是观测噪声变量，H表示观测矩阵。

惯导里程计组合导航的观测量来源于里程计测量，惯导/视觉定位组合导航的观测量来源于UAV视觉定位信息，另外用于实现数据融合的Kalman滤波的***方程可以通过扩展惯性导航的误差方程得到。惯导的φ角误差方程可以表示如下

惯性导航是通过测量飞行器的加速度与角速度(惯性)，并自动进行积分运算，获得飞行器瞬时速度、姿态和瞬时位置数据的技术。

公式(11)中上角标和下角标中的字母均表示坐标系。其中，i表示惯性坐标系，该坐标系是相对于惯性空间不动。n表示导航坐标系，一般选择当地水平的北东地坐标系为导航坐标系。e表示地球坐标系，坐标原点在地心，X轴在赤道平面内指向本初子午线，Z轴沿着地球自转轴指向北极点。b表示载体坐标系，坐标原点位于惯导设备的中心，X轴指向前，Z轴指向下。

公式(11)中δrⁿ,δvⁿ,ψⁿ分别表示投影到导航坐标系n的位置误差、速度误差和姿态误差。fⁿ表示投影到导航坐标系n的由加速度计测量得到的比力测量值。

表示导航坐标系n相对于惯性坐标系i的角速度投影到导航坐标系n中。

ε^b表示投影到载体坐标系b的加速度计零偏和陀螺仪零偏。

表示由载体坐标系到导航坐标系的姿态变换矩阵。加速度计

和陀螺仪零偏ε^b可以作为待估计量扩展到Kalman滤波的状态量中，考虑到惯性传感器零偏在估计周期内可以近似为常量，因此其微分量可表示为：

把公式(12)并入公式(11)，惯导的误差方程可以扩展为：

考虑与里程计测速相关的误差参数有三个，δK_D,δα_H,δα_P，分别表示里程计的比例因子误差、航向角安装误差和俯仰角安装误差。可定义矢量δd表示里程计的上述误差：

δd＝[δK_D,δα_H,δα_P]^T (13)

类似陀螺零偏和加表零偏，里程计误差的微分量也可表示为：

把公式(14)并入公式(11)，惯导phi角误差方程可以扩展为：

定义位置误差方程、速度误差方程、姿态误差方程、加速度计零偏方程、陀螺零偏方程、里程计误差方程的驱动噪声分别为u_r,u_v,u_ψ,

u_ε,u_d，则方程(15)可变换为：

根据Kalman滤波的***方程(9)和方程(16)，***状态变量X和***过程噪声变量U的定义如下：

根据Kalman滤波的***方程(9)和方程(16)，***矩阵F可表示为：

非零矩阵F_ij可表示如下，其中(X)_N,(X)_E,(X)_D表示导航坐标系变量在NED方向的分量：

方程(17)-(19)一起定义了方程(9)中给出的Kalman滤波***方程，其中***的状态变量X定义在方程(17)中，***的过程噪声变量U定义在方程(18)中，***矩阵F定义在方程(19)中。

从里程计的测量方程出发推导惯导/里程计组合的观测方程。首先定义里程计坐标系为m。里程计坐标系m的坐标原点可以定义到车辆上的任意位置，此处定义为与载体坐标系b的原点重合。里程计坐标系m的X轴指向车的前进方向，Z轴垂直于X轴指向下。由于惯导在车辆上安装时存在安装误差，坐标系m和坐标系b在X轴、Y轴和Z轴方向上分别存在着横滚角、俯仰角和航向角安装误差，分别表示为α_R,α_P,α_H，代表从坐标系b转向坐标系m的一组欧拉角。

在载体坐标系b中，里程计测量的速度可表示为

和惯导的横滚安装误差角α_R无关，仅和惯导的俯仰安装误差角α_P、航向安装误差角α_H、以及里程计的比例因子K_D有关。通过对方程(20)进行扰动分析，并忽略掉高阶小量，可得到里程计的测量误差方程：

最后化简得到惯导里程计组合导航的观测方程：

该观测方程建立了***观测量Z_A和***状态量X的关系，其中δVⁿ表示惯导的速度误差矢量，ψⁿ表示惯导的姿态误差矢量，δd表示里程计的比例因子和安装误差矢量，P_D表示里程计输出的原始脉冲数；I_A表示矩阵

UAV视觉定位的误差来源比较复杂，相机参数、基准图误差和匹配算法都会带来定位误差，其中有些误差会引起确定性的定位误差，有些误差会引起随机的定位误差。理论上讲，确定性的定位误差可以通过建模在组合导航滤波过程中进行估计计算，为了降低问题的复杂度，本数据融合方法只考虑随机的定位误差，基于上述简化，UAV视觉定位的测量方程类似卫星与惯导组合导航方程，可以表示为：

其中P表示真实无误差的位置，W_p表示UAV视觉定位的随机误差。

P和W_p可分别表示为：

方程(25)为导出的惯导与视觉定位组合导航的***观测方程，δrⁿ，w_rB分别为观测变量和噪声变量。

Z_B＝δrⁿ+w_rB (25)

本发明提出的无人机(UAV)视觉辅助的惯导/里程计的地面车辆定位方案作为自主导航方法的一种，利用UAV视觉定位技术辅助惯导/里程计组合导航能够有效地提高对地面车辆定位精度。自主导航方法惯导/里程计组合导航仍然存在累积误差，为了消除该累积误差，本发明在车上配置一架搭载了相机的小型旋翼无人机，无人机可根据需要自主升空拍照，采用图像匹配定位技术对地面车辆定位，并把定位信息发送给地面车辆导航***进行惯导/里程计组合导航***累积误差的修正，完成定位后无人机自主返回并降落在车辆上。

本发明的核心针对前面描述的已有车辆自主导航方法的缺陷，提出了一种新的基于无人机视觉辅助的惯导和里程计融合的自主导航方案，并采用卡尔曼滤波来实现上述数据的融合。由于来源于惯导、里程计和UAV视觉定位的数据往往数据同步和频率特征差异比较大，无法通过单一的Kalman滤波器实现惯导数据、里程计数据和UAV视觉定位数据的融合，本实施例采用了一种的新的滤波结构来实现上述数据的融合。

车辆自主定位往往需要在不接受外部信号的条件下提供高精度的车辆导航信息，其中惯性导航作为自主导航中最为重要的导航方法会由于惯性传感器的漂移误差随时间积累。通过里程计和惯性导航的融合，虽然减缓了导航误差的累积，但是由于里程计的测量信息中不包含绝对位置信息，里程计和惯导组合导航***的误差会由于里程计的测量误差和惯性传感器的漂移等问题随时间积累。通过添加路标点与惯导/里程计进行组合导航时需要预先在特定位置设置路标点，路标点的建设和维护工作量巨大，并带来成本增加，难以推广应用。传统的地图匹配方法对惯导信息进行修正时，可以有效抑制垂直于道路方向的位置误差，但无法抑制沿着道路方向的误差，意味着长时间导航时，组合导航***仍然存在着导航误差的持续累积问题。视觉SLAM和惯导的组合导航需要车辆行驶条件为闭环检测条件则一定程度上限制了车辆的运动轨迹。

本发明提出一种无人机辅助惯性导航/里程计的地面车辆定位方案，满足车辆自主导航需求，具有不依赖卫星信号、不依赖路标，抗干扰强和定位精度高的特点。同时采取一种新的滤波结构解决了数据融合时来源于惯导、里程计和UAV视觉定位数据的同步和频率特征差异较大的问题。通过本发明能够解决之前车辆自主导航时精度不够，场景受限的问题，有着更广泛的应用场景。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.一种基于图像匹配/惯导/里程计的车辆自主导航方法，其特征在于，包括以下步骤：

步骤一：在需要定位的地面车辆上配置一架装备了对地拍照相机的无人机；

步骤二：启动车载惯导/里程计组合导航***，车辆在运动条件下由驾驶人员判断无人机是否具备起飞条件；

步骤三：如果具备起飞条件则无人机自主起飞并自主跟踪车辆，同时对地拍照并利用拍摄的实时图像和提前制作的基准图匹配，实现地面车辆的定位；

步骤四：得到定位结果后发送给车辆导航计算机进行数据融合；

步骤五：得到车辆定位结果后，无人机自主降落返回车辆；

数据融合采用卡尔曼滤波结构，其中惯导里程计组合导航的观测方程：

其中，δVⁿ表示惯导的速度误差矢量，ψⁿ表示惯导的姿态误差矢量，δd表示里程计的安装误差矢量；K_D表示里程计的比例因子；P_D表示里程计输出的原始脉冲数；I_A表示矩阵

惯导与视觉定位组合导航的***观测方程为：

Z_B＝δrⁿ+w_rB (25)

其中δrⁿ，w_rB分别为观测变量和噪声变量。

2.根据权利要求1所述的一种基于图像匹配/惯导/里程计的车辆自主导航方法，其特征在于，步骤三中无人机对车辆自主跟踪控制的具体方法是：在无人机距离车辆大于10米时，采用基于UWB的技术对无人机进行控制；在无人机距离车辆距离小于10米时，采用基于嵌套二维码识别的技术对无人机进行控制。

3.根据权利要求2所述的一种基于图像匹配/惯导/里程计的车辆自主导航方法，其特征在于，基于嵌套二维码的无人机定位过程包括：

A、在车顶部配置二维码图像，并标定出图像中特征点的位置坐标，

B、通过无人机上的对地拍照相机实时检测降落平台上的二维码标志获取无人机的相对位置和姿态信息；

C、利用P3P算法进行定位解算，已知条件为二维码特征点A,B,C,D在二维码局部坐标系下的坐标，通过小孔成像原理和几何约束求解得到特征点A,B,C,D在相机坐标系X_C,Y_C,Z_C,下的坐标，构成了二维码坐标系与相机坐标系之间的转换，得到无人机相对车辆的位置和姿态。

4.根据权利要求1所述的一种基于图像匹配/惯导/里程计的车辆自主导航方法，其特征在于，无人机拍摄的实时图像与基准图像的匹配采用基于结构张量的异源图像快速匹配方法实现，包括张量提取、张量分析和张量匹配三个步骤。

5.根据权利要求4所述的一种基于图像匹配/惯导/里程计的车辆自主导航方法，其特征在于，所述张量提取的具体方法是：设原图像I大小为m×m像素，灰度矩阵为I(x,y),f是高斯滤波器，通过公式(2)计算得到图像水平x方向梯度g_x和垂直y方向梯度g_y；

根据公式(3)计算图像结构张量T；

对结构张量进行分解：

λ_i和e_i分别是二维矩阵T的特征值和特征向量，λ₁-λ₂代表张量的曲线显著性。

6.根据权利要求4所述的一种基于图像匹配/惯导/里程计的车辆自主导航方法，其特征在于，所述张量分析的具体方法是：将图像分为平坦区域R_f、平滑结构区域R_s和非平滑结构区域R_c；

所述平坦区域对应图像中无纹理或纹理较弱的区域，计算过程如下：

R_f＝{P|Trace(T)＜th,P∈I} (5)

式中，th为设定阈值，I为图像所有像素；

所述平滑结构区域对应图像的边缘部分，计算过程如下：

R_s＝{P|λ₁-λ₂＞λ₂,P∈I-R_f} (6)

非平滑结构区域对应图像的角点或噪声严重的部分，计算过程如下：

R_c＝{P|λ₁-λ₂≤λ₂,P∈I-R_f} (7)。

7.根据权利要求1～6任意一项所述的一种基于图像匹配/惯导/里程计的车辆自主导航方法，其特征在于，卡尔曼滤波的***方程为：

其中，X表示***的状态变量，U表示过程噪声变量，F表示***矩阵，具体定义如下：

X＝[δrⁿ δvⁿ ψⁿ ▽ⁿ εⁿ δd]^T (17)

式中，δrⁿ,δvⁿ,ψⁿ分别表示投影到导航坐标系n的位置误差、速度误差和姿态误差；▽ⁿ,εⁿ表示投影到导航坐标系n的加速度计零偏和陀螺仪零偏；δd表示里程计误差；

U＝[u_r u_v u_ψ u_▽ u_ε u_d]^T (18)

u_r,u_v,u_ψ,u_▽,u_ε,u_d分别表示位置误差的驱动噪声、速度误差的驱动噪声、姿态误差的驱动噪声、加速度计零偏的驱动噪声、陀螺零偏的驱动噪声、里程计误差的驱动噪声；

***矩阵F表示为：

8.根据权利要求7所述的一种基于图像匹配/惯导/里程计的车辆自主导航方法，其特征在于，式(19)中的非零矩阵F_ij具体如下：

式中，i表示惯性坐标系，n表示导航坐标系，选择当地水平的北东地坐标系为导航坐标系；e表示地球坐标系，坐标原点在地心，X轴在赤道平面内指向本初子午线，Z轴沿着地球自转轴指向北极点；b表示载体坐标系，坐标原点位于惯导设备的中心，X轴指向前，Z轴指向下；

fⁿ表示投影到导航坐标系n的由加速度计测量得到的比力测量值；

表示导航坐标系n相对于惯性坐标系i的角速度投影到导航坐标系n中；

表示由载体坐标系到导航坐标系的姿态变换矩阵；下标N、E、D表示导航坐标系变量在北、东、地方向的分量。

Images