【路径规划】使用 STOMP 进行路径规划和优化（Matlab实现）

“在代码的海洋里，有无尽的知识等待你去发现。我就是那艘领航的船，带你乘风破浪，驶向代码的彼岸。

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

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

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

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

目录

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码实现

💥1 概述

在路径规划方面，STOMP 采用随机采样和探索来在复杂环境中找到可行路径。它通过扰动初始路径生成一系列候选轨迹。这些扰动旨在探索不同的可能路径和配置。通过评估每个候选轨迹的质量，STOMP 逐渐收敛到更好的路径。 对于优化，STOMP 迭代地改进路径以最小化特定的成本函数。这些成本函数可以考虑诸如路径长度、平滑度、避障和时间效率等因素。通过重复的采样和优化步骤，STOMP 可以找到不仅可行而且在定义的标准方面经过优化的路径。 STOMP 的一个优点是它能够处理高维空间和复杂约束。它可以处理动态环境，并在规划过程中适应环境的变化。此外，与其他一些优化方法相比，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);

🎉3 参考文献

文章中一些内容引自网络，会注明出处或引用为参考文献，难免有未尽之处，如有不妥，请随时联系删除。

[1]袁雷,贾小林,顾娅军,等.融合椭圆约束的快速行进树路径规划算法[J/OL].计算机应用研究:1-6[2024-09-12].https://doi.org/10.19734/j.issn.1001-3695.2024.05.0162.

[2]罗统,张民,梁承宇.复杂环境下多无人机协同目标跟踪路径规划[J].兵工自动化,2024,43(09):90-96.

🌈4 Matlab代码实现