- 1双向最大匹配算法（含完整代码实现，ui界面）正向最大匹配算法，逆向最大匹配算法_chinesedic.txt
- 2Could not find a version that satisfies the requirement torch==1.4.0 (from torchvision)_error: could not find a version that satisfies the
- 30基础如何进入IT行业？
- 4可信联邦学习最新论文：从信息论角度，提出隐私与模型性能的No-free lunch定理...
- 5深入浅出 -- 系统架构之微服务架构的新挑战
- 6倾斜航测技术解决方案_倾斜航测航带重叠
- 7宝宝小爬虫Selenium自动获取网页cookie+Curl数据查询_selenium curl
- 8YOLO~综述
- 9ESP32（MicroPython） 四足机器人（一）_四足机器人寻迹esp32
- 10ros小问题之rosdep update time out问题
基于车辆运动学的规划算法 - Hybrid A*/ State Lattice Planning (前置知识：自行车模型、Dubins、Reeds Shepp、多项式曲线、螺旋曲线)_计算自行车的运动学坐标系
赞
踩
本文将按顺序讲解自行车模型、Dubins和Reeds Shepp曲线、多项式曲线、螺旋曲线，再讲解两个案例：混合A*和State Lattice Planning
1 自行车模型
自行车模型：简化左右轮为一个轮子
两个自由度：后轮提供纵向速度或者加速度信息，前轮提供转向信息
自行车模型有三种定义参考点的方式：后轴中心、质心、前轴中心
基本假设：
（1）车辆在一个二维平面上运动（不会考虑上下坡和颠簸路段的运动）
（2）不考虑轮胎的滑移：假定车轮的运动方向和速度方向相等的
高速轮胎会有变形，轮胎的实际速度方向可能和当前车轮方向不一致。
1.1 以后轮为参考点的单车模型
世界坐标系后轴中心的坐标：
(
x
,
y
,
θ
)
(x,y,\theta)
(x,y,θ)
系统的输入：
v
、
ξ
v、\xi
v、ξ
有了上面的运动学模型，给定一个初始量
(
x
0
,
y
0
,
θ
0
)
(x_0,y_0,\theta_0)
(x0,y0,θ0)，系统的初始输入
(
v
0
、
ξ
0
)
(v_0、\xi_0)
(v0、ξ0)，同时给定一小段时间
Δ
t
\Delta t
Δt，通过这个模型，就能预测下一时刻的状态
(
x
1
,
y
1
,
θ
1
)
(x_1,y_1,\theta_1)
(x1,y1,θ1)
1.2 以前轮为参考点的单车模型
给定一个初始量
(
x
0
,
y
0
,
θ
0
)
(x_0,y_0,\theta_0)
(x0,y0,θ0)，系统的初始输入
(
v
0
、
ξ
0
)
(v_0、\xi_0)
(v0、ξ0)，同时给定一小段时间
Δ
t
\Delta t
Δt，通过这个模型，就能预测下一时刻的状态
(
x
1
,
y
1
,
θ
1
)
(x_1,y_1,\theta_1)
(x1,y1,θ1)
1.3 以质心为参考点的单车模型
1.4 前向积分
（1）使用转向角作为输入会导致转向角瞬间变化，所以更准确的是使用转向角的变化率
ϕ
\phi
ϕ作为模型的输入
（2）离散化模型，给定输入
[
v
,
ϕ
]
[v,\phi]
[v,ϕ]生成下一个状态
2 运动基元的生成方法
2.1 Dubins曲线
Dubins曲线
Task：最小化曲线的长度（汽车在任意起点
q
I
q_I
qI和终点
q
G
q_G
qG行驶）
假定速度为常量，系统被简化为：
u u u选自区间 U = [ − t a n ϕ m a x , t a n ϕ m a x ] U=[-tan\phi_{max},tan\phi_{max}] U=[−tanϕmax,tanϕmax]，进一步简化，假定 t a n ϕ = 1 tan\phi=1 tanϕ=1，满足 ϕ ∈ ( 0 , π / 2 ) \phi\in (0,\pi /2) ϕ∈(0,π/2)
Dubins证明了对于两个配置（位姿），Dubins曲线的最短路径总是表示不超过下面这三个运动基元的组合
(1) S：直行
(2) L：以最大转角向左运动
(3) R：以最大转角向右运动
Dubins曲线表明只有这六个词可能是最优的，任意两个配置之间的最短路径总是可以用这些词之一来表示：
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
L
/
R
L/R
L/R：下标表示在原有运动基元累积的旋转总量
S
S
S：下标表示行进的总距离
β
\beta
β：必须大于
π
\pi
π（如果小于
π
\pi
π，一定会有另一种组合是最优的）
对于给定的 q I q_I qI和 q G q_G qG，有两个问题
（1）哪一种组合最优：解析法求解
（2） α , β , γ , d \alpha, \beta ,\gamma, d α,β,γ,d的值如何表示 ：几何法求解
2.2 Reeds Shepp曲线
Dubins曲线只允许车辆正向运动，Reeds Shepp曲线允许车辆反向运动
u
1
u_1
u1：指定车辆前向或者后向
u 1 ∈ [ − 1 , 1 ] u_1\in[-1,1] u1∈[−1,1]
u 2 u_2 u2：指定车辆向左还是向右、直行
u 2 ∈ [ − t a n ϕ m a x , t a n ϕ m a x ] u_2\in[-tan\phi_{max},tan\phi_{max}] u2∈[−tanϕmax,tanϕmax]，进一步简化 u 2 ∈ [ − 1 , 1 ] u_2\in[-1,1] u2∈[−1,1]，满足 ϕ ∈ ( 0 , π / 2 ) \phi\in (0,\pi /2) ϕ∈(0,π/2)
因为Reeds Shepp曲线引入了倒车，组合数也更多，有48种方式
箭头表示汽车的朝向，红色曲线是Reeds-Shepp曲线，而黑色曲线是Dubins曲线。注意二者有时是重合的
只要存在连接起止位姿的无碰撞路径，就存在无碰撞的Reeds-Shepp曲线。但这个结果对Dubins曲线却不适用。
理论上存在但是没有给实际寻找的方法，采用随即搜索的方法，黑色表示障碍物
Dubins曲线和Reeds-Shepp曲线的用处
具有车轮的东西都属于非完整约束系统，不包括特殊的车轮（比如麦克纳姆轮），一般用Dubins曲线和Reeds-Shepp曲线作为复杂模型的近似最短路径，在实际执行时并不一定严格地遵循这些曲线
2.3 多项式曲线 - 五次多项式
五次多项式
开始点和终点状态作为边界条件
(
x
s
,
v
s
,
a
s
)
(x_s,v_s,a_s)
(xs,vs,as)
(
x
e
,
v
e
,
a
e
)
(x_e,v_e,a_e)
(xe,ve,ae)
具体推导详见：b站老王 自动驾驶决策规划学习记录（二）
五次多项式的应用
（1）在Frenet坐标系下，用五次多项式描述纵向
s
(
t
)
s(t)
s(t)和横向运动
l
(
t
)
l(t)
l(t)
把横向和纵向运动结合得到如下在世界坐标系下的轨迹，然后选择一条最优（绿色）的轨迹
轨迹生成的完整流程
p
o
s
→
v
e
l
→
a
c
c
→
j
e
r
k
pos\to vel\to acc\to jerk
pos→vel→acc→jerk
五次多项式总体目标：
m
i
n
i
m
i
z
e
∫
0
T
j
e
r
k
2
(
t
)
d
t
minimize\int_{0}^{T} jerk^2(t)dt
minimize∫0Tjerk2(t)dt
（2）描述运动的方式
l
(
s
)
l(s)
l(s),
s
(
t
)
s(t)
s(t)
求解过程
（3）使用五次多项式生成状态格路径集
2.4 螺旋曲线 - Cubic Spiral Curve
螺旋曲线由曲率定义为弧长的函数
k
(
s
)
=
d
s
3
+
c
s
2
+
b
s
+
a
k(s)=ds^3+cs^2+bs+a
k(s)=ds3+cs2+bs+a
使用简单的运动学质点模型
k
=
d
θ
/
d
s
k=d\theta/ds
k=dθ/ds
螺旋位置没有闭式解。
x
(
s
)
,
y
(
s
)
x(s), y(s)
x(s),y(s) 的公式是
F
r
e
s
n
e
l
Fresnel
Fresnel积分
三角函数里面套了个函数，这种函数大多数没有解析解。
采用数值方法评估 F r e s n e l Fresnel Fresnel积分
S
i
m
p
s
o
n
Simpson
Simpson方法：
基本思想：
通过一个二次函数近似原函数的值
（1）Simpson的方法比其他更简单的数值方法（例如中点和梯形规则）更精确
（2）划分区间n决定了积分评估的准确性和效率
随着n增加，得到了更准确的近似，也会有更多的计算负担
求解a，b，c，d四个参数
工程技巧：对原问题做参数映射
（1） p 0 p_0 p0和 p 3 p_3 p3对应于给定的初始状态和结束状态，降低维度
（2）重映射公式 p 0 + p 1 s + p 2 s 2 + p 3 s 3 p_0+p_1s+p_2s^2+p_3s^3 p0+p1s+p2s2+p3s3确保 p i p_i pi大小接近，在优化中添加数值稳定性
p
0
,
p
1
,
p
2
,
p
3
p_0,p_1,p_2,p_3
p0,p1,p2,p3对应沿整个路径等距的4个点处的曲率
s
f
s_f
sf对应路径的总弧长
生成的多项式：
使用shooting方法求解参数
优化目标：
使用牛顿法做方程组求解
牛顿法基本思想：
（1）首先给定初值，然后用初值切线近似原函数的值
（2）迭代公式为
x
n
+
1
=
x
n
−
f
(
x
n
)
f
′
(
x
n
)
x_{n+1}=x_n-\frac{f(x_n)}{{f}^{'}(x_n)}
xn+1=xn−f′(xn)f(xn)
初值选择策略：
（1）从预先计算的查找表中获取
（2）采用上一次迭代的结果值作为当前的初值
3 Hybrid A*
基本思想：
（1）在控制空间采样，通过前向积分方式生成运动基元作为整个graph的边。
（2）使用栅格地图修建一些节点减少图的大小（如果多个state在一个栅格里，选择一个最优的state）
（3）使用类似A*的方法从状态图中搜索路径
搜索算法的图形比较
（1）A* 将成本与单元格的中心相关联，并且只访问对应于网格单元格中心的状态
（2）混合A* 将连续状态与每个单元格相关联，单元格的分数是其相关连续状态的成本
3.1 Hybrid A* 算法流程
算法流程基本与A*相似
有几处不同：
（1）g(n)累计成本：距离、改变行驶方向、惩罚转向等
（2）估计成本h(n)：欧几里得/曼哈顿距离
（3）通过从控制空间和前向积分采样来展开邻居节点
（4）合并在离散空间中占据相同单元格的连续坐标状态
3.2 启发式函数设计
（1）Non-holonomic-without-obstacles
- 启发式函数考虑了车辆运动学的特征，最大转角，忽略了环境
- 估计基于Reeds Shepp路径的最短距离
（2）holonomic-with-obstacles
- 启发式函数没有考虑车辆运动学的特征，只考虑了障碍物
- 估计基于目标节点与当前正在扩展的顶点之间的最短距离
（a）欧几里得距离
（b）Non-holonomic-without-obstacles
（c）Non-holonomic-without-obstacles
（d）联合the non-holonomic-without-obstacles 和holonomic-with-obstacles
3.3 工程上面的trick
分析扩展
问题：离散化搜索永远不会达到精确的连续目标状态（精度取决于A*网格的分辨率）
解决：使用Reeds Shepp路径分析连接当前节点和目标
好处：这种方法大大提高了稀疏障碍物的搜索速度
分层规划
（1）Hybrid A* 生成的路径通常是次优的，值得进一步改进
（2）用共轭梯度(CG)下降法平滑混合A*路径
4 State Lattice Planning
基本思想：
（1）在状态空间下采样到一系列终点状态
Frenet坐标系（
l
,
l
′
,
l
′
′
l,l',l''
l,l′,l′′）、笛卡尔坐标系（
x
,
y
,
θ
,
k
x,y,\theta,k
x,y,θ,k）
（2）求解边界值BVP问题获得运动基元
（3）为每条path打分
（4）选择最优的path
4.1 Conformal Lattice
（1）在结构化道路行驶，Conformal Lattice定义了一种采样规则
（2）目标状态沿着道路从目标点横向偏移采样
（3）可以在避开障碍物的同时加快规划过程
4.2 规划过程
Sample Goal States
（1）目标状态沿道路从目标点横向偏移采样
（2）采样规则根据自车速度和其他因素调整
生成路径集合
解决BVP问题
（1）Frenet坐标系：五次多项式连接
（2）笛卡尔坐标系：三次螺旋曲线连接
打分和选择
成本函数设计：
（1）安全性
（2）平滑性
（3）中心偏移
5 正向运动学和逆向运动学方法的比较
正向运动学（控制空间采样）
（1）容易计算：给定初始状态和控制变量，通过整合运动学/动力学模型可以获得整个状态轨迹
（2）弱目标引导：依靠启发式函数来指导搜索过程
（3）具有更强的探索环境的能力
逆向运动学（状态空间采样）
（1） 难以计算：需要解决相对复杂的边界值问题
（2）强目标引导：直接向目标生成采样状态
（3）精心设计采样规则
非结构化场景（泊车）倾向于正向运动学方法，基于Hybrid思想，先搜一条粗解，再平滑实现泊车轨迹
结构化场景倾向于逆向运动学方法
