2维平面下STOMP路径规划（MATLAB）

世界尽头的青豆

已于 2025-04-11 09:52:28 修改

阅读量481

点赞数 13

CC 4.0 BY-SA版权

文章标签：平面

于 2025-04-02 10:22:02 首次发布

本文链接：https://blog.csdn.net/m0_50643362/article/details/146934440

STOMP 是一种基于路径优化的运动规划方法。在原论文中STOMP是用于机械臂规划的算法，但它同样也适用于更简单的平面避障。通过确定评价路径质量的性能指标并制定运动状态对应的代价函数，在多次迭代后STOMP算法可收敛到最小代价路径。这篇博客就略去算法原理的详细分析，按照算法伪代码编写STOMP的二维示例。伪代码如下：

1. 伪代码分析

(1) 初始化

首先预处理算法中用到的矩阵。 $\mathbf{A}$ 是一个有限差分矩阵，实际上就是加速度离散化后的到的系数矩阵。乘上 $\theta$ 就能够反映加速度项 $\ddot{\theta}$ 。这里的 $\mathbf{A}$ 如下：

$\mathbf{A} = \begin{bmatrix} 1 & 0 & 0 & \cdots & 0 & 0 & 0 \\ -2 & 1 & 0 & \cdots & 0 & 0 & 0 \\ 1 & -2 & 1 & \cdots & 0 & 0 & 0 \\ \vdots & \vdots & \ddots & \ddots & \vdots & \vdots \\ 0 & 0 & 0 & \cdots & 1 & -2 & 1 \\ 0 & 0 & 0 & \cdots & 0 & 1 & -2 \\ 0 & 0 & 0 & \cdots & 0 & 0 & 1 \\ \end{bmatrix}$

因此文中的 $\theta^T \mathbf{R} \theta$ 项实际上就是 $\theta^T \mathbf{A}^T \mathbf{A} \theta=\|\ddot{\theta}\|^2$ ，当它越小，轨迹就越平滑。 $\mathbf{R}$ 矩阵实际上并不体现在代价函数中，即后续优化时用到的代价 $S$ 中不含有任何平滑信息，它之所以能起作用，是因为 $\mathbf{R}$ 在优化时以 $\mathbf{M}=\mathbf{R}^{-1}$ 的形式作用在梯度下降中。这里的 $\mathbf{M}$ 需要进行缩放，使得每列元素缩放后最大为 $1/N$ ，这是为了控制参数更新的范围，使其不要变化过大。 $N$ 是整个轨迹点集的长度。

(2) 循环

首先生成正态分布的噪声 $\epsilon$ ，对于机械臂而言需要生成 $K$ 条轨迹， $N$ 个状态点， $J$ 个配置空间状态（即关节角度），共 $K \times N \times J$ 个噪声。对于2维平面的简单避障示例，配置空间状态直接使用工作空间状态，即 $(x,y)$ 共2个状态即可。
将噪声加入初始轨迹就得到 $K$ 条噪声轨迹，然后对第 $k\in K$ 条轨迹的所有 $n \in N$ 个点都计算代价函数

$q(\theta)=\sum^T_{t=0}{q_o(\theta_t)+q_c(\theta_t)+q_t(\theta_t)}$

这里只取障碍物项 $q_o$ 。并且简化起见，省去了 $q_o$ 中的速度项，即 $q_o$ 为：

$q_o=\sum_{b\in \mathcal{B}} {\max{(\epsilon + r - d(x))}}$

因此就是，这里就是对每条轨迹的每个点都计算到障碍物的代价函数 $S(\tilde{\theta}_{k,~n})$ ，存成一个 $N\times K$ 大小的数组。

接着，对第 $n$ 个点，取出所有 $K$ 条轨迹的代价 $S(\tilde{\theta}_{k,~n})=q(\tilde{\theta}_{k,~n})$ ，找到最大值和最小值，并计算概率：

$P(\tilde{\theta}_{k,~n})=e^{-h\frac{S(\theta_{k,n})-\min{S(\theta_{k,n})}}{\max{S(\theta_{k,n})}-\min{S(\theta_{k,n})}}}$

