三维路径规划基于matlab RRT_Star算法三维路径规划含Matlab源码 1571期

Posted 2021-12-31 紫极神光

一、获取代码方式

获取代码方式1：
通过订阅紫极神光博客付费专栏，凭支付凭证，私信博主，可获得此代码。

获取代码方式2：
完整代码已上传我的资源：【三维路径规划】基于matlab RRT_Star算法三维路径规划【含Matlab源码 1571期】

备注：
订阅紫极神光博客付费专栏，可免费获得1份代码（有效期为订阅日起，三天内有效）；

二、RRT算法简介

0 引言
随着现代技术的发展，飞行器种类不断变多，应用也日趋专一化、完善化，如专门用作植保的大疆PS-X625无人机，用作街景拍摄与监控巡察的宝鸡行翼航空科技的X8无人机，以及用作水下救援的白鲨MIX水下无人机等，决定飞行器性能主要是内部的飞控系统和外部的路径规划问题。就路径问题而言，在具体实施任务时仅靠操作员手中的遥控器控制无人飞行器执行相应的工作，可能会对操作员心理以及技术提出极高的要求，为了避免个人操作失误，进而造成飞行器损坏的危险，一种解决问题的方法就是对飞行器进行航迹规划。
飞行器的测量精度，航迹路径的合理规划，飞行器工作时的稳定性、安全性等这些变化对飞行器的综合控制系统要求越来越高。无人机航路规划是为了保证无人机完成特定的飞行任务，并且能够在完成任务的过程中躲避各种障碍、威胁区域而设计出最优航迹路线的问题。常见的航迹规划算法如图1所示。

图1 常见路径规划算法
文中主要对无人机巡航阶段的航迹规划进行研究，假设无人机在飞行中维持高度与速度不变，那么航迹规划成为一个二维平面的规划问题。在航迹规划算法中,A算法计算简单，容易实现。在改进A算法基础上，提出一种新的、易于理解的改进A算法的无人机航迹规划方法。传统A算法将规划区域栅格化，节点扩展只限于栅格线的交叉点，在栅格线的交叉点与交叉点之间往往存在一定角度的两个运动方向。将存在角度的两段路径无限放大、细化，然后分别用两段上的相应路径规划点作为切点，找到相对应的组成内切圆的圆心，然后作弧，并求出相对应的两切点之间的弧所对应的圆心角，根据下式计算出弧线的长度

式中：R———内切圆的半径；
α———切点之间弧线对应的圆心角。

二、RRT算法简介

1 RRT定义
RRT（Rapidly-Exploring Random Tree）算法是一种基于采样的路径规划算法，常用于移动机器人路径规划，适合解决高维空间和复杂约束下的路径规划问题。基本思想是以产生随机点的方式通过一个步长向目标点搜索前进，有效躲避障碍物，避免路径陷入局部极小值，收敛速度快。本文通过matlab实现RRT算法，解决二维平面的路径规划问题。
2 地图
为了方便算法的实现，使用离散量来表达环境地图。其中，数值0表示无障碍物的空区域，数值1表示该区域有障碍物。

RRT算法中搜索到的顶点坐标为连续点，在地图中产生随机点，算法将通过连续的点构建树。此过程中，对树枝和顶点进行检测，检测顶点所处位置是否是空区域。下载附录中.dat文件，绘制地图。

colormap=[1 1 1; 0 0 0; 1 0 0; 0 1 0; 0 0 1];
imshow(uint8(map),colormap)

note:数据中的列为x轴，行为y轴

3 RRT算法原理
通过matlab程序构建从起始位置到目标位置的树，并生成连接两个点的路径。使用一颗中心点在起始点的树，而不是两颗树（一个中心点在起始位置，一个中心点在目标位置）。
编写一个matlab函数，输入和输出有相同的形式。

function [vertices, edges, path] = rrt(map, q_start, q_goal, k, delta_q, p)

