常见路径规划算法代码

2023-03-12 20:15| 来源: 网络整理| 查看: 265

简述

此文并没有太多的食用价值，只是将下面这篇文章中人工势场法、A*算法、DWA(动态窗口法)、RRT star 算法这四个算法的代码复制粘贴到此文中。既方便大伙打开文章就能看代码，也能解决因为matlab版本差异导致的中文注释乱码的问题。

下面这篇文章是对这几个算法的简单演示与介绍。

此文代码下载： Poao-路径规划算法.zip - 蓝奏云

哈哈哈，终于找到机会水一篇文章咯。

重点：务必要借助目录进行跳转，不然会发疯的！！

目录中的二级小标题即为该算法下的子文件名，大伙们完全可以复制粘贴此文中的代码并创建对应的.m文件。

1 DWA(动态窗口)下图中，移动的小圆圈为机器人、蓝色的线束为预测的路径、紫色的椭圆为障碍物、红色的※标志着机器人的目标点。

DWA算法皆在“dwa.m”文件中，算法的实现流程在 DynamicWindowApproach 函数中，其具体功能实现则需主要关注Evaluation、GenerateTrajectory、CalcDynamicWindow等函数。

dwa.m% ------------------------------------------------------------------------- % File : DWA 算法 % Discription : Mobile Robot Motion Planning with Dynamic Window Approach % Author :Yuncheng Jiang % License : Modified BSD Software License Agreement % 出处：https://b23.tv/rGKUTW - b站up主【WHEELTEC】 % 源代码已经配备丰富的注释，我在其基础上添加了一些个人理解。 % —— 2021/10/30 Poaoz % ------------------------------------------------------------------------- % 流程梳理 - dwa动态窗口算法 % 1）设置初始化参数：起点、终点、障碍物、小车的速度加速度限制等 % 2）根据小车当前状态及参数，计算出小车接下来一小段时间可达到的状态（主要为速度、加速度范围） % 3）根据上述计算而得的速度、加速度，模拟出小车接下来一小段时间可达到的路径 % 4) 借助评价函数，对上述路径进行评估，并选取出最优解，然后使小车执行（执行对应的速度、角速度） % 5）再以小车新的位置及状态为基础，重复上述“2-5”，直到判断出小车到达终点。 % 闲谈：前面学习了RRT、A*、人工势能法，综合来看，这几种方法的套路是类似的。 % 相比较，DWA更加灵活，无需栅格化地图并且更贴合小车运动实际。 % 该函数相当于dwa算法的main函数，内容包括参数设定、流程的梳理、绘图。 function [] = dwa_V_1_0() close all; clear ; disp('Dynamic Window Approach sample program start!!') %% 机器人的初期状态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)] % x=[0 0 pi/2 0 0]'; % 5x1矩阵列矩阵位置 0，0 航向 pi/2 ,速度、角速度均为0 x = [0 0 pi/10 0 0]'; % 下标宏定义状态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)] POSE_X = 1; %坐标 X POSE_Y = 2; %坐标 Y YAW_ANGLE = 3; %机器人航向角 V_SPD = 4; %机器人速度 W_ANGLE_SPD = 5; %机器人角速度 goal = [10,10]; % 目标点位置 [x(m),y(m)] % 障碍物位置列表 [x(m) y(m)] obstacle=[%0 2; 3 10*rand(1); % 4 4; % 5 4; % 5 5; 6 10*rand(1); % 5 9 % 7 8 8 10*rand(1); 2 5; 4 2; 7 7; 9 9 ]; %边界障碍物，防止跑出图外 for i =-1 for j = -1:12 obstacle = [obstacle; [i,j]]; end end for i =12 for j = -1:12 obstacle = [obstacle; [i,j]]; end end for j =-2 for i = -1:12 obstacle = [obstacle; [i,j]]; end end for j=13 for i= -1:12 obstacle = [obstacle; [i,j]]; end end obstacleR = 0.5;% 冲突判定用的障碍物半径 global dt; dt = 0.1;% 时间[s] 每一条计算得到的路径，由多个点组成 dt即为每个点之间的时间间隔 % evalParam[4]/dt+1 = 每条路径的构成点数目这两个参数更改后，dwa算法的具体效果也将有所变化 % 机器人运动学模型参数 % 最高速度m/s],最高旋转速度[rad/s],加速度[m/ss],旋转加速度[rad/ss], % 速度分辨率[m/s],转速分辨率[rad/s]] Kinematic = [1.0,toRadian(20.0),0.2,toRadian(50.0),0.01,toRadian(1)]; % 调用函数里面的 model %定义Kinematic的下标含义 % Kinematic 在路径计算相关函数中，大量用到 MD_MAX_V = 1;% 最高速度m/s] MD_MAX_W = 2;% 最高旋转速度[rad/s] MD_ACC = 3;% 加速度[m/ss] MD_VW = 4;% 旋转加速度[rad/ss] MD_V_RESOLUTION = 5;% 速度分辨率[m/s] MD_W_RESOLUTION = 6;% 转速分辨率[rad/s]] % 评价函数参数 [heading,dist,velocity,predictDT] % 航向得分的比重、距离得分的比重、速度得分的比重、向前模拟轨迹的时间 evalParam = [0.045, 0.1 ,0.1, 3.0]; % evalParam = [2, 0.2 ,0.2, 3.0]; area = [-3 14 -3 14];% 模拟区域范围 [xmin xmax ymin ymax] % 模拟实验的结果 result.x=[]; %累积存储走过的轨迹点的状态值 tic; % 估算程序运行时间开始 flag_obstacle = [1-2*rand(1) 1-2*rand(1) 1-2*rand(1)]; vel_obstacle = 0.05; temp = 0; abc = 0; %movcount=0; %% Main loop 循环运行 5000次指导达到目的地或者 5000次运行结束 for i = 1:5000 % DWA参数输入返回控制量 u = [v(m/s),w(rad/s)] 和轨迹～即机器人将采用的控制参数 [u,traj] = DynamicWindowApproach(x,Kinematic,goal,evalParam,obstacle,obstacleR); % 算出下发速度u/当前速度u x = f(x,u); % 机器人移动到下一个时刻的状态量根据当前速度和角速度推导下一刻的位置和角度 abc = abc+1; % 历史轨迹的保存 result.x = [result.x; x']; %最新结果以行的形式添加到result.x，保存的是所有状态参数值，包括坐标xy、朝向、线速度、角速度，其实应该是只取坐标就OK % 是否到达目的地 if norm(x(POSE_X:POSE_Y)-goal') 10 && flag_obstacle(j) > 0 || obstacle(j,2) = 3*R %最大分数限制 dist = 3*R; end %% heading的评价函数计算 ok % 输入参数：当前位置、目标位置 % 输出参数：航向参数得分 = 180 - 偏差值 % 当前车的航向和相对于目标点的航向偏离程度越小分数越高最大180分 function heading = CalcHeadingEval(x,goal) theta = toDegree(x(3));% 机器人朝向 goalTheta = toDegree(atan2(goal(2)-x(2),goal(1)-x(1)));% 目标点相对于机器人本身的方位 % 下面的 targetTheta 也就是小车当前航向与目标点的差值（正数） if goalTheta > theta targetTheta = goalTheta-theta;% [deg] else targetTheta = theta-goalTheta;% [deg] end heading = 180 - targetTheta; %% 计算动态窗口 model - 速度加速度等基本参数。 ok % 返回最小速度最大速度最小角速度最大角速度速度 function Vr = CalcDynamicWindow(x,model) V_SPD = 4;%机器人速度 W_ANGLE_SPD = 5;%机器人角速度 MD_MAX_V = 1;% 最高速度m/s] MD_MAX_W = 2;% 最高旋转速度[rad/s] MD_ACC = 3;% 加速度[m/ss] MD_VW = 4;% 旋转加速度[rad/ss] global dt; % 车子速度的最大最小范围依次为：最小速度最大速度最小角速度最大角速度速度 Vs=[0 model(MD_MAX_V) -model(MD_MAX_W) model(MD_MAX_W)]; % 根据当前速度以及加速度限制计算的动态窗口依次为：最小速度最大速度最小角速度最大角速度速度 Vd = [x(V_SPD)-model(MD_ACC)*dt x(V_SPD)+model(MD_ACC)*dt ... x(W_ANGLE_SPD)-model(MD_VW)*dt x(W_ANGLE_SPD)+model(MD_VW)*dt]; % 最终的Dynamic Window Vtmp = [Vs;Vd]; % 2 X 4矩阵每一列依次为：最小速度最大速度最小角速度最大角速度速度 Vr = [max(Vtmp(:,1)) min(Vtmp(:,2)) max(Vtmp(:,3)) min(Vtmp(:,4))]; % 设定的参数与计算的速度比较 %% Motion Model 根据当前状态推算下一个控制周期（dt）的状态。 oh！坐标变换的计算原理？ % u = [vt; wt];当前时刻的速度、角速度 x = 状态[x(m),y(m),yaw(Rad),v(m/s),w(rad/s)] function x = f(x, u) global dt; F = [1 0 0 0 0 0 1 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0]; B = [dt*cos(x(3)) 0 dt*sin(x(3)) 0 0 dt 1 0 0 1]; x= F*x+B*u; % 为何这样计算，暂不明白 % 弧度和角度之间的换算 %% degree to radian function radian = toRadian(degree) radian = degree/180*pi; %% radian to degree function degree = toDegree(radian) degree = radian/pi*180; %% END2 人工势场法

