STOMP: Stochastic Trajectory Optimization for Motion Planning 文章解读

一、个人总结

STOMP是一种基于随机轨迹优化的框架，它通过生成带有噪声的轨迹来探索初始轨迹（可能是不可行的）周围的空间，然后将这些轨迹结合起来，以产生具有较低成本的更新轨迹。这种方法不需要梯度信息（梯度信息概率化），因此可以优化那些可能没有导数的一般成本函数，例如与约束和电机扭矩相对应的成本。（需要提供原始轨迹，有点类似给定了RRT的树干，然后在树干周围进随机采样，并优化全局路径。其中原始路径的质量很重要，且密度越大，优化效果越好）

算法计算流程（对应原文的Table1）：
初始化：
首先，确定机器人的起始位置和目标位置，以及一个初始的轨迹（可能是直线或随机生成的）。这个初始轨迹可能不满足所有的约束条件，比如避开障碍物。

成本函数定义：
定义一个成本函数，它结合了障碍物成本、平滑性成本等。例如，障碍物成本可以通过与障碍物的距离来衡量，平滑性成本可以通过轨迹的加速度变化来衡量。

生成噪声轨迹：
在每一步迭代中，生成一系列带有随机噪声的轨迹。这些噪声是通过在初始轨迹上添加高斯噪声来实现的，噪声的协方差矩阵通常与控制成本相关。

计算成本：
对每个生成的噪声轨迹，计算其在整个运动过程中的成本。这包括与障碍物的交互、轨迹的平滑性以及可能的扭矩成本。

更新轨迹：
使用所有噪声轨迹的成本信息来更新候选解。STOMP算法通过计算一个梯度估计来更新轨迹，这个梯度估计是基于概率加权的噪声参数的凸组合。

迭代优化：
重复步骤3到5，直到满足收敛条件，比如成本不再显著下降或达到最大迭代次数。

输出最优轨迹：
最终，算法输出一个优化后的轨迹，这个轨迹在满足所有约束条件的同时，具有最低的成本。

二、用一个二维的路径优化例子说明文章计算流程

• 起点：$(0,0)$
• 终点：$(9,9)$
• 障碍物：位于 $(5,5)$，半径为 $1$
• 轨迹离散化点数：$5$ 个点，包括起点和终点
• 轨迹初始化：直线轨迹
• 带噪声轨迹数量：$K=2$

1. 轨迹初始化

初始轨迹为直线插值：

$$\theta =[(0,0),(2.25,2.25),(4.5,4.5),(6.75,6.75),(9,9)]$$

2. 噪声轨迹的生成

生成 $K=2$ 条带噪声的轨迹，噪声值如下：

• 第一条噪声轨迹 ${\tilde{\theta }}_1$：

$${ϵ}_1=[(0,0),(0.1,-0.1),(-0.2,0.1),(0.2,-0.2),(0,0)]$$

因此：

$${\tilde{\theta }}_1=[(0,0),(2.35,2.15),(4.3,4.6),(6.95,6.55),(9,9)]$$

• 第二条噪声轨迹 ${\tilde{\theta }}_2$：

$${ϵ}_2=[(0,0),(-0.05,0.05),(0.15,-0.15),(-0.1,0.1),(0,0)]$$

因此：

$${\tilde{\theta }}_2=[(0,0),(2.2,2.3),(4.65,4.35),(6.65,6.85),(9,9)]$$

3. 代价函数的计算

3.1 碰撞代价

障碍物参数：

• 中心：$(5,5)$
• 半径：$r=1$
• 安全距离：$ϵ=0.2$

计算每条带噪声轨迹中每个点的碰撞代价 $q_o$：

• 第一条噪声轨迹 ${\tilde{\theta }}_1$：
  • 点 $(2.35,2.15)$：

$$d=\sqrt{(2.35-5{)}^2+(2.15-5{)}^2}\approx 3.88>1+0.2\Rightarrow q_o=0$$
- 点 $(4.3,4.6)$：

$$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$$
- 点 $(6.95,6.55)$：

