5.1反应式导航

机器人的定义：一种能感知、规划并行动的目标导向机械。

5.1.1Braitenberg车

Braitenberg车是一种非常简单的目标达成机器人，其特点是传感器与电机之间直接连接。小车自身既没有其作业环境的信息，也没有任何路径规划可言。

其Simulink模型为

>> sl_braitenberg

如图，它通过直接从传感器导出的转向信号来达到目标。

机器人两侧各安装一个传感器，传感器由图中黑色模块所示。通过传感器相对于机器人的位置以及感知函数进行参数化。

被感知的势场由一个简单的平方反比函数定义：

小车的速度是：

S_R和S_L分别是右侧和左侧的传感器值的读数。

转向角由两个传感器读数的差值决定：

当左右侧传感器的值相等时机器人直行移动。

从Simulink菜单或命令行开始仿真：

5.1.2简单自动机

一种反应式机器人“虫子”能避开不可达区域或障碍物搜索到达目标地点，针对虫子机器人的控制算法有很多，它们都具备感知接近障碍物的能力，与Braitenberg车不同的是，它们在感知和驱动之间加入了一个状态机和其他逻辑，并具有记忆能力。

给机器人加载一个障碍场：

>> load mapl

我们创建一个bug2类的实例：

>> bug = Bug2 (map);

目标是

>> bug.goal  = [50;  35];

使用path方法来运行仿真：

>> bug.path([20;  10]);

其中的参数是机器人的初始位置。图中的一系列浅灰色点线即表示这种算法产生的运动路径。

bug2算法所用的策略非常简单，它先让机器人沿着一条直线向目标点运动。如果遇到障碍机器人就逆时针绕行，直到回到最初那条直线上的一点，这个点比之前遇到障碍时的点离目标更近。

如果指定一个输出参数：

>> p = bug.path([20;   10]);

它将以矩阵P返回路径。查看该矩阵：

>> about (p)

p [double) :  332x2  (5312 bytes)

其中每一行表示路径上的一点，本例中一共是332个点。也可以不指定出发点调用该函数：

>> p  =  bug.path();

出发点将通过点击图上的任意点来选择。

不依赖地图使这种算法的功能受到很大局限。

5.2基于地图的路径规划

给出一些假设：

1.   机器人在网格中活动，且只占一格单元格

2.   机器人不含有任何非完整约束，且可以移动到相邻单元格

3.   机器人可以确定它在平面上的位置

4.   机器人可以使用地图计算它要走的路径。

所有的例子中都可以用到以下参数：

分别对应目标点、起始点和世界地图。

5.2.1距离变换

考虑只有一个代表目标的非零元素的矩阵。

为了把距离转换用于机器人导航，我们创建一个对象DXform，它是从类Navigation衍生出来的：

>> dx  =  DXform(map);

然后创建一个能达到指定目标的规划：

>>  dx. plan (goal)

可以将其在屏幕上显示出来：

>> dx.plot(p)

如图，红色的障碍物区域正被叠加到距离地图上，其中任何一点的灰度等级表示了它到目标点的距离，从而为障碍物绕行提供了依据。

无论机器人从什么地方开始，都会朝着距离目标最近的相邻网格移动，这种算法一直持续，直到抵达目的地为止。

例如寻找一条从点start开始到目标的路径，只需输入：

>> dx.path(start);

如果调用path方法时给定了一个输出参数，则会跳过动画，如

>>  p =  dx.path(start);

路径被返回到一个矩阵，每一行代表一个点。我们可以将这些点画出：

>> dx.plot(p)

要查看路径包括的点数，输入：

>> numrows(p)

 ans=

205

可以看出，其点数要比bug2所得小很多。与bug2不同，这种规划算法找到的是距离更短的顺时针绕行路径。

通过以下指令显示距离变换的迭代过程：

>> dx.plan(goal,  0.1);

可以绘制出其3D图像：

>>dx.plot3d(p)

5.2.2 D⋇

1.D⋇将占用网络推广为一张成本地图

2.D⋇是通过寻找一条路径使得运行的总成本最低。

3.D⋇的关键特性是它支持增量式的重复规划，这是非常重要的。

首先建立一个D导航对象：

>> ds=Dstar(map);

D规划将过去的占用网络地图转换为成本地图，从而可以得到：

>>c=ds.costmap();

这里c取1或者无穷大，分别代表空闲和被占用的单元。

然后生成一个移动到目的地的规划：

>>ds.plan(goal);

在MATLAB中这个最初的规划阶段是非常慢的，需要花费几十秒的时间：

>> ds. niter

ans=

10558

以上指令显示了规划循环迭代的次数。

图显示了从任何一个初始点到达目标的路径：

>>   ds.path(start);

可以看到，机器人又一次选择了障碍左边的较短的路径，这和采用距离转换方法得到的结果一样。

D的真正威力在于它能在任务中有效地更改成本地图。

采用rnodify_cost方法告诉矿所要做的修改，例如：

>> for  y=78:85

>> forx=l2:45

