【路径规划】基于matlab人工势场算法多机器人协同编队避障路径规划【含Matlab源码 1192期】

一、简介

基于matlab人工势场算法多机器人协同编队避障路径规划

二、源代码

function [ output_args ] = formation_avoidance3( input_args )%% 初始化 位置pose、速度V、加速度控制量controlinit_f=[-3 -6 0; %%%[x y th]-5 6 0;2 4 pi/4; 5 -3 -pi/4;3 0 pi/2];           pose_x=init_f(:,1);pose_y=init_f(:,2);pose_th=init_f(:,3);%% follower相对leader的位置delta_x=[-2 -6 -2 -6 0];   % 相对间隔误差   delta_y=[4 4 -4 -4 0];  %领航者与自己无误差fol_num=4;        N=5;             % 4follower and 1 leadercountmax=2000;dt=0.1;gama=3;di=0.02; %%%重心偏移 单位mK0=0;%%% 直线K1 K2都设在0.2左右 k3=0%%% 圆不考虑K2=0 K1，K3设为0.2左右K1=1;%%%位置偏差线速度调节K2=1;%%位置偏差角速度调节K3=0.01;%%朝向偏差角速度调节%     %% 通信拓扑图:1-4行为follower 最后一行为leaderA=[0 1 1 1 1;     % a(ij)0 0 0 0 1;0 0 0 1 1;0 0 1 0 1;0 0 0 0 0];
%    %% 通信拓扑图:1-4行为follower 最后一行为leader
%     A=[0 0 0 0 1;     % a(ij)
%        0 0 0 0 1;
%        0 0 0 0 1;
%        0 0 0 0 1;
%        0 0 0 0 0];A=[0 1 1 1 3;     % a(ij)0 0 0 0 3;0 0 0 1 3;0 0 1 0 3;0 0 0 0 0];linear_v(:,1)=[0;0;0;0;1];angular_w(:,1)=[0;0;0;0;1];k=0;% 最高速度m/s],最高旋转速度[rad/s],加速度[m/ss],旋转加速度[rad/ss]]Kinematic=[1.5,toRadian(60.0),0.5,toRadian(50.0)];%% 运动限制error_temp(1:fol_num,1:4)=0;%%行为follower个数、列为两个时刻的x,y偏差in_degree=sum(A,2);%% 开始循环 走顺时针圆周
%     figure;for count=1:countmaxk=k+1;linear_v(N,k+1)=linear_v(N,k);%领航者速度不变angular_w(N,k+1)=angular_w(N,k);
%         if count==500
%             linear_v(N,k+1)=0.5;%领航者速度不变
%              angular_w(N,k+1)=0.5;
%         end
%         if count==1000
%             linear_v(N,k+1)=0.5;%领航者速度不变
%              angular_w(N,k+1)=0;
%         endfor i=1:fol_numsum_delta_x=0;sum_delta_y=0;for j=1:N %%考虑邻居对它的影响if k==1temp_x=0;temp_y=0;elsetemp_x=(pose_x(j,k)-pose_x(j,k-1))/dt;temp_y=(pose_y(j,k)-pose_y(j,k-1))/dt;end   sum_delta_x=sum_delta_x+A(i,j)*(temp_x+gama*((pose_x(j,k)-pose_x(i,k))-(delta_x(j)-delta_x(i))));sum_delta_y=sum_delta_y+A(i,j)*(temp_y+gama*((pose_y(j,k)-pose_y(i,k))-(delta_y(j)-delta_y(i))));endsum_delta_x=sum_delta_x/in_degree(i);sum_delta_y=sum_delta_y/in_degree(i);linear_v(i,k+1)=cos(pose_th(i,k))*sum_delta_x+sin(pose_th(i,k))*sum_delta_y;angular_w(i,k+1)=(-sin(pose_th(i,k))*sum_delta_x+cos(pose_th(i,k))*sum_delta_y)/di;u_old=[linear_v(i,k);angular_w(i,k)];u=[linear_v(i,k+1);angular_w(i,k+1)];%%%加入速度限制u=limit(u_old,u,Kinematic);old_position=[pose_x(i,k);pose_y(i,k);pose_th(i,k)];new_position=motion(old_position,u,dt);pose_x(i,k+1)=new_position(1)-di*cos(new_position(3));pose_y(i,k+1)=new_position(2)-di*sin(new_position(3));pose_th(i,k+1)=new_position(3);end%% 更新领航者old_position=[pose_x(N,k);pose_y(N,k);pose_th(N,k)];u=[linear_v(N,k+1);angular_w(N,k+1)];new_position=motion(old_position,u,dt);pose_x(N,k+1)=new_position(1);pose_y(N,k+1)=new_position(2);pose_th(N,k+1)=new_position(3);tt_x(1:4,k)=pose_x(5,k);error_x(:,k)=tt_x(1:4,k)-pose_x(1:4,k)+(delta_x(1:4))';tt_y(1:4,k)=pose_y(5,k);error_y(:,k)=tt_y(1:4,k)-pose_y(1:4,k)+(delta_y(1:4))';function interpoint( x1,y1,x2,y2,x3,y3,colo,lstyle)
%UNTITLED2 此处显示有关此函数的摘要
%   此处显示详细说明
if (nargin==6)colo='k';lstyle=':';
end
syms k b m n x y;
if(x1==x2)%x1x2直线斜率不存在solx=x1;soly=y3;
elseif(y1==y2)%x1x2直线斜率为0solx=x3;soly=y1;
else solk=(y2-y1)/(x2-x1);solb=y2-solk*x2;solk1=-1/solk;solb1=y3-solk1*x3;solx=(solb1-solb)/(solk-solk1);soly=solk*solx+solb;
%     [solx,soly] = solve(solk1*x-y+solb1==0,solk*x-y+solb==0,x,y);
end
line([x1,solx],[y1,soly],'color',colo,'linestyle',lstyle);
line([x3,solx],[y3,soly],'color',colo,'linestyle',lstyle);
end