其中：
map：.mat文件中的地图矩阵
q_start：起点的x和y坐标
q_goal：目标点的x和y坐标
k: 在目标点无法找到是，控制产生搜索树的最大迭代次数为k次
delta_q : q_new 和 q_near之间的距离
p: 将q_goal 作为q_rand 的概率，当随机产生的随机数小于p，将目标点作为随机点q_rand，当随机产生的数大于p时，产生一个随机点作为q_rand
vertices：顶点的x和y坐标，生成随机树的过程中产生的所有的点的坐标都存储在这个矩阵中，第一个点为起点，最后一个点为目标点。是一个2行n列的矩阵
deges：生成随机树的所有树枝，一共有n-1个树枝，因此该矩阵有n-1行，每一行的两列分别表示两个点的索引号。一旦搜索到目标点，最后一行将表示目标点，沿着目标点回溯，即可找到路径
path: 从起始点到目标点的索引，是一个行向量
下面用一个图来表示上面提到的算法里的一些变量：

4 障碍物检测
检测树枝（即q_near和q_new之间的edge）是否处于自由空间，可以使用增量法或者等分法，示意图如下(假设两点之间有10个点，左图为为增量检测法，右图为等分法，从示意图中可以看出使用等分法检测次数更少)：

在本文中，使用k=10000，delta_q=50,p=0.3, 我们将获得如下结果：

5 路径平滑处理
完成基本的RRT算法之后，我们获得了一条从起点到终点的路径，现在对这条路径进行平滑和降噪处理，处理完成之后，我们将得到一条更短的路径。
采用贪心算法：
连接q_start和q_goal,如果检测到两个点之间有障碍物，则将q_goal替换为前一个点，直到两个点能连接上（中间无障碍物）为止。一旦q_goal被连接上，
在matlab中定义平滑函数：

function [path_smooth] = smooth(map, path, vertices, delta)

其中：
path: 从起始点到目标位置的路径索引号
vertices：树中所有的顶点坐标
delta：增量距离，用来检测路径顶点之间的直接连接是否在自由空间之内，每个edge都被delta分割成几段
path_smooth：经过平滑处理之后，路径点将会减少，用path_smooth记录平滑之后的路径,仍然是一个行向量，记录路径的索引号

平滑处理之后的路径为：

6 总结
RRT算法是一种增量式的搜索算法，基于概率的思想，它是一种概率完备的路径优化算法，具有求解速度上的优势。RRT基本算法有其自身缺陷，求解得到的路径通常质量不好，带有棱角，不够光滑。因此需要对路径进行平滑处理，才能得到适合机器人路径跟踪的路径曲线。

四、部分源代码

%%   RRT* 3D state space
%%  - create a path from a start node to an end node
%%    using the RRT algorithm.
%%  - RRT* = Rapidly-exploring Random Tree star
%% This short programm is writen based other basic RRT code, and used for test my hardware work .
%% Apologize no time to add the enough comments 
 
function RRT_star_3D;

clear all;
% create random world,determine the parameters of simulation
Size = 16384;       % the 2D world is a 100 by 100 map

Obs_Cordi = [1,1,1;1,1,1;1,1,1];
NumObstacles = 3; % randomly obstacles distributed on the map
maxObstacleRadius = 0.05; % define the max radius of obstacle
maxturningangle = 1.05;  %75deg
Heading = 0.758;% 45 degree; ccw rotation, vector from positive x-axis
world = createWorld(NumObstacles,[Size;Size;Size],[0;0;0],maxObstacleRadius,Obs_Cordi);
segmentLength = 200; % standard length of path segments
hyper_R  = 450;

start_node =[129,129,129,0,0,0];    %start node's idx is 1. node array format ：[x, y, chi(indicate if this node get to the end node), cost, parent's idx]
end_node   = [1800,1800,1800,0,0,0];

% establish tree starting with the start node
tree = start_node;

% check to see if start_node connects directly to end_node
if ( (norm(start_node(1:3)-end_node(1:3))<segmentLength )... % norm calculatet the distance
    &&(collision(start_node,end_node,world)==0) ) 
  path = [start_node; end_node];
else
  numPaths = 0; 
  while numPaths<2000,
      [tree,flag] = extendTree(tree,end_node,segmentLength,world,maxturningangle,hyper_R);
      numPaths = numPaths + 1;
  end
end

