CN104848867A - 基于视觉筛选的无人驾驶汽车组合导航方法 - Google Patents

Abstract

基于视觉筛选的无人驾驶汽车组合导航方法，该方法包括以下步骤：坐标系换算；建筑物遮挡角度识别；非视距环境下卫星信号可用性判断；改进的自适应平方根容积卡尔曼滤波算法；提出了以视觉信息为筛选条件的组合导航算法，引入了“剔除非视距传播卫星数据”的概念并给出判断方法，用来剔除由于建筑物遮挡而造成精度下降的GPS卫星数据信息；本发明区别于传统组合方法采用视觉导航用以实时并行处理数据，而是采用视觉信息对GPS数据有针对性的筛选，避免了传统采用视觉导航而造成的维数灾难；提出了改进的自适应平方根容积卡尔曼滤波算法，并考虑到无人驾驶智能车行驶在城市路况中导航数据强非线性的问题。

Description

基于视觉筛选的无人驾驶汽车组合导航方法

技术领域

本发明涉及一种基于视觉筛选的无人驾驶汽车组合导航方法，属于组合导航相关领域。

技术背景

驾驶员的主观因素是导致许多交通事故的根本原因，约90％的交通事故源自驾驶员的错误。为提供交通安全性和运输效率，自主驾驶智能车辆的研究受到许多国家的重视，并成为研究的重要内容。继2012年谷歌公司发布第一款无人驾驶智能车后，2015年4月，由汽车配件商德尔福研发的一辆无人驾驶汽车完成了横跨美国之旅，再次说明近年来无人驾驶智能车技术的飞速发展。2009年我国国家自然科学基金委员会创办“中国智能车未来挑战赛”，并受到各大高校、科研院所及企业的重视。智能车辆实现无人驾驶控制的前提是其配备的导航系统能够实时、准确地提供车辆的状态信息。为了避免发生碰撞，需要自主导航车辆达到足够的定位精度。

现阶段导航系统不再局限于单一的传感器所提供的地理位置信息，在一台无人驾驶汽车上甚至装配二十余台传感设备，根据车载导航传感器所提供的状态信息形式的不同，可将车载传感器分为提供绝对信息的传感器包括惯性导航(INS)、全球定位系统(GPS)，与提供相对信息的传感器如视觉导航(VISION)、引导磁钉或电缆。其中INS,GPS信息更为丰富完备。不同的车载导航传感器各有优劣，且车辆运行环境复杂，依靠单一的传感器无法保证在复杂工况下的可靠导航。因此车载导航系统多采用多传感器进行组合导航，常见组合形式有INS/GPS组合导航：通过将GPS观测的姿态信息引入组合导航滤波模型中，提高组合导航系统的估计精度，但该方式并没顾及在GPS信号丢失的情况下如何确保导航精度，GPS信号易受电子干扰，在城市化道路中易受建筑群遮挡，造成信号丢失或跳变。另一种组合形式为INS/GPS/视觉的组合方式，该方法存在高程方向上的累积误差较大，且矩阵维数较高，计算时间较长，易出现数值不稳定。

基于视觉筛选的无人驾驶汽车组合导航方法首先通过摄像头采集的视觉信息对GPS接收的卫星信息进行筛选后再进行GPS/INS组合导航，并将导航数据通过改进的滤波算法进行滤波，最终为无人驾驶智能车控制模块提供稳定，精准的导航信息。

发明内容

针对无人驾驶智能车在城市道路环境中，全球定位系统(GPS)信号以非视距(NLOS)传播而造成伪距估计偏差，导致定位不准确的问题，提出了一种以视觉信息为筛选条件的组合导航方法。采用该方法的导航系统能有效控制无人驾驶智能车城市道路环境中GPS信号偏差，提高了导航信息的可靠性、准确性。

一种基于视觉筛选的无人驾驶汽车组合导航方法，包括以下步骤：

步骤1，坐标系换算。

步骤1.1：大地坐标系换算到视觉坐标系。

摄像头成像变换涉及不同坐标系之间的转换，包括大地坐标系、摄像机坐标系、成像平面坐标系和图像平面坐标系。在无人驾驶智能车导航系统的视觉系统中，需首先将摄像头在大地坐标系中的位置换算到视觉坐标系。

步骤1.2：换算视觉坐标系相对车体坐标系。

步骤1.3：将视觉、车体、大地坐标系进行统一。

GPS输出数据为ECEF坐标系统下的经度、纬度、高度，并不适合于平面运动的车辆导航。采用通用横轴墨卡托坐标系(Universal Transverse Mercator，UTM)将经纬度转化到平面坐标系并转换到“东北天”大地坐标系下，以此坐标系为主坐标系，并统一其他坐标系。

步骤2，建筑物遮挡角度识别。

步骤2.1：图像预处理。

通过摄像头采集的周围图像信息，首先提取出导航基准线，确定导航路径中的特征点。主要流程包括灰度变换、图像分割、图像去噪、提取候选点、直线拟合、确定已知点。

步骤2.2：道路识别。

步骤2.3：建筑物识别。

通过计算熵值，进行填充处理，分离出天空与遮挡建筑物。

步骤2.3：计算建筑物俯仰角

确定建筑物上隅角、下隅角坐标值，并分别与车体中心点即图像底边中心点连线，同时找到过建筑物上隅角下隅角坐标的直线段，沿直线段分别向水平中线，垂直中线做垂线。分别计算各点连线距离，并换算出建筑物俯仰角余弦值即为建筑物遮挡角度余弦值。

步骤3，非视距环境下卫星信号可用性判断。

步骤3.1：计算卫星高度与车体所在位置水平切线的夹角余弦值。

步骤3.2：判断接收的卫星信号是否被建筑物遮挡。

