【路径规划】使用 LP 进行路径规划（Matlab实现）-CSDN博客

本文链接：https://blog.csdn.net/Matlab88888/article/details/142341591

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

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

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

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

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

💥1 概述

线性规划（Linear Programming，LP）是一种在数学优化领域广泛应用的方法，近年来也被应用于路径规划问题中。在路径规划领域，使用 LP 进行路径规划具有以下特点和优势：一、规划原理 LP 通过建立数学模型来确定最优路径。首先，将路径规划问题转化为一个线性规划问题，明确目标函数和约束条件。目标函数通常是最小化路径长度、时间消耗或成本等，而约束条件则可能包括起点和终点的位置、障碍物的限制、交通规则等。通过求解这个线性规划问题，可以得到满足约束条件下的最优路径。二、优势体现 1. 精确性：LP 基于严格的数学模型进行求解，能够提供精确的路径规划结果。它可以考虑各种复杂的约束条件，确保生成的路径在实际应用中是可行的。 2. 可扩展性：对于大规模的路径规划问题，LP 可以有效地处理。可以通过增加变量和约束条件来适应不同规模和复杂程度的问题，具有较好的可扩展性。 3. 灵活性：可以根据不同的需求和场景调整目标函数和约束条件。例如，可以根据不同的交通状况、时间限制或资源约束来优化路径规划。总之，使用 LP 进行路径规划为解决各种路径规划问题提供了一种有效的方法，具有精确性、可扩展性和灵活性等优势，在物流、机器人导航和交通规划等领域有着广泛的应用前景。

📚2 运行结果

主函数部分代码：

clearvars;
close all; clc;

%% PARAMS
dt = 0.5;               % s
M = 16;     % circle discretization

v_max = 70;             % cm/s
w_max = pi/3;           % rad/s
sigma = 2;
f_max = sigma * w_max * v_max;  % maximum allowable force
mu = sigma * w_max;
%% Populating Objects in the Environment
% Object radii
r_O1 = 40;              % cm
r_O2 = 40;              % cm
r_O3 = 40;              % cm
r_G = 20;               % cm

% A = vehicle([0;0],[0.10;0], mu, dt);
A1 = vehicle([0;0],[0.10;0], mu, dt, 0);
A2 = vehicle([700;0],[-0.10;0], mu, dt, 0);
A3 = vehicle([500;600],[0.5;-0], mu, dt, 0);
G = target([700;700],[13;-2], dt, r_G);
O_1 = obstacle([500;500],[-7;-6], dt, r_O1);
O_2 = obstacle([500;700],[8;-3], dt, r_O2);
O_3 = obstacle([700;500],[4;8], dt, r_O3);

% Limits & bounds
L_min = 30;                  % [cm]

tF = 22;        % [in s]
% Decision variable:
% [DV_x, DV_y, d_1, d_2, q_1, q_2]^T

plot(G.Position(1), G.Position(2), 'go','MarkerSize', G.Radius); hold on
plot(O_1.Position(1), O_1.Position(2), 'ko','MarkerSize', O_1.Radius);
plot(O_2.Position(1), O_2.Position(2), 'ko','MarkerSize', O_2.Radius);
plot(O_3.Position(1), O_3.Position(2), 'ko','MarkerSize', O_3.Radius);
for i = 1:round(tF/dt)
    % Stuff that occurs at every time step
    
    % Robot 1
    [x1,fval,exitflag] =  perTime(A1,G,O_1,O_2,O_3,f_max,M);
    F1 = [x1(1);x1(2)];
    
    % Robot 2
    [x2,fval,exitflag] =  perTime(A2,A1,O_1,O_2,O_3,f_max,M);
    F2 = [x2(1);x2(2)];
    
    % Robot 3
    [x3,fval,exitflag] =  perTime(A3,A1,O_1,O_2,O_3,f_max,M);
    F3 = [x3(1);x3(2)];