一种视觉信息辅助的惯性/里程计组合导航方法及装置与流程

1.本发明属于多传感器数据融合技术领域，尤其涉及一种视觉信息辅助的惯性/里程计组合导航方法及装置。


背景技术：

2.在众多瓶颈技术中，高精度、可靠、持续的导航与定位，是自动驾驶车辆稳定运行的必备条件之一。基于实时动态差分的卫星导航定位技术在自动驾驶中应用最为广泛，但容易受到环境干扰导致信号中断，尤其是市区场景，由于高楼、立交桥、隧道等建筑物和茂密树冠等环境因素对卫星信号的影响，遮挡、弱信号、多路径效应等问题大幅降低了导航系统数据可用性。
3.在自动驾驶车辆运行的复杂环境中，任何单一的定位传感器都难以实现精确、可靠的导航定位，基于不同的测量原理，rtk gnss、惯性导航设备、里程计、激光雷达、视觉等定位传感器各有优缺点，且具有很强的互补性。为了解决组合导航系统可靠性低、连续性差、鲁棒性不足的问题，融合多种传感器量测数据的组合导航技术成为了重要研究方向。
4.在卫星导航拒止环境中，惯性/里程计组合导航是一种极其重要的导航模式，具有信息全面、自主性强、可靠性高、性能稳定的优点，并且，通过将里程计的安装误差、标度系数误差作为状态变量进行估计和补偿，可以进一步提升惯性/里程计组合导航系统的精度保持效果。但是，由于里程计测量信息单一，其用于组合导航的量测信息依赖惯性导航输出的姿态矩阵，造成状态变量可观测度较低，误差补偿效果较弱，导航系统难实现以长时间精度保持。


技术实现要素：