比较卫星夹角余弦值与建筑物遮挡角度余弦值，判断接收的卫星信号是否被建筑物遮挡，若遮挡，则将此卫星提供的GPS数据剔除。

步骤4，改进的自适应平方根容积卡尔曼滤波算法。

步骤4.1：计算容积点及权重。

步骤4.2：初始化。

步骤4.3：时间更新。

步骤4.4：测量更新。

与现有方法相比，本发明具有以下优点：

(1)提出了以视觉信息为筛选条件的组合导航算法，引入了“剔除非视距传播卫星数据”的概念并给出判断方法，用来剔除由于建筑物遮挡而造成精度下降的GPS卫星数据信息；

(2)本发明区别于传统组合方法采用视觉导航用以实时并行处理数据，而是采用视觉信息对GPS数据有针对性的筛选，方法新颖，避免了传统采用视觉导航而造成的维数灾难；

(3)提出了改进的自适应平方根容积卡尔曼滤波算法，考虑到无人驾驶智能车行驶在城市路况中导航数据强非线性的问题；

附图说明

图1为组合导航系统框架示意图；

图2为本发明所涉及的方法流程图；

图3为视觉/车体/GPS坐标系换算到大地坐标系示意图；

图4为视觉模块所识别的周围建筑物遮挡角度示意图；

图5为非视距传输条件下建筑物遮挡角度与GPS卫星俯仰角度关系示意图；

图6为采用基于视觉筛选的组合导航方法与未采用此方法的导航路径对比图。

具体实施方式

下面结合附图和实施例对本发明做进一步说明。

基于视觉筛选的无人驾驶汽车组合导航系统架构示意图如图1所示。本发明所述方法的流程图如图2所示，包括以下步骤：

步骤1，坐标系换算。

步骤1.1：大地坐标系换算到视觉坐标系。

摄像头成像变换涉及不同坐标系之间的转换，包括大地坐标系、摄像机坐标系、成像平面坐标系和图像平面坐标系。

在智能车视觉系统中，需将大地坐标系换算成摄像头坐标系大地坐标系原点O_w在车体重心正下方地面上，z_v轴垂直向上，x_v轴平行地面指向车体前进方向，y_v轴在图中垂直纸面向外。O_c为摄像机坐标系原点，与O_w点的水平距离为l₁，与地面的垂直距离为l₂。α,β,γ分别为摄像头绕x,y,z旋转角度，本文摄像头只绕y_c轴旋转，该轴与水平面倾斜角为α，旋转角度β、γ为零。如图2所示，(x_c y_c z_c)^T为摄像头坐标系，大地坐标系中点(x_w y_w z_w)^T换算到(x_c y_c z_c)^T为摄像头坐标系采用(1)式方法。

$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]=R(\mathrm{\α},\mathrm{\β},\mathrm{\γ})\·\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\end{array}\right]+\left[\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right]---\left(1\right)$

$R=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\α}& \sin \mathrm{\α}\\ 0& -\sin \mathrm{\α}& \cos \mathrm{\α}\end{array}\right]---\left(2\right)$

其中(t_x t_y t_z)^T为平移向量，R为旋转矩阵。根据图像处理的需要摄像机内部会将摄像机坐标系转换到平面坐标系，同时成像平面坐标系再转换到平面坐标系。

步骤1.2：换算视觉坐标系相对车体坐标系。

根据步骤1.1将已经换算好的视觉坐标系与车体坐标系再次转换

步骤1.3：将视觉、车体、大地坐标系进行统一。

GPS输出数据为ECEF坐标系统下的经度、纬度、高度，并不适合于平面运动的车辆导航。采用通用横轴墨卡托坐标系(Universal Transverse Mercator，UTM)将经纬度转化到平面坐标系并转换到“东北天”大地坐标系下，以此坐标系为主坐标系，并统一其他坐标系，见附图说明，图3视觉/车体/GPS坐标系换算到大地坐标系示意图。具体转换方法为：

定义视觉坐标系的原点为摄像机光心在地面上的投影点，其中X轴的正方向为行驶方向的负方向，Y轴的正方向垂直于X轴，水平向右。图3中的特征点即为图像处理算法得到的建筑物群中的特征点，可以将视觉坐标系转换到以车为主的视觉坐标系下，其在视觉坐标系中的坐标定义为(x_pvision，y_pvision)。摄像头在大地坐标系中的位置是由GPS实时获得的，其坐标为(x_v,y_v)，通过此坐标和智能车的航向角可以将视觉坐标系转换到大地坐标系下，从而求出目标点在大地坐标系下的坐标(x_p,y_p)。因此，建筑物特征点在大地坐标中的坐标与视觉坐标系中的坐标可表示为：

其中，为车辆行驶航向角，由GPS提供

步骤2，建筑物遮挡角度识别。

步骤2.1：图像预处理。

通过摄像头采集的周围图像信息，首先提取出导航基准线，确定导航路径中的特征点。主要流程包括灰度变换、图像分割、图像去噪、提取候选点、直线拟合、确定已知点。对城市环境道路图像进行处理分析时，选择适宜的彩色特征模型有利于问题的顺利解决。综合分析道路周围建筑物群的特点，发现2G-R-B特征增强建筑物与背景的对比度，因此本方法应用此特征对图像进行灰度变换。得到灰度图像后，需要对图像进行分割，综合考虑了处理时间和处理效果等因素，选择最大类间方差(OTSU)分割算法对灰度图像进行分割，将建筑物群设定为目标像素，即白像素。分割后的图像中，目标像素群中存在许多噪声点，因此选用形态学的膨胀和腐蚀算法对图像进行去噪，膨胀选用1×3模版，腐蚀选用3×1模版。

步骤2.2：道路识别。