如图所示，绿线为机器人行驶的路径，红色椭圆为障碍物，机器人从左下角的起点运动到了右上角的终点。

该matlab程序中的“main.m” 为主程序，而“path_plan.m”则为算法的具体实现程序。

2.1 main.m% ------------------------------------------------------------------------- % 人工势场算法 % 出处：GitHub-https://github.com/zzuwz/Artificial-Potential-Field % 参考提供源代码作者提供的资料，依照个人理解，添加了大量中文注释 % —— 2021/10/29 Poaoz % ------------------------------------------------------------------------- % 流程梳理 - 人工势场法 % 1）起点、终点、障碍物、迭代次数、取点半径等参数的设定 % 2）以起点为中心，作半径为r的圆，从圆上取八个均布的点 % 3）分别计算八个点的前进“代价”—— 终点对其的引力+所有障碍物对其的斥力 % 4）取“代价”最小的点的坐标，结合现有起点，计算得到新的起点，然后重复上述内容 % 5）当发现一个点距离终点很近 or 迭代的次数计算完程序停止。 %% clc clear close all figure(1); axis([0 15 0 15]); % 地图 15x15 begin=[1;2]; % 起点 over=[14;13]; % 终点 obstacle=[5 8 10 7 10 5 ;5 6 9 9 13 10]; % 障碍物x;y坐标 % 绘制起点、终点、障碍物 hold on; plot(begin(1),begin(2),'*b','MarkerSize',10); plot(over(1),over(2),'*b','MarkerSize',10); plot(obstacle(1,:),obstacle(2,:),'ob'); for i=1:size(obstacle,2) % 在个障碍物点处，绘制椭圆。 'Curvature' 矩形的曲率 rectangle('Position',[obstacle(1,i)-0.5,obstacle(2,i)-0.5,1,1],'Curvature',[1,1],'FaceColor','r'); end point= path_plan(begin,over,obstacle); % 计算并绘制出路径2.2 path_plan.m% 算法梳理 % 1）起点、终点、障碍物、迭代次数、取点半径等的设定 % 2）以起点为中心，作半径为r的圆，均匀从圆上取八个点 % 3）分别计算八个点的前进“代价” 即终点对其的引力+所有障碍物对其的斥力 % 4）取“代价”最小的点的坐标，结合现有起点，计算得到新的起点，然后重复上述内容 % 5）当发现一个点距离终点很近 or 迭代的次数计算完了程序停止。 % 该程序中，computP 负责代价计算，为核心计算函数。可对其进行修改，以实现其他优化功能。 % 参数：起点终点障碍物的坐标 % 返回值： point储存的一系列起点信息 function [ point ] = path_plan(begin,over,obstacle) iters=1; % 迭代次数 curr=begin; % 起点坐标 testR=0.2; % 测试8点的圆的半径为0.5 while (norm(curr-over)>0.2) && (iters

