2019年第十六届中国研究生数学建模竞赛F题·一种快速找到最优解的算法（提供Matlab源码）

0. 源码+数据

链接：https://pan.baidu.com/s/1Autyo3Q_iDJ5ka8oD8LEag
提取码：gaec

1. 问题介绍

复杂环境下航迹快速规划是智能飞行器控制的一个重要课题。由于系统结构限制，这类飞行器的定位系统无法对自身进行精准定位，一旦定位误差积累到一定程度可能导致任务失败。因此，在飞行过程中对定位误差进行校正是智能飞行器航迹规划中一项重要任务。本题目研究智能飞行器在系统定位精度限制下的航迹快速规划问题。
假设飞行器的飞行区域如下图所示，出发点为A点，目的地为B点。其航迹约束如下：

1）飞行器在空间飞行过程中需要实时定位，其定位误差包括垂直误差和水平误差。飞行器每飞行1m，垂直误差和水平误差将各增加δ个专用单位,，以下简称单位。到达终点时垂直误差和水平误差均应小于θ个单位，并且为简化问题，假设当垂直误差和水平误差均小于θ个单位时，飞行器仍能够按照规划路径飞行。
2）飞行器在飞行过程中需要对定位误差进行校正。飞行区域中存在一些安全位置（称之为校正点）可用于误差校正，当飞行器到达校正点即能够根据该位置的误差校正类型进行误差校正。校正垂直和水平误差的位置可根据地形在航迹规划前确定（如图1为某条航迹的示意图, 黄色的点为水平误差校正点，蓝色的点为垂直误差校正点，出发点为A点，目的地为B点，黑色曲线代表一条航迹）。可校正的飞行区域分布位置依赖于地形，无统一规律。若垂直误差、水平误差都能得到及时校正，则飞行器可以按照预定航线飞行，通过若干个校正点进行误差校正后最终到达目的地。
3）在出发地A点，飞行器的垂直和水平误差均为0。
4）飞行器在垂直误差校正点进行垂直误差校正后，其垂直误差将变为0，水平误差保持不变。
5）飞行器在水平误差校正点进行水平误差校正后，其水平误差将变为0，垂直误差保持不变。
6）当飞行器的垂直误差不大于α_1个单位，水平误差不大于α_2个单位时才能进行垂直误差校正。
7）当飞行器的垂直误差不大于β_1个单位，水平误差不大于β_2个单位时才能进行水平误差校正。

请你们团队为上述智能飞行器建立从A点飞到B点的航迹规划一般模型和算法并完成以下问题：
问题1. 针对附件1中的数据规划满足条件（1）~（7）时飞行器的航迹，并且综合考虑以下优化目标：
（A）航迹长度尽可能小；（B）经过校正区域进行校正的次数尽可能少。
其中附件1数据的参数为：
α 1 = 25 ， α 2 = 15 ， β 1 = 20 ， β 2 = 25 ， θ = 30 ， δ = 0.001 {\alpha _1} = 25，{\alpha _2} = 15，{\beta _1} = 20，{\beta _2} = 25，\theta = 30，\delta = 0.001 α1=25，α2=15，β1=20，β2=25，θ=30，δ=0.001

2. 贪心寻找较优解

2.1 算法步骤

寻找可行驶到的下一步
评估下一步：到终点的距离小为优（飞行器行驶到下一校正点时，可能累计误差小于 θ 评估下一步：到终点的距离小为优（飞行器行驶到下一校正点时，可能累计误差小于\theta 评估下一步：到终点的距离小为优（飞行器行驶到下一校正点时，可能累计误差小于θ, ）
以50条优解进行同时搜索
待达到终点后，选择路程最短的解为输出

2.2 源码

2.2.1 主程序