图像预处理完毕后，需要在图像中提取出代表建筑物特征的候选点，本方法选用基于移动窗口的垂直投影法进行候选点的选取，基本方法为：设定一个1×20的模板，将此模板从图像底层开始由左向右移动，计算模版中目标像素的灰度值，扫描一行结束后，计算各列灰度值的平均值和标准差并设定其和为灰度阈值，图像中灰度值大于阈值的部分即为目标区域，由于提取的部分为道路的边界，在边界周围灰度值存在跳跃，因此可以通过计算前后两点间的灰度差提取出边界候选点，按照此方法将模版依次向上移动一个像素，直到距离顶部20个像素为止。根据提取出的候选点，运用基于已知点的Hough变换方法进行直线拟合，已知点取所有候选点的分布中心。在得到建筑物群之后，选择其中的一个点作为代表路径的特征点，记录其在图像坐标系中的位置。该方法中对图像的处理并未强调车道线的显像效果，因组合导航系统中已结合MapX控件，通过GPS/INS组合可实现将车辆本体置于车道中的目的。

步骤2.3：建筑物识别。

进行车道划分后，通过改进的二维熵阈值法方法对周围环境进行熵值估计，将熵值大的区域填充，目的在于分离天空与周围建筑物。

步骤2.3：计算建筑物俯仰角

进行填充后，首先，找到分割区域中垂直线段，中央水平线做垂线，并记录交点坐标，同理，将水平线段，向中央水平线做垂线，并记录交点，同时在填充区域内找到已分割区域中水平垂直线交点并记录坐标值。然后，分别计算各交点到车体点(图像处理中专业名词)的距离。最后，将处于同一垂直线上的点(大于一个的)到车体点的距离余弦值，附图4为视觉模块所识别的周围建筑物遮挡角度示意图。

建筑物的俯仰角换算方法定义如下：

其中，表示车体位置到前方第n个建筑物下隅角的向量，表示车体位置到前方第n个建筑物下隅角的向量。

步骤3，非视距环境下卫星信号可用性检测

采用视觉模块来判定GPS卫星传输信息的可用性，主要由于GPS基于无线电定位技术，无线电定位技术是通过检测与观察者之间的距离的特征参数估计出来目标位置。其中视距传播(Line of Sight，LOS)是无线电定位技术中准确测量定位参数的必要条件。GPS定位系统在日广野地区定位精度高的主要原因是满足视距传播LOS的条件。但配备了GPS的智能车辆行驶在城市道路上，因受到信号衰退和阴影效应的影响，接收机接收到的信号，可能不包含视距传播信号或者视距传播信号微弱，即为非视距传播(Not Line of Sight，NLOS)。以视距传播信号的有无和所占的地位为标准，移动终端与基站间的无线信号传播分成三种情况：直射信号占支配地位(Dominant Direct Path,DDP)、直射信号不占支配地位(Non Dominant DirectPath,NDDP)、和接收不到直射信号(Undetected Direct Path,DDP)。根据GPS工作原理可知在DDP的情况下，GPS系统定位精度可达到最高，而在UDP的情况下定位估计的精度低。在城市道路情况下，智能车辆对导航系统定位精度要求较高，但由于建筑物群的遮挡，GPS移动端与基站之间视距传播的情况非常少，无线信号只能通过反射，散射和衍射方式到达接收端，大多数为非视距传播(NLOS)。此时的无线信号通过多种途径被接收，而多径效应会带来时延不同步、信号衰减、极化改变、链路不稳定等一系列问题。

步骤3.1：计算卫星高度与车体所在位置水平切线的夹角余弦值

采用一种改进的NLOS识别方法，主要原理为将GPS的数据信息主要为卫星位置信息与视觉模块得到的楼宇遮挡角度信息融合，进而判断车体周围的建筑物是否遮挡了GPS所提供的卫星信号。接收机从每个卫星接收海拔和方位角信号与摄像头采集的前方建筑物上隅角与高度信息进行融合，原理如图5非视距传输条件下建筑物遮挡角度与GPS卫星俯仰角度关系示意图。

其中，a，b分别表示被建筑物遮挡而进行非视距传播的卫星与未被遮挡直接进行视距传播的卫星。如果卫星与的高度E_a，E_b以及与航向角夹角A_a，A_b属于建筑物B_n遮挡区域，此卫星被定义为NLOS情况。卫星高度与车体所在位置水平切线的夹角定义为

$\cos \left(E_a\right)=\frac{\cos \left(L_e\right)\cos \left(L_s\right)\cos (L_{\mathrm{si}}-l_e)+\sin \left(L_e\right)\sin \left(L_s\right)}{\sqrt{1+{\left(\frac{r_e}{r_s}\right)}^2-2\·\left(\frac{r_e}{r_s}\right)\·\cos \left(L_e\right)\cos \left(L_s\right)\cos (L_{\mathrm{si}}-l_e)+\sin \left(L_e\right)\sin \left(L_s\right)}}---\left(5\right)$

其中，r_s为卫星轨道半径，r_e为地球半L_s为卫星所处纬度，L_e为参照点所处纬度，l_s卫星所处经度，l_e参考点的经度。

步骤3.2：比较卫星夹角余弦值与建筑物遮挡角度余弦值，判断接受的卫星信号是否被建筑物遮挡，若遮挡，则将此卫星提供的GPS数据剔除。

具体方法为：若cos(E_a)＜cos(E_bn)，则说明卫星S_a俯仰角度大于前方建筑物B_n的俯仰角度，接收机接收到的S_a卫星传输数据属于视距传播，精确度度高。若cos(E_a)≥cos(E_bn)则说明卫星S_a俯仰角度小于前方建筑物B_n的俯仰角度，接收机接收到的S_a卫星传输数据属于NLOS传播方式，精确度相对较低，此颗卫星提供的数据信息不予以考虑。由于采用双目摄像头可以直接采集车体到前方建筑物距离信息，因此采用此方法计算量小，实时性好，能很好的满足实际工程需求。

