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

本文链接：https://blog.csdn.net/qq_46009436/article/details/142385846

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

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

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

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

目录

💥1 概述

2.1 问题建模

2.2 示例建模

2.3 求解方法

3.1 优势

3.2 局限

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码实现

💥1 概述

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

使用线性规划（LP）进行路径规划研究文档

一、引言

路径规划问题是在具有障碍物的环境中，按照某一评价标准，规划出一条从起始位置到达终点位置的最优或次优的无碰撞路径。线性规划（Linear Programming, LP）作为一种数学优化技术，常用于在给定线性等式或不等式约束条件下，求解线性目标函数的最优解。在路径规划领域，尽管LP不是最直接的解决方案，但通过适当的建模和转换，LP可以用于解决特定类型的路径规划问题。

二、LP在路径规划中的应用

2.1 问题建模

在将LP应用于路径规划时，首先需要将路径规划问题转化为LP问题。这通常涉及以下步骤：

定义决策变量：决策变量可以表示路径中的关键点（如节点或路段的选择）。
建立目标函数：目标函数通常是路径的总成本、时间或距离的最小化。
设定约束条件：约束条件包括障碍物避让、路径连续性、资源限制等。

2.2 示例建模

假设在一个简单的网格环境中，目标是从左上角（起点）移动到右下角（终点），每个网格单元代表一个可能的路径点，且路径点之间的移动成本是已知的。我们可以将问题建模为如下LP问题：

决策变量：xij，表示是否选择从点 i 到点 j 的路径（xij=1 表示选择，xij=0 表示不选择）。
目标函数：最小化总成本，即 min∑i,jcijxij，其中 cij 是从点 i 到点 j 的成本。
约束条件：
- 流量守恒：每个非起点非终点的点必须满足流入量等于流出量。
- 起点流出：从起点流出的路径只能有一条。
- 终点流入：到终点的路径只能有一条。
- 避障：路径不能穿越障碍物所在的网格。

2.3 求解方法

LP问题可以使用专门的求解软件（如Lindo、Excel Solver等）进行求解。在求解过程中，软件会根据目标函数和约束条件，通过迭代算法找到最优解或近似最优解。

三、LP路径规划的优势与局限

3.1 优势

数学严谨性：LP问题有严格的数学基础和求解方法，能够保证解的最优性或近似最优性。
灵活性：通过调整目标函数和约束条件，可以灵活地处理不同类型的路径规划问题。

3.2 局限

模型复杂度：将复杂的路径规划问题转换为LP模型可能非常困难，特别是当环境中有大量障碍物或路径选择较多时。
计算复杂度：对于大规模问题，LP求解的计算量可能非常大，导致求解时间过长。

四、结论与展望

尽管LP在路径规划中的应用有其局限性，但作为一种强大的数学优化工具，它在处理特定类型的路径规划问题时仍然具有重要价值。未来，随着算法和计算技术的发展，LP在路径规划领域的应用将更加广泛和深入。同时，也可以探索将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)];