✅博主简介：热爱科研的Matlab仿真开发者，修心和技术同步精进，Matlab项目合作可私信。
🍎个人主页：海神之光
🏆代码获取方式：
海神之光Matlab王者学习之路—代码获取方式
⛳️座右铭：行百里者，半于九十。
更多Matlab仿真内容点击👇
Matlab图像处理（进阶版）
路径规划（Matlab）
神经网络预测与分类（Matlab）
优化求解（Matlab）
语音处理（Matlab）
信号处理（Matlab）
车间调度（Matlab）
⛄一、无人机简介
0 引言
随着现代技术的发展，飞行器种类不断变多，应用也日趋专一化、完善化，如专门用作植保的大疆PS-X625无人机，用作街景拍摄与监控巡察的宝鸡行翼航空科技的X8无人机，以及用作水下救援的白鲨MIX水下无人机等，决定飞行器性能主要是内部的飞控系统和外部的路径规划问题。就路径问题而言，在具体实施任务时仅靠操作员手中的遥控器控制无人飞行器执行相应的工作，可能会对操作员心理以及技术提出极高的要求，为了避免个人操作失误，进而造成飞行器损坏的危险，一种解决问题的方法就是对飞行器进行航迹规划。
飞行器的测量精度，航迹路径的合理规划，飞行器工作时的稳定性、安全性等这些变化对飞行器的综合控制系统要求越来越高。无人机航路规划是为了保证无人机完成特定的飞行任务，并且能够在完成任务的过程中躲避各种障碍、威胁区域而设计出最优航迹路线的问题。
1 常见的航迹规划算法
图1 常见路径规划算法
文中主要对无人机巡航阶段的航迹规划进行研究，假设无人机在飞行中维持高度与速度不变，那么航迹规划成为一个二维平面的规划问题。在航迹规划算法中,A算法计算简单，容易实现。在改进A算法基础上，提出一种新的、易于理解的改进A算法的无人机航迹规划方法。传统A算法将规划区域栅格化，节点扩展只限于栅格线的交叉点，在栅格线的交叉点与交叉点之间往往存在一定角度的两个运动方向。将存在角度的两段路径无限放大、细化，然后分别用两段上的相应路径规划点作为切点，找到相对应的组成内切圆的圆心，然后作弧，并求出相对应的两切点之间的弧所对应的圆心角，根据下式计算出弧线的长度
式中：R———内切圆的半径；
α———切点之间弧线对应的圆心角。
⛄三、人工势场算法简介
人工势场算法之所以无法适应复杂的环境, 是因为斥力场系数、引力场系数在计算时是固定不变的。
1 人工势场法基本原理
人工势场法的实质是对无人机的飞行区域人为地定义势场[17], 该势场为地图中出现的障碍物斥力场和终点引力场的向量叠加。传统人工势场的定义如下:
假设无人机的位置为X= (x, y, z) , 则目标点引力场与无人机电子之间的电势场为
式中:Katt为势场增益系数;XG为目标点的位置。
定义Fatt (X) 为引力势场的引力,
定义障碍物的斥力势场为
定义障碍物的斥力为
式中:Frep为障碍物对无人机的斥力势场系数;X-X0的单位是m, 表示移动的无人机到各个障碍物的动态距离;ρ0为障碍物的影响距离。所以移动的无人机像一个电子一样, 它的总势场为无人机与障碍物各个势场的和:
对无人机的作用力Ftotal为
由式 (6) 可以算出无人机的下一步运动轨迹。
虽然人工势场法有很多优点, 但是在实际飞行中, 环境比较复杂的时候, 经常出现障碍物在目标位置附近的情况, 当无人机向目标点飞行时, Fatt减小Frep增大, 此时会出现无人机在终点区域拐弯的情况;当无人机处在障碍物运动时, 可能出现无人机处在合力为零点的情况, 因而无人机不能到达目标点。
⛄三、部分源代码
%% 清空变量
clear
clc
%% 初始化参数
startPoint = [-650 -100 -300];
goalPoint = [350 350 600];
thr = 5;
k = 0.000001; %引力增益系数
m = 2000000; %斥力增益系数
r = 100; %障碍物影响距离
iterStep = 1;
iter = 1;
iterMax = 5000;
objMat = [-470 -20 -144;-300 50 160
195 310 445;0 200 200];
objNumber = size(objMat,1);
objRadius = 80;
positionMat = zeros(3,iterMax);
gravitationForce = zeros(3,1);
repulsiveForce = zeros(3,1);
sumForce = zeros(3,1);
angleGoal = zeros(3,1);
angleObj = zeros(3,objNumber);
positionNow = startPoint;
positionMat(:,1) = positionNow;
%% 迭代循环，寻找路径
while iter < iterMax
%% 计算当前位置和目标位置以及障碍物的比例信息
[angleObj,angleGoal] = calcuAngle(objNumber,objMat,positionNow,angleObj,goalPoint);
%% 计算引力
gravitationForce = calcuGravitationForce(positionNow,goalPoint,gravitationForce,k,angleGoal);
GF(:,iter) = gravitationForce;
%% 计算斥力
repulsiveForce = repulsiveForce*0;
repulsiveForce = calcuRepulsiveForce(repulsiveForce,objNumber,positionNow,objMat,angleObj,r,m);
RF(:,iter) = repulsiveForce;
%% 计算合力
sumForce = repulsiveForce+gravitationForce;
%% 计算下一步位置
positionNow = positionNow+iterStep*sumForce'./(sum(abs(sumForce)));
iter = iter+1;
positionMat(:,iter) = positionNow;
%% 判断当前位置与目标点位置是否小于阈值
if sqrt((positionNow(1)-goalPoint(1))^2+(positionNow(2)-goalPoint(2))^2+(positionNow(3)-goalPoint(3))^2)<thr
break;
end
end
%% 画障碍物
drawObject(objRadius,objNumber,objMat)
%% 画路径图
positionMat(:,iter+1:end) = [];
line(positionMat(1,:),positionMat(2,:),positionMat(3,:),‘Color’,‘r’,‘LineWidth’,2)
plot3(startPoint(1),startPoint(2),startPoint(3),‘bo’,‘MarkerSize’,4)
plot3(goalPoint(1),goalPoint(2),goalPoint(3),‘bo’,‘MarkerSize’,4)
text(startPoint(1),startPoint(2),startPoint(3),‘起点’);
text(goalPoint(1),goalPoint(2),goalPoint(3),‘终点’)
xlabel(‘x’)
ylabel(‘y’)
zlabel(‘z’)
position = positionMat(:,1:20:end);
jointAngle = zeros(size(position,2),6);
%% 配置机械臂参数
Theta = [0 0 90 0 0 -90];
Arm_1.alpha = 90*pi/180;
Arm_1.a = 0;
Arm_1.d = 89.459;
Arm_1.theta = Theta(1)*pi/180;
Arm_2.alpha = 0*pi/180;
Arm_2.a = -425;
Arm_2.d = 0;
Arm_2.theta = Theta(2)*pi/180;
Arm_3.alpha = 0*pi/180;
Arm_3.a = -392.25;
Arm_3.d = 0;
Arm_3.theta = Theta(3)*pi/180;
Arm_4.alpha = 90*pi/180;
Arm_4.a = 0;
Arm_4.d = 109.15;
Arm_4.theta = Theta(4)*pi/180;
Arm_5.alpha = -90*pi/180;
Arm_5.a = 0;
Arm_5.d = 94.65;
Arm_5.theta = Theta(5)*pi/180;
Arm_6.alpha = 0*pi/180;
Arm_6.a = 0;
Arm_6.d = 82.3;
Arm_6.theta = Theta(6)*pi/180;
% T = [nx ox ax px;
% ny oy ay py;
% nz oz az pz;
% 0 0 0 1 ];
%% 建立机械臂模型
% theta d a alpha offset
L(1)=Link([0 Arm_1.d Arm_1.a Arm_1.alpha ]); %定义连杆的D-H参数
L(2)=Link([0 Arm_2.d Arm_2.a Arm_2.alpha ]);
L(3)=Link([0 Arm_3.d Arm_3.a Arm_3.alpha ]);
L(4)=Link([0 Arm_4.d Arm_4.a Arm_4.alpha ]);
L(5)=Link([0 Arm_5.d Arm_5.a Arm_5.alpha ]);
L(6)=Link([0 Arm_6.d Arm_6.a Arm_6.alpha ]);
robot = SerialLink(L,‘name’,‘myRobot’);
T = zhengyundongx(Arm_1,Arm_2,Arm_3,Arm_4,Arm_5,Arm_6);
for k1 = 1:size(position,2)
T(1,4) = position(1,k1);
T(2,4) = position(2,k1);
T(3,4) = position(3,k1);
inverseEightAngle = niyundongx(Arm_1,Arm_2,Arm_3,Arm_4,Arm_5,Arm_6,T);
if k1 == 1
jointAngle(k1,:) = inverseEightAngle(1,:);
else
stepPreJoint = zhengyundongx2(Arm_1,Arm_2,Arm_3,Arm_4,Arm_5,Arm_6,jointAngle(k1-1,:));
sumJoint = inf;
indexJoint = -1;
for k2 = 1:size(inverseEightAngle,1)
stepNextJoint = zhengyundongx2(Arm_1,Arm_2,Arm_3,Arm_4,Arm_5,Arm_6,inverseEightAngle(k2,:));
if sumJoint>sum(sum(stepNextJoint-stepPreJoint))
indexJoint = k2;
sumJoint = sum(sum(stepNextJoint-stepPreJoint));
end
end
jointAngle(k1,:) = inverseEightAngle(indexJoint,:);
end
end
⛄四、运行结果
⛄五、matlab版本及参考文献
1 matlab版本
2014a
2 参考文献
[1]焦阳.基于改进蚁群算法的无人机三维路径规划研究[J].舰船电子工程. 2019,39(03)
3 备注
简介此部分摘自互联网，仅供参考，若侵权，联系删除
🍅 仿真咨询
1 各类智能优化算法改进及应用
生产调度、经济调度、装配线调度、充电优化、车间调度、发车优化、水库调度、三维装箱、物流选址、货位优化、公交排班优化、充电桩布局优化、车间布局优化、集装箱船配载优化、水泵组合优化、解医疗资源分配优化、设施布局优化、可视域基站和无人机选址优化
2 机器学习和深度学习方面
卷积神经网络（CNN）、LSTM、支持向量机（SVM）、最小二乘支持向量机（LSSVM）、极限学习机（ELM）、核极限学习机（KELM）、BP、RBF、宽度学习、DBN、RF、RBF、DELM、XGBOOST、TCN实现风电预测、光伏预测、电池寿命预测、辐射源识别、交通流预测、负荷预测、股价预测、PM2.5浓度预测、电池健康状态预测、水体光学参数反演、NLOS信号识别、地铁停车精准预测、变压器故障诊断
3 图像处理方面
图像识别、图像分割、图像检测、图像隐藏、图像配准、图像拼接、图像融合、图像增强、图像压缩感知
4 路径规划方面
旅行商问题（TSP）、车辆路径问题（VRP、MVRP、CVRP、VRPTW等）、无人机三维路径规划、无人机协同、无人机编队、机器人路径规划、栅格地图路径规划、多式联运运输问题、车辆协同无人机路径规划、天线线性阵列分布优化、车间布局优化
5 无人机应用方面
无人机路径规划、无人机控制、无人机编队、无人机协同、无人机任务分配
6 无线传感器定位及布局方面
传感器部署优化、通信协议优化、路由优化、目标定位优化、Dv-Hop定位优化、Leach协议优化、WSN覆盖优化、组播优化、RSSI定位优化
7 信号处理方面
信号识别、信号加密、信号去噪、信号增强、雷达信号处理、信号水印嵌入提取、肌电信号、脑电信号、信号配时优化
8 电力系统方面
微电网优化、无功优化、配电网重构、储能配置
9 元胞自动机方面
交通流 人群疏散 病毒扩散 晶体生长
10 雷达方面
卡尔曼滤波跟踪、航迹关联、航迹融合
677
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
