路径规划算法(0) -路径规划与 Dubins, Reeds-Shepp曲线的python实现
参考资料: http://planning.cs.uiuc.edu/node821.html
一般常见的路径规划算法, 如 Dijkstra, A*, BFS算法, 重点关注可达性, 搜索速度和效率上等问题, 这些轨迹或以栅格或以图的形式给出路径, 事实上还没有考虑到,实际机器人的运动特征, 这里就要介绍到两种路径规划中常用到的曲线, Dubins曲线和 Reeds-Shepp 曲线, 最大的区别 Dubins曲线适用于只能前向行驶的车, 而Reeds-Shepp曲线适用于既可以向前也可以后退的车
Dubins 曲线
Dubins曲线通常用于机器人和控制理论领域，作为规划轮式机器人、飞机和水下车辆路径的一种方式。Dubins曲线给出了两个有向点之间的最短路径, 这样的路径是实际上可以去实际跟随执行的.
简单的运动模型如下,
x ˙ = V c o s ( θ ) \dot{x}= V cos(\theta) x˙=Vcos(θ)
y ˙ = V s i n ( θ ) \dot{y}= V sin(\theta) y˙=Vsin(θ)
θ ˙ = μ \dot{\theta} = \mu θ˙=μ
其中(x,y) 是汽车的位置, θ \theta θ 是航向, μ \mu μ为转弯速度. 这样的条件下, 对于规划路径有几个限制.
- 机器人最大的速度 V m a x V_{max} Vmax
- 最大的转弯角度 ϕ m a x \phi_{max} ϕmax, 也对应这最小的转弯半径.
Dubins曲线可以用常用的三种运动描述, S S S 直线行驶, L L L左转, R R R 右转. 所以常用的集中运动可以总结如下:
L R L , R L R , L S L , L S R , R S L , R S R { LRL, RLR, LSL, LSR, RSL, RSR} LRL,RLR,LSL,LSR,RSL,RSR
这样的轨迹就被称为 Dubins路径, 两个有向点的最短路径可以如下如所示:
观察上图, R α S d L γ R_{\alpha} S_{d}L_{\gamma} RαSdLγ 由两个相切圆和一段直线组成, 而 R α L β R γ R_{\alpha} L_{\beta}R_{\gamma} RαLβRγ , 所以现在的问题变成了 那种路径方式有最小时间消耗., 我们可以尝试编程解决.
(1) RSL 路径方程举例:
最小转弯半径为 r r r, q l ( x l , y l , θ 1 ) q_{l}(x_l,y_l,\theta_{1}) ql(xl,yl,θ1), q G ( x G , y G , θ 2 ) q_{G}(x_G,y_G,\theta_{2}) qG(xG,yG,θ2)
- 添加约束
- $y轴约束: y_G - y_l $
- $x轴约束: x_G - x_l $
- θ 约 束 : α − β = θ 2 − θ 1 \theta 约束: \alpha - \beta = \theta_2 -\theta_1 θ约束:α−β=θ2−θ1
- 路 径 总 长 度 S = r s i n ( α ) + d + r s i n ( γ ) 路径总长度S = r sin(\alpha) + d + r sin(\gamma) 路径总长度S=rsin(α)+d+rsin(γ)
具体细节就先不展示了, 方程应该很好列出来, 总之当最小转弯半径 r r r确定之后, α \alpha α, β \beta β, d d d, 就能解出来了.
Reeds-Shepp 曲线
现在来考虑 Reeds-Shepp 型的车, 与Dubins车型唯一区别就是. 它可以向后走
同理, Reeds-Shepp 曲线加入了后退的逻辑, 可能的路径类型就扩大很多, 可以规划为9类, 一共有48种
应该也是可以根据约束列方程求解; 我将继续探索, 来日完善…