这里根据 $q$ 的计算，可能出现分母为0的情况，将其处理为0即可。

然后计算 $\delta{\theta}$ 用于轨迹更新：

$[\delta \boldsymbol{\theta}]_i = \sum_{k=1}^K P(\tilde{\boldsymbol{\theta}}_{k,i}) [\boldsymbol{\epsilon}_k]_i$

再利用 $\delta{\theta}=M\theta$ 更新 $\theta = \theta + \delta \theta$ ，就完成了一次迭代。

最后计算轨迹的代价函数：

$Q(\theta)=\sum^N_{i=1}q(\theta_i)+\frac{1}{2}\theta^T R\theta$

$Q$ 收敛即为退出循环的条件。但这里的 $\theta^T R\theta$ 应为 $J\times J$ 大小的矩阵，因此将矩阵的所有值相加得到最终的代价函数。

2. MATLAB代码

对应的MATLAB部分代码如下：

(1) 初始化

A = diag(ones(n_waypoints-1,1),1) + diag(ones(n_waypoints-1,1),-1) - 2*eye(n_waypoints);
A = [[1, zeros(1,n_waypoints-1)]; A; [zeros(1,n_waypoints-1), 1]];
R = A' * A;
Rinv = inv(R);
M = 1 / n_waypoints * Rinv ./ max(Rinv, [], 1); % 将最大值限制在1/N
Rinv = 5 * Rinv / sum(sum(Rinv)); % 调整噪声的方差

(2) 障碍成本函数计算

function q_obs = compute_obs_cost(theta, obs)
    epsilon = 0.05;  % 安全阈值
    q_obs = 0;
    for i = 1:size(obs, 1)
        dist = norm(theta - obs(i,1:2)) - obs(i, 3);
        q_obs = q_obs + max(epsilon - dist, 0); % 这里省去了论文中的速度项
    end
end

(3) 成本函数计算

function [S, Q] = compute_cost(theta, obs, R)
    [N, ~, K] = size(theta);

    S = zeros(N, K); % 对于第k条轨迹，第n个路径点，都计算q，作为S的值
    Q = zeros(1, K); % 对于第k条轨迹，总的代价和为Q
    
    % 障碍物成本
    for k = 1:K
        for n = 1:N
            for i = 1:size(obs,1)
                q_obs = compute_obs_cost(theta(n,:,k), obs);
                S(n, k) = q_obs;
            end
        end
        % 总成本
        Q(k) = sum(theta(2:end-1,:,k)' * R * theta(2:end-1,:,k), "all") / 2 + sum(S(:,k));
    end
end

(4) 概率计算

function P = compute_prob(S)
    h = 10;
    [N, K] = size(S);
    P = zeros(N, K);

    for n = 1:N
        S_min = min(S, [], 2); % 取所有K条轨迹中每个路径点的最小值
        S_max = max(S, [], 2); % 取所有K条轨迹中每个路径点的最大值

        for k = 1:K
            P(n,k) = exp(-h * (S(n,k) - S_min(n)) / (S_max(n) - S_min(n)));              
        end
        P(isnan(P)) = 0;
        P(n, :) = P(n, :) / sum(P(n, :), 2); % 归一化
        P(isnan(P)) = 0;
        
    end
end

(5) theta更新量计算

function delta_theta = compute_delta_theta(P, noise, M)
    [N, ~] = size(P);
    delta_theta = zeros(N - 2, 2); % N-2是因为噪声在生成时不考虑起点和终点，因此是少两个值的

    for n = 1: N - 2
        delta_theta(n, 1) = sum(reshape(noise(n, 1, :),1,[]) .* P(n + 1, :), 2);
        delta_theta(n, 2) = sum(reshape(noise(n, 2, :),1,[]) .* P(n + 1, :), 2);
    end

    delta_theta = M * delta_theta;

    delta_theta = [[0, 0]; delta_theta; [0, 0]]; % 这里再加上起点和终点的噪声
end