在本章中,你将利用马里奥机器人来探索你家里的不同房间。在本章的末尾,你将学会利用编程实现简单的机器人沿墙(Following Wall)算法程序,来帮助马里奥沿着墙面行进并避开角落。
本章中讲到的沿墙算法,是反馈控制器(Feedback Controller)的一个例子。控制器是指一个设备(或者说一个软件),来监视和改变一个系统(比如你的机器人)的行为。为了使机器人行进时距离墙面一定的距离,我们需要用到传感器的距离检测结果来作为控制器的一个反馈输入。而驱动机器人行进的集线器电机则是输出端。
本章中你将学习到的前两个沿墙算法程序,利用的是继电器式控制器(Bang-Bang Controller),或者叫做开关控制器(On-Off Controller),之所以这样称呼是因为控制指令(转向角度)只在两个状态之间切换。本章的最后一个程序将会用到比例型反馈控制器(Proportional Feedback Controller),这种控制器将会根据离墙的距离来调整转向角度。
在开始前,我们需要将马里奥的传感器方向向前,如下图所示。

请确保顺时针旋转传感器并听到三下“咔哒”声音,以使传感器的角度向右偏45度,如下图所示。这样能够让马里奥可以探测偏右侧方向的范围。

下面2张图展示了马里奥沿墙移动的轨迹。马里奥将沿着右侧墙面移动,就像之前颜色轨迹线移动一样,马里奥会保持与墙之前的距离,当这个距离达到下限门槛值时,马里奥会向驶离墙面的方向移动,当距离达到上限门槛值时,马里奥会向驶向墙面的方向移动,虽然这会让马里奥的行进路线看起来曲曲折折,但是马里奥与墙面的平均距离基本维持在一个常数,这个距离也能让马里奥躲避墙面和墙角等障碍物。


解决沿墙问题
你也许会注意到,对于机器人编程来说,解决同一个问题往往会有多种方法。在下面的内容里,我们将利用不同的流程模块来不断地优化机器人的沿墙算法。
使用等待模块
在开始我们的挑战之前,让我们先利用伪代码来写一下我们的思路。具体如下
开始无限循环
- 右转(驶向墙面)
- 等待直到检测距离小于8
- 左转(驶离墙面)
- 等待直到检测距离大于8
回到无限循环开头
使用真等待模块
在第8章中,我们利用“真”等待模块来根据特定的判断条件出发程序执行。现在我们要为马里奥实现沿墙行进功能,我们可以利用 “真”等待模块来根据马里奥与墙之间的距离,来给马里奥不同的指令。具体程序实现如下:

程序写好后,把马里奥放在靠近墙体的地方,让墙体在马里奥右侧,执行程序,看看会发生什么?
使用选择模块
你可以利用选择模块,来实现跟上面程序完全相同的功能。程序如下。

上图的程序中,在无限循环(序号1)内部,选择模块(序号2)根据判断条件(传感器探测距离大于8,即上图中序号3、4、5),来执行不同的命令。当探测距离大于8时,执行右转指令(序号6),否则,执行左转指令(序号7)。
你可以能已经注意到了,在上面的程序中我们加入了一个0.2秒钟的延时模块(序号8),这是为了降低序号1的无限循环模块的循环速度,以便让马里奥在收到上一条命令后有一个0.2秒的执行时间(也就是能够按照当前指令行进0.2秒的时间),如果没有这0.2秒的延时,序号1的无限循环模块将连续不间断循环的给马里奥发送指令,这有可能会让马里奥的运行情况产生一定的不确定性。
请注意我们在上面的程序中并没有使用定时限基底移动转向模块,是因为该模块在执行结束后会让电机有一个刹车动作,从而使马里奥的行进看起来非常顿挫。我们在上面的程序中使用的是序号6和7所示的驱动基底移动转向模块,并配合一个0.2秒的延时模块,这样能够让马里奥持续行进,减少行进时的顿挫感。
使用条件“真”启动模块
你还可以使用条件“真”启动模块,来实现上面的沿墙算法功能,如下图所示。