$$d=\sqrt{(6.95-5{)}^2+(6.55-5{)}^2}\approx 2.47>1+0.2\Rightarrow q_o=0$$
- 总碰撞代价：

$$S_o({\tilde{\theta }}_1)=0+0.394+0=0.394$$

• 第二条噪声轨迹 ${\tilde{\theta }}_2$：
  • 点 $(2.2,2.3)$：

$$d=\sqrt{(2.2-5{)}^2+(2.3-5{)}^2}\approx 3.89>1+0.2\Rightarrow q_o=0$$
- 点 $(4.65,4.35)$：

$$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$$
- 点 $(6.65,6.85)$：

$$d=\sqrt{(6.65-5{)}^2+(6.85-5{)}^2}\approx 2.47>1+0.2\Rightarrow q_o=0$$
- 总碰撞代价：

$$S_o({\tilde{\theta }}_2)=0+0.462+0=0.462$$

3.2 平滑代价

为了简化计算，我们假设平滑代价的权重 $R$ 已经包含在噪声生成过程中。因此，此示例中主要关注碰撞代价。

总代价：

$$S({\tilde{\theta }}_1)=S_o({\tilde{\theta }}_1)=0.394$$

$$S({\tilde{\theta }}_2)=S_o({\tilde{\theta }}_2)=0.462$$

4. 权重计算

使用以下公式计算每条轨迹的权重概率 $P({\tilde{\theta }}_k)$：

$$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)}}$$

假设调节参数 $\lambda =1$：

$$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({\tilde{\theta }}_2)=\frac{e^{-0.462}}{e^{-0.394}+e^{-0.462}}\approx \frac{0.629}{0.673+0.629}\approx 0.471$$

5. 轨迹更新

为了进行轨迹更新，我们需要计算梯度和 $M$ 矩阵。

5.1 梯度计算（这不是特别明白，大概意思应该是一般的收敛算法是用差分梯度的方法来迭代拟合，标准公式应该满足原文式子8，但是这里概率化了从而转成公式10来代替，推导过程还是不明白）

梯度是通过带权重的噪声轨迹的累积和计算得到的。在二维空间中，我们分别对 $x$ 和 $y$ 方向计算梯度。

对于每个轨迹的每个时间步，噪声变化为：

$${ϵ}_{k,i}=({ϵ}_{k,x,i},{ϵ}_{k,y,i})$$

具体数值如下：

• 第一条噪声轨迹 ${\tilde{\theta }}_1$：

$${ϵ}_1=[(0,0),(0.1,-0.1),(-0.2,0.1),(0.2,-0.2),(0,0)]$$

• 第二条噪声轨迹 ${\tilde{\theta }}_2$：

$${ϵ}_2=[(0,0),(-0.05,0.05),(0.15,-0.15),(-0.1,0.1),(0,0)]$$

计算加权噪声累积和（梯度）：

$$\Delta {\theta }_i=\sum _{k=1}^{K}P({\tilde{\theta }}_k)\cdot {ϵ}_{k,i}$$

具体计算：

• 时间步 1（点 $(2.25,2.25)$）：