% 2019年第十六届中国研究生数学建模竞赛F题·多约束条件下智能飞行器航迹快速规划
% --优先考虑增添误差长度后到终点的距离
% 目录
% a. 预处理
% b. 主循环
%     b.1 寻找下一步的可行点
%     b.2 评价
%     b.3 选择
% c. 可视化输出
% _________________________________________________________________________________________%% a. 预处理
clear,clc
load data1.mat
% 第一列 校正点的序号
% 第二至四列 x,y,z坐标
% 第五列 校正点的类型（0表示水平误差校正点，1表示垂直误差校正点，2表示起点，3表示终点）data1(:,1) = data1(:,1)+1;         % 编号从0开始，更改为从1开始a1 = 25;
a2 = 15;
b1 = 20;
b2 = 25;
c = 30;
d = 0.001;Goal_Coordinate = data1(end,2:4);  % 终点坐标max_Length = c/0.001;              % 正常行驶的最大距离% 各点之间的代价矩阵
tic
Cost_matrix = zeros(size(data1,1),size(data1,1));
Length_matrix =  zeros(size(data1,1),size(data1,1));
for i = 1:size(data1,1)for j = 1:size(data1,1)Length_matrix(i,j) = sqrt(sum((data1(i,2:4)-data1(j,2:4)).^2));Cost_matrix(i,j) = Length_matrix(i,j);if Length_matrix(i,j) >= max_LengthCost_matrix(i,j) = inf;elseCost_matrix(i,j) = Cost_matrix(i,j)*d;endend
end
toc%% b. 主循环
tic
New_path = [{1},{[0,0]},{inf},{0}];
while isempty(find(cell2mat(New_path(:,4))==1, 1))Mother_New_path = {};for j = 1:size(New_path,1)
%% b.1 寻找下一步的可行点% 当前点信息num = New_path{j,1}(end,1);                 % 当前点编号Coordinate = data1(num,2:4);                % 当前点的坐标Vertical_error = New_path{j,2}(1,1);        % 当前点的垂直误差     Horizontal_error = New_path{j,2}(1,2);      % 当前点的水平误差   % 寻找下一步可行点Path_Vertical = [];Path_Horizontal = [];for i = 2:size(data1,1)if isempty(find(New_path{j,1}==i, 1)) && Cost_matrix(num,i) ~= inf % 如果和之前的路径不重复，并在最大距离以内% 分以下三种情况：% 可以走当前路径，并能够进行垂直校正，保存路径% 可以走当前路径，并能够进行水平校正，保存路径% 附：如果三种情况都不存在，既是没有可行的下一步节点   if (Cost_matrix(num,i)+Vertical_error <= a1) && (Cost_matrix(num,i)+Horizontal_error <= a2) && data1(i,5) == 1 Path_Vertical(end+1,:) = [i,0,Cost_matrix(num,i)+Horizontal_error]; elseif (Cost_matrix(num,i)+Vertical_error <= b1) && (Cost_matrix(num,i)+Horizontal_error <= b2) && data1(i,5) == 0 Path_Horizontal(end+1,:) = [i,Cost_matrix(num,i)+Vertical_error,0]; endendend% 所有的待选择路径 Path_selecting% 第一列是节点序号% 第二列是垂直误差% 第三列是水平误差% 第四列是到终点的距离Path_selecting = [Path_Vertical;Path_Horizontal]; if isempty(Path_selecting)continueendfor i = 1:size(Path_selecting,1)Path_selecting(i,4) = Length_matrix(data1(end,1),data1(Path_selecting(i,1),1));Path_selecting(i,5) = Length_matrix(data1(end,1),data1(Path_selecting(i,1),1))-max(Path_selecting(i,2:3))/d;end%% b.2 评价Evaluate = sortrows(Path_selecting,5); % 按照终点的距离排序%% b.3 选择Path_choosed = Evaluate(1:ceil(0.8*size(Evaluate,1)),1:4);Son_New_path = cell(size(Path_choosed,1),2);for i = 1:size(Path_choosed,1)Son_New_path(i,1) = {[New_path{j,1};Path_choosed(i,1)]}; % 第一列为当前路径（使用点的编号表示）Son_New_path(i,2) = {Path_choosed(i,2:3)};               % 第二列为当前路径的垂直误差和水平误差Son_New_path(i,3) = {Path_choosed(i,4)};                 % 第三列为剩余距离if ((c-max(Son_New_path{i,2}))/d)-Path_choosed(i,4) >=0Son_New_path(i,4) = {1};elseSon_New_path(i,4) = {0};endendMother_New_path(end+1:end+size(Son_New_path,1),:) = Son_New_path;end[~,index] = sort(cell2mat(Mother_New_path(:,3)));if size(index,1)>100New_path = Mother_New_path(index(1:50,:),:);elseNew_path = Mother_New_path(index(1:ceil(0.5*size(index,1))),:);end
end
New_path = New_path(cell2mat(New_path(:,4))==1,1); % 提取所有的可行路径
for i = 1:size(New_path,1)New_path{i,1}(end+1,1) = data1(end,1);         % 加上终点New_path{i,2} = size(New_path{i,1},1);         % 统计点数Length = 0;for j = 1:size(New_path{i,1},1)-1Length = Length+Length_matrix(New_path{i,1}(j+1,1),New_path{i,1}(j,1));endNew_path{i,3} = Length;                        % 统计距离
end
[~,b] = sort(cell2mat(New_path(:,3)));
Result = New_path(b,:);
toc
%% c. 可视化输出
close all
show(data1,Result{1, 1});
% 1.049