RRT算法文件夹也有两个m文件，其中"rrt_run.m"为主程序，其主要进行了相关参数及地图的初始化，具体的算法实现在“PlanPathRRTstar.m”文件当中。

3.1 rrt_run.m% ------------------------------------------------------------------------- % RRT* 算法 % 出处：GitHub-https://github.com/wntun/RRT-star % 参考提供源代码作者提供的资料，依照个人理解，添加了大量中文注释 % —— 2021/10/29 Poaoz % ------------------------------------------------------------------------- % 流程梳理 - RRT算法 % 1) 进行起点、终点、障碍物、前进步长、迭代次数等参数的设定，rrt树的初始化 % 2) 在地图上随机采样一个点a，并找出现有节点中与其最近的节点b % 3）沿最近的节点b到采样点a方向，根据机器人步长，求得新的节点c % 4）对新的节点c进行障碍物判断 & 找到c的父节点（最近点 or 可到达的点-eighbors） % 5）如果新节点符合要求，将其插入到rrt树中（携带四个rrt参数）并进行节点重连的路径优化计算 % 6）判断该新节点c是否“到达目标点”（按条件修改rrt的第四个参数），并持续重复上述“2-6” % 7）迭代次数达到设定值后，根据得到的rrt树，找出最佳路径 function []=rrt_run % 画矩形块：[x,y,a,b]起始点(x,y),宽a，长b function[] = rect(x,y,l,b) hold on rectangle('Position',[x,y,l,b],'FaceColor',[0 0.5 0.5]) end % 画圆的函数 function circle(x,y,r) ang=0:0.01:2*pi; xp=r*cos(ang); yp=r*sin(ang); plot(x+xp,y+yp, '.r'); end % 画图，及一些输入参数的设置 figure; axis([0,200,0,200]) set(gca, 'XTick', 0:10:200) set(gca, 'YTick', 10:10:200) grid ON hold on % 画矩形块充当障碍物 rect(130,70,20,60); % 起点(130,70)，右上角方向，宽20长60的矩形 rect(70,135,60,20); % 起点(70,135)，右上角方向，宽60长20的矩形 p_start = [30;160]; % 起点，目标点设定 p_goal = [160;80]; rob.x = 30; % 机器人所在起点坐标 rob.y = 160; % 初始化参数 param.obstacles =[130,70,20,60; 70,135,60,20;]; % 对应矩形块 param.threshold = 2; param.maxNodes = 800; param.step_size = 5; % 机器人每次行进步数 param.neighbourhood = 5; % 寻找子节点的距离 param.random_seed = 40; plot(rob.x,rob.y,'.r') text(rob.x-5, rob.y+9, 'Starting Point'); % 机器人的起点标识 % plot(p_goal(1), p_goal(2), '.r') circle(p_goal(1), p_goal(2), 2) % 目标点画半径2的圆 text(p_goal(1), p_goal(2), 'Goal'); % 目标点标注文字 % 进行搜图并出结果 result = PlanPathRRTstar(param, p_start, p_goal) end3.2 PlanPathRRTstar.mfunction result = PlanPathRRTstar(param, p_start, p_goal) % RRT* % credit : Anytime Motion Planning using the RRT*, S. Karaman, et. al. % calculates the path using RRT* algorithm % param : parameters for the problem % 1) threshold : stopping criteria (distance between goal and current % node) % 2) maxNodes : maximum nodes for rrt tree % 3) neighborhood : distance limit used in finding neighbors % 4) obstacle : must be rectangle-shaped #limitation % 5) step_size : the maximum distance that a robot can move at a time % (must be equal to neighborhood size) #limitation % 6) random_seed : to control the random number generation % p_start : [x;y] coordinates % p_goal : [x;y] coordinates % variable naming : when it comes to describe node, if the name is with % 'node', it means the coordinates of that node or it is just an index of % rrt tree % rrt struct : 1) p : coordinate, 2) iPrev : parent index, 3) cost : % distance % obstacle can only be detected at the end points but not along the line % between the points 障碍物只能在端点处检测到，而不能沿着端点之间的直线检测到 % for cost, Euclidean distance is considered. 代价计算--欧几里得距离 % output : cost, rrt, time_taken % whether goal is reached or not, depends on the minimum distance between % any node and goal % GitHub作者：GitHub-https://github.com/wntun/RRT-star % 中文简述如下，给关系程序理解的大部分程序段都添加了注释。 % 部分细节可能存在偏差，望海涵。 % -- by Poaoz 2021/10/29 % 知乎：https://www.zhihu.com/people/panda-13-16 % 初始化参数简述 param % 1) threshold : 停止搜索的距离阈值--当前节点于目标点的距离 % 2) maxNodes : RRT树的最大节点数 % 3) neighborhood : 父节点到下一个节点的步长 % 4) obstacle : 该程序的障碍物只能是矩形的，可通过修改函数【isObstacleFree(node_free)】替换其他的障碍物 % 5) step_size : 机器人一次可前进的步长，此处需与neighborhood相等 % 6) random_seed : 控制随机数生成的种子 % 代价计算说明：采用norm计算两个坐标点之间的距离-欧几里得距离 % rrt 树参数结构简述 % 1) p : 节点坐标 pose % 2) iPrev : 该节点的父节点索引（rrt树中的索引）联系了该节点与父节点 % 3) cost : 累计代价 = 父节点代价 + 父节点到该子节点的距离代价【欧几里得距离】 % 4) goalReached : 是否到达目标点标志（与设定的threshold有关） % while循环进行的内容 % 1)地图上随机采样一个点 % 2）找出现有节点中，与该采样点最近的节点 % 3）沿最近的节点到采样点方向，根据机器人步长，求得新的节点 % 4）新节点的障碍物判断 & 找到新节点的父节点（最近点 or 可到达的点neighbors_ind） % 5）将新节点插入到rrt树中（非障碍物 & 四个rrt参数）这里有节点重连的路径优化 % 6）判断该节点是否满足阈值条件 “达到目标点”-threshold （第四个rrt参数） % while循环结束后：根据得到到rrt树，计算出最终路径并绘制 % 最短路径寻找简述： % 1）找出rrt中 goalReached=1 的节点 % 2）找出上述节点中，cost最小的节点（选一个就可） % 3）根据该节点的rrt中第二个参数iPrev，找到其父节点 % 4）重新上述“3）”直到发现父节点为开始节点，便可得到一条cost最小的路径 % rrt树的四个参数 field1 = 'p'; field2 = 'iPrev'; % 借助该参数，可将整个路径搜索出来。 field3 = 'cost'; field4 = 'goalReached'; rng(param.random_seed); % 用指定的 randomseed 初始化随机数生成器 tic; % tic开始计时，常与toc配合使用，toc停止计时（算法运行的时间计时） start(); % 执行路径规划功能函数 function start() % s = struct(field1,value1,...,fieldN,valueN) 创建一个包含多个字段的结构体数组 rrt(1) = struct(field1, p_start, field2, 0, field3, 0, field4, 0); N = param.maxNodes; % 迭代次数 iterations j = 1; % while endcondition>param.threshold %&& j=op1(1) && nx=op1(2) && ny

【本文地址】

公司简介

联系我们