$$\Delta x_1=0.529\times 0.1+0.471\times (-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$$

• 时间步 2（点 $(4.5,4.5)$）：

$$\Delta x_2=0.529\times (-0.2)+0.471\times 0.15=-0.1058+0.07065=-0.03515$$

$$\Delta y_2=0.529\times 0.1+0.471\times (-0.15)=0.0529-0.07065=-0.01775$$

• 时间步 3（点 $(6.75,6.75)$）：

$$\Delta x_3=0.529\times 0.2+0.471\times (-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$$

• 时间步 0 和 4（起点和终点）：

$$\Delta x_0=\Delta x_4=0,\Delta y_0=\Delta y_4=0$$

因此，梯度（加权噪声累积和）为：

$$\Delta \theta =[(0,0),(0.02935,-0.02935),(-0.03515,-0.01775),(0.0587,-0.0587),(0,0)]$$

5.2 矩阵 $M$ 的构建

矩阵 $M$ 用于平滑更新轨迹。根据论文，$M$ 是通过对 $R^{-1}$ 进行缩放得到的，其中 $R=A^{T}A$。

有限差分矩阵 $A$：

对于 $N=5$ 个点，计算加速度（即二阶差分），$A$ 为 $3\times 5$ 的矩阵：

$$A=\left[\begin{array}{ccccc}1& -2& 1& 0& 0\\ 0& 1& -2& 1& 0\\ 0& 0& 1& -2& 1\end{array}\right]$$

计算 $R=A^{T}A$：

$$A^T=\left[\begin{array}{ccc}1& 0& 0\\ -2& 1& 0\\ 1& -2& 1\\ 0& 1& -2\\ 0& 0& 1\end{array}\right]$$

$$R=A^{T}A=\left[\begin{array}{ccccc}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{array}\right]$$

计算 $R^{-1}$：

由于 $R$ 是一个 $5\times 5$ 的矩阵，直接计算逆矩阵较为复杂。在实际应用中，可以使用数值计算工具（如 NumPy）来计算。为了简化示例，我们假设 $R^{-1}$ 已知或使用单位矩阵近似（仅用于说明）。

缩放 $R^{-1}$ 以得到 $M$：

根据论文，矩阵 $M$ 是通过将 $R^{-1}$ 的每一列缩放，使每列的最大元素为 $\frac{1}{N}$。在本示例中，假设 $M$ 已简化为单位矩阵 $I$，即：

$$M=I=\left[\begin{array}{ccccc}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{array}\right]$$

注意：在实际应用中，$M$ 应包含平滑性信息，具体取决于 $R^{-1}$ 的计算结果。

5.3 更新轨迹

使用梯度和 $M$ 矩阵更新轨迹：

$$\theta \leftarrow \theta +M\cdot \Delta \theta$$

具体计算：

• 时间步 0：

$${\theta }_0=(0,0)+(0,0)=(0,0)$$

• 时间步 1：

$${\theta }_1=(2.25,2.25)+(0.02935,-0.02935)=(2.27935,2.22065)$$

• 时间步 2：

$${\theta }_2=(4.5,4.5)+(-0.03515,-0.01775)=(4.46485,4.48225)$$

• 时间步 3：

$${\theta }_3=(6.75,6.75)+(0.0587,-0.0587)=(6.8087,6.6913)$$

• 时间步 4：

$${\theta }_4=(9,9)+(0,0)=(9,9)$$

更新后的轨迹：

$${\theta }_{\text{new}}=[(0,0),(2.27935,2.22065),(4.46485,4.48225),(6.8087,6.6913),(9,9)]$$

结论

通过上述具体数值示例，我们详细展示了 STOMP 算法在二维空间中如何计算梯度和使用 $M$ 矩阵更新轨迹的过程。关键步骤包括：

• 生成带噪声轨迹：通过添加随机噪声探索轨迹空间。
• 计算代价函数：评估每条轨迹的碰撞和平滑代价。
• 计算权重：根据代价函数确定每条轨迹的权重。
• 计算梯度：加权累积噪声轨迹，作为轨迹更新的方向。
• 更新轨迹：通过 $M$ 矩阵平滑更新轨迹，确保轨迹的平滑性和避障性。

通过多次迭代，STOMP 算法能够有效地优化轨迹，使机器人能够在避开障碍物的同时保持路径的平滑性。

三、M矩阵用于尺寸变换，当考虑加速度时，M矩阵不为单位阵，可以如下：

如果矩阵 $M$ 不是单位矩阵，它会影响轨迹更新的平滑性。在STOMP算法中，矩阵 $M$ 是通过对矩阵 $R^{-1}$ 进行缩放后得到的，其中 $R$ 是基于加速度的二次项的矩阵。矩阵 $M$ 的主要作用是对轨迹的更新进行平滑处理，以确保轨迹更新时不会出现不连续或剧烈的跳变。

为了回答你关于 $M$ 不是单位矩阵时如何完成轨迹更新的过程，我们需要明确 $M$ 的来源，并展示如何计算和应用 $M$ 矩阵。

1. 回顾矩阵 $R$ 和 $M$ 的构建

1.1 矩阵 $A$（有限差分矩阵）

在STOMP算法中，矩阵 $A$ 用来计算轨迹的加速度（即二阶差分）。对于5个时间步（包括起点和终点）的轨迹，矩阵 $A$ 是一个 $3\times 5$ 的有限差分矩阵，用来计算每个点的加速度：

$$A=\left[\begin{array}{ccccc}1& -2& 1& 0& 0\\ 0& 1& -2& 1& 0\\ 0& 0& 1& -2& 1\end{array}\right]$$

1.2 矩阵 $R$

矩阵 $R$ 是通过 $A$ 的转置和 $A$ 相乘得到的。$R=A^{T}A$：

$$A^T=\left[\begin{array}{ccc}1& 0& 0\\ -2& 1& 0\\ 1& -2& 1\\ 0& 1& -2\\ 0& 0& 1\end{array}\right]$$

$$R=A^{T}A=\left[\begin{array}{ccccc}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{array}\right]$$

1.3 矩阵 $R^{-1}$

矩阵 $R$ 是正定矩阵，因此可以计算其逆矩阵 $R^{-1}$。为了简单起见，我们可以使用数值计算工具（如NumPy）来计算 $R^{-1}$ 的数值解。实际应用中，数值解会用于轨迹更新时的平滑处理。

1.4 矩阵 $M$

矩阵 $M$ 是 $R^{-1}$ 经过缩放后的矩阵。缩放的规则是将每一列的最大元素缩放到 $\frac{1}{N}$，其中 $N$ 是轨迹中的离散时间步数。在我们的例子中，$N=5$。

矩阵 $M$ 的构建公式为：

$$M=\frac{1}{N}\times R^{-1}$$

具体的缩放系数根据每一列的最大元素确定，以确保轨迹的平滑性和更新幅度不过大。

2. 计算矩阵 $M$ 的具体步骤

假设我们已经得到了 $R^{-1}$ 矩阵的数值解。接下来，我们需要对其进行缩放处理，确保每一列的最大元素等于 $\frac{1}{N}$。

例如，如果 $R^{-1}$ 的数值解为（假设）：

$$R^{-1}=\left[\begin{array}{ccccc}{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{array}\right]$$

我们对每一列进行缩放，确保最大值为 $\frac{1}{N}$：

$$M_{ij}=\frac{a_{ij}}{\max (a_{1j},a_{2j},a_{3j},a_{4j},a_{5j})}\times \frac{1}{N}$$

这个缩放矩阵 $M$ 将用于平滑轨迹更新。

3. 使用 $M$ 矩阵进行轨迹更新

在计算好 $M$ 矩阵后，我们使用加权噪声累积和（梯度）来更新轨迹。具体的更新公式为：

$${\theta }_{\text{new}}={\theta }_{\text{old}}+M\cdot \Delta \theta$$

其中：

• ${\theta }_{\text{old}}$ 是前一轮迭代的轨迹
• $\Delta \theta$ 是根据每条噪声轨迹的加权累积得到的梯度
• $M$ 是经过缩放的 $R^{-1}$ 矩阵，确保轨迹更新的平滑性

我们通过矩阵 $M$ 的平滑效果，可以避免轨迹更新过程中出现剧烈的跳变，确保轨迹的平滑性，同时根据噪声探索的结果优化轨迹。

4. 实际数值示例

假设经过数值计算，矩阵 $R^{-1}$ 为：

$$R^{-1}=\left[\begin{array}{ccccc}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{array}\right]$$

我们将每一列的最大元素缩放到 $\frac{1}{5}=0.2$，以得到矩阵 $M$：

$$M=\left[\begin{array}{ccccc}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{array}\right]$$

接下来，假设我们得到的梯度 $\Delta \theta$ 为：

$$\Delta \theta =[(0,0),(0.02935,-0.02935),(-0.03515,-0.01775),(0.0587,-0.0587),(0,0)]$$

我们将矩阵 $M$ 应用于 $\Delta \theta$ 上进行更新：

对于轨迹的 $x$ 和 $y$ 分量，我们分别计算：

$${\theta }_{\text{new},x}={\theta }_{\text{old},x}+M\cdot \Delta {\theta }_x$$

$${\theta }_{\text{new},y}={\theta }_{\text{old},y}+M\cdot \Delta {\theta }_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. 迭代和收敛

重复以下步骤，直到轨迹代价收敛：

1. 生成新的带噪声轨迹：基于更新后的轨迹 ${\theta }_{\text{new}}$，再次生成带噪声的轨迹。
2. 计算代价函数：包括碰撞代价和平滑代价。
3. 计算权重：根据代价函数计算每条轨迹的权重概率。
4. 计算梯度：加权累积噪声轨迹。
5. 更新轨迹：使用梯度和 $M$ 矩阵更新轨迹。

在每次迭代中，轨迹会逐步优化，代价函数 $S(\theta )$ 会逐步减少，轨迹将趋向于避开障碍物并保持平滑。

上述例子代码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$ 与障碍物的距离，并将其转化为一个成本值，这个成本值在优化过程中会被最小化。公式如下：

$$q_o({\theta }_t)=\sum _{b\in B}\max (ϵ+r_b-d(x_b),0)\Vert {\dot{x}}_b\Vert$$

这里：

• $q_o({\theta }_t)$ 是在时间步 $t$ 的障碍物成本。
• $B$ 是机器人中所有可能与障碍物发生碰撞的部件的集合。
• $ϵ$ 是安全距离，通常是一个正值。
• $r_b$ 是部件 $b$ 的半径。
• $d(x_b)$ 是从部件 $b$ 的中心到最近障碍物的距离。
• ${\dot{x}}_b$ 是部件 $b$ 的速度。

计算步骤

1. 计算每个部件中心到障碍物的距离 $d(x_b)$：这通常通过计算部件中心位置 $x_b$ 到最近障碍物的欧氏距离来完成。
2. 评估碰撞条件：对于每个部件，检查是否满足 $ϵ+r_b>d(x_b)$，如果满足，意味着部件与障碍物的距离小于安全距离加上部件半径，存在潜在碰撞。
3. 计算障碍物成本：如果存在潜在碰撞，计算成本为 $\max (ϵ+r_b-d(x_b),0)\Vert {\dot{x}}_b\Vert$，否则成本为0。
4. 对所有部件的成本求和：将所有部件的障碍物成本加起来，得到总的障碍物成本 $q_o({\theta }_t)$。

举例说明

假设一个6轴机器人的某个部件 $b$ 在时间步 $t$ 的位置是 $(x_b,y_b,z_b)$，障碍物的位置是 $(x_{ob},y_{ob},z_{ob})$，部件 $b$ 的半径 $r_b$ 是 0.1 米，安全距离 $ϵ$ 设为 0.05 米。

1. 计算距离 $d(x_b)$：

$$d(x_b)=\sqrt{(x_b-x_{ob}{)}^2+(y_b-y_{ob}{)}^2+(z_b-z_{ob}{)}^2}$$
2. 评估碰撞条件：
- 假设 $d(x_b)=0.12$ 米，$r_b=0.1$ 米，$ϵ=0.05$ 米。
- 则 $ϵ+r_b=0.15$ 米，因为 $0.15>0.12$，所以存在潜在碰撞。
3. 计算障碍物成本：
- 成本为 $\max (0.15-0.12,0)\cdot \Vert {\dot{x}}_b\Vert =0.03\cdot \Vert {\dot{x}}_b\Vert$。
- 假设部件的速度 ${\dot{x}}_b$ 为 $(0.1\text{m/s},0,0)$，则 $\Vert {\dot{x}}_b\Vert =0.1$。
- 总成本为 $0.03\cdot 0.1=0.003$。
4. 对所有部件的成本求和：
- 如果机器人有多个部件，我们需要对每个部件重复上述步骤，然后求和得到 $q_o({\theta }_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()