2.2.2 子函数

function show(data,path)
%% 散点图
H_correct = data(data(:,5)==0,:); % 水平误差校正点
V_correct = data(data(:,5)==1,:); % 垂直误差校正点start_end = data([1,end],:);       % 起点，终点坐标figure(1)
scatter3(H_correct(:,2),H_correct(:,3),H_correct(:,4),200,'.','r')     % H_correct
hold on
scatter3(V_correct(:,2),V_correct(:,3),V_correct(:,4),200,'.','b')     % V_correctscatter3(start_end(1,2),start_end(1,3),start_end(1,4),300,'p','g')     % start point
scatter3(start_end(2,2),start_end(2,3),start_end(2,4),300,'p','m')     % end point
%% 路线图
path1data = data(path,:);
for i = 2:size(path1data,1)if i>1plot3(path1data(i-1:i,2),path1data(i-1:i,3),path1data(i-1:i,4),'color','k');endpause(0.05);
end
legend('水平误差校正点','垂直误差校正点','起点','终点','航迹');
hold off

2.2.3 输出

Path0 : [ 1 504 70 238 156 339 458 556 437 613 ] | 10次校正（包含起始点） | 路程长度：10489.84

3. 精确枚举寻找最优解

3.1 算法步骤

输入贪心算法得到的解
对所有的路径自起点开始逐步枚举
对路径进行精确删减（如果已经行驶的路程+当前位置到终点的距离>贪心算法得到路程长度，则进行删减）
保留所有寻找到的解，进行pareto排序，取第一前沿为输出

3.2 源码

3.2.1 主程序

