标系，同时成像平面坐标系再转换到平 面坐标系； 步骤1. 2 ;换算视觉坐标系相对车体坐标系； 根据步骤1.1将已经换算好的视觉坐标系与车体坐标系再次转换 步骤1. 3;将视觉、车体、大地坐标系进行统一； GI^S输出数据为ECEF坐标系统下的经度、绅度、高度，并不适合于平面运动的车辆导 航；采用通用横轴墨卡托坐标系将经绅度转化到平面坐标系并转换到"东北天"大地坐标系 下，W此坐标系为主坐标系，并统一其他坐标系；具体转换方法为： 定义视觉坐标系的原点为摄像机光屯、在地面上的投影点，其中X轴的正方向为行驶方 向的负方向，Y轴的正方向垂直于X轴，水平向右；将视觉坐标系转换到W车为主的视觉坐 标系下，其在视觉坐标系中的坐标定义为（XpYW。。，ypYkiJ;摄像头在大地坐标系中的位置 是由GI^S实时获得的，其坐标为（Xy，yj，通过此坐标和智能车的航向角可W将视觉坐标系 转换到大地坐标系下，从而求出目标点在大地坐标系下的坐标（Xp，yp);因此，建筑物特征 点在大地坐标中的坐标与视觉坐标系中的坐标可表示为： 轴的正方向为行驶方向的负方向，Y轴的正方向垂直于X轴，水平向右；特征点即为图 像处理算法得到的建筑物群中的特征点，将视觉坐标系转换到W车为主的视觉坐标系下， 其在视觉坐标系中的坐标定义为（XpYki。。，ypYW。。）；摄像头在大地坐标系中的位置是由GPS 实时获得的，其坐标为（XY，yy)，通过此坐标和智能车的航向角可W将视觉坐标系转换到大 地坐标系下，从而求出目标点在大地坐标系下的坐标（Xp，yp);因此，建筑物特征点在大地 坐标中的坐标与视觉坐标系中的坐标可表示为：
其中，巧为车辆行驶航向角，由GI^S提供。
3.根据权利要求1所述的一种基于视觉筛选的无人驾驶汽车组合导航方法，其特征在 于；步骤2,建筑物遮挡角度识别； 步骤2. 1 ;图像预处理； 通过摄像头采集的周围图像信息，首先提取出导航基准线，确定导航路径中的特征点； 主要流程包括灰度变换、图像分割、图像去噪、提取候选点、直线拟合、确定已知点；对城市 环境道路图像进行处理分析时，选择适宜的彩色特征模型有利于问题的顺利解决；综合分 析道路周围建筑物群的特点，发现2G-R-B特征增强建筑物与背景的对比度，因此本方法应 用此特征对图像进行灰度变换；得到灰度图像后，需要对图像进行分割，综合考虑了处理时 间和处理效果等因素，选择最大类间方差（OTSU)分割算法对灰度图像进行分割，将建筑物 群设定为目标像素，即白像素；分割后的图像中，目标像素群中存在许多噪声点，因此选用 形态学的膨胀和腐蚀算法对图像进行去噪，膨胀选用1X3模版，腐蚀选用3X1模版； 步骤2. 2 ;道路识别； 图像预处理完毕后，需要在图像中提取出代表建筑物特征的候选点，本方法选用基于 移动窗口的垂直投影法进行候选点的选取，基本方法为：设定一个1X20的模板，将此模板 从图像底层开始由左向右移动，计算模版中目标像素的灰度值，扫描一行结束后，计算各列 灰度值的平均值和标准差并设定其和为灰度阔值，图像中灰度值大于阔值的部分即为目标 区域，由于提取的部分为道路的边界，在边界周围灰度值存在跳跃，因此可W通过计算前后 两点间的灰度差提取出边界候选点，按照此方法将模版依次向上移动一个像素，直到距离 顶部20个像素为止；根据提取出的候选点，运用基于已知点的化U曲变换方法进行直线拟 合，已知点取所有候选点的分布中屯、；在得到建筑物群之后，选择其中的一个点作为代表路 径的特征点，记录其在图像坐标系中的位置；该方法中对图像的处理并未强调车道线的显 像效果，因组合导航系统中已结合MapX控件，通过GPS/INS组合可实现将车辆本体置于车 道中的目的； 步骤2. 3 ;建筑物识别； 进行车道划分后，通过改进的二维滴阔值法方法对周围环境进行滴值估计，将滴值大 的区域填充，目的在于分离天空与周围建筑物； 步骤2. 3 ;计算建筑物俯仰角 进行填充后，首先，找到分割区域中垂直线段，中央水平线做垂线，并记录交点坐标，同 理，将水平线段，向中央水平线做垂线，并记录交点，同时在填充区域内找到已分割区域中 水平垂直线交点并记录坐标值；然后，分别计算各交点到车体点的距离；最后，将处于同一 垂直线上的点到车体点的距离余弦值； 建筑物的俯仰角换算方法定义如下：
其中，巧I：表示车体位置到前方第n个建筑物下隅角的向量，巧：；表示车体位置到前 方第n个建筑物下隅角的向量。
4.根据权利要求1所述的一种基于视觉筛选的无人驾驶汽车组合导航方法，其特征在 于；步骤3,非视距环境下卫星信号可用性检测 采用视觉模块来判定GI^S卫星传输信息的可用性，由于GI^S基于无线电定位，无线电定 位是通过检测与观察者之间的距离的特征参数估计出来目标位置；其中视距传播是无线电 定位技术中准确测量定位参数的必要条件；GI^S定位系统在日广野地区定位精度高的原因 是满足视距传播LOS的条件；但配备了GI^S的智能车辆行驶在城市道路上，因受到信号衰退 和阴影效应的影响，接收机接收到的信号，可能不包含视距传播信号或者视距传播信号微 弱，即为非视距传播；W视距传播信号的有无和所占的地位为标准，移动终端与基站间的无 线信号传播分成=种情况；直射信号占支配地位、直射信号不占支配地位和接收不到直射 信号；根据GPS工作原理可知在DDP的情况下，GI^S系统定位精度可达到最高，而在UDP的 情况下定位估计的精度低；在城市道路情况下，智能车辆对导航系统定位精度要求较高，但 由于建筑物群的遮挡，GI^S移动端与基站之间视距传播的情况非常少，无线信号只能通过反 射，散射和衍射方式到达接收端，大多数为非视距传播； 步骤3. 1 ;计算卫星高度与车体所在位置水平切线的夹角余弦值 采用一种改进的化0S识别方法，将GI^S的数据信息主要为卫星位置信息与视觉模块得 到的楼宇遮挡角度信息融合，进而判断车体周围的建筑物是否遮挡了GI^S所提供的卫星信 号；接收机从每个卫星接收海拔和方位角信号与摄像头采集的前方建筑物上隅角与高度信 息进行融合； 其中，a,b分别表示被建筑物遮挡而进行非视距传播的卫星与未被遮挡直接进行视距 传播的卫星；如果卫星与的高度E。，EbW及与航向角夹角A。，Ab属于建筑物B。遮挡区域，此 卫星被定义为化0S情况；卫星高度与车体所在位置水平切线的夹角定义为
其中，r,为卫星轨道半径，r。为地球半L,为卫星所处绅度，L。为参照点所处绅度，1 ,卫 星所处经度，1。参考点的经度； 步骤3. 2 ;比较卫星夹角余弦值与建筑物遮挡角度余弦值，判断接受的卫星信号是否 被建筑物遮挡，若遮挡，则将此卫星提供的GI^S数据剔除； 具体方法为；若cos(E。）<cos(Ej，则说明卫星S。俯仰角度大于前方建筑物B。的俯仰 角度，接收机接收到的S。卫星传输数据属于视距传播，精确度度高；若cos巧。）>cos(EJ 则说明卫星S。俯仰角度小于前方建筑物B。的俯仰角度，接收机接收到的S。卫星传输数据 属于化0S传播方式，精确度相对较低，此颗卫星提供的数据信息不予W考虑。
5.根据权利要求1所述的一种基于视觉筛选的无人驾驶汽车组合导航方法，其特征在 于；步骤4,改进的自适应平方根容积卡尔曼滤波算法； 步骤4. 1 ;计算容积点及权重； 首先需要确定系统方程，经过视觉筛选后的GI^S数据与INS采用松组合模式，状态变量X取为15维，包括3个相对平面坐标系的位置及速度误差，姿态角误差、加速度计偏差，巧螺 漂移； X = FX +U(6) X=[Sr 5v 5i]) ▽ e]T (7) F为系统状态转移矩阵；u为状态过程噪声向量； 然后确定观测模型，组合导航数据融合采用松禪合方式，取经过视觉筛选后的GPS、INS输出的位置和速度之差作为观测值，构造观测量：
式中，Zr(t)是位置观测量；Zv(t)是速度观测量；reps(t)是GPS的位置观测值；riws(t) 是INS的位置计算值；veps(t)是GPS的速度观测值；viws(t)是INS的速度计算值；riws(O)、 Viws(O)是INS的初始位置和速度；a(t)是INS所测得的加速度；则SRCKF-KF的量测方程为
其中，耀=[增L，〇,〇,臘,J代表GPS测量矩阵嗯，,=[1，0,0;化0] 确定W上基本构架后，计算容积点及权重；算法选取化个容积点，n表示状态维数，
Ci表示第i个容积点，《i为相应容积点权重.容积点化表示容积点总是状态维数的 二倍，[l]i表示完整对称点集； 步骤4. 2;初始化； 而=冲、-〇] (12) & = [(-、〇 -為)咕-有)r]} (13 ) 其中chol{ ? }表示化olesky分解，并产生容积点Ci及权重； 步骤4. 3;时间更新； 求容积点值 义"-1 = &-1禹+ = 1，2，..'2? (14) 通过非线性状态方程进行容积点传播 义a-二/(屯*-1) (15 ) 状态预测 义*=凌，诉-1 (16) 计算估计预测误差协方差矩阵的平方根 &:=的。似;.50',])(口） 其中tria( ?)代表上=角分解，S为下=角分解阵，，表示Qk_i平方根分解式； 计算估计预测误差协方差矩阵的平方根
步骤4. 4 ;测量更新； 计算容积点
传播容积点 ^i,k-K^i.k) (20) 计算两个估计值
平方根Oiolesky因式分解 =tria{[Ai^Sr']) (22) 新息协方差矩阵平方根
a巧自适应渐消因子，用来能够平衡状态方程、预测信息与观测信息权重，最终达到 控制状态模型扰动异常对滤波解的影响； 计算ASRCKF滤波增益 =巧(28) 计算状态估计值 (Z, - ) (29) 计算状态估计误差协方差矩阵 Sk=triai[n-Wk,、W*&l)(3〇) 经滤波后导航系统将精确的定位信息传送至上位机进行决策，并进行底层控制。
【专利摘要】基于视觉筛选的无人驾驶汽车组合导航方法，该方法包括以下步骤：坐标系换算；建筑物遮挡角度识别；非视距环境下卫星信号可用性判断；改进的自适应平方根容积卡尔曼滤波算法；提出了以视觉信息为筛选条件的组合导航算法，引入了“剔除非视距传播卫星数据”的概念并给出判断方法，用来剔除由于建筑物遮挡而造成精度下降的GPS卫星数据信息；本发明区别于传统组合方法采用视觉导航用以实时并行处理数据，而是采用视觉信息对GPS数据有针对性的筛选，避免了传统采用视觉导航而造成的维数灾难；提出了改进的自适应平方根容积卡尔曼滤波算法，并考虑到无人驾驶智能车行驶在城市路况中导航数据强非线性的问题。
【IPC分类】G01S19-49, G01C21-34
【公开号】CN104848867
【申请号】CN201510243231
【发明人】段建民, 石慧, 战宇辰, 刘丹
【申请人】北京工业大学
【公开日】2015年8月19日
【申请日】2015年5月13日