路径规划之基于二次规划的路径平滑Matlab代码

参考:

自动驾驶决策规划算法第二章第二节(上) 参考线模块_哔哩哔哩_bilibili

自动驾驶决策规划算法第二章第二节(下) 参考线代码实践_哔哩哔哩_bilibili

QP函数,二次规划的逻辑

Matlab 复制代码
function [smooth_path_x,smooth_path_y] = QP(path_x, path_y, w_cost_smooth, w_cost_length, w_cost_ref, x_lb, x_ub, y_lb, y_ub)

%   采用二次规划平滑路径
%   0.5X'Hx + f'x = min
%   lb <= x <= ub

% 输入:
% path_x, path_y: 待平滑路径
%  w_cost_smooth, w_cost_length, w_cost_re:平滑代价,紧凑代价,几何相似代价权重系数
% x_lb, x_ub, y_lb, y_ub:允许x, y变化的上下界(约束条件)

% 输出:
% smooth_path_x,smooth_path_y:平滑后路径

% 分析二次规划的形式
% H = 2 * (w_cost_smooth * A1' * A1 + w_cost_length * A2' * A2 + w_cost_ref * A3' * A3)
% f = -2 * w_cost_ref * path(待优化路径的xy组合)
% A1 = [1, 0, -2, 0, 1, 0              
%          1, 0, -2, 0, 1, 0
%             1, 0, -2, 0, 1, 0
%                1, 0, -2, 0, 1, 0
%                    .............]
% A2 = [1, 0, -1, 0,              
%          1, 0, -1, 0,
%             1, 0, -1, 0,
%                1, 0, -1, 0,
%                    .............]
% A3 为2n*2n的单位矩阵,n为平滑点的个数

n = length(path_x);

% 初始化平滑路径
smooth_path_x = zeros(n,1);
smooth_path_y = zeros(n,1);

% 初始化损失函数相关信息
A1 = zeros(2*n-4, 2*n);
A2 = zeros(2*n-2, 2*n);
A3 = eye(2*n, 2*n);
path = zeros(2*n, 1);

% 初始化不等式约束
lb = zeros(2*n, 1);
ub = zeros(2*n, 1);

% 初始化等式约束(起点和终点必须一致)
Aeq = zeros(2*n, 2*n);
beq = zeros(2*n, 1);

%计算f、lb、ub
for i = 1:n
    path(2*i-1) = path_x(i);
    path(2*i) = path_y(i);

    lb(2*i-1) = path_x(i) - x_lb;
    lb(2*i) = path_y(i) - y_lb;

    ub(2*i-1) = path_x(i) + x_ub;
    ub(2*i) = path_y(i) + y_ub;
end

% 计算A1
for i = 1:2*n-5
    A1(i, i) = 1;
    A1(i, i+2) = -2;
    A1(i, i+4) = 1;
    A1(i+1, i+1) = 1;
    A1(i+1, i+3) = -2;
    A1(i+1, i+5) = 1;
end

% 计算A2
for i = 1:2*n-3
    A2(i,i) = 1;
    A2(i,i+2) = -1;
    A2(i+1,i+1) = 1;
    A2(i+1,i+3) = -1;
end

H = 2 * (w_cost_smooth * (A1' * A1) + w_cost_length * (A2' * A2) + w_cost_ref * (A3' * A3));
f = -2 * w_cost_ref * path;

% 计算Aeq、beq
Aeq(1,1) = 1;
Aeq(2,2) = 1;
Aeq(2*n-1,2*n-1) = 1;
Aeq(2*n,2*n) = 1;
beq(1,1) = path(1,1);
beq(2,1) = path(2,1);
beq(2*n-1,1) = path(2*n-1,1);
beq(2*n,1) = path(2*n,1);

%起点
X0 = path;

% 进行二次规划
X = quadprog(H,f,[],[],Aeq,beq,lb,ub,X0);

for i = 1:n
    smooth_path_x(i) = X(2*i-1);
    smooth_path_y(i) = X(2*i);
end
 

end

main函数,运行QP

Matlab 复制代码
clear;
clc;

path_x = [0, 1, 2, 4, 5, 5, 5, 6, 7, 8, 9, 10, 11, 12, 12, 13, 14, 14];
path_y = [0, 0, 0, 0, 0, 1, 2, 3, 4, 5, 9, 10, 11, 12, 12, 13, 14, 14];
w_cost_smooth = 3;
w_cost_length = 2;
w_cost_ref = 1; 
x_lb = 1;
x_ub = 1;
y_lb = 1;
y_ub = 1;

[smooth_path_x, smooth_path_y] = QP(path_x, path_y, w_cost_smooth, w_cost_length, w_cost_ref, x_lb, x_ub, y_lb, y_ub);

hold on;
plot(path_x, path_y,'Color','red');
plot(smooth_path_x, smooth_path_y,'Color','blue');

运行结果:蓝色为原路径,红色为平滑路径

相关推荐
昨日之日20069 分钟前
Wan2.2-S2V - 音频驱动图像生成电影级质量的数字人视频 ComfyUI工作流 支持50系显卡 一键整合包下载
人工智能·音视频
纪元A梦13 分钟前
贪心算法应用:化工反应器调度问题详解
算法·贪心算法
深圳市快瞳科技有限公司43 分钟前
小场景大市场:猫狗识别算法在宠物智能设备中的应用
算法·计算机视觉·宠物
liulilittle1 小时前
OPENPPP2 —— IP标准校验和算法深度剖析:从原理到SSE2优化实现
网络·c++·网络协议·tcp/ip·算法·ip·通信
SEO_juper3 小时前
大型语言模型SEO(LLM SEO)完全手册:驾驭搜索新范式
人工智能·语言模型·自然语言处理·chatgpt·llm·seo·数字营销
攻城狮7号3 小时前
腾讯混元翻译模型Hunyuan-MT-7B开源,先前拿了30个冠军
人工智能·hunyuan-mt-7b·腾讯混元翻译模型·30个冠军
superlls3 小时前
(算法 哈希表)【LeetCode 349】两个数组的交集 思路笔记自留
java·数据结构·算法
zezexihaha3 小时前
从“帮写文案”到“管生活”:个人AI工具的边界在哪?
人工智能
算家云4 小时前
nano banana官方最强Prompt模板来了!六大场景模板详解
人工智能·谷歌·ai大模型·算家云·ai生图·租算力,到算家云·nano banana 提示词
暴躁的大熊4 小时前
AI助力决策:告别生活与工作中的纠结,明析抉择引领明智选择
人工智能