plotWorld(world,tree);
clc
tree
size(tree)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% createWorld
%%  - create random world with obstacles
%%  the first element is the north coordinate
%%  the second element is the south coordinate
%% 
function world = createWorld(NumObstacles, NEcorner, SWcorner,maxRadius, obstacle_cordi);

  % check to make sure that the region is nonempty
  if (NEcorner(1) <= SWcorner(1)) | (NEcorner(2) <= SWcorner(2)) | (NEcorner(3) <= SWcorner(3)),%% NEcorner(1:2)=(100, 100) SWcorner(1:2)=(0,0)
      disp('Not valid corner specifications!')
      world=[];
      
  % create world data structure
  else
    temp=size(obstacle_cordi);
    NumObstacles=temp(1);
    world.NumObstacles = NumObstacles;
    world.NEcorner = NEcorner;
    world.SWcorner = SWcorner;
     
    for i=1:NumObstacles,
        world.radius(i) = maxRadius;
        world.cn(i) = obstacle_cordi(i,1);      % give the X coordinate to current obstacle
        world.ce(i) = obstacle_cordi(i,2);       % give the Y coordinate to current obstacle
        world.cz(i) = obstacle_cordi(i,3); 
    end
  end
%% generateRandomNode
%%   create a random node (initialize)
function node=generateRandomNode(world);

% randomly pick configuration 
pn       = (world.NEcorner(1)-world.SWcorner(1))*rand;
pe       = (world.NEcorner(2)-world.SWcorner(2))*rand;
pz       = (world.NEcorner(3)-world.SWcorner(3))*rand;
chi      = 0;                                               % what's the chi????????
cost     = 0;
node     = [pn, pe, pz,chi, cost, 0];

% check collision with obstacle
while collision(node, node, world),
  pn       = (world.NEcorner(1)-world.SWcorner(1))*rand;
  pe       = (world.NEcorner(2)-world.SWcorner(2))*rand;
  pe       = (world.NEcorner(3)-world.SWcorner(3))*rand;
  chi      = 0;
  cost     = 0;
  node     = [pn, pe,pz, chi, cost, 0];
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% collision
%%   check to see if a node is in collsion with obstacles
function collision_flag = collision(node, parent, world);

collision_flag = 0;

if ((node(1)>world.NEcorner(1))...      % to see if the node is go beyond the boundary
    | (node(1)<world.SWcorner(1))...
    | (node(2)>world.NEcorner(2))...
    | (node(2)<world.SWcorner(2))...
    |(node(3)>world.NEcorner(3))...
    |(node(3)<world.SWcorner(3)))
  collision_flag = 1;
else

    for sigma = 0:.25:1, %%sigma can be adjusted to add more samples 
    p = sigma*node(1:3) + (1-sigma)*parent(1:3);  %% parent denote the end node?
      % check each obstacle
      for i=1:world.NumObstacles,
        if (norm([p(1);p(2);p(3)]-[world.cn(i); world.ce(i); world.cz(i)])<=1.5*world.radius(i)),
            collision_flag = 1;
            break;  
        end
      end
    end
end
%% extendTree
%%   extend tree by randomly selecting point and growing tree toward that
%%   point
function [new_tree,flag] = extendTree(tree,end_node,segmentLength,world,maxturningangle,hyper_R);

  hyper_tree = [];
  flag1 = 0;    
   while flag1==0,
  
    randomPoint = [...
        (world.NEcorner(1)-world.SWcorner(1))*rand,...
        (world.NEcorner(2)-world.SWcorner(2))*rand,...
        (world.NEcorner(3)-world.SWcorner(3))*rand];

  
    tmp = tree(:,1:3)-ones(size(tree,1),1)*randomPoint;     
    [dist,idx] = min(diag(tmp*tmp'));
    cost     = tree(idx,5) + segmentLength;
    new_point = (randomPoint-tree(idx,1:3));
    new_point = tree(idx,1:3)+new_point/norm(new_point)*segmentLength; 
    new_node = [new_point, 0, cost, idx];

     min_cost1 = new_node(5:5);
    
      if (size(tree,1) >=2),
         for j=1:size(tree,1); 
            if( norm(new_node(1:3)-tree(j,1:3))  < hyper_R  )
               hyper_tree = [hyper_tree;j, tree(j,1:6)];
            end
         end
      end    
  
  

五、运行结果

六、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 包子阳,余继周,杨杉.智能优化算法及其MATLAB实例（第2版）[M].电子工业出版社，2016.
[2]张岩,吴水根.MATLAB优化算法源代码[M].清华大学出版社，2017.
[3]RRT路径规划算法