上面的程序中,我们利用一个条件“真”启动模块(序号1)来根据判断条件(检测距离大于8)触发右转指令(序号2),同时利用另一个条件“真”启动模块(序号3)来根据判断条件(检测距离小于9)触发左转指令(序号4)。要开始上图中的程序,需要手动一次点击序号1的条件“真”启动模块和序号3的条件“真”启动模块。
利用数学让沿墙算法程序更顺滑
到目前为止我们编写的几个沿墙算法程序,都是根据距离探测值让机器人在两个方向上切换,但是这样就会导致机器人的移动不够平稳顺滑。为了让机器人的移动更加顺滑,我们将借助数学模块,让机器人的运动方向根据距离探测结果进行调整。
关于数学模块的说明参加 https://www.legofind.com/archives/1618
我们将借助数学模块,根据传感器的距离检测值来计算出机器人的移动方向。
比例转向
下图所示的程序,将根据不同的距离探测值,将机器人的转向角度调整为不同值。

在上图程序中,当机器人与墙面的距离适中时,程序将仅仅微调机器人的转向角度。但是当机器人与墙的距离过大或过小时,程序将加大转向角度的调整力度。
工程师们将上面的算法称之为比例控制器,因为其根据输入值成比例的调整输出指令。当机器人与墙的距离过大或过小时,程序将加大转向角度,当机器人与墙面的距离适中时,程序将减小转向角度。 上面程序中将传感器距离检测值减去7,再乘以10,是为了给机器人一个适合的转向角度。当机器人距离墙面过近时,程序还有应急处置方案。

数学运算模块是实现上述程序的关键。首先我们利用一个减法运算模块来计算当前实际距离Y与期望理想距离R的差,将这个差值记作E。
E = Y – R
Y的取值范围是0至10,R我们设置为7,那么E的取值范围就是-7至3。
但现在有个问题,电机控制模块里的转向值输入范围是-100至100,而上面E的取值范围-7至3,如果直接作为转向输入,显示是太小了,几乎无法影响机器人的转向控制。为了通过E的值能更好的控制机器人的转向,我们需要将其乘以一个我们称之为增益(Gain)的常数K,所得结果也就是电机转向指令的输入U,即
U = K × E
我们将K的值设置为10,那么电机转向指令的输入值U的取值范围就变成-70至30。
此时,机器人离墙的距离与期望值偏差越大,转向指令的输入也就也大。在电机控制模块后面我们加入了一个0.2秒的延时模块,作用与之前一样,是为了防止电机的控制指令输出的过于频繁。
调整R和K的值
你可以对机器人的期望离墙距离R进行调整,来控制机器人运行轨迹与墙体的距离。此外你还可以调整增益K的值,增益K能够决定机器人的在行进中的反应,如下图所示。

正如上图你看到的,增益值K越大,机器人的反应越灵敏,其行进轨迹也就越不平顺。而增益值K减小,会降低机器人的转向控制力度,其行进轨迹会更加平顺,但是对机器人的控制就会变弱,特别是在靠近墙角时,有可能因为转向不及时导致与墙体碰撞。
在调整增益值K的时候,你就需要在行进轨迹顺滑度和反应灵敏度上进行权衡。
我们的沿墙算法还有一些局限性。首先,机器人只能沿着右侧的墙体行进,当然,你也可以通过调整传感器方向和程序,来让机器人沿着左侧的墙体行进。其次,一旦机器人离开墙体过远,程序将失效,由于探测不到墙体的有效距离,机器人将原地转圈。
避开墙角
按照目前的K和R的设定值,机器人应该可以避免其右轮被墙面卡住,但是当来到墙角的时候,目前的比例控制可能不足以让机器人完全避开墙角,因为在墙角处机器人很可能在转向指令足够避开墙角前,已经撞上墙角了。
为了解决这个问题,我们用一个选择模块,来启动一组专门用于逃离墙角的程序,启动条件是探测距离小于3。

我们利用两个电机模块来使机器人在遇到墙角左转时,能够退后并远离墙角,从而使机器人与墙角之间保持足够的距离,来让我们的比例控制算法继续有效执行。