>> ds.modify_cost([x,y],  2);

>> end

>> end

再一次应用规划算法来更新规划：

>> as.plan();

但这次迭代的次数仅是

>> ds.niter

ans=

3178

它只有原来规划所需次数的30%。机器人的新路径为

>> ds.path(start);

5.2.3沃罗诺伊路线图法

在路径规划的术语中，把生成一个计划称为“规划阶段”，而利用规划阶段的结果找到一条A到B的路径则称为“查询阶段”。

规划和查询的计算成本差异导致了路线图方法的产生，这种方法查询的既有起始位置也有目标位置。规划阶段提供的分析也支持不断变化的出发点和目标点。机器人导航问题变成要在环境中建立一个绕开障碍物的路网,就像铁路网一样。在文献中这样一个网络被称为路线图。路线图只需要计算一次，然后就像使用铁路网一样用它引导我们从任何初始位置去到任何目标位置。

将一种称为细化的形态学图像处理算法作用到图5.9(a)上，可以算出自由空间的拓扑

骨架:>>skeleton =ithin(free);

计算结果如图5.9(b)所示。我们看到障碍物区变大，而白色单元代表的自由空间变成了细线相连的网络,它们与最初障碍物的边界是等距的。

图5.9(c)显示的是自由空间网络叠加到原图上的效果。我们已经创建好了一个覆盖全区域的路径网络,可以用它来实现避开障碍物的移动。这些路径就是一般化沃罗诺伊图的边。

如图5.9(d)所示。每个像素的值代表它到最近障碍物的距离,其脊线与图5.9(b)所示的骨架是对应的。虽然细化或骨架化算法也与距离变换一样是计算量很大的迭代算法,但它为我们很好地展示了在自由空间中寻找路径的方法和原理。

5.2.4概率路线图方法

前面介绍的距离变换和骨架化方法的计算量都很大，对于较大的地图不适用。为改善上述问题，于是产生了基于概率的方法。这些方法在全局地图上稀疏采样,其中最著名的方法就是概率路线图法，或称PRM法。

用工具箱实现这个方法的第一步是创建一个PRM 对象:>> prm = PRM(map)

然后生成规划:

>> prm.plan()

注意，这种方法并不把goal作为一个参数传给规划器,因为规划生成与目标位置是独立的。产生一条路径分为两个步骤:规划和查询。规划阶段首先在自由空间中找N个随机点默认值是100。然后用直线将每个点与其最近的邻点连接,不穿过障碍，这样就形成了一个节点数最小且开环的网络图。PRM算法的优点就在于只要检验有限个点就可以确保所有点及其形成的路径是不穿过障碍的网。计算结果就存储在PRM对象中，该对象的概要可显示如下:

>> prm

Drm =

PRM:100x100

graph size:100

dist thresh:30.000000

100 vertices

712 edges

components

其中给出了图中边和连接元素的数量。结果可以用下面的指令画出:

>> prm.visualize()

为了提高运算效率,我们不得不做出一些重要的牺牲。首先,在自由空间中随机取样意味着每次运行该规划器都会产生一个不同的路线图，从而导致生成不同路径及路径长度。例如我们再运行一次规划器:

>> prm.plan();

它将产生如图5.10(b)所示的图，该图具有与图5.10(a)不同的节点和不同的边。

5.2.5 RRT

该规划器可以考虑机器人的运动模型，从而不用像以前的规

刘器那样假设机器人具有全方向运动能力。

我们讨论的这种规划器称为快速搜索随机树(Rapidly-exploringRandom Tree)，简写为RRT。它和PRM 一样,也是一种概率算法，主要步骤如下:保持一份机器人的位姿图,其中每个节点对应一个机器人位姿为ξeSE(2),用三维向量ξ~(x,y,0)表示。图中的第一个节点是机器人的某个初始位姿。然后选择一个随机位姿,再从图中找出一个与该随机位姿最接近的节点.--接近程度是用包括距离和方向的成本函数来衡量的。接着计算出一个控制指令，使机器人在一个固定时间段内从“移动到。它达到的点记作,,并加入到图形中。

我们创建了一个规划，并绘制出结果:

rrt.plan();

rrt.plot();

规划图如图5.13所示。可以看到在位姿空间中各个路径都有良好的收敛性，不仅在x和y方向上，还包括姿态，这也是该算法被称为“快速搜索”的原因。

RRT算法一个很重要的部分就是要计算一个控制输人让机器人从起始点行进到，是采用一种更简单的、类似于随机抽样策略的方法。控制器先随机选择机器人前进或后退以及转向角，再模拟出一个固定的时间段内自行车模型的运动，最后计算出到最近的距离。如此重复多次，选择出最佳的控制输入。在其路径上最接近5的一点被选出作为,并成为图中一个新的节点。

用RRT处理障碍问题也十分简单。如果点位于障碍物内部就直接舍去;如果从到5的路径与障碍物相交就不把点加入图中。得到的结果就是这个非完整约束机器人可行驶并不会碰障碍物的一组路径，即路线图。