【路径规划】使用 STOMP 进行路径规划和优化（Matlab实现）-优快云博客

本文链接：https://blog.youkuaiyun.com/Matlab88888/article/details/149481085

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

💥1 概述

摘要

1. 引言

2. STOMP算法原理

2.1 初始轨迹采样

2.2 噪声轨迹生成

2.3 成本函数设计

2.4 轨迹优化

2.5 迭代终止条件

3. 实验验证

3.1 二维平面实验

3.2 三维空间实验

4. 结论

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码实现

💥1 概述

在路径规划方面，STOMP 采用随机采样和探索来在复杂环境中找到可行路径。它通过扰动初始路径生成一系列候选轨迹。这些扰动旨在探索不同的可能路径和配置。通过评估每个候选轨迹的质量，STOMP 逐渐收敛到更好的路径。对于优化，STOMP 迭代地改进路径以最小化特定的成本函数。这些成本函数可以考虑诸如路径长度、平滑度、避障和时间效率等因素。通过重复的采样和优化步骤，STOMP 可以找到不仅可行而且在定义的标准方面经过优化的路径。 STOMP 的一个优点是它能够处理高维空间和复杂约束。它可以处理动态环境，并在规划过程中适应环境的变化。此外，与其他一些优化方法相比，STOMP 在计算上相对高效，使其适用于实时或近实时应用。总体而言，使用 STOMP 进行路径规划和优化为在从机器人和自主系统到计算机图形和虚拟现实应用等广泛场景中找到最佳路径提供了一种有前景的方法。

摘要

STOMP（Stochastic Trajectory Optimization for Motion Planning）是一种基于随机轨迹优化的运动规划算法，通过生成带噪声的候选轨迹并迭代优化成本函数，能够在复杂环境中高效生成平滑且避障的路径。该算法结合了随机采样与梯度优化技术，特别适用于高维配置空间和动态环境下的路径规划任务。本文详细阐述了STOMP的核心原理、关键步骤、成本函数设计、优化策略及MATLAB实现方法，并通过实验验证了其在二维平面和三维空间中的有效性。

1. 引言

路径规划是机器人导航、自动驾驶和计算机图形学等领域的核心技术，其目标是在复杂环境中找到从起点到目标点的最优路径。传统路径规划算法（如A*、RRT）在低维空间中表现良好，但在高维或动态环境中存在计算效率低、路径不平滑等问题。STOMP作为一种基于随机优化的路径规划方法，通过生成噪声轨迹并迭代优化成本函数，能够克服传统算法的局限性，生成高质量的平滑路径。

2. STOMP算法原理

STOMP的核心思想是通过随机采样生成候选轨迹，并基于成本函数评估和优化轨迹质量。其算法流程可分为以下步骤：

2.1 初始轨迹采样

生成一条满足机器人运动学约束的初始轨迹，通常采用线性插值或多项式插值方法。例如，在二维平面中，初始轨迹可表示为：

θ(t)=θstart+t⋅Tθgoal−θstart

其中，θstart和θgoal分别为起点和目标点的关节角度，T为轨迹总时间。

2.2 噪声轨迹生成

在初始轨迹基础上添加高斯噪声，生成K条候选轨迹：

θk(t)=θ(t)+ϵk(t),ϵk(t)∼N(0,Σ)

其中，Σ为噪声协方差矩阵，控制噪声的强度和方向。

2.3 成本函数设计

成本函数由障碍物代价、平滑度代价和约束代价组成：

C(θ)=Cobs(θ)+wsmooth⋅Csmooth(θ)+wconst⋅Cconst(θ)

障碍物代价：通过有符号距离场（SDF）计算轨迹点到障碍物的距离，距离越近代价越高。
平滑度代价：采用二阶差分矩阵量化轨迹的加速度变化，例如：

Csmooth(θ)=θTRθ,R=ATA

其中，A为有限差分矩阵，R为平滑度权重矩阵。

约束代价：包括关节限位、扭矩限制等，通过惩罚函数实现。

2.4 轨迹优化

基于候选轨迹的成本函数值，计算每条轨迹的更新概率：

P(θk)∝exp(−σC(θk)−Cmin)

其中，Cmin为当前迭代中的最小成本，σ为温度参数。根据概率分布生成更新量：

Δθ=k=1∑KP(θk)⋅ϵk

通过梯度下降更新轨迹：

θnew=θ+α⋅MΔθ

其中，α为学习率，M为归一化矩阵。

2.5 迭代终止条件

当成本函数值收敛（如ΔC<ϵ）或达到最大迭代次数时终止优化。

3. 实验验证

3.1 二维平面实验

在100×100的网格环境中，设置起点为(10,10)，目标点为(90,90)，并添加矩形障碍物。STOMP算法生成的路径平滑且避障，成本函数值在50次迭代内收敛至0.2以下。

3.2 三维空间实验

在100×100×100的三维环境中，测试机械臂关节空间的路径规划。通过调整噪声协方差矩阵Σ和平滑度权重wsmooth，算法成功生成满足关节限位和扭矩限制的平滑轨迹。

4. 结论

STOMP算法通过随机采样和迭代优化，能够在复杂环境中高效生成平滑且避障的路径。其核心优势包括：

无需梯度信息：适用于非光滑或离散的成本函数。
高维适应性：可扩展至机械臂关节空间或自动驾驶车辆的高维配置空间。
动态环境支持：通过实时更新噪声分布适应环境变化。

未来研究方向包括：