步骤4，改进的自适应平方根容积卡尔曼滤波算法。

步骤4.1：计算容积点及权重。

首先需要确定系统方程，经过视觉筛选后的GPS数据与INS采用松组合模式，状态变量X取为15维，包括3个相对平面坐标系的位置及速度误差，姿态角误差、加速度计偏差，陀螺漂移。

$\stackrel{\·}{X}=\mathrm{FX}+u---\left(6\right)$

X＝[δr δv δψ ▽ ε]^T  (7)

F为系统状态转移矩阵；u为状态过程噪声向量。

然后确定观测模型，组合导航数据融合采用松耦合方式，取经过视觉筛选后的GPS、

INS输出的位置和速度之差作为观测值，构造观测量：

$\begin{array}{c}{Z}_r\left(t\right)=r_{\mathrm{GPS}}\left(t\right)-r_{\mathrm{INS}}\left(t\right)=\\ r_{\mathrm{GPS}}\left(t\right)-(r_{\mathrm{INS}}\left(0\right)+{\∫}_0^t{v}_{\mathrm{INS}}\left(0\right)\mathrm{dt}+t{\∫}_0{\∫}_0^{t}a\left(t\right)\mathrm{dt})\end{array}---\left(8\right)$

$\begin{array}{c}{Z}_v\left(t\right)=v_{\mathrm{GPS}}\left(t\right)-v_{\mathrm{INS}}\left(t\right)=\\ v_{\mathrm{GPS}}\left(t\right)-(v_{\mathrm{INS}}\left(0\right)+{\∫}_0^{t}a\left(t\right)\mathrm{dt})\end{array}---\left(9\right)$

式中，Z_r(t)是位置观测量；Z_v(t)是速度观测量；r_GPS(t)是GPS的位置观测值；r_INS(t)是INS的位置计算值；v_GPS(t)是GPS的速度观测值；v_INS(t)是INS的速度计算值；r_INS(0)、v_INS(0)是INS的初始位置和速度；a(t)是INS所测得的加速度。则SRCKF-KF的量测方程为

$z_{k+1}^{\mathrm{gps}}=h_{k+1}^{\mathrm{gps}}(x_{k+1},u_{k+1})+v_{k+1}^{\mathrm{gps}}---\left(10\right)$

其中， $h_{k+1}^{\mathrm{gps}}=[h_{k+1,e}^{\mathrm{gps}},\mathrm{0,0},h_{k+1,n}^{\mathrm{gps}}]$ 代表GPS测量矩阵 $h_{k+1,n}^{\mathrm{gps}}=[\mathrm{1,0,0};\mathrm{0,1,0}]$

确定以上基本构架后，计算容积点及权重。算法选取2n个容积点，n表示状态维数，

${\mathrm{\ζ}}_i=\sqrt{\frac{2n}{2}}{\left[1\right]}_i$

${\mathrm{\ω}}_i=\frac{1}{2n}---\left(11\right)$

i＝1,2,…,2n

ξ_i表示第i个容积点，ω_i为相应容积点权重.容积点2n表示容积点总是状态维数的二倍，[1]_i表示完整对称点集。

步骤4.2：初始化。

${\hat{x}}_0=E\left[x_0\right]---\left(12\right)$

$S_0=\mathrm{chol}\left\{\right[(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\left]\right\}---\left(13\right)$

其中chol{·}表示Cholesky分解，并产生容积点ξ_i及权重。

步骤4.3：时间更新。

求容积点值

通过非线性状态方程进行容积点传播

$X_{i,k}^{*}=f\left(x_{i,k-1}\right)---\left(15\right)$

状态预测

${\hat{x}}_k={\mathrm{\φ}}_{k,k-1}{\hat{x}}_{k-1}---\left(16\right)$

计算估计预测误差协方差矩阵的平方根

$S_k^{-}=\mathrm{tria}\begin{array}{c}\left(\left[\begin{array}{cc}{\mathrm{\λ}}_k^{*}& S_{Q_{k-1}}\end{array}\right]\right)---\left(17\right)\end{array}$

其中tria(·)代表上三角分解,S为下三角分解矩阵.表示Q_k-1平方根分解式.

计算估计预测误差协方差矩阵的平方根

${\mathrm{\λ}}_k^{*}\frac{1}{\sqrt{2n}}\left[\begin{array}{cccc}{X}_{1,k}^{*}-{\hat{x}}_k^{-}& X_{2,k}^{*}-{\hat{x}}_k^{-}& ...& X_{2n,k}^{*}-{\hat{x}}_k^{-}\end{array}\right]---\left(18\right)$

步骤4.4：测量更新。

计算容积点

传播容积点

$Z_{i,k}=h\left(X_{i,k}^{-}\right)---\left(20\right)$

计算两个估计值

${\hat{z}}_k=\underset{i=1}{\overset{2n}{\mathrm{\Σ}}}{\mathrm{\ω}}_i{Z}_{i,k}---\left(21\right)$

平方根Cholesky因式分解

$S_{\mathrm{zz},k}=\mathrm{tria}\begin{array}{c}\left(\left[\begin{array}{cc}{\mathrm{\λ}}_k& S_{R_k}\end{array}\right]\right)---\left(22\right)\end{array}$

新息协方差矩阵平方根

$P_{\mathrm{XZ},K|k-1}=\frac{1}{{\mathrm{\α}}_k}{\mathrm{\γ}}_k{{\mathrm{\λ}}_k}^T---\left(23\right)$

其中

${\mathrm{\λ}}_k=\frac{1}{\sqrt{2n}}\left[\begin{array}{cccc}{Z}_{1,k}^{*}-{\hat{z}}_k& Z_{2,k}^{*}-{\hat{z}}_k& ...& Z_{2n,k}^{*}-{\hat{z}}_k\end{array}\right]---\left(24\right)$

${\mathrm{\γ}}_k=\frac{1}{\sqrt{2n}}\left[\begin{array}{cccc}{X}_{1,k}^{-}-x_k^{-}& X_{2,k}^{-}-x_k^{-}& ...& X_{2n,k}^{-}-x_k^{-}\end{array}\right]---\left(25\right)$

${\mathrm{\&alpha;}}_k=\left\{\begin{array}{c}{\mathrm{\&lambda;}}_{0,k},{\mathrm{\&lambda;}}_{0,k}<\mathrm{1,0}<{\mathrm{\&alpha;}}_k\&le;1\\ 1,{\mathrm{\&lambda;}}_{0,k}\&GreaterEqual;1\end{array}\right.---\left(26\right)$

${\mathrm{\&lambda;}}_{0,k}=\frac{\mathrm{tr}\left({\tilde{y}}_k{{\tilde{y}}_{\text{k}}}^T\right)}{\mathrm{tr}\left(\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&omega;}}_i^{\left(c\right)}({\mathrm{\&chi;}}_{i,(k,k-1)}-{\hat{Z}}_{k,k-1}){({\mathrm{\&chi;}}_{i,(k,k-1)}-{\hat{Z}}_{k,k-1})}^T\right)}---\left(27\right)$