% 2019年第十六届中国研究生数学建模竞赛F题
% 多约束条件下智能飞行器航迹快速规划
% -- 精确穷举
% 目录
% a. 预处理
% b. 主循环
%     b.1 寻找下一步的可行点
% c. 可视化输出%% a. 预处理
clear,clc
load data1.mat
% 第一列 校正点的序号
% 第二至四列 x,y,z坐标
% 第五列 校正点的类型（0表示水平误差校正点，1表示垂直误差校正点，2表示起点，3表示终点）data1(:,1) = data1(:,1)+1;         % 编号从0开始，更改为从1开始% data1(100:500,:) = [];
% data1(:,1) = (1:size(data1,1));a1 = 25;
a2 = 15;
b1 = 20;
b2 = 25;
c = 30;
d = 0.001;max_length = pdist2(data1(1,2:4),data1(end,2:4));
max_error = 1.049*100000 - max_length;vector_start_goal = (data1(end,2:4)-data1(1,2:4));Goal_Coordinate = data1(end,2:4);  % 终点坐标max_Length = c/0.001;              % 正常行驶的最大距离% 各点之间的代价矩阵
tic
Cost_matrix = zeros(size(data1,1),size(data1,1));
Length_matrix =  zeros(size(data1,1),size(data1,1));
for i = 1:size(data1,1)for j = 1:size(data1,1)Length_matrix(i,j) = sqrt(sum((data1(i,2:4)-data1(j,2:4)).^2));Cost_matrix(i,j) = Length_matrix(i,j);if Length_matrix(i,j) >= max_LengthCost_matrix(i,j) = inf;elseCost_matrix(i,j) = Cost_matrix(i,j)*d;endend
end
toc%% b. 主循环
tic
New_path = [{1},{[0,0]},{inf},{0}];
sss = cell(1,4);
while ~isempty(New_path)Mother_New_path = {};for j = 1:size(New_path,1)
%% b.1 寻找下一步的可行点% 当前点信息num = New_path{j,1}(end,1);                 % 当前点编号Coordinate = data1(num,2:4);                % 当前点的坐标Vertical_error = New_path{j,2}(1,1);        % 当前点的垂直误差     Horizontal_error = New_path{j,2}(1,2);      % 当前点的水平误差if size(New_path{j,1},1) > 1Length = 0;for ss = 1:size(New_path{j,1},1)-1Length = Length+Length_matrix(New_path{j,1}(ss+1,1),New_path{j,1}(ss,1));endLength_AC = Length;                        % 统计距离elseLength_AC = 0;end% 寻找下一步可行点Path_Vertical = [];Path_Horizontal = [];for i = 2:size(data1,1)if isempty(find(New_path{j,1}==i, 1)) && Cost_matrix(num,i) ~= inf % 如果和之前的路径不重复，并在最大距离以内% 分以下三种情况：% 可以走当前路径，并能够进行垂直校正，保存路径% 可以走当前路径，并能够进行水平校正，保存路径% 附：如果三种情况都不存在，既是没有可行的下一步节点   if (Cost_matrix(num,i)+Vertical_error <= a1) && (Cost_matrix(num,i)+Horizontal_error <= a2) && data1(i,5) == 1 Path_Vertical(end+1,:) = [i,0,Cost_matrix(num,i)+Horizontal_error]; elseif (Cost_matrix(num,i)+Vertical_error <= b1) && (Cost_matrix(num,i)+Horizontal_error <= b2) && data1(i,5) == 0 Path_Horizontal(end+1,:) = [i,Cost_matrix(num,i)+Vertical_error,0]; endendend% 所有的待选择路径 Path_selecting% 第一列是节点序号% 第二列是垂直误差% 第三列是水平误差% 第四列是到终点的距离Path_selecting = [Path_Vertical;Path_Horizontal]; if isempty(Path_selecting)continueendfor i = 1:size(Path_selecting,1)Path_selecting(i,4) = Length_matrix(data1(end,1),data1(Path_selecting(i,1),1)); % 到终点的距离Length_AC1 = Length_AC + Length_matrix(num,data1(Path_selecting(i,1),1));Path_selecting(i,5) = Length_AC1 + Path_selecting(i,4) - max_length - max_error;endif isempty(find(Path_selecting(:,5)<=0, 1))continueelsePath_selecting(Path_selecting(:,5)>0,:) = [];endPath_choosed = Path_selecting(:,1:4);Son_New_path = cell(size(Path_choosed,1),2);for i = 1:size(Path_choosed,1)Son_New_path(i,1) = {[New_path{j,1};Path_choosed(i,1)]}; % 第一列为当前路径（使用点的编号表示）Son_New_path(i,2) = {Path_choosed(i,2:3)};               % 第二列为当前路径的垂直误差和水平误差Son_New_path(i,3) = {Path_choosed(i,4)};                 % 第三列为剩余距离if ((c-max(Son_New_path{i,2}))/d)-Path_choosed(i,4) >=0Son_New_path(i,4) = {1};elseSon_New_path(i,4) = {0};endendMother_New_path(end+1:end+size(Son_New_path,1),:) = Son_New_path;endif ~isempty(Mother_New_path)sss = [sss;Mother_New_path(cell2mat(Mother_New_path(:,4))==1,:)] ;Mother_New_path(cell2mat(Mother_New_path(:,4))==1,:) = [];New_path = Mother_New_path;elsebreakend
end
New_path = sss(2:end,:);
for i = 1:size(New_path,1)New_path{i,1}(end+1,1) = data1(end,1);         % 加上终点New_path{i,2} = size(New_path{i,1},1);         % 统计点数Length = 0;for j = 1:size(New_path{i,1},1)-1Length = Length+Length_matrix(New_path{i,1}(j+1,1),New_path{i,1}(j,1));endNew_path{i,3} = Length;                        % 统计距离
end
[~,b] = sort(cell2mat(New_path(:,3)));
Result = New_path(b,:);
toc%% c. 可视化输出
close all
[sorted_based_on_front] = Pareto_front_rank([ cell2mat(Result(:,2)),cell2mat(Result(:,3))/10000],2);
for i = 1:size(sorted_based_on_front,1)if sorted_based_on_front(i,3) == 1show(data1,Result{sorted_based_on_front(i,4), 1})Result{sorted_based_on_front(i,4),1}close allelsebreakend
end

