动态环境中基于碰撞预测的局部路径规划方法
合集下载
相关主题
- 1、下载文档前请自行甄别文档内容的完整性，平台不提供额外的编辑、内容补充、找答案等附加服务。
- 2、"仅部分预览"的文档,不可在线预览部分如存在完整性等问题,可反馈申请退款(可完整预览的文档不适用该条件!)。
- 3、如文档侵犯您的权益，请联系客服反馈,我们会尽快为您处理(人工客服工作时间：9:00-18:30)。
sign Automation ,1991 ,32 (1) :2412248. [ 12 ] 许焕卫 , 黄洪钟 , 何俐萍 . 稳健设计中的稳健可行
[ 9 ] Youn B D ,Choi K K ,Du L ,et al. Integration of Po s2 sibilit y - based Optimizatio n and Ro bust Design fo r Epistemic U ncertainty [ J ] . Jo urnal of Mechanical Design ,2006 ,129 ( 8) :9452958. [ 10 ] Taguchi G. Quality Engineering Thro ugh Design Optimization[ M ] . New York : Krauss International Press ,1986. [ 11 ] Ramakrishnan B ,Rao S S. A Robust Optimization Ap2 proach Using Taguchi ’ s Loss Function for Solving Nonlinear Optimization Problems[J ]. Advances in De2
性分析 [ J ] . 清 华 大 学 学 报 , 2007 , 47 ( z2 ) : 17212
1744. ( 编辑 袁兴玲)
作者简介 : 孙 伟 , 男 ,1967 年生 。大连理工大学机械工程学院 教授 、 博士研究生导师 。主要研究方向为复杂机械装备的设计及 优化 、 机械传动及结构 CAD/ CA E 等 。董荣梅 , 女 , 1979 年 生 。 大连理工大学机械工程学院博士研究生 。许焕卫 , 男 , 1974 年 生 。大连理工大学机械工程学院博士研究生 。
动态环境中基于碰撞预测的局部路径规划方法 — — — 高 扬 孙树栋 赫东峰
动态环境中基于碰撞预测的局部路径规划方法
高 扬 孙树栋 赫东峰
西北工业大学现代设计与集成制造技术教育部重点实验室 ,西安 ,710072
摘要 : 提出一种基于碰撞预测的局部路径规划方法 。以自适应无色卡尔曼滤波器跟踪障碍物获得 障碍物位置 、 相对运动速度及其概率分布 ,预测障碍物与机器人的潜在碰撞区域 ,并依其位置 、 速度的不 确定性扩充碰撞危险区域以重构环境威胁区分布 。以自由路径表征可以通过的自由空间 ,引入风险函 数评估碰撞的风险 。通过搜索最优自由路径及其风险值压缩表示环境信息 ,使得利用模糊控制器进行 局部路径规划的实时性获得提高 。该方法对动态障碍物及其不确定性均有较强的适应能力 。实验及仿 真结果均表明 ,在动态环境中该方法所得路径优于已有方法 。 关键词 : 局部路径规划 ; 自由路径 ; 模糊控制 ; 动态环境 ; 碰撞预测 中图分类号 : TP242 文章编号 :1004 — 132X ( 2009) 21 — 2553 — 05
Oi ( 图 1 ) , 它表示了膨胀后障碍物 i 的碰撞威胁 , oi
x oi , y oi 为 o i 在 R 0 坐标系的直角坐标 , X oi , Y oi 为 o i
′
′
′
表征了该区域的位置 。 定义危险区为可能碰撞的 区域 ( 对静态障碍物 i , 其危险区等价于 Oi ) , 定义 与危险区无相交的扇区为自由扇区 , 约定本文涉 及的长度单位均为 m , 角度单位为度 。 由于常用传 感器的不可穿透性 , 对物体的观测往往只及一面 , 因此假设符合实际 。
0 引言
作为一个重要研究领域 , 移动机器人路径规 划能够寻找一条起点至终点的最优路径使之安全 到达目标 。依环境信息预知程度不同 , 路径规划 可分为所有信息预知的全局路径规划 、 至少部分 信息未预知的局部路径规划[ 1 ] 。 由于难于预知环境中的全部信息 , 实际中必 须依赖传感器所得有限信息实时规划路径 ,因此
A Local Path Planning of Mobile Robots in Dynamic Environment Based on Prediction of Coll ision Gao Yang Sun Shudo ng He Do ngfeng School of Mechat ro nic Nort hwestern Polytechnical U niversit y , Xi ’ an ,710072 Abstract : A local pat h planning based o n p redictio n of collisio n was p ropo sed. The velocit y of o b2 stacles and it s p ro babilit y dist ributio n was estimated by an adap tive unscented Kalman filter ( U KF ) , so t hat t he dangero us regio n (potential collisio n regio n ) was p redicted. Based o n t he p ro babilit y dis2 t ributio n of t he dangero us regio n , it was t hen expanded to ensure t he securit y. By finding t he best “f ree road ”and estimating t he “danger ” , all informatio n needed in pat h planning was t hen co m2 p ressed. Co nsequently t he efficiency of t he f uzzy logic co nt roller was imp roved significantly , which was al so employed to calculate t he speed of t he ro bot . The result s of bot h simulatio ns and experiment s p rove t hat t he met hod get s bet ter effect in bot h static enviro nment and dynamic enviro nment wit h un2 certaint y. Key words : local pat h planning ; f ree road ; f uzzy lo gic co nt rol ; dynamic enviro nment ; collisio n p redict
′ 到 O′ 式 i 的中心 o i , 坐标变换关系如式 ( 4 ) 所示 。 ′ ′ 中 , roi 、 <oi 为 o i 的极坐标 ; r′ < oi 、 oi 为 o i 的极坐标 , ′
其中 , 中心 oi 的极坐标 r oi 、 <oi 分别表示障碍物 i 到 机器人的最小距离 、 i 的中心角度 ; <il 、 <ir 分别为 i 的左右边界角 。 式 ( 1) 描述了一扇型障碍物区域
收稿日期 :2008 — 12 — 04 基金项目 :
(200820)
ຫໍສະໝຸດ Baidu
西 北工业大学研究生创业种子基金资助项目
局部路径规划成为该领域的研究热点 。常用局部 路径规划方法主要有人工势场法[ 2 ] 、 改进矢量场 [3 ] 图法 ( imp roved vector field histogram , V F H +) 、 模糊逻辑算法[ 4 ] 、 人工神经网络方法 [ 5 ] 等 。 然而上述方法均未考虑动态障碍物的影响 , 在动 态环境中可能导致碰撞并严重影响路径效率 , 因 此动态未知环境中的路径规划问题引起了众多学 者关注 。席裕庚等 [ 6 ] 提出了基于滚动窗口的路径 规划方法 ,该方法通过场景预测 ,以滚动方式在线 规划 。 Ko 等 [ 7 ] 在建立势场时引入了障碍物速度 。 韩永等 [ 8 ] 引入了速度势场改进人工势场法 。上述 方法多假设物体速度已知 , 而在实际中障碍物速
OD i ( Oi , vL i ,ω i) roi =
′ ′
( 3)
X oi + Y oi
′
′
1 环境建模
现对本文方法作如下假设 : ( 1) 机器人等效为点 , 障碍物依安全半径 rs 膨化处理 。 以机器人中心为原点建立机器人坐标 系 , 正 前方 为 90° , 逆时 针为正 , 角度 范围 ( 0° , 360° ]。 后文坐标默认为此坐系中坐标 。 ( 2) 机器人能够感知路径目标坐标 ( rT , <T ) 。 障碍物平动 , 对视野范围内障碍物传感器可提供 如下信息 :
(
x oi y oi
-
xd yd
)
( 4)
=
设 t0 时刻机器人从位置 R0 ( 记机器人坐标系
R0 ) 经时间步长Δt 运行至 t 1 时刻位置 R1 ( 记机器
人坐标系 R1 ) , 如图 2 所示 。 另设 R0 处对障碍物 i 观测得 Oi , 此时对应坐标系 R1 处对 i 的观测为
Oi 。 则不难由 Oi 的中心 o i 映射至坐标系 R 1 中得
π +θ d) ] 2
D ( dl + dr ) Rd = 2 ( dr - dl ) dr - dl θ d = D
式中 , dl 、 dr 为 Δt 时间内机器人左右轮位移 ; R d 为回转半 径 ; D 为机器人左右轮距 ( 易得 dl = dr 的情况) 。
θ 在 R1 坐标系中的直角坐标 。 xd 、 yd 、 d 分别为 R1 在 R0 坐标系中的 x 、 y 轴坐标以及坐标系 R 1 相对 坐标系 R0 的旋转角度 , 此三者依机器人运动模型 不难由下式得到
xd yd = [ Rd [ sin (
π +θ d) - 1 ] 2
( 5)
Rd co s (
Oi ( oi , <il , <ir ) oi ( roi , <oi ) ( 1) ( 2)
′ <oi = arctan ( Y′ oi , X oi )
X′ oi Y oi x oi y oi
′
=
co sθ d
- sinθ d roi co s <oi roi sin <oi
sinθ d co sθ d
只能依赖传感器信息进行估计 。 然而传感器存在 噪声 , 速度估计模型也存在误差 。 因此以高斯分布 噪声描述传感器噪声及速度估计过程中的误差 , 并采用一种基于自适应 U KF 滤波器的机动目标 跟踪算法 , 获得物体运动速度及其概率分布 。 2 . 1 速度估计数学模型 对动态障碍物本文采用如下相对速度进行描 述 :径向速度 vL ( m/ s , 远离为正) 表征障碍物距离 的变化率 ; 角速度 ω( rad/ s , 逆时针为正 ) 表征障 碍物角度的变化率 。 则动态障碍物 i 可进一步描 述为障碍物区域 :
・2 5 5 3 ・
中国机械工程第 20 卷第 21 期 2009 年 11 月上半月
度难于准确获得 。因此需综合考虑障碍物速度估 计方式及其带来的不确定性 。 本文以高斯分布噪声描述物体运动速度及速 度估计过程中的不确定性 。采用自适应无色卡尔 曼滤波[ 9 ] ( unscented kalman filter ,U KF ) 对障碍 物进行跟踪 ,以获得物体运动速度及其概率分布 , 并据此提出一种新的路径规划算法 : ① 预测可能 碰撞位置构建危险区域 , 并依其概率分布进行扩 充; ② 搜寻可供通行的最优自由路径 ,引入风险函 数评价碰撞风险 ; ③ 以模糊控制器计算机器人线 速度 、 角速度 ,获得所需轮速 。该算法较充分地考 虑了障碍物速度及其不确定性的影响 , 在动态环 境仿真及实验中均表现出良好效果 。
[ 9 ] Youn B D ,Choi K K ,Du L ,et al. Integration of Po s2 sibilit y - based Optimizatio n and Ro bust Design fo r Epistemic U ncertainty [ J ] . Jo urnal of Mechanical Design ,2006 ,129 ( 8) :9452958. [ 10 ] Taguchi G. Quality Engineering Thro ugh Design Optimization[ M ] . New York : Krauss International Press ,1986. [ 11 ] Ramakrishnan B ,Rao S S. A Robust Optimization Ap2 proach Using Taguchi ’ s Loss Function for Solving Nonlinear Optimization Problems[J ]. Advances in De2
性分析 [ J ] . 清 华 大 学 学 报 , 2007 , 47 ( z2 ) : 17212
1744. ( 编辑 袁兴玲)
作者简介 : 孙 伟 , 男 ,1967 年生 。大连理工大学机械工程学院 教授 、 博士研究生导师 。主要研究方向为复杂机械装备的设计及 优化 、 机械传动及结构 CAD/ CA E 等 。董荣梅 , 女 , 1979 年 生 。 大连理工大学机械工程学院博士研究生 。许焕卫 , 男 , 1974 年 生 。大连理工大学机械工程学院博士研究生 。
动态环境中基于碰撞预测的局部路径规划方法 — — — 高 扬 孙树栋 赫东峰
动态环境中基于碰撞预测的局部路径规划方法
高 扬 孙树栋 赫东峰
西北工业大学现代设计与集成制造技术教育部重点实验室 ,西安 ,710072
摘要 : 提出一种基于碰撞预测的局部路径规划方法 。以自适应无色卡尔曼滤波器跟踪障碍物获得 障碍物位置 、 相对运动速度及其概率分布 ,预测障碍物与机器人的潜在碰撞区域 ,并依其位置 、 速度的不 确定性扩充碰撞危险区域以重构环境威胁区分布 。以自由路径表征可以通过的自由空间 ,引入风险函 数评估碰撞的风险 。通过搜索最优自由路径及其风险值压缩表示环境信息 ,使得利用模糊控制器进行 局部路径规划的实时性获得提高 。该方法对动态障碍物及其不确定性均有较强的适应能力 。实验及仿 真结果均表明 ,在动态环境中该方法所得路径优于已有方法 。 关键词 : 局部路径规划 ; 自由路径 ; 模糊控制 ; 动态环境 ; 碰撞预测 中图分类号 : TP242 文章编号 :1004 — 132X ( 2009) 21 — 2553 — 05
Oi ( 图 1 ) , 它表示了膨胀后障碍物 i 的碰撞威胁 , oi
x oi , y oi 为 o i 在 R 0 坐标系的直角坐标 , X oi , Y oi 为 o i
′
′
′
表征了该区域的位置 。 定义危险区为可能碰撞的 区域 ( 对静态障碍物 i , 其危险区等价于 Oi ) , 定义 与危险区无相交的扇区为自由扇区 , 约定本文涉 及的长度单位均为 m , 角度单位为度 。 由于常用传 感器的不可穿透性 , 对物体的观测往往只及一面 , 因此假设符合实际 。
0 引言
作为一个重要研究领域 , 移动机器人路径规 划能够寻找一条起点至终点的最优路径使之安全 到达目标 。依环境信息预知程度不同 , 路径规划 可分为所有信息预知的全局路径规划 、 至少部分 信息未预知的局部路径规划[ 1 ] 。 由于难于预知环境中的全部信息 , 实际中必 须依赖传感器所得有限信息实时规划路径 ,因此
A Local Path Planning of Mobile Robots in Dynamic Environment Based on Prediction of Coll ision Gao Yang Sun Shudo ng He Do ngfeng School of Mechat ro nic Nort hwestern Polytechnical U niversit y , Xi ’ an ,710072 Abstract : A local pat h planning based o n p redictio n of collisio n was p ropo sed. The velocit y of o b2 stacles and it s p ro babilit y dist ributio n was estimated by an adap tive unscented Kalman filter ( U KF ) , so t hat t he dangero us regio n (potential collisio n regio n ) was p redicted. Based o n t he p ro babilit y dis2 t ributio n of t he dangero us regio n , it was t hen expanded to ensure t he securit y. By finding t he best “f ree road ”and estimating t he “danger ” , all informatio n needed in pat h planning was t hen co m2 p ressed. Co nsequently t he efficiency of t he f uzzy logic co nt roller was imp roved significantly , which was al so employed to calculate t he speed of t he ro bot . The result s of bot h simulatio ns and experiment s p rove t hat t he met hod get s bet ter effect in bot h static enviro nment and dynamic enviro nment wit h un2 certaint y. Key words : local pat h planning ; f ree road ; f uzzy lo gic co nt rol ; dynamic enviro nment ; collisio n p redict
′ 到 O′ 式 i 的中心 o i , 坐标变换关系如式 ( 4 ) 所示 。 ′ ′ 中 , roi 、 <oi 为 o i 的极坐标 ; r′ < oi 、 oi 为 o i 的极坐标 , ′
其中 , 中心 oi 的极坐标 r oi 、 <oi 分别表示障碍物 i 到 机器人的最小距离 、 i 的中心角度 ; <il 、 <ir 分别为 i 的左右边界角 。 式 ( 1) 描述了一扇型障碍物区域
收稿日期 :2008 — 12 — 04 基金项目 :
(200820)
ຫໍສະໝຸດ Baidu
西 北工业大学研究生创业种子基金资助项目
局部路径规划成为该领域的研究热点 。常用局部 路径规划方法主要有人工势场法[ 2 ] 、 改进矢量场 [3 ] 图法 ( imp roved vector field histogram , V F H +) 、 模糊逻辑算法[ 4 ] 、 人工神经网络方法 [ 5 ] 等 。 然而上述方法均未考虑动态障碍物的影响 , 在动 态环境中可能导致碰撞并严重影响路径效率 , 因 此动态未知环境中的路径规划问题引起了众多学 者关注 。席裕庚等 [ 6 ] 提出了基于滚动窗口的路径 规划方法 ,该方法通过场景预测 ,以滚动方式在线 规划 。 Ko 等 [ 7 ] 在建立势场时引入了障碍物速度 。 韩永等 [ 8 ] 引入了速度势场改进人工势场法 。上述 方法多假设物体速度已知 , 而在实际中障碍物速
OD i ( Oi , vL i ,ω i) roi =
′ ′
( 3)
X oi + Y oi
′
′
1 环境建模
现对本文方法作如下假设 : ( 1) 机器人等效为点 , 障碍物依安全半径 rs 膨化处理 。 以机器人中心为原点建立机器人坐标 系 , 正 前方 为 90° , 逆时 针为正 , 角度 范围 ( 0° , 360° ]。 后文坐标默认为此坐系中坐标 。 ( 2) 机器人能够感知路径目标坐标 ( rT , <T ) 。 障碍物平动 , 对视野范围内障碍物传感器可提供 如下信息 :
(
x oi y oi
-
xd yd
)
( 4)
=
设 t0 时刻机器人从位置 R0 ( 记机器人坐标系
R0 ) 经时间步长Δt 运行至 t 1 时刻位置 R1 ( 记机器
人坐标系 R1 ) , 如图 2 所示 。 另设 R0 处对障碍物 i 观测得 Oi , 此时对应坐标系 R1 处对 i 的观测为
Oi 。 则不难由 Oi 的中心 o i 映射至坐标系 R 1 中得
π +θ d) ] 2
D ( dl + dr ) Rd = 2 ( dr - dl ) dr - dl θ d = D
式中 , dl 、 dr 为 Δt 时间内机器人左右轮位移 ; R d 为回转半 径 ; D 为机器人左右轮距 ( 易得 dl = dr 的情况) 。
θ 在 R1 坐标系中的直角坐标 。 xd 、 yd 、 d 分别为 R1 在 R0 坐标系中的 x 、 y 轴坐标以及坐标系 R 1 相对 坐标系 R0 的旋转角度 , 此三者依机器人运动模型 不难由下式得到
xd yd = [ Rd [ sin (
π +θ d) - 1 ] 2
( 5)
Rd co s (
Oi ( oi , <il , <ir ) oi ( roi , <oi ) ( 1) ( 2)
′ <oi = arctan ( Y′ oi , X oi )
X′ oi Y oi x oi y oi
′
=
co sθ d
- sinθ d roi co s <oi roi sin <oi
sinθ d co sθ d
只能依赖传感器信息进行估计 。 然而传感器存在 噪声 , 速度估计模型也存在误差 。 因此以高斯分布 噪声描述传感器噪声及速度估计过程中的误差 , 并采用一种基于自适应 U KF 滤波器的机动目标 跟踪算法 , 获得物体运动速度及其概率分布 。 2 . 1 速度估计数学模型 对动态障碍物本文采用如下相对速度进行描 述 :径向速度 vL ( m/ s , 远离为正) 表征障碍物距离 的变化率 ; 角速度 ω( rad/ s , 逆时针为正 ) 表征障 碍物角度的变化率 。 则动态障碍物 i 可进一步描 述为障碍物区域 :
・2 5 5 3 ・
中国机械工程第 20 卷第 21 期 2009 年 11 月上半月
度难于准确获得 。因此需综合考虑障碍物速度估 计方式及其带来的不确定性 。 本文以高斯分布噪声描述物体运动速度及速 度估计过程中的不确定性 。采用自适应无色卡尔 曼滤波[ 9 ] ( unscented kalman filter ,U KF ) 对障碍 物进行跟踪 ,以获得物体运动速度及其概率分布 , 并据此提出一种新的路径规划算法 : ① 预测可能 碰撞位置构建危险区域 , 并依其概率分布进行扩 充; ② 搜寻可供通行的最优自由路径 ,引入风险函 数评价碰撞风险 ; ③ 以模糊控制器计算机器人线 速度 、 角速度 ,获得所需轮速 。该算法较充分地考 虑了障碍物速度及其不确定性的影响 , 在动态环 境仿真及实验中均表现出良好效果 。