三、运行结果

四、备注

版本：2014a

【路径规划】基于matlab人工势场算法多机器人协同编队避障路径规划【含Matlab源码 1192期】相关推荐

【APF三维路径规划】基于matlab人工势场算法无人机三维路径规划【含Matlab源码 168期】
一.获取代码方式获取代码方式1: 通过订阅紫极神光博客付费专栏,凭支付凭证,私信博主,可获得此代码. 获取代码方式2: 完整代码已上传我的资源:[三维路径规划]基于matlab人工势场算法无人机三维 ...
【Matlab人脸识别】形态学教室人数统计（带面板）【含GUI源码 1703期】
一.代码运行视频(哔哩哔哩) [Matlab人脸识别]形态学教室人数统计(带面板)[含GUI源码 1703期] 二.matlab版本及参考文献 1 matlab版本 2014a 2 参考文献 [1]孟 ...
【Matlab语音加密】语音信号加密解密（带面板）【含GUI源码 181期】
一.代码运行视频(哔哩哔哩) [Matlab语音加密]语音信号加密解密(带面板)[含GUI源码 181期] 二.matlab版本及参考文献 1 matlab版本 2014a 2 参考文献 [1]韩纪庆 ...
matlab人工势场法三维演示图,运动规划入门 | 5. 白话人工势场法，从原理到Matlab实现...
如何利用人工势场进行运动规划? 1.1 引力势场(Attractive Potential Field) 人工势场这个特殊的势场并不是一个单一的场,其实它是由两个场叠加组合而成的,一个是引力场,一个是 ...
人工势场算法 Matlab版源码
人工势场算法,用于路径规划 main.m程序 %初始化车的参数 Xo=[0 0];%起点位置 k=15;%计算引力需要的增益系数 K=0;%初始化 m=5;%计算斥力的增益系数,都是自己设定的. Po ...
【Matlab路径规划】改进的遗传算法机器人避障路径规划【含GUI源码 703期】
一.代码运行视频(哔哩哔哩) [Matlab路径规划]改进的遗传算法机器人避障路径规划[含GUI源码 703期] 二.matlab版本及参考文献 1 matlab版本 2014a 2 参考文献 [1] ...
【Matlab图像加密】正交拉丁方置乱算法图像加解密【含GUI源码 182期】
一.代码运行视频(哔哩哔哩) [Matlab图像加密]正交拉丁方置乱算法图像加解密[含GUI源码 182期] 二.matlab版本及参考文献一.代码运行视频(哔哩哔哩) [Matlab图像处理]自动 ...
【Matlab验证码识别】遗传算法和最大熵优化+大津法（OTSU）+自定义阈值数字验证码识别【含GUI源码 1694期】
一.代码运行视频(哔哩哔哩) [Matlab验证码识别]遗传算法和最大熵优化+大津法(OTSU)+自定义阈值数字验证码识别[含GUI源码 1694期] 二.matlab版本及参考文献 1 matlab ...
【Matlab人脸识别】BP神经网络人脸识别（含识别率）【含GUI源码 891期】
一.代码运行视频(哔哩哔哩) [Matlab人脸识别]BP神经网络人脸识别(含识别率)[含GUI源码 891期] 二.matlab版本及参考文献 1 matlab版本 2014a 2 参考文献 [1] ...
【Matlab人脸识别】人脸实时检测与跟踪【含GUI源码 673期】
一.代码运行视频(哔哩哔哩) [Matlab人脸识别]人脸实时检测与跟踪[含GUI源码 673期] 二.matlab版本及参考文献 1 matlab版本 2014a 2 参考文献 [1]孟逸凡,柳益君 ...

【路径规划】基于matlab人工势场算法多机器人协同编队避障路径规划【含Matlab源码 1192期】

一、简介

二、源代码

三、运行结果

四、备注

【路径规划】基于matlab人工势场算法多机器人协同编队避障路径规划【含Matlab源码 1192期】相关推荐

最新文章

热门文章