α_k为自适应渐消因子，用来能够平衡状态方程、预测信息与观测信息权重，最终达到控制状态模型扰动异常对滤波解的影响。

计算ASRCKF滤波增益

$W_k=(P_{\mathrm{xz},k|k-1}/S_{\mathrm{zz},k}^T)/S_{\mathrm{zz},k}---\left(28\right)$

计算状态估计值

${\hat{x}}_k=x_k^{-}+W_k(z_k-{\hat{z}}_k)---\left(29\right)$

计算状态估计误差协方差矩阵

$S_k=\mathrm{tria}\left(\left[\begin{array}{cc}{\mathrm{\&gamma;}}_k-W_k{\mathrm{\&lambda;}}_k& W_k{S}_{R_k}\end{array}\right]\right)---\left(30\right)$

经滤波后导航系统将精确的定位信息传送至上位机进行决策，并进行底层控制。采用此方法的车辆位置估计对比见图6采用基于视觉筛选的组合导航方法与未采用此方法的导航路径对比图。

Claims (5)

1.一种基于视觉筛选的无人驾驶汽车组合导航方法，其特征在于：该方法包括以下步骤，

步骤1，坐标系换算；

步骤1.1：大地坐标系换算到视觉坐标系；

摄像头成像变换涉及不同坐标系之间的转换，包括大地坐标系、摄像机坐标系、成像平面坐标系和图像平面坐标系；在无人驾驶智能车导航系统的视觉系统中，需首先将摄像头在大地坐标系中的位置换算到视觉坐标系；

步骤1.2：换算视觉坐标系相对车体坐标系；

步骤1.3：将视觉、车体、大地坐标系进行统一；

GPS输出数据为ECEF坐标系统下的经度、纬度、高度，并不适合于平面运动的车辆导航；采用通用横轴墨卡托坐标系将经纬度转化到平面坐标系并转换到“东北天”大地坐标系下，以此坐标系为主坐标系，并统一其他坐标系；

步骤2，建筑物遮挡角度识别；

步骤2.1：图像预处理；

通过摄像头采集的周围图像信息，首先提取出导航基准线，确定导航路径中的特征点；主要流程包括灰度变换、图像分割、图像去噪、提取候选点、直线拟合、确定已知点；

步骤2.2：道路识别；

步骤2.3：建筑物识别；

通过计算熵值，进行填充处理，分离出天空与遮挡建筑物；

步骤2.3：计算建筑物俯仰角

确定建筑物上隅角、下隅角坐标值，并分别与车体中心点即图像底边中心点连线，同时找到过建筑物上隅角下隅角坐标的直线段，沿直线段分别向水平中线，垂直中线做垂线；分别计算各点连线距离，并换算出建筑物俯仰角余弦值即为建筑物遮挡角度余弦值；

步骤3，非视距环境下卫星信号可用性判断；

步骤3.1：计算卫星高度与车体所在位置水平切线的夹角余弦值；

步骤3.2：判断接收的卫星信号是否被建筑物遮挡；

比较卫星夹角余弦值与建筑物遮挡角度余弦值，判断接收的卫星信号是否被建筑物遮挡，若遮挡，则将此卫星提供的GPS数据剔除；

步骤4，改进的自适应平方根容积卡尔曼滤波算法；

步骤4.1：计算容积点及权重；

步骤4.2：初始化；

步骤4.3：时间更新；

步骤4.4：测量更新。

2.根据权利要求1所述的一种基于视觉筛选的无人驾驶汽车组合导航方法，其特征在于：所述方法的流程包括以下步骤，

步骤1，坐标系换算；

步骤1.1：大地坐标系换算到视觉坐标系；

摄像头成像变换涉及不同坐标系之间的转换，包括大地坐标系、摄像机坐标系、成像平面坐标系和图像平面坐标系；

在智能车视觉系统中，需将大地坐标系换算成摄像头坐标系大地坐标系原点O_w在车体重心正下方地面上，z_v轴垂直向上，x_v轴平行地面指向车体前进方向，y_v轴在图中垂直纸面向外；O_c为摄像机坐标系原点，与O_w点的水平距离为l₁，与地面的垂直距离为l₂；α,β,γ分别为摄像头绕x,y,z旋转角度，本文摄像头只绕y_c轴旋转，该轴与水平面倾斜角为α，旋转角度β、γ为零；(x_c y_c z_c)^T为摄像头坐标系，大地坐标系中点(x_w y_w z_w)^T换算到(x_c y_c z_c)^T为摄像头坐标系采用(1)式方法；

