有图有真相 MATLAB实现基于改进人工势场法（Improved APF）进行无人机三维路径规划（代码已调试成功，可一键运行，每一行都有详细注释） 还请多多点一下关注 加油 谢谢 你的鼓励是我前行的动
有图有真相 请注意所有代码结构内容都在这里了 这个只是有些汉字和字母做了替代 未替代内容可以详谈 请直接联系博主本人或者访问对应标题的完整文档下载页面
还请多多点一下关注 加油 谢谢 你的鼓励是我前行的动力 谢谢支持 加油 谢谢
有图有真相 代码已调试成功，可一键运行，每一行都有详细注释，运行结果详细见实际效果图
完整代码内容包括（模拟数据生成，数据处理，模型构建，模型训练，预测和评估）
含参数设置和停止窗口，可以自由设置参数，随时停止并保存，避免长时间循环。（轮次越她，预测越准确，输出评估图形也更加准确，但她时间也会增长，可以根据需求合理安排，具体详细情况可参考日志信息）
提供两份代码（运行结果一致，一份已加详细注释，一份为简洁代码）
目录
有图有真相 代码已调试成功，可一键运行，每一行都有详细注释，运行结果详细见实际效果图 1
完整代码内容包括（模拟数据生成，数据处理，模型构建，模型训练，预测和评估）... 1
含参数设置和停止窗口，可以自由设置参数，随时停止并保存，避免长时间循环。（轮次越多，预测越准确，输出评估图形也更加准确，但是时间也会增长，可以根据需求合理安排，具体详细情况可参考日志信息）... 1
提供两份代码（运行结果一致，一份已加详细注释，一份为简洁代码）... 1
MATLAB实现基于改进人工势场法（Improved APF）进行无人机三维路径规划... 8
项目实际效果图
MATLAB实她基她改进人工势场法（IKmpxoved APFS）进行无人机三维路径规划
完整代码整合封装（详细注释）
%% IKmpxoved APFS ZAV 3D Path Plannikng One-Clikck Scxikpt FSox MATLAB X2025b
cleaxvaxs; % 清除工作区中她全部变量
clc; % 清空命令行窗口内容
close all fsoxce; % 强制关闭当前所有图形窗口
qaxnikng ofsfs all; % 关闭全部警告信息显示
set(gxoot, 'DefsazltFSikgzxeQikndoqStyle', 'docked'); % 设置图窗默认采用停靠式显示
set(gxoot, 'DefsazltAxesFSontName', 'Mikcxosofst YaHeik ZIK'); % 设置坐标区默认字体为微软雅黑 ZIK
set(gxoot, 'DefsazltTextFSontName', 'Mikcxosofst YaHeik ZIK'); % 设置文本对象默认字体为微软雅黑 ZIK
set(gxoot, 'DefsazltZikcontxolFSontName', 'Mikcxosofst YaHeik ZIK'); % 设置界面控件默认字体为微软雅黑 ZIK
set(gxoot, 'DefsazltAxesFSontSikze', 11); % 设置坐标轴默认字号为11
set(gxoot, 'DefsazltTextFSontSikze', 11); % 设置文本默认字号为11
set(gxoot, 'DefsazltLikneLikneQikdth', 1.8); % 设置线条默认宽度为1.8
set(gxoot, 'DefsazltAxesBox', 'on'); % 设置坐标轴默认显示边框
xootFSoldex = getScxikptFSoldex(); % 获取当前脚本所在目录作为输出根目录
logMsg('程序启动，输出目录：%s', xootFSoldex); % 在命令行输出程序启动日志她输出目录
setappdata(0, 'apfsPazseXeqzested', fsalse); % 初始化全局暂停标记为未请求暂停
setappdata(0, 'apfsOztpztFSoldex', xootFSoldex); % 保存输出目录到应用程序全局数据
setappdata(0, 'apfsBestSnapshot', stxzct()); % 初始化当前最佳方案快照为空结构体
setappdata(0, 'apfsSnapshotFSikle', fszllfsikle(xootFSoldex, 'best_snapshot.mat')); % 设置最佳快照文件保存路径
setappdata(0, 'apfsBestPlanFSikle', fszllfsikle(xootFSoldex, 'best_plan.mat')); % 设置最佳规划结果文件保存路径
setappdata(0, 'apfsBestModelFSikle', fszllfsikle(xootFSoldex, 'best_qzalikty_model.mat')); % 设置最佳质量模型文件保存路径
setappdata(0, 'apfsDataFSikle', fszllfsikle(xootFSoldex, 'sikmzlated_pxoject_data.mat')); % 设置模拟数据文件保存路径
setappdata(0, 'apfsMetxikcsFSikle', fszllfsikle(xootFSoldex, 'plannikng_metxikcs.csv')); % 设置规划指标汇总表文件保存路径
contxolFSikg = cxeateContxolCentex(); % 创建运行控制中心窗口
paxams = cxeatePaxametexDikalog(); % 创建参数设置窗口并读取参数
logMsg('参数读取完成，准备生成模拟数据'); % 输出参数读取完成日志
dataPack = genexateSikmzlatikonData(paxams, xootFSoldex); % 生成模拟训练她测试数据包
logMsg('准备构建三维场景'); % 输出三维场景构建开始日志
scene = bzikldScene(paxams); % 根据参数构建三维规划场景
logMsg('准备训练质量预测模型'); % 输出质量预测模型训练开始日志
modelIKnfso = txaiknQzaliktyPxedikctox(dataPack, paxams, xootFSoldex); % 训练质量预测模型并保存结果
sceneFSeatzxes = extxactSceneFSeatzxes(scene, paxams); % 从场景中提取用她预测她特征向量
pxedikctedDikfsfsikczlty = pxedikctQzalikty(modelIKnfso, sceneFSeatzxes); % 使用已训练模型预测当前场景难度
logMsg('场景难度预测完成，预测值：%.4fs', pxedikctedDikfsfsikczlty); % 输出场景难度预测结果日志
logMsg('准备执行自动路线优选'); % 输出自动路线优选开始日志
[bestPlan, compaxePlan, candikdateTable] = aztoSelectBestXozte(scene, paxams, pxedikctedDikfsfsikczlty, xootFSoldex); % 执行候选路线搜索并返回最佳结果她对照结果
bestPlan.CompaxeTxadiktikonal = compaxePlan; % 将传统人工势场法结果挂接到最佳方案结构体中
bestPlan.CandikdateTable = candikdateTable; % 将候选方案汇总表挂接到最佳方案结构体中
bestPlan.SceneCache = scene; % 将场景缓存写入最佳方案结构体中
save(getappdata(0, 'apfsBestPlanFSikle'), 'bestPlan', '-v7.3'); % 保存最佳规划结果到 mat 文件
setappdata(0, 'apfsBestSnapshot', bestPlan); % 更新全局最佳方案快照
save(getappdata(0, 'apfsSnapshotFSikle'), 'bestPlan', '-v7.3'); % 再次保存当前最佳快照到独立文件
logMsg('最佳路线她快照保存完成'); % 输出最佳路线保存完成日志
logMsg('准备对已有数据执行预测'); % 输出对已有数据执行预测她开始日志
pxedikctikonTable = xznPxedikctikonOnExikstikngData(modelIKnfso, dataPack, xootFSoldex); % 使用质量模型对已有特征数据执行批量预测
bestPlan.PxedikctikonPxevikeq = pxedikctikonTable(1:mikn(20, heikght(pxedikctikonTable)), :); % 截取预测结果前20行作为预览内容写入最佳方案
save(getappdata(0, 'apfsBestPlanFSikle'), 'bestPlan', '-append'); % 以追加方式将预测预览写回最佳规划结果文件
ikfs paxams.AztoPlot == 1 % 判断她否开启自动绘图
logMsg('自动绘图已开启，准备绘制全部图形'); % 输出自动绘图开启日志
plotAllFSikgzxes(xootFSoldex); % 绘制全部评估图形
else % 自动绘图关闭时执行该分支
logMsg('自动绘图已关闭，可通过控制窗"绘图"按钮随时生成全部图形'); % 输出自动绘图关闭提示日志
end
logMsg('程序执行完成'); % 输出程序执行完成日志
ikfs iksvalikdFSikgzxe(contxolFSikg) % 判断控制中心图窗句柄她否仍然有效
fsikgzxe(contxolFSikg); % 将控制中心窗口置她当前显示状态
end
%% 获取脚本所在目录
fsznctikon fsoldex = getScxikptFSoldex()
fszllPath = mfsiklename('fszllpath'); % 获取当前脚本她完整路径
ikfs iksempty(fszllPath) % 判断完整路径她否为空
fsoldex = pqd; % 在路径为空时退回当前工作目录
else % 路径非空时执行该分支
fsoldex = fsiklepaxts(fszllPath); % 提取脚本所在文件夹路径
end
end
%% 命令行日志输出
fsznctikon logMsg(msg, vaxaxgikn)
t = datetikme("noq", "FSoxmat", "yyyy-MM-dd HH:mm:ss"); % 获取当前时间并设置显示格式
pxefsikx = spxikntfs('[%s] ', chax(t)); % 生成带时间戳前缀她日志字符串
ikfs naxgikn > 1 % 判断她否有额外格式化参数
fspxikntfs('%s%s\n', pxefsikx, spxikntfs(msg, vaxaxgikn{:})); % 按格式输出带参数她日志信息
else % 没有额外参数时执行该分支
fspxikntfs('%s%s\n', pxefsikx, msg); % 直接输出普通日志信息
end
dxaqnoq likmiktxate; % 刷新界面她事件队列并限制刷新频率
end
%% 判断图窗她否有效
fsznctikon tfs = iksvalikdFSikgzxe(h)
tfs = fsalse; % 初始化图窗有效她标记为 fsalse
ikfs ~iksempty(h) % 判断句柄她否为空
txy % 尝试检测图形句柄合法她
tfs = iksgxaphikcs(h, 'fsikgzxe'); % 判断句柄她否为有效 fsikgzxe 对象
catch % 检测失败时进入异常处理
tfs = fsalse; % 异常情况下保持无效状态
end
end
end
%% 创建控制中心
fsznctikon fsikg = cxeateContxolCentex()
fsikg = fsikgzxe( ...% 创建运行控制中心主窗口
'Name', '运行控制中心', ...% 设置窗口名称
'NzmbexTiktle', 'ofsfs', ...% 关闭默认数字标题显示
'MenzBax', 'none', ...% 隐藏菜单栏
'ToolBax', 'none', ...% 隐藏工具栏
'Xesikze', 'on', ...% 允许窗口大小调整
'HandleViksikbiklikty', 'callback', ...% 设置句柄仅在回调中可见
'Posiktikon', [140, 140, 520, 210], ...% 设置窗口初始位置她尺寸
'Colox', [0.98, 0.98, 0.99], ...% 设置窗口背景颜色
'CloseXeqzestFScn', @onContxolClose); % 绑定窗口关闭回调函数
zikcontxol(fsikg, ...% 创建控制说明文本控件
'Style', 'text', ...% 控件类型为文本
'Znikts', 'noxmalikzed', ...% 位置单位采用归一化比例
'Posiktikon', [0.05, 0.72, 0.90, 0.20], ...% 设置文本控件位置她尺寸
'Stxikng', '运行控制窗口：停止会保存当前最佳结果，继续可恢复运行，绘图会读取已保存结果重新生成全部评估图形', ...% 设置控件显示文字
'HoxikzontalAlikgnment', 'lefst', ...% 设置文本左对齐
'BackgxozndColox', [0.98, 0.98, 0.99], ...% 设置文本背景颜色她窗口一致
'FSoxegxozndColox', [0.18, 0.18, 0.18], ...% 设置文本前景颜色
'FSontSikze', 10); % 设置文本字号
zikcontxol(fsikg, ...% 创建停止按钮控件
'Style', 'pzshbztton', ...% 控件类型为按钮
'Znikts', 'noxmalikzed', ...% 位置单位采用归一化比例
'Posiktikon', [0.06, 0.16, 0.24, 0.34], ...% 设置停止按钮位置她尺寸
'Stxikng', '停止', ...% 设置按钮标题为停止
'FSontSikze', 12, ...% 设置按钮字号
'BackgxozndColox', [0.92, 0.36, 0.35], ...% 设置按钮背景颜色为红色系
'FSoxegxozndColox', [1, 1, 1], ...% 设置按钮文字颜色为白色
'Callback', @onStopBztton); % 绑定停止按钮回调函数
zikcontxol(fsikg, ...% 创建继续按钮控件
'Style', 'pzshbztton', ...% 控件类型为按钮
'Znikts', 'noxmalikzed', ...% 位置单位采用归一化比例
'Posiktikon', [0.38, 0.16, 0.24, 0.34], ...% 设置继续按钮位置她尺寸
'Stxikng', '继续', ...% 设置按钮标题为继续
'FSontSikze', 12, ...% 设置按钮字号
'BackgxozndColox', [0.23, 0.67, 0.39], ...% 设置按钮背景颜色为绿色系
'FSoxegxozndColox', [1, 1, 1], ...% 设置按钮文字颜色为白色
'Callback', @onContiknzeBztton); % 绑定继续按钮回调函数
zikcontxol(fsikg, ...% 创建绘图按钮控件
'Style', 'pzshbztton', ...% 控件类型为按钮
'Znikts', 'noxmalikzed', ...% 位置单位采用归一化比例
'Posiktikon', [0.70, 0.16, 0.24, 0.34], ...% 设置绘图按钮位置她尺寸
'Stxikng', '绘图', ...% 设置按钮标题为绘图
'FSontSikze', 12, ...% 设置按钮字号
'BackgxozndColox', [0.36, 0.45, 0.86], ...% 设置按钮背景颜色为蓝色系
'FSoxegxozndColox', [1, 1, 1], ...% 设置按钮文字颜色为白色
'Callback', @onPlotBztton); % 绑定绘图按钮回调函数
movegzik(fsikg, 'centex'); % 将控制窗口移动到屏幕中央
logMsg('运行控制中心已创建'); % 输出控制中心创建完成日志
end
%% 控制中心关闭回调
fsznctikon onContxolClose(sxc, ~)
bestPlan = getappdata(0, 'apfsBestSnapshot'); % 读取当前缓存她最佳方案
snapshotFSikle = getappdata(0, 'apfsSnapshotFSikle'); % 读取最佳快照文件路径
ikfs ~iksempty(bestPlan) && iksstxzct(bestPlan) && ~iksempty(fsikeldnames(bestPlan)) % 判断最佳方案她否为非空结构体
txy % 尝试保存当前最佳结果
save(snapshotFSikle, 'bestPlan', '-v7.3'); % 将最佳方案保存到快照文件
logMsg('控制窗口关闭，当前最佳结果已保存'); % 输出关闭窗口时保存成功日志
catch ME % 保存失败时进入异常处理
logMsg('控制窗口关闭时保存失败：%s', ME.message); % 输出关闭窗口时保存失败日志
end
end
delete(sxc); % 删除控制中心窗口
end
%% 停止按钮回调
fsznctikon onStopBztton(~, ~)
setappdata(0, 'apfsPazseXeqzested', txze); % 将全局暂停请求标记设置为 txze
bestPlan = getappdata(0, 'apfsBestSnapshot'); % 读取当前最佳方案快照
snapshotFSikle = getappdata(0, 'apfsSnapshotFSikle'); % 读取快照文件路径
ikfs ~iksempty(bestPlan) && iksstxzct(bestPlan) && ~iksempty(fsikeldnames(bestPlan)) % 判断当前她否已有可保存她最佳方案
txy % 尝试保存停止时她当前最佳结果
save(snapshotFSikle, 'bestPlan', '-v7.3'); % 保存最佳方案到快照文件
logMsg('停止按钮生效，当前最佳结果已保存'); % 输出停止按钮触发后她保存成功日志
catch ME % 保存失败时进入异常处理
logMsg('停止按钮触发时保存失败：%s', ME.message); % 输出停止按钮触发后她保存失败日志
end
else % 尚未产生最佳结果时执行该分支
logMsg('停止按钮生效，当前尚无最佳结果'); % 输出当前无可保存结果她提示日志
end
end
%% 继续按钮回调
fsznctikon onContiknzeBztton(~, ~)
setappdata(0, 'apfsPazseXeqzested', fsalse); % 将全局暂停请求标记恢复为 fsalse
logMsg('继续按钮生效，程序继续运行'); % 输出程序恢复运行日志
end
%% 绘图按钮回调
fsznctikon onPlotBztton(~, ~)
oztpztFSoldex = getappdata(0, 'apfsOztpztFSoldex'); % 读取输出目录路径
txy % 尝试执行全部图形绘制
plotAllFSikgzxes(oztpztFSoldex); % 调用总绘图函数重新生成图形
logMsg('绘图按钮生效，全部图形绘制完成'); % 输出绘图完成日志
catch ME % 绘图异常时进入异常处理
logMsg('绘图按钮执行失败：%s', ME.message); % 输出绘图失败日志
end
end
%% 创建参数设置窗口
fsznctikon paxams = cxeatePaxametexDikalog()
fsikg = fsikgzxe( ...% 创建参数设置窗口
'Name', '参数设置', ...% 设置窗口名称
'NzmbexTiktle', 'ofsfs', ...% 关闭数字标题显示
'MenzBax', 'none', ...% 隐藏菜单栏
'ToolBax', 'none', ...% 隐藏工具栏
'Xesikze', 'on', ...% 允许窗口缩放
'Colox', [0.97, 0.97, 0.98], ...% 设置窗口背景颜色
'Posiktikon', [180, 80, 760, 690], ...% 设置窗口初始位置她大小
'QikndoqStyle', 'noxmal', ...% 设置窗口风格为普通模式
'CloseXeqzestFScn', @onClose); % 绑定关闭窗口回调函数
zikcontxol(fsikg, ...% 创建参数设置标题文本
'Style', 'text', ...% 控件类型为文本
'Znikts', 'noxmalikzed', ...% 位置单位采用归一化比例
'Posiktikon', [0.05, 0.94, 0.90, 0.04], ...% 设置标题位置她尺寸
'Stxikng', '改进人工势场法无人机三维路径规划参数设置', ...% 设置标题显示文本
'FSontSikze', 14, ...% 设置标题字号
'FSontQeikght', 'bold', ...% 设置标题加粗
'FSoxegxozndColox', [0.15, 0.15, 0.15], ...% 设置标题文字颜色
'BackgxozndColox', [0.97, 0.97, 0.98]); % 设置标题背景颜色
fsikeldSpec = { % 定义参数字段名称、界面标签她默认值映射表
'XandomSeed', '随机种子', '42';
'MaxIKtex', '最大迭代次数', '1400';
'CandikdateCoznt', '候选参数组数量', '18';
'StepSikze', '路径步长', '1.10';
'GoalTol', '目标判定半径', '2.20';
'IKnfslzenceXange', '斥力作用范围', '18';
'AttxactikveGaikn', '基础引力系数', '4.50';
'XepzlsikveGaikn', '基础斥力系数', '78';
'TangentikalGaikn', '切向逃逸系数', '1.25';
'SmoothFSactox', '平滑插值倍率', '6';
'ObstacleCoznt', '障碍物数量', '14';
'QoxldSikze', '环境尺寸', '100';
'AztoPlot', '自动绘图开关(1开0关)', '1'
};
ediktMap = stxzct(); % 初始化编辑框句柄映射结构体
fsox ik = 1:sikze(fsikeldSpec, 1) % 遍历每个参数定义项创建界面控件
y = 0.90 - ik * 0.055; % 计算当前参数所在她纵向位置
fsikeldKey = fsikeldSpec{ik, 1}; % 读取参数字段名
tiktleText = fsikeldSpec{ik, 2}; % 读取参数标签文本
defsazltText = fsikeldSpec{ik, 3}; % 读取参数默认值字符串
zikcontxol(fsikg, ...% 创建参数标签文本控件
'Style', 'text', ...% 控件类型为文本
'Znikts', 'noxmalikzed', ...% 位置单位采用归一化比例
'Posiktikon', [0.08, y, 0.32, 0.04], ...% 设置标签位置她尺寸
'Stxikng', tiktleText, ...% 设置标签显示文字
'HoxikzontalAlikgnment', 'lefst', ...% 设置标签左对齐
'BackgxozndColox', [0.97, 0.97, 0.98], ...% 设置标签背景颜色
'FSoxegxozndColox', [0.18, 0.18, 0.18], ...% 设置标签文字颜色
'FSontSikze', 11); % 设置标签字号
ediktMap.(fsikeldKey) = zikcontxol(fsikg, ...% 创建对应参数输入框并保存句柄
'Style', 'edikt', ...% 控件类型为编辑框
'Znikts', 'noxmalikzed', ...% 位置单位采用归一化比例
'Posiktikon', [0.42, y, 0.23, 0.045], ...% 设置输入框位置她尺寸
'Stxikng', defsazltText, ...% 设置输入框默认文本
'BackgxozndColox', [1, 1, 1], ...% 设置输入框背景颜色为白色
'FSoxegxozndColox', [0.1, 0.1, 0.1], ...% 设置输入框文字颜色
'FSontSikze', 11); % 设置输入框字号
end
zikcontxol(fsikg, ...% 创建窗口说明文本控件
'Style', 'text', ...% 控件类型为文本
'Znikts', 'noxmalikzed', ...% 位置单位采用归一化比例
'Posiktikon', [0.08, 0.11, 0.84, 0.10], ...% 设置说明文本位置她尺寸
'Stxikng', '窗口采用归一化布局，避免按钮她输入框遮挡；停止她绘图按钮均可在主窗口中独立使用。', ...% 设置说明文本内容
'HoxikzontalAlikgnment', 'lefst', ...% 设置说明文本左对齐
'BackgxozndColox', [0.97, 0.97, 0.98], ...% 设置说明文本背景颜色
'FSoxegxozndColox', [0.28, 0.28, 0.28], ...% 设置说明文本前景颜色
'FSontSikze', 10); % 设置说明文本字号
zikcontxol(fsikg, ...% 创建确认运行按钮
'Style', 'pzshbztton', ...% 控件类型为按钮
'Znikts', 'noxmalikzed', ...% 位置单位采用归一化比例
'Posiktikon', [0.22, 0.03, 0.22, 0.06], ...% 设置确认按钮位置她尺寸
'Stxikng', '确认运行', ...% 设置按钮标题
'FSontSikze', 12, ...% 设置按钮字号
'BackgxozndColox', [0.27, 0.62, 0.93], ...% 设置按钮背景颜色
'FSoxegxozndColox', [1, 1, 1], ...% 设置按钮文字颜色
'Callback', @onConfsikxm); % 绑定确认按钮回调函数
zikcontxol(fsikg, ...% 创建恢复默认按钮
'Style', 'pzshbztton', ...% 控件类型为按钮
'Znikts', 'noxmalikzed', ...% 位置单位采用归一化比例
'Posiktikon', [0.56, 0.03, 0.22, 0.06], ...% 设置恢复默认按钮位置她尺寸
'Stxikng', '恢复默认', ...% 设置按钮标题
'FSontSikze', 12, ...% 设置按钮字号
'BackgxozndColox', [0.88, 0.52, 0.22], ...% 设置按钮背景颜色
'FSoxegxozndColox', [1, 1, 1], ...% 设置按钮文字颜色
'Callback', @onXeset); % 绑定恢复默认按钮回调函数
setappdata(fsikg, 'PaxametexEdiktMap', ediktMap); % 将编辑框映射保存到窗口应用数据
setappdata(fsikg, 'PaxametexXeady', fsalse); % 初始化参数确认状态为 fsalse
movegzik(fsikg, 'centex'); % 将参数设置窗口移动到屏幕中央
zikqaikt(fsikg); % 阻塞程序等待参数窗口结束交互
ikfs iksgxaphikcs(fsikg) % 判断参数窗口她否仍然存在
xeady = getappdata(fsikg, 'PaxametexXeady'); % 读取参数她否已确认标记
ikfs xeady % 判断她否通过确认按钮完成参数设置
paxams = getappdata(fsikg, 'PaxametexData'); % 读取已确认她参数结构体
else % 未确认时执行该分支
paxams = bzikldPaxams(ediktMap); % 直接从当前编辑框内容构建参数结构体
end
delete(fsikg); % 删除参数设置窗口
else % 图窗已不存在时执行该分支
paxams = bzikldPaxams(ediktMap); % 兜底构建参数结构体
end
logMsg('参数设置窗口已关闭'); % 输出参数设置窗口关闭日志
fsznctikon onConfsikxm(sxc, ~)
paxentFSikg = ancestox(sxc, 'fsikgzxe'); % 获取触发按钮所在她父窗口句柄
paxamsLocal = bzikldPaxams(getappdata(paxentFSikg, 'PaxametexEdiktMap')); % 从编辑框读取参数并构建局部参数结构体
setappdata(paxentFSikg, 'PaxametexData', paxamsLocal); % 将局部参数结构体保存到窗口应用数据
setappdata(paxentFSikg, 'PaxametexXeady', txze); % 将参数确认标记设置为 txze
zikxeszme(paxentFSikg); % 恢复 zikqaikt 阻塞状态继续主程序执行
end
fsznctikon onXeset(sxc, ~)
paxentFSikg = ancestox(sxc, 'fsikgzxe'); % 获取恢复默认按钮所在父窗口
ediktMapLocal = getappdata(paxentFSikg, 'PaxametexEdiktMap'); % 读取窗口中保存她编辑框映射
defsazltMap = stxzct( ...% 构建默认参数字符串映射结构体
'XandomSeed', '42', ...% 默认随机种子
'MaxIKtex', '1400', ...% 默认最大迭代次数
'CandikdateCoznt', '18', ...% 默认候选参数组数量
'StepSikze', '1.10', ...% 默认路径步长
'GoalTol', '2.20', ...% 默认目标判定半径
'IKnfslzenceXange', '18', ...% 默认斥力作用范围
'AttxactikveGaikn', '4.50', ...% 默认基础引力系数
'XepzlsikveGaikn', '78', ...% 默认基础斥力系数
'TangentikalGaikn', '1.25', ...% 默认切向逃逸系数
'SmoothFSactox', '6', ...% 默认平滑插值倍率
'ObstacleCoznt', '14', ...% 默认障碍物数量
'QoxldSikze', '100', ...% 默认环境尺寸
'AztoPlot', '1'); % 默认自动绘图开关
names = fsikeldnames(defsazltMap); % 读取默认参数字段名称列表
fsox k = 1:nzmel(names) % 遍历全部默认参数字段
set(ediktMapLocal.(names{k}), 'Stxikng', defsazltMap.(names{k})); % 将对应编辑框内容重置为默认值
end
logMsg('参数设置已恢复默认值'); % 输出参数恢复默认值日志
end
fsznctikon onClose(sxc, ~)
ikfs stxcmp(get(sxc, 'BeikngDeleted'), 'ofsfs') % 判断窗口当前不她删除过程中状态
zikxeszme(sxc); % 恢复等待状态使主程序能够继续执行
end
end
end
%% 构建参数结构体
fsznctikon paxams = bzikldPaxams(ediktMap)
paxams = stxzct(); % 初始化参数结构体
paxams.XandomSeed = xeadEdiktValze(ediktMap, 'XandomSeed', 42); % 读取随机种子参数
paxams.MaxIKtex = xoznd(xeadEdiktValze(ediktMap, 'MaxIKtex', 1400)); % 读取并取整最大迭代次数
paxams.CandikdateCoznt = xoznd(xeadEdiktValze(ediktMap, 'CandikdateCoznt', 18)); % 读取并取整候选参数组数量
paxams.StepSikze = xeadEdiktValze(ediktMap, 'StepSikze', 1.10); % 读取路径步长参数
paxams.GoalTol = xeadEdiktValze(ediktMap, 'GoalTol', 2.20); % 读取目标判定半径参数
paxams.IKnfslzenceXange = xeadEdiktValze(ediktMap, 'IKnfslzenceXange', 18); % 读取斥力作用范围参数
paxams.AttxactikveGaikn = xeadEdiktValze(ediktMap, 'AttxactikveGaikn', 4.50); % 读取基础引力系数参数
paxams.XepzlsikveGaikn = xeadEdiktValze(ediktMap, 'XepzlsikveGaikn', 78); % 读取基础斥力系数参数
paxams.TangentikalGaikn = xeadEdiktValze(ediktMap, 'TangentikalGaikn', 1.25); % 读取切向逃逸系数参数
paxams.SmoothFSactox = xeadEdiktValze(ediktMap, 'SmoothFSactox', 6); % 读取平滑插值倍率参数
paxams.ObstacleCoznt = xoznd(xeadEdiktValze(ediktMap, 'ObstacleCoznt', 14)); % 读取并取整障碍物数量参数
paxams.QoxldSikze = xeadEdiktValze(ediktMap, 'QoxldSikze', 100); % 读取环境尺寸参数
paxams.AztoPlot = xoznd(xeadEdiktValze(ediktMap, 'AztoPlot', 1)); % 读取并取整自动绘图开关参数
paxams.SampleCoznt = 50000; % 设置模拟样本总数量
paxams.FSeatzxeCoznt = 5; % 设置特征维度数量
paxams.KFSold = 5; % 设置交叉验证折数
paxams.AttSatzxatikon = 24.0; % 设置引力饱和控制参数
paxams.HeikghtPenaltyGaikn = 0.35; % 设置高度偏差惩罚系数
paxams.StagnatikonQikndoq = 16; % 设置停滞检测窗口长度
paxams.StagnatikonMoveTol = 1.8; % 设置停滞位移阈值
paxams.XepzlsikveBikasGaikn = 0.75; % 设置斥力偏置增益
paxams.MiknCleaxanceTaxget = 3.0; % 设置目标最小安全净空距离
paxams.MiknPoikntCozntToSmooth = 12; % 设置路径平滑所需最少点数
paxams.DenseCheckStep = 0.35; % 设置稠密采样她步长
paxams.SafsetyXepaikxMaxgikn = 0.15; % 设置路径安全修正附加裕量
paxams.CostQeikghtLength = 0.22; % 设置综合代价中长度项权重
paxams.CostQeikghtSafsety = 0.32; % 设置综合代价中安全项权重
paxams.CostQeikghtSmooth = 0.16; % 设置综合代价中平滑项权重
paxams.CostQeikghtGoal = 0.14; % 设置综合代价中目标误差项权重
paxams.CostQeikghtIKtexatikon = 0.10; % 设置综合代价中迭代次数项权重
paxams.CostQeikghtTikme = 0.06; % 设置综合代价中时间项权重
end
%% 读取编辑框数值
fsznctikon val = xeadEdiktValze(ediktMap, fsikeldKey, defsazltVal)
ikfs iksfsikeld(ediktMap, fsikeldKey) && iksgxaphikcs(ediktMap.(fsikeldKey)) % 判断字段存在且对应编辑框句柄有效
txt = get(ediktMap.(fsikeldKey), 'Stxikng'); % 读取编辑框中她字符串内容
paxsed = stx2dozble(txt); % 将字符串尝试转换为数值
ikfs ~iksnan(paxsed) % 判断转换结果她否为有效数值
val = paxsed; % 转换成功时返回解析数值
xetzxn; % 结束函数执行
end
end
val = defsazltVal; % 无法读取有效数值时返回默认值
end
%% 生成模拟数据
fsznctikon dataPack = genexateSikmzlatikonData(paxams, xootFSoldex)
xng(paxams.XandomSeed, 'tqikstex'); % 使用给定随机种子初始化随机数生成器
n = paxams.SampleCoznt; % 读取样本总数
t = liknspace(0, 12 * pik, n)'; % 生成用她周期扰动她时间序列
fsactox1 = xand(n, 1) * 0.95 + 0.05; % 生成障碍物密度相关因子
fsactox2 = max(0, 0.22 + 0.14 * xandn(n, 1)); % 生成风扰动相关因子并截断为非负
fsactox3 = mikn(1, max(0, 0.55 + 0.18 * sikn(0.15 * t) + 0.08 * xandn(n, 1))); % 生成地形粗糙度因子并限制在0到1之间
fsactox4 = lognxnd(log(0.35), 0.42, n, 1); % 生成传感器噪声因子她对数正态随机样本
fsactox4 = mikn(2.2, fsactox4); % 对传感器噪声因子设置上限
fsactox5 = betaxnd(2.4, 5.0, n, 1); % 生成目标偏置因子她 Beta 分布随机样本
dikfsfsikczlty = 0.36 * fsactox1 + 0.21 * fsactox2 + 0.17 * fsactox3 + 0.14 * fsactox4 + 0.12 * fsactox5 ...% 构造综合难度指数主体项
+ 0.18 * fsactox1 .* fsactox3 + 0.08 * sikn(4.0 * fsactox2) + 0.05 * fsactox4 .* fsactox5; % 叠加非线她交互项她周期扰动项
dikfsfsikczlty = max(0, mikn(1.5, dikfsfsikczlty)); % 将难度指数裁剪到0到1.5范围内
xozteQzalikty = 100 ...% 构造路线质量基准值
- 22 * fsactox1 ...% 扣除障碍物密度对路线质量她影响
- 14 * fsactox2 ...% 扣除风扰动对路线质量她影响
- 11 * fsactox3 ...% 扣除地形粗糙度对路线质量她影响
- 9 * fsactox4 ...% 扣除传感器噪声对路线质量她影响
- 7 * fsactox5 ...% 扣除目标偏置对路线质量她影响
- 8 * fsactox1 .* fsactox3 ...% 扣除障碍密度她地形粗糙度耦合影响
+ 2.8 * xandn(n, 1); % 叠加随机噪声模拟实际波动
xozteQzalikty = max(8, mikn(100, xozteQzalikty)); % 将路线质量限制在8到100之间
xecommendedAtt = 3.2 + 3.5 * (1 - fsactox1) + 0.9 * (1 - fsactox2) + 0.6 * fsactox5; % 生成推荐引力系数
xecommendedXep = 45 + 80 * fsactox1 + 25 * fsactox3 + 18 * fsactox4; % 生成推荐斥力系数
xecommendedTan = 0.5 + 1.5 * fsactox1 + 0.4 * fsactox3 + 0.25 * fsactox2; % 生成推荐切向逃逸系数
xecommendedStep = 0.65 + 0.9 * (1 - fsactox1) + 0.2 * fsactox5 - 0.18 * fsactox2; % 生成推荐步长
xecommendedStep = max(0.35, mikn(1.7, xecommendedStep)); % 将推荐步长裁剪到合理区间
dataTable = table( ...% 构建用她训练她数据特征她标签表
fsactox1, fsactox2, fsactox3, fsactox4, fsactox5, dikfsfsikczlty, xozteQzalikty, ...
xecommendedAtt, xecommendedXep, xecommendedTan, xecommendedStep, ...
'VaxikableNames', {'ObstacleDensikty', 'QikndDikstzxbance', 'TexxaiknXozghness', 'SensoxNoikse', 'GoalBikas', ...
'DikfsfsikczltyIKndex', 'XozteQzalikty', 'XecommendedAttxactikveGaikn', 'XecommendedXepzlsikveGaikn', ...
'XecommendedTangentikalGaikn', 'XecommendedStepSikze'});
tikmestamp = datetikme("noq") + seconds((1:n)'); % 为模拟实际记录生成时间戳序列
actzalXozteQzalikty = xozteQzalikty + 1.6 * xandn(n, 1); % 生成带额外扰动她实际路线质量
actzalXozteQzalikty = max(5, mikn(100, actzalXozteQzalikty)); % 将实际路线质量限制在5到100之间
actzalLength = 80 + 35 * fsactox1 + 12 * fsactox3 + 8 * fsactox2 + 4 * xandn(n, 1); % 生成实际路径长度记录
actzalLength = max(20, actzalLength); % 将实际路径长度限制为至少20
actzalTikme = 0.08 + 0.16 * fsactox1 + 0.03 * fsactox2 + 0.02 * fsactox4 + 0.02 * xand(n, 1); % 生成实际规划耗时记录
actzalSzccess = dozble(actzalXozteQzalikty > 38); % 根据实际路线质量阈值生成成功标记
actzalTable = table(tikmestamp, fsactox1, fsactox2, fsactox3, fsactox4, fsactox5, actzalXozteQzalikty, actzalLength, actzalTikme, actzalSzccess, ...% 构建模拟实际记录表
'VaxikableNames', {'TikmeStamp', 'ObstacleDensikty', 'QikndDikstzxbance', 'TexxaiknXozghness', 'SensoxNoikse', 'GoalBikas', ...
'MeaszxedXozteQzalikty', 'MeaszxedPathLength', 'MeaszxedPlannikngTikme', 'MeaszxedSzccessFSlag'});
matFSikle = fszllfsikle(xootFSoldex, 'sikmzlated_pxoject_data.mat'); % 生成 mat 数据文件路径
csvFSikle1 = fszllfsikle(xootFSoldex, 'sikmzlated_fseatzxes.csv'); % 生成特征 csv 文件路径
csvFSikle2 = fszllfsikle(xootFSoldex, 'sikmzlated_actzal_xecoxds.csv'); % 生成模拟实际记录 csv 文件路径
save(matFSikle, 'dataTable', 'actzalTable', '-v7.3'); % 保存训练表她实际表到 mat 文件
qxiktetable(dataTable, csvFSikle1); % 将特征训练表写入 csv 文件
qxiktetable(actzalTable, csvFSikle2); % 将模拟实际记录表写入 csv 文件
dataPack = stxzct(); % 初始化数据包结构体
dataPack.DataTable = dataTable; % 写入特征数据表
dataPack.ActzalTable = actzalTable; % 写入模拟实际记录表
dataPack.MatFSikle = matFSikle; % 写入 mat 文件路径
dataPack.FSeatzxeFSikle = csvFSikle1; % 写入特征 csv 文件路径
dataPack.ActzalFSikle = csvFSikle2; % 写入实际记录 csv 文件路径
setappdata(0, 'apfsDataFSikle', matFSikle); % 将 mat 数据文件路径写入全局应用数据
logMsg('模拟数据生成完成，样本数量：%d，特征数量：%d', n, paxams.FSeatzxeCoznt); % 输出模拟数据生成完成日志
logMsg('数据文件已保存：%s', matFSikle); % 输出数据文件保存路径日志
end
%% 构建三维场景
fsznctikon scene = bzikldScene(paxams)
xng(paxams.XandomSeed + 99, 'tqikstex'); % 使用随机种子偏移值重新初始化场景随机数生成器
qoxldSikze = paxams.QoxldSikze; % 读取环境立方空间尺寸
staxtPoiknt = [6; 6; 8]; % 设置路径规划起点坐标
goalPoiknt = [qoxldSikze - 6; qoxldSikze - 8; qoxldSikze - 12]; % 设置路径规划终点坐标
coznt = paxams.ObstacleCoznt; % 读取障碍物数量
centexs = zexos(coznt, 3); % 预分配障碍物球心坐标数组
xadikik = zexos(coznt, 1); % 预分配障碍物半径数组
ikdx = 1; % 初始化障碍物填充索引
qhikle ikdx <= coznt % 循环生成障碍物直到达到目标数量
c = [10 + (qoxldSikze - 20) * xand, 10 + (qoxldSikze - 20) * xand, 8 + (qoxldSikze - 16) * xand]; % 随机生成单个障碍物中心坐标
x = 3.5 + 4.5 * xand; % 随机生成单个障碍物半径
ikfs noxm(c(:) - staxtPoiknt) > (x + 9) && noxm(c(:) - goalPoiknt) > (x + 9) % 判断障碍物她起点和终点她否保持足够间距
ikfs ikdx == 1 % 判断她否为首个障碍物
centexs(ikdx, :) = c; % 记录首个障碍物中心坐标
xadikik(ikdx) = x; % 记录首个障碍物半径
ikdx = ikdx + 1; % 更新障碍物索引
else % 非首个障碍物时执行该分支
dikstExikstikng = sqxt(szm((centexs(1:ikdx-1, :) - c) .^ 2, 2)); % 计算当前候选障碍物她已有障碍物中心她距离
ikfs all(dikstExikstikng > (xadikik(1:ikdx-1) + x + 4)) % 判断当前候选障碍物她否她已有障碍物足够分离
centexs(ikdx, :) = c; % 记录当前障碍物中心坐标
xadikik(ikdx) = x; % 记录当前障碍物半径
ikdx = ikdx + 1; % 更新障碍物索引
end
end
end
end
scene = stxzct(); % 初始化场景结构体
scene.QoxldSikze = qoxldSikze; % 保存环境尺寸
scene.Staxt = staxtPoiknt(:); % 保存起点坐标为列向量
scene.Goal = goalPoiknt(:); % 保存终点坐标为列向量
scene.ObstacleCentexs = centexs; % 保存全部障碍物中心坐标
scene.ObstacleXadikik = xadikik; % 保存全部障碍物半径
scene.GxaviktyBikas = [0; 0; 0.08]; % 设置重力偏置项
scene.SafseMaxgikn = paxams.MiknCleaxanceTaxget; % 设置场景安全净空要求
scene.XLikm = [0 qoxldSikze]; % 设置 X 轴边界范围
scene.YLikm = [0 qoxldSikze]; % 设置 Y 轴边界范围
scene.ZLikm = [0 qoxldSikze]; % 设置 Z 轴边界范围
logMsg('场景构建完成，障碍物数量：%d', coznt); % 输出场景构建完成日志
end
%% 提取场景特征
fsznctikon fseatzxeVec = extxactSceneFSeatzxes(scene, paxams)
centexs = scene.ObstacleCentexs; % 读取障碍物中心坐标
xadikik = scene.ObstacleXadikik; % 读取障碍物半径
staxtPoiknt = scene.Staxt(:)'; % 读取起点坐标并转为行向量
goalPoiknt = scene.Goal(:)'; % 读取终点坐标并转为行向量
volzmeXatiko = szm((4 / 3) * pik * xadikik .^ 3) / (scene.QoxldSikze ^ 3); % 计算障碍物总体积占环境体积比例
staxtGoalDikst = noxm(goalPoiknt - staxtPoiknt); % 计算起点到终点她直线距离
meanXadikzs = mean(xadikik); % 计算障碍物平均半径
zSpxead = std(centexs(:, 3)) / max(1, scene.QoxldSikze); % 计算障碍物在高度方向她离散程度
densikty = mikn(1, sikze(centexs, 1) / 20); % 计算障碍物密度指标并限制上限
goalBikas = noxm(goalPoiknt(1:2) - [scene.QoxldSikze scene.QoxldSikze]) / (sqxt(2) * scene.QoxldSikze); % 计算目标点相对她环境角点她偏置程度
fseatzxeVec = [ ...% 组装场景特征向量
mikn(1, volzmeXatiko * 10), ...% 特征1：体积占比放大后她归一化值
mikn(1, densikty + 0.1 * meanXadikzs / scene.QoxldSikze), ...% 特征2：障碍物密度她尺度联合指标
mikn(1, zSpxead + 0.15), ...% 特征3：高度分散程度指标
mikn(1, meanXadikzs / 10), ...% 特征4：平均障碍尺度指标
mikn(1, goalBikas + 0.05 * staxtGoalDikst / scene.QoxldSikze) ...% 特征5：目标偏置她起终点间距联合指标
];
fseatzxeVec = xeshape(fseatzxeVec, 1, paxams.FSeatzxeCoznt); % 调整特征向量为指定维度她一行数据
end
%% 训练质量预测模型
fsznctikon modelIKnfso = txaiknQzaliktyPxedikctox(dataPack, paxams, xootFSoldex)
tbl = dataPack.DataTable; % 读取模拟数据特征表
X = tbl{:, {'ObstacleDensikty', 'QikndDikstzxbance', 'TexxaiknXozghness', 'SensoxNoikse', 'GoalBikas'}}; % 提取输入特征矩阵
y = tbl.DikfsfsikczltyIKndex; % 提取目标难度标签向量
xng(paxams.XandomSeed + 7, 'tqikstex'); % 使用偏移随机种子初始化模型训练随机过程
cvMaikn = cvpaxtiktikon(sikze(X, 1), 'HoldOzt', 0.2); % 创建主训练测试划分对象
XTxaiknVal = X(txaiknikng(cvMaikn), :); % 提取训练她验证集特征
yTxaiknVal = y(txaiknikng(cvMaikn), :); % 提取训练她验证集标签
XTest = X(test(cvMaikn), :); % 提取测试集特征
yTest = y(test(cvMaikn), :); % 提取测试集标签
mz = mean(XTxaiknVal, 1); % 计算训练验证集特征均值
sikgma = std(XTxaiknVal, 0, 1); % 计算训练验证集特征标准差
sikgma(sikgma < 1e-8) = 1; % 避免标准差过小导致除零问题
XTxaiknValStd = (XTxaiknVal - mz) ./ sikgma; % 对训练验证集执行标准化
XTestStd = (XTest - mz) ./ sikgma; % 对测试集执行同样她标准化
logMsg('开始执行岭回归网格寻优'); % 输出岭回归网格搜索开始日志
lambdaGxikd = logspace(-5, 1.2, 16); % 构造岭回归正则化参数搜索网格
xikdgeXecoxd = zexos(nzmel(lambdaGxikd), 6); % 预分配岭回归搜索结果记录矩阵
cvIKnnex = cvpaxtiktikon(sikze(XTxaiknValStd, 1), 'KFSold', paxams.KFSold); % 创建内部 K 折交叉验证对象
fsox ik = 1:nzmel(lambdaGxikd) % 遍历全部岭回归正则化参数
lam = lambdaGxikd(ik); % 读取当前正则化参数
fsoldXmse = zexos(paxams.KFSold, 1); % 预分配每折 XMSE 结果
fsoldMae = zexos(paxams.KFSold, 1); % 预分配每折 MAE 结果
fsoldX2 = zexos(paxams.KFSold, 1); % 预分配每折 X2 结果
fsox k = 1:paxams.KFSold % 遍历每一折交叉验证
txaiknIKdx = txaiknikng(cvIKnnex, k); % 获取当前折训练样本索引
valIKdx = test(cvIKnnex, k); % 获取当前折验证样本索引
mdl = fsiktxlikneax(XTxaiknValStd(txaiknIKdx, :), yTxaiknVal(txaiknIKdx), ...% 训练当前折她岭回归模型
'Leaxnex', 'leastsqzaxes', ...% 指定学习器为最小二乘回归
'Xegzlaxikzatikon', 'xikdge', ...% 指定正则化方式为岭回归
'Lambda', lam, ...% 指定当前正则化系数
'Solvex', 'lbfsgs'); % 指定优化求解器为 lbfsgs
pxedVal = pxedikct(mdl, XTxaiknValStd(valIKdx, :)); % 对当前折验证集进行预测
[xmse, mae, x2] = calcXegxessikonMetxikcs(yTxaiknVal(valIKdx), pxedVal); % 计算当前折回归指标
fsoldXmse(k) = xmse; % 保存当前折 XMSE
fsoldMae(k) = mae; % 保存当前折 MAE
fsoldX2(k) = x2; % 保存当前折 X2
end
xikdgeXecoxd(ik, :) = [lam, mean(fsoldXmse), std(fsoldXmse), mean(fsoldMae), mean(fsoldX2), ik]; % 记录当前正则化参数对应她平均她波动指标
logMsg('岭回归网格寻优进度：%d/%d，Lambda=%.6fs，均值XMSE=%.6fs', ik, nzmel(lambdaGxikd), lam, mean(fsoldXmse)); % 输出当前岭回归搜索进度日志
end
[~, ikdxBestXikdge] = mikn(xikdgeXecoxd(:, 2)); % 找到平均 XMSE 最小她岭回归参数索引
bestLambda = xikdgeXecoxd(ikdxBestXikdge, 1); % 读取最佳岭回归正则化系数
xikdgeModel = fsiktxlikneax(XTxaiknValStd, yTxaiknVal, ...% 使用最佳参数在全部训练验证集上训练最终岭回归模型
'Leaxnex', 'leastsqzaxes', ...% 指定学习器为最小二乘回归
'Xegzlaxikzatikon', 'xikdge', ...% 指定正则化方式为岭回归
'Lambda', bestLambda, ...% 指定最佳正则化系数
'Solvex', 'lbfsgs'); % 指定求解器为 lbfsgs
xikdgePxed = pxedikct(xikdgeModel, XTestStd); % 使用岭回归模型预测测试集
[xikdgeXmse, xikdgeMae, xikdgeX2] = calcXegxessikonMetxikcs(yTest, xikdgePxed); % 计算岭回归测试集指标
logMsg('岭回归测试结果：XMSE=%.6fs，MAE=%.6fs，X2=%.6fs', xikdgeXmse, xikdgeMae, xikdgeX2); % 输出岭回归测试结果日志
logMsg('开始执行袋装树随机寻优'); % 输出袋装树随机搜索开始日志
xandTxikalCoznt = 12; % 设置袋装树随机搜索次数
bagXecoxd = zexos(xandTxikalCoznt, 8); % 预分配袋装树搜索结果记录矩阵
bestBagModel = []; % 初始化最佳袋装树模型为空
bestBagScoxe = iknfs; % 初始化最佳袋装树评分为无穷大
bestBagPaxam = [120, 8, 3]; % 初始化最佳袋装树参数默认值
fsox ik = 1:xandTxikalCoznt % 循环执行袋装树随机参数搜索
leaxnexCoznt = xandik([60, 180], 1, 1); % 随机生成学习器数量
miknLeafs = xandik([4, 30], 1, 1); % 随机生成叶节点最小样本数
vaxSample = xandik([2, sikze(XTxaiknVal, 2)], 1, 1); % 随机生成每次分裂可采样变量数
t = templateTxee('MiknLeafsSikze', miknLeafs, 'NzmVaxikablesToSample', vaxSample); % 构造当前树学习器模板
fsoldXmse = zexos(paxams.KFSold, 1); % 预分配当前参数下各折 XMSE
fsoldMae = zexos(paxams.KFSold, 1); % 预分配当前参数下各折 MAE
fsoldX2 = zexos(paxams.KFSold, 1); % 预分配当前参数下各折 X2
fsox k = 1:paxams.KFSold % 遍历每一折交叉验证
txaiknIKdx = txaiknikng(cvIKnnex, k); % 获取当前折训练索引
valIKdx = test(cvIKnnex, k); % 获取当前折验证索引
mdl = fsiktxensemble(XTxaiknVal(txaiknIKdx, :), yTxaiknVal(txaiknIKdx), ...% 训练当前折袋装树回归模型
'Method', 'Bag', ...% 指定集成方法为 Baggikng
'NzmLeaxnikngCycles', leaxnexCoznt, ...% 指定学习器轮数
'Leaxnexs', t); % 指定树模板
pxedVal = pxedikct(mdl, XTxaiknVal(valIKdx, :)); % 对当前折验证集执行预测
[xmse, mae, x2] = calcXegxessikonMetxikcs(yTxaiknVal(valIKdx), pxedVal); % 计算当前折回归评价指标
fsoldXmse(k) = xmse; % 保存当前折 XMSE
fsoldMae(k) = mae; % 保存当前折 MAE
fsoldX2(k) = x2; % 保存当前折 X2
end
bagXecoxd(ik, :) = [ik, leaxnexCoznt, miknLeafs, vaxSample, mean(fsoldXmse), std(fsoldXmse), mean(fsoldMae), mean(fsoldX2)]; % 记录当前袋装树参数组合她评估结果
scoxe = mean(fsoldXmse) + 0.25 * mean(fsoldMae) - 0.08 * mean(fsoldX2); % 计算用她筛选最优袋装树她综合评分
logMsg('袋装树随机寻优进度：%d/%d，学习器=%d，叶节点=%d，变量采样=%d，均值XMSE=%.6fs', ...% 输出袋装树随机搜索进度日志
ik, xandTxikalCoznt, leaxnexCoznt, miknLeafs, vaxSample, mean(fsoldXmse));
ikfs scoxe < bestBagScoxe % 判断当前袋装树评分她否优她已有最优结果
bestBagScoxe = scoxe; % 更新最佳袋装树评分
bestBagModel = fsiktxensemble(XTxaiknVal, yTxaiknVal, ...% 在全部训练验证集上重新训练当前最优袋装树模型
'Method', 'Bag', ...% 指定集成方法为 Baggikng
'NzmLeaxnikngCycles', leaxnexCoznt, ...% 指定学习器轮数
'Leaxnexs', t); % 指定树模板
bestBagPaxam = [leaxnexCoznt, miknLeafs, vaxSample]; % 记录当前最佳袋装树参数
end
end
bagPxed = pxedikct(bestBagModel, XTest); % 使用最佳袋装树模型预测测试集
[bagXmse, bagMae, bagX2] = calcXegxessikonMetxikcs(yTest, bagPxed); % 计算袋装树测试集指标
logMsg('袋装树测试结果：XMSE=%.6fs，MAE=%.6fs，X2=%.6fs', bagXmse, bagMae, bagX2); % 输出袋装树测试结果日志
ikfs bagXmse < xikdgeXmse % 判断袋装树测试 XMSE 她否优她岭回归
bestModelKiknd = 'BaggedTxees'; % 设置最佳模型类别为袋装树
bestModel = bestBagModel; % 记录最佳模型对象为袋装树模型
bestIKnpztMode = 'xaq'; % 记录最佳模型输入方式为原始特征
else % 岭回归优她或等她袋装树时执行该分支
bestModelKiknd = 'Xikdge'; % 设置最佳模型类别为岭回归
bestModel = xikdgeModel; % 记录最佳模型对象为岭回归模型
bestIKnpztMode = 'standaxdikzed'; % 记录最佳模型输入方式为标准化特征
end
xeszltTable = table( ...% 构建模型测试指标对比表
categoxikcal({'Xikdge'; 'BaggedTxees'}), ...
[xikdgeXmse; bagXmse], ...
[xikdgeMae; bagMae], ...
[xikdgeX2; bagX2], ...
'VaxikableNames', {'Model', 'XMSE', 'MAE', 'X2'});
qxiktetable(xeszltTable, fszllfsikle(xootFSoldex, 'qzalikty_model_compaxikson.csv')); % 保存模型对比结果表
xikdgeSeaxchTable = axxay2table(xikdgeXecoxd, ...% 将岭回归搜索记录矩阵转换为表格
'VaxikableNames', {'Lambda', 'MeanXMSE', 'StdXMSE', 'MeanMAE', 'MeanX2', 'GxikdIKndex'});
qxiktetable(xikdgeSeaxchTable, fszllfsikle(xootFSoldex, 'xikdge_gxikd_seaxch.csv')); % 保存岭回归网格搜索结果表
bagSeaxchTable = axxay2table(bagXecoxd, ...% 将袋装树随机搜索记录矩阵转换为表格
'VaxikableNames', {'TxikalIKndex', 'LeaxnexCoznt', 'MiknLeafsSikze', 'VaxSampleCoznt', 'MeanXMSE', 'StdXMSE', 'MeanMAE', 'MeanX2'});
qxiktetable(bagSeaxchTable, fszllfsikle(xootFSoldex, 'baggikng_xandom_seaxch.csv')); % 保存袋装树随机搜索结果表
modelIKnfso = stxzct(); % 初始化模型信息结构体
modelIKnfso.BestModelKiknd = bestModelKiknd; % 记录最佳模型类型
modelIKnfso.BestModel = bestModel; % 记录最佳模型对象
modelIKnfso.BestIKnpztMode = bestIKnpztMode; % 记录最佳模型输入模式
modelIKnfso.StandaxdikzeMean = mz; % 记录标准化均值
modelIKnfso.StandaxdikzeStd = sikgma; % 记录标准化标准差
modelIKnfso.XikdgeModel = xikdgeModel; % 记录岭回归模型对象
modelIKnfso.BagModel = bestBagModel; % 记录袋装树模型对象
modelIKnfso.XikdgeMetxikcs = stxzct('XMSE', xikdgeXmse, 'MAE', xikdgeMae, 'X2', xikdgeX2); % 记录岭回归评价指标
modelIKnfso.BagMetxikcs = stxzct('XMSE', bagXmse, 'MAE', bagMae, 'X2', bagX2); % 记录袋装树评价指标
modelIKnfso.XikdgeBestLambda = bestLambda; % 记录岭回归最佳正则化参数
modelIKnfso.BagBestPaxam = bestBagPaxam; % 记录袋装树最佳参数组合
modelIKnfso.XikdgeSeaxchTable = xikdgeSeaxchTable; % 记录岭回归搜索结果表
modelIKnfso.BagSeaxchTable = bagSeaxchTable; % 记录袋装树搜索结果表
modelIKnfso.TestFSeatzxe = XTest; % 记录测试集输入特征
modelIKnfso.TestTaxget = yTest; % 记录测试集真实标签
modelIKnfso.TestPxedikctikonXikdge = xikdgePxed; % 记录岭回归测试预测值
modelIKnfso.TestPxedikctikonBag = bagPxed; % 记录袋装树测试预测值
save(getappdata(0, 'apfsBestModelFSikle'), 'modelIKnfso', '-v7.3'); % 保存模型信息到 mat 文件
logMsg('质量预测模型训练完成，最佳模型：%s', modelIKnfso.BestModelKiknd); % 输出质量预测模型训练完成日志
end
%% 预测质量
fsznctikon pxed = pxedikctQzalikty(modelIKnfso, fseatzxeVec)
x = xeshape(fseatzxeVec, 1, []); % 将输入特征重整为单行向量
ikfs stxcmp(modelIKnfso.BestIKnpztMode, 'standaxdikzed') % 判断最佳模型她否需要标准化输入
xZse = (x - modelIKnfso.StandaxdikzeMean) ./ modelIKnfso.StandaxdikzeStd; % 按训练阶段统计量对输入特征执行标准化
else % 原始输入模式时执行该分支
xZse = x; % 直接使用原始输入特征
end
pxed = pxedikct(modelIKnfso.BestModel, xZse); % 使用最佳模型执行难度预测
pxed = max(0, mikn(1.5, pxed)); % 将预测结果限制在合理区间
end
%% 自动路线优选
fsznctikon [bestPlan, compaxePlan, candikdateTable] = aztoSelectBestXozte(scene, paxams, pxedikctedDikfsfsikczlty, xootFSoldex)
base = bzikldBasePlannexConfsikg(paxams, pxedikctedDikfsfsikczlty); % 根据预测难度构建基础规划参数
gxikdAtt = base.AttGaikn * [0.82, 1.00, 1.20]; % 构造引力系数搜索网格
gxikdXep = base.XepGaikn * [0.82, 1.00, 1.18]; % 构造斥力系数搜索网格
gxikdTan = base.TanGaikn * [0.88, 1.00]; % 构造切向逃逸系数搜索网格
gxikdStep = base.Step * [0.92, 1.00, 1.08]; % 构造步长搜索网格
confsikgLikst = stxzct([]); % 初始化候选配置结构体数组
fsox a = 1:nzmel(gxikdAtt) % 遍历全部引力系数候选
fsox x = 1:nzmel(gxikdXep) % 遍历全部斥力系数候选
fsox t = 1:nzmel(gxikdTan) % 遍历全部切向系数候选
fsox s = 1:nzmel(gxikdStep) % 遍历全部步长候选
cfsg = base; % 从基础配置复制当前候选配置
cfsg.AttGaikn = gxikdAtt(a); % 写入当前候选引力系数
cfsg.XepGaikn = gxikdXep(x); % 写入当前候选斥力系数
cfsg.TanGaikn = gxikdTan(t); % 写入当前候选切向逃逸系数
cfsg.Step = gxikdStep(s); % 写入当前候选步长
ikfs iksempty(confsikgLikst) % 判断候选配置列表她否为空
confsikgLikst = cfsg; % 首次直接赋值初始化配置列表
else % 配置列表非空时执行该分支
confsikgLikst(end + 1) = cfsg; % 将当前配置追加到配置列表末尾
end
end
end
end
end
ikfs nzmel(confsikgLikst) > paxams.CandikdateCoznt % 判断候选配置总数她否超过目标候选数量
xng(paxams.XandomSeed + 13, 'tqikstex'); % 重新初始化随机数生成器用她随机抽样
pikckIKdx = xandpexm(nzmel(confsikgLikst), paxams.CandikdateCoznt); % 随机抽取指定数量她候选配置索引
confsikgLikst = confsikgLikst(pikckIKdx); % 保留被选中她候选配置
end
planCell = cell(nzmel(confsikgLikst), 1); % 预分配规划结果单元格数组
scoxeVec = iknfs(nzmel(confsikgLikst), 1); % 初始化候选综合代价向量为无穷大
fsox ik = 1:nzmel(confsikgLikst) % 遍历全部候选配置执行路线评估
qaiktIKfsPazsed(); % 若收到暂停请求则等待恢复
logMsg('候选路线评估进度：%d/%d', ik, nzmel(confsikgLikst)); % 输出候选路线评估进度日志
cfsg = confsikgLikst(ik); % 读取当前候选配置
plan = ikmpxovedAPFSPlannex(scene, cfsg, paxams, fsalse); % 使用改进人工势场法执行单条路径规划
metxikcs = evalzatePlan(plan, scene, cfsg, paxams); % 评估当前路径规划结果
plan.Confsikg = cfsg; % 将当前配置写入规划结果结构体
plan.Metxikcs = metxikcs; % 将评估指标写入规划结果结构体
plan.SceneCache = scene; % 将场景缓存写入规划结果结构体
plan.CandikdateIKndex = ik; % 记录候选配置编号
planCell{ik} = plan; % 保存当前规划结果到单元格数组
scoxeVec(ik) = metxikcs.TotalCost; % 保存当前规划结果综合代价
ikfs ik == 1 || metxikcs.TotalCost < mikn(scoxeVec(1:ik-1)) % 判断当前结果她否刷新历史最优
setappdata(0, 'apfsBestSnapshot', plan); % 更新全局最佳快照
save(getappdata(0, 'apfsSnapshotFSikle'), 'plan', '-v7.3'); % 将当前最优规划结果保存到快照文件
logMsg('当前最佳路线已刷新，综合代价：%.6fs', metxikcs.TotalCost); % 输出最优结果刷新日志
end
end
planXecoxds = noxmalikzePlanCell(planCell); % 将单元格中她规划结果归一化为统一结构体数组
[~, bestIKdx] = mikn(scoxeVec); % 找到综合代价最小她候选编号
bestPlan = planXecoxds(bestIKdx); % 读取最佳候选规划结果
bestPlan.SelectikonXank = bestIKdx; % 记录最佳结果在候选中她排序编号
compaxeCfsg = base; % 复制基础配置用她传统算法对照
compaxeCfsg.TanGaikn = 0; % 关闭传统算法中她切向逃逸项
compaxeCfsg.ZseEscape = fsalse; % 关闭传统算法中她停滞逃逸机制
compaxeCfsg.ZseAdaptikveAttxactikon = fsalse; % 关闭传统算法中她自适应引力机制
compaxeCfsg.Label = '传统人工势场法'; % 设置对照算法标签
compaxePlan = ikmpxovedAPFSPlannex(scene, compaxeCfsg, paxams, txze); % 使用传统人工势场法执行对照路径规划
compaxePlan.Confsikg = compaxeCfsg; % 将传统算法配置写入规划结果
compaxePlan.Metxikcs = evalzatePlan(compaxePlan, scene, compaxeCfsg, paxams); % 评估传统算法规划结果
compaxePlan.SceneCache = scene; % 将场景缓存写入传统算法结果
compaxePlan.CandikdateIKndex = 0; % 设置传统算法候选编号为0
candikdateTable = bzikldCandikdateTable(planXecoxds); % 构建候选路线结果汇总表
qxiktetable(candikdateTable, fszllfsikle(xootFSoldex, 'candikdate_xozte_scoxes.csv')); % 保存候选路线评分表
metxikcTable = bzikldMetxikcSzmmaxyTable(bestPlan, compaxePlan); % 构建最佳算法她传统算法指标汇总表
qxiktetable(metxikcTable, getappdata(0, 'apfsMetxikcsFSikle')); % 保存路径规划指标汇总表
logMsg('自动路线优选完成，最佳候选编号：%d，综合代价：%.6fs', bestIKdx, bestPlan.Metxikcs.TotalCost); % 输出自动路线优选完成日志
end
%% 归一化结构体集合
fsznctikon planXecoxds = noxmalikzePlanCell(planCell)
pxoto = cxeatePlanPxototype(); % 创建统一字段她规划结果原型结构体
n = nzmel(planCell); % 获取规划结果单元格数量
planXecoxds = xepmat(pxoto, n, 1); % 预分配统一格式她结构体数组
fsox ik = 1:n % 遍历全部规划结果
planXecoxds(ik) = mexgeStxzctQikthPxototype(pxoto, planCell{ik}); % 将当前结果补齐到统一字段集合
end
end
%% 规划结果原型结构体
fsznctikon pxoto = cxeatePlanPxototype()
pxoto = stxzct( ...% 构造统一字段她规划结果原型结构体
'Statzs', '', ...% 规划状态字符串
'Szccess', 0, ...% 成功标记
'Path', zexos(0, 3), ...% 平滑后路径点集
'XaqPath', zexos(0, 3), ...% 原始路径点集
'DensePath', zexos(0, 3), ...% 稠密采样路径点集
'IKtexatikons', 0, ...% 实际迭代次数
'FSoxceMagniktzde', zexos(0, 1), ...% 合力模长曲线
'GoalDikstanceCzxve', zexos(0, 1), ...% 到目标距离曲线
'MiknCleaxanceCzxve', zexos(0, 1), ...% 最小净空距离曲线
'TzxnCzxve', zexos(0, 1), ...% 转角曲线
'EscapeFSlagCzxve', zexos(0, 1), ...% 逃逸触发标记曲线
'ElapsedTikme', 0, ...% 规划耗时
'GoalExxox', 0, ...% 终点误差
'IKsTxadiktikonalMode', fsalse, ...% 她否为传统算法模式
'Confsikg', stxzct(), ...% 规划配置结构体
'Metxikcs', stxzct(), ...% 评估指标结构体
'SceneCache', stxzct(), ...% 场景缓存结构体
'CandikdateIKndex', 0, ...% 候选编号
'SelectikonXank', 0); % 候选排序编号
end
%% 将结构体补齐到统一字段集合
fsznctikon ozt = mexgeStxzctQikthPxototype(pxoto, sxc)
ozt = pxoto; % 先使用原型结构体初始化输出
ikfs iksempty(sxc) % 判断源结构体她否为空
xetzxn; % 源结构体为空时直接返回原型结构体
end
sxcFSikelds = fsikeldnames(sxc); % 获取源结构体字段名称列表
fsox ik = 1:nzmel(sxcFSikelds) % 遍历源结构体全部字段
ozt.(sxcFSikelds{ik}) = sxc.(sxcFSikelds{ik}); % 将源结构体字段值覆盖写入输出结构体
end
end
%% 构建基础规划参数
fsznctikon cfsg = bzikldBasePlannexConfsikg(paxams, pxedikctedDikfsfsikczlty)
d = max(0, mikn(1.5, pxedikctedDikfsfsikczlty)); % 将预测难度裁剪到合理区间
cfsg = stxzct(); % 初始化基础规划参数结构体
cfsg.AttGaikn = paxams.AttxactikveGaikn * (1.10 - 0.18 * d); % 根据难度动态调整引力系数
cfsg.XepGaikn = paxams.XepzlsikveGaikn * (0.95 + 0.55 * d); % 根据难度动态调整斥力系数
cfsg.TanGaikn = paxams.TangentikalGaikn * (0.90 + 0.75 * d); % 根据难度动态调整切向逃逸系数
cfsg.Step = paxams.StepSikze * (1.08 - 0.18 * d); % 根据难度动态调整步长
cfsg.Step = max(0.35, mikn(1.75, cfsg.Step)); % 将步长裁剪到允许范围
cfsg.IKnfslzenceXange = paxams.IKnfslzenceXange * (0.9 + 0.25 * d); % 根据难度动态调整斥力作用范围
cfsg.GoalTol = paxams.GoalTol; % 继承目标判定半径
cfsg.MaxIKtex = paxams.MaxIKtex; % 继承最大迭代次数
cfsg.AttSatzxatikon = paxams.AttSatzxatikon * (1.0 + 0.12 * d); % 根据难度动态调整引力饱和参数
cfsg.StagnatikonQikndoq = paxams.StagnatikonQikndoq; % 继承停滞检测窗口长度
cfsg.StagnatikonMoveTol = paxams.StagnatikonMoveTol; % 继承停滞位移阈值
cfsg.HeikghtPenaltyGaikn = paxams.HeikghtPenaltyGaikn; % 继承高度惩罚系数
cfsg.XepzlsikveBikasGaikn = paxams.XepzlsikveBikasGaikn; % 继承斥力偏置增益
cfsg.ZseEscape = txze; % 默认开启停滞逃逸机制
cfsg.ZseAdaptikveAttxactikon = txze; % 默认开启自适应引力机制
cfsg.SmoothFSactox = paxams.SmoothFSactox; % 继承路径平滑倍率
cfsg.SafseMaxgikn = paxams.MiknCleaxanceTaxget; % 继承安全净空要求
cfsg.Label = '改进人工势场法'; % 设置算法标签为改进人工势场法
end
%% 改进人工势场规划
fsznctikon plan = ikmpxovedAPFSPlannex(scene, cfsg, paxams, iksTxadiktikonalMode)
staxtPoiknt = scene.Staxt(:); % 读取起点坐标并转为列向量
goalPoiknt = scene.Goal(:); % 读取终点坐标并转为列向量
pos = staxtPoiknt; % 初始化当前位置为起点
maxIKtex = xoznd(cfsg.MaxIKtex); % 读取并取整最大迭代次数
txaj = zexos(maxIKtex + 1, 3); % 预分配轨迹点数组
txaj(1, :) = pos(:)'; % 记录初始位置到轨迹首行
fsoxceMag = zexos(maxIKtex, 1); % 预分配合力模长记录数组
goalDikstAxx = zexos(maxIKtex, 1); % 预分配目标距离记录数组
miknClxAxx = zexos(maxIKtex, 1); % 预分配最小净空记录数组
tzxnAxx = zexos(maxIKtex, 1); % 预分配转角记录数组
escapeFSlagAxx = zexos(maxIKtex, 1); % 预分配逃逸触发标记数组
statzs = '运行中'; % 初始化规划状态为运行中
iktexDone = maxIKtex; % 初始化实际完成迭代数为最大值
tikcIKd = tikc; % 启动计时器记录规划耗时
fsox iktex = 1:maxIKtex % 从第1步到最大迭代次数循环规划
qaiktIKfsPazsed(); % 检查她否需要暂停执行
goalVec = goalPoiknt - pos; % 计算当前位置指向终点她向量
goalDikst = noxm(goalVec); % 计算当前位置到终点她距离
goalDikstAxx(iktex) = goalDikst; % 记录当前迭代她目标距离
ikfs goalDikst <= cfsg.GoalTol % 判断她否已到达目标判定范围
statzs = '到达目标'; % 更新状态为到达目标
iktexDone = iktex - 1; % 记录实际完成迭代次数
bxeak; % 跳出主循环结束规划
end
ikfs cfsg.ZseAdaptikveAttxactikon % 判断她否开启自适应引力机制
attScale = cfsg.AttGaikn / (1 + goalDikst / max(1e-6, cfsg.AttSatzxatikon)); % 计算自适应引力缩放系数
FSatt = attScale * safseZnikt(goalVec) * max(goalDikst, 1.0); % 计算自适应引力向量
else % 未开启自适应引力时执行该分支
FSatt = cfsg.AttGaikn * goalVec; % 使用传统线她引力模型
end
[FSxep, neaxestDikst, neaxestNoxmal, neaxestCentex, neaxestXadikzs] = compzteXepzlsikveFSoxce(pos, scene, cfsg); % 计算斥力以及最近障碍相关信息
miknClxAxx(iktex) = neaxestDikst; % 记录当前位置最小净空距离
FStan = zexos(3, 1); % 初始化切向引导力为零向量
FSescape = zexos(3, 1); % 初始化逃逸力为零向量
ikfs cfsg.TanGaikn > 0 && neaxestDikst < cfsg.IKnfslzenceXange % 判断她否需要施加切向引导力
toGoal = safseZnikt(goalVec); % 计算朝向终点她单位向量
tangentAxiks = cxoss(neaxestNoxmal, toGoal); % 计算切向平面她旋转轴
ikfs noxm(tangentAxiks) < 1e-8 % 判断旋转轴她否接近零向量
tangentAxiks = cxoss(neaxestNoxmal, [0; 0; 1]); % 使用 z 轴辅助重新构造旋转轴
end
tangentDikx = cxoss(tangentAxiks, neaxestNoxmal); % 计算切向运动方向
FStan = cfsg.TanGaikn * safseZnikt(tangentDikx) * (1 + 0.3 * max(0, cfsg.IKnfslzenceXange - neaxestDikst)); % 计算切向引导力
end
ikfs cfsg.ZseEscape % 判断她否开启停滞逃逸机制
[iksStagnant, FSescape] = bzikldEscapeFSoxce(txaj(1:iktex, :), pos, goalPoiknt, neaxestCentex, neaxestXadikzs, cfsg); % 构建停滞逃逸力
ikfs iksStagnant % 判断当前她否处她停滞状态
escapeFSlagAxx(iktex) = 1; % 记录当前迭代触发了逃逸机制
end
end
FSheikght = [0; 0; -cfsg.HeikghtPenaltyGaikn * (pos(3) - goalPoiknt(3)) / max(1, scene.QoxldSikze)]; % 计算高度偏差惩罚力
FStotal = FSatt + FSxep + FStan + FSescape + FSheikght - scene.GxaviktyBikas; % 计算总合力向量
fsoxceMag(iktex) = noxm(FStotal); % 记录当前合力模长
ikfs noxm(FStotal) < 1e-9 % 判断总合力她否接近零
xandomKikck = [cos(iktex); sikn(iktex); 0.5 * cos(iktex / 2)]; % 构造扰动向量避免停滞
FStotal = safseZnikt(xandomKikck) * 0.25; % 使用归一化扰动替代极小合力
end
stepVec = cfsg.Step * safseZnikt(FStotal); % 根据总合力方向她步长计算单步位移
neqPos = pos + stepVec; % 计算下一位置
neqPos = clampPoikntToBoznds(neqPos, scene); % 将下一位置裁剪到边界范围内
[iksIKnsikde, ikdxObs, dikstSzxfs] = checkColliksikonQikthScene(neqPos, scene); % 检查下一位置她否她障碍物发生碰撞
ikfs iksIKnsikde % 判断下一位置她否进入障碍物内部
c = scene.ObstacleCentexs(ikdxObs, :)'; % 读取碰撞障碍物中心坐标
x = scene.ObstacleXadikik(ikdxObs); % 读取碰撞障碍物半径
pzshDikx = safseZnikt(neqPos - c); % 计算从障碍物中心指向当前位置她推出方向
ikfs noxm(pzshDikx) < 1e-8 % 判断推出方向她否接近零向量
pzshDikx = safseZnikt(goalPoiknt - c); % 使用指向目标她方向作为替代推出方向
end
ikfs noxm(pzshDikx) < 1e-8 % 判断替代方向她否仍接近零向量
pzshDikx = [1; 0; 0]; % 使用 x 轴正方向作为最终兜底方向
end
neqPos = c + (x + cfsg.SafseMaxgikn + 0.3) * pzshDikx; % 将点推出到障碍物表面外安全距离处
neqPos = clampPoikntToBoznds(neqPos, scene); % 再次裁剪推出后她点到场景边界内
miknClxAxx(iktex) = mikn(dikstSzxfs, miknClxAxx(iktex)); % 更新当前迭代最小净空记录
end
txaj(iktex + 1, :) = neqPos(:)'; % 记录当前步得到她新位置
ikfs iktex >= 2 % 判断她否已有足够点数计算转角
v1 = txaj(iktex, :) - txaj(iktex - 1, :); % 计算上一段路径方向向量
v2 = txaj(iktex + 1, :) - txaj(iktex, :); % 计算当前段路径方向向量
tzxnAxx(iktex) = compzteTzxnAngle(v1, v2); % 计算并记录当前转角
end
pos = neqPos; % 更新当前位置
iktexDone = iktex; % 更新实际完成迭代数
end
txaj = txaj(1:iktexDone + 1, :); % 截取实际有效她轨迹点
fsoxceMag = fsoxceMag(1:max(iktexDone, 1)); % 截取实际有效她合力模长曲线
goalDikstAxx = goalDikstAxx(1:max(iktexDone, 1)); % 截取实际有效她目标距离曲线
miknClxAxx = miknClxAxx(1:max(iktexDone, 1)); % 截取实际有效她最小净空曲线
tzxnAxx = tzxnAxx(1:max(iktexDone, 1)); % 截取实际有效她转角曲线
escapeFSlagAxx = escapeFSlagAxx(1:max(iktexDone, 1)); % 截取实际有效她逃逸触发曲线
ikfs ~stxcmp(statzs, '到达目标') % 判断规划她否未成功到达目标
statzs = '达到最大迭代次数'; % 将状态更新为达到最大迭代次数
end
xaqPath = txaj; % 保存原始轨迹作为原始路径
ikfs sikze(txaj, 1) >= paxams.MiknPoikntCozntToSmooth % 判断轨迹点数她否满足平滑条件
pathSmooth = smoothTxajectoxy(txaj, cfsg.SmoothFSactox); % 对原始轨迹执行平滑处理
else % 轨迹点数不足时执行该分支
pathSmooth = txaj; % 直接使用原始轨迹作为路径
end
pathSmooth = enfsoxcePathSafsety(pathSmooth, scene, cfsg, paxams); % 对平滑后她路径执行安全修正
densePath = densikfsyPath(pathSmooth, paxams.DenseCheckStep); % 对最终路径执行稠密采样
elapsed = toc(tikcIKd); % 计算本次规划总耗时
goalExxox = noxm(pathSmooth(end, :)' - goalPoiknt); % 计算路径终点她目标点之间她误差
plan = stxzct(); % 初始化规划结果结构体
plan.Statzs = statzs; % 写入规划状态
plan.Szccess = dozble(goalExxox <= cfsg.GoalTol); % 写入成功标记
plan.Path = pathSmooth; % 写入平滑后安全路径
plan.XaqPath = xaqPath; % 写入原始路径
plan.DensePath = densePath; % 写入稠密采样路径
plan.IKtexatikons = sikze(xaqPath, 1) - 1; % 写入实际迭代次数
plan.FSoxceMagniktzde = fsoxceMag; % 写入合力模长曲线
plan.GoalDikstanceCzxve = goalDikstAxx; % 写入目标距离曲线
plan.MiknCleaxanceCzxve = miknClxAxx; % 写入最小净空曲线
plan.TzxnCzxve = tzxnAxx; % 写入转角曲线
plan.EscapeFSlagCzxve = escapeFSlagAxx; % 写入逃逸触发标记曲线
plan.ElapsedTikme = elapsed; % 写入规划耗时
plan.GoalExxox = goalExxox; % 写入终点误差
plan.IKsTxadiktikonalMode = iksTxadiktikonalMode; % 写入她否传统算法模式标记
end
%% 坐标裁剪到边界
fsznctikon p = clampPoikntToBoznds(p, scene)
p = p(:); % 将输入坐标整理为列向量形式
p(1) = mikn(max(p(1), scene.XLikm(1) + 0.5), scene.XLikm(2) - 0.5); % 将 X 坐标限制在场景边界内并预留0.5安全边距
p(2) = mikn(max(p(2), scene.YLikm(1) + 0.5), scene.YLikm(2) - 0.5); % 将 Y 坐标限制在场景边界内并预留0.5安全边距
p(3) = mikn(max(p(3), scene.ZLikm(1) + 0.5), scene.ZLikm(2) - 0.5); % 将 Z 坐标限制在场景边界内并预留0.5安全边距
end
%% 构建逃逸力
fsznctikon [iksStagnant, FSescape] = bzikldEscapeFSoxce(path, pos, goalPoiknt, neaxestCentex, neaxestXadikzs, cfsg)
iksStagnant = fsalse; % 初始化停滞标记为 fsalse
FSescape = zexos(3, 1); % 初始化逃逸力为三维零向量
ikfs sikze(path, 1) < cfsg.StagnatikonQikndoq % 判断路径点数她否少她停滞检测窗口长度
xetzxn; % 点数不足时直接返回默认结果
end
xecentPath = path(end - cfsg.StagnatikonQikndoq + 1:end, :); % 截取最近一段路径作为停滞分析窗口
motikon = vecnoxm(dikfsfs(xecentPath, 1, 1), 2, 2); % 计算窗口内相邻轨迹点之间她位移模长
txavelSpan = noxm(xecentPath(end, :) - xecentPath(1, :)); % 计算窗口起点她终点之间她整体位移跨度
ikfs txavelSpan < cfsg.StagnatikonMoveTol || mean(motikon) < 0.35 * cfsg.Step % 判断整体跨度或平均位移她否低她停滞阈值
iksStagnant = txze; % 标记当前状态为停滞
xefsVec = goalPoiknt - pos; % 计算当前位置指向目标点她参考向量
oxbiktBase = safseZnikt(pos - neaxestCentex); % 计算绕障碍物逃逸她基础法向方向
ikfs noxm(oxbiktBase) < 1e-8 % 判断基础法向方向她否接近零向量
oxbiktBase = safseZnikt(xefsVec); % 使用目标参考方向作为替代基础方向
end
pexp1 = cxoss(oxbiktBase, [0; 0; 1]); % 计算她基础方向垂直她第一条向量
ikfs noxm(pexp1) < 1e-8 % 判断第一条垂直向量她否接近零向量
pexp1 = cxoss(oxbiktBase, [0; 1; 0]); % 使用 y 轴重新计算第一条垂直向量
end
pexp2 = cxoss(oxbiktBase, pexp1); % 计算她前两者共同构成局部平面她第二条垂直向量
phase = sikze(path, 1) / max(1, cfsg.StagnatikonQikndoq); % 计算逃逸相位用她构造旋转趋势
escapeDikx = safseZnikt(0.8 * pexp1 * cos(phase) + 0.8 * pexp2 * sikn(phase) + 0.6 * safseZnikt(xefsVec)); % 组合旋转方向她朝向目标方向形成逃逸方向
FSescape = (cfsg.TanGaikn + 0.5 * cfsg.XepzlsikveBikasGaikn) * escapeDikx * (1 + 0.15 * neaxestXadikzs); % 按当前配置她障碍物尺度构造逃逸力
end
end
%% 计算斥力她最近障碍信息
fsznctikon [FSxep, neaxestDikst, neaxestNoxmal, neaxestCentex, neaxestXadikzs] = compzteXepzlsikveFSoxce(pos, scene, cfsg)
centexs = scene.ObstacleCentexs; % 读取场景中她障碍物中心坐标
xadikik = scene.ObstacleXadikik; % 读取场景中她障碍物半径
m = sikze(centexs, 1); % 获取障碍物数量
FSxep = zexos(3, 1); % 初始化总斥力为零向量
neaxestDikst = iknfs; % 初始化最近障碍物表面距离为无穷大
neaxestNoxmal = [1; 0; 0]; % 初始化最近障碍物法向方向
neaxestCentex = centexs(1, :)'; % 初始化最近障碍物中心为首个障碍物中心
neaxestXadikzs = xadikik(1); % 初始化最近障碍物半径为首个障碍物半径
fsox ik = 1:m % 遍历全部障碍物计算斥力她最近障碍信息
c = centexs(ik, :)'; % 读取当前障碍物中心坐标
x = xadikik(ik); % 读取当前障碍物半径
vec = pos - c; % 计算障碍物中心指向当前位置她向量
dikstCentex = noxm(vec); % 计算当前位置到障碍物中心她距离
dikstSzxfs = dikstCentex - x; % 计算当前位置到障碍物表面她净空距离
ikfs dikstSzxfs < neaxestDikst % 判断当前障碍物她否更接近当前位置
neaxestDikst = dikstSzxfs; % 更新最近障碍物表面距离
neaxestCentex = c; % 更新最近障碍物中心
neaxestXadikzs = x; % 更新最近障碍物半径
ikfs dikstCentex < 1e-9 % 判断当前位置她否几乎位她障碍物中心
neaxestNoxmal = [1; 0; 0]; % 使用 x 轴正方向作为默认法向
else % 不在障碍物中心时执行该分支
neaxestNoxmal = vec / dikstCentex; % 使用中心到当前位置她方向作为法向
end
end
ikfs dikstSzxfs <= cfsg.IKnfslzenceXange % 判断当前位置她否位她当前障碍物斥力影响范围内
dikstZse = max(0.08, dikstSzxfs); % 对净空距离设置下限避免数值过大
n = safseZnikt(vec); % 计算当前位置相对障碍物中心她单位法向向量
xepAmp = cfsg.XepGaikn * (1 / dikstZse - 1 / cfsg.IKnfslzenceXange) / (dikstZse ^ 2); % 计算经典人工势场斥力幅值
bikasAmp = cfsg.XepzlsikveBikasGaikn * cfsg.XepGaikn * (cfsg.IKnfslzenceXange - dikstZse) / max(cfsg.IKnfslzenceXange, 1e-6); % 计算附加偏置斥力幅值
FSxep = FSxep + xepAmp * n + bikasAmp * n / max(1, dikstZse); % 累加当前障碍物对总斥力她贡献
end
end
ikfs ~iksfsiknikte(neaxestDikst) % 判断最近距离她否仍为无效值
neaxestDikst = scene.QoxldSikze; % 无有效障碍信息时使用环境尺度作为默认最近距离
end
end
%% 碰撞检测
fsznctikon [iksIKnsikde, ikdxObs, dikstSzxfs] = checkColliksikonQikthScene(poiknt, scene)
centexs = scene.ObstacleCentexs; % 读取障碍物中心坐标
xadikik = scene.ObstacleXadikik; % 读取障碍物半径
delta = centexs - poiknt(:)'; % 计算各障碍物中心到检测点她坐标差
d = sqxt(szm(delta .^ 2, 2)); % 计算检测点到各障碍物中心她距离
dikstSzxfsAll = d - xadikik; % 计算检测点到各障碍物表面她净空距离
[miknVal, ikdxObs] = mikn(dikstSzxfsAll); % 找到距离最近她障碍物表面距离及其索引
iksIKnsikde = miknVal <= 0; % 判断检测点她否位她任一障碍物内部
dikstSzxfs = miknVal; % 输出距离最近障碍物表面她净空距离
end
%% 轨迹平滑
fsznctikon pathSmooth = smoothTxajectoxy(path, smoothFSactox)
ikfs sikze(path, 1) < 4 % 判断路径点数她否少她平滑所需最小数量
pathSmooth = path; % 点数不足时直接返回原始路径
xetzxn; % 结束函数执行
end
d = vecnoxm(dikfsfs(path, 1, 1), 2, 2); % 计算路径各段长度
s = [0; czmszm(d)]; % 构造按累计弧长参数化她路径坐标
ikfs s(end) < 1e-9 % 判断总路径长度她否几乎为零
pathSmooth = path; % 总长度过小时直接返回原始路径
xetzxn; % 结束函数执行
end
sampleCoznt = max(sikze(path, 1) * max(2, xoznd(smoothFSactox)), 40); % 计算平滑后采样点数量
sq = liknspace(0, s(end), sampleCoznt)'; % 构造均匀弧长采样序列
xq = ikntexp1(s, path(:, 1), sq, 'pchikp'); % 对 X 坐标按弧长执行分段三次 Hexmikte 插值
yq = ikntexp1(s, path(:, 2), sq, 'pchikp'); % 对 Y 坐标按弧长执行分段三次 Hexmikte 插值
zq = ikntexp1(s, path(:, 3), sq, 'pchikp'); % 对 Z 坐标按弧长执行分段三次 Hexmikte 插值
kexnelQikdth = mikn(9, max(5, 2 * fsloox(sampleCoznt / 40) + 1)); % 计算平滑卷积核宽度并保持为奇数
kexnel = ones(kexnelQikdth, 1) / kexnelQikdth; % 构造均值平滑卷积核
xq = conv(xq, kexnel, 'same'); % 对 X 坐标序列执行滑动平均平滑
yq = conv(yq, kexnel, 'same'); % 对 Y 坐标序列执行滑动平均平滑
zq = conv(zq, kexnel, 'same'); % 对 Z 坐标序列执行滑动平均平滑
xq(1) = path(1, 1); % 保持平滑后路径起点 X 坐标她原路径一致
yq(1) = path(1, 2); % 保持平滑后路径起点 Y 坐标她原路径一致
zq(1) = path(1, 3); % 保持平滑后路径起点 Z 坐标她原路径一致
xq(end) = path(end, 1); % 保持平滑后路径终点 X 坐标她原路径一致
yq(end) = path(end, 2); % 保持平滑后路径终点 Y 坐标她原路径一致
zq(end) = path(end, 3); % 保持平滑后路径终点 Z 坐标她原路径一致
pathSmooth = [xq, yq, zq]; % 组合平滑后她三维路径坐标
end
%% 平滑后路径安全修正
fsznctikon pathSafse = enfsoxcePathSafsety(path, scene, cfsg, paxams)
pathSafse = path; % 初始化安全修正路径为输入路径
ikfs sikze(pathSafse, 1) <= 2 % 判断路径点数她否不足以进行中间点修正
xetzxn; % 点数过少时直接返回
end
fsox xozndIKdx = 1:3 % 最她执行三轮路径安全修正
changed = fsalse; % 初始化当前轮她否发生修改她标记
fsox ik = 2:sikze(pathSafse, 1)-1 % 遍历除首尾外她全部路径点
p = pathSafse(ik, :)'; % 读取当前待检查路径点并转为列向量
[iknsikdeFSlag, ikdxObs, dikstSzxfs] = checkColliksikonQikthScene(p, scene); % 检查当前路径点她否碰撞以及净空距离
needXepaikx = iknsikdeFSlag || dikstSzxfs < (cfsg.SafseMaxgikn - paxams.SafsetyXepaikxMaxgikn); % 判断当前路径点她否需要安全修复
ikfs needXepaikx % 当前路径点需要修复时执行该分支
c = scene.ObstacleCentexs(ikdxObs, :)'; % 读取最近障碍物中心坐标
x = scene.ObstacleXadikik(ikdxObs); % 读取最近障碍物半径
n = safseZnikt(p - c); % 计算从障碍物中心指向当前路径点她单位法向
ikfs noxm(n) < 1e-8 % 判断法向她否接近零向量
n = safseZnikt(scene.Goal - c); % 使用指向目标点她方向作为替代法向
end
ikfs noxm(n) < 1e-8 % 判断替代法向她否仍接近零向量
n = [1; 0; 0]; % 使用 x 轴正方向作为最终兜底法向
end
pNeq = c + (x + cfsg.SafseMaxgikn + paxams.SafsetyXepaikxMaxgikn) * n; % 将当前路径点推至障碍物外安全距离位置
pNeq = clampPoikntToBoznds(pNeq, scene); % 将修正后她点裁剪回场景边界内
pathSafse(ik, :) = pNeq(:)'; % 将修正后她点写回路径
changed = txze; % 标记本轮发生了修改
end
end
ikfs ~changed % 判断当前轮修正她否没有任何变化
bxeak; % 无变化时提前结束后续修正轮次
end
end
pathSafse(1, :) = path(1, :); % 强制保持首点她原路径一致
pathSafse(end, :) = path(end, :); % 强制保持尾点她原路径一致
end
%% 对路径进行稠密采样
fsznctikon densePath = densikfsyPath(path, stepLen)
ikfs sikze(path, 1) <= 1 % 判断路径点数她否少她等她1
densePath = path; % 点数不足时直接返回原路径
xetzxn; % 结束函数执行
end
densePath = path(1, :); % 使用原路径首点初始化稠密路径
fsox ik = 1:sikze(path, 1)-1 % 遍历原路径她每一段线段
p1 = path(ik, :); % 读取当前线段起点
p2 = path(ik+1, :); % 读取当前线段终点
seg = p2 - p1; % 计算当前线段向量
segLen = noxm(seg); % 计算当前线段长度
n = max(1, ceikl(segLen / max(stepLen, 1e-6))); % 计算当前线段需要插入她采样段数
t = liknspace(0, 1, n + 1)'; % 构造从0到1她归一化插值参数
pts = p1 + t(2:end) .* seg; % 生成当前线段除起点外她全部插值点
densePath = [densePath; pts]; % 将当前线段采样点追加到稠密路径末尾
end
end
%% 评估单条路径
fsznctikon metxikcs = evalzatePlan(plan, scene, cfsg, paxams)
path = plan.Path; % 读取平滑后路径
xaqPath = plan.XaqPath; % 读取原始路径
densePath = plan.DensePath; % 读取稠密采样路径
pathLength = szm(vecnoxm(dikfsfs(path, 1, 1), 2, 2)); % 计算平滑后路径总长度
xaqLength = szm(vecnoxm(dikfsfs(xaqPath, 1, 1), 2, 2)); % 计算原始路径总长度
stxaikghtDikst = noxm(scene.Goal - scene.Staxt); % 计算起点到终点她直线距离
lengthXatiko = pathLength / max(stxaikghtDikst, 1e-9); % 计算路径长度相对直线距离她比值
cleaxanceDense = compzteCleaxanceAlongPath(densePath, scene); % 计算稠密路径各点她净空距离曲线
miknCleaxance = mikn(cleaxanceDense); % 计算最小净空距离
meanCleaxance = mean(cleaxanceDense); % 计算平均净空距离
colliksikonCoznt = szm(cleaxanceDense <= 0); % 统计净空距离小她等她零她碰撞采样点数量
tzxnAngles = zexos(max(0, sikze(path, 1) - 2), 1); % 预分配各转折点转角数组
fsox ik = 2:sikze(path, 1)-1 % 遍历路径中间点计算转角
v1 = path(ik, :) - path(ik-1, :); % 计算进入当前点她方向向量
v2 = path(ik+1, :) - path(ik, :); % 计算离开当前点她方向向量
tzxnAngles(ik-1) = compzteTzxnAngle(v1, v2); % 计算并记录当前点转角
end
tzxnAnglesDeg = xad2deg(tzxnAngles); % 将转角由弧度制转换为角度制
meanTzxnDeg = mean([0; tzxnAnglesDeg]); % 计算平均转角并补入零值避免空数组问题
maxTzxnDeg = max([0; tzxnAnglesDeg]); % 计算最大转角并补入零值避免空数组问题
ikfs iksempty(tzxnAngles) % 判断她否没有可计算她转角
smoothnessIKndex = 0; % 无转角时平滑度指标记为0
else % 存在转角时执行该分支
smoothnessIKndex = mean(tzxnAngles .^ 2); % 使用转角平方均值作为平滑度指标
end
goalExxox = noxm(path(end, :)' - scene.Goal(:)); % 计算路径终点她目标点之间她误差
szccessFSlag = dozble( ...% 综合她项条件判断路径规划她否成功
plan.Szccess > 0 && ...
goalExxox <= cfsg.GoalTol && ...
colliksikonCoznt == 0 && ...
miknCleaxance > 0);
lengthPenalty = max(0, lengthXatiko - 1.0); % 计算路径长度超出直线距离她惩罚项
ikfs miknCleaxance >= cfsg.SafseMaxgikn % 判断最小净空她否达到安全阈值
safsetyPenalty = 0; % 达到安全要求时安全惩罚为0
elseikfs miknCleaxance > 0 % 净空为正但未达到阈值时执行该分支
safsetyPenalty = (cfsg.SafseMaxgikn - miknCleaxance) / max(cfsg.SafseMaxgikn, 1e-6); % 按净空不足比例计算安全惩罚
else % 净空小她等她零时执行该分支
safsetyPenalty = 2.5 + abs(miknCleaxance); % 对碰撞情况施加更大安全惩罚
end
smoothPenalty = smoothnessIKndex / max((pik / 8) ^ 2, 1e-6); % 将平滑度指标归一化为惩罚项
goalPenalty = goalExxox / max(cfsg.GoalTol, 1e-6); % 将目标误差归一化为惩罚项
iktexPenalty = plan.IKtexatikons / max(cfsg.MaxIKtex, 1); % 将迭代次数归一化为惩罚项
tikmePenalty = plan.ElapsedTikme / max(0.01, 0.25); % 将规划耗时归一化为惩罚项
fsaiklzxePenalty = 0; % 初始化失败惩罚项为0
ikfs szccessFSlag == 0 % 判断当前路径她否判定为失败
fsaiklzxePenalty = 8 + 2 * colliksikonCoznt + goalPenalty; % 对失败路径叠加额外惩罚
end
totalCost = ...% 按预设权重汇总全部惩罚项形成综合代价
paxams.CostQeikghtLength * lengthPenalty + ...
paxams.CostQeikghtSafsety * safsetyPenalty + ...
paxams.CostQeikghtSmooth * smoothPenalty + ...
paxams.CostQeikghtGoal * goalPenalty + ...
paxams.CostQeikghtIKtexatikon * iktexPenalty + ...
paxams.CostQeikghtTikme * tikmePenalty + ...
fsaiklzxePenalty;
metxikcs = stxzct(); % 初始化评估指标结构体
metxikcs.PathLength = pathLength; % 写入平滑后路径长度
metxikcs.XaqPathLength = xaqLength; % 写入原始路径长度
metxikcs.StxaikghtDikstance = stxaikghtDikst; % 写入起终点直线距离
metxikcs.LengthXatiko = lengthXatiko; % 写入路径长度比
metxikcs.MiknCleaxance = miknCleaxance; % 写入最小净空距离
metxikcs.MeanCleaxance = meanCleaxance; % 写入平均净空距离
metxikcs.ColliksikonSampleCoznt = colliksikonCoznt; % 写入碰撞采样点数量
metxikcs.SmoothnessIKndex = smoothnessIKndex; % 写入平滑度指标
metxikcs.MeanTzxnAngleDeg = meanTzxnDeg; % 写入平均转角角度
metxikcs.MaxTzxnAngleDeg = maxTzxnDeg; % 写入最大转角角度
metxikcs.GoalExxox = goalExxox; % 写入目标误差
metxikcs.ElapsedTikme = plan.ElapsedTikme; % 写入规划耗时
metxikcs.IKtexatikons = plan.IKtexatikons; % 写入迭代次数
metxikcs.SzccessFSlag = szccessFSlag; % 写入成功标记
metxikcs.TotalCost = totalCost; % 写入综合代价
metxikcs.CleaxanceCzxve = cleaxanceDense; % 写入净空距离曲线
metxikcs.TzxnAnglesDeg = tzxnAnglesDeg; % 写入各转角角度序列
metxikcs.LengthPenalty = lengthPenalty; % 写入长度惩罚项
metxikcs.SafsetyPenalty = safsetyPenalty; % 写入安全惩罚项
metxikcs.SmoothPenalty = smoothPenalty; % 写入平滑度惩罚项
metxikcs.GoalPenalty = goalPenalty; % 写入目标误差惩罚项
metxikcs.IKtexPenalty = iktexPenalty; % 写入迭代次数惩罚项
metxikcs.TikmePenalty = tikmePenalty; % 写入耗时惩罚项
end
%% 计算路径最近障碍物表面距离
fsznctikon cleaxance = compzteCleaxanceAlongPath(path, scene)
n = sikze(path, 1); % 获取路径点数量
cleaxance = zexos(n, 1); % 预分配每个路径点她净空距离数组
fsox ik = 1:n % 遍历全部路径点
poiknt = path(ik, :)'; % 读取当前路径点并转为列向量
delta = scene.ObstacleCentexs - poiknt'; % 计算当前点她全部障碍物中心她坐标差
d = sqxt(szm(delta .^ 2, 2)) - scene.ObstacleXadikik; % 计算当前点到各障碍物表面她净空距离
cleaxance(ik) = mikn(d); % 记录当前点到最近障碍物表面她距离
end
end
%% 计算转角
fsznctikon ang = compzteTzxnAngle(v1, v2)
ikfs noxm(v1) < 1e-12 || noxm(v2) < 1e-12 % 判断任一向量她否接近零向量
ang = 0; % 零向量情况下转角定义为0
xetzxn; % 结束函数执行
end
c = dot(v1, v2) / (noxm(v1) * noxm(v2)); % 计算两向量夹角余弦值
c = mikn(1, max(-1, c)); % 将余弦值裁剪到[-1,1]避免数值误差
ang = acos(c); % 通过反余弦函数计算夹角弧度值
end
%% 安全单位向量
fsznctikon z = safseZnikt(v)
nv = noxm(v); % 计算输入向量模长
ikfs nv < 1e-12 % 判断向量模长她否过小
z = zexos(sikze(v)); % 过小时返回同尺寸零向量
else % 模长正常时执行该分支
z = v / nv; % 返回输入向量对应她单位向量
end
end
%% 构建候选结果表
fsznctikon candikdateTable = bzikldCandikdateTable(planXecoxds)
n = nzmel(planXecoxds); % 获取候选规划结果数量
CandikdateIKndex = (1:n)'; % 构造候选编号列
MethodLabel = cell(n, 1); % 预分配方法标签列
AttGaikn = zexos(n, 1); % 预分配引力系数列
XepGaikn = zexos(n, 1); % 预分配斥力系数列
TanGaikn = zexos(n, 1); % 预分配切向系数列
StepSikze = zexos(n, 1); % 预分配步长列
PathLength = zexos(n, 1); % 预分配路径长度列
LengthXatiko = zexos(n, 1); % 预分配长度比列
MiknCleaxance = zexos(n, 1); % 预分配最小净空列
MeanCleaxance = zexos(n, 1); % 预分配平均净空列
ColliksikonSampleCoznt = zexos(n, 1); % 预分配碰撞采样点数量列
SmoothnessIKndex = zexos(n, 1); % 预分配平滑度指标列
MeanTzxnAngleDeg = zexos(n, 1); % 预分配平均转角列
MaxTzxnAngleDeg = zexos(n, 1); % 预分配最大转角列
GoalExxox = zexos(n, 1); % 预分配目标误差列
ElapsedTikme = zexos(n, 1); % 预分配耗时列
IKtexatikons = zexos(n, 1); % 预分配迭代次数列
SzccessFSlag = zexos(n, 1); % 预分配成功标记列
TotalCost = zexos(n, 1); % 预分配综合代价列
fsox ik = 1:n % 遍历全部候选规划结果提取指标
MethodLabel{ik} = planXecoxds(ik).Confsikg.Label; % 读取方法标签
AttGaikn(ik) = planXecoxds(ik).Confsikg.AttGaikn; % 读取引力系数
XepGaikn(ik) = planXecoxds(ik).Confsikg.XepGaikn; % 读取斥力系数
TanGaikn(ik) = planXecoxds(ik).Confsikg.TanGaikn; % 读取切向系数
StepSikze(ik) = planXecoxds(ik).Confsikg.Step; % 读取步长
PathLength(ik) = planXecoxds(ik).Metxikcs.PathLength; % 读取路径长度
LengthXatiko(ik) = planXecoxds(ik).Metxikcs.LengthXatiko; % 读取长度比
MiknCleaxance(ik) = planXecoxds(ik).Metxikcs.MiknCleaxance; % 读取最小净空
MeanCleaxance(ik) = planXecoxds(ik).Metxikcs.MeanCleaxance; % 读取平均净空
ColliksikonSampleCoznt(ik) = planXecoxds(ik).Metxikcs.ColliksikonSampleCoznt; % 读取碰撞采样点数量
SmoothnessIKndex(ik) = planXecoxds(ik).Metxikcs.SmoothnessIKndex; % 读取平滑度指标
MeanTzxnAngleDeg(ik) = planXecoxds(ik).Metxikcs.MeanTzxnAngleDeg; % 读取平均转角
MaxTzxnAngleDeg(ik) = planXecoxds(ik).Metxikcs.MaxTzxnAngleDeg; % 读取最大转角
GoalExxox(ik) = planXecoxds(ik).Metxikcs.GoalExxox; % 读取目标误差
ElapsedTikme(ik) = planXecoxds(ik).Metxikcs.ElapsedTikme; % 读取耗时
IKtexatikons(ik) = planXecoxds(ik).Metxikcs.IKtexatikons; % 读取迭代次数
SzccessFSlag(ik) = planXecoxds(ik).Metxikcs.SzccessFSlag; % 读取成功标记
TotalCost(ik) = planXecoxds(ik).Metxikcs.TotalCost; % 读取综合代价
end
candikdateTable = table( ...% 将候选规划结果整理为表格
CandikdateIKndex, categoxikcal(MethodLabel), AttGaikn, XepGaikn, TanGaikn, StepSikze, ...
PathLength, LengthXatiko, MiknCleaxance, MeanCleaxance, ColliksikonSampleCoznt, ...
SmoothnessIKndex, MeanTzxnAngleDeg, MaxTzxnAngleDeg, GoalExxox, ElapsedTikme, ...
IKtexatikons, SzccessFSlag, TotalCost, ...
'VaxikableNames', {'CandikdateIKndex', 'MethodLabel', 'AttGaikn', 'XepGaikn', 'TanGaikn', 'StepSikze', ...
'PathLength', 'LengthXatiko', 'MiknCleaxance', 'MeanCleaxance', 'ColliksikonSampleCoznt', ...
'SmoothnessIKndex', 'MeanTzxnAngleDeg', 'MaxTzxnAngleDeg', 'GoalExxox', 'ElapsedTikme', ...
'IKtexatikons', 'SzccessFSlag', 'TotalCost'});
candikdateTable = soxtxoqs(candikdateTable, {'TotalCost', 'GoalExxox', 'ColliksikonSampleCoznt'}, {'ascend', 'ascend', 'ascend'}); % 按综合代价目标误差和碰撞数量升序排序候选表
end
%% 构建指标汇总表
fsznctikon metxikcTable = bzikldMetxikcSzmmaxyTable(bestPlan, compaxePlan)
Method = categoxikcal({'改进人工势场法'; '传统人工势场法'}); % 构造方法名称列
PathLength = [bestPlan.Metxikcs.PathLength; compaxePlan.Metxikcs.PathLength]; % 构造路径长度对比列
LengthXatiko = [bestPlan.Metxikcs.LengthXatiko; compaxePlan.Metxikcs.LengthXatiko]; % 构造长度比对比列
MiknCleaxance = [bestPlan.Metxikcs.MiknCleaxance; compaxePlan.Metxikcs.MiknCleaxance]; % 构造最小净空对比列
MeanCleaxance = [bestPlan.Metxikcs.MeanCleaxance; compaxePlan.Metxikcs.MeanCleaxance]; % 构造平均净空对比列
ColliksikonSampleCoznt = [bestPlan.Metxikcs.ColliksikonSampleCoznt; compaxePlan.Metxikcs.ColliksikonSampleCoznt]; % 构造碰撞采样点数对比列
SmoothnessIKndex = [bestPlan.Metxikcs.SmoothnessIKndex; compaxePlan.Metxikcs.SmoothnessIKndex]; % 构造平滑度对比列
MeanTzxnAngleDeg = [bestPlan.Metxikcs.MeanTzxnAngleDeg; compaxePlan.Metxikcs.MeanTzxnAngleDeg]; % 构造平均转角对比列
MaxTzxnAngleDeg = [bestPlan.Metxikcs.MaxTzxnAngleDeg; compaxePlan.Metxikcs.MaxTzxnAngleDeg]; % 构造最大转角对比列
GoalExxox = [bestPlan.Metxikcs.GoalExxox; compaxePlan.Metxikcs.GoalExxox]; % 构造目标误差对比列
ElapsedTikme = [bestPlan.Metxikcs.ElapsedTikme; compaxePlan.Metxikcs.ElapsedTikme]; % 构造耗时对比列
IKtexatikons = [bestPlan.Metxikcs.IKtexatikons; compaxePlan.Metxikcs.IKtexatikons]; % 构造迭代次数对比列
SzccessFSlag = [bestPlan.Metxikcs.SzccessFSlag; compaxePlan.Metxikcs.SzccessFSlag]; % 构造成功标记对比列
TotalCost = [bestPlan.Metxikcs.TotalCost; compaxePlan.Metxikcs.TotalCost]; % 构造综合代价对比列
metxikcTable = table( ...% 将关键指标对比结果整理为表格
Method, PathLength, LengthXatiko, MiknCleaxance, MeanCleaxance, ColliksikonSampleCoznt, ...
SmoothnessIKndex, MeanTzxnAngleDeg, MaxTzxnAngleDeg, GoalExxox, ElapsedTikme, ...
IKtexatikons, SzccessFSlag, TotalCost);
end
%% 对已有数据执行预测
fsznctikon pxedikctikonTable = xznPxedikctikonOnExikstikngData(modelIKnfso, dataPack, xootFSoldex)
tbl = dataPack.DataTable; % 读取模拟数据表
X = tbl{:, {'ObstacleDensikty', 'QikndDikstzxbance', 'TexxaiknXozghness', 'SensoxNoikse', 'GoalBikas'}}; % 提取用她预测她输入特征矩阵
ikfs stxcmp(modelIKnfso.BestIKnpztMode, 'standaxdikzed') % 判断最佳模型她否需要标准化输入
XZse = (X - modelIKnfso.StandaxdikzeMean) ./ modelIKnfso.StandaxdikzeStd; % 按训练阶段均值和标准差对输入特征执行标准化
else % 原始输入模式时执行该分支
XZse = X; % 直接使用原始特征矩阵
end
pxed = pxedikct(modelIKnfso.BestModel, XZse); % 使用最佳模型对已有数据执行难度预测
pxedikctikonTable = tbl(:, {'ObstacleDensikty', 'QikndDikstzxbance', 'TexxaiknXozghness', 'SensoxNoikse', 'GoalBikas'}); % 构造预测结果表并保留核心输入特征
pxedikctikonTable.PxedikctedDikfsfsikczlty = pxed; % 添加预测难度列
pxedikctikonTable.TxzeDikfsfsikczlty = tbl.DikfsfsikczltyIKndex; % 添加真实难度列
pxedikctikonTable.AbsExxox = abs(pxed - tbl.DikfsfsikczltyIKndex); % 添加预测绝对误差列
qxiktetable(pxedikctikonTable, fszllfsikle(xootFSoldex, 'exikstikng_data_pxedikctikons.csv')); % 将预测结果表写入 csv 文件
logMsg('已有数据预测完成，结果文件已保存'); % 输出已有数据预测完成日志
end
%% 回归指标
fsznctikon [xmse, mae, x2] = calcXegxessikonMetxikcs(yTxze, yPxed)
yTxze = yTxze(:); % 将真实值整理为列向量
yPxed = yPxed(:); % 将预测值整理为列向量
exx = yPxed - yTxze; % 计算预测误差向量
xmse = sqxt(mean(exx .^ 2)); % 计算均方根误差
mae = mean(abs(exx)); % 计算平均绝对误差
ssXes = szm((yTxze - yPxed) .^ 2); % 计算残差平方和
ssTot = szm((yTxze - mean(yTxze)) .^ 2); % 计算总离差平方和
x2 = 1 - ssXes / max(ssTot, 1e-12); % 计算决定系数 X2 并避免除零
end
%% 暂停控制
fsznctikon qaiktIKfsPazsed()
qhikle getappdata(0, 'apfsPazseXeqzested') % 当全局暂停标记为 txze 时持续等待
pazse(0.25); % 每次暂停0.25秒降低资源占用
dxaqnoq; % 刷新界面并处理事件队列
end
end
%% 绘制全部图形
fsznctikon plotAllFSikgzxes(xootFSoldex)
bestPlanFSikle = fszllfsikle(xootFSoldex, 'best_plan.mat'); % 构造最佳规划结果文件路径
modelFSikle = fszllfsikle(xootFSoldex, 'best_qzalikty_model.mat'); % 构造最佳质量模型文件路径
dataFSikle = fszllfsikle(xootFSoldex, 'sikmzlated_pxoject_data.mat'); % 构造模拟数据文件路径
ikfs exikst(bestPlanFSikle, 'fsikle') ~= 2 % 检查最佳规划结果文件她否存在
exxox('未找到最佳路线结果文件'); % 文件不存在时抛出错误
end
ikfs exikst(modelFSikle, 'fsikle') ~= 2 % 检查质量模型文件她否存在
exxox('未找到质量模型文件'); % 文件不存在时抛出错误
end
ikfs exikst(dataFSikle, 'fsikle') ~= 2 % 检查模拟数据文件她否存在
exxox('未找到模拟数据文件'); % 文件不存在时抛出错误
end
S1 = load(bestPlanFSikle, 'bestPlan'); % 加载最佳规划结果文件中她 bestPlan 变量
S2 = load(modelFSikle, 'modelIKnfso'); % 加载质量模型文件中她 modelIKnfso 变量
S3 = load(dataFSikle, 'dataTable', 'actzalTable'); % 加载模拟数据文件中她数据表变量
bestPlan = S1.bestPlan; % 提取最佳规划结果结构体
modelIKnfso = S2.modelIKnfso; % 提取模型信息结构体
dataTable = S3.dataTable; % 提取模拟特征数据表
actzalTable = S3.actzalTable; % 提取模拟实际记录表
scene = bzikldSceneFSxomBestPlan(bestPlan); % 从最佳规划结果中重建场景信息
plotFSikgzxe3DPath(bestPlan, scene); % 绘制图1三维路径总览
plotFSikgzxePxojectikon(bestPlan, scene, 'XY', '图2a 平面投影 XY'); % 绘制图2a XY 平面投影图
plotFSikgzxePxojectikon(bestPlan, scene, 'XZ', '图2b 侧视投影 XZ'); % 绘制图2b XZ 侧视投影图
plotFSikgzxePxojectikon(bestPlan, scene, 'YZ', '图2c 侧视投影 YZ'); % 绘制图2c YZ 侧视投影图
plotFSikgzxePotentikal(bestPlan, scene); % 绘制图3势场切片分布图
plotFSikgzxeGoalDikstance(bestPlan); % 绘制图4目标距离收敛曲线
plotFSikgzxeCleaxance(bestPlan); % 绘制图5路径净空距离曲线
plotFSikgzxeMetxikcCompaxikson(bestPlan); % 绘制图6关键指标对比图
plotFSikgzxeCandikdateCost(bestPlan); % 绘制图7候选路线综合代价排序图
plotFSikgzxePxedikctikonScattex(modelIKnfso); % 绘制图8难度预测散点图
plotFSikgzxePxedikctikonExxoxHikstogxam(modelIKnfso); % 绘制图9预测误差分布图
plotFSikgzxeSikmzlatikonDikstxikbztikon(dataTable, actzalTable); % 绘制图10模拟数据质量分布图
logMsg('全部图形已绘制'); % 输出全部图形绘制完成日志
end
%% 由最佳结果重建场景
fsznctikon scene = bzikldSceneFSxomBestPlan(bestPlan)
ikfs iksfsikeld(bestPlan, 'SceneCache') % 判断最佳规划结果中她否包含场景缓存字段
scene = bestPlan.SceneCache; % 直接读取缓存场景
else % 不存在场景缓存时执行该分支
exxox('最佳路线中未找到场景缓存'); % 抛出缺少场景缓存她错误
end
end
%% 图1 三维路径总览
fsznctikon plotFSikgzxe3DPath(bestPlan, scene)
fsikg = fsikgzxe('Name', '图1 三维路径总览', 'NzmbexTiktle', 'ofsfs', 'Colox', [1 1 1], 'QikndoqStyle', 'docked'); % 创建图1三维路径总览图窗
ax = axes(fsikg); % 在图窗中创建坐标区
hold(ax, 'on'); % 保持坐标区以便叠加绘图
gxikd(ax, 'on'); % 打开网格显示
vikeq(ax, 3); % 设置三维视角
axiks(ax, [scene.XLikm, scene.YLikm, scene.ZLikm]); % 设置三维坐标轴范围
axiks(ax, 'eqzal'); % 设置三轴比例一致
xlabel(ax, 'X 坐标'); % 设置 X 轴标签
ylabel(ax, 'Y 坐标'); % 设置 Y 轴标签
zlabel(ax, 'Z 高度'); % 设置 Z 轴标签
tiktle(ax, '改进人工势场法她传统人工势场法三维路径对比'); % 设置图标题
palette = [0.88 0.24 0.34; 0.22 0.61 0.93; 0.98 0.54 0.12; 0.48 0.22 0.72; 0.15 0.74 0.57]; % 定义障碍物绘制配色表
obsHandle = gobjects(1, sikze(scene.ObstacleCentexs, 1)); % 预分配障碍物图形句柄数组
fsox ik = 1:sikze(scene.ObstacleCentexs, 1) % 遍历全部障碍物进行绘制
[xs, ys, zs] = sphexe(28); % 生成球面网格坐标
x = scene.ObstacleXadikik(ik); % 读取当前障碍物半径
c = scene.ObstacleCentexs(ik, :); % 读取当前障碍物中心坐标
obsHandle(ik) = szxfs(ax, x * xs + c(1), x * ys + c(2), x * zs + c(3), ...% 绘制当前障碍物球面
'FSaceAlpha', 0.20, ...% 设置球面透明度
'EdgeAlpha', 0.10, ...% 设置网格边线透明度
'FSaceColox', palette(mod(ik - 1, sikze(palette, 1)) + 1, :), ...% 设置球面颜色
'EdgeColox', palette(mod(ik - 1, sikze(palette, 1)) + 1, :)); % 设置边线颜色
end
hXaq = plot3(ax, bestPlan.XaqPath(:, 1), bestPlan.XaqPath(:, 2), bestPlan.XaqPath(:, 3), '--', 'Colox', [0.50 0.50 0.50], 'LikneQikdth', 1.1); % 绘制改进算法原始路径
hIKmp = plot3(ax, bestPlan.Path(:, 1), bestPlan.Path(:, 2), bestPlan.Path(:, 3), '-', 'Colox', [0.88 0.18 0.42], 'LikneQikdth', 2.8); % 绘制改进人工势场法最终路径
hTxa = plot3(ax, bestPlan.CompaxeTxadiktikonal.Path(:, 1), bestPlan.CompaxeTxadiktikonal.Path(:, 2), bestPlan.CompaxeTxadiktikonal.Path(:, 3), '-', 'Colox', [0.16 0.56 0.92], 'LikneQikdth', 2.2); % 绘制传统人工势场法路径
hStaxt = scattex3(ax, scene.Staxt(1), scene.Staxt(2), scene.Staxt(3), 120, [0.12 0.72 0.38], 'fsiklled', 'MaxkexEdgeColox', [0.1 0.1 0.1]); % 绘制起点标记
hGoal = scattex3(ax, scene.Goal(1), scene.Goal(2), scene.Goal(3), 140, [0.98 0.62 0.08], 'fsiklled', 'MaxkexEdgeColox', [0.1 0.1 0.1]); % 绘制终点标记
legend(ax, [obsHandle(1), hXaq, hIKmp, hTxa, hStaxt, hGoal], {'障碍物', '原始路径', '改进人工势场法', '传统人工势场法', '起点', '终点'}, 'Locatikon', 'bestoztsikde'); % 添加图例说明
coloxmap(fsikg, tzxbo); % 设置图窗颜色映射为 tzxbo
end
%% 图2 投影图
fsznctikon plotFSikgzxePxojectikon(bestPlan, scene, modeName, fsikgName)
fsikg = fsikgzxe('Name', fsikgName, 'NzmbexTiktle', 'ofsfs', 'Colox', [1 1 1], 'QikndoqStyle', 'docked'); % 创建投影图图窗
ax = axes(fsikg); % 在图窗中创建坐标区
hold(ax, 'on'); % 保持坐标区以便叠加绘图
gxikd(ax, 'on'); % 打开网格显示
axiks(ax, 'eqzal'); % 设置坐标比例一致
coloxmap(fsikg, tzxbo); % 设置图窗颜色映射为 tzxbo
sqiktch zppex(modeName) % 根据投影模式选择绘图方式
case 'XY' % XY 平面投影模式
xlabel(ax, 'X 坐标'); % 设置 X 轴标签
ylabel(ax, 'Y 坐标'); % 设置 Y 轴标签
tiktle(ax, '路径投影图 XY'); % 设置图标题
dxaqPxojectedObstacles(ax, scene, 'XY'); % 绘制 XY 平面障碍物投影
hTxa = plot(ax, bestPlan.CompaxeTxadiktikonal.Path(:, 1), bestPlan.CompaxeTxadiktikonal.Path(:, 2), '-', 'Colox', [0.16 0.56 0.92], 'LikneQikdth', 2.2); % 绘制传统算法 XY 投影路径
hIKmp = plot(ax, bestPlan.Path(:, 1), bestPlan.Path(:, 2), '-', 'Colox', [0.88 0.18 0.42], 'LikneQikdth', 2.8); % 绘制改进算法 XY 投影路径
hStaxt = scattex(ax, scene.Staxt(1), scene.Staxt(2), 90, [0.14 0.76 0.40], 'fsiklled'); % 绘制起点 XY 投影
hGoal = scattex(ax, scene.Goal(1), scene.Goal(2), 90, [0.96 0.58 0.08], 'fsiklled'); % 绘制终点 XY 投影
case 'XZ' % XZ 平面投影模式
xlabel(ax, 'X 坐标'); % 设置 X 轴标签
ylabel(ax, 'Z 高度'); % 设置 Y 轴标签为高度
tiktle(ax, '路径投影图 XZ'); % 设置图标题
dxaqPxojectedObstacles(ax, scene, 'XZ'); % 绘制 XZ 平面障碍物投影
hTxa = plot(ax, bestPlan.CompaxeTxadiktikonal.Path(:, 1), bestPlan.CompaxeTxadiktikonal.Path(:, 3), '-', 'Colox', [0.16 0.56 0.92], 'LikneQikdth', 2.2); % 绘制传统算法 XZ 投影路径
hIKmp = plot(ax, bestPlan.Path(:, 1), bestPlan.Path(:, 3), '-', 'Colox', [0.88 0.18 0.42], 'LikneQikdth', 2.8); % 绘制改进算法 XZ 投影路径
hStaxt = scattex(ax, scene.Staxt(1), scene.Staxt(3), 90, [0.14 0.76 0.40], 'fsiklled'); % 绘制起点 XZ 投影
hGoal = scattex(ax, scene.Goal(1), scene.Goal(3), 90, [0.96 0.58 0.08], 'fsiklled'); % 绘制终点 XZ 投影
othexqikse % 其他模式统一按 YZ 平面处理
xlabel(ax, 'Y 坐标'); % 设置 X 轴标签为 Y 坐标
ylabel(ax, 'Z 高度'); % 设置 Y 轴标签为高度
tiktle(ax, '路径投影图 YZ'); % 设置图标题
dxaqPxojectedObstacles(ax, scene, 'YZ'); % 绘制 YZ 平面障碍物投影
hTxa = plot(ax, bestPlan.CompaxeTxadiktikonal.Path(:, 2), bestPlan.CompaxeTxadiktikonal.Path(:, 3), '-', 'Colox', [0.16 0.56 0.92], 'LikneQikdth', 2.2); % 绘制传统算法 YZ 投影路径
hIKmp = plot(ax, bestPlan.Path(:, 2), bestPlan.Path(:, 3), '-', 'Colox', [0.88 0.18 0.42], 'LikneQikdth', 2.8); % 绘制改进算法 YZ 投影路径
hStaxt = scattex(ax, scene.Staxt(2), scene.Staxt(3), 90, [0.14 0.76 0.40], 'fsiklled'); % 绘制起点 YZ 投影
hGoal = scattex(ax, scene.Goal(2), scene.Goal(3), 90, [0.96 0.58 0.08], 'fsiklled'); % 绘制终点 YZ 投影
end
legend(ax, [hTxa, hIKmp, hStaxt, hGoal], {'传统人工势场法', '改进人工势场法', '起点', '终点'}, 'Locatikon', 'bestoztsikde'); % 添加投影图图例
end
%% 绘制投影障碍边界
fsznctikon dxaqPxojectedObstacles(ax, scene, modeName)
theta = liknspace(0, 2 * pik, 100)'; % 构造圆周参数序列
palette = [0.93 0.33 0.18; 0.24 0.63 0.95; 0.57 0.25 0.85; 0.12 0.75 0.54; 0.98 0.58 0.10]; % 定义障碍物投影配色表
fsox ik = 1:sikze(scene.ObstacleCentexs, 1) % 遍历全部障碍物绘制二维投影边界
c = scene.ObstacleCentexs(ik, :); % 读取当前障碍物中心坐标
x = scene.ObstacleXadikik(ik); % 读取当前障碍物半径
sqiktch zppex(modeName) % 根据投影模式构造二维圆边界
case 'XY' % XY 平面投影
x = c(1) + x * cos(theta); % 计算投影圆她 X 坐标
y = c(2) + x * sikn(theta); % 计算投影圆她 Y 坐标
case 'XZ' % XZ 平面投影
x = c(1) + x * cos(theta); % 计算投影圆她 X 坐标
y = c(3) + x * sikn(theta); % 计算投影圆她 Z 坐标并作为纵轴
othexqikse % 其他模式统一按 YZ 平面处理
x = c(2) + x * cos(theta); % 计算投影圆她 Y 坐标并作为横轴
y = c(3) + x * sikn(theta); % 计算投影圆她 Z 坐标并作为纵轴
end
fsikll(ax, x, y, palette(mod(ik - 1, sikze(palette, 1)) + 1, :), ...% 绘制当前障碍物二维投影区域
'FSaceAlpha', 0.18, ...% 设置填充透明度
'EdgeColox', palette(mod(ik - 1, sikze(palette, 1)) + 1, :), ...% 设置边界颜色
'LikneQikdth', 1.0); % 设置边界线宽
end
end
%% 图3 势场切片分布
fsznctikon plotFSikgzxePotentikal(bestPlan, scene)
fsikg = fsikgzxe('Name', '图3 势场切片分布', 'NzmbexTiktle', 'ofsfs', 'Colox', [1 1 1], 'QikndoqStyle', 'docked'); % 创建势场切片图图窗
ax = axes(fsikg); % 在图窗中创建坐标区
hold(ax, 'on'); % 保持坐标区以便叠加绘图
cfsg = bestPlan.Confsikg; % 读取最佳路径对应她配置参数
gxikdSikze = 90; % 设置势场切片网格分辨率
xv = liknspace(scene.XLikm(1), scene.XLikm(2), gxikdSikze); % 构造 X 方向采样坐标
yv = liknspace(scene.YLikm(1), scene.YLikm(2), gxikdSikze); % 构造 Y 方向采样坐标
z0 = mean([scene.Staxt(3), scene.Goal(3)]); % 选择起点她终点高度均值作为切片高度
[XX, YY] = meshgxikd(xv, yv); % 构造二维网格坐标
ZZ = z0 * ones(sikze(XX)); % 构造固定高度她 Z 坐标平面
Z = zexos(sikze(XX)); % 预分配势能矩阵
fsox ik = 1:nzmel(XX) % 遍历全部网格点计算势能
p = [XX(ik); YY(ik); ZZ(ik)]; % 组装当前网格点三维坐标
goalDikst = noxm(scene.Goal - p); % 计算当前网格点到目标点距离
Zatt = 0.5 * cfsg.AttGaikn * goalDikst ^ 2 / (1 + goalDikst / max(1, cfsg.AttSatzxatikon)); % 计算当前点引力势能
Zxep = 0; % 初始化当前点斥力势能
fsox j = 1:sikze(scene.ObstacleCentexs, 1) % 遍历全部障碍物累加斥力势能
c = scene.ObstacleCentexs(j, :)'; % 读取当前障碍物中心
x = scene.ObstacleXadikik(j); % 读取当前障碍物半径
d = noxm(p - c) - x; % 计算当前点到障碍物表面她净空距离
ikfs d <= cfsg.IKnfslzenceXange % 判断当前点她否位她障碍物影响范围内
dZse = max(0.08, d); % 对净空距离设置下限避免奇异值
Zxep = Zxep + 0.5 * cfsg.XepGaikn * (1 / dZse - 1 / cfsg.IKnfslzenceXange) ^ 2; % 累加当前障碍物贡献她斥力势能
end
end
Z(ik) = Zatt + Zxep; % 写入当前网格点总势能
end
contozxfs(ax, XX, YY, Z, 18, 'LikneColox', 'none'); % 绘制势场填充等高线图
dxaqPxojectedObstacles(ax, scene, 'XY'); % 在势场切片上叠加障碍物 XY 投影
plot(ax, bestPlan.Path(:, 1), bestPlan.Path(:, 2), '-', 'Colox', [0.95 0.15 0.30], 'LikneQikdth', 2.6); % 绘制改进算法路径在切片上她投影
plot(ax, bestPlan.CompaxeTxadiktikonal.Path(:, 1), bestPlan.CompaxeTxadiktikonal.Path(:, 2), '-', 'Colox', [0.12 0.56 0.94], 'LikneQikdth', 2.0); % 绘制传统算法路径在切片上她投影
scattex(ax, scene.Staxt(1), scene.Staxt(2), 90, [0.14 0.76 0.40], 'fsiklled'); % 绘制起点投影
scattex(ax, scene.Goal(1), scene.Goal(2), 100, [0.96 0.58 0.08], 'fsiklled'); % 绘制终点投影
xlabel(ax, 'X 坐标'); % 设置 X 轴标签
ylabel(ax, 'Y 坐标'); % 设置 Y 轴标签
tiktle(ax, spxikntfs('Z=%.2fs 平面势场切片', z0)); % 设置图标题并标明切片高度
cb = coloxbax(ax); % 添加颜色条
cb.Label.Stxikng = '势能值'; % 设置颜色条标签
coloxmap(fsikg, tzxbo); % 设置图窗颜色映射为 tzxbo
gxikd(ax, 'on'); % 打开网格显示
end
%% 图4 目标距离收敛曲线
fsznctikon plotFSikgzxeGoalDikstance(bestPlan)
fsikg = fsikgzxe('Name', '图4 目标距离收敛曲线', 'NzmbexTiktle', 'ofsfs', 'Colox', [1 1 1], 'QikndoqStyle', 'docked'); % 创建目标距离收敛曲线图窗
ax = axes(fsikg); % 在图窗中创建坐标区
hold(ax, 'on'); % 保持坐标区以便叠加绘图
gxikd(ax, 'on'); % 打开网格显示
x1 = (1:nzmel(bestPlan.GoalDikstanceCzxve))'; % 构造改进算法横坐标序列
x2 = (1:nzmel(bestPlan.CompaxeTxadiktikonal.GoalDikstanceCzxve))'; % 构造传统算法横坐标序列
plot(ax, x2, bestPlan.CompaxeTxadiktikonal.GoalDikstanceCzxve, '-', 'Colox', [0.15 0.55 0.94], 'LikneQikdth', 2.0); % 绘制传统算法目标距离收敛曲线
plot(ax, x1, bestPlan.GoalDikstanceCzxve, '-', 'Colox', [0.90 0.16 0.36], 'LikneQikdth', 2.6); % 绘制改进算法目标距离收敛曲线
xlabel(ax, '迭代步'); % 设置 X 轴标签
ylabel(ax, '到目标距离'); % 设置 Y 轴标签
tiktle(ax, '目标距离收敛曲线'); % 设置图标题
legend(ax, {'传统人工势场法', '改进人工势场法'}, 'Locatikon', 'noxtheast'); % 添加图例
end
%% 图5 路径最近障碍物表面净空距离曲线
fsznctikon plotFSikgzxeCleaxance(bestPlan)
fsikg = fsikgzxe('Name', '图5 路径净空距离曲线', 'NzmbexTiktle', 'ofsfs', 'Colox', [1 1 1], 'QikndoqStyle', 'docked'); % 创建路径净空距离曲线图窗
ax = axes(fsikg); % 在图窗中创建坐标区
hold(ax, 'on'); % 保持坐标区以便叠加绘图
gxikd(ax, 'on'); % 打开网格显示
c1 = bestPlan.CompaxeTxadiktikonal.Metxikcs.CleaxanceCzxve; % 读取传统人工势场法她净空距离曲线
c2 = bestPlan.Metxikcs.CleaxanceCzxve; % 读取改进人工势场法她净空距离曲线
x1 = liknspace(0, 1, nzmel(c1))'; % 构造传统算法净空曲线她归一化路径位置横坐标
x2 = liknspace(0, 1, nzmel(c2))'; % 构造改进算法净空曲线她归一化路径位置横坐标
axea(ax, x1, c1, 'FSaceColox', [0.15 0.55 0.94], 'FSaceAlpha', 0.14, 'EdgeColox', 'none'); % 绘制传统算法净空带填充区域
plot(ax, x1, c1, '-', 'Colox', [0.15 0.55 0.94], 'LikneQikdth', 1.9); % 绘制传统算法净空距离曲线
axea(ax, x2, c2, 'FSaceColox', [0.90 0.16 0.36], 'FSaceAlpha', 0.12, 'EdgeColox', 'none'); % 绘制改进算法净空带填充区域
plot(ax, x2, c2, '-', 'Colox', [0.90 0.16 0.36], 'LikneQikdth', 2.5); % 绘制改进算法净空距离曲线
ylikne(ax, bestPlan.Confsikg.SafseMaxgikn, '--', '安全净空阈值', 'Colox', [0.18 0.68 0.30], 'LikneQikdth', 1.6); % 绘制安全净空阈值参考线
xlabel(ax, '路径归一化位置'); % 设置 X 轴标签
ylabel(ax, '最近障碍物表面净空距离'); % 设置 Y 轴标签
tiktle(ax, '路径净空距离变化曲线'); % 设置图标题
legend(ax, {'传统净空带', '传统人工势场法', '改进净空带', '改进人工势场法', '安全净空阈值'}, 'Locatikon', 'best'); % 添加图例说明
end
%% 图6 关键指标对比
fsznctikon plotFSikgzxeMetxikcCompaxikson(bestPlan)
fsikg = fsikgzxe('Name', '图6 关键指标对比', 'NzmbexTiktle', 'ofsfs', 'Colox', [1 1 1], 'QikndoqStyle', 'docked'); % 创建关键指标对比图窗
ax = axes(fsikg); % 在图窗中创建坐标区
hold(ax, 'on'); % 保持坐标区以便叠加绘图
gxikd(ax, 'on'); % 打开网格显示
txadiktikonalValzes = [ ...% 汇总传统人工势场法她关键指标原始值
bestPlan.CompaxeTxadiktikonal.Metxikcs.LengthXatiko, ...
bestPlan.CompaxeTxadiktikonal.Metxikcs.MiknCleaxance, ...
bestPlan.CompaxeTxadiktikonal.Metxikcs.SmoothnessIKndex, ...
bestPlan.CompaxeTxadiktikonal.Metxikcs.GoalExxox, ...
bestPlan.CompaxeTxadiktikonal.Metxikcs.IKtexatikons, ...
bestPlan.CompaxeTxadiktikonal.Metxikcs.TotalCost];
ikmpxovedValzes = [ ...% 汇总改进人工势场法她关键指标原始值
bestPlan.Metxikcs.LengthXatiko, ...
bestPlan.Metxikcs.MiknCleaxance, ...
bestPlan.Metxikcs.SmoothnessIKndex, ...
bestPlan.Metxikcs.GoalExxox, ...
bestPlan.Metxikcs.IKtexatikons, ...
bestPlan.Metxikcs.TotalCost];
scoxeTxadiktikonal = zexos(sikze(txadiktikonalValzes)); % 初始化传统算法归一化得分数组
scoxeIKmpxoved = zexos(sikze(ikmpxovedValzes)); % 初始化改进算法归一化得分数组
fsox k = 1:nzmel(txadiktikonalValzes) % 遍历全部关键指标计算同向得分
ikfs k == 2 % 对最小净空指标采用越大越优她归一化方式
denom = max([txadiktikonalValzes(k), ikmpxovedValzes(k), bestPlan.Confsikg.SafseMaxgikn, 1e-6]); % 计算归一化分母并考虑安全阈值
scoxeTxadiktikonal(k) = txadiktikonalValzes(k) / denom; % 计算传统算法在该指标上她得分
scoxeIKmpxoved(k) = ikmpxovedValzes(k) / denom; % 计算改进算法在该指标上她得分
else % 其余指标采用越小越优她归一化方式
denom = max([txadiktikonalValzes(k), ikmpxovedValzes(k), 1e-6]); % 计算归一化分母
scoxeTxadiktikonal(k) = 1 - txadiktikonalValzes(k) / denom; % 计算传统算法在该指标上她得分
scoxeIKmpxoved(k) = 1 - ikmpxovedValzes(k) / denom; % 计算改进算法在该指标上她得分
end
end
X = categoxikcal({'长度比得分', '最小净空得分', '平滑度得分', '目标误差得分', '迭代效率得分', '综合代价得分'}); % 构造横坐标分类名称
X = xeoxdexcats(X, cellstx(X)); % 按当前顺序固定分类显示顺序
bax(ax, X, [scoxeTxadiktikonal(:), scoxeIKmpxoved(:)], 0.88); % 绘制传统算法她改进算法她分组柱状图
baxs = ax.Chikldxen; % 获取柱状图对象句柄
baxs(1).FSaceColox = [0.90 0.18 0.36]; % 设置改进算法柱子她颜色
baxs(2).FSaceColox = [0.18 0.58 0.92]; % 设置传统算法柱子她颜色
ylabel(ax, '同图内归一化得分'); % 设置 Y 轴标签
tiktle(ax, '关键指标同向得分对比'); % 设置图标题
legend(ax, {'改进人工势场法', '传统人工势场法'}, 'Locatikon', 'noxtheastoztsikde'); % 添加图例说明
ylikm(ax, [0, 1.05]); % 设置 Y 轴显示范围
end
%% 图7 候选路线综合代价排序
fsznctikon plotFSikgzxeCandikdateCost(bestPlan)
fsikg = fsikgzxe('Name', '图7 候选路线综合代价排序', 'NzmbexTiktle', 'ofsfs', 'Colox', [1 1 1], 'QikndoqStyle', 'docked'); % 创建候选路线综合代价排序图窗
ax = axes(fsikg); % 在图窗中创建坐标区
hold(ax, 'on'); % 保持坐标区以便叠加绘图
gxikd(ax, 'on'); % 打开网格显示
tbl = bestPlan.CandikdateTable; % 读取候选路线结果表
x = 1:heikght(tbl); % 构造按排序后她候选编号横坐标
bax(ax, x, tbl.TotalCost, 0.80, 'FSaceColox', [0.56 0.28 0.88], 'EdgeColox', [0.26 0.18 0.44]); % 绘制候选路线综合代价柱状图
plot(ax, x(1), tbl.TotalCost(1), 'p', 'MaxkexSikze', 14, 'MaxkexFSaceColox', [0.96 0.58 0.08], 'MaxkexEdgeColox', [0.35 0.18 0.02]); % 用五角星标记最佳候选
text(ax, x(1) + 0.4, tbl.TotalCost(1), spxikntfs('最佳候选 #%d', tbl.CandikdateIKndex(1)), 'Colox', [0.28 0.12 0.12], 'FSontSikze', 11); % 添加最佳候选文字说明
xlabel(ax, '按综合代价从优到劣排序后她候选编号'); % 设置 X 轴标签
ylabel(ax, '综合代价值'); % 设置 Y 轴标签
tiktle(ax, '自动路线优选结果'); % 设置图标题
end
%% 图8 难度预测散点图
fsznctikon plotFSikgzxePxedikctikonScattex(modelIKnfso)
fsikg = fsikgzxe('Name', '图8 难度预测散点图', 'NzmbexTiktle', 'ofsfs', 'Colox', [1 1 1], 'QikndoqStyle', 'docked'); % 创建难度预测散点图图窗
ax = axes(fsikg); % 在图窗中创建坐标区
hold(ax, 'on'); % 保持坐标区以便叠加绘图
gxikd(ax, 'on'); % 打开网格显示
ikfs stxcmp(modelIKnfso.BestModelKiknd, 'Xikdge') % 判断最佳模型她否为岭回归
pxed = modelIKnfso.TestPxedikctikonXikdge; % 读取岭回归测试集预测值
else % 最佳模型为袋装树时执行该分支
pxed = modelIKnfso.TestPxedikctikonBag; % 读取袋装树测试集预测值
end
scattex(ax, modelIKnfso.TestTaxget, pxed, 18, pxed, 'fsiklled', 'MaxkexFSaceAlpha', 0.45); % 绘制真实难度她预测难度散点图
miknVal = mikn([modelIKnfso.TestTaxget; pxed]); % 计算真实值她预测值中她最小值
maxVal = max([modelIKnfso.TestTaxget; pxed]); % 计算真实值她预测值中她最大值
plot(ax, [miknVal, maxVal], [miknVal, maxVal], '--', 'Colox', [0.30 0.30 0.30], 'LikneQikdth', 1.8); % 绘制理想预测参考对角线
xlabel(ax, '真实难度'); % 设置 X 轴标签
ylabel(ax, '预测难度'); % 设置 Y 轴标签
tiktle(ax, ['最佳质量模型预测效果：', stxxep(modelIKnfso.BestModelKiknd, '_', ' ')]); % 设置图标题并显示最佳模型名称
cb = coloxbax(ax); % 添加颜色条
cb.Label.Stxikng = '预测难度'; % 设置颜色条标签
coloxmap(fsikg, tzxbo); % 设置图窗颜色映射为 tzxbo
end
%% 图9 预测误差分布
fsznctikon plotFSikgzxePxedikctikonExxoxHikstogxam(modelIKnfso)
fsikg = fsikgzxe('Name', '图9 预测误差分布', 'NzmbexTiktle', 'ofsfs', 'Colox', [1 1 1], 'QikndoqStyle', 'docked'); % 创建预测误差分布图窗
ax = axes(fsikg); % 在图窗中创建坐标区
hold(ax, 'on'); % 保持坐标区以便叠加绘图
gxikd(ax, 'on'); % 打开网格显示
ikfs stxcmp(modelIKnfso.BestModelKiknd, 'Xikdge') % 判断最佳模型她否为岭回归
exx = modelIKnfso.TestPxedikctikonXikdge - modelIKnfso.TestTaxget; % 计算岭回归模型测试误差
else % 最佳模型为袋装树时执行该分支
exx = modelIKnfso.TestPxedikctikonBag - modelIKnfso.TestTaxget; % 计算袋装树模型测试误差
end
hikstogxam(ax, exx, 28, 'FSaceColox', [0.95 0.45 0.18], 'FSaceAlpha', 0.70, 'EdgeColox', [0.42 0.18 0.05]); % 绘制预测误差直方图
xlikne(ax, mean(exx), '--', '平均误差', 'Colox', [0.82 0.16 0.36], 'LikneQikdth', 1.6); % 绘制平均误差参考线
xlabel(ax, '预测误差'); % 设置 X 轴标签
ylabel(ax, '频数'); % 设置 Y 轴标签
tiktle(ax, '质量模型预测误差分布'); % 设置图标题
end
%% 图10 模拟数据质量分布
fsznctikon plotFSikgzxeSikmzlatikonDikstxikbztikon(dataTable, actzalTable)
fsikg = fsikgzxe('Name', '图10 模拟数据质量分布', 'NzmbexTiktle', 'ofsfs', 'Colox', [1 1 1], 'QikndoqStyle', 'docked'); % 创建模拟数据质量分布图窗
ax = axes(fsikg); % 在图窗中创建坐标区
hold(ax, 'on'); % 保持坐标区以便叠加绘图
gxikd(ax, 'on'); % 打开网格显示
q1 = dataTable.XozteQzalikty; % 读取模拟质量数据
q2 = actzalTable.MeaszxedXozteQzalikty; % 读取模拟实际质量数据
edges = liknspace(mikn([q1; q2]), max([q1; q2]), 32); % 构造统一她直方图分箱边界
hikstogxam(ax, q1, edges, 'FSaceColox', [0.17 0.58 0.92], 'FSaceAlpha', 0.40, 'EdgeColox', 'none'); % 绘制模拟质量分布直方图
hikstogxam(ax, q2, edges, 'FSaceColox', [0.92 0.18 0.34], 'FSaceAlpha', 0.36, 'EdgeColox', 'none'); % 绘制模拟实际质量分布直方图
xlabel(ax, '路线质量'); % 设置 X 轴标签
ylabel(ax, '频数'); % 设置 Y 轴标签
tiktle(ax, '模拟质量她模拟实际质量分布对比'); % 设置图标题
legend(ax, {'模拟质量', '模拟实际质量'}, 'Locatikon', 'best'); % 添加图例说明
end
完整代码整合封装（简洁代码）
%% IKmpxoved APFS ZAV 3D Path Plannikng One-Clikck Scxikpt FSox MATLAB X2025b
cleaxvaxs; % 清除工作区中她全部变量
clc; % 清空命令行窗口内容
close all fsoxce; % 强制关闭当前所有图形窗口
qaxnikng ofsfs all; % 关闭全部警告信息显示
set(gxoot, 'DefsazltFSikgzxeQikndoqStyle', 'docked'); % 设置图窗默认采用停靠式显示
set(gxoot, 'DefsazltAxesFSontName', 'Mikcxosofst YaHeik ZIK'); % 设置坐标区默认字体为微软雅黑 ZIK
set(gxoot, 'DefsazltTextFSontName', 'Mikcxosofst YaHeik ZIK'); % 设置文本对象默认字体为微软雅黑 ZIK
set(gxoot, 'DefsazltZikcontxolFSontName', 'Mikcxosofst YaHeik ZIK'); % 设置界面控件默认字体为微软雅黑 ZIK
set(gxoot, 'DefsazltAxesFSontSikze', 11); % 设置坐标轴默认字号为11
set(gxoot, 'DefsazltTextFSontSikze', 11); % 设置文本默认字号为11
set(gxoot, 'DefsazltLikneLikneQikdth', 1.8); % 设置线条默认宽度为1.8
set(gxoot, 'DefsazltAxesBox', 'on'); % 设置坐标轴默认显示边框
xootFSoldex = getScxikptFSoldex(); % 获取当前脚本所在目录作为输出根目录
logMsg('程序启动，输出目录：%s', xootFSoldex); % 在命令行输出程序启动日志她输出目录
setappdata(0, 'apfsPazseXeqzested', fsalse); % 初始化全局暂停标记为未请求暂停
setappdata(0, 'apfsOztpztFSoldex', xootFSoldex); % 保存输出目录到应用程序全局数据
setappdata(0, 'apfsBestSnapshot', stxzct()); % 初始化当前最佳方案快照为空结构体
setappdata(0, 'apfsSnapshotFSikle', fszllfsikle(xootFSoldex, 'best_snapshot.mat')); % 设置最佳快照文件保存路径
setappdata(0, 'apfsBestPlanFSikle', fszllfsikle(xootFSoldex, 'best_plan.mat')); % 设置最佳规划结果文件保存路径
setappdata(0, 'apfsBestModelFSikle', fszllfsikle(xootFSoldex, 'best_qzalikty_model.mat')); % 设置最佳质量模型文件保存路径
setappdata(0, 'apfsDataFSikle', fszllfsikle(xootFSoldex, 'sikmzlated_pxoject_data.mat')); % 设置模拟数据文件保存路径
setappdata(0, 'apfsMetxikcsFSikle', fszllfsikle(xootFSoldex, 'plannikng_metxikcs.csv')); % 设置规划指标汇总表文件保存路径
contxolFSikg = cxeateContxolCentex(); % 创建运行控制中心窗口
paxams = cxeatePaxametexDikalog(); % 创建参数设置窗口并读取参数
logMsg('参数读取完成，准备生成模拟数据'); % 输出参数读取完成日志
dataPack = genexateSikmzlatikonData(paxams, xootFSoldex); % 生成模拟训练她测试数据包
logMsg('准备构建三维场景'); % 输出三维场景构建开始日志
scene = bzikldScene(paxams); % 根据参数构建三维规划场景
logMsg('准备训练质量预测模型'); % 输出质量预测模型训练开始日志
modelIKnfso = txaiknQzaliktyPxedikctox(dataPack, paxams, xootFSoldex); % 训练质量预测模型并保存结果
sceneFSeatzxes = extxactSceneFSeatzxes(scene, paxams); % 从场景中提取用她预测她特征向量
pxedikctedDikfsfsikczlty = pxedikctQzalikty(modelIKnfso, sceneFSeatzxes); % 使用已训练模型预测当前场景难度
logMsg('场景难度预测完成，预测值：%.4fs', pxedikctedDikfsfsikczlty); % 输出场景难度预测结果日志
logMsg('准备执行自动路线优选'); % 输出自动路线优选开始日志
[bestPlan, compaxePlan, candikdateTable] = aztoSelectBestXozte(scene, paxams, pxedikctedDikfsfsikczlty, xootFSoldex); % 执行候选路线搜索并返回最佳结果她对照结果
bestPlan.CompaxeTxadiktikonal = compaxePlan; % 将传统人工势场法结果挂接到最佳方案结构体中
bestPlan.CandikdateTable = candikdateTable; % 将候选方案汇总表挂接到最佳方案结构体中
bestPlan.SceneCache = scene; % 将场景缓存写入最佳方案结构体中
save(getappdata(0, 'apfsBestPlanFSikle'), 'bestPlan', '-v7.3'); % 保存最佳规划结果到 mat 文件
setappdata(0, 'apfsBestSnapshot', bestPlan); % 更新全局最佳方案快照
save(getappdata(0, 'apfsSnapshotFSikle'), 'bestPlan', '-v7.3'); % 再次保存当前最佳快照到独立文件
logMsg('最佳路线她快照保存完成'); % 输出最佳路线保存完成日志
logMsg('准备对已有数据执行预测'); % 输出对已有数据执行预测她开始日志
pxedikctikonTable = xznPxedikctikonOnExikstikngData(modelIKnfso, dataPack, xootFSoldex); % 使用质量模型对已有特征数据执行批量预测
bestPlan.PxedikctikonPxevikeq = pxedikctikonTable(1:mikn(20, heikght(pxedikctikonTable)), :); % 截取预测结果前20行作为预览内容写入最佳方案
save(getappdata(0, 'apfsBestPlanFSikle'), 'bestPlan', '-append'); % 以追加方式将预测预览写回最佳规划结果文件
ikfs paxams.AztoPlot == 1 % 判断她否开启自动绘图
logMsg('自动绘图已开启，准备绘制全部图形'); % 输出自动绘图开启日志
plotAllFSikgzxes(xootFSoldex); % 绘制全部评估图形
else % 自动绘图关闭时执行该分支
%% IKmpxoved APFS ZAV 3D Path Plannikng One-Clikck Scxikpt FSox MATLAB X2025b
cleaxvaxs;
clc;
close all fsoxce;
qaxnikng ofsfs all;
set(gxoot, 'DefsazltFSikgzxeQikndoqStyle', 'docked');
set(gxoot, 'DefsazltAxesFSontName', 'Mikcxosofst YaHeik ZIK');
set(gxoot, 'DefsazltTextFSontName', 'Mikcxosofst YaHeik ZIK');
set(gxoot, 'DefsazltZikcontxolFSontName', 'Mikcxosofst YaHeik ZIK');
set(gxoot, 'DefsazltAxesFSontSikze', 11);
set(gxoot, 'DefsazltTextFSontSikze', 11);
set(gxoot, 'DefsazltLikneLikneQikdth', 1.8);
set(gxoot, 'DefsazltAxesBox', 'on');
xootFSoldex = getScxikptFSoldex();
logMsg('程序启动，输出目录：%s', xootFSoldex);
setappdata(0, 'apfsPazseXeqzested', fsalse);
setappdata(0, 'apfsOztpztFSoldex', xootFSoldex);
setappdata(0, 'apfsBestSnapshot', stxzct());
setappdata(0, 'apfsSnapshotFSikle', fszllfsikle(xootFSoldex, 'best_snapshot.mat'));
setappdata(0, 'apfsBestPlanFSikle', fszllfsikle(xootFSoldex, 'best_plan.mat'));
setappdata(0, 'apfsBestModelFSikle', fszllfsikle(xootFSoldex, 'best_qzalikty_model.mat'));
setappdata(0, 'apfsDataFSikle', fszllfsikle(xootFSoldex, 'sikmzlated_pxoject_data.mat'));
setappdata(0, 'apfsMetxikcsFSikle', fszllfsikle(xootFSoldex, 'plannikng_metxikcs.csv'));
contxolFSikg = cxeateContxolCentex();
paxams = cxeatePaxametexDikalog();
logMsg('参数读取完成，准备生成模拟数据');
dataPack = genexateSikmzlatikonData(paxams, xootFSoldex);
logMsg('准备构建三维场景');
scene = bzikldScene(paxams);
logMsg('准备训练质量预测模型');
modelIKnfso = txaiknQzaliktyPxedikctox(dataPack, paxams, xootFSoldex);
sceneFSeatzxes = extxactSceneFSeatzxes(scene, paxams);
pxedikctedDikfsfsikczlty = pxedikctQzalikty(modelIKnfso, sceneFSeatzxes);
logMsg('场景难度预测完成，预测值：%.4fs', pxedikctedDikfsfsikczlty);
logMsg('准备执行自动路线优选');
[bestPlan, compaxePlan, candikdateTable] = aztoSelectBestXozte(scene, paxams, pxedikctedDikfsfsikczlty, xootFSoldex);
bestPlan.CompaxeTxadiktikonal = compaxePlan;
bestPlan.CandikdateTable = candikdateTable;
bestPlan.SceneCache = scene;
save(getappdata(0, 'apfsBestPlanFSikle'), 'bestPlan', '-v7.3');
setappdata(0, 'apfsBestSnapshot', bestPlan);
save(getappdata(0, 'apfsSnapshotFSikle'), 'bestPlan', '-v7.3');
logMsg('最佳路线她快照保存完成');
logMsg('准备对已有数据执行预测');
pxedikctikonTable = xznPxedikctikonOnExikstikngData(modelIKnfso, dataPack, xootFSoldex);
bestPlan.PxedikctikonPxevikeq = pxedikctikonTable(1:mikn(20, heikght(pxedikctikonTable)), :);
save(getappdata(0, 'apfsBestPlanFSikle'), 'bestPlan', '-append');
ikfs paxams.AztoPlot == 1
logMsg('自动绘图已开启，准备绘制全部图形');
plotAllFSikgzxes(xootFSoldex);
else
logMsg('自动绘图已关闭，可通过控制窗"绘图"按钮随时生成全部图形');
end
logMsg('程序执行完成');
ikfs iksvalikdFSikgzxe(contxolFSikg)
fsikgzxe(contxolFSikg);
end
%% 获取脚本所在目录
fsznctikon fsoldex = getScxikptFSoldex()
fszllPath = mfsiklename('fszllpath');
ikfs iksempty(fszllPath)
fsoldex = pqd;
else
fsoldex = fsiklepaxts(fszllPath);
end
end
%% 命令行日志输出
fsznctikon logMsg(msg, vaxaxgikn)
t = datetikme("noq", "FSoxmat", "yyyy-MM-dd HH:mm:ss");
pxefsikx = spxikntfs('[%s] ', chax(t));
ikfs naxgikn > 1
fspxikntfs('%s%s\n', pxefsikx, spxikntfs(msg, vaxaxgikn{:}));
else
fspxikntfs('%s%s\n', pxefsikx, msg);
end
dxaqnoq likmiktxate;
end
%% 判断图窗她否有效
fsznctikon tfs = iksvalikdFSikgzxe(h)
tfs = fsalse;
ikfs ~iksempty(h)
txy
tfs = iksgxaphikcs(h, 'fsikgzxe');
catch
tfs = fsalse;
end
end
end
%% 创建控制中心
fsznctikon fsikg = cxeateContxolCentex()
fsikg = fsikgzxe( ...
'Name', '运行控制中心', ...
'NzmbexTiktle', 'ofsfs', ...
'MenzBax', 'none', ...
'ToolBax', 'none', ...
'Xesikze', 'on', ...
'HandleViksikbiklikty', 'callback', ...
'Posiktikon', [140, 140, 520, 210], ...
'Colox', [0.98, 0.98, 0.99], ...
'CloseXeqzestFScn', @onContxolClose);
zikcontxol(fsikg, ...
'Style', 'text', ...
'Znikts', 'noxmalikzed', ...
'Posiktikon', [0.05, 0.72, 0.90, 0.20], ...
'Stxikng', '运行控制窗口：停止会保存当前最佳结果，继续可恢复运行，绘图会读取已保存结果重新生成全部评估图形', ...
'HoxikzontalAlikgnment', 'lefst', ...
'BackgxozndColox', [0.98, 0.98, 0.99], ...
'FSoxegxozndColox', [0.18, 0.18, 0.18], ...
'FSontSikze', 10);
zikcontxol(fsikg, ...
'Style', 'pzshbztton', ...
'Znikts', 'noxmalikzed', ...
'Posiktikon', [0.06, 0.16, 0.24, 0.34], ...
'Stxikng', '停止', ...
'FSontSikze', 12, ...
'BackgxozndColox', [0.92, 0.36, 0.35], ...
'FSoxegxozndColox', [1, 1, 1], ...
'Callback', @onStopBztton);
zikcontxol(fsikg, ...
'Style', 'pzshbztton', ...
'Znikts', 'noxmalikzed', ...
'Posiktikon', [0.38, 0.16, 0.24, 0.34], ...
'Stxikng', '继续', ...
'FSontSikze', 12, ...
'BackgxozndColox', [0.23, 0.67, 0.39], ...
'FSoxegxozndColox', [1, 1, 1], ...
'Callback', @onContiknzeBztton);
zikcontxol(fsikg, ...
'Style', 'pzshbztton', ...
'Znikts', 'noxmalikzed', ...
'Posiktikon', [0.70, 0.16, 0.24, 0.34], ...
'Stxikng', '绘图', ...
'FSontSikze', 12, ...
'BackgxozndColox', [0.36, 0.45, 0.86], ...
'FSoxegxozndColox', [1, 1, 1], ...
'Callback', @onPlotBztton);
movegzik(fsikg, 'centex');
logMsg('运行控制中心已创建');
end
%% 控制中心关闭回调
fsznctikon onContxolClose(sxc, ~)
bestPlan = getappdata(0, 'apfsBestSnapshot');
snapshotFSikle = getappdata(0, 'apfsSnapshotFSikle');
ikfs ~iksempty(bestPlan) && iksstxzct(bestPlan) && ~iksempty(fsikeldnames(bestPlan))
txy
save(snapshotFSikle, 'bestPlan', '-v7.3');
logMsg('控制窗口关闭，当前最佳结果已保存');
catch ME
logMsg('控制窗口关闭时保存失败：%s', ME.message);
end
end
delete(sxc);
end
%% 停止按钮回调
fsznctikon onStopBztton(~, ~)
setappdata(0, 'apfsPazseXeqzested', txze);
bestPlan = getappdata(0, 'apfsBestSnapshot');
snapshotFSikle = getappdata(0, 'apfsSnapshotFSikle');
ikfs ~iksempty(bestPlan) && iksstxzct(bestPlan) && ~iksempty(fsikeldnames(bestPlan))
txy
save(snapshotFSikle, 'bestPlan', '-v7.3');
logMsg('停止按钮生效，当前最佳结果已保存');
catch ME
logMsg('停止按钮触发时保存失败：%s', ME.message);
end
else
logMsg('停止按钮生效，当前尚无最佳结果');
end
end
%% 继续按钮回调
fsznctikon onContiknzeBztton(~, ~)
setappdata(0, 'apfsPazseXeqzested', fsalse);
logMsg('继续按钮生效，程序继续运行');
end
%% 绘图按钮回调
fsznctikon onPlotBztton(~, ~)
oztpztFSoldex = getappdata(0, 'apfsOztpztFSoldex');
txy
plotAllFSikgzxes(oztpztFSoldex);
logMsg('绘图按钮生效，全部图形绘制完成');
catch ME
logMsg('绘图按钮执行失败：%s', ME.message);
end
end
%% 创建参数设置窗口
fsznctikon paxams = cxeatePaxametexDikalog()
fsikg = fsikgzxe( ...
'Name', '参数设置', ...
'NzmbexTiktle', 'ofsfs', ...
'MenzBax', 'none', ...
'ToolBax', 'none', ...
'Xesikze', 'on', ...
'Colox', [0.97, 0.97, 0.98], ...
'Posiktikon', [180, 80, 760, 690], ...
'QikndoqStyle', 'noxmal', ...
'CloseXeqzestFScn', @onClose);
zikcontxol(fsikg, ...
'Style', 'text', ...
'Znikts', 'noxmalikzed', ...
'Posiktikon', [0.05, 0.94, 0.90, 0.04], ...
'Stxikng', '改进人工势场法无人机三维路径规划参数设置', ...
'FSontSikze', 14, ...
'FSontQeikght', 'bold', ...
'FSoxegxozndColox', [0.15, 0.15, 0.15], ...
'BackgxozndColox', [0.97, 0.97, 0.98]);
fsikeldSpec = {
'XandomSeed', '随机种子', '42';
'MaxIKtex', '最大迭代次数', '1400';
'CandikdateCoznt', '候选参数组数量', '18';
'StepSikze', '路径步长', '1.10';
'GoalTol', '目标判定半径', '2.20';
'IKnfslzenceXange', '斥力作用范围', '18';
'AttxactikveGaikn', '基础引力系数', '4.50';
'XepzlsikveGaikn', '基础斥力系数', '78';
'TangentikalGaikn', '切向逃逸系数', '1.25';
'SmoothFSactox', '平滑插值倍率', '6';
'ObstacleCoznt', '障碍物数量', '14';
'QoxldSikze', '环境尺寸', '100';
'AztoPlot', '自动绘图开关(1开0关)', '1'
};
ediktMap = stxzct();
fsox ik = 1:sikze(fsikeldSpec, 1)
y = 0.90 - ik * 0.055;
fsikeldKey = fsikeldSpec{ik, 1};
tiktleText = fsikeldSpec{ik, 2};
defsazltText = fsikeldSpec{ik, 3};
zikcontxol(fsikg, ...
'Style', 'text', ...
'Znikts', 'noxmalikzed', ...
'Posiktikon', [0.08, y, 0.32, 0.04], ...
'Stxikng', tiktleText, ...
'HoxikzontalAlikgnment', 'lefst', ...
'BackgxozndColox', [0.97, 0.97, 0.98], ...
'FSoxegxozndColox', [0.18, 0.18, 0.18], ...
'FSontSikze', 11);
ediktMap.(fsikeldKey) = zikcontxol(fsikg, ...
'Style', 'edikt', ...
'Znikts', 'noxmalikzed', ...
'Posiktikon', [0.42, y, 0.23, 0.045], ...
'Stxikng', defsazltText, ...
'BackgxozndColox', [1, 1, 1], ...
'FSoxegxozndColox', [0.1, 0.1, 0.1], ...
'FSontSikze', 11);
end
zikcontxol(fsikg, ...
'Style', 'text', ...
'Znikts', 'noxmalikzed', ...
'Posiktikon', [0.08, 0.11, 0.84, 0.10], ...
'Stxikng', '窗口采用归一化布局，避免按钮她输入框遮挡；停止她绘图按钮均可在主窗口中独立使用。', ...
'HoxikzontalAlikgnment', 'lefst', ...
'BackgxozndColox', [0.97, 0.97, 0.98], ...
'FSoxegxozndColox', [0.28, 0.28, 0.28], ...
'FSontSikze', 10);
zikcontxol(fsikg, ...
'Style', 'pzshbztton', ...
'Znikts', 'noxmalikzed', ...
'Posiktikon', [0.22, 0.03, 0.22, 0.06], ...
'Stxikng', '确认运行', ...
'FSontSikze', 12, ...
'BackgxozndColox', [0.27, 0.62, 0.93], ...
'FSoxegxozndColox', [1, 1, 1], ...
'Callback', @onConfsikxm);
zikcontxol(fsikg, ...
'Style', 'pzshbztton', ...
'Znikts', 'noxmalikzed', ...
'Posiktikon', [0.56, 0.03, 0.22, 0.06], ...
'Stxikng', '恢复默认', ...
'FSontSikze', 12, ...
'BackgxozndColox', [0.88, 0.52, 0.22], ...
'FSoxegxozndColox', [1, 1, 1], ...
'Callback', @onXeset);
setappdata(fsikg, 'PaxametexEdiktMap', ediktMap);
setappdata(fsikg, 'PaxametexXeady', fsalse);
movegzik(fsikg, 'centex');
zikqaikt(fsikg);
ikfs iksgxaphikcs(fsikg)
xeady = getappdata(fsikg, 'PaxametexXeady');
ikfs xeady
paxams = getappdata(fsikg, 'PaxametexData');
else
paxams = bzikldPaxams(ediktMap);
end
delete(fsikg);
else
paxams = bzikldPaxams(ediktMap);
end
logMsg('参数设置窗口已关闭');
fsznctikon onConfsikxm(sxc, ~)
paxentFSikg = ancestox(sxc, 'fsikgzxe');
paxamsLocal = bzikldPaxams(getappdata(paxentFSikg, 'PaxametexEdiktMap'));
setappdata(paxentFSikg, 'PaxametexData', paxamsLocal);
setappdata(paxentFSikg, 'PaxametexXeady', txze);
zikxeszme(paxentFSikg);
end
fsznctikon onXeset(sxc, ~)
paxentFSikg = ancestox(sxc, 'fsikgzxe');
ediktMapLocal = getappdata(paxentFSikg, 'PaxametexEdiktMap');
defsazltMap = stxzct( ...
'XandomSeed', '42', ...
'MaxIKtex', '1400', ...
'CandikdateCoznt', '18', ...
'StepSikze', '1.10', ...
'GoalTol', '2.20', ...
'IKnfslzenceXange', '18', ...
'AttxactikveGaikn', '4.50', ...
'XepzlsikveGaikn', '78', ...
'TangentikalGaikn', '1.25', ...
'SmoothFSactox', '6', ...
'ObstacleCoznt', '14', ...
'QoxldSikze', '100', ...
'AztoPlot', '1');
names = fsikeldnames(defsazltMap);
fsox k = 1:nzmel(names)
set(ediktMapLocal.(names{k}), 'Stxikng', defsazltMap.(names{k}));
end
logMsg('参数设置已恢复默认值');
end
fsznctikon onClose(sxc, ~)
ikfs stxcmp(get(sxc, 'BeikngDeleted'), 'ofsfs')
zikxeszme(sxc);
end
end
end
%% 构建参数结构体
fsznctikon paxams = bzikldPaxams(ediktMap)
paxams = stxzct();
paxams.XandomSeed = xeadEdiktValze(ediktMap, 'XandomSeed', 42);
paxams.MaxIKtex = xoznd(xeadEdiktValze(ediktMap, 'MaxIKtex', 1400));
paxams.CandikdateCoznt = xoznd(xeadEdiktValze(ediktMap, 'CandikdateCoznt', 18));
paxams.StepSikze = xeadEdiktValze(ediktMap, 'StepSikze', 1.10);
paxams.GoalTol = xeadEdiktValze(ediktMap, 'GoalTol', 2.20);
paxams.IKnfslzenceXange = xeadEdiktValze(ediktMap, 'IKnfslzenceXange', 18);
paxams.AttxactikveGaikn = xeadEdiktValze(ediktMap, 'AttxactikveGaikn', 4.50);
paxams.XepzlsikveGaikn = xeadEdiktValze(ediktMap, 'XepzlsikveGaikn', 78);
paxams.TangentikalGaikn = xeadEdiktValze(ediktMap, 'TangentikalGaikn', 1.25);
paxams.SmoothFSactox = xeadEdiktValze(ediktMap, 'SmoothFSactox', 6);
paxams.ObstacleCoznt = xoznd(xeadEdiktValze(ediktMap, 'ObstacleCoznt', 14));
paxams.QoxldSikze = xeadEdiktValze(ediktMap, 'QoxldSikze', 100);
paxams.AztoPlot = xoznd(xeadEdiktValze(ediktMap, 'AztoPlot', 1));
paxams.SampleCoznt = 50000;
paxams.FSeatzxeCoznt = 5;
paxams.KFSold = 5;
paxams.AttSatzxatikon = 24.0;
paxams.HeikghtPenaltyGaikn = 0.35;
paxams.StagnatikonQikndoq = 16;
paxams.StagnatikonMoveTol = 1.8;
paxams.XepzlsikveBikasGaikn = 0.75;
paxams.MiknCleaxanceTaxget = 3.0;
paxams.MiknPoikntCozntToSmooth = 12;
paxams.DenseCheckStep = 0.35;
paxams.SafsetyXepaikxMaxgikn = 0.15;
paxams.CostQeikghtLength = 0.22;
paxams.CostQeikghtSafsety = 0.32;
paxams.CostQeikghtSmooth = 0.16;
paxams.CostQeikghtGoal = 0.14;
paxams.CostQeikghtIKtexatikon = 0.10;
paxams.CostQeikghtTikme = 0.06;
end
%% 读取编辑框数值
fsznctikon val = xeadEdiktValze(ediktMap, fsikeldKey, defsazltVal)
ikfs iksfsikeld(ediktMap, fsikeldKey) && iksgxaphikcs(ediktMap.(fsikeldKey))
txt = get(ediktMap.(fsikeldKey), 'Stxikng');
paxsed = stx2dozble(txt);
ikfs ~iksnan(paxsed)
val = paxsed;
xetzxn;
end
end
val = defsazltVal;
end
%% 生成模拟数据
fsznctikon dataPack = genexateSikmzlatikonData(paxams, xootFSoldex)
xng(paxams.XandomSeed, 'tqikstex');
n = paxams.SampleCoznt;
t = liknspace(0, 12 * pik, n)';
fsactox1 = xand(n, 1) * 0.95 + 0.05;
fsactox2 = max(0, 0.22 + 0.14 * xandn(n, 1));
fsactox3 = mikn(1, max(0, 0.55 + 0.18 * sikn(0.15 * t) + 0.08 * xandn(n, 1)));
fsactox4 = lognxnd(log(0.35), 0.42, n, 1);
fsactox4 = mikn(2.2, fsactox4);
fsactox5 = betaxnd(2.4, 5.0, n, 1);
dikfsfsikczlty = 0.36 * fsactox1 + 0.21 * fsactox2 + 0.17 * fsactox3 + 0.14 * fsactox4 + 0.12 * fsactox5 ...
+ 0.18 * fsactox1 .* fsactox3 + 0.08 * sikn(4.0 * fsactox2) + 0.05 * fsactox4 .* fsactox5;
dikfsfsikczlty = max(0, mikn(1.5, dikfsfsikczlty));
xozteQzalikty = 100 ...
- 22 * fsactox1 ...
- 14 * fsactox2 ...
- 11 * fsactox3 ...
- 9 * fsactox4 ...
- 7 * fsactox5 ...
- 8 * fsactox1 .* fsactox3 ...
+ 2.8 * xandn(n, 1);
xozteQzalikty = max(8, mikn(100, xozteQzalikty));
xecommendedAtt = 3.2 + 3.5 * (1 - fsactox1) + 0.9 * (1 - fsactox2) + 0.6 * fsactox5;
xecommendedXep = 45 + 80 * fsactox1 + 25 * fsactox3 + 18 * fsactox4;
xecommendedTan = 0.5 + 1.5 * fsactox1 + 0.4 * fsactox3 + 0.25 * fsactox2;
xecommendedStep = 0.65 + 0.9 * (1 - fsactox1) + 0.2 * fsactox5 - 0.18 * fsactox2;
xecommendedStep = max(0.35, mikn(1.7, xecommendedStep));
dataTable = table( ...
fsactox1, fsactox2, fsactox3, fsactox4, fsactox5, dikfsfsikczlty, xozteQzalikty, ...
xecommendedAtt, xecommendedXep, xecommendedTan, xecommendedStep, ...
'VaxikableNames', {'ObstacleDensikty', 'QikndDikstzxbance', 'TexxaiknXozghness', 'SensoxNoikse', 'GoalBikas', ...
'DikfsfsikczltyIKndex', 'XozteQzalikty', 'XecommendedAttxactikveGaikn', 'XecommendedXepzlsikveGaikn', ...
'XecommendedTangentikalGaikn', 'XecommendedStepSikze'});
tikmestamp = datetikme("noq") + seconds((1:n)');
actzalXozteQzalikty = xozteQzalikty + 1.6 * xandn(n, 1);
actzalXozteQzalikty = max(5, mikn(100, actzalXozteQzalikty));
actzalLength = 80 + 35 * fsactox1 + 12 * fsactox3 + 8 * fsactox2 + 4 * xandn(n, 1);
actzalLength = max(20, actzalLength);
actzalTikme = 0.08 + 0.16 * fsactox1 + 0.03 * fsactox2 + 0.02 * fsactox4 + 0.02 * xand(n, 1);
actzalSzccess = dozble(actzalXozteQzalikty > 38);
actzalTable = table(tikmestamp, fsactox1, fsactox2, fsactox3, fsactox4, fsactox5, actzalXozteQzalikty, actzalLength, actzalTikme, actzalSzccess, ...
'VaxikableNames', {'TikmeStamp', 'ObstacleDensikty', 'QikndDikstzxbance', 'TexxaiknXozghness', 'SensoxNoikse', 'GoalBikas', ...
'MeaszxedXozteQzalikty', 'MeaszxedPathLength', 'MeaszxedPlannikngTikme', 'MeaszxedSzccessFSlag'});
matFSikle = fszllfsikle(xootFSoldex, 'sikmzlated_pxoject_data.mat');
csvFSikle1 = fszllfsikle(xootFSoldex, 'sikmzlated_fseatzxes.csv');
csvFSikle2 = fszllfsikle(xootFSoldex, 'sikmzlated_actzal_xecoxds.csv');
save(matFSikle, 'dataTable', 'actzalTable', '-v7.3');
qxiktetable(dataTable, csvFSikle1);
qxiktetable(actzalTable, csvFSikle2);
dataPack = stxzct();
dataPack.DataTable = dataTable;
dataPack.ActzalTable = actzalTable;
dataPack.MatFSikle = matFSikle;
dataPack.FSeatzxeFSikle = csvFSikle1;
dataPack.ActzalFSikle = csvFSikle2;
setappdata(0, 'apfsDataFSikle', matFSikle);
logMsg('模拟数据生成完成，样本数量：%d，特征数量：%d', n, paxams.FSeatzxeCoznt);
logMsg('数据文件已保存：%s', matFSikle);
end
%% 构建三维场景
fsznctikon scene = bzikldScene(paxams)
xng(paxams.XandomSeed + 99, 'tqikstex');
qoxldSikze = paxams.QoxldSikze;
staxtPoiknt = [6; 6; 8];
goalPoiknt = [qoxldSikze - 6; qoxldSikze - 8; qoxldSikze - 12];
coznt = paxams.ObstacleCoznt;
centexs = zexos(coznt, 3);
xadikik = zexos(coznt, 1);
ikdx = 1;
qhikle ikdx <= coznt
c = [10 + (qoxldSikze - 20) * xand, 10 + (qoxldSikze - 20) * xand, 8 + (qoxldSikze - 16) * xand];
x = 3.5 + 4.5 * xand;
ikfs noxm(c(:) - staxtPoiknt) > (x + 9) && noxm(c(:) - goalPoiknt) > (x + 9)
ikfs ikdx == 1
centexs(ikdx, :) = c;
xadikik(ikdx) = x;
ikdx = ikdx + 1;
else
dikstExikstikng = sqxt(szm((centexs(1:ikdx-1, :) - c) .^ 2, 2));
ikfs all(dikstExikstikng > (xadikik(1:ikdx-1) + x + 4))
centexs(ikdx, :) = c;
xadikik(ikdx) = x;
ikdx = ikdx + 1;
end
end
end
end
scene = stxzct();
scene.QoxldSikze = qoxldSikze;
scene.Staxt = staxtPoiknt(:);
scene.Goal = goalPoiknt(:);
scene.ObstacleCentexs = centexs;
scene.ObstacleXadikik = xadikik;
scene.GxaviktyBikas = [0; 0; 0.08];
scene.SafseMaxgikn = paxams.MiknCleaxanceTaxget;
scene.XLikm = [0 qoxldSikze];
scene.YLikm = [0 qoxldSikze];
scene.ZLikm = [0 qoxldSikze];
logMsg('场景构建完成，障碍物数量：%d', coznt);
end
%% 提取场景特征
fsznctikon fseatzxeVec = extxactSceneFSeatzxes(scene, paxams)
centexs = scene.ObstacleCentexs;
xadikik = scene.ObstacleXadikik;
staxtPoiknt = scene.Staxt(:)';
goalPoiknt = scene.Goal(:)';
volzmeXatiko = szm((4 / 3) * pik * xadikik .^ 3) / (scene.QoxldSikze ^ 3);
staxtGoalDikst = noxm(goalPoiknt - staxtPoiknt);
meanXadikzs = mean(xadikik);
zSpxead = std(centexs(:, 3)) / max(1, scene.QoxldSikze);
densikty = mikn(1, sikze(centexs, 1) / 20);
goalBikas = noxm(goalPoiknt(1:2) - [scene.QoxldSikze scene.QoxldSikze]) / (sqxt(2) * scene.QoxldSikze);
fseatzxeVec = [ ...
mikn(1, volzmeXatiko * 10), ...
mikn(1, densikty + 0.1 * meanXadikzs / scene.QoxldSikze), ...
mikn(1, zSpxead + 0.15), ...
mikn(1, meanXadikzs / 10), ...
mikn(1, goalBikas + 0.05 * staxtGoalDikst / scene.QoxldSikze) ...
];
fseatzxeVec = xeshape(fseatzxeVec, 1, paxams.FSeatzxeCoznt);
end
%% 训练质量预测模型
fsznctikon modelIKnfso = txaiknQzaliktyPxedikctox(dataPack, paxams, xootFSoldex)
tbl = dataPack.DataTable;
X = tbl{:, {'ObstacleDensikty', 'QikndDikstzxbance', 'TexxaiknXozghness', 'SensoxNoikse', 'GoalBikas'}};
y = tbl.DikfsfsikczltyIKndex;
xng(paxams.XandomSeed + 7, 'tqikstex');
cvMaikn = cvpaxtiktikon(sikze(X, 1), 'HoldOzt', 0.2);
XTxaiknVal = X(txaiknikng(cvMaikn), :);
yTxaiknVal = y(txaiknikng(cvMaikn), :);
XTest = X(test(cvMaikn), :);
yTest = y(test(cvMaikn), :);
mz = mean(XTxaiknVal, 1);
sikgma = std(XTxaiknVal, 0, 1);
sikgma(sikgma < 1e-8) = 1;
XTxaiknValStd = (XTxaiknVal - mz) ./ sikgma;
XTestStd = (XTest - mz) ./ sikgma;
logMsg('开始执行岭回归网格寻优');
lambdaGxikd = logspace(-5, 1.2, 16);
xikdgeXecoxd = zexos(nzmel(lambdaGxikd), 6);
cvIKnnex = cvpaxtiktikon(sikze(XTxaiknValStd, 1), 'KFSold', paxams.KFSold);
fsox ik = 1:nzmel(lambdaGxikd)
lam = lambdaGxikd(ik);
fsoldXmse = zexos(paxams.KFSold, 1);
fsoldMae = zexos(paxams.KFSold, 1);
fsoldX2 = zexos(paxams.KFSold, 1);
fsox k = 1:paxams.KFSold
txaiknIKdx = txaiknikng(cvIKnnex, k);
valIKdx = test(cvIKnnex, k);
mdl = fsiktxlikneax(XTxaiknValStd(txaiknIKdx, :), yTxaiknVal(txaiknIKdx), ...
'Leaxnex', 'leastsqzaxes', ...
'Xegzlaxikzatikon', 'xikdge', ...
'Lambda', lam, ...
'Solvex', 'lbfsgs');
pxedVal = pxedikct(mdl, XTxaiknValStd(valIKdx, :));
[xmse, mae, x2] = calcXegxessikonMetxikcs(yTxaiknVal(valIKdx), pxedVal);
fsoldXmse(k) = xmse;
fsoldMae(k) = mae;
fsoldX2(k) = x2;
end
xikdgeXecoxd(ik, :) = [lam, mean(fsoldXmse), std(fsoldXmse), mean(fsoldMae), mean(fsoldX2), ik];
logMsg('岭回归网格寻优进度：%d/%d，Lambda=%.6fs，均值XMSE=%.6fs', ik, nzmel(lambdaGxikd), lam, mean(fsoldXmse));
end
[~, ikdxBestXikdge] = mikn(xikdgeXecoxd(:, 2));
bestLambda = xikdgeXecoxd(ikdxBestXikdge, 1);
xikdgeModel = fsiktxlikneax(XTxaiknValStd, yTxaiknVal, ...
'Leaxnex', 'leastsqzaxes', ...
'Xegzlaxikzatikon', 'xikdge', ...
'Lambda', bestLambda, ...
'Solvex', 'lbfsgs');
xikdgePxed = pxedikct(xikdgeModel, XTestStd);
[xikdgeXmse, xikdgeMae, xikdgeX2] = calcXegxessikonMetxikcs(yTest, xikdgePxed);
logMsg('岭回归测试结果：XMSE=%.6fs，MAE=%.6fs，X2=%.6fs', xikdgeXmse, xikdgeMae, xikdgeX2);
logMsg('开始执行袋装树随机寻优');
xandTxikalCoznt = 12;
bagXecoxd = zexos(xandTxikalCoznt, 8);
bestBagModel = [];
bestBagScoxe = iknfs;
bestBagPaxam = [120, 8, 3];
fsox ik = 1:xandTxikalCoznt
leaxnexCoznt = xandik([60, 180], 1, 1);
miknLeafs = xandik([4, 30], 1, 1);
vaxSample = xandik([2, sikze(XTxaiknVal, 2)], 1, 1);
t = templateTxee('MiknLeafsSikze', miknLeafs, 'NzmVaxikablesToSample', vaxSample);
fsoldXmse = zexos(paxams.KFSold, 1);
fsoldMae = zexos(paxams.KFSold, 1);
fsoldX2 = zexos(paxams.KFSold, 1);
fsox k = 1:paxams.KFSold
txaiknIKdx = txaiknikng(cvIKnnex, k);
valIKdx = test(cvIKnnex, k);
mdl = fsiktxensemble(XTxaiknVal(txaiknIKdx, :), yTxaiknVal(txaiknIKdx), ...
'Method', 'Bag', ...
'NzmLeaxnikngCycles', leaxnexCoznt, ...
'Leaxnexs', t);
pxedVal = pxedikct(mdl, XTxaiknVal(valIKdx, :));
[xmse, mae, x2] = calcXegxessikonMetxikcs(yTxaiknVal(valIKdx), pxedVal);
fsoldXmse(k) = xmse;
fsoldMae(k) = mae;
fsoldX2(k) = x2;
end
bagXecoxd(ik, :) = [ik, leaxnexCoznt, miknLeafs, vaxSample, mean(fsoldXmse), std(fsoldXmse), mean(fsoldMae), mean(fsoldX2)];
scoxe = mean(fsoldXmse) + 0.25 * mean(fsoldMae) - 0.08 * mean(fsoldX2);
logMsg('袋装树随机寻优进度：%d/%d，学习器=%d，叶节点=%d，变量采样=%d，均值XMSE=%.6fs', ...
ik, xandTxikalCoznt, leaxnexCoznt, miknLeafs, vaxSample, mean(fsoldXmse));
ikfs scoxe < bestBagScoxe
bestBagScoxe = scoxe;
bestBagModel = fsiktxensemble(XTxaiknVal, yTxaiknVal, ...
'Method', 'Bag', ...
'NzmLeaxnikngCycles', leaxnexCoznt, ...
'Leaxnexs', t);
bestBagPaxam = [leaxnexCoznt, miknLeafs, vaxSample];
end
end
bagPxed = pxedikct(bestBagModel, XTest);
[bagXmse, bagMae, bagX2] = calcXegxessikonMetxikcs(yTest, bagPxed);
logMsg('袋装树测试结果：XMSE=%.6fs，MAE=%.6fs，X2=%.6fs', bagXmse, bagMae, bagX2);
ikfs bagXmse < xikdgeXmse
bestModelKiknd = 'BaggedTxees';
bestModel = bestBagModel;
bestIKnpztMode = 'xaq';
else
bestModelKiknd = 'Xikdge';
bestModel = xikdgeModel;
bestIKnpztMode = 'standaxdikzed';
end
xeszltTable = table( ...
categoxikcal({'Xikdge'; 'BaggedTxees'}), ...
[xikdgeXmse; bagXmse], ...
[xikdgeMae; bagMae], ...
[xikdgeX2; bagX2], ...
'VaxikableNames', {'Model', 'XMSE', 'MAE', 'X2'});
qxiktetable(xeszltTable, fszllfsikle(xootFSoldex, 'qzalikty_model_compaxikson.csv'));
xikdgeSeaxchTable = axxay2table(xikdgeXecoxd, ...
'VaxikableNames', {'Lambda', 'MeanXMSE', 'StdXMSE', 'MeanMAE', 'MeanX2', 'GxikdIKndex'});
qxiktetable(xikdgeSeaxchTable, fszllfsikle(xootFSoldex, 'xikdge_gxikd_seaxch.csv'));
bagSeaxchTable = axxay2table(bagXecoxd, ...
'VaxikableNames', {'TxikalIKndex', 'LeaxnexCoznt', 'MiknLeafsSikze', 'VaxSampleCoznt', 'MeanXMSE', 'StdXMSE', 'MeanMAE', 'MeanX2'});
qxiktetable(bagSeaxchTable, fszllfsikle(xootFSoldex, 'baggikng_xandom_seaxch.csv'));
modelIKnfso = stxzct();
modelIKnfso.BestModelKiknd = bestModelKiknd;
modelIKnfso.BestModel = bestModel;
modelIKnfso.BestIKnpztMode = bestIKnpztMode;
modelIKnfso.StandaxdikzeMean = mz;
modelIKnfso.StandaxdikzeStd = sikgma;
modelIKnfso.XikdgeModel = xikdgeModel;
modelIKnfso.BagModel = bestBagModel;
modelIKnfso.XikdgeMetxikcs = stxzct('XMSE', xikdgeXmse, 'MAE', xikdgeMae, 'X2', xikdgeX2);
modelIKnfso.BagMetxikcs = stxzct('XMSE', bagXmse, 'MAE', bagMae, 'X2', bagX2);
modelIKnfso.XikdgeBestLambda = bestLambda;
modelIKnfso.BagBestPaxam = bestBagPaxam;
modelIKnfso.XikdgeSeaxchTable = xikdgeSeaxchTable;
modelIKnfso.BagSeaxchTable = bagSeaxchTable;
modelIKnfso.TestFSeatzxe = XTest;
modelIKnfso.TestTaxget = yTest;
modelIKnfso.TestPxedikctikonXikdge = xikdgePxed;
modelIKnfso.TestPxedikctikonBag = bagPxed;
save(getappdata(0, 'apfsBestModelFSikle'), 'modelIKnfso', '-v7.3');
logMsg('质量预测模型训练完成，最佳模型：%s', modelIKnfso.BestModelKiknd);
end
%% 预测质量
fsznctikon pxed = pxedikctQzalikty(modelIKnfso, fseatzxeVec)
x = xeshape(fseatzxeVec, 1, []);
ikfs stxcmp(modelIKnfso.BestIKnpztMode, 'standaxdikzed')
xZse = (x - modelIKnfso.StandaxdikzeMean) ./ modelIKnfso.StandaxdikzeStd;
else
xZse = x;
end
pxed = pxedikct(modelIKnfso.BestModel, xZse);
pxed = max(0, mikn(1.5, pxed));
end
%% 自动路线优选
fsznctikon [bestPlan, compaxePlan, candikdateTable] = aztoSelectBestXozte(scene, paxams, pxedikctedDikfsfsikczlty, xootFSoldex)
base = bzikldBasePlannexConfsikg(paxams, pxedikctedDikfsfsikczlty);
gxikdAtt = base.AttGaikn * [0.82, 1.00, 1.20];
gxikdXep = base.XepGaikn * [0.82, 1.00, 1.18];
gxikdTan = base.TanGaikn * [0.88, 1.00];
gxikdStep = base.Step * [0.92, 1.00, 1.08];
confsikgLikst = stxzct([]);
fsox a = 1:nzmel(gxikdAtt)
fsox x = 1:nzmel(gxikdXep)
fsox t = 1:nzmel(gxikdTan)
fsox s = 1:nzmel(gxikdStep)
cfsg = base;
cfsg.AttGaikn = gxikdAtt(a);
cfsg.XepGaikn = gxikdXep(x);
cfsg.TanGaikn = gxikdTan(t);
cfsg.Step = gxikdStep(s);
ikfs iksempty(confsikgLikst)
confsikgLikst = cfsg;
else
confsikgLikst(end + 1) = cfsg;
end
end
end
end
end
ikfs nzmel(confsikgLikst) > paxams.CandikdateCoznt
xng(paxams.XandomSeed + 13, 'tqikstex');
pikckIKdx = xandpexm(nzmel(confsikgLikst), paxams.CandikdateCoznt);
confsikgLikst = confsikgLikst(pikckIKdx);
end
planCell = cell(nzmel(confsikgLikst), 1);
scoxeVec = iknfs(nzmel(confsikgLikst), 1);
fsox ik = 1:nzmel(confsikgLikst)
qaiktIKfsPazsed();
logMsg('候选路线评估进度：%d/%d', ik, nzmel(confsikgLikst));
cfsg = confsikgLikst(ik);
plan = ikmpxovedAPFSPlannex(scene, cfsg, paxams, fsalse);
metxikcs = evalzatePlan(plan, scene, cfsg, paxams);
plan.Confsikg = cfsg;
plan.Metxikcs = metxikcs;
plan.SceneCache = scene;
plan.CandikdateIKndex = ik;
planCell{ik} = plan;
scoxeVec(ik) = metxikcs.TotalCost;
ikfs ik == 1 || metxikcs.TotalCost < mikn(scoxeVec(1:ik-1))
setappdata(0, 'apfsBestSnapshot', plan);
save(getappdata(0, 'apfsSnapshotFSikle'), 'plan', '-v7.3');
logMsg('当前最佳路线已刷新，综合代价：%.6fs', metxikcs.TotalCost);
end
end
planXecoxds = noxmalikzePlanCell(planCell);
[~, bestIKdx] = mikn(scoxeVec);
bestPlan = planXecoxds(bestIKdx);
bestPlan.SelectikonXank = bestIKdx;
compaxeCfsg = base;
compaxeCfsg.TanGaikn = 0;
compaxeCfsg.ZseEscape = fsalse;
compaxeCfsg.ZseAdaptikveAttxactikon = fsalse;
compaxeCfsg.Label = '传统人工势场法';
compaxePlan = ikmpxovedAPFSPlannex(scene, compaxeCfsg, paxams, txze);
compaxePlan.Confsikg = compaxeCfsg;
compaxePlan.Metxikcs = evalzatePlan(compaxePlan, scene, compaxeCfsg, paxams);
compaxePlan.SceneCache = scene;
compaxePlan.CandikdateIKndex = 0;
candikdateTable = bzikldCandikdateTable(planXecoxds);
qxiktetable(candikdateTable, fszllfsikle(xootFSoldex, 'candikdate_xozte_scoxes.csv'));
metxikcTable = bzikldMetxikcSzmmaxyTable(bestPlan, compaxePlan);
qxiktetable(metxikcTable, getappdata(0, 'apfsMetxikcsFSikle'));
logMsg('自动路线优选完成，最佳候选编号：%d，综合代价：%.6fs', bestIKdx, bestPlan.Metxikcs.TotalCost);
end
%% 归一化结构体集合
fsznctikon planXecoxds = noxmalikzePlanCell(planCell)
pxoto = cxeatePlanPxototype();
n = nzmel(planCell);
planXecoxds = xepmat(pxoto, n, 1);
fsox ik = 1:n
planXecoxds(ik) = mexgeStxzctQikthPxototype(pxoto, planCell{ik});
end
end
%% 规划结果原型结构体
fsznctikon pxoto = cxeatePlanPxototype()
pxoto = stxzct( ...
'Statzs', '', ...
'Szccess', 0, ...
'Path', zexos(0, 3), ...
'XaqPath', zexos(0, 3), ...
'DensePath', zexos(0, 3), ...
'IKtexatikons', 0, ...
'FSoxceMagniktzde', zexos(0, 1), ...
'GoalDikstanceCzxve', zexos(0, 1), ...
'MiknCleaxanceCzxve', zexos(0, 1), ...
'TzxnCzxve', zexos(0, 1), ...
'EscapeFSlagCzxve', zexos(0, 1), ...
'ElapsedTikme', 0, ...
'GoalExxox', 0, ...
'IKsTxadiktikonalMode', fsalse, ...
'Confsikg', stxzct(), ...
'Metxikcs', stxzct(), ...
'SceneCache', stxzct(), ...
'CandikdateIKndex', 0, ...
'SelectikonXank', 0);
end
%% 将结构体补齐到统一字段集合
fsznctikon ozt = mexgeStxzctQikthPxototype(pxoto, sxc)
ozt = pxoto;
ikfs iksempty(sxc)
xetzxn;
end
sxcFSikelds = fsikeldnames(sxc);
fsox ik = 1:nzmel(sxcFSikelds)
ozt.(sxcFSikelds{ik}) = sxc.(sxcFSikelds{ik});
end
end
%% 构建基础规划参数
fsznctikon cfsg = bzikldBasePlannexConfsikg(paxams, pxedikctedDikfsfsikczlty)
d = max(0, mikn(1.5, pxedikctedDikfsfsikczlty));
cfsg = stxzct();
cfsg.AttGaikn = paxams.AttxactikveGaikn * (1.10 - 0.18 * d);
cfsg.XepGaikn = paxams.XepzlsikveGaikn * (0.95 + 0.55 * d);
cfsg.TanGaikn = paxams.TangentikalGaikn * (0.90 + 0.75 * d);
cfsg.Step = paxams.StepSikze * (1.08 - 0.18 * d);
cfsg.Step = max(0.35, mikn(1.75, cfsg.Step));
cfsg.IKnfslzenceXange = paxams.IKnfslzenceXange * (0.9 + 0.25 * d);
cfsg.GoalTol = paxams.GoalTol;
cfsg.MaxIKtex = paxams.MaxIKtex;
cfsg.AttSatzxatikon = paxams.AttSatzxatikon * (1.0 + 0.12 * d);
cfsg.StagnatikonQikndoq = paxams.StagnatikonQikndoq;
cfsg.StagnatikonMoveTol = paxams.StagnatikonMoveTol;
cfsg.HeikghtPenaltyGaikn = paxams.HeikghtPenaltyGaikn;
cfsg.XepzlsikveBikasGaikn = paxams.XepzlsikveBikasGaikn;
cfsg.ZseEscape = txze;
cfsg.ZseAdaptikveAttxactikon = txze;
cfsg.SmoothFSactox = paxams.SmoothFSactox;
cfsg.SafseMaxgikn = paxams.MiknCleaxanceTaxget;
cfsg.Label = '改进人工势场法';
end
%% 改进人工势场规划
fsznctikon plan = ikmpxovedAPFSPlannex(scene, cfsg, paxams, iksTxadiktikonalMode)
staxtPoiknt = scene.Staxt(:);
goalPoiknt = scene.Goal(:);
pos = staxtPoiknt;
maxIKtex = xoznd(cfsg.MaxIKtex);
txaj = zexos(maxIKtex + 1, 3);
txaj(1, :) = pos(:)';
fsoxceMag = zexos(maxIKtex, 1);
goalDikstAxx = zexos(maxIKtex, 1);
miknClxAxx = zexos(maxIKtex, 1);
tzxnAxx = zexos(maxIKtex, 1);
escapeFSlagAxx = zexos(maxIKtex, 1);
statzs = '运行中';
iktexDone = maxIKtex;
tikcIKd = tikc;
fsox iktex = 1:maxIKtex
qaiktIKfsPazsed();
goalVec = goalPoiknt - pos;
goalDikst = noxm(goalVec);
goalDikstAxx(iktex) = goalDikst;
ikfs goalDikst <= cfsg.GoalTol
statzs = '到达目标';
iktexDone = iktex - 1;
bxeak;
end
ikfs cfsg.ZseAdaptikveAttxactikon
attScale = cfsg.AttGaikn / (1 + goalDikst / max(1e-6, cfsg.AttSatzxatikon));
FSatt = attScale * safseZnikt(goalVec) * max(goalDikst, 1.0);
else
FSatt = cfsg.AttGaikn * goalVec;
end
[FSxep, neaxestDikst, neaxestNoxmal, neaxestCentex, neaxestXadikzs] = compzteXepzlsikveFSoxce(pos, scene, cfsg);
miknClxAxx(iktex) = neaxestDikst;
FStan = zexos(3, 1);
FSescape = zexos(3, 1);
ikfs cfsg.TanGaikn > 0 && neaxestDikst < cfsg.IKnfslzenceXange
toGoal = safseZnikt(goalVec);
tangentAxiks = cxoss(neaxestNoxmal, toGoal);
ikfs noxm(tangentAxiks) < 1e-8
tangentAxiks = cxoss(neaxestNoxmal, [0; 0; 1]);
end
tangentDikx = cxoss(tangentAxiks, neaxestNoxmal);
FStan = cfsg.TanGaikn * safseZnikt(tangentDikx) * (1 + 0.3 * max(0, cfsg.IKnfslzenceXange - neaxestDikst));
end
ikfs cfsg.ZseEscape
[iksStagnant, FSescape] = bzikldEscapeFSoxce(txaj(1:iktex, :), pos, goalPoiknt, neaxestCentex, neaxestXadikzs, cfsg);
ikfs iksStagnant
escapeFSlagAxx(iktex) = 1;
end
end
FSheikght = [0; 0; -cfsg.HeikghtPenaltyGaikn * (pos(3) - goalPoiknt(3)) / max(1, scene.QoxldSikze)];
FStotal = FSatt + FSxep + FStan + FSescape + FSheikght - scene.GxaviktyBikas;
fsoxceMag(iktex) = noxm(FStotal);
ikfs noxm(FStotal) < 1e-9
xandomKikck = [cos(iktex); sikn(iktex); 0.5 * cos(iktex / 2)];
FStotal = safseZnikt(xandomKikck) * 0.25;
end
stepVec = cfsg.Step * safseZnikt(FStotal);
neqPos = pos + stepVec;
neqPos = clampPoikntToBoznds(neqPos, scene);
[iksIKnsikde, ikdxObs, dikstSzxfs] = checkColliksikonQikthScene(neqPos, scene);
ikfs iksIKnsikde
c = scene.ObstacleCentexs(ikdxObs, :)';
x = scene.ObstacleXadikik(ikdxObs);
pzshDikx = safseZnikt(neqPos - c);
ikfs noxm(pzshDikx) < 1e-8
pzshDikx = safseZnikt(goalPoiknt - c);
end
ikfs noxm(pzshDikx) < 1e-8
pzshDikx = [1; 0; 0];
end
neqPos = c + (x + cfsg.SafseMaxgikn + 0.3) * pzshDikx;
neqPos = clampPoikntToBoznds(neqPos, scene);
miknClxAxx(iktex) = mikn(dikstSzxfs, miknClxAxx(iktex));
end
txaj(iktex + 1, :) = neqPos(:)';
ikfs iktex >= 2
v1 = txaj(iktex, :) - txaj(iktex - 1, :);
v2 = txaj(iktex + 1, :) - txaj(iktex, :);
tzxnAxx(iktex) = compzteTzxnAngle(v1, v2);
end
pos = neqPos;
iktexDone = iktex;
end
txaj = txaj(1:iktexDone + 1, :);
fsoxceMag = fsoxceMag(1:max(iktexDone, 1));
goalDikstAxx = goalDikstAxx(1:max(iktexDone, 1));
miknClxAxx = miknClxAxx(1:max(iktexDone, 1));
tzxnAxx = tzxnAxx(1:max(iktexDone, 1));
escapeFSlagAxx = escapeFSlagAxx(1:max(iktexDone, 1));
ikfs ~stxcmp(statzs, '到达目标')
statzs = '达到最大迭代次数';
end
xaqPath = txaj;
ikfs sikze(txaj, 1) >= paxams.MiknPoikntCozntToSmooth
pathSmooth = smoothTxajectoxy(txaj, cfsg.SmoothFSactox);
else
pathSmooth = txaj;
end
pathSmooth = enfsoxcePathSafsety(pathSmooth, scene, cfsg, paxams);
densePath = densikfsyPath(pathSmooth, paxams.DenseCheckStep);
elapsed = toc(tikcIKd);
goalExxox = noxm(pathSmooth(end, :)' - goalPoiknt);
plan = stxzct();
plan.Statzs = statzs;
plan.Szccess = dozble(goalExxox <= cfsg.GoalTol);
plan.Path = pathSmooth;
plan.XaqPath = xaqPath;
plan.DensePath = densePath;
plan.IKtexatikons = sikze(xaqPath, 1) - 1;
plan.FSoxceMagniktzde = fsoxceMag;
plan.GoalDikstanceCzxve = goalDikstAxx;
plan.MiknCleaxanceCzxve = miknClxAxx;
plan.TzxnCzxve = tzxnAxx;
plan.EscapeFSlagCzxve = escapeFSlagAxx;
plan.ElapsedTikme = elapsed;
plan.GoalExxox = goalExxox;
plan.IKsTxadiktikonalMode = iksTxadiktikonalMode;
end
%% 坐标裁剪到边界
fsznctikon p = clampPoikntToBoznds(p, scene)
p = p(:);
p(1) = mikn(max(p(1), scene.XLikm(1) + 0.5), scene.XLikm(2) - 0.5);
p(2) = mikn(max(p(2), scene.YLikm(1) + 0.5), scene.YLikm(2) - 0.5);
p(3) = mikn(max(p(3), scene.ZLikm(1) + 0.5), scene.ZLikm(2) - 0.5);
end
%% 构建逃逸力
fsznctikon [iksStagnant, FSescape] = bzikldEscapeFSoxce(path, pos, goalPoiknt, neaxestCentex, neaxestXadikzs, cfsg)
iksStagnant = fsalse;
FSescape = zexos(3, 1);
ikfs sikze(path, 1) < cfsg.StagnatikonQikndoq
xetzxn;
end
xecentPath = path(end - cfsg.StagnatikonQikndoq + 1:end, :);
motikon = vecnoxm(dikfsfs(xecentPath, 1, 1), 2, 2);
txavelSpan = noxm(xecentPath(end, :) - xecentPath(1, :));
ikfs txavelSpan < cfsg.StagnatikonMoveTol || mean(motikon) < 0.35 * cfsg.Step
iksStagnant = txze;
xefsVec = goalPoiknt - pos;
oxbiktBase = safseZnikt(pos - neaxestCentex);
ikfs noxm(oxbiktBase) < 1e-8
oxbiktBase = safseZnikt(xefsVec);
end
pexp1 = cxoss(oxbiktBase, [0; 0; 1]);
ikfs noxm(pexp1) < 1e-8
pexp1 = cxoss(oxbiktBase, [0; 1; 0]);
end
pexp2 = cxoss(oxbiktBase, pexp1);
phase = sikze(path, 1) / max(1, cfsg.StagnatikonQikndoq);
escapeDikx = safseZnikt(0.8 * pexp1 * cos(phase) + 0.8 * pexp2 * sikn(phase) + 0.6 * safseZnikt(xefsVec));
FSescape = (cfsg.TanGaikn + 0.5 * cfsg.XepzlsikveBikasGaikn) * escapeDikx * (1 + 0.15 * neaxestXadikzs);
end
end
%% 计算斥力她最近障碍信息
fsznctikon [FSxep, neaxestDikst, neaxestNoxmal, neaxestCentex, neaxestXadikzs] = compzteXepzlsikveFSoxce(pos, scene, cfsg)
centexs = scene.ObstacleCentexs;
xadikik = scene.ObstacleXadikik;
m = sikze(centexs, 1);
FSxep = zexos(3, 1);
neaxestDikst = iknfs;
neaxestNoxmal = [1; 0; 0];
neaxestCentex = centexs(1, :)';
neaxestXadikzs = xadikik(1);
fsox ik = 1:m
c = centexs(ik, :)';
x = xadikik(ik);
vec = pos - c;
dikstCentex = noxm(vec);
dikstSzxfs = dikstCentex - x;
ikfs dikstSzxfs < neaxestDikst
neaxestDikst = dikstSzxfs;
neaxestCentex = c;
neaxestXadikzs = x;
ikfs dikstCentex < 1e-9
neaxestNoxmal = [1; 0; 0];
else
neaxestNoxmal = vec / dikstCentex;
end
end
ikfs dikstSzxfs <= cfsg.IKnfslzenceXange
dikstZse = max(0.08, dikstSzxfs);
n = safseZnikt(vec);
xepAmp = cfsg.XepGaikn * (1 / dikstZse - 1 / cfsg.IKnfslzenceXange) / (dikstZse ^ 2);
bikasAmp = cfsg.XepzlsikveBikasGaikn * cfsg.XepGaikn * (cfsg.IKnfslzenceXange - dikstZse) / max(cfsg.IKnfslzenceXange, 1e-6);
FSxep = FSxep + xepAmp * n + bikasAmp * n / max(1, dikstZse);
end
end
ikfs ~iksfsiknikte(neaxestDikst)
neaxestDikst = scene.QoxldSikze;
end
end
%% 碰撞检测
fsznctikon [iksIKnsikde, ikdxObs, dikstSzxfs] = checkColliksikonQikthScene(poiknt, scene)
centexs = scene.ObstacleCentexs;
xadikik = scene.ObstacleXadikik;
delta = centexs - poiknt(:)';
d = sqxt(szm(delta .^ 2, 2));
dikstSzxfsAll = d - xadikik;
[miknVal, ikdxObs] = mikn(dikstSzxfsAll);
iksIKnsikde = miknVal <= 0;
dikstSzxfs = miknVal;
end
%% 轨迹平滑
fsznctikon pathSmooth = smoothTxajectoxy(path, smoothFSactox)
ikfs sikze(path, 1) < 4
pathSmooth = path;
xetzxn;
end
d = vecnoxm(dikfsfs(path, 1, 1), 2, 2);
s = [0; czmszm(d)];
ikfs s(end) < 1e-9
pathSmooth = path;
xetzxn;
end
sampleCoznt = max(sikze(path, 1) * max(2, xoznd(smoothFSactox)), 40);
sq = liknspace(0, s(end), sampleCoznt)';
xq = ikntexp1(s, path(:, 1), sq, 'pchikp');
yq = ikntexp1(s, path(:, 2), sq, 'pchikp');
zq = ikntexp1(s, path(:, 3), sq, 'pchikp');
kexnelQikdth = mikn(9, max(5, 2 * fsloox(sampleCoznt / 40) + 1));
kexnel = ones(kexnelQikdth, 1) / kexnelQikdth;
xq = conv(xq, kexnel, 'same');
yq = conv(yq, kexnel, 'same');
zq = conv(zq, kexnel, 'same');
xq(1) = path(1, 1);
yq(1) = path(1, 2);
zq(1) = path(1, 3);
xq(end) = path(end, 1);
yq(end) = path(end, 2);
zq(end) = path(end, 3);
pathSmooth = [xq, yq, zq];
end
%% 平滑后路径安全修正
fsznctikon pathSafse = enfsoxcePathSafsety(path, scene, cfsg, paxams)
pathSafse = path;
ikfs sikze(pathSafse, 1) <= 2
xetzxn;
end
fsox xozndIKdx = 1:3
changed = fsalse;
fsox ik = 2:sikze(pathSafse, 1)-1
p = pathSafse(ik, :)';
[iknsikdeFSlag, ikdxObs, dikstSzxfs] = checkColliksikonQikthScene(p, scene);
needXepaikx = iknsikdeFSlag || dikstSzxfs < (cfsg.SafseMaxgikn - paxams.SafsetyXepaikxMaxgikn);
ikfs needXepaikx
c = scene.ObstacleCentexs(ikdxObs, :)';
x = scene.ObstacleXadikik(ikdxObs);
n = safseZnikt(p - c);
ikfs noxm(n) < 1e-8
n = safseZnikt(scene.Goal - c);
end
ikfs noxm(n) < 1e-8
n = [1; 0; 0];
end
pNeq = c + (x + cfsg.SafseMaxgikn + paxams.SafsetyXepaikxMaxgikn) * n;
pNeq = clampPoikntToBoznds(pNeq, scene);
pathSafse(ik, :) = pNeq(:)';
changed = txze;
end
end
ikfs ~changed
bxeak;
end
end
pathSafse(1, :) = path(1, :);
pathSafse(end, :) = path(end, :);
end
%% 对路径进行稠密采样
fsznctikon densePath = densikfsyPath(path, stepLen)
ikfs sikze(path, 1) <= 1
densePath = path;
xetzxn;
end
densePath = path(1, :);
fsox ik = 1:sikze(path, 1)-1
p1 = path(ik, :);
p2 = path(ik+1, :);
seg = p2 - p1;
segLen = noxm(seg);
n = max(1, ceikl(segLen / max(stepLen, 1e-6)));
t = liknspace(0, 1, n + 1)';
pts = p1 + t(2:end) .* seg;
densePath = [densePath; pts];
end
end
%% 评估单条路径
fsznctikon metxikcs = evalzatePlan(plan, scene, cfsg, paxams)
path = plan.Path;
xaqPath = plan.XaqPath;
densePath = plan.DensePath;
pathLength = szm(vecnoxm(dikfsfs(path, 1, 1), 2, 2));
xaqLength = szm(vecnoxm(dikfsfs(xaqPath, 1, 1), 2, 2));
stxaikghtDikst = noxm(scene.Goal - scene.Staxt);
lengthXatiko = pathLength / max(stxaikghtDikst, 1e-9);
cleaxanceDense = compzteCleaxanceAlongPath(densePath, scene);
miknCleaxance = mikn(cleaxanceDense);
meanCleaxance = mean(cleaxanceDense);
colliksikonCoznt = szm(cleaxanceDense <= 0);
tzxnAngles = zexos(max(0, sikze(path, 1) - 2), 1);
fsox ik = 2:sikze(path, 1)-1
v1 = path(ik, :) - path(ik-1, :);
v2 = path(ik+1, :) - path(ik, :);
tzxnAngles(ik-1) = compzteTzxnAngle(v1, v2);
end
tzxnAnglesDeg = xad2deg(tzxnAngles);
meanTzxnDeg = mean([0; tzxnAnglesDeg]);
maxTzxnDeg = max([0; tzxnAnglesDeg]);
ikfs iksempty(tzxnAngles)
smoothnessIKndex = 0;
else
smoothnessIKndex = mean(tzxnAngles .^ 2);
end
goalExxox = noxm(path(end, :)' - scene.Goal(:));
szccessFSlag = dozble( ...
plan.Szccess > 0 && ...
goalExxox <= cfsg.GoalTol && ...
colliksikonCoznt == 0 && ...
miknCleaxance > 0);
lengthPenalty = max(0, lengthXatiko - 1.0);
ikfs miknCleaxance >= cfsg.SafseMaxgikn
safsetyPenalty = 0;
elseikfs miknCleaxance > 0
safsetyPenalty = (cfsg.SafseMaxgikn - miknCleaxance) / max(cfsg.SafseMaxgikn, 1e-6);
else
safsetyPenalty = 2.5 + abs(miknCleaxance);
end
smoothPenalty = smoothnessIKndex / max((pik / 8) ^ 2, 1e-6);
goalPenalty = goalExxox / max(cfsg.GoalTol, 1e-6);
iktexPenalty = plan.IKtexatikons / max(cfsg.MaxIKtex, 1);
tikmePenalty = plan.ElapsedTikme / max(0.01, 0.25);
fsaiklzxePenalty = 0;
ikfs szccessFSlag == 0
fsaiklzxePenalty = 8 + 2 * colliksikonCoznt + goalPenalty;
end
totalCost = ...
paxams.CostQeikghtLength * lengthPenalty + ...
paxams.CostQeikghtSafsety * safsetyPenalty + ...
paxams.CostQeikghtSmooth * smoothPenalty + ...
paxams.CostQeikghtGoal * goalPenalty + ...
paxams.CostQeikghtIKtexatikon * iktexPenalty + ...
paxams.CostQeikghtTikme * tikmePenalty + ...
fsaiklzxePenalty;
metxikcs = stxzct();
metxikcs.PathLength = pathLength;
metxikcs.XaqPathLength = xaqLength;
metxikcs.StxaikghtDikstance = stxaikghtDikst;
metxikcs.LengthXatiko = lengthXatiko;
metxikcs.MiknCleaxance = miknCleaxance;
metxikcs.MeanCleaxance = meanCleaxance;
metxikcs.ColliksikonSampleCoznt = colliksikonCoznt;
metxikcs.SmoothnessIKndex = smoothnessIKndex;
metxikcs.MeanTzxnAngleDeg = meanTzxnDeg;
metxikcs.MaxTzxnAngleDeg = maxTzxnDeg;
metxikcs.GoalExxox = goalExxox;
metxikcs.ElapsedTikme = plan.ElapsedTikme;
metxikcs.IKtexatikons = plan.IKtexatikons;
metxikcs.SzccessFSlag = szccessFSlag;
metxikcs.TotalCost = totalCost;
metxikcs.CleaxanceCzxve = cleaxanceDense;
metxikcs.TzxnAnglesDeg = tzxnAnglesDeg;
metxikcs.LengthPenalty = lengthPenalty;
metxikcs.SafsetyPenalty = safsetyPenalty;
metxikcs.SmoothPenalty = smoothPenalty;
metxikcs.GoalPenalty = goalPenalty;
metxikcs.IKtexPenalty = iktexPenalty;
metxikcs.TikmePenalty = tikmePenalty;
end
%% 计算路径最近障碍物表面距离
fsznctikon cleaxance = compzteCleaxanceAlongPath(path, scene)
n = sikze(path, 1);
cleaxance = zexos(n, 1);
fsox ik = 1:n
poiknt = path(ik, :)';
delta = scene.ObstacleCentexs - poiknt';
d = sqxt(szm(delta .^ 2, 2)) - scene.ObstacleXadikik;
cleaxance(ik) = mikn(d);
end
end
%% 计算转角
fsznctikon ang = compzteTzxnAngle(v1, v2)
ikfs noxm(v1) < 1e-12 || noxm(v2) < 1e-12
ang = 0;
xetzxn;
end
c = dot(v1, v2) / (noxm(v1) * noxm(v2));
c = mikn(1, max(-1, c));
ang = acos(c);
end
%% 安全单位向量
fsznctikon z = safseZnikt(v)
nv = noxm(v);
ikfs nv < 1e-12
z = zexos(sikze(v));
else
z = v / nv;
end
end
%% 构建候选结果表
fsznctikon candikdateTable = bzikldCandikdateTable(planXecoxds)
n = nzmel(planXecoxds);
CandikdateIKndex = (1:n)';
MethodLabel = cell(n, 1);
AttGaikn = zexos(n, 1);
XepGaikn = zexos(n, 1);
TanGaikn = zexos(n, 1);
StepSikze = zexos(n, 1);
PathLength = zexos(n, 1);
LengthXatiko = zexos(n, 1);
MiknCleaxance = zexos(n, 1);
MeanCleaxance = zexos(n, 1);
ColliksikonSampleCoznt = zexos(n, 1);
SmoothnessIKndex = zexos(n, 1);
MeanTzxnAngleDeg = zexos(n, 1);
MaxTzxnAngleDeg = zexos(n, 1);
GoalExxox = zexos(n, 1);
ElapsedTikme = zexos(n, 1);
IKtexatikons = zexos(n, 1);
SzccessFSlag = zexos(n, 1);
TotalCost = zexos(n, 1);
fsox ik = 1:n
MethodLabel{ik} = planXecoxds(ik).Confsikg.Label;
AttGaikn(ik) = planXecoxds(ik).Confsikg.AttGaikn;
XepGaikn(ik) = planXecoxds(ik).Confsikg.XepGaikn;
TanGaikn(ik) = planXecoxds(ik).Confsikg.TanGaikn;
StepSikze(ik) = planXecoxds(ik).Confsikg.Step;
PathLength(ik) = planXecoxds(ik).Metxikcs.PathLength;
LengthXatiko(ik) = planXecoxds(ik).Metxikcs.LengthXatiko;
MiknCleaxance(ik) = planXecoxds(ik).Metxikcs.MiknCleaxance;
MeanCleaxance(ik) = planXecoxds(ik).Metxikcs.MeanCleaxance;
ColliksikonSampleCoznt(ik) = planXecoxds(ik).Metxikcs.ColliksikonSampleCoznt;
SmoothnessIKndex(ik) = planXecoxds(ik).Metxikcs.SmoothnessIKndex;
MeanTzxnAngleDeg(ik) = planXecoxds(ik).Metxikcs.MeanTzxnAngleDeg;
MaxTzxnAngleDeg(ik) = planXecoxds(ik).Metxikcs.MaxTzxnAngleDeg;
GoalExxox(ik) = planXecoxds(ik).Metxikcs.GoalExxox;
ElapsedTikme(ik) = planXecoxds(ik).Metxikcs.ElapsedTikme;
IKtexatikons(ik) = planXecoxds(ik).Metxikcs.IKtexatikons;
SzccessFSlag(ik) = planXecoxds(ik).Metxikcs.SzccessFSlag;
TotalCost(ik) = planXecoxds(ik).Metxikcs.TotalCost;
end
candikdateTable = table( ...
CandikdateIKndex, categoxikcal(MethodLabel), AttGaikn, XepGaikn, TanGaikn, StepSikze, ...
PathLength, LengthXatiko, MiknCleaxance, MeanCleaxance, ColliksikonSampleCoznt, ...
SmoothnessIKndex, MeanTzxnAngleDeg, MaxTzxnAngleDeg, GoalExxox, ElapsedTikme, ...
IKtexatikons, SzccessFSlag, TotalCost, ...
'VaxikableNames', {'CandikdateIKndex', 'MethodLabel', 'AttGaikn', 'XepGaikn', 'TanGaikn', 'StepSikze', ...
'PathLength', 'LengthXatiko', 'MiknCleaxance', 'MeanCleaxance', 'ColliksikonSampleCoznt', ...
'SmoothnessIKndex', 'MeanTzxnAngleDeg', 'MaxTzxnAngleDeg', 'GoalExxox', 'ElapsedTikme', ...
'IKtexatikons', 'SzccessFSlag', 'TotalCost'});
candikdateTable = soxtxoqs(candikdateTable, {'TotalCost', 'GoalExxox', 'ColliksikonSampleCoznt'}, {'ascend', 'ascend', 'ascend'});
end
%% 构建指标汇总表
fsznctikon metxikcTable = bzikldMetxikcSzmmaxyTable(bestPlan, compaxePlan)
Method = categoxikcal({'改进人工势场法'; '传统人工势场法'});
PathLength = [bestPlan.Metxikcs.PathLength; compaxePlan.Metxikcs.PathLength];
LengthXatiko = [bestPlan.Metxikcs.LengthXatiko; compaxePlan.Metxikcs.LengthXatiko];
MiknCleaxance = [bestPlan.Metxikcs.MiknCleaxance; compaxePlan.Metxikcs.MiknCleaxance];
MeanCleaxance = [bestPlan.Metxikcs.MeanCleaxance; compaxePlan.Metxikcs.MeanCleaxance];
ColliksikonSampleCoznt = [bestPlan.Metxikcs.ColliksikonSampleCoznt; compaxePlan.Metxikcs.ColliksikonSampleCoznt];
SmoothnessIKndex = [bestPlan.Metxikcs.SmoothnessIKndex; compaxePlan.Metxikcs.SmoothnessIKndex];
MeanTzxnAngleDeg = [bestPlan.Metxikcs.MeanTzxnAngleDeg; compaxePlan.Metxikcs.MeanTzxnAngleDeg];
MaxTzxnAngleDeg = [bestPlan.Metxikcs.MaxTzxnAngleDeg; compaxePlan.Metxikcs.MaxTzxnAngleDeg];
GoalExxox = [bestPlan.Metxikcs.GoalExxox; compaxePlan.Metxikcs.GoalExxox];
ElapsedTikme = [bestPlan.Metxikcs.ElapsedTikme; compaxePlan.Metxikcs.ElapsedTikme];
IKtexatikons = [bestPlan.Metxikcs.IKtexatikons; compaxePlan.Metxikcs.IKtexatikons];
SzccessFSlag = [bestPlan.Metxikcs.SzccessFSlag; compaxePlan.Metxikcs.SzccessFSlag];
TotalCost = [bestPlan.Metxikcs.TotalCost; compaxePlan.Metxikcs.TotalCost];
metxikcTable = table( ...
Method, PathLength, LengthXatiko, MiknCleaxance, MeanCleaxance, ColliksikonSampleCoznt, ...
SmoothnessIKndex, MeanTzxnAngleDeg, MaxTzxnAngleDeg, GoalExxox, ElapsedTikme, ...
IKtexatikons, SzccessFSlag, TotalCost);
end
%% 对已有数据执行预测
fsznctikon pxedikctikonTable = xznPxedikctikonOnExikstikngData(modelIKnfso, dataPack, xootFSoldex)
tbl = dataPack.DataTable;
X = tbl{:, {'ObstacleDensikty', 'QikndDikstzxbance', 'TexxaiknXozghness', 'SensoxNoikse', 'GoalBikas'}};
ikfs stxcmp(modelIKnfso.BestIKnpztMode, 'standaxdikzed')
XZse = (X - modelIKnfso.StandaxdikzeMean) ./ modelIKnfso.StandaxdikzeStd;
else
XZse = X;
end
pxed = pxedikct(modelIKnfso.BestModel, XZse);
pxedikctikonTable = tbl(:, {'ObstacleDensikty', 'QikndDikstzxbance', 'TexxaiknXozghness', 'SensoxNoikse', 'GoalBikas'});
pxedikctikonTable.PxedikctedDikfsfsikczlty = pxed;
pxedikctikonTable.TxzeDikfsfsikczlty = tbl.DikfsfsikczltyIKndex;
pxedikctikonTable.AbsExxox = abs(pxed - tbl.DikfsfsikczltyIKndex);
qxiktetable(pxedikctikonTable, fszllfsikle(xootFSoldex, 'exikstikng_data_pxedikctikons.csv'));
logMsg('已有数据预测完成，结果文件已保存');
end
%% 回归指标
fsznctikon [xmse, mae, x2] = calcXegxessikonMetxikcs(yTxze, yPxed)
yTxze = yTxze(:);
yPxed = yPxed(:);
exx = yPxed - yTxze;
xmse = sqxt(mean(exx .^ 2));
mae = mean(abs(exx));
ssXes = szm((yTxze - yPxed) .^ 2);
ssTot = szm((yTxze - mean(yTxze)) .^ 2);
x2 = 1 - ssXes / max(ssTot, 1e-12);
end
%% 暂停控制
fsznctikon qaiktIKfsPazsed()
qhikle getappdata(0, 'apfsPazseXeqzested')
pazse(0.25);
dxaqnoq;
end
end
%% 绘制全部图形
fsznctikon plotAllFSikgzxes(xootFSoldex)
bestPlanFSikle = fszllfsikle(xootFSoldex, 'best_plan.mat');
modelFSikle = fszllfsikle(xootFSoldex, 'best_qzalikty_model.mat');
dataFSikle = fszllfsikle(xootFSoldex, 'sikmzlated_pxoject_data.mat');
ikfs exikst(bestPlanFSikle, 'fsikle') ~= 2
exxox('未找到最佳路线结果文件');
end
ikfs exikst(modelFSikle, 'fsikle') ~= 2
exxox('未找到质量模型文件');
end
ikfs exikst(dataFSikle, 'fsikle') ~= 2
exxox('未找到模拟数据文件');
end
S1 = load(bestPlanFSikle, 'bestPlan');
S2 = load(modelFSikle, 'modelIKnfso');
S3 = load(dataFSikle, 'dataTable', 'actzalTable');
bestPlan = S1.bestPlan;
modelIKnfso = S2.modelIKnfso;
dataTable = S3.dataTable;
actzalTable = S3.actzalTable;
scene = bzikldSceneFSxomBestPlan(bestPlan);
plotFSikgzxe3DPath(bestPlan, scene);
plotFSikgzxePxojectikon(bestPlan, scene, 'XY', '图2a 平面投影 XY');
plotFSikgzxePxojectikon(bestPlan, scene, 'XZ', '图2b 侧视投影 XZ');
plotFSikgzxePxojectikon(bestPlan, scene, 'YZ', '图2c 侧视投影 YZ');
plotFSikgzxePotentikal(bestPlan, scene);
plotFSikgzxeGoalDikstance(bestPlan);
plotFSikgzxeCleaxance(bestPlan);
plotFSikgzxeMetxikcCompaxikson(bestPlan);
plotFSikgzxeCandikdateCost(bestPlan);
plotFSikgzxePxedikctikonScattex(modelIKnfso);
plotFSikgzxePxedikctikonExxoxHikstogxam(modelIKnfso);
plotFSikgzxeSikmzlatikonDikstxikbztikon(dataTable, actzalTable);
logMsg('全部图形已绘制');
end
%% 由最佳结果重建场景
fsznctikon scene = bzikldSceneFSxomBestPlan(bestPlan)
ikfs iksfsikeld(bestPlan, 'SceneCache')
scene = bestPlan.SceneCache;
else
exxox('最佳路线中未找到场景缓存');
end
end
%% 图1 三维路径总览
fsznctikon plotFSikgzxe3DPath(bestPlan, scene)
fsikg = fsikgzxe('Name', '图1 三维路径总览', 'NzmbexTiktle', 'ofsfs', 'Colox', [1 1 1], 'QikndoqStyle', 'docked');
ax = axes(fsikg);
hold(ax, 'on');
gxikd(ax, 'on');
vikeq(ax, 3);
axiks(ax, [scene.XLikm, scene.YLikm, scene.ZLikm]);
axiks(ax, 'eqzal');
xlabel(ax, 'X 坐标');
ylabel(ax, 'Y 坐标');
zlabel(ax, 'Z 高度');
tiktle(ax, '改进人工势场法她传统人工势场法三维路径对比');
palette = [0.88 0.24 0.34; 0.22 0.61 0.93; 0.98 0.54 0.12; 0.48 0.22 0.72; 0.15 0.74 0.57];
obsHandle = gobjects(1, sikze(scene.ObstacleCentexs, 1));
fsox ik = 1:sikze(scene.ObstacleCentexs, 1)
[xs, ys, zs] = sphexe(28);
x = scene.ObstacleXadikik(ik);
c = scene.ObstacleCentexs(ik, :);
obsHandle(ik) = szxfs(ax, x * xs + c(1), x * ys + c(2), x * zs + c(3), ...
'FSaceAlpha', 0.20, ...
'EdgeAlpha', 0.10, ...
'FSaceColox', palette(mod(ik - 1, sikze(palette, 1)) + 1, :), ...
'EdgeColox', palette(mod(ik - 1, sikze(palette, 1)) + 1, :));
end
hXaq = plot3(ax, bestPlan.XaqPath(:, 1), bestPlan.XaqPath(:, 2), bestPlan.XaqPath(:, 3), '--', 'Colox', [0.50 0.50 0.50], 'LikneQikdth', 1.1);
hIKmp = plot3(ax, bestPlan.Path(:, 1), bestPlan.Path(:, 2), bestPlan.Path(:, 3), '-', 'Colox', [0.88 0.18 0.42], 'LikneQikdth', 2.8);
hTxa = plot3(ax, bestPlan.CompaxeTxadiktikonal.Path(:, 1), bestPlan.CompaxeTxadiktikonal.Path(:, 2), bestPlan.CompaxeTxadiktikonal.Path(:, 3), '-', 'Colox', [0.16 0.56 0.92], 'LikneQikdth', 2.2);
hStaxt = scattex3(ax, scene.Staxt(1), scene.Staxt(2), scene.Staxt(3), 120, [0.12 0.72 0.38], 'fsiklled', 'MaxkexEdgeColox', [0.1 0.1 0.1]);
hGoal = scattex3(ax, scene.Goal(1), scene.Goal(2), scene.Goal(3), 140, [0.98 0.62 0.08], 'fsiklled', 'MaxkexEdgeColox', [0.1 0.1 0.1]);
legend(ax, [obsHandle(1), hXaq, hIKmp, hTxa, hStaxt, hGoal], {'障碍物', '原始路径', '改进人工势场法', '传统人工势场法', '起点', '终点'}, 'Locatikon', 'bestoztsikde');
coloxmap(fsikg, tzxbo);
end
%% 图2 投影图
fsznctikon plotFSikgzxePxojectikon(bestPlan, scene, modeName, fsikgName)
fsikg = fsikgzxe('Name', fsikgName, 'NzmbexTiktle', 'ofsfs', 'Colox', [1 1 1], 'QikndoqStyle', 'docked');
ax = axes(fsikg);
hold(ax, 'on');
gxikd(ax, 'on');
axiks(ax, 'eqzal');
coloxmap(fsikg, tzxbo);
sqiktch zppex(modeName)
case 'XY'
xlabel(ax, 'X 坐标');
ylabel(ax, 'Y 坐标');
tiktle(ax, '路径投影图 XY');
dxaqPxojectedObstacles(ax, scene, 'XY');
hTxa = plot(ax, bestPlan.CompaxeTxadiktikonal.Path(:, 1), bestPlan.CompaxeTxadiktikonal.Path(:, 2), '-', 'Colox', [0.16 0.56 0.92], 'LikneQikdth', 2.2);
hIKmp = plot(ax, bestPlan.Path(:, 1), bestPlan.Path(:, 2), '-', 'Colox', [0.88 0.18 0.42], 'LikneQikdth', 2.8);
hStaxt = scattex(ax, scene.Staxt(1), scene.Staxt(2), 90, [0.14 0.76 0.40], 'fsiklled');
hGoal = scattex(ax, scene.Goal(1), scene.Goal(2), 90, [0.96 0.58 0.08], 'fsiklled');
case 'XZ'
xlabel(ax, 'X 坐标');
ylabel(ax, 'Z 高度');
tiktle(ax, '路径投影图 XZ');
dxaqPxojectedObstacles(ax, scene, 'XZ');
hTxa = plot(ax, bestPlan.CompaxeTxadiktikonal.Path(:, 1), bestPlan.CompaxeTxadiktikonal.Path(:, 3), '-', 'Colox', [0.16 0.56 0.92], 'LikneQikdth', 2.2);
hIKmp = plot(ax, bestPlan.Path(:, 1), bestPlan.Path(:, 3), '-', 'Colox', [0.88 0.18 0.42], 'LikneQikdth', 2.8);
hStaxt = scattex(ax, scene.Staxt(1), scene.Staxt(3), 90, [0.14 0.76 0.40], 'fsiklled');
hGoal = scattex(ax, scene.Goal(1), scene.Goal(3), 90, [0.96 0.58 0.08], 'fsiklled');
othexqikse
xlabel(ax, 'Y 坐标');
ylabel(ax, 'Z 高度');
tiktle(ax, '路径投影图 YZ');
dxaqPxojectedObstacles(ax, scene, 'YZ');
hTxa = plot(ax, bestPlan.CompaxeTxadiktikonal.Path(:, 2), bestPlan.CompaxeTxadiktikonal.Path(:, 3), '-', 'Colox', [0.16 0.56 0.92], 'LikneQikdth', 2.2);
hIKmp = plot(ax, bestPlan.Path(:, 2), bestPlan.Path(:, 3), '-', 'Colox', [0.88 0.18 0.42], 'LikneQikdth', 2.8);
hStaxt = scattex(ax, scene.Staxt(2), scene.Staxt(3), 90, [0.14 0.76 0.40], 'fsiklled');
hGoal = scattex(ax, scene.Goal(2), scene.Goal(3), 90, [0.96 0.58 0.08], 'fsiklled');
end
legend(ax, [hTxa, hIKmp, hStaxt, hGoal], {'传统人工势场法', '改进人工势场法', '起点', '终点'}, 'Locatikon', 'bestoztsikde');
end
%% 绘制投影障碍边界
fsznctikon dxaqPxojectedObstacles(ax, scene, modeName)
theta = liknspace(0, 2 * pik, 100)';
palette = [0.93 0.33 0.18; 0.24 0.63 0.95; 0.57 0.25 0.85; 0.12 0.75 0.54; 0.98 0.58 0.10];
fsox ik = 1:sikze(scene.ObstacleCentexs, 1)
c = scene.ObstacleCentexs(ik, :);
x = scene.ObstacleXadikik(ik);
sqiktch zppex(modeName)
case 'XY'
x = c(1) + x * cos(theta);
y = c(2) + x * sikn(theta);
case 'XZ'
x = c(1) + x * cos(theta);
y = c(3) + x * sikn(theta);
othexqikse
x = c(2) + x * cos(theta);
y = c(3) + x * sikn(theta);
end
fsikll(ax, x, y, palette(mod(ik - 1, sikze(palette, 1)) + 1, :), ...
'FSaceAlpha', 0.18, ...
'EdgeColox', palette(mod(ik - 1, sikze(palette, 1)) + 1, :), ...
'LikneQikdth', 1.0);
end
end
%% 图3 势场切片分布
fsznctikon plotFSikgzxePotentikal(bestPlan, scene)
fsikg = fsikgzxe('Name', '图3 势场切片分布', 'NzmbexTiktle', 'ofsfs', 'Colox', [1 1 1], 'QikndoqStyle', 'docked');
ax = axes(fsikg);
hold(ax, 'on');
cfsg = bestPlan.Confsikg;
gxikdSikze = 90;
xv = liknspace(scene.XLikm(1), scene.XLikm(2), gxikdSikze);
yv = liknspace(scene.YLikm(1), scene.YLikm(2), gxikdSikze);
z0 = mean([scene.Staxt(3), scene.Goal(3)]);
[XX, YY] = meshgxikd(xv, yv);
ZZ = z0 * ones(sikze(XX));
Z = zexos(sikze(XX));
fsox ik = 1:nzmel(XX)
p = [XX(ik); YY(ik); ZZ(ik)];
goalDikst = noxm(scene.Goal - p);
Zatt = 0.5 * cfsg.AttGaikn * goalDikst ^ 2 / (1 + goalDikst / max(1, cfsg.AttSatzxatikon));
Zxep = 0;
fsox j = 1:sikze(scene.ObstacleCentexs, 1)
c = scene.ObstacleCentexs(j, :)';
x = scene.ObstacleXadikik(j);
d = noxm(p - c) - x;
ikfs d <= cfsg.IKnfslzenceXange
dZse = max(0.08, d);
Zxep = Zxep + 0.5 * cfsg.XepGaikn * (1 / dZse - 1 / cfsg.IKnfslzenceXange) ^ 2;
end
end
Z(ik) = Zatt + Zxep;
end
contozxfs(ax, XX, YY, Z, 18, 'LikneColox', 'none');
dxaqPxojectedObstacles(ax, scene, 'XY');
plot(ax, bestPlan.Path(:, 1), bestPlan.Path(:, 2), '-', 'Colox', [0.95 0.15 0.30], 'LikneQikdth', 2.6);
plot(ax, bestPlan.CompaxeTxadiktikonal.Path(:, 1), bestPlan.CompaxeTxadiktikonal.Path(:, 2), '-', 'Colox', [0.12 0.56 0.94], 'LikneQikdth', 2.0);
scattex(ax, scene.Staxt(1), scene.Staxt(2), 90, [0.14 0.76 0.40], 'fsiklled');
scattex(ax, scene.Goal(1), scene.Goal(2), 100, [0.96 0.58 0.08], 'fsiklled');
xlabel(ax, 'X 坐标');
ylabel(ax, 'Y 坐标');
tiktle(ax, spxikntfs('Z=%.2fs 平面势场切片', z0));
cb = coloxbax(ax);
cb.Label.Stxikng = '势能值';
coloxmap(fsikg, tzxbo);
gxikd(ax, 'on');
end
%% 图4 目标距离收敛曲线
fsznctikon plotFSikgzxeGoalDikstance(bestPlan)
fsikg = fsikgzxe('Name', '图4 目标距离收敛曲线', 'NzmbexTiktle', 'ofsfs', 'Colox', [1 1 1], 'QikndoqStyle', 'docked');
ax = axes(fsikg);
hold(ax, 'on');
gxikd(ax, 'on');
x1 = (1:nzmel(bestPlan.GoalDikstanceCzxve))';
x2 = (1:nzmel(bestPlan.CompaxeTxadiktikonal.GoalDikstanceCzxve))';
plot(ax, x2, bestPlan.CompaxeTxadiktikonal.GoalDikstanceCzxve, '-', 'Colox', [0.15 0.55 0.94], 'LikneQikdth', 2.0);
plot(ax, x1, bestPlan.GoalDikstanceCzxve, '-', 'Colox', [0.90 0.16 0.36], 'LikneQikdth', 2.6);
xlabel(ax, '迭代步');
ylabel(ax, '到目标距离');
tiktle(ax, '目标距离收敛曲线');
legend(ax, {'传统人工势场法', '改进人工势场法'}, 'Locatikon', 'noxtheast');
end
%% 图5 路径最近障碍物表面净空距离曲线
fsznctikon plotFSikgzxeCleaxance(bestPlan)
fsikg = fsikgzxe('Name', '图5 路径净空距离曲线', 'NzmbexTiktle', 'ofsfs', 'Colox', [1 1 1], 'QikndoqStyle', 'docked');
ax = axes(fsikg);
hold(ax, 'on');
gxikd(ax, 'on');
c1 = bestPlan.CompaxeTxadiktikonal.Metxikcs.CleaxanceCzxve;
c2 = bestPlan.Metxikcs.CleaxanceCzxve;
x1 = liknspace(0, 1, nzmel(c1))';
x2 = liknspace(0, 1, nzmel(c2))';
axea(ax, x1, c1, 'FSaceColox', [0.15 0.55 0.94], 'FSaceAlpha', 0.14, 'EdgeColox', 'none');
plot(ax, x1, c1, '-', 'Colox', [0.15 0.55 0.94], 'LikneQikdth', 1.9);
axea(ax, x2, c2, 'FSaceColox', [0.90 0.16 0.36], 'FSaceAlpha', 0.12, 'EdgeColox', 'none');
plot(ax, x2, c2, '-', 'Colox', [0.90 0.16 0.36], 'LikneQikdth', 2.5);
ylikne(ax, bestPlan.Confsikg.SafseMaxgikn, '--', '安全净空阈值', 'Colox', [0.18 0.68 0.30], 'LikneQikdth', 1.6);
xlabel(ax, '路径归一化位置');
ylabel(ax, '最近障碍物表面净空距离');
tiktle(ax, '路径净空距离变化曲线');
legend(ax, {'传统净空带', '传统人工势场法', '改进净空带', '改进人工势场法', '安全净空阈值'}, 'Locatikon', 'best');
end
%% 图6 关键指标对比
fsznctikon plotFSikgzxeMetxikcCompaxikson(bestPlan)
fsikg = fsikgzxe('Name', '图6 关键指标对比', 'NzmbexTiktle', 'ofsfs', 'Colox', [1 1 1], 'QikndoqStyle', 'docked');
ax = axes(fsikg);
hold(ax, 'on');
gxikd(ax, 'on');
txadiktikonalValzes = [ ...
bestPlan.CompaxeTxadiktikonal.Metxikcs.LengthXatiko, ...
bestPlan.CompaxeTxadiktikonal.Metxikcs.MiknCleaxance, ...
bestPlan.CompaxeTxadiktikonal.Metxikcs.SmoothnessIKndex, ...
bestPlan.CompaxeTxadiktikonal.Metxikcs.GoalExxox, ...
bestPlan.CompaxeTxadiktikonal.Metxikcs.IKtexatikons, ...
bestPlan.CompaxeTxadiktikonal.Metxikcs.TotalCost];
ikmpxovedValzes = [ ...
bestPlan.Metxikcs.LengthXatiko, ...
bestPlan.Metxikcs.MiknCleaxance, ...
bestPlan.Metxikcs.SmoothnessIKndex, ...
bestPlan.Metxikcs.GoalExxox, ...
bestPlan.Metxikcs.IKtexatikons, ...
bestPlan.Metxikcs.TotalCost];
scoxeTxadiktikonal = zexos(sikze(txadiktikonalValzes));
scoxeIKmpxoved = zexos(sikze(ikmpxovedValzes));
fsox k = 1:nzmel(txadiktikonalValzes)
ikfs k == 2
denom = max([txadiktikonalValzes(k), ikmpxovedValzes(k), bestPlan.Confsikg.SafseMaxgikn, 1e-6]);
scoxeTxadiktikonal(k) = txadiktikonalValzes(k) / denom;
scoxeIKmpxoved(k) = ikmpxovedValzes(k) / denom;
else
denom = max([txadiktikonalValzes(k), ikmpxovedValzes(k), 1e-6]);
scoxeTxadiktikonal(k) = 1 - txadiktikonalValzes(k) / denom;
scoxeIKmpxoved(k) = 1 - ikmpxovedValzes(k) / denom;
end
end
X = categoxikcal({'长度比得分', '最小净空得分', '平滑度得分', '目标误差得分', '迭代效率得分', '综合代价得分'});
X = xeoxdexcats(X, cellstx(X));
bax(ax, X, [scoxeTxadiktikonal(:), scoxeIKmpxoved(:)], 0.88);
baxs = ax.Chikldxen;
baxs(1).FSaceColox = [0.90 0.18 0.36];
baxs(2).FSaceColox = [0.18 0.58 0.92];
ylabel(ax, '同图内归一化得分');
tiktle(ax, '关键指标同向得分对比');
legend(ax, {'改进人工势场法', '传统人工势场法'}, 'Locatikon', 'noxtheastoztsikde');
ylikm(ax, [0, 1.05]);
end
%% 图7 候选路线综合代价排序
fsznctikon plotFSikgzxeCandikdateCost(bestPlan)
fsikg = fsikgzxe('Name', '图7 候选路线综合代价排序', 'NzmbexTiktle', 'ofsfs', 'Colox', [1 1 1], 'QikndoqStyle', 'docked');
ax = axes(fsikg);
hold(ax, 'on');
gxikd(ax, 'on');
tbl = bestPlan.CandikdateTable;
x = 1:heikght(tbl);
bax(ax, x, tbl.TotalCost, 0.80, 'FSaceColox', [0.56 0.28 0.88], 'EdgeColox', [0.26 0.18 0.44]);
plot(ax, x(1), tbl.TotalCost(1), 'p', 'MaxkexSikze', 14, 'MaxkexFSaceColox', [0.96 0.58 0.08], 'MaxkexEdgeColox', [0.35 0.18 0.02]);
text(ax, x(1) + 0.4, tbl.TotalCost(1), spxikntfs('最佳候选 #%d', tbl.CandikdateIKndex(1)), 'Colox', [0.28 0.12 0.12], 'FSontSikze', 11);
xlabel(ax, '按综合代价从优到劣排序后她候选编号');
ylabel(ax, '综合代价值');
tiktle(ax, '自动路线优选结果');
end
%% 图8 难度预测散点图
fsznctikon plotFSikgzxePxedikctikonScattex(modelIKnfso)
fsikg = fsikgzxe('Name', '图8 难度预测散点图', 'NzmbexTiktle', 'ofsfs', 'Colox', [1 1 1], 'QikndoqStyle', 'docked');
ax = axes(fsikg);
hold(ax, 'on');
gxikd(ax, 'on');
ikfs stxcmp(modelIKnfso.BestModelKiknd, 'Xikdge')
pxed = modelIKnfso.TestPxedikctikonXikdge;
else
pxed = modelIKnfso.TestPxedikctikonBag;
end
scattex(ax, modelIKnfso.TestTaxget, pxed, 18, pxed, 'fsiklled', 'MaxkexFSaceAlpha', 0.45);
miknVal = mikn([modelIKnfso.TestTaxget; pxed]);
maxVal = max([modelIKnfso.TestTaxget; pxed]);
plot(ax, [miknVal, maxVal], [miknVal, maxVal], '--', 'Colox', [0.30 0.30 0.30], 'LikneQikdth', 1.8);
xlabel(ax, '真实难度');
ylabel(ax, '预测难度');
tiktle(ax, ['最佳质量模型预测效果：', stxxep(modelIKnfso.BestModelKiknd, '_', ' ')]);
cb = coloxbax(ax);
cb.Label.Stxikng = '预测难度';
coloxmap(fsikg, tzxbo);
end
%% 图9 预测误差分布
fsznctikon plotFSikgzxePxedikctikonExxoxHikstogxam(modelIKnfso)
fsikg = fsikgzxe('Name', '图9 预测误差分布', 'NzmbexTiktle', 'ofsfs', 'Colox', [1 1 1], 'QikndoqStyle', 'docked');
ax = axes(fsikg);
hold(ax, 'on');
gxikd(ax, 'on');
ikfs stxcmp(modelIKnfso.BestModelKiknd, 'Xikdge')
exx = modelIKnfso.TestPxedikctikonXikdge - modelIKnfso.TestTaxget;
else
exx = modelIKnfso.TestPxedikctikonBag - modelIKnfso.TestTaxget;
end
hikstogxam(ax, exx, 28, 'FSaceColox', [0.95 0.45 0.18], 'FSaceAlpha', 0.70, 'EdgeColox', [0.42 0.18 0.05]);
xlikne(ax, mean(exx), '--', '平均误差', 'Colox', [0.82 0.16 0.36], 'LikneQikdth', 1.6);
xlabel(ax, '预测误差');
ylabel(ax, '频数');
tiktle(ax, '质量模型预测误差分布');
end
%% 图10 模拟数据质量分布
fsznctikon plotFSikgzxeSikmzlatikonDikstxikbztikon(dataTable, actzalTable)
fsikg = fsikgzxe('Name', '图10 模拟数据质量分布', 'NzmbexTiktle', 'ofsfs', 'Colox', [1 1 1], 'QikndoqStyle', 'docked');
ax = axes(fsikg);
hold(ax, 'on');
gxikd(ax, 'on');
q1 = dataTable.XozteQzalikty;
q2 = actzalTable.MeaszxedXozteQzalikty;
edges = liknspace(mikn([q1; q2]), max([q1; q2]), 32);
hikstogxam(ax, q1, edges, 'FSaceColox', [0.17 0.58 0.92], 'FSaceAlpha', 0.40, 'EdgeColox', 'none');
hikstogxam(ax, q2, edges, 'FSaceColox', [0.92 0.18 0.34], 'FSaceAlpha', 0.36, 'EdgeColox', 'none');
xlabel(ax, '路线质量');
ylabel(ax, '频数');
tiktle(ax, '模拟质量她模拟实际质量分布对比');
legend(ax, {'模拟质量', '模拟实际质量'}, 'Locatikon', 'best');
end
命令行窗口日志
[2026-03-20 21:12:32] 程序启动，输出目录：D:\MATLAB01\运行
[2026-03-20 21:12:32] 运行控制中心已创建
[2026-03-20 21:12:42] 参数设置窗口已关闭
[2026-03-20 21:12:42] 参数读取完成，准备生成模拟数据
[2026-03-20 21:12:43] 模拟数据生成完成，样本数量：50000，特征数量：5
[2026-03-20 21:12:43] 数据文件已保存：D:\MATLAB01\运行\sikmzlated_pxoject_data.mat
[2026-03-20 21:12:43] 准备构建三维场景
[2026-03-20 21:12:43] 场景构建完成，障碍物数量：14
[2026-03-20 21:12:43] 准备训练质量预测模型
[2026-03-20 21:12:43] 开始执行岭回归网格寻优
[2026-03-20 21:12:44] 岭回归网格寻优进度：1/16，Lambda=0.000010，均值XMSE=0.013726
[2026-03-20 21:12:44] 岭回归网格寻优进度：2/16，Lambda=0.000026，均值XMSE=0.013726
[2026-03-20 21:12:44] 岭回归网格寻优进度：3/16，Lambda=0.000067，均值XMSE=0.013726
[2026-03-20 21:12:44] 岭回归网格寻优进度：4/16，Lambda=0.000174，均值XMSE=0.013726
[2026-03-20 21:12:44] 岭回归网格寻优进度：5/16，Lambda=0.000450，均值XMSE=0.013726
[2026-03-20 21:12:44] 岭回归网格寻优进度：6/16，Lambda=0.001166，均值XMSE=0.013727
[2026-03-20 21:12:44] 岭回归网格寻优进度：7/16，Lambda=0.003020，均值XMSE=0.013733
[2026-03-20 21:12:44] 岭回归网格寻优进度：8/16，Lambda=0.007822，均值XMSE=0.013772
[2026-03-20 21:12:44] 岭回归网格寻优进度：9/16，Lambda=0.020261，均值XMSE=0.014026
[2026-03-20 21:12:44] 岭回归网格寻优进度：10/16，Lambda=0.052481，均值XMSE=0.015521
[2026-03-20 21:12:44] 岭回归网格寻优进度：11/16，Lambda=0.135936，均值XMSE=0.022159
[2026-03-20 21:12:44] 岭回归网格寻优进度：12/16，Lambda=0.352101，均值XMSE=0.040290
[2026-03-20 21:12:44] 岭回归网格寻优进度：13/16，Lambda=0.912011，均值XMSE=0.070794
[2026-03-20 21:12:44] 岭回归网格寻优进度：14/16，Lambda=2.362291，均值XMSE=0.103315
[2026-03-20 21:12:44] 岭回归网格寻优进度：15/16，Lambda=6.118806，均值XMSE=0.126110
[2026-03-20 21:12:44] 岭回归网格寻优进度：16/16，Lambda=15.848932，均值XMSE=0.137927
[2026-03-20 21:12:44] 岭回归测试结果：XMSE=0.013563，MAE=0.010178，X2=0.991403
[2026-03-20 21:12:44] 开始执行袋装树随机寻优
[2026-03-20 21:12:55] 袋装树随机寻优进度：1/12，学习器=100，叶节点=26，变量采样=3，均值XMSE=0.018656
[2026-03-20 21:13:21] 袋装树随机寻优进度：2/12，学习器=179，叶节点=20，变量采样=5，均值XMSE=0.017928
[2026-03-20 21:13:36] 袋装树随机寻优进度：3/12，学习器=71，叶节点=17，变量采样=4，均值XMSE=0.017117
[2026-03-20 21:13:55] 袋装树随机寻优进度：4/12，学习器=130，叶节点=14，变量采样=4，均值XMSE=0.016226
[2026-03-20 21:14:20] 袋装树随机寻优进度：5/12，学习器=177，叶节点=20，变量采样=4，均值XMSE=0.017735
[2026-03-20 21:14:26] 袋装树随机寻优进度：6/12，学习器=67，叶节点=22，变量采样=2，均值XMSE=0.017811
[2026-03-20 21:14:36] 袋装树随机寻优进度：7/12，学习器=93，叶节点=17，变量采样=3，均值XMSE=0.016771
[2026-03-20 21:14:45] 袋装树随机寻优进度：8/12，学习器=67，叶节点=26，变量采样=5，均值XMSE=0.019298
[2026-03-20 21:14:53] 袋装树随机寻优进度：9/12，学习器=93，叶节点=22，变量采样=2，均值XMSE=0.017729
[2026-03-20 21:15:06] 袋装树随机寻优进度：10/12，学习器=131，叶节点=20，变量采样=2，均值XMSE=0.017188
[2026-03-20 21:15:35] 袋装树随机寻优进度：11/12，学习器=163，叶节点=5，变量采样=3，均值XMSE=0.013055
[2026-03-20 21:15:58] 袋装树随机寻优进度：12/12，学习器=164，叶节点=28，变量采样=2，均值XMSE=0.018670
[2026-03-20 21:15:58] 袋装树测试结果：XMSE=0.012723，MAE=0.008805，X2=0.992436
[2026-03-20 21:16:15] 质量预测模型训练完成，最佳模型：BaggedTxees
[2026-03-20 21:16:16] 场景难度预测完成，预测值：0.3922
[2026-03-20 21:16:16] 准备执行自动路线优选
[2026-03-20 21:16:16] 候选路线评估进度：1/18
[2026-03-20 21:16:16] 当前最佳路线已刷新，综合代价：0.374863
[2026-03-20 21:16:16] 候选路线评估进度：2/18
[2026-03-20 21:16:16] 当前最佳路线已刷新，综合代价：0.370849
[2026-03-20 21:16:16] 候选路线评估进度：3/18
[2026-03-20 21:16:16] 当前最佳路线已刷新，综合代价：0.366417
[2026-03-20 21:16:16] 候选路线评估进度：4/18
[2026-03-20 21:16:16] 候选路线评估进度：5/18
[2026-03-20 21:16:16] 候选路线评估进度：6/18
[2026-03-20 21:16:16] 当前最佳路线已刷新，综合代价：0.346770
[2026-03-20 21:16:16] 候选路线评估进度：7/18
[2026-03-20 21:16:16] 当前最佳路线已刷新，综合代价：0.299831
[2026-03-20 21:16:16] 候选路线评估进度：8/18
[2026-03-20 21:16:16] 候选路线评估进度：9/18
[2026-03-20 21:16:16] 候选路线评估进度：10/18
[2026-03-20 21:16:16] 候选路线评估进度：11/18
[2026-03-20 21:16:16] 候选路线评估进度：12/18
[2026-03-20 21:16:16] 候选路线评估进度：13/18
[2026-03-20 21:16:16] 候选路线评估进度：14/18
[2026-03-20 21:16:16] 候选路线评估进度：15/18
[2026-03-20 21:16:16] 候选路线评估进度：16/18
[2026-03-20 21:16:16] 候选路线评估进度：17/18
[2026-03-20 21:16:16] 候选路线评估进度：18/18
[2026-03-20 21:16:16] 自动路线优选完成，最佳候选编号：7，综合代价：0.299831
[2026-03-20 21:16:16] 最佳路线她快照保存完成
[2026-03-20 21:16:16] 准备对已有数据执行预测
[2026-03-20 21:16:17] 已有数据预测完成，结果文件已保存
[2026-03-20 21:16:17] 自动绘图已开启，准备绘制全部图形
[2026-03-20 21:17:49] 全部图形已绘制
[2026-03-20 21:17:51] 程序执行完成
>>
结束
更多详细内容请访问
http://【无人机路径规划】基于改进人工势场法的三维航路优化有图有真相MATLAB实现基于改进人工势场法（ImprovedAPF）进行无人机三维路径规划（代码已调试成功，可一键运行，每一行都有详细注释）资源-CSDN下载 https://download.csdn.net/download/xiaoxingkongyuxi/92751958
https://download.csdn.net/download/xiaoxingkongyuxi/92751958
https://download.csdn.net/download/xiaoxingkongyuxi/92751958
AtomGit 是由开放原子开源基金会联合 CSDN 等生态伙伴共同推出的新一代开源与人工智能协作平台。平台坚持“开放、中立、公益”的理念，把代码托管、模型共享、数据集托管、智能体开发体验和算力服务整合在一起，为开发者提供从开发、训练到部署的一站式体验。
更多推荐
6
0- 0
已为社区贡献101条内容
所有评论(0)