原理相比于最原始的 RRT 算法的一些缺点，提出的一种改进的 RRT 算法 为了加快随机树到达目标点的速度，简单的改进方法是：在随机树每次的生长过程中，根据随机概率（0.0 到 1.0 的随机值 p）来选择生长方向是目标点还是随机点。2001 年，LaValle在采样策略方面引入 RRT GoalBias 与 RRT GoalZoom，RRT GoalBias 方法中，规划器随机采样的同时,以一定概率向最终目标运动；RRTGoalZoom 方法中，规划器分别在整个空间和目标点周围的空间进行采样。 和普通RRT的区别仅在于随机撒点的时候有区别，这个p越大，算法越快，但对于复杂地形，可能会陷入局部极小处，反而变慢。一般取p=0.1程序示例参考Rapidly-Exploring Random Trees: A New Tool for Path Planning
2.1 原理双树RRT是在原本RRT的基础上多加了⼀颗随机探索树，各自从起点和终点向外探索拓展，直到两棵树相遇时规划算法收敛。这种改进过的探索策略可以⼤⼤提⾼RRT的运⾏效率。 双树RRT中存在两颗随机树，我们将其命名为A和B，A以起点为根节点，B以终点为根节点。两颗随机树的拓展方式和单树RRT的别无二致，同样都需要经历随机采样+步⻓限制+碰撞检测这三个步骤，但是不同的地⽅在于双树RRT的随机树是交替生长的，⽐⽅说第⼀轮迭代中A树向外⽣⻓，第⼆轮便切换为B树⽣⻓，如此循环。 在每轮迭代中，随机树除了向外拓展之外，还会多出⼀个步骤，就是遍历另一颗随机树中的所有节点，找出离NewNode最近的节点，用于判断两颗随机树是否相遇。 假设算法经历了N次迭代以后，已经拓展出如下图所示的两颗随机树。并且在下⼀轮迭代中，轮到A树进⾏拓展，A树在图中⽤绿⾊线条表示，B树⽤黄色线条表示。当进⼊本轮迭代后，算法成功拓展出A树的新节点NewNode_A，此时算法将遍历B树中的所有节点，找出B树中离NewNode_A最近的节点ClosestNode_B。并判断⼆者是否满⾜步⻓限制以及是否可以通过步⻓检测。如下图所示，这种情况明显⽆法通过碰撞检测。那么A树和B树在这⼀轮迭代中⽆法相遇，需要接着下⼀轮迭代。进⼊下⼀轮迭代，这次便切换为B树进⾏拓展，假设算法拓展出的NewNode_B以及遍历A树后得到的ClosestNode_A如下图所示，经过判断发现⼆者满⾜步⻓限制并且通过了碰撞检测，那么这时A树和B树就成功得相遇了，规划算法收敛当算法收敛以后，只需在两棵树的相遇处分别沿着⽗节点回溯便可以找出从起点到终点的有效路径。注意：双向RRT和RRT的区别不仅仅是在于双向生长，双向RRT比RRT更“贪心”，相比于RRT在生长RRT树的时候，是每产生一个随机点，如果能通过碰撞检测，就往该随机点的方向生长一次，然后该随机点就被废弃了，下一步想继续生长RRT树的话，就只能继续生成新的随机点，每个随机点最多利用一次。而双向RRT在生长RRT树时，先先生成一个随机点，然后，该树往该随机点的方向生长，直到碰到障碍物或则生长到该随机点，这样，一个随机点就被多次利用，加快了速度。2.2 伪码2.3 程序示例2.4 收敛性分析双向RRT的收敛性分析可以应用RRT的收敛性分析2.5 参考1、RRT-Connect: An Efficient Approach to Single-Query Path Planning2、https://www.guyuehome.com/9405
1.1 RRT算法思路我们有两个节点，一个绿色的起点，一个黄色的终点对于RRT，我们做的第一件事就是将起点设置为随机树的根，那么我们就拥有了一颗只有根节点的树这棵树光秃秃的，只有根节点的话不但难看，而且还没用。那么我们这时候就需要从这个根节点出发，向外拓展出新的叶⼦。拓展的方式很简单，就是随机采样+步⻓限制+碰撞检测。 RRT在每轮迭代中会⽣成⼀个随机采样点NewNode，如果NewNode位于自由区域，那么我们就可以遍历随机树中已有的全部节点，找出距离NewNode最近的节点ClosestNode。利⽤距离函数dist(NewNode, ClosestNode)得到二者之间的距离，如果满足步长限制的话，我们将接着对这两个节点进⾏碰撞检测，如果不满足步长限制的话，我们需要沿着NewNode和ClosestNode的连线⽅向，找出⼀个符合步长限制的中间点，用来替代NewNode。最后如果NewNode和ClosestNode通过了碰撞检测，就意味着二者之间存在边（edge），我们便可以将NewNode添加进随机树中。首先以第一轮迭代为例，因为刚开始我们的随机树中只有根节点，所以⽆论NewNode位于何处，遍历出的最近节点ClosestNode必然是根节点。 假设我们遇到下图这种情况，虽然采样点NewNode位于步⻓限制之内，但是却很不巧没有落在自由区域，即采样点落在障碍物的位置时，这个采样点会被算法舍弃。假设我们的步⻓限制为R，也就是说对于每个ClosestNode节点来说，只有当NewNode落在其半径为R的圆的范围内时，这个随机采样点NewNode才有可能被直接采纳。如下图所示，该红⾊随机采样点虽然位于⾃由区域，但是明显在根节点的步⻓限制之外。不过这个节点并不会被简单粗暴地舍弃。⽽是会沿着ClosestNode和NewNode的连线，找出符合步⻓限制的中间点，将这个中间点作为新的采样点。如下图所示，蓝点就可以替代红点作为新的采样点。那么假设我们已经通过第⼀轮迭代拓展出第⼀个叶⼦节点A，毫⽆疑问地A的⽗节点就是根节点，假设我们第⼆轮迭代的随机采样点NewNode为图中的点B，B落在A的步⻓限制范围内，但是A，B之间由于障碍物的阻挡，⽆法通过碰撞检测，于是B就会被算法舍弃。假设我们的随机采样点是下图中的B’，明显B’位于⾃由区域，满⾜步⻓条件，并且可以通过与点A的碰撞检测，那么我们就在B’和A之间添加⼀条边，并且将A设置为B’的⽗节点。学过数据结构的⼩伙伴⼀定知道，在树结构中每个节点最多只有⼀个⽗节点，⽗节点可以拥有多个⼦节点。在经历了N轮迭代后，我们已经获得了⼀颗如下图所示的随机树，这时我们发现此时的随机采样点竟然幸运地落在了终点的步⻓限制范围内，并且⼆者之间不存在障碍物。这时我们便可以认为，该采样点和终点之间存在⼀条边，于是将该节点设为终点的⽗节点，并把终点添加进随机树。此时算法就可以结束迭代了，即规划算法收敛。当规划算法收敛以后，只需要从终点开始，沿着其⽗节点进⾏回溯，就可以找 到起点-终点之间的有效路径。那么总结⼀下，RRT⽣成的每轮迭代中都包含以下这些流程：1. ⽣成⼀个随机采样点NewNode，并判断采样点是否位于⾃由区域2. 遍历随机树，找出距离NewNode最近的节点ClosestNode3. 判断NewNode是否在ClosestNode的步⻓限制范围内，否则寻找中间点替代NewNode4. 判断NewNode和ClosestNode之间是否存在障碍物，即碰撞检测。5. 如果NewNode满⾜以上所有约束条件，则将NewNode添加进随机树，设置ClosestNode为NewNode的⽗节点。6. 判断NewNode是否在终点的步⻓限制范围内，并对其⼆者做碰撞检测。如果满⾜条件则将该NewNode设为终点的⽗节点，并将终点加⼊随机树，即可结束迭代。否则继续迭代。1.2 伪码1.3 RRT的收敛性分析本节主要介绍了该规划⽅法的理论特性。Theorem 1 如果存在长度为k的连接序列，则将 x_init 连接到 x_goal 所预期的迭代次数不超过k/p Pf. 参考S.M. Lavalle and J.J. Kuffffner. "Randomized Kinodynamic Planning." The International Journal of Robotics Research. Vol. 20, Number 5, 2001, pp. 378 – 400. 第392页Theorem 2 Pf. 参考S.M. Lavalle and J.J. Kuffffner. "Randomized Kinodynamic Planning." The International Journal of Robotics Research. Vol. 20, Number 5, 2001, pp. 378 – 400. 第393页Theorem 3 当顶点数趋于无穷时，在 x_init 处初始化的RRT包含 x_goal 的概率将趋近于1 Pf. 参考S.M. Lavalle and J.J. Kuffffner. "Randomized Kinodynamic Planning." The International Journal of Robotics Research. Vol. 20, Number 5, 2001, pp. 378 – 400. 第394页定理1和定理2表示了规划器的收敛率，定理3建⽴了规划器是概率完备的（即，当迭代数趋于⽆穷时，找到解的概率趋于1我们可以看出，由于算法在未达到收敛条件之前是在不断进⾏迭代的，所以只要在规划的起点和终点之间是存在有效的路径，那么只要迭代的次数够多，那么采样点就够多，随机树就长得越茂密，能探索到的区域就⾜够⼴，就必然可以找到有效的路径。所以RRT是概率完备的。但是由于采样点每次都是随机的，所以算法并不能保证找到的路径是最优的路径。因此RRT是⾮最优的。1.4 程序示例1.5 参考1、S.M. Lavalle and J.J. Kuffffner. "Randomized Kinodynamic Planning." The International Journal of Robotics Research. Vol. 20, Number 5, 2001, pp. 378 – 400.2、Rapidly-Exploring Random Trees: A New Tool for Path Planning3、https://www.guyuehome.com/9405
什么是路径规划？ 路径规划（也叫运动学规划），任务是确定控制输入，以驱动机器人从初始配置和速度到目标配置和速度，同时服从基于物理的动力学模型，且能确保机器人在环境中避开障碍。说白了，就是给你一张地图，且已知障碍物分布，以及起始点和目标点的坐标，希望你根据这些信息，找到一条从起点到终点的能绕开障碍物的有效路径，如果可以，还希望这条有效路径尽可能最优（最短），并且希望找到这条有效路径的时间尽可能短（算法足够高效） 目前流行的路径规划分为两大类：基于采样的路径规划和基于搜索的路径规划。运动规划的状态空间是应用于机器人变换的集合，称为位姿空间（configuration space），引入了 C-空间、C-空间障碍物、自由空间等一系列概念，下面介绍一些概念：位姿（configuration）机器人一个位姿指的是一组相互独立的参数集，它能完全确定机器人上所有的点在工作空间 W 中的位置，这些参数用来完整描述机器人在工作空间 W 中的状态。一个位姿通常表示为带有位置和方向参数的一个向量(vector)，用 q 表示。自由度（degrees of freedom）机器人的自由度定义为机器人运动过程中决定其运动状态的所有独立参数的数目，即 q 的维数。位姿空间（configuration space）位姿空间是机器人所有可能位姿组成的集合，代表了机器人所有可能的运动结果，称为 C-空间，也可简记为 C。距离函数（distance function）C-空间中的距离函数定义为该空间中的一个映射
数据可视化（Data Visualization）是关于数据视觉表现形式的科学技术研究，指利用计算机图形学和图像处理技术，将数据转换为图形或图像在屏幕上显示出来，成为对人类视觉更为友好的图形图像的过程。本文使用的北太天元版本为 Baltamatica 2.1.3.2 Windows版1. 首先需要确保北太天元已经加载了 绘图插件 graph， 可以在 帮助 》 插件 中查看，软件安装完成后会默认加载绘图插件。 2. 在绘图插件加载后，如何查看绘图插件提供哪些函数呢？有两个方法，一个是直接在命令行窗口输入 plugin_help(‘graph’) 就可以查看目前绘图插件提供的全部函数，或者在命令行窗口输入 help , 这个命令会返回北太天元里提供的所有内核提供的命令、脚本提供的命令和 插件 [graph(已授权)] 提供的命令。如下图所示 3. 如何查看某个绘图函数的具体用法呢？在命令行窗口直接输入 help 函数名，例如help plot3 使用 plot3 绘制三维螺旋线。t=0:pi/50:10*pi;plot3(sin(t),cos(t),t);axis square;以上代码运行的结果如下图所示。 下面介绍一下北太天元提供的二维图形特殊二维图形1. 条形图bar(X,Y)：X是坐标，Y是高度，条形的跨度是x坐标的最小间距 bar(Y)：对Y绘制条形图。如果Y为矩阵，Y的每一行聚集在一起。横2. 区域图区域图用于显示向量或者矩阵中的元素在对应的x下，在所有元素中所占的比例。默认情况下，函数area将矩阵中各行的元素集中，将这些值绘成曲线，并填充曲线和x轴之间的空间。其调用语法如下。area(Y)：绘制向量Y。area(X,Y)：绘制 Y 中的值对 x 坐标 X 的图。然后，该函数根据 Y 的形状填充曲线之间的区域：如果 Y 是向量，则该图包含一条曲线。area 填充该曲线和水平轴之间的区域。如果 Y 是矩阵，则该图对 Y 中的每列都包含一条曲线。area 填充这些曲线之间的区域并堆叠它们，从而显示在每个 x 坐标处每个行元素在总高度中的相对量。3. 饼形图在统计学中，经常要使用饼形图来表示各个统计量占总量的份额，饼形图可以显示向量或矩阵中的元素占总体的百分比。在北太天元中可以使用pie函数来绘制二维饼形图，其调用语法如下。pie(X)：使用 X 中的数据绘制饼图。饼图的每个扇区代表 X 中的一个元素。如果 sum(X) ≤ 1，X 中的值直接指定饼图扇区的面积。如果 sum(X) < 1，pie 仅绘制部分饼图。如果 sum(X) > 1，则 pie 通过 X/sum(X) 对值进行归一化，以确定饼图的每个扇区的面积。4. 直方图直方图用于直观地显示数据的分布情况。在北太天元中提供了两个函数用于直方图的绘制：hist和polarhistogram。hist主要是用于直角坐标系直方图的绘制；polarhistogram主要用于极坐标系下直方图的绘制。下文主要介绍hist函数的用法。hist函数的调用语法如下。n=hist(Y)：绘制Y的直方图。n=hist(Y,nbins)：指定分格的数目。5. 针状图在北太天元中，可以使用函数stem生成二维离散图形。stem函数调用语法如下：stem(Y)：绘制Y的数据序列，图形起始于X轴，并在每个数据点处绘制一个小圆圈。strm(X,Y)：按照指定的X绘制数据序列Y。6. 方向矢量图和速度矢量图在北太天元中可以绘制方向矢量图和速度矢量图。quiver函数用来绘制箭状图或者速度矢量图，其调用语法如下。quiver(x,y,u,v)：绘制矢量图，参数x和y用于指定矢量的位置，u和v用于指定要绘制的矢量。quiver(u,v)：绘制矢量图，矢量的位置为默认值。梯度方向也就是速度方向，本例使用quiver函数即可达到目的。7. 等高线的绘制等高线用于创建、显示并标注由一个或多个矩阵确定的等值线。北太天元中提供有一些函数用于绘制等高线：contour 显示矩阵Z的二维等高线图meshc 创建一个匹配有二维等高线图的网格图contourf 显示矩阵Z的二维等高线图，并在各等高线之间用实体颜色填充surfc 创建一个匹配有二维等高线图的曲面图 这里只介绍最常用的函数contour，其他函数请读者自行查阅帮助文档。contour函数用于绘制二维等高线图，其调用语法如下。contour(Z)：绘制矩阵Z的等高线，绘制时将Z在x-y平面插值，等高线数量和数值由系统根据Z自动确定。contour(X,Y,Z)：绘制矩阵Z的等高线，坐标值由矩阵X和Y指定，矩阵X、Y、Z的维数必须相同。contour(X,Y,Z,“ShowText”,“on”)：绘制矩阵Z的等高线，坐标值由矩阵X和Y指定三维图形,通过ShowText后的参数为“on”或者“off”,设置图像是否显示标注。三维图形除了绘制二维图形，北太天元还提供一系列三维图形绘制函数，下文将对这些函数进行详细说明。绘制三维曲线图在北太天元中，plot3函数用于绘制三维曲线图。该函数的用法和plot类似，其调用语法如下。plot3(X,Y,Z)： 绘制三维空间中的坐标。要绘制由线段连接的一组坐标，请将 X、Y、Z 指定为相同长度的向量。要在同一组坐标轴上绘制多组坐标，请将 X、Y 或 Z 中的至少一个指定为矩阵，其他指定为向量。plot3(X,Y,Z,LineSpec)： 使用指定的线型、标记和颜色创建绘图。plot3(X1,Y1,Z1,...,Xn,Yn,Zn)： 在同一组坐标轴上绘制多组坐标。使用此语法作为将多组坐标指定为矩阵的替代方法。plot3(X1,Y1,Z1,LineSpec1,...,Xn,Yn,Zn,LineSpecn)： 可为每个 XYZ 三元组指定特定的线型、标记和颜色。您可以对某些三元组指定 LineSpec，而对其他三元组省略它。plot3(...,Name,Value)： 使用一个或多个名称-值对组参数指定 Line 属性。绘制三维曲面图在北太天元中，除了plot3函数可用于绘制三维图形外，还有一些函数可以用来绘制三维网格图和曲面图。下面分别介绍这些函数。1. 三维网格图mesh函数用于绘制三维网格图，其调用语法如下。mesh(X,Y,Z)： 创建一个网格图，该网格图为三维曲面，有实色边颜色，无面颜色。该函数将矩阵 Z 中的值绘制为由 X 和 Y 定义的 x-y 平面中的网格上方的高度。边颜色因 Z 指定的高度而异。mesh(Z)： 创建一个网格图，并将 Z 中元素的列索引和行索引用作 x 坐标和 y坐标。mesh(Z,C)： 进一步指定边的颜色。mesh(___,C)： 进一步指定边的颜色。mesh(ax,___)： 将图形绘制到 ax 指定的坐标区中，而不是当前坐标区中。指定坐标区作为第一个输入参数。mesh(___,Name,Value)： 使用一个或多个名称-值对组参数指定曲面属性。例如，'FaceAlpha',0.5 创建半透明网格图。2. 三维曲面图函数surf用来绘制三维表面图形，其调用语法如下。surf(X,Y,Z) 创建一个三维曲面图，它是一个具有实色边和实色面的三维曲面。该函数将矩阵 Z 中的值绘制为由 X 和 Y 定义的 x-y 平面中的网格上方的高度。曲面的颜色根据 Z 指定的高度而变化。surf(Z) 创建一个曲面图，并将 Z 中元素的列索引和行索引用作 x 坐标和 y 坐标。
在使用北太天元编写一个程序时，经常需要从外部读入数据，或者将程序运行的结果保存为文件，北太天元主要支持以下格式数据文件的导入导出：.mat、.txt、.csv、.xls、.xlsx。具体介绍及用法如下。一、MAT文件的导入导出1. MAT文件的导出1.1 使用save函数
>> help save
save 将工作区变量保存到文件中。
save(filename)，将当前工作区中的所有变量存储在名为 filename 的二进制文件 MAT 文件中。
filename 为字符向量或字符串标量。例如，将文件名指定为 "myFile" 或 "myFile.mat"。
如果未指定文件名，则将数据保存到名为 baltamatica.mat 的文件中。
如果 filename 不包含扩展名，则会默认补充 '.mat' 扩展名。如果文件名不包含完整路径，则保存在当前文件夹中。
保存路径必须具有写入文件的权限。
当第一个参数为 '-struct' 时，会将 '-struct' 后的第二个参数 (非以 '-' 开头的参数) 当做 filename，
如 save('-struct',structname,filename,fieldnames)。
save(filename,variables)，仅存储指定的变量。filename 和 variables 为字符向量或字符串标量。
variables 可使用 '*' 通配符匹配模式。例如，save('data.mat','A*') 保存以 A 开头的所有变量。
save(filename,'-struct',structname,fieldnames)，将标量结构体的字段存储为单个变量。
如果使用了 fieldnames 参数，则 save 函数仅存储结构体中的 fieldnames 字段。
fieldnames 与 variables 具有相同的形式。不能在同一调用中指定 variables 和 '-struct' 来保存数据。示例：保存structure数组实例。例如有如下的structure数组s1：s1.a = 22.33; s1.b =”Steve”; s1.c = ' World!';使用 save 命令，可将整个structure数组保存为struct_data.mat。>> save('struct_data.mat', 's1');1.2 使用界面操作在工作区中使用鼠标左键选中要保存的数据，点击鼠标右键，会弹出操作框，点击保存即可，在工作区空白处点击鼠标右键，可保存工作区的所有变量。2.MAT文件的导入2.1使用load函数>> help load
load 将文件变量加载到工作区中。
load(filename)，将 MAT 文件中的变量加载到工作区。
filename 为字符向量或字符串标量。例如，将文件名指定为 "myFile" 或 "myFile.mat"。
如果未指定文件名，默认读取当前路径下的 baltamatica.mat。
load(filename,variables)，读取指定的变量。filename 和 variables 为字符向量或字符串标量。
variables 可使用 '*' 通配符匹配模式。例如，load('data.mat','A*') 加载以 A 开头的所有变量。
load(filename,'-mat') 将 filename 视为 mat 文件，而不管文件扩展名如何。
load(filename,'-mat',variables) 加载 filename 文件中的指定变量。示例：将A.mat文件中的变量导入到structure数组s中。>> s=load("A.mat")
s =
1x1 struct
结构体：
A1: [3x3 double]
A2: "string"
A3: [1x3 double]
A4: [1x2 cell array]2.2 使用界面操作点击菜单栏的“导入“->”导入数据”弹出导入文件操作界面二、TXT、CSV及Excel文件的导入导出1. TXT、CSV及Excel文件的导入1.1 使用readmatrix函数（1）导入文本文件文本文件的数据格式在行和列上必须采取一致的模式，并使用分隔符来分隔各个数据项。分隔符可以是空格、逗号、分号或其他字符，单个的数据可以是字母、数值字符或它们的混合形式。示例：文件data.txt包含了两行数据，各数据之间由空格分隔。1 2 34 5 6>> m=readmatrix("data.txt") %使用readmatrix导入数据
m =
2x3 double
1 2 3
4 5 6（2）导入csv数据csv文件是逗号分隔的纯文件文件。除了可以使用readmatrix函数读取之外，同时也可以使用csvread函数，推荐使用readmatrix函数。示例：文件data.csv中的数据如下：1.2,2.5,3.2,4.65.4,6.2,7.1,8.2>> m=readmatrix("data.csv") %使用readmatrix导入数据
m =
2x4 double
1.2000 2.5000 3.2000 4.6000
5.4000 6.2000 7.1000 8.2000（3）导入Excel数据Excel文件包含.xls及.xlsx两类文件。示例：文件data.xlsx中的数据如下：>> m=readmatrix("data.xlsx") %使用readmatrix导入数据
m =
5x2 double
1.0000 2.0000
3.0000 4.0000
5.0000 6.0000
NaN NaN
7.0000 8.00001.2 使用界面操作点击菜单栏的“导入“->”导入数据”，弹出导入文件操作界面，选择文件导入即可。2. TXT、CSV及Excel文件的导出2.1使用writematrix函数（1）保存txt文本文件示例：将a=[1,2,3;4,5,6;7,8,9]'所表示的矩阵数据存储到“w_data.txt”的文件当中，以制表符分隔。>> a = 3x3 double 1 4 7 2 5 8 3 6 9 >> writematrix(a,"w_data.txt","Delimiter","-")使用文本查看器查看w_data.txt：（2）保存csv文本文件示例：将a=[1,2,3;4,5,6;7,8,9]'所表示的矩阵数据存储到“w_data.csv”的文件当中。
>> writematrix(a,"w_data.csv")使用wps查看器查看w_data.csv：（3）保存xlsx文本文件
>> writematrix(a,"w_data.xlsx")使用wps查看器查看w_data.xlsx：注：暂不支持.xls文件格式的导出操作，excel推荐使用.xlsx的导出文件格式。2.2 使用界面操作在工作区中使用鼠标左键选中要保存的数据，点击鼠标右键，会弹出操作框，点击“导出变量”即可。该方式仅支持”.xlsx”格式文件的导出。
公众号