$\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]=R(\mathrm{\&alpha;},\mathrm{\&beta;},\mathrm{\&gamma;})\&CenterDot;\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\end{array}\right]+\left[\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right]---\left(1\right)$

$R=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \mathrm{\&alpha;}& \sin \mathrm{\&alpha;}\\ 0& -\sin \mathrm{\&alpha;}& \cos \mathrm{\&alpha;}\end{array}\right]---\left(2\right)$

其中(t_x t_y t_z)^T为平移向量，R为旋转矩阵为平移向量，R为旋转矩阵；根据图像处理的需要摄像机内部会将摄像机坐标系转换到平面坐标系，同时成像平面坐标系再转换到平面坐标系；

步骤1.2：换算视觉坐标系相对车体坐标系；

根据步骤1.1将已经换算好的视觉坐标系与车体坐标系再次转换

步骤1.3：将视觉、车体、大地坐标系进行统一；

GPS输出数据为ECEF坐标系统下的经度、纬度、高度，并不适合于平面运动的车辆导航；采用通用横轴墨卡托坐标系将经纬度转化到平面坐标系并转换到“东北天”大地坐标系下，以此坐标系为主坐标系，并统一其他坐标系；具体转换方法为：

定义视觉坐标系的原点为摄像机光心在地面上的投影点，其中X轴的正方向为行驶方向的负方向，Y轴的正方向垂直于X轴，水平向右；将视觉坐标系转换到以车为主的视觉坐标系下，其在视觉坐标系中的坐标定义为(x_pvision，y_pvision)；摄像头在大地坐标系中的位置是由GPS实时获得的，其坐标为(x_v,y_v)，通过此坐标和智能车的航向角可以将视觉坐标系转换到大地坐标系下，从而求出目标点在大地坐标系下的坐标(x_p,y_p)；因此，建筑物特征点在大地坐标中的坐标与视觉坐标系中的坐标可表示为：

轴的正方向为行驶方向的负方向，Y轴的正方向垂直于X轴，水平向右；特征点即为图像处理算法得到的建筑物群中的特征点，将视觉坐标系转换到以车为主的视觉坐标系下，其在视觉坐标系中的坐标定义为(x_pvision，y_pvision)；摄像头在大地坐标系中的位置是由GPS实时获得的，其坐标为(x_v,y_v)，通过此坐标和智能车的航向角可以将视觉坐标系转换到大地坐标系下，从而求出目标点在大地坐标系下的坐标(x_p,y_p)；因此，建筑物特征点在大地坐标中的坐标与视觉坐标系中的坐标可表示为：

其中，为车辆行驶航向角，由GPS提供。

3.根据权利要求1所述的一种基于视觉筛选的无人驾驶汽车组合导航方法，其特征在于：步骤2，建筑物遮挡角度识别；

步骤2.1：图像预处理；

通过摄像头采集的周围图像信息，首先提取出导航基准线，确定导航路径中的特征点；主要流程包括灰度变换、图像分割、图像去噪、提取候选点、直线拟合、确定已知点；对城市环境道路图像进行处理分析时，选择适宜的彩色特征模型有利于问题的顺利解决；综合分析道路周围建筑物群的特点，发现2G-R-B特征增强建筑物与背景的对比度，因此本方法应用此特征对图像进行灰度变换；得到灰度图像后，需要对图像进行分割，综合考虑了处理时间和处理效果等因素，选择最大类间方差(OTSU)分割算法对灰度图像进行分割，将建筑物群设定为目标像素，即白像素；分割后的图像中，目标像素群中存在许多噪声点，因此选用形态学的膨胀和腐蚀算法对图像进行去噪，膨胀选用1×3模版，腐蚀选用3×1模版；

步骤2.2：道路识别；

图像预处理完毕后，需要在图像中提取出代表建筑物特征的候选点，本方法选用基于移动窗口的垂直投影法进行候选点的选取，基本方法为：设定一个1×20的模板，将此模板从图像底层开始由左向右移动，计算模版中目标像素的灰度值，扫描一行结束后，计算各列灰度值的平均值和标准差并设定其和为灰度阈值，图像中灰度值大于阈值的部分即为目标区域，由于提取的部分为道路的边界，在边界周围灰度值存在跳跃，因此可以通过计算前后两点间的灰度差提取出边界候选点，按照此方法将模版依次向上移动一个像素，直到距离顶部20个像素为止；根据提取出的候选点，运用基于已知点的Hough变换方法进行直线拟合，已知点取所有候选点的分布中心；在得到建筑物群之后，选择其中的一个点作为代表路径的特征点，记录其在图像坐标系中的位置；该方法中对图像的处理并未强调车道线的显像效果，因组合导航系统中已结合MapX控件，通过GPS/INS组合可实现将车辆本体置于车道中的目的；

步骤2.3：建筑物识别；

进行车道划分后，通过改进的二维熵阈值法方法对周围环境进行熵值估计，将熵值大的区域填充，目的在于分离天空与周围建筑物；

步骤2.3：计算建筑物俯仰角

进行填充后，首先，找到分割区域中垂直线段，中央水平线做垂线，并记录交点坐标，同理，将水平线段，向中央水平线做垂线，并记录交点，同时在填充区域内找到已分割区域中水平垂直线交点并记录坐标值；然后，分别计算各交点到车体点的距离；最后，将处于同一垂直线上的点到车体点的距离余弦值；

建筑物的俯仰角换算方法定义如下：

其中，表示车体位置到前方第n个建筑物下隅角的向量，表示车体位置到前方第n个建筑物下隅角的向量。

4.根据权利要求1所述的一种基于视觉筛选的无人驾驶汽车组合导航方法，其特征在于：步骤3，非视距环境下卫星信号可用性检测