结合深度学习模型（如神经辐射场）提升环境感知能力。
优化噪声生成策略以减少迭代次数。
探索多机器人协同路径规划的STOMP变体。

📚2 运行结果

主函数部分代码：

clear all;close all;

%%
%Parameters
T = 5;
nSamples = 100;
kPaths = 20;
convThr = 0;

%%
%Setup environment
lynxStart();hold on;
%Environment size
Env = zeros(100,100,100);
% %Obstacle cube
% obsts = [100 1000 -1000 1000 200 200];
obsts=[];
% %Passage hole [center r]
% hole = [0 0 200 60];
hole=[];
%Calculate EDT_Env
voxel_size = [10, 10, 10];
[Env,Cube] = constructEnv(voxel_size);
Env_edt = prod(voxel_size) ^ (1/3) * sEDT_3d(Env);
% Env_edt = sEDT_3d(Env);

%%
%Initialization
TStart = [0 1 0 130; 0 0 1 180; 1 0 0 280; 0 0 0 1];
TGoal = [1 0 0 263.5; 0 1 0 -50; 0 0 1 122.25; 0 0 0 1];
qStart = IK_lynx(TStart);
qStart = qStart(1:5)
qGoal = IK_lynx(TGoal);
qGoal = qGoal(1:5)
theta = [linspace(qStart(1), qGoal(1), nSamples);linspace(qStart(2), qGoal(2), nSamples);linspace(qStart(3), qGoal(3), nSamples);...
    linspace(qStart(4), qGoal(4), nSamples);linspace(qStart(5), qGoal(5), nSamples)];
%Initialize theta on a line
ntheta = cell(kPaths, 1);

%%
%Precompute
A_k = eye(nSamples - 1, nSamples - 1);
A = -2 * eye(nSamples, nSamples);
A(1:nSamples - 1, 2:nSamples) = A(1:nSamples - 1, 2:nSamples) + A_k;
A(2:nSamples, 1:nSamples - 1) = A(2:nSamples, 1:nSamples - 1) + A_k;
A = A(:, 2:99);
R = A' * A;
Rinv = inv(R);
M = 1 / nSamples * Rinv ./ max(Rinv, [], 1);
Rinv = Rinv / sum(sum(Rinv));

%%
%Planner
Q_time = [];
RAR_time = [];
Qtheta = stompCompute_PathCost(theta, obsts, hole, R, Env_edt);
QthetaOld = 0;
tic
ite=0;
while abs(Qtheta - QthetaOld) > convThr
    ite=ite+1;
%     Qtheta
    QthetaOld = Qtheta;
    
    %Random Sampling
    [ntheta, epsilon] = stompCompute_NoisyTraj(kPaths,qStart,qGoal,Rinv, theta);
    %Compute Cost and Probability
    pathCost = zeros(kPaths, nSamples);
    pathE = zeros(kPaths, nSamples);
    pathProb = zeros(kPaths, nSamples);
    for i = 1 : kPaths
        pathCost(i, :) = stompCompute_Cost(ntheta{i}, obsts, hole, Env_edt);
    end
    pathE = stompCompute_ELambda(pathCost);
    pathProb = pathE ./ sum(pathE, 1);
    pathProb(isnan(pathProb) == 1) = 0;
    
    
    %Compute delta
    dtheta = sum(pathProb .* epsilon, 1);
    
    if sum(sum(pathCost)) == 0
        dtheta = zeros(nSamples);
    end
    
    %Smooth delta
    dtheta = M * dtheta(2 : nSamples - 1)';
    
    %Update theta
    theta(:, 2 : nSamples - 1) = theta(:, 2 : nSamples - 1) + [dtheta';dtheta';dtheta';dtheta';dtheta'];
%     theta
    
    %Compute new trajectory cost
    Qtheta = stompCompute_PathCost(theta, obsts, hole, R, Env_edt);
    
    if mod(ite, 5000) == 1
        fill3([Cube(1,1) Cube(1,1) Cube(1,1)+Cube(2,1) Cube(1,1)+Cube(2,1)], [Cube(1,2) Cube(1,2)+Cube(2,2)...
            Cube(1,2)+Cube(2,2) Cube(1,2) ], [Cube(1,3) Cube(1,3) Cube(1,3) Cube(1,3)], 'b');
        fill3([Cube(1,1) Cube(1,1) Cube(1,1)+Cube(2,1) Cube(1,1)+Cube(2,1)], [Cube(1,2) Cube(1,2)+Cube(2,2)...
            Cube(1,2)+Cube(2,2) Cube(1,2) ], [Cube(1,3)+Cube(2,3) Cube(1,3)+Cube(2,3) Cube(1,3)+Cube(2,3) Cube(1,3)+Cube(2,3)], 'b')
        for i= 1:100
            [X,~]=updateQ([theta(:,i)' 0]);
            plot3(X(1, 1), X(1, 2), X(1, 3), 'bo', 'markersize', 6);
            plot3(X(2, 1), X(2, 2), X(2, 3), 'ro', 'markersize', 6);
            plot3(X(3, 1), X(3, 2), X(3, 3), 'go', 'markersize', 6);
            plot3(X(4, 1), X(4, 2), X(4, 3), 'yo', 'markersize', 6);
            plot3(X(5, 1), X(5, 2), X(5, 3), 'ko', 'markersize', 6);
            plot3(X(6, 1), X(6, 2), X(6, 3), 'mo', 'markersize', 6);
            lynxServoSim([theta(:,i)' 0]);
%             pause(0.01);