5.针对卫星拒止环境下传统惯性/里程计组合导航系统状态变量可观测度低、误差估计效果差、难以实现长时间精度保持的问题，本技术实施例的目的是提供一种视觉信息辅助的惯性/里程计组合导航方法及装置。
6.根据本技术实施例的第一方面，提供一种视觉信息辅助的惯性/里程计组合导航方法，包括：
7.根据高精度地图和车辆当前定位，构建道路姿态矩阵
8.基于车辆上安装的相机，利用机器视觉算法对车道线进行检测和识别，从而计算车道线和车辆之间的夹角β0；
9.根据所述道路姿态矩阵车道线与车辆之间的夹角β0，计算视觉/地图姿态矩阵c
v/map
；
10.构建姿态组合滤波器模型，将视觉/地图姿态矩阵c
v/map
和惯性导航输出的惯性姿态矩阵作为观测量，对惯性姿态矩阵误差进行估计修正；
11.获取所述姿态组合滤波器模型输出的融合姿态矩阵，将里程计速度分解至导航坐
标系；
12.构建惯性/里程计组合导航系统滤波器模型，对惯性导航误差、器件误差和里程计误差进行修正，从而进行导航。
13.进一步地，根据高精度地图和车辆当前定位，构建道路姿态矩阵包括：
14.基于当前定位和高精度地图，得到道路的方向角φ、坡度角θ和横坡角γ参数；
15.假设车辆为刚体模型，并且车胎和地面接触良好，将其近似作为航向角、俯仰角和横滚角，构建道路姿态矩阵
[0016][0017]
进一步地，基于车辆上安装的相机，利用机器视觉算法对车道线进行检测和识别，从而计算车道线和车辆之间的夹角β0，包括：
[0018]
车道线坐标获取：以所述相机中心为原点，正前方为y轴，平向右为x轴，建立双目相机载体坐标系，利用机器视觉算法，对车道线进行检测和识别，并将其在所述双目相机载体坐标系中的三维坐标a(xi,yi,zi)，投影至o-xy平面，得到车道线的二维坐标a(xi,yi)；
[0019]
车道线切线方程获取：在水平坐标系中，利用二次函数对a(xi,yi)进行拟合，得到车道线的函数曲线，通过对所述车道线的函数曲线进行微分，得到车道线的切线表达式；
[0020]
获取车辆与车道线之间的交汇点：根据所述相机安装的高度h垂直视场角α参数，计算得到相机视野中的车道线起始点a1(x1,y1)和相机之间的垂直距离δy＝h/tanα，根据所述车道线起始点a1(x1,y1)和所述垂直距离δy，得到当前车辆位置对应的车道线坐标a0(x0,y0)，其中y0＝y
1-δy；
[0021]
车道线和车辆之间的夹角夹角β0计算：计算二次函数曲线y＝f(x)在a0(x0,y0)点处的切线斜率kc，将所述切线和y轴之间的夹角近似为车道线和车辆当前行驶方向之间的夹角。
[0022]
进一步地，根据所述道路姿态矩阵车道线与车辆之间的夹角β0，计算视觉/地图姿态矩阵c
v/map
，如下式：
[0023][0024]
进一步地，所述姿态组合滤波器模型包括观测方程和状态方程，
[0025]
所述观测方程如下所示：
[0026][0027]
其中，δγ、δθ、分别为观测得到的惯性和视觉/地图姿态矩阵之间的横滚角、俯仰角和航向角之差；γ
imu
、θ
imu
、分别为惯性导航输出的横滚角、俯仰角和航向角；
γ
v/map
、θ
v/map
、分别为由视觉/地图姿态矩阵得到的滚角、俯仰角和航向角；φe、φn、φu分别为东向、北向和天向的失准角；
[0028]
选取姿态、位置、速度、陀螺仪零偏、加速度计零偏误差为系统状态变量状态向量，所述状态方程如下所示：
[0029][0030][0031][0032]
其中，ve、vn、vu分别表示东向、北向和天向速度；δve、δvn分别表示东向、北向速度误差；ε
x
、εy、εz分别表示三个陀螺仪误差；ω
ie
表示地球自转角速度；l，rm，rn分别表示纬度、地球子午圈曲率半径和地球卯酉圈曲率半径，分别表示三个加速度计误差；fe、fn、fu分别表示东向、北向和天向的比力，ω
ie
为地球自转角速度。
[0033]
进一步地，获取所述姿态组合滤波器模型输出的融合姿态矩阵，将里程计速度分解至导航坐标系，包括：
[0034]
根据惯性导航设备的数据输出频率，对所述姿态融合滤波器进行状态更新；
[0035]
根据所述地图姿态矩阵数据输出频率，对姿态融合矩阵进行量测更新，并实时读取融合姿态矩阵输出的姿态角度参数，构建融合姿态矩阵
[0036]
对里程计输出的车辆行驶速度数据进行分解，得到导航坐标系下的速度测量值，如式(8)所示：
[0037][0038]
其中，为导航坐标系中里程计速度量测值；为车辆本体坐标系到导航坐标系转换的融合姿态矩阵；为里程计坐标系到车辆本体坐标系转换的姿态矩阵，由里程计的安装误差参数俯仰角δθ、横滚角δγ和航向角构成；为里程计直接输出的速度量测数据。
[0039]
进一步地，构建惯性/里程计组合导航系统滤波器模型，对惯性导航误差、器件误差和里程计误差进行修正，包括：
[0040]
选取姿态、位置、速度、陀螺仪零偏、加速度计零偏、里程计误差为惯性/里程计组合导航系统的系统状态变量：
[0041][0042]
δθ、δkd表示里程计的俯仰角安装误差、航向角安装误差和刻度系数误差；
[0043]
惯性/里程计组合导航系统状态方程为：
[0044][0045]
其中(f
ins
)9×9和(f
sg
)9×6的具体表达式见所述姿态组合滤波器模型的状态方程；
[0046]
惯性/里程计组合导航系统的观测方程为：
[0047][0048]
上式中，和分别表示导航坐标系中的惯性导航速度和速度误差；和分别表示导航坐标系中的里程计的速度和速度误差；
×
表示里程计直接输出的速度量测数据的叉乘矩阵，i为单位矩阵，v为观测噪声矩阵。
[0049]
根据本技术实施例的第二方面，提供一种视觉信息辅助的惯性/里程计组合导航装置，包括：
[0050]
构建模块，用于根据高精度地图和车辆当前定位，构建道路姿态矩阵
[0051]
第一计算模块，用于基于车辆上安装的相机，利用机器视觉算法对车道线进行检测和识别，从而计算车道线和车辆之间的夹角β0；
[0052]
第二计算模块，用于根据所述道路姿态矩阵车道线与车辆之间的夹角β0，计算视觉/地图姿态矩阵c
v/map
；
[0053]
修正模块，用于构建姿态组合滤波器模型，将视觉/地图姿态矩阵c
v/map
和惯性导航输出的惯性姿态矩阵作为观测量，对惯性姿态矩阵误差进行估计修正；
[0054]
分解模块，用于获取所述姿态组合滤波器模型输出的融合姿态矩阵，将里程计速
度分解至导航坐标系；
[0055]
导航模块，用于构建惯性/里程计组合导航系统滤波器模型，对惯性导航误差、器件误差和里程计误差进行修正，从而进行导航。
[0056]
根据本技术实施例的第三方面，提供一种电子设备，包括：
[0057]
一个或多个处理器；
[0058]
存储器，用于存储一个或多个程序；
[0059]
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如第一方面所述的方法。
[0060]
根据本技术实施例的第四方面，提供一种计算机可读存储介质，其上存储有计算机指令，该指令被处理器执行时实现如第一方面所述方法的步骤。
[0061]
本技术的实施例提供的技术方案可以包括以下有益效果：
[0062]
由上述实施例可知，本技术通过采用多传感器(惯导、相机、里程计)数据融合的技术手段，构建了一种基于视觉和高精度地图的姿态矩阵，并对里程计数据进行分解，实现了惯性/里程计组合导航系统中里程计数据和姿态矩阵误差解耦，有效克服了可观测度不足带来的误差快速发散问题，进而实现了组合导航系统精度提升。
[0063]
应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本技术。
附图说明
[0064]
此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本技术的实施例，并与说明书一起用于解释本技术的原理。
[0065]
图1是根据一示例性实施例示出的视觉辅助惯性/里程计组合导航方法原理图。
[0066]
图2是根据一示例性实施例示出的车道线和车辆夹角计算原理图。
[0067]
图3是根据一示例性实施例示出的一种视觉信息辅助的惯性/里程计组合导航装置的框图。
[0068]
图4是根据一示例性实施例示出的一种电子设备的示意图。
具体实施方式
[0069]
这里将详细地对示例性实施例进行说明，其示例表示在附图中。下面的描述涉及附图时，除非另有表示，不同附图中的相同数字表示相同或相似的要素。以下示例性实施例中所描述的实施方式并不代表与本技术相一致的所有实施方式。
[0070]
在本技术使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本技术。在本技术和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。还应当理解，本文中使用的术语“和/或”是指并包含一个或多个相关联的列出项目的任何或所有可能组合。
[0071]
应当理解，尽管在本技术可能采用术语第一、第二、第三等来描述各种信息，但这些信息不应限于这些术语。这些术语仅用来将同一类型的信息彼此区分开。例如，在不脱离本技术范围的情况下，第一信息也可以被称为第二信息，类似地，第二信息也可以被称为第一信息。取决于语境，如在此所使用的词语“如果”可以被解释成为“在
……
时”或“当
……
时”或“响应于确定”。
[0072]
图1是根据一示例性实施例示出的一种视觉信息辅助的惯性/里程计组合导航方法的流程图，如图1所示，该方法可以包括以下步骤：
[0073]
步骤s11：根据高精度地图和车辆当前定位，构建道路姿态矩阵
[0074]
步骤s12：基于车辆上安装的相机，利用机器视觉算法对车道线进行检测和识别，从而计算车道线和车辆之间的夹角β0；
[0075]
步骤s13：根据所述道路姿态矩阵车道线与车辆之间的夹角β0，计算视觉/地图姿态矩阵c
v/map
；
[0076]
步骤s14：构建姿态组合滤波器模型，将视觉/地图姿态矩阵c
v/map
和惯性导航输出的惯性姿态矩阵作为观测量，对惯性姿态矩阵误差进行估计修正；
[0077]
步骤s15：获取所述姿态组合滤波器模型输出的融合姿态矩阵，将里程计速度分解至导航坐标系；
[0078]
步骤s16：构建惯性/里程计组合导航系统滤波器模型，对惯性导航误差、器件误差和里程计误差进行修正，从而进行导航。
[0079]
由上述实施例可知，本技术通过采用多传感器(惯导、相机、里程计)数据融合的技术手段，构建了一种基于视觉和高精度地图的姿态矩阵，并对里程计数据进行分解，实现了惯性/里程计组合导航系统中里程计数据和姿态矩阵误差解耦，有效克服了可观测度不足带来的误差快速发散问题，进而实现了组合导航系统精度提升。
[0080]
在步骤s11的具体实施中，根据高精度地图和车辆当前定位，构建道路姿态矩阵
[0081]
具体地，高精度地图是相对于普通地图来说的，它提供了更高精度，内容更为丰富的地图信息。高精度地图中的位置数据精度可达分米级，同时还包含道路的坡度、横坡角、方向角度参数；在信息类型方面，除了包括普通地图都有的元素之外，还包含路灯、护栏、红绿灯等几十个甚至上百个要素的信息。该步骤可以包括以下子步骤：
[0082]
基于当前定位和高精度地图，得到道路的方向角φ、坡度角θ和横坡角γ参数；
[0083]
假设车辆为刚体模型，并且车胎和地面接触良好，将其近似作为航向角、俯仰角和横滚角，构建道路姿态矩阵
[0084][0085]
在步骤s12的具体实施中，基于车辆上安装的相机，利用机器视觉算法对车道线进行检测和识别，从而计算车道线和车辆之间的夹角β0；
[0086]
具体地，该步骤可以包括以下子步骤：
[0087]
步骤s21：车道线坐标获取：以所述相机中心为原点，正前方为y轴，平向右为x轴，建立双目相机载体坐标系，利用机器视觉算法，对车道线进行检测和识别，并将其在所述双目相机载体坐标系中的三维坐标a(xi,yi,zi)，投影至o-xy平面，得到车道线的二维坐标a(xi,yi)；
[0088]
步骤s22：车道线切线方程获取：在水平坐标系中，利用二次函数对a(xi,yi)进行拟合，得到车道线的函数曲线，通过对所述车道线的函数曲线进行微分，得到车道线的切线表达式；
[0089]
步骤s23：获取车辆与车道线之间的交汇点：根据所述相机安装的高度h垂直视场角α参数，计算得到相机视野中的车道线起始点a1(x1,y1)和相机之间的垂直距离δy＝h/tanα，根据所述车道线起始点a1(x1,y1)和所述垂直距离δy，得到当前车辆位置对应的车道线坐标a0(x0,y0)，其中y0＝y
1-δy；
[0090]
步骤s24：车道线和车辆之间的夹角夹角β0计算：计算二次函数曲线y＝f(x)在a0(x0,y0)点处的切线斜率kc，将所述切线和y轴之间的夹角近似为车道线和车辆当前行驶方向之间的夹角。
[0091]
在步骤s21-s24的一个实施例中。如图2，相机水平居中安装于车辆前挡风玻璃，坐标轴平行于车体坐标系，o-xy直角坐标系为相机坐标系在水平方向的投影，原点o位于相机中心点，y轴为车头正前方，x轴垂直于y轴，指向车体右侧。y＝f(x)为车道线的拟合函数曲线，a1(x1,y1)为相机中图像下边界和车道线交汇点，a0(x0,y0)为车道线延长线和x轴交点，y＝k0x+b0为点a0(x0,y0)对应的切线方程，切线和y轴的夹角即为β0。
[0092]
在步骤s13的具体实施中，根据所述道路姿态矩阵车道线与车辆之间的夹角β0，计算视觉/地图姿态矩阵c
v/map
；
[0093]
具体地，所述视觉/地图姿态矩阵c
v/map
如下式：
[0094][0095]
在步骤s14的具体实施中，构建姿态组合滤波器模型，将视觉/地图姿态矩阵c
v/map
和惯性导航输出的惯性姿态矩阵作为观测量，对惯性姿态矩阵误差进行估计修正；
[0096]
具体地，可以采用kalman滤波器作为所述姿态组合滤波器模型，也可选择其余滤波器，该设置为本领域的常规设定。所述姿态组合滤波器模型包括观测方程和状态方程，
[0097]
所述观测方程如下所示：
[0098][0099]
其中，δγ、δθ、分别为观测得到的惯性和视觉/地图姿态矩阵之间的横滚角、俯仰角和航向角之差；γ
imu
、θ
imu
、分别为惯性导航输出的横滚角、俯仰角和航向角；γ
v/map
、θ
v/map
、分别为由视觉/地图姿态矩阵得到的滚角、俯仰角和航向角；φe、φn、φu分别为东向、北向和天向的失准角；
[0100]
选取姿态、位置、速度、陀螺仪零偏、加速度计零偏误差为系统状态变量状态向量：
[0101][0102]
所述状态方程如下所示：
[0103][0104][0105][0106]
其中，ve、vn、vu分别表示东向、北向和天向速度；δve、δvn分别表示东向、北向速度误差；ε
x
、εy、εz分别表示三个陀螺仪误差；ω
ie
表示地球自转角速度；l，rm，rn分别表示纬度、地球子午圈曲率半径和地球卯酉圈曲率半径，分别表示三个加速度计误差；fe、fn、fu分别表示东向、北向和天向的比力，ω
ie
为地球自转角速度，c
ij
是对惯性姿态矩阵误差进行估计修正得到的融合姿态矩阵第i行第j列的元素，其余同理。
[0107]
在步骤s15的具体实施中，获取所述姿态组合滤波器模型输出的融合姿态矩阵，将里程计速度分解至导航坐标系；
[0108]
具体地，此步骤可以包括以下子步骤：
[0109]
根据惯性导航设备的数据输出频率，对所述姿态融合滤波器进行状态更新；
[0110]
根据所述地图姿态矩阵数据输出频率，对姿态融合矩阵进行量测更新，并实时读取融合姿态矩阵输出的姿态角度参数，构建融合姿态矩阵
[0111]
对里程计输出的车辆行驶速度数据进行分解，得到导航坐标系下的速度测量值，如式(8)所示：
[0112][0113]
其中，为导航坐标系中里程计速度量测值；为车辆本体坐标系到导航坐标系转换的融合姿态矩阵；为里程计坐标系到车辆本体坐标系转换的姿态矩阵，由里程计的安装误差参数俯仰角δθ、横滚角δγ和航向角构成；为里程计直接输出的速度量测数据。
[0114]
在步骤s16的具体实施中，构建惯性/里程计组合导航系统滤波器模型，对惯性导航误差、器件误差和里程计误差进行修正，从而进行导航；
[0115]
具体地，选取姿态、位置、速度、陀螺仪零偏、加速度计零偏、里程计误差为惯性/里程计组合导航系统的系统状态变量：
[0116][0117]
δθ、δkd表示里程计的俯仰角安装误差、航向角安装误差和刻度系数误差；
[0118]
惯性/里程计组合导航系统状态方程为：
[0119][0120]
其中(f
ins
)9×9和(f
sg
)9×6的具体表达式见所述姿态组合滤波器模型的状态方程；
[0121]
惯性/里程计组合导航系统的观测方程为：
[0122][0123]
上式中，和分别表示导航坐标系中的惯性导航速度和速度误差；和分别表示导航坐标系中的里程计的速度和速度误差；表示里程计直接输出的速度量测数据的叉乘矩阵，i为单位矩阵，v为观测噪声矩阵。
[0124]
与前述的视觉信息辅助的惯性/里程计组合导航方法的实施例相对应，本技术还提供了视觉信息辅助的惯性/里程计组合导航装置的实施例。
[0125]
图3是根据一示例性实施例示出的一种视觉信息辅助的惯性/里程计组合导航装置框图。参照图3，该装置可以包括：
[0126]
构建模块21，用于根据高精度地图和车辆当前定位，构建道路姿态矩阵
[0127]
第一计算模块22，用于基于车辆上安装的相机，利用机器视觉算法对车道线进行检测和识别，从而计算车道线和车辆之间的夹角β0；
[0128]
第二计算模块23，用于根据所述道路姿态矩阵车道线与车辆之间的夹角β0，计算视觉/地图姿态矩阵c
v/map
；
[0129]
修正模块24，用于构建姿态组合滤波器模型，将视觉/地图姿态矩阵c
v/map
和惯性导航输出的惯性姿态矩阵作为观测量，对惯性姿态矩阵误差进行估计修正；
[0130]
分解模块25，用于获取所述姿态组合滤波器模型输出的融合姿态矩阵，将里程计
速度分解至导航坐标系；
[0131]
导航模块26，用于构建惯性/里程计组合导航系统滤波器模型，对惯性导航误差、器件误差和里程计误差进行修正，从而进行导航。
[0132]
关于上述实施例中的装置，其中各个模块执行操作的具体方式已经在有关该方法的实施例中进行了详细描述，此处将不做详细阐述说明。
[0133]
对于装置实施例而言，由于其基本对应于方法实施例，所以相关之处参见方法实施例的部分说明即可。以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本技术方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
[0134]
相应的，本技术还提供一种电子设备，包括：一个或多个处理器；存储器，用于存储一个或多个程序；当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如上述的视觉信息辅助的惯性/里程计组合导航方法。如图4所示，为本发明实施例提供的一种视觉信息辅助的惯性/里程计组合导航方法所在任意具备数据处理能力的设备的一种硬件结构图，除了图4所示的处理器、内存以及网络接口之外，实施例中装置所在的任意具备数据处理能力的设备通常根据该任意具备数据处理能力的设备的实际功能，还可以包括其他硬件，对此不再赘述。
[0135]
相应的，本技术还提供一种计算机可读存储介质，其上存储有计算机指令，该指令被处理器执行时实现如上述的视觉信息辅助的惯性/里程计组合导航方法。所述计算机可读存储介质可以是前述任一实施例所述的任意具备数据处理能力的设备的内部存储单元，例如硬盘或内存。所述计算机可读存储介质也可以是外部存储设备，例如所述设备上配备的插接式硬盘、智能存储卡(smart media card，smc)、sd卡、闪存卡(flash card)等。进一步的，所述计算机可读存储介还可以既包括任意具备数据处理能力的设备的内部存储单元也包括外部存储设备。所述计算机可读存储介质用于存储所述计算机程序以及所述任意具备数据处理能力的设备所需的其他程序和数据，还可以用于暂时地存储已经输出或者将要输出的数据。
[0136]
本领域技术人员在考虑说明书及实践这里公开的内容后，将容易想到本技术的其它实施方案。本技术旨在涵盖本技术的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本技术的一般性原理并包括本技术未公开的本技术领域中的公知常识或惯用技术手段。
[0137]
应当理解的是，本技术并不局限于上面已经描述并在附图中示出的精确结构，并且可以在不脱离其范围进行各种修改和改变。