采用视觉模块来判定GPS卫星传输信息的可用性，由于GPS基于无线电定位，无线电定位是通过检测与观察者之间的距离的特征参数估计出来目标位置；其中视距传播是无线电定位技术中准确测量定位参数的必要条件；GPS定位系统在日广野地区定位精度高的原因是满足视距传播LOS的条件；但配备了GPS的智能车辆行驶在城市道路上，因受到信号衰退和阴影效应的影响，接收机接收到的信号，可能不包含视距传播信号或者视距传播信号微弱，即为非视距传播；以视距传播信号的有无和所占的地位为标准，移动终端与基站间的无线信号传播分成三种情况：直射信号占支配地位、直射信号不占支配地位和接收不到直射信号；根据GPS工作原理可知在DDP的情况下，GPS系统定位精度可达到最高，而在UDP的情况下定位估计的精度低；在城市道路情况下，智能车辆对导航系统定位精度要求较高，但由于建筑物群的遮挡，GPS移动端与基站之间视距传播的情况非常少，无线信号只能通过反射，散射和衍射方式到达接收端，大多数为非视距传播；

步骤3.1：计算卫星高度与车体所在位置水平切线的夹角余弦值

采用一种改进的NLOS识别方法，将GPS的数据信息主要为卫星位置信息与视觉模块得到的楼宇遮挡角度信息融合，进而判断车体周围的建筑物是否遮挡了GPS所提供的卫星信号；接收机从每个卫星接收海拔和方位角信号与摄像头采集的前方建筑物上隅角与高度信息进行融合；

其中，a，b分别表示被建筑物遮挡而进行非视距传播的卫星与未被遮挡直接进行视距传播的卫星；如果卫星与的高度E_a，E_b以及与航向角夹角A_a，A_b属于建筑物B_n遮挡区域，此卫星被定义为NLOS情况；卫星高度与车体所在位置水平切线的夹角定义为

$\cos \left(E_a\right)=\frac{\cos \left(L_e\right)\cos \left(L_s\right)\cos (L_{\mathrm{si}}-l_e)+\sin \left(L_e\right)\sin \left(L_s\right)}{\sqrt{1+{\left(\frac{r_e}{r_s}\right)}^2-2\&CenterDot;\left(\frac{r_e}{r_s}\right)\&CenterDot;\cos \left(L_e\right)\cos \left(L_s\right)\cos (L_{\mathrm{si}}-l_e)+\sin \left(L_e\right)\sin \left(L_s\right)}}---\left(5\right)$

其中，r_s为卫星轨道半径，r_e为地球半L_s为卫星所处纬度，L_e为参照点所处纬度，l_s卫星所处经度，l_e参考点的经度；

步骤3.2：比较卫星夹角余弦值与建筑物遮挡角度余弦值，判断接受的卫星信号是否被建筑物遮挡，若遮挡，则将此卫星提供的GPS数据剔除；

具体方法为：若cos(E_a)＜cos(E_bn)，则说明卫星S_a俯仰角度大于前方建筑物B_n的俯仰角度，接收机接收到的S_a卫星传输数据属于视距传播，精确度度高；若cos(E_a)≥cos(E_bn)则说明卫星S_a俯仰角度小于前方建筑物B_n的俯仰角度，接收机接收到的S_a卫星传输数据属于NLOS传播方式，精确度相对较低，此颗卫星提供的数据信息不予以考虑。

5.根据权利要求1所述的一种基于视觉筛选的无人驾驶汽车组合导航方法，其特征在于：步骤4，改进的自适应平方根容积卡尔曼滤波算法；

步骤4.1：计算容积点及权重；

首先需要确定系统方程，经过视觉筛选后的GPS数据与INS采用松组合模式，状态变量X取为15维，包括3个相对平面坐标系的位置及速度误差，姿态角误差、加速度计偏差，陀螺漂移；

$\stackrel{\&CenterDot;}{X}=\mathrm{FX}+u---\left(6\right)$

X＝[δr δv δψ ▽ ε]^T  (7)

F为系统状态转移矩阵；u为状态过程噪声向量；

然后确定观测模型，组合导航数据融合采用松耦合方式，取经过视觉筛选后的GPS、INS输出的位置和速度之差作为观测值，构造观测量：

$\begin{array}{c}{Z}_r\left(t\right)=r_{\mathrm{GPS}}\left(t\right)-r_{\mathrm{INS}}\left(t\right)=\\ r_{\mathrm{GPS}}\left(t\right)-(r_{\mathrm{INS}}\left(0\right)+{\&Integral;}_0^t{v}_{\mathrm{INS}}\left(0\right)\mathrm{dt}+t{\&Integral;}_0{\&Integral;}_0^{t}a\left(t\right)\mathrm{dt})\end{array}---\left(8\right)$

$\begin{array}{c}{Z}_v\left(t\right)=v_{\mathrm{GPS}}\left(t\right)-v_{\mathrm{INS}}\left(t\right)=\\ v_{\mathrm{GPS}}\left(t\right)-(v_{\mathrm{INS}}\left(0\right)+{\&Integral;}_0^{t}a\left(t\right)\mathrm{dt})\end{array}---\left(9\right)$

式中，Z_r(t)是位置观测量；Z_v(t)是速度观测量；r_GPS(t)是GPS的位置观测值；r_INS(t)是INS的位置计算值；v_GPS(t)是GPS的速度观测值；v_INS(t)是INS的速度计算值；r_INS(0)、v_INS(0)是INS的初始位置和速度；a(t)是INS所测得的加速度；则SRCKF-KF的量测方程为

$z_{k+1}^{\mathrm{gps}}=h_{k+1}^{\mathrm{gps}}(x_{k+1},u_{k+1})+v_{k+1}^{\mathrm{gps}}---\left(10\right)$

