STOMP: Stochastic Trajectory Optimization for Motion Planning 文章解读
一、个人总结
STOMP是一种基于随机轨迹优化的框架,它通过生成带有噪声的轨迹来探索初始轨迹(可能是不可行的)周围的空间,然后将这些轨迹结合起来,以产生具有较低成本的更新轨迹。这种方法不需要梯度信息(梯度信息概率化),因此可以优化那些可能没有导数的一般成本函数,例如与约束和电机扭矩相对应的成本。(需要提供原始轨迹,有点类似给定了RRT的树干,然后在树干周围进随机采样,并优化全局路径。其中原始路径的质量很重要,且密度越大,优化效果越好)
算法计算流程(对应原文的Table1):
初始化:
首先,确定机器人的起始位置和目标位置,以及一个初始的轨迹(可能是直线或随机生成的)。这个初始轨迹可能不满足所有的约束条件,比如避开障碍物。
成本函数定义:
定义一个成本函数,它结合了障碍物成本、平滑性成本等。例如,障碍物成本可以通过与障碍物的距离来衡量,平滑性成本可以通过轨迹的加速度变化来衡量。
生成噪声轨迹:
在每一步迭代中,生成一系列带有随机噪声的轨迹。这些噪声是通过在初始轨迹上添加高斯噪声来实现的,噪声的协方差矩阵通常与控制成本相关。
计算成本:
对每个生成的噪声轨迹,计算其在整个运动过程中的成本。这包括与障碍物的交互、轨迹的平滑性以及可能的扭矩成本。
更新轨迹:
使用所有噪声轨迹的成本信息来更新候选解。STOMP算法通过计算一个梯度估计来更新轨迹,这个梯度估计是基于概率加权的噪声参数的凸组合。
迭代优化:
重复步骤3到5,直到满足收敛条件,比如成本不再显著下降或达到最大迭代次数。
输出最优轨迹:
最终,算法输出一个优化后的轨迹,这个轨迹在满足所有约束条件的同时,具有最低的成本。
二、用一个二维的路径优化例子说明文章计算流程
- 起点: ( 0 , 0 ) (0, 0) (0,0)
- 终点: ( 9 , 9 ) (9, 9) (9,9)
- 障碍物:位于 ( 5 , 5 ) (5, 5) (5,5),半径为 1 1 1
- 轨迹离散化点数: 5 5 5 个点,包括起点和终点
- 轨迹初始化:直线轨迹
- 带噪声轨迹数量: K = 2 K = 2 K=2
1. 轨迹初始化
初始轨迹为直线插值:
θ = [ ( 0 , 0 ) , ( 2.25 , 2.25 ) , ( 4.5 , 4.5 ) , ( 6.75 , 6.75 ) , ( 9 , 9 ) ] \theta = [(0,0), (2.25,2.25), (4.5,4.5), (6.75,6.75), (9,9)] θ=[(0,0),(2.25,2.25),(4.5,4.5),(6.75,6.75),(9,9)]
2. 噪声轨迹的生成
生成 K = 2 K = 2 K=2 条带噪声的轨迹,噪声值如下:
- 第一条噪声轨迹 θ ~ 1 \tilde{\theta}_1 θ~1:
ϵ 1 = [ ( 0 , 0 ) , ( 0.1 , − 0.1 ) , ( − 0.2 , 0.1 ) , ( 0.2 , − 0.2 ) , ( 0 , 0 ) ] \epsilon_{1} = [(0,0), (0.1,-0.1), (-0.2,0.1), (0.2,-0.2), (0,0)] ϵ1=[(0,0),(0.1,−0.1),(−0.2,0.1),(0.2,−0.2),(0,0)]
因此:
θ ~ 1 = [ ( 0 , 0 ) , ( 2.35 , 2.15 ) , ( 4.3 , 4.6 ) , ( 6.95 , 6.55 ) , ( 9 , 9 ) ] \tilde{\theta}_1 = [(0,0), (2.35, 2.15), (4.3, 4.6), (6.95, 6.55), (9,9)] θ~1=[(0,0),(2.35,2.15),(4.3,4.6),(6.95,6.55),(9,9)]
- 第二条噪声轨迹 θ ~ 2 \tilde{\theta}_2 θ~2:
ϵ 2 = [ ( 0 , 0 ) , ( − 0.05 , 0.05 ) , ( 0.15 , − 0.15 ) , ( − 0.1 , 0.1 ) , ( 0 , 0 ) ] \epsilon_{2} = [(0,0), (-0.05,0.05), (0.15,-0.15), (-0.1,0.1), (0,0)] ϵ2=[(0,0),(−0.05,0.05),(0.15,−0.15),(−0.1,0.1),(0,0)]
因此:
θ ~ 2 = [ ( 0 , 0 ) , ( 2.2 , 2.3 ) , ( 4.65 , 4.35 ) , ( 6.65 , 6.85 ) , ( 9 , 9 ) ] \tilde{\theta}_2 = [(0,0), (2.2, 2.3), (4.65, 4.35), (6.65, 6.85), (9,9)] θ~2=[(0,0),(2.2,2.3),(4.65,4.35),(6.65,6.85),(9,9)]
3. 代价函数的计算
3.1 碰撞代价
障碍物参数:
- 中心: ( 5 , 5 ) (5,5) (5,5)
- 半径: r = 1 r = 1 r=1
- 安全距离: ϵ = 0.2 \epsilon = 0.2 ϵ=0.2
计算每条带噪声轨迹中每个点的碰撞代价 q o q_o qo:
- 第一条噪声轨迹
θ
~
1
\tilde{\theta}_1
θ~1:
- 点 ( 2.35 , 2.15 ) (2.35, 2.15) (2.35,2.15):
d
=
(
2.35
−
5
)
2
+
(
2.15
−
5
)
2
≈
3.88
>
1
+
0.2
⇒
q
o
=
0
d = \sqrt{(2.35 - 5)^2 + (2.15 - 5)^2} \approx 3.88 > 1 + 0.2 \Rightarrow q_o = 0
d=(2.35−5)2+(2.15−5)2≈3.88>1+0.2⇒qo=0
- 点
(
4.3
,
4.6
)
(4.3, 4.6)
(4.3,4.6):
d
=
(
4.3
−
5
)
2
+
(
4.6
−
5
)
2
≈
0.806
<
1
+
0.2
⇒
q
o
=
1
+
0.2
−
0.806
=
0.394
d = \sqrt{(4.3 - 5)^2 + (4.6 - 5)^2} \approx 0.806 < 1 + 0.2 \Rightarrow q_o = 1 + 0.2 - 0.806 = 0.394
d=(4.3−5)2+(4.6−5)2≈0.806<1+0.2⇒qo=1+0.2−0.806=0.394
- 点
(
6.95
,
6.55
)
(6.95, 6.55)
(6.95,6.55):
d
=
(
6.95
−
5
)
2
+
(
6.55
−
5
)
2
≈
2.47
>
1
+
0.2
⇒
q
o
=
0
d = \sqrt{(6.95 - 5)^2 + (6.55 - 5)^2} \approx 2.47 > 1 + 0.2 \Rightarrow q_o = 0
d=(6.95−5)2+(6.55−5)2≈2.47>1+0.2⇒qo=0
- 总碰撞代价:
S o ( θ ~ 1 ) = 0 + 0.394 + 0 = 0.394 S_o(\tilde{\theta}_1) = 0 + 0.394 + 0 = 0.394 So(θ~1)=0+0.394+0=0.394
- 第二条噪声轨迹
θ
~
2
\tilde{\theta}_2
θ~2:
- 点 ( 2.2 , 2.3 ) (2.2, 2.3) (2.2,2.3):
d
=
(
2.2
−
5
)
2
+
(
2.3
−
5
)
2
≈
3.89
>
1
+
0.2
⇒
q
o
=
0
d = \sqrt{(2.2 - 5)^2 + (2.3 - 5)^2} \approx 3.89 > 1 + 0.2 \Rightarrow q_o = 0
d=(2.2−5)2+(2.3−5)2≈3.89>1+0.2⇒qo=0
- 点
(
4.65
,
4.35
)
(4.65, 4.35)
(4.65,4.35):
d
=
(
4.65
−
5
)
2
+
(
4.35
−
5
)
2
≈
0.738
<
1
+
0.2
⇒
q
o
=
1
+
0.2
−
0.738
=
0.462
d = \sqrt{(4.65 - 5)^2 + (4.35 - 5)^2} \approx 0.738 < 1 + 0.2 \Rightarrow q_o = 1 + 0.2 - 0.738 = 0.462
d=(4.65−5)2+(4.35−5)2≈0.738<1+0.2⇒qo=1+0.2−0.738=0.462
- 点
(
6.65
,
6.85
)
(6.65, 6.85)
(6.65,6.85):
d
=
(
6.65
−
5
)
2
+
(
6.85
−
5
)
2
≈
2.47
>
1
+
0.2
⇒
q
o
=
0
d = \sqrt{(6.65 - 5)^2 + (6.85 - 5)^2} \approx 2.47 > 1 + 0.2 \Rightarrow q_o = 0
d=(6.65−5)2+(6.85−5)2≈2.47>1+0.2⇒qo=0
- 总碰撞代价:
S o ( θ ~ 2 ) = 0 + 0.462 + 0 = 0.462 S_o(\tilde{\theta}_2) = 0 + 0.462 + 0 = 0.462 So(θ~2)=0+0.462+0=0.462
3.2 平滑代价
为了简化计算,我们假设平滑代价的权重 R R R 已经包含在噪声生成过程中。因此,此示例中主要关注碰撞代价。
总代价:
S ( θ ~ 1 ) = S o ( θ ~ 1 ) = 0.394 S(\tilde{\theta}_1) = S_o(\tilde{\theta}_1) = 0.394 S(θ~1)=So(θ~1)=0.394
S ( θ ~ 2 ) = S o ( θ ~ 2 ) = 0.462 S(\tilde{\theta}_2) = S_o(\tilde{\theta}_2) = 0.462 S(θ~2)=So(θ~2)=0.462
4. 权重计算
使用以下公式计算每条轨迹的权重概率 P ( θ ~ k ) P(\tilde{\theta}_k) P(θ~k):
P ( θ ~ k ) = e − 1 λ S ( θ ~ k ) ∑ l = 1 K e − 1 λ S ( θ ~ l ) P(\tilde{\theta}_k) = \frac{e^{-\frac{1}{\lambda} S(\tilde{\theta}_k)}}{\sum_{l=1}^{K} e^{-\frac{1}{\lambda} S(\tilde{\theta}_l)}} P(θ~k)=∑l=1Ke−λ1S(θ~l)e−λ1S(θ~k)
假设调节参数 λ = 1 \lambda = 1 λ=1:
P ( θ ~ 1 ) = e − 0.394 e − 0.394 + e − 0.462 ≈ 0.673 0.673 + 0.629 ≈ 0.529 P(\tilde{\theta}_1) = \frac{e^{-0.394}}{e^{-0.394} + e^{-0.462}} \approx \frac{0.673}{0.673 + 0.629} \approx 0.529 P(θ~1)=e−0.394+e−0.462e−0.394≈0.673+0.6290.673≈0.529
P ( θ ~ 2 ) = e − 0.462 e − 0.394 + e − 0.462 ≈ 0.629 0.673 + 0.629 ≈ 0.471 P(\tilde{\theta}_2) = \frac{e^{-0.462}}{e^{-0.394} + e^{-0.462}} \approx \frac{0.629}{0.673 + 0.629} \approx 0.471 P(θ~2)=e−0.394+e−0.462e−0.462≈0.673+0.6290.629≈0.471
5. 轨迹更新
为了进行轨迹更新,我们需要计算梯度和 M M M 矩阵。
5.1 梯度计算(这不是特别明白,大概意思应该是一般的收敛算法是用差分梯度的方法来迭代拟合,标准公式应该满足原文式子8,但是这里概率化了从而转成公式10来代替,推导过程还是不明白)
梯度是通过带权重的噪声轨迹的累积和计算得到的。在二维空间中,我们分别对 x x x 和 y y y 方向计算梯度。
对于每个轨迹的每个时间步,噪声变化为:
ϵ k , i = ( ϵ k , x , i , ϵ k , y , i ) \epsilon_{k,i} = (\epsilon_{k,x,i}, \epsilon_{k,y,i}) ϵk,i=(ϵk,x,i,ϵk,y,i)
具体数值如下:
- 第一条噪声轨迹 θ ~ 1 \tilde{\theta}_1 θ~1:
ϵ 1 = [ ( 0 , 0 ) , ( 0.1 , − 0.1 ) , ( − 0.2 , 0.1 ) , ( 0.2 , − 0.2 ) , ( 0 , 0 ) ] \epsilon_{1} = [(0,0), (0.1,-0.1), (-0.2,0.1), (0.2,-0.2), (0,0)] ϵ1=[(0,0),(0.1,−0.1),(−0.2,0.1),(0.2,−0.2),(0,0)]
- 第二条噪声轨迹 θ ~ 2 \tilde{\theta}_2 θ~2:
ϵ 2 = [ ( 0 , 0 ) , ( − 0.05 , 0.05 ) , ( 0.15 , − 0.15 ) , ( − 0.1 , 0.1 ) , ( 0 , 0 ) ] \epsilon_{2} = [(0,0), (-0.05,0.05), (0.15,-0.15), (-0.1,0.1), (0,0)] ϵ2=[(0,0),(−0.05,0.05),(0.15,−0.15),(−0.1,0.1),(0,0)]
计算加权噪声累积和(梯度):
Δ θ i = ∑ k = 1 K P ( θ ~ k ) ⋅ ϵ k , i \Delta \theta_i = \sum_{k=1}^{K} P(\tilde{\theta}_k) \cdot \epsilon_{k,i} Δθi=k=1∑KP(θ~k)⋅ϵk,i
具体计算:
- 时间步 1(点 ( 2.25 , 2.25 ) (2.25, 2.25) (2.25,2.25)):
Δ x 1 = 0.529 × 0.1 + 0.471 × ( − 0.05 ) = 0.0529 − 0.02355 = 0.02935 \Delta x_1 = 0.529 \times 0.1 + 0.471 \times (-0.05) = 0.0529 - 0.02355 = 0.02935 Δx1=0.529×0.1+0.471×(−0.05)=0.0529−0.02355=0.02935
Δ y 1 = 0.529 × ( − 0.1 ) + 0.471 × 0.05 = − 0.0529 + 0.02355 = − 0.02935 \Delta y_1 = 0.529 \times (-0.1) + 0.471 \times 0.05 = -0.0529 + 0.02355 = -0.02935 Δy1=0.529×(−0.1)+0.471×0.05=−0.0529+0.02355=−0.02935
- 时间步 2(点 ( 4.5 , 4.5 ) (4.5, 4.5) (4.5,4.5)):
Δ x 2 = 0.529 × ( − 0.2 ) + 0.471 × 0.15 = − 0.1058 + 0.07065 = − 0.03515 \Delta x_2 = 0.529 \times (-0.2) + 0.471 \times 0.15 = -0.1058 + 0.07065 = -0.03515 Δx2=0.529×(−0.2)+0.471×0.15=−0.1058+0.07065=−0.03515
Δ y 2 = 0.529 × 0.1 + 0.471 × ( − 0.15 ) = 0.0529 − 0.07065 = − 0.01775 \Delta y_2 = 0.529 \times 0.1 + 0.471 \times (-0.15) = 0.0529 - 0.07065 = -0.01775 Δy2=0.529×0.1+0.471×(−0.15)=0.0529−0.07065=−0.01775
- 时间步 3(点 ( 6.75 , 6.75 ) (6.75, 6.75) (6.75,6.75)):
Δ x 3 = 0.529 × 0.2 + 0.471 × ( − 0.1 ) = 0.1058 − 0.0471 = 0.0587 \Delta x_3 = 0.529 \times 0.2 + 0.471 \times (-0.1) = 0.1058 - 0.0471 = 0.0587 Δx3=0.529×0.2+0.471×(−0.1)=0.1058−0.0471=0.0587
Δ y 3 = 0.529 × ( − 0.2 ) + 0.471 × 0.1 = − 0.1058 + 0.0471 = − 0.0587 \Delta y_3 = 0.529 \times (-0.2) + 0.471 \times 0.1 = -0.1058 + 0.0471 = -0.0587 Δy3=0.529×(−0.2)+0.471×0.1=−0.1058+0.0471=−0.0587
- 时间步 0 和 4(起点和终点):
Δ x 0 = Δ x 4 = 0 , Δ y 0 = Δ y 4 = 0 \Delta x_0 = \Delta x_4 = 0, \quad \Delta y_0 = \Delta y_4 = 0 Δx0=Δx4=0,Δy0=Δy4=0
因此,梯度(加权噪声累积和)为:
Δ θ = [ ( 0 , 0 ) , ( 0.02935 , − 0.02935 ) , ( − 0.03515 , − 0.01775 ) , ( 0.0587 , − 0.0587 ) , ( 0 , 0 ) ] \Delta \theta = [(0,0), (0.02935, -0.02935), (-0.03515, -0.01775), (0.0587, -0.0587), (0,0)] Δθ=[(0,0),(0.02935,−0.02935),(−0.03515,−0.01775),(0.0587,−0.0587),(0,0)]
5.2 矩阵 M M M 的构建
矩阵 M M M 用于平滑更新轨迹。根据论文, M M M 是通过对 R − 1 R^{-1} R−1 进行缩放得到的,其中 R = A T A R = A^T A R=ATA。
有限差分矩阵 A A A:
对于 N = 5 N = 5 N=5 个点,计算加速度(即二阶差分), A A A 为 3 × 5 3 \times 5 3×5 的矩阵:
A = [ 1 − 2 1 0 0 0 1 − 2 1 0 0 0 1 − 2 1 ] A = \begin{bmatrix} 1 & -2 & 1 & 0 & 0 \\ 0 & 1 & -2 & 1 & 0 \\ 0 & 0 & 1 & -2 & 1 \\ \end{bmatrix} A= 100−2101−2101−2001
计算 R = A T A R = A^T A R=ATA:
A T = [ 1 0 0 − 2 1 0 1 − 2 1 0 1 − 2 0 0 1 ] A^T = \begin{bmatrix} 1 & 0 & 0 \\ -2 & 1 & 0 \\ 1 & -2 & 1 \\ 0 & 1 & -2 \\ 0 & 0 & 1 \\ \end{bmatrix} AT= 1−210001−210001−21
R = A T A = [ 1 − 2 1 0 0 − 2 5 − 4 1 0 1 − 4 6 − 4 1 0 1 − 4 5 − 2 0 0 1 − 2 1 ] R = A^T A = \begin{bmatrix} 1 & -2 & 1 & 0 & 0 \\ -2 & 5 & -4 & 1 & 0 \\ 1 & -4 & 6 & -4 & 1 \\ 0 & 1 & -4 & 5 & -2 \\ 0 & 0 & 1 & -2 & 1 \\ \end{bmatrix} R=ATA= 1−2100−25−4101−46−4101−45−2001−21
计算 R − 1 R^{-1} R−1:
由于 R R R 是一个 5 × 5 5 \times 5 5×5 的矩阵,直接计算逆矩阵较为复杂。在实际应用中,可以使用数值计算工具(如 NumPy)来计算。为了简化示例,我们假设 R − 1 R^{-1} R−1 已知或使用单位矩阵近似(仅用于说明)。
缩放 R − 1 R^{-1} R−1 以得到 M M M:
根据论文,矩阵 M M M 是通过将 R − 1 R^{-1} R−1 的每一列缩放,使每列的最大元素为 1 N \frac{1}{N} N1。在本示例中,假设 M M M 已简化为单位矩阵 I I I,即:
M = I = [ 1 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 0 1 ] M = I = \begin{bmatrix} 1 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 \\ 0 & 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \\ \end{bmatrix} M=I= 1000001000001000001000001
注意:在实际应用中, M M M 应包含平滑性信息,具体取决于 R − 1 R^{-1} R−1 的计算结果。
5.3 更新轨迹
使用梯度和 M M M 矩阵更新轨迹:
θ ← θ + M ⋅ Δ θ \theta \leftarrow \theta + M \cdot \Delta \theta θ←θ+M⋅Δθ
具体计算:
- 时间步 0:
θ 0 = ( 0 , 0 ) + ( 0 , 0 ) = ( 0 , 0 ) \theta_0 = (0,0) + (0,0) = (0,0) θ0=(0,0)+(0,0)=(0,0)
- 时间步 1:
θ 1 = ( 2.25 , 2.25 ) + ( 0.02935 , − 0.02935 ) = ( 2.27935 , 2.22065 ) \theta_1 = (2.25, 2.25) + (0.02935, -0.02935) = (2.27935, 2.22065) θ1=(2.25,2.25)+(0.02935,−0.02935)=(2.27935,2.22065)
- 时间步 2:
θ 2 = ( 4.5 , 4.5 ) + ( − 0.03515 , − 0.01775 ) = ( 4.46485 , 4.48225 ) \theta_2 = (4.5, 4.5) + (-0.03515, -0.01775) = (4.46485, 4.48225) θ2=(4.5,4.5)+(−0.03515,−0.01775)=(4.46485,4.48225)
- 时间步 3:
θ 3 = ( 6.75 , 6.75 ) + ( 0.0587 , − 0.0587 ) = ( 6.8087 , 6.6913 ) \theta_3 = (6.75, 6.75) + (0.0587, -0.0587) = (6.8087, 6.6913) θ3=(6.75,6.75)+(0.0587,−0.0587)=(6.8087,6.6913)
- 时间步 4:
θ 4 = ( 9 , 9 ) + ( 0 , 0 ) = ( 9 , 9 ) \theta_4 = (9,9) + (0,0) = (9,9) θ4=(9,9)+(0,0)=(9,9)
更新后的轨迹:
θ new = [ ( 0 , 0 ) , ( 2.27935 , 2.22065 ) , ( 4.46485 , 4.48225 ) , ( 6.8087 , 6.6913 ) , ( 9 , 9 ) ] \theta_{\text{new}} = [(0,0), (2.27935, 2.22065), (4.46485, 4.48225), (6.8087, 6.6913), (9,9)] θnew=[(0,0),(2.27935,2.22065),(4.46485,4.48225),(6.8087,6.6913),(9,9)]
结论
通过上述具体数值示例,我们详细展示了 STOMP 算法在二维空间中如何计算梯度和使用 M M M 矩阵更新轨迹的过程。关键步骤包括:
- 生成带噪声轨迹:通过添加随机噪声探索轨迹空间。
- 计算代价函数:评估每条轨迹的碰撞和平滑代价。
- 计算权重:根据代价函数确定每条轨迹的权重。
- 计算梯度:加权累积噪声轨迹,作为轨迹更新的方向。
- 更新轨迹:通过 M M M 矩阵平滑更新轨迹,确保轨迹的平滑性和避障性。
通过多次迭代,STOMP 算法能够有效地优化轨迹,使机器人能够在避开障碍物的同时保持路径的平滑性。
三、M矩阵用于尺寸变换,当考虑加速度时,M矩阵不为单位阵,可以如下:
如果矩阵 M M M 不是单位矩阵,它会影响轨迹更新的平滑性。在STOMP算法中,矩阵 M M M 是通过对矩阵 R − 1 R^{-1} R−1 进行缩放后得到的,其中 R R R 是基于加速度的二次项的矩阵。矩阵 M M M 的主要作用是对轨迹的更新进行平滑处理,以确保轨迹更新时不会出现不连续或剧烈的跳变。
为了回答你关于 M M M 不是单位矩阵时如何完成轨迹更新的过程,我们需要明确 M M M 的来源,并展示如何计算和应用 M M M 矩阵。
1. 回顾矩阵 R R R 和 M M M 的构建
1.1 矩阵 A A A(有限差分矩阵)
在STOMP算法中,矩阵 A A A 用来计算轨迹的加速度(即二阶差分)。对于5个时间步(包括起点和终点)的轨迹,矩阵 A A A 是一个 3 × 5 3 \times 5 3×5 的有限差分矩阵,用来计算每个点的加速度:
A = [ 1 − 2 1 0 0 0 1 − 2 1 0 0 0 1 − 2 1 ] A = \begin{bmatrix} 1 & -2 & 1 & 0 & 0 \\ 0 & 1 & -2 & 1 & 0 \\ 0 & 0 & 1 & -2 & 1 \\ \end{bmatrix} A= 100−2101−2101−2001
1.2 矩阵 R R R
矩阵 R R R 是通过 A A A 的转置和 A A A 相乘得到的。 R = A T A R = A^T A R=ATA:
A T = [ 1 0 0 − 2 1 0 1 − 2 1 0 1 − 2 0 0 1 ] A^T = \begin{bmatrix} 1 & 0 & 0 \\ -2 & 1 & 0 \\ 1 & -2 & 1 \\ 0 & 1 & -2 \\ 0 & 0 & 1 \\ \end{bmatrix} AT= 1−210001−210001−21
R = A T A = [ 1 − 2 1 0 0 − 2 5 − 4 1 0 1 − 4 6 − 4 1 0 1 − 4 5 − 2 0 0 1 − 2 1 ] R = A^T A = \begin{bmatrix} 1 & -2 & 1 & 0 & 0 \\ -2 & 5 & -4 & 1 & 0 \\ 1 & -4 & 6 & -4 & 1 \\ 0 & 1 & -4 & 5 & -2 \\ 0 & 0 & 1 & -2 & 1 \\ \end{bmatrix} R=ATA= 1−2100−25−4101−46−4101−45−2001−21
1.3 矩阵 R − 1 R^{-1} R−1
矩阵 R R R 是正定矩阵,因此可以计算其逆矩阵 R − 1 R^{-1} R−1。为了简单起见,我们可以使用数值计算工具(如NumPy)来计算 R − 1 R^{-1} R−1 的数值解。实际应用中,数值解会用于轨迹更新时的平滑处理。
1.4 矩阵 M M M
矩阵 M M M 是 R − 1 R^{-1} R−1 经过缩放后的矩阵。缩放的规则是将每一列的最大元素缩放到 1 N \frac{1}{N} N1,其中 N N N 是轨迹中的离散时间步数。在我们的例子中, N = 5 N = 5 N=5。
矩阵 M M M 的构建公式为:
M = 1 N × R − 1 M = \frac{1}{N} \times R^{-1} M=N1×R−1
具体的缩放系数根据每一列的最大元素确定,以确保轨迹的平滑性和更新幅度不过大。
2. 计算矩阵 M M M 的具体步骤
假设我们已经得到了 R − 1 R^{-1} R−1 矩阵的数值解。接下来,我们需要对其进行缩放处理,确保每一列的最大元素等于 1 N \frac{1}{N} N1。
例如,如果 R − 1 R^{-1} R−1 的数值解为(假设):
R − 1 = [ a 11 a 12 a 13 a 14 a 15 a 21 a 22 a 23 a 24 a 25 a 31 a 32 a 33 a 34 a 35 a 41 a 42 a 43 a 44 a 45 a 51 a 52 a 53 a 54 a 55 ] R^{-1} = \begin{bmatrix} a_{11} & a_{12} & a_{13} & a_{14} & a_{15} \\ a_{21} & a_{22} & a_{23} & a_{24} & a_{25} \\ a_{31} & a_{32} & a_{33} & a_{34} & a_{35} \\ a_{41} & a_{42} & a_{43} & a_{44} & a_{45} \\ a_{51} & a_{52} & a_{53} & a_{54} & a_{55} \\ \end{bmatrix} R−1= a11a21a31a41a51a12a22a32a42a52a13a23a33a43a53a14a24a34a44a54a15a25a35a45a55
我们对每一列进行缩放,确保最大值为 1 N \frac{1}{N} N1:
M i j = a i j max ( a 1 j , a 2 j , a 3 j , a 4 j , a 5 j ) × 1 N M_{ij} = \frac{a_{ij}}{\max(a_{1j}, a_{2j}, a_{3j}, a_{4j}, a_{5j})} \times \frac{1}{N} Mij=max(a1j,a2j,a3j,a4j,a5j)aij×N1
这个缩放矩阵 M M M 将用于平滑轨迹更新。
3. 使用 M M M 矩阵进行轨迹更新
在计算好 M M M 矩阵后,我们使用加权噪声累积和(梯度)来更新轨迹。具体的更新公式为:
θ new = θ old + M ⋅ Δ θ \theta_{\text{new}} = \theta_{\text{old}} + M \cdot \Delta \theta θnew=θold+M⋅Δθ
其中:
- θ old \theta_{\text{old}} θold 是前一轮迭代的轨迹
- Δ θ \Delta \theta Δθ 是根据每条噪声轨迹的加权累积得到的梯度
- M M M 是经过缩放的 R − 1 R^{-1} R−1 矩阵,确保轨迹更新的平滑性
我们通过矩阵 M M M 的平滑效果,可以避免轨迹更新过程中出现剧烈的跳变,确保轨迹的平滑性,同时根据噪声探索的结果优化轨迹。
4. 实际数值示例
假设经过数值计算,矩阵 R − 1 R^{-1} R−1 为:
R − 1 = [ 0.2 − 0.1 0.1 0.0 0.0 − 0.1 0.3 − 0.2 0.1 0.0 0.1 − 0.2 0.4 − 0.2 0.1 0.0 0.1 − 0.2 0.3 − 0.1 0.0 0.0 0.1 − 0.1 0.2 ] R^{-1} = \begin{bmatrix} 0.2 & -0.1 & 0.1 & 0.0 & 0.0 \\ -0.1 & 0.3 & -0.2 & 0.1 & 0.0 \\ 0.1 & -0.2 & 0.4 & -0.2 & 0.1 \\ 0.0 & 0.1 & -0.2 & 0.3 & -0.1 \\ 0.0 & 0.0 & 0.1 & -0.1 & 0.2 \\ \end{bmatrix} R−1= 0.2−0.10.10.00.0−0.10.3−0.20.10.00.1−0.20.4−0.20.10.00.1−0.20.3−0.10.00.00.1−0.10.2
我们将每一列的最大元素缩放到 1 5 = 0.2 \frac{1}{5} = 0.2 51=0.2,以得到矩阵 M M M:
M = [ 0.2 − 0.0667 0.05 0.0 0.0 − 0.1 0.2 − 0.1 0.0667 0.0 0.1 − 0.1333 0.2 − 0.1333 0.05 0.0 0.0667 − 0.1 0.2 − 0.0667 0.0 0.0 0.05 − 0.0667 0.2 ] M = \begin{bmatrix} 0.2 & -0.0667 & 0.05 & 0.0 & 0.0 \\ -0.1 & 0.2 & -0.1 & 0.0667 & 0.0 \\ 0.1 & -0.1333 & 0.2 & -0.1333 & 0.05 \\ 0.0 & 0.0667 & -0.1 & 0.2 & -0.0667 \\ 0.0 & 0.0 & 0.05 & -0.0667 & 0.2 \\ \end{bmatrix} M= 0.2−0.10.10.00.0−0.06670.2−0.13330.06670.00.05−0.10.2−0.10.050.00.0667−0.13330.2−0.06670.00.00.05−0.06670.2
接下来,假设我们得到的梯度 Δ θ \Delta \theta Δθ 为:
Δ θ = [ ( 0 , 0 ) , ( 0.02935 , − 0.02935 ) , ( − 0.03515 , − 0.01775 ) , ( 0.0587 , − 0.0587 ) , ( 0 , 0 ) ] \Delta \theta = [(0, 0), (0.02935, -0.02935), (-0.03515, -0.01775), (0.0587, -0.0587), (0, 0)] Δθ=[(0,0),(0.02935,−0.02935),(−0.03515,−0.01775),(0.0587,−0.0587),(0,0)]
我们将矩阵 M M M 应用于 Δ θ \Delta \theta Δθ 上进行更新:
对于轨迹的 x x x 和 y y y 分量,我们分别计算:
θ new , x = θ old , x + M ⋅ Δ θ x \theta_{\text{new}, x} = \theta_{\text{old}, x} + M \cdot \Delta \theta_x θnew,x=θold,x+M⋅Δθx
θ new , y = θ old , y + M ⋅ Δ θ y \theta_{\text{new}, y} = \theta_{\text{old}, y} + M \cdot \Delta \theta_y θnew,y=θold,y+M⋅Δθy
矩阵计算代码demo:
import numpy as np
# M matrix
M = np.array([
[0.2, -0.0667, 0.05, 0.0, 0.0],
[-0.1, 0.2, -0.1, 0.0667, 0.0],
[0.1, -0.1333, 0.2, -0.1333, 0.05],
[0.0, 0.0667, -0.1, 0.2, -0.0667],
[0.0, 0.0, 0.05, -0.0667, 0.2]
])
# Delta theta (gradient) for x and y
delta_theta_x = np.array([0, 0.02935, -0.03515, 0.0587, 0])
delta_theta_y = np.array([0, -0.02935, -0.01775, -0.0587, 0])
# Old trajectory for x and y
theta_old_x = np.array([0, 2.25, 4.5, 6.75, 9])
theta_old_y = np.array([0, 2.25, 4.5, 6.75, 9])
# New theta after applying M * delta_theta
theta_new_x = theta_old_x + M @ delta_theta_x
theta_new_y = theta_old_y + M @ delta_theta_y
theta_new_x, theta_new_y
6. 迭代和收敛
重复以下步骤,直到轨迹代价收敛:
- 生成新的带噪声轨迹:基于更新后的轨迹 θ new \theta_{\text{new}} θnew,再次生成带噪声的轨迹。
- 计算代价函数:包括碰撞代价和平滑代价。
- 计算权重:根据代价函数计算每条轨迹的权重概率。
- 计算梯度:加权累积噪声轨迹。
- 更新轨迹:使用梯度和 M M M 矩阵更新轨迹。
在每次迭代中,轨迹会逐步优化,代价函数 S ( θ ) S(\theta) S(θ) 会逐步减少,轨迹将趋向于避开障碍物并保持平滑。
上述例子代码demo(障碍物缩小到0.2):
import numpy as np
import matplotlib.pyplot as plt
# 定义障碍物和碰撞代价函数
def obstacle_cost(x, y, x_obstacle=5, y_obstacle=5, radius=0.2, epsilon=0.2):
d = np.sqrt((x - x_obstacle) ** 2 + (y - y_obstacle) ** 2)
return np.maximum(0, radius + epsilon - d) ** 2 # 碰撞代价平方以加大惩罚
# 初始轨迹
theta_old_x = np.array([0, 2.25, 4.5, 6.75, 9])
theta_old_y = np.array([0, 2.25, 4.5, 6.75, 9])
# 平滑矩阵 M
M = np.array([
[0.2, -0.0667, 0.05, 0.0, 0.0],
[-0.1, 0.2, -0.1, 0.0667, 0.0],
[0.1, -0.1333, 0.2, -0.1333, 0.05],
[0.0, 0.0667, -0.1, 0.2, -0.0667],
[0.0, 0.0, 0.05, -0.0667, 0.2]
])
# 迭代次数
num_iterations = 1000
K = 10 # 每次生成 10 条噪声轨迹
# 噪声轨迹生成函数
def generate_noise_trajectories(theta_old_x, theta_old_y, K):
epsilon_x = np.random.uniform(-0.3, 0.3, (K, len(theta_old_x))) # 更大的噪声幅度
epsilon_y = np.random.uniform(-0.3, 0.3, (K, len(theta_old_y)))
theta_noise_x = np.array([theta_old_x + epsilon_x[k] for k in range(K)])
theta_noise_y = np.array([theta_old_y + epsilon_y[k] for k in range(K)])
return theta_noise_x, theta_noise_y, epsilon_x, epsilon_y
# STOMP轨迹优化的主要循环
for iteration in range(num_iterations):
print(f"迭代 {iteration + 1}/{num_iterations}")
# 生成噪声轨迹
theta_noise_x, theta_noise_y, epsilon_x, epsilon_y = generate_noise_trajectories(theta_old_x, theta_old_y, K)
# 计算每条噪声轨迹的碰撞代价
S_noise = np.array([np.sum(obstacle_cost(theta_noise_x[k], theta_noise_y[k])) for k in range(K)])
# 计算权重
lambda_param = 1
exp_neg_costs = np.exp(-S_noise / lambda_param)
P = exp_neg_costs / np.sum(exp_neg_costs) # 权重归一化
# 计算加权的噪声梯度
delta_theta_x = np.sum(P[:, np.newaxis] * epsilon_x, axis=0)
delta_theta_y = np.sum(P[:, np.newaxis] * epsilon_y, axis=0)
# 使用 M 矩阵更新轨迹
theta_new_x = theta_old_x + M @ delta_theta_x
theta_new_y = theta_old_y + M @ delta_theta_y
# 更新旧轨迹以进行下一次迭代
theta_old_x = theta_new_x
theta_old_y = theta_new_y
# 打印最终更新后的轨迹
print("最终更新后的 x 方向轨迹:", theta_new_x)
print("最终更新后的 y 方向轨迹:", theta_new_y)
# 8. 可视化轨迹和障碍物
fig, ax = plt.subplots(figsize=(8, 8))
plt.rcParams['font.sans-serif']=['SimHei'] #用来正常显示中文标签
plt.rcParams['axes.unicode_minus'] = False #用来正常显示负号
# 绘制初始和更新后的轨迹
ax.plot([0, 2.25, 4.5, 6.75, 9], [0, 2.25, 4.5, 6.75, 9], 'bo-', label='初始轨迹', markersize=8)
ax.plot(theta_new_x, theta_new_y, 'ro-', label='最终更新的轨迹', markersize=8)
# 障碍物圆圈
obstacle = plt.Circle((5, 5), 0.2, color='k', fill=False, label='障碍物 (半径=0.2)')
ax.add_artist(obstacle)
# 设置坐标轴和显示范围
ax.set_xlim(0, 10)
ax.set_ylim(0, 10)
ax.grid(True)
ax.legend()
ax.set_title("STOMP 轨迹优化示例(多次迭代 + 强避障)")
ax.set_xlabel("X坐标")
ax.set_ylabel("Y坐标")
# 显示图像
plt.show()
四、如何拓展到6轴机器人:
‘The algorithm in Table I was derived for a one-dimensional discretized trajectory. Scaling this to multiple dimensions simply involves performing the sampling and update steps for each dimension independently in each iteration.’原话是独立空间进行拓展,大概就是每个维度自己更新自己的,然后由正解回到笛卡尔空间(像碰撞约束还是在这个空间中算)。
(我们讨论下公式13)
在论文中,公式 (13) 提供了一种计算障碍成本的方法。这个公式是用来量化机器人在任意时间步
t
t
t 与障碍物的距离,并将其转化为一个成本值,这个成本值在优化过程中会被最小化。公式如下:
q o ( θ t ) = ∑ b ∈ B max ( ϵ + r b − d ( x b ) , 0 ) ∥ x ˙ b ∥ q_o(\theta_t) = \sum_{b \in B} \max(\epsilon + r_b - d(x_b), 0) \| \dot{x}_b \| qo(θt)=b∈B∑max(ϵ+rb−d(xb),0)∥x˙b∥
这里:
- q o ( θ t ) q_o(\theta_t) qo(θt) 是在时间步 t t t 的障碍物成本。
- B B B 是机器人中所有可能与障碍物发生碰撞的部件的集合。
- ϵ \epsilon ϵ 是安全距离,通常是一个正值。
- r b r_b rb 是部件 b b b 的半径。
- d ( x b ) d(x_b) d(xb) 是从部件 b b b 的中心到最近障碍物的距离。
- x ˙ b \dot{x}_b x˙b 是部件 b b b 的速度。
计算步骤
- 计算每个部件中心到障碍物的距离 d ( x b ) d(x_b) d(xb):这通常通过计算部件中心位置 x b x_b xb 到最近障碍物的欧氏距离来完成。
- 评估碰撞条件:对于每个部件,检查是否满足 ϵ + r b > d ( x b ) \epsilon + r_b > d(x_b) ϵ+rb>d(xb),如果满足,意味着部件与障碍物的距离小于安全距离加上部件半径,存在潜在碰撞。
- 计算障碍物成本:如果存在潜在碰撞,计算成本为 max ( ϵ + r b − d ( x b ) , 0 ) ∥ x ˙ b ∥ \max(\epsilon + r_b - d(x_b), 0) \| \dot{x}_b \| max(ϵ+rb−d(xb),0)∥x˙b∥,否则成本为0。
- 对所有部件的成本求和:将所有部件的障碍物成本加起来,得到总的障碍物成本 q o ( θ t ) q_o(\theta_t) qo(θt)。
举例说明
假设一个6轴机器人的某个部件 b b b 在时间步 t t t 的位置是 ( x b , y b , z b ) (x_b, y_b, z_b) (xb,yb,zb),障碍物的位置是 ( x o b , y o b , z o b ) (x_{ob}, y_{ob}, z_{ob}) (xob,yob,zob),部件 b b b 的半径 r b r_b rb 是 0.1 米,安全距离 ϵ \epsilon ϵ 设为 0.05 米。
- 计算距离 d ( x b ) d(x_b) d(xb):
d
(
x
b
)
=
(
x
b
−
x
o
b
)
2
+
(
y
b
−
y
o
b
)
2
+
(
z
b
−
z
o
b
)
2
d(x_b) = \sqrt{(x_b - x_{ob})^2 + (y_b - y_{ob})^2 + (z_b - z_{ob})^2}
d(xb)=(xb−xob)2+(yb−yob)2+(zb−zob)2
2. 评估碰撞条件:
- 假设
d
(
x
b
)
=
0.12
d(x_b) = 0.12
d(xb)=0.12 米,
r
b
=
0.1
r_b = 0.1
rb=0.1 米,
ϵ
=
0.05
\epsilon = 0.05
ϵ=0.05 米。
- 则
ϵ
+
r
b
=
0.15
\epsilon + r_b = 0.15
ϵ+rb=0.15 米,因为
0.15
>
0.12
0.15 > 0.12
0.15>0.12,所以存在潜在碰撞。
3. 计算障碍物成本:
- 成本为
max
(
0.15
−
0.12
,
0
)
⋅
∥
x
˙
b
∥
=
0.03
⋅
∥
x
˙
b
∥
\max(0.15 - 0.12, 0) \cdot \| \dot{x}_b \| = 0.03 \cdot \| \dot{x}_b \|
max(0.15−0.12,0)⋅∥x˙b∥=0.03⋅∥x˙b∥。
- 假设部件的速度
x
˙
b
\dot{x}_b
x˙b 为
(
0.1
m/s
,
0
,
0
)
(0.1 \, \text{m/s}, 0, 0)
(0.1m/s,0,0),则
∥
x
˙
b
∥
=
0.1
\| \dot{x}_b \| = 0.1
∥x˙b∥=0.1。
- 总成本为
0.03
⋅
0.1
=
0.003
0.03 \cdot 0.1 = 0.003
0.03⋅0.1=0.003。
4. 对所有部件的成本求和:
- 如果机器人有多个部件,我们需要对每个部件重复上述步骤,然后求和得到
q
o
(
θ
t
)
q_o(\theta_t)
qo(θt)。
通过这种方式,公式 (13) 能够为每个时间步提供关于障碍物成本的量化,这将在STOMP优化过程中被用来引导轨迹的修正,以避免碰撞。
乘以部件的速度是为什么?既然是检查是否碰撞,只要检查当前位置是否碰撞不就可以吗?
一个不成熟的多轴demo:
import numpy as np
import matplotlib.pyplot as plt
# 初始化6轴机器人的起始和目标姿态
theta_start = np.array([0, 0, 0, 0, 0, 0])
theta_goal = np.array([np.pi/2, np.pi/2, np.pi/4, np.pi/4, np.pi/6, np.pi/6])
# 离散化轨迹为10个时间步
N = 10
time_steps = np.linspace(0, 1, N)
theta_old = np.array([theta_start + t * (theta_goal - theta_start) for t in time_steps])
# 平滑矩阵 M,应用于每个关节
M = np.eye(N) # 简化为单位矩阵,实际应用中需要根据轨迹优化计算
# 迭代次数
num_iterations = 20
K = 5 # 每次生成 5 条噪声轨迹
# 噪声生成函数,6个关节
def generate_noise_trajectories(theta_old, K):
epsilon = np.random.uniform(-0.1, 0.1, (K, N, 6)) # 生成噪声
theta_noise = np.array([theta_old + epsilon[k] for k in range(K)]) # 为每个关节生成噪声轨迹
return theta_noise, epsilon
# 假设障碍物位于机器人工作空间的某个位置,我们定义一个简单的碰撞代价函数
def obstacle_cost(theta):
# 假设末端执行器的运动是关节1和关节2的简单函数
# 计算末端执行器位置,并返回与障碍物的距离
end_effector_x = np.cos(theta[:, 0]) + np.cos(theta[:, 1])
end_effector_y = np.sin(theta[:, 0]) + np.sin(theta[:, 1])
# 假设障碍物位于 (1, 1)
obstacle_x, obstacle_y = 1, 1
distance = np.sqrt((end_effector_x - obstacle_x)**2 + (end_effector_y - obstacle_y)**2)
return np.maximum(0, 1.0 - distance) ** 2 # 距离小于1时产生代价
# STOMP优化过程
for iteration in range(num_iterations):
print(f"迭代 {iteration + 1}/{num_iterations}")
# 生成噪声轨迹
theta_noise, epsilon = generate_noise_trajectories(theta_old, K)
# 计算每条噪声轨迹的碰撞代价
S_noise = np.array([np.sum(obstacle_cost(theta_noise[k])) for k in range(K)])
# 计算权重
lambda_param = 1
exp_neg_costs = np.exp(-S_noise / lambda_param)
P = exp_neg_costs / np.sum(exp_neg_costs) # 权重归一化
# 计算加权的噪声梯度
delta_theta = np.sum(P[:, np.newaxis, np.newaxis] * epsilon, axis=0)
# 使用 M 矩阵更新轨迹(每个关节轨迹分别处理)
theta_new = theta_old + M @ delta_theta
# 更新旧轨迹以进行下一次迭代
theta_old = theta_new
# 打印最终更新后的轨迹
print("最终更新后的轨迹:", theta_new)
# 绘制最终关节1和关节2的轨迹变化
plt.plot(time_steps, theta_new[:, 0], label='关节1')
plt.plot(time_steps, theta_new[:, 1], label='关节2')
plt.xlabel('时间')
plt.ylabel('关节角度')
plt.legend()
plt.grid(True)
plt.title('6轴机器人关节轨迹优化')
plt.show()