3.2.2 子函数

function [sorted_based_on_front]=Pareto_front_rank(x,n_obj)
% Input  x      需要pareto前沿排序的点集合，一行代表一个点，一列代表点的一个维
%        n_obj  表示点坐标的维度，也即是目标的个数
% Output sorted_based_on_front      返回x的第n_obj+1列就是pareto前沿排序的序号
% sorted_based_on_front 倒数第二列是pareto前沿排序，倒数第一列是原索引N=size(x,1);%计算要排序点的个数
front = 1;
F(front).f = [];
individual = [];
for i = 1 : N% Number of individuals that dominate this individualindividual(i).n = 0;% Individuals which this individual dominateindividual(i).p = [];for j = 1 : Ndom_less = 0;dom_equal = 0;dom_more = 0;for k = 1 : n_objif (x(i,k) < x(j,k))dom_less = dom_less + 1;elseif (x(i,k) == x(j,k))dom_equal = dom_equal + 1;elsedom_more = dom_more + 1;endendif dom_less == 0 && dom_equal ~= n_objindividual(i).n = individual(i).n + 1;elseif dom_more == 0 && dom_equal ~= n_objindividual(i).p = [individual(i).p j];endend   if individual(i).n == 0x(i,n_obj + 1) = 1;F(front).f = [F(front).f i];end
end
% Find the subsequent fronts
while ~isempty(F(front).f)Q = [];for i = 1 : length(F(front).f)if ~isempty(individual(F(front).f(i)).p)for j = 1 : length(individual(F(front).f(i)).p)individual(individual(F(front).f(i)).p(j)).n = ...individual(individual(F(front).f(i)).p(j)).n - 1;if individual(individual(F(front).f(i)).p(j)).n == 0x(individual(F(front).f(i)).p(j),n_obj  + 1) = ...front + 1;Q = [Q individual(F(front).f(i)).p(j)];endendendendfront =  front + 1;F(front).f = Q;
endx(:,end+1)=(1:size(x,1))';
sorted_based_on_front=sortrows(x,n_obj+1);
% [temp,index_of_fronts] = sort(x(:,n_obj  + 1));
% for i = 1 : length(index_of_fronts)
%     sorted_based_on_front(i,:) = x(index_of_fronts(i),:);
% endend