其中， $h_{k+1}^{\mathrm{gps}}=[h_{k+1,e}^{\mathrm{gps}},\mathrm{0,0},h_{k+1,n}^{\mathrm{gps}}]$ 代表GPS测量矩阵 $h_{k+1,n}^{\mathrm{gps}}=[\mathrm{1,0,0};\mathrm{0,1,0}]$

确定以上基本构架后，计算容积点及权重；算法选取2n个容积点，n表示状态维数，

${\mathrm{\&zeta;}}_i=\sqrt{\frac{2n}{2}}{\left[1\right]}_i$

${\mathrm{\&omega;}}_i=\frac{1}{2n}---\left(11\right)$

i＝1,2,…,2n

ξ_i表示第i个容积点，ω_i为相应容积点权重.容积点2n表示容积点总是状态维数的二倍，[1]_i表示完整对称点集；

步骤4.2：初始化；

${\hat{x}}_0=E\left[x_0\right]---\left(12\right)$

$S_0=\mathrm{chol}\left\{\right[(x_0-{\hat{x}}_0){(x_0-{\hat{x}}_0)}^T\left]\right\}---\left(13\right)$

其中chol{·}表示Cholesky分解，并产生容积点ξ_i及权重；

步骤4.3：时间更新；

求容积点值

通过非线性状态方程进行容积点传播

$X_{i,k}^{*}=f\left(x_{i,k-1}\right)---\left(15\right)$

状态预测

${\hat{x}}_k={\mathrm{\&phi;}}_{k,k-1}{\hat{x}}_{k-1}---\left(16\right)$

计算估计预测误差协方差矩阵的平方根

$S_k^{-}=\mathrm{tria}\left(\left[\begin{array}{cc}{\mathrm{\&lambda;}}_k^{*}& S_{Q_{k-1}}\end{array}\right]\right)---\left(17\right)$

其中tria(·)代表上三角分解,S为下三角分解阵，表示Q_k-1平方根分解式；

计算估计预测误差协方差矩阵的平方根

${\mathrm{\&lambda;}}_k^{*}=\frac{1}{\sqrt{2n}}\left[\begin{array}{cccc}{X}_{1,k}^{*}-{\hat{x}}_k^{-}& X_{2,k}^{*}-{\hat{x}}_k^{-}& ...& X_{2n,k}^{*}-{\hat{x}}_k^{-}\end{array}\right]---\left(18\right)$

步骤4.4：测量更新；

计算容积点

传播容积点

$Z_{i,k}=h\left(X_{i,k}^{-}\right)---\left(20\right)$

计算两个估计值

${\hat{z}}_k=\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&omega;}}_i{Z}_{i,k}---\left(21\right)$

平方根Cholesky因式分解

$S_{\mathrm{zz},k}=\mathrm{tria}\left(\left[\begin{array}{cc}{\mathrm{\&lambda;}}_k& S_{R_k}\end{array}\right]\begin{array}{}\end{array}\right)---\left(22\right)$

新息协方差矩阵平方根

$P_{\mathrm{XZ},K|k-1}=\frac{1}{{\mathrm{\&alpha;}}_k}{\mathrm{\&gamma;}}_k{{\mathrm{\&lambda;}}_k}^T---\left(23\right)$

其中

${\mathrm{\&lambda;}}_k=\frac{1}{\sqrt{2n}}\left[\begin{array}{cccc}{Z}_{1,k}^{*}-{\hat{z}}_k& Z_{2,k}^{*}-{\hat{z}}_k& ...& Z_{2n,k}^{*}-{\hat{z}}_k\end{array}\right]---\left(24\right)$

${\mathrm{\&gamma;}}_k=\frac{1}{\sqrt{2n}}\left[\begin{array}{cccc}{X}_{1,k}^{-}-x_k^{-}& X_{2,k}^{-}-x_k^{-}& ...& X_{2n,k}^{-}-x_k^{-}\end{array}\right]---\left(25\right)$

${\mathrm{\&alpha;}}_k=\left\{\begin{array}{c}{\mathrm{\&lambda;}}_{0,k},{\mathrm{\&lambda;}}_{0,k}<\mathrm{1,0}<{\mathrm{\&alpha;}}_k\&le;1\\ 1,{\mathrm{\&lambda;}}_{0,k}\&GreaterEqual;1\end{array}\right.---\left(26\right)$

${\mathrm{\&lambda;}}_{0,k}=\frac{\mathrm{tr}\left({\tilde{y}}_k{{\tilde{y}}_k}^T\right)}{\mathrm{tr}\left(\underset{i=1}{\overset{2n}{\mathrm{\&Sigma;}}}{\mathrm{\&omega;}}_i^{\left(c\right)}({\mathrm{\&chi;}}_{i,(k,k-1)}-{\hat{Z}}_{k,k-1}){({\mathrm{\&chi;}}_{i,(k,k-1)}-{\hat{Z}}_{k,-k1})}^T\right)}---\left(27\right)$

α_k为自适应渐消因子，用来能够平衡状态方程、预测信息与观测信息权重，最终达到控制状态模型扰动异常对滤波解的影响；

计算ASRCKF滤波增益

$W_k=(P_{\mathrm{xz},k|k-1}/S_{\mathrm{zz},k}^T)/S_{\mathrm{zz},k}---\left(28\right)$

计算状态估计值

${\hat{x}}_k=x_k^{-}+W_k(z_k-{\hat{z}}_k)---\left(29\right)$

计算状态估计误差协方差矩阵

$S_k=\mathrm{tria}\left(\left[\begin{array}{cc}{\mathrm{\&gamma;}}_k-W_k{\mathrm{\&lambda;}}_k& {W_{k}S}_{R_k}\end{array}\right]\right)---\left(30\right)$

经滤波后导航系统将精确的定位信息传送至上位机进行决策，并进行底层控制。