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

荔枝科研社

已于 2024-09-13 11:16:42 修改

阅读量448

点赞数 5

文章标签： matlab 数据库前端

于 2024-09-13 11:16:14 首次发布

本文链接：https://blog.csdn.net/weixin_46039719/article/details/142205893

版权

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

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

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

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

目录

💥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);