引言
Boundary value problem (BVP):给出机器人在起始点与终止点的状态,设计出一条状态转移的轨迹。是stated sampled lattice planning的基础,在motion planning技术栈中的位置如下。
Optimal boundary value problem:按某种原则设计出一条最优轨迹。
建模
对于二维/三维空间中的机器人,通常在每个维度上分别进行轨迹设计。此处以三维空间中的无人机为例,考察其在一个轴向的运动。
无人机状态 s = ( p , v , a ) s=(p, v, a) s=(p,v,a)
使用jerk作为控制输入: u = j u=j u=j
状态方程:
s
˙
=
f
s
(
s
,
u
)
=
(
v
,
a
,
j
)
\dot{s}=f_s(s,u)=(v,a,j)
s˙=fs(s,u)=(v,a,j)
目标:最小化jerk二次方的积分,即
min
J
:
=
1
T
∫
0
T
j
(
t
)
2
d
t
\min J : = \frac{1}{T}\int_{0}^{T}j(t)^{2}dt
minJ:=T1∫0Tj(t)2dt
待求解的量是 u ( t ) u(t) u(t)
求解
寻找最优轨迹的一般形式是极小化代价函数
J
=
h
(
s
(
T
)
)
+
∫
0
T
g
(
s
(
t
)
,
u
(
t
)
)
⋅
d
t
J=h(s(T))+\int_{0}^{T} g(s(t), u(t)) \cdot d t
J=h(s(T))+∫0Tg(s(t),u(t))⋅dt
其中,第一项反映了末状态与理想状态的差别,可理解为惩罚项,第二项反映了状态转移过程的代价 (transition cost)。
为求解最优的
u
(
t
)
u(t)
u(t)(即最优
j
j
j),可以使用庞特里亚金极小值原理:引入costate
λ
=
(
λ
1
,
λ
2
,
λ
3
)
\lambda=(\lambda_1,\lambda_2,\lambda_3)
λ=(λ1,λ2,λ3),构建Hamiltonian funciton
H
(
s
,
j
,
λ
)
=
1
T
j
2
+
λ
T
f
s
(
s
,
j
)
=
1
T
j
2
+
λ
1
v
+
λ
2
a
+
λ
3
j
\begin{aligned} H(s, j, \lambda) &=\frac{1}{T} j^{2}+\lambda^{T} f_{s}(s, j) \\ &=\frac{1}{T} j^{2}+\lambda_{1} v+\lambda_{2} a+\lambda_{3} j \end{aligned}
H(s,j,λ)=T1j2+λTfs(s,j)=T1j2+λ1v+λ2a+λ3j
在继续向下之前,首先简要介绍庞特里亚金极小值原理 (Pontryagin’s minimum principle):
下面开始求解,由庞特里亚金极小值原理,
λ
˙
=
−
∇
s
H
(
s
∗
,
j
∗
,
λ
)
=
(
0
,
−
λ
1
,
−
λ
2
)
\dot{\lambda}=-\nabla_{s} H\left(s^{*}, j^{*}, \lambda\right)=\left(0,-\lambda_{1},-\lambda_{2}\right)
λ˙=−∇sH(s∗,j∗,λ)=(0,−λ1,−λ2)
引入待定系数
α
,
β
,
γ
\alpha,\beta,\gamma
α,β,γ,易写出
λ
(
t
)
=
1
T
[
−
2
α
2
α
t
+
2
β
−
α
t
2
−
2
β
t
−
2
γ
]
\lambda(t)=\frac{1}{T}\left[\begin{array}{c} -2 \alpha \\ 2 \alpha t+2 \beta \\ -\alpha t^{2}-2 \beta t-2 \gamma \end{array}\right]
λ(t)=T1⎣⎡−2α2αt+2β−αt2−2βt−2γ⎦⎤
进而,可得最优jerk
j
∗
(
t
)
=
arg
min
j
(
t
)
H
(
s
∗
(
t
)
,
j
(
t
)
,
λ
(
t
)
)
=
a
r
g
min
j
(
t
)
[
1
T
j
2
+
1
T
(
−
α
t
2
−
2
β
t
−
2
γ
)
j
]
=
1
2
α
t
2
+
β
t
+
γ
\begin{aligned} j^{*}(t) &=\arg \min _{j(t)} H\left(s^{*}(t), j(t), \lambda(t)\right) \\ &= arg \min _{j(t)}\left [ \frac{1}{T} j^{2}+\frac{1}{T}(-\alpha t^{2}-2 \beta t-2 \gamma)j \right ] \\ &=\frac{1}{2} \alpha t^{2}+\beta t+\gamma \end{aligned}
j∗(t)=argj(t)minH(s∗(t),j(t),λ(t))=argj(t)min[T1j2+T1(−αt2−2βt−2γ)j]=21αt2+βt+γ
通过对jerk的三次积分,可得最优的
s
(
t
)
s(t)
s(t)
s
∗
(
t
)
=
[
α
120
t
5
+
β
24
t
4
+
γ
6
t
3
+
a
0
2
t
2
+
v
0
t
+
p
0
α
24
t
4
+
β
6
t
3
+
γ
2
t
2
+
a
0
t
+
v
0
α
6
t
3
+
β
2
t
2
+
γ
t
+
a
0
]
s^{*}(t)=\left[\begin{array}{c} \frac{\alpha}{120} t^{5}+\frac{\beta}{24} t^{4}+\frac{\gamma}{6} t^{3}+\frac{a_{0}}{2} t^{2}+v_{0} t+p_{0} \\ \frac{\alpha}{24} t^{4}+\frac{\beta}{6} t^{3}+\frac{\gamma}{2} t^{2}+a_{0} t+v_{0} \\ \frac{\alpha}{6} t^{3}+\frac{\beta}{2} t^{2}+\gamma t+a_{0} \end{array}\right]
s∗(t)=⎣⎡120αt5+24βt4+6γt3+2a0t2+v0t+p024αt4+6βt3+2γt2+a0t+v06αt3+2βt2+γt+a0⎦⎤
按对末状态的要求,分三种情况讨论。
情况一:Fully Defined End Translational State
要求末状态 s ( T ) s(T) s(T)的每个分量严格等于给定值
设期望的末状态
s
(
T
)
=
(
p
f
,
v
f
,
a
f
)
s(T)=(p_f,v_f,a_f)
s(T)=(pf,vf,af),以
s
(
T
)
s(T)
s(T)第一维为例,有
s
∗
(
T
)
−
s
(
0
)
=
α
120
T
5
+
β
24
T
4
+
r
6
T
3
+
a
0
2
T
2
+
v
0
T
+
p
0
−
p
0
=
p
f
−
p
0
s^{*}(T)-s(0)=\frac{\alpha}{120}T^5+\frac{\beta}{24}T^4+\frac{r}{6}T^3+\frac{a_0}{2}T^2+v_{0}T+p_0-p_0=p_f-p_0
s∗(T)−s(0)=120αT5+24βT4+6rT3+2a0T2+v0T+p0−p0=pf−p0
记
Δ
p
=
p
f
−
p
0
−
v
0
T
−
1
2
a
0
T
2
\Delta p=p_{f}-p_{0}-v_{0} T-\frac{1}{2} a_{0} T^{2}
Δp=pf−p0−v0T−21a0T2,移项整理得
α
120
T
5
+
β
24
T
4
+
r
6
T
3
=
Δ
p
\frac{\alpha}{120}T^5+\frac{\beta}{24}T^4+\frac{r}{6}T^3=\Delta p
120αT5+24βT4+6rT3=Δp
类似地,写出
s
(
T
)
s(T)
s(T)所有维度上的改变量,并写作矩阵形式,有
[
1
120
T
5
1
24
T
4
1
6
T
3
1
24
T
4
1
6
T
3
1
2
T
2
1
6
T
3
1
2
T
2
T
]
[
α
β
γ
]
=
[
Δ
p
Δ
v
Δ
a
]
\left[\begin{array}{ccc} \frac{1}{120} T^{5} & \frac{1}{24} T^{4} & \frac{1}{6} T^{3} \\ \frac{1}{24} T^{4} & \frac{1}{6} T^{3} & \frac{1}{2} T^{2} \\ \frac{1}{6} T^{3} & \frac{1}{2} T^{2} & T \end{array}\right]\left[\begin{array}{c} \alpha \\ \beta \\ \gamma \end{array}\right]=\left[\begin{array}{c} \Delta p \\ \Delta v \\ \Delta a \end{array}\right]
⎣⎡1201T5241T461T3241T461T321T261T321T2T⎦⎤⎣⎡αβγ⎦⎤=⎣⎡ΔpΔvΔa⎦⎤
其中,
[
Δ
p
Δ
v
Δ
a
]
=
[
p
f
−
p
0
−
v
0
T
−
1
2
a
0
T
2
v
f
−
v
0
−
a
0
T
a
f
−
a
0
]
\left[\begin{array}{c} \Delta p \\ \Delta v \\ \Delta a \end{array}\right]=\left[\begin{array}{c} p_{f}-p_{0}-v_{0} T-\frac{1}{2} a_{0} T^{2} \\ v_{f}-v_{0}-a_{0} T \\ a_{f}-a_{0} \end{array}\right]
⎣⎡ΔpΔvΔa⎦⎤=⎣⎡pf−p0−v0T−21a0T2vf−v0−a0Taf−a0⎦⎤
从而解出
[
α
β
γ
]
=
1
T
5
[
720
−
360
T
60
T
2
−
360
T
168
T
2
−
24
T
3
60
T
2
−
24
T
3
3
T
4
]
[
Δ
p
Δ
v
Δ
a
]
\left[\begin{array}{l} \alpha \\ \beta \\ \gamma \end{array}\right]=\frac{1}{T^{5}}\left[\begin{array}{lll} 720 & -360 T & 60 T^{2} \\ -360 T & 168 T^{2} & -24 T^{3} \\ 60 T^{2} & -24 T^{3} & 3 T^{4} \end{array}\right]\left[\begin{array}{c} \Delta p \\ \Delta v \\ \Delta a \end{array}\right]
⎣⎡αβγ⎦⎤=T51⎣⎡720−360T60T2−360T168T2−24T360T2−24T33T4⎦⎤⎣⎡ΔpΔvΔa⎦⎤
情况二:Partially Defined End Translational State
要求末状态 s ( T ) s(T) s(T)的部分分量等于给定值
对于 s ( T ) s(T) s(T)第 i i i维的的分量 s i ( T ) s_{i}(T) si(T),若给定 s i ( T ) s_{i}(T) si(T)的值,则该维度上受积极约束,并称下标 i i i属于积极集: i ∈ A i\in \mathcal{A} i∈A
此时,由庞特里亚金极小值原理,对于所有自由分量
s
j
(
T
)
s_{j}(T)
sj(T),其对应的
λ
j
(
T
)
\lambda_{j}(T)
λj(T)
λ
j
(
T
)
=
∂
h
(
s
∗
(
T
)
)
∂
s
j
,
for
j
∉
A
\lambda_{j}(T)=\frac{\partial h\left(s^{*}(T)\right)}{\partial s_{j}}, \text { for } j \notin \mathcal{A}
λj(T)=∂sj∂h(s∗(T)), for j∈/A
由上文
s
∗
(
t
)
s^{*}(t)
s∗(t)的表达式可知,
s
∗
(
T
)
s^{*}(T)
s∗(T)是一个只与
T
T
T有关的函数,因此
λ
j
(
T
)
=
∂
h
(
s
∗
(
T
)
)
∂
s
j
=
0
,
for
j
∉
A
\lambda_{j}(T)=\frac{\partial h\left(s^{*}(T)\right)}{\partial s_{j}}=0, \text { for } j \notin \mathcal{A}
λj(T)=∂sj∂h(s∗(T))=0, for j∈/A
即对于末状态的所有自由分量,其相应的 λ j ( T ) = 0 \lambda_{j}(T)=0 λj(T)=0
下面举一个例子。
Example:固定终点的 p p p与 v v v,对 a a a不做要求,求最优jerk
由
p
p
p与
v
v
v的末状态,有
[
1
120
T
5
1
24
T
4
1
6
T
3
1
24
T
4
1
6
T
3
1
2
T
2
]
[
α
β
]
=
[
Δ
p
Δ
v
]
\left[\begin{array}{cc} \frac{1}{120} T^{5} & \frac{1}{24} T^{4} & \frac{1}{6} T^{3} \\ \frac{1}{24} T^{4} & \frac{1}{6} T^{3} & \frac{1}{2} T^{2} \\ \end{array}\right]\left[\begin{array}{c} \alpha \\ \beta \\ \end{array}\right]=\left[\begin{array}{c} \Delta p \\ \Delta v \\ \end{array}\right]
[1201T5241T4241T461T361T321T2][αβ]=[ΔpΔv]
由于
a
a
a是自由分量,因此
λ
(
T
)
\lambda(T)
λ(T)的第三个分量为0,即
−
α
T
−
2
β
−
2
T
γ
=
0
-\alpha T-2 \beta -\frac{2}{T} \gamma=0
−αT−2β−T2γ=0
以上两式联立,有
[
1
120
T
5
1
24
T
4
1
6
T
3
1
24
T
4
1
6
T
3
1
2
T
2
T
2
2
T
]
[
α
β
γ
]
=
[
Δ
p
Δ
v
0
]
\left[\begin{array}{ccc} \frac{1}{120} T^{5} & \frac{1}{24} T^{4} & \frac{1}{6} T^{3} \\ \frac{1}{24} T^{4} & \frac{1}{6} T^{3} & \frac{1}{2} T^{2} \\ T & 2 & \frac{2}{T} \end{array}\right]\left[\begin{array}{c} \alpha \\ \beta \\ \gamma \end{array}\right]=\left[\begin{array}{c} \Delta p \\ \Delta v \\ 0 \end{array}\right]
⎣⎡1201T5241T4T241T461T3261T321T2T2⎦⎤⎣⎡αβγ⎦⎤=⎣⎡ΔpΔv0⎦⎤
解得
[
α
β
γ
]
=
1
T
5
[
320
−
120
T
−
200
T
72
T
2
40
T
2
−
12
T
3
]
[
Δ
p
Δ
v
]
\left[\begin{array}{c} \alpha \\ \beta \\ \gamma \end{array}\right]=\frac{1}{T^{5}}\left[\begin{array}{lc} 320 & -120 T \\ -200 T & 72 T^{2} \\ 40 T^{2} & -12 T^{3} \end{array}\right]\left[\begin{array}{c} \Delta p \\ \Delta v \end{array}\right]
⎣⎡αβγ⎦⎤=T51⎣⎡320−200T40T2−120T72T2−12T3⎦⎤[ΔpΔv]
类似地,还可以解出固定 p p p与 a a a、固定 v v v与 a a a等共计6种末状态部分固定的问题。详见参考[1]。
情况三:Motion Primitive Cost
未指定末状态的任意分量
简言之,一个motion primitive就是一套【机器人初始状态、运动时间、 p , v , a p,v,a p,v,a的某种组合构成的末状态】的组合,详见参考[1]。
这一类情况不是要求解出具体的
α
,
β
,
γ
\alpha,\beta,\gamma
α,β,γ,而是计算出评估一个motion primitive的代价函数的具体表达式:
J
=
1
T
∫
0
T
j
(
t
)
2
d
t
=
1
T
∫
0
T
(
1
2
α
t
2
+
β
t
+
γ
)
2
d
t
=
1
20
α
2
T
4
+
1
4
α
β
T
3
+
1
3
(
α
γ
+
β
2
)
T
2
+
β
γ
T
+
γ
2
\begin{aligned} J &= \frac{1}{T}\int_{0}^{T}j(t)^{2}dt \\ &= \frac{1}{T}\int_{0}^{T}\left (\frac{1}{2} \alpha t^{2}+\beta t+\gamma\right ) ^{2}dt \\ &=\frac{1}{20}\alpha^2T^4+\frac{1}{4}\alpha \beta T^3+\frac{1}{3}\left (\alpha \gamma + \beta^2 \right )T^2+\beta \gamma T+\gamma^2 \end{aligned}
J=T1∫0Tj(t)2dt=T1∫0T(21αt2+βt+γ)2dt=201α2T4+41αβT3+31(αγ+β2)T2+βγT+γ2
回归到sample in state space的state lattice planning,这一代价函数可以用于评估不同的候选motion primitive。
总结
前两种情况是对具体任务的求解,第三种情况是对motion primitive的评价——在运动规划过程中,按某种规则生成一系列候选motion primitive,使用情况三中代价函数进行评价,筛选出某个(些)合适的motion primitive。
参考
[1] Mueller M W, Hehn M, D’Andrea R. A computationally efficient motion primitive for quadrocopter trajectory generation[J]. IEEE transactions on robotics, 2015, 31(6): 1294-1310.
[2] https://www.shenlanxueyuan.com/course/450