3.2.3 输出

Path1：[ 1 504 201 81 238 171 279 370 215 398 613 ] | 11个校正点 | 路程：10351.69
Path2：[ 1 504 295 92 608 541 251 341 278 613 ] | 10个校正点 | 路程：10486.11

2019年第十六届中国研究生数学建模竞赛F题·一种快速找到最优解的算法（提供Matlab源码）相关推荐

【题目解析】2019年第十六届中国研究生数学建模竞赛B题 --天文导航中的星图识别+
2019年第十六届中国研究生数学建模竞赛B题天文导航中的星图识别天文导航(Celestial Navigation)是基于天体已知的坐标位置和运动规律,应用观测天体的天文坐标值来确定航行体的空间位 ...
2019年第十六届中国研究生数学建模竞赛E题全球变暖？【分享交流】
2019年第十六届中国研究生数学建模竞赛E题全球变暖? 全球气候变暖的解释是由于温室效应不断积累所致[1,2].事实上,由于人们焚烧化石燃料,如石油.煤炭等,或砍伐森林并将其焚烧时会产生大量的二氧化 ...
2019年第十六届中国研究生数学建模竞赛A题试题
A题无线智能传播模型 1 无线信道建模背景随着5G NR技术的发展,5G在全球范围内的应用也在不断地扩大.运营商在部署5G网络的过程中,需要合理地选择覆盖区域内的基站站址,进而通过部署基站来满足用 ...
电子科大计算机学院郑欣,我校研究生在“华为杯”第十六届中国研究生数学建模竞赛中获佳绩...
近日,"华为杯"第十六届中国研究生数学建模竞赛颁奖大会在福州大学举行.电子科技大学参赛队伍斩获全国一等奖1项.二等奖14项.三等奖12项,获奖数量再创新高.本次赛事共有全国486个 ...
“华为杯”第十五届中国研究生数学建模竞赛-对恐怖袭击事件记录数据的量化分析（Python,Pandas,Scikit-learn,PyTorch，Matplotlib,seaborn）
首先先说一下编程的工具 Python:编程语言 Pandas:数据处理,清洗,分析的工具 Scikit-learn:机器学习工具箱 PyTorch:深度学习搭建神经网络,训练等的工具 Matplotl ...
2020年“华为杯”第十七届中国研究生数学建模竞赛B题心得（满纸荒唐言，一把辛酸泪）
满纸荒唐言,一把辛酸泪. 都云作者痴,谁解其中味? --纪念2016-2020所有的数学建模论文古人说得好:"书到用时方恨少,事非经过不知难".做数学建模的我:"方法用 ...
2020年第十七届中国研究生数学建模竞赛B题 -解题思路
题目B:汽油辛烷值建模随着当今计算机与各类程序软件的开发使用,化学计量学不断发展,人们可以在近红外光谱区内采集大量的数据,并使用各种有效的统计方法,把近红外光谱技术应用于定性与定量.近红外光谱为分子 ...
2022年中国研究生数学建模竞赛F题-COVID-19疫情期间生活物资的科学管理问题
一.背景介绍进入2022年以来全国范围内陆续出现了多次较大规模疫情爆发事件[1-2].在大规模疫情爆发期间由于我国采用封闭式管理方式来实现疫情的快速清零,从而疫情期间各类人群的科学管理变得尤为重要[ ...
2022年中国研究生数学建模竞赛F题思路及参考代码-COVID-19疫情期间生活物资的科学管理问题
一.背景介绍进入2022年以来全国范围内陆续出现了多次较大规模疫情爆发事件[1-2].在大规模疫情爆发期间由于我国采用封闭式管理方式来实现疫情的快速清零,从而疫情期间各类人群的科学管理变得尤为重要[ ...

2019年第十六届中国研究生数学建模竞赛F题·一种快速找到最优解的算法（提供Matlab源码）

目录