基于路标的智能车辆定位_钱钧

高 , 并且 算法 复杂度 高 , 可 靠性 差. 全 球定位 系统 (GPS)在城区环境中工作时 , 容易因建筑物 、树木等 阻挡而导致信号丢失 , 并且因受精度和成本等因素 影响而限制了其在智能车辆上的应用.
本文以应用 于城区 智能交 通系 统的 CyberC3 智能车辆为研究平台 , 使用激光雷达做为外部传感 器 , 对环境中的圆柱形人工路标进行检测和特征提
行加权平均 , 获得路标半径 a 的精确估计值. 当 n =
1 时 , 路标中心 B 直接表示为(a +ρ1 , θ1 ).
2. 2 测量模型
激光雷达水平固定安装在车辆前部 , 其中心 L
(见图 1)在车辆坐标系 xmOm ym 中的位置为(xm , L ,
ym , L), 图 1 中 ym , L =0. 根据车辆的当前状态估计值
uk =[ ΔAk , Δψk] T
其中 , k 为第 k 个采样时刻. 智能车辆使用阿克曼转
向机构 , 为简化运动模型 , 将车辆近似看做为三轮车
模型 , 即前轮的左 、右轮用位于两轮中间的虚拟中心
轮 M 代替 , 如图 1 所示. 则
ΔAk =(Dk - D k- 1)Cd
(1)
Δψk =(ΔAk t an φk )/b
过程噪声 wk =[ w 1 , k , w 2 , k] T . 则运动过程为
xk
xk-1
yk = yk-1 +
ψk
ψk- 1
(ΔAk +w 1 , k)cos[ ψk-1 +(Δψk +w 2 , k)/2]
(ΔAk +w 1 , k)sin[ ψk- 1 +(Δψk +w 2 , k)/ 2] (3) Δψk +w2 , k
为使激光雷达获得可靠的测量信息 , 在环境中 放置圆柱形人工路标. 路标在激光雷达数据中对应 为若干个距离变化较小 、角度相邻的测量点 , 形成路 标的圆弧形外表面轮廓. 将测量数据分成若干个可 能成为路标的点集 , 提取路标中心位置 , 再与已知路 标地图进行数据关联. 根据先验知识 , 路标半径与路 标到激光雷达测量中心的距离及对应的测量点数目 存在比例关系 , 可以依此估计路标半径 a 的粗略值. 为了提取路标中心 B 在激光雷达视角坐标系 x sOs y s 中的极坐标位置(ρB , θB ), 点 Os 为激光雷达 测量中心. 首先根据对应路标的 n 个连续测量点 M j
=2
-
2 (ω1
n- 1
+ωn ) j =1
ωj
+ 2
ωj+1
ρBj
(8)
n ≥2
如果
θB -
θj +θj+1 2
≤ CT H
其中 , CTH 为阈值常数. 此时由于中垂线与射线近乎
平行 , ρB j 的值趋向无穷大 , 则使用
ρBj
=a
+ρj
+ρj+1 2
确定路标中心位置后 , 将其到 n 个测量点的距离进
Xk , 通过坐标变换获得激光雷达测量中心在全局坐
标系 xOy 中的位置坐标(x L , y L), 激光雷达的方向
基于路标的智能车辆定位
钱 钧a , 杨汝清a , 王 晨a , 周启龙a , 杨 明b
(上海交通大学 a. 机器人研究所 ;b. 自动化系 , 上海 200240)
摘 要 :为解决智能车辆在城市环境中的定位问题 , 使用激光雷达检测和提取圆柱形路标的中心 位置 , 并与已知地图进行数据关联 ;以 Cy be rC3 智能车辆为移动平台 , 建立了车辆运动模型和传感 器测量模型 , 使用扩展卡尔曼滤波算法对激光雷达和编码器数据进行融合 , 计算车辆的全局位姿. 实验结果证明该方法可以获得较高的定位精度 , 解决了车辆自主导航的关键问题 , 并可推广应用到 基于自然特征的全局定位. 关键词 :智能车辆 ;定位 ;激光雷达 ;路标 ;扩展卡尔曼滤波器 中图分类号 :T P 242. 6 文献标识码 :A
精确的定位信息提供依据.
1 车辆运动模型
Cy berC3 通过控制后轮驱动电动机和方向盘转
向电动机的转动实现自动驾驶 , 在驱动电动机转轴
和方向盘转轴上分别安装里程编码器和方向盘编码
器 , 编码器数据由数字信号处理器(DSP)采集 , 并通
过控制器局域网(CAN)总线发送到车载计算机. 智
能车辆的控制输入用行驶里程增量 ΔAk 和方向角 增量 Δψk 表示 , 即
89 6
上 海 交 通 大 学 学 报
第 41 卷
(ρj , θj), j =1 , 2 , … , n , 获得 θB 的值(见图 2):
n
n
∑ ∑ θB = ωjθj , ωj =1
(6)
j =1
j =1
加权系数 ωj 与 ρj 成反比.
图 2 圆柱形路标中心的提取 Fig . 2 Ex tractio n o f the center o f cy lindrical landmark
收稿日期 :2006-06-26 基金项目 :欧盟亚洲信息技术与通信计划(CN / A sia-IT &C /002 (88667)), 欧盟第六框架 CyberCars-2 (EC-FP6-IS T-028062)资助项目 作者简介 :钱 钧(1979-), 男 , 安徽舒城人 , 博士生 , 主要研究方向为移动机器人定位与导航. 杨汝清(联系人), 男 , 教授 , 博士生导师 ,
电话(T el. ):021-34206374 ;E-mail :rqy ang @s jtu . edu. cn.
第 6 期
钱 钧 , 等 :基于路标的智能车辆定位
89 5
取 , 将其中心位置与已知路标地图进行关联. 结合里 程编码器和方向盘编码器数据 , 分别建立车辆运动 模型和传感器测量模型, 应用扩展卡尔曼滤波 (EK F)算法进行车辆位姿估计[ 2 , 3] . 通过多 次室外 实验 , 分析位姿估计误差结果及其影响因素 , 为获得
Shanghai 200240 , China)
Abst ract :Aiming a t estimat ing locatio n o f int ellig ent vehicle in an urban enviro nment , a laser scanne r w as used to det ect and ex t ract t he po sitio n o f the center o f each cy lindrical landm ark. T he result of feat ure ext ractio n i s associat ed w i th know n map. Based on a mobi le pla tf orm nam ed Cy berC3 i ntelligent vehicle , model s of vehicle mo tion and senso r measurem ent w ere built accordingly . An ex tended Kalman fil ter algori thm co mpletes t he f usion o f dat a fro m laser scanner and encoders so as to calculat e glo bal pose o f the vehicle. T he experi ment al result s show high accuracy of po se est imatio n and the key problem in aut onomo us navig ation o f vehicle i s resolved. T he appli catio n of this m ethod can be ex tended t o natural feature based global localizati on. Key words :int ellig ent v ehicle ;localization ;laser scanner ;landmark ;ex tended Kalman fil ter (EKF)
ωj 的具 体取值方 法 :对 每个 测量点 添加属 性
sj , 按照距离值 ρj 从大到小排序 , 距离为最大值的 sj
值赋为 1 , 再选择次大值 , 其 sj =2 , 依此类推 , 直到
剩下最小距离值 , 其 sj =T , 通过该方法将 n 个距离
值分成 T 段(T ≤n). 由于存在 ρj 值相同的情况 , 因
此对应第 q 段的数目为 t q 个 , q =1 , 2 , …, T , 则
T
∑ ωj =sj
qt q
(7)
q =1
现以相邻两测量点为端点做中垂线 , 并与从 Os 点发
出 、倾角为 θB 的射线相交 , 交点为 B j(ρBj , θB ), 根据 其距离值 ρBj 确定 ρB 的值 ,
∑ ρB
雅可比矩阵为 :
1 0 - ΔAk si n ψa
JfX
=
f Xk- 1 s ψa
(4)
00
1
cos ψa
-
ΔAk sin 2
ψa
Jfw
=
f wk
=
sin ψa
ΔA 2
k
co
s
ψa
(5)
0
1
式中 , ψa =ψk -1 +Δ2ψk .
2 传感器测量模型
CyberC3 使用的 二维激 光雷 达为 SICK LM S 291-S05 激光测距传感器[ 4 , 5] . 该传感器在标准参数 配置下 , 距离在 20 m 以内时测量误差为 ±5 cm ;扫 描获得的相邻激光束中心间的夹角为 0. 5°, 而激光 在环境中物体表面形成的光斑直径比相邻光斑间距 离大 , 即相邻激光束覆盖的范围存在部分相互重叠 现象 , 因此 , 存在较小的角度不确定性. 当波特率设 置为 500 Kb / s 时 , 数据获取频率为 37. 5 Hz. 激光 雷达与车载计算机间通过 RS-422 至通用串行总线 (U S B)接 口转 换器 进行 数据 通信 . 2. 1 特征检测与提取