第七讲机器人距离探测

表（Panasonic PNA4602M）的频率特性图。这个特性图显示了红外线传感器在接收到不同于38.5 kHz 在区域3。也就是说，当机器人发送37500 Hz和38250Hz频率的红外线时能发现物体，发送39500 Hz 、40500 Hz 以及41500 Hz频率的红外线就不能发现物体。如果你移动物体到区域2，那么发送37500 Hz、 38250 Hz以及39500Hz的红外线可以发现物体，而发送40500Hz和41500Hz频率的红外线就不能发现物体。

基础机器人制作与编程

LOOKUP Index, [Value0, Value1, …ValueN], Variable

如果变量Index的取值为0，那么方括号中Value0的值将被赋给变量Variable。如果变量Index 的取值为1，那么方括号中Value1的值将被赋给变量Variable。列表中可以储存256个数值，但对于下面的例程而言，你只需要5个数值。下面是怎样使用它的例子：

FOR freqSelect = 0 TO 4

LOOKUP freqSelect,[37500,38250,39500,40500,41500],irFrequency FREQOUT 8,1, irFrequency

irDetect = IN9

' Commands not shown...

NEXT

在FOR…NEXT 循环语句第一次执行时，freqSelect 的值为0，所以LOOKUP 命令把37500 赋予变量 irFrequency 。因为在执行命令LOOKUP 之后，变量irFrequency值为37500, 所以 FREQOUT 发送该频率到与P8连接的红外线发光二极管上。和前面章节中一样，数值IN9 保存到变量irDetect 。在FOR…NEXT 循环语句第二次执行时，freqSelect 的值为1，所以LOOKUP 命令把38250赋予变量 irFrequency 。第三次重复上述工作时，把39500赋予变量 irFrequency ，依次类推。

例程――TestLeftFrequencySweep.bs2

TestLeftFrequencySweep.bs2 要做两件事情。首先，它测试左边的IR LED/探测器（与P8和 P9连接）以确认它们的距离探测功能正常。然后，它也演示如何完成如图7-2所示的频率扫描。

当你运行该程序时，调试终端将会显示你所测量的区域。会有很多可能的“yes”或“no”

排列模式出现，如图7-3所示是其中的两个。测试模式将由探测器内部的滤波器特性决定。

程序通过计算 “No” 出现的数量，就可以确定目标在哪个区域。注意即使图7-3所示的两个调试终端的测试结果不同，但它们都有三个“Yes”和两个“No”出现，所以在两个例子中“Zone 2”都是探测到的目标区域。

紧记，这种距离测量方法显然不是非常地精确。然而，它为机器人跟随，跟踪和其他行为提供了一个足够好的探测距离的能力。

z 输入、保存并运行程序TestLeftFrequencySweep.bs2。

z 用一张纸或卡片面对IR LED/探测器测试距离探测。

z 开始时把纸片贴近IR LED/探测器，大概1cm距离远。调试终端中显示区域应该为0或1。

z 逐渐把纸片远离IR LED，并记录每一个让zone值增大的距离。

图7-2 机器人红外频率探测区域

基础机器人制作与编程

' Test IR detector distance responses to frequency sweep.

' {$STAMP BS2} ' Stamp directive.

' {$PBASIC 2.5} ' PBASIC directive.

'[ Variables ]--- freqSelect VAR Nib

irFrequency VAR Word irDetect VAR Bit distance VAR Nib

'--[ Initialization ]--- DEBUG CLS," OBJECT", CR,"FREQUENCY DETECTED", CR,"--- ---"

'[ Main Routine ]--- DO

distance = 0

FOR freqSelect = 0 TO 4

LOOKUP freqSelect,[37500,38250,39500,40500,41500], irFrequency FREQOUT 8,1, irFrequency

irDetect = IN9

distance = distance + irDetect

DEBUG CRSRXY, 4, (freqSelect + 3), DEC5 irFrequency DEBUG CRSRXY, 11, freqSelect + 3

IF (irDetect = 0) THEN DEBUG "Yes" ELSE DEBUG "No "

PAUSE 100 NEXT

DEBUG CR,"--- ---", CR,"Zone ", DEC1 distance LOOP

该你了――测试右边的IR LED/探测器

尽管还有许多标注，但你只需通过替换下面两行代码来修改该程序来测试右边的IR LED/

探测器：

图7-3 距离探测测试输出示例

FREQOUT 2,1, irFrequency irDetect = IN0

FREQOUT 8,1, irFrequency

irDetect = IN9 改为

基础机器人制作与编程

z 运行该程序，检验这对IR LED/探测器能否测量到同样的距离。

同时显示两个距离

用一个快捷的程序让机器人的两个探测器同时进行距离探测，有时是非常有用的。下面的程序由子程序组成，这些子程序可以方便的拷贝和粘贴到其它需要距离探测的程序中。

例程――DisplayBothDistances.bs2

z 输入、保存并运行程序DisplayBothDistances.bs2 。

z 用纸片重复对每个IR LED进行距离探测，然后对两个IR LED同时进行测试。

' --[ Title ]--- ' DisplayBothDistances.bs2

' Test IR detector distance responses of both IR LED/detector pairs to ' frequency sweep.

' {$STAMP BS2} ' Stamp directive.

' {$PBASIC 2.5} ' PBASIC directive.

' --[ Variables ]--- freqSelect VAR Nib

irFrequency VAR Word irDetectLeft VAR Bit irDetectRight VAR Bit distanceLeft VAR Nib distanceRight VAR Nib

'--[ Initialization ]--- DEBUG CLS,"IR OBJECT ZONE", CR,"Left Right", CR,"--- ---"

' ---[ MainRoutine ]--- DO

GOSUB Get_Distances GOSUB Display_Distances LOOP

'--[ Subroutine – Get_Distances ]--- --- Get_Distances:

distanceLeft = 0 distanceRight = 0

FOR freqSelect = 0 TO 4

LOOKUP freqSelect,[37500,38250,39500,40500,41500], irFrequency FREQOUT 8,1,irFrequency

irDetectLeft = IN9

distanceLeft = distanceLeft + irDetectLeft FREQOUT 2,1,irFrequency

irDetectRight = IN0

distanceRight = distanceRight + irDetectRight PAUSE 100

NEXT RETURN

'--[ Subroutine – Display_Distances ]--- Display_Distances:

DEBUG CRSRXY,2,3, DEC1 distanceLeft, CRSRXY,9,3, DEC1 distanceRight

RETURN

该你了――更多的距离测试

z 尝试测量不同距离的物体，弄清物体的颜色和（或）材质是否会造成距离测量的差异。

基础机器人制作与编程

本任务要你让一个机器人跟随另一个机器人行走。跟随的机器人，也叫尾随机器人，它必须知道距离引导车有多远。如果尾随机器人落在后面，它必须能察觉并加速。如果尾随机器人距离引导车太近，它也要能察觉并减速。如果当前距离正好合适，它会等待直到测量距离变远或变近。

距离仅仅是机器人和其它自动化机器需要控制一种变量之一。当一个机器被设计用来自动维持某一数值，比如距离、压力或液位时，它一般都包含一个控制系统。这些系统有时由传感器和阀门组成，有时由传感器和电机组成。在本书使用的机器人里面，控制系统由传感器和连续旋转电机组成。除此之外，控制系统还必须有某些处理器可以接受传感器的测量结果并把它们转化控制指令输出给电机，由电机转化为机械运动。你必须对处理器编程来基于传感器的输入作出决定，从而控制机械输出。对本书使用的机器人而言，处理器就是BASIC Stamp 2 。

闭环控制是一种常用的维持控制目标的方法，它很好地帮助机器人保持与一个物体之间的距离。闭环控制算法类型多种多样，最常用的有滞后、比例、积分以及微分控制。所有这些控制方法都将在后续教材《过程控制》中详细介绍。

绝大部分控制算法都可以用很少的几行PBASIC代码来实现。事实上，如图7-4所示的比例控制环的主要部分可以精简为一行PBASIC代码。这个图称为方框图，它描述机器人用到的比例控制过程的步骤，即机器人用右边的IR LED/探测器探测距离并用右边的伺服电机调节机器人之间的位置以维持适当的距离。

请你仔细观察一下图7-4中的数字，学习一下比例控制是如何工作的。这个特殊的例子是右边的IR LED/探测器和右边的伺服电机的比例控制方框图。设定位置为2，说明你想机器人维持它和任何它探测到的物体之间的距离是2。测量的距离为4，距离太远。误差是设定值减去测量值的差，即2 - 4 = - 2 ，这在圆圈的左方以符号的形式指出，这个圆圈叫求和点。接着，误差传入一个操作框。这个操作框显示，误差将乘以一个比例常数Kp。Kp的值为35。该操作框的输出显示为–235 = –70，这叫输出校正。这个输出校正结果输入到另一个求和点，这时它与电机的零点脉冲宽度750相加。相加的结果是680，这个脉宽可以让电机大约以3/4全速顺时针旋转。这让机器人右轮向前、朝着物体的方向旋转。这个修正值加入到由机器人、以及距离为4的目标物体组成的整个系统中，

第二次经过闭环，测量距离可能发生变化，但是没有问题，因为不管测量距离如何变，这个控制环路将会计算出一个数值，让电机旋转来纠正任何误差。修正值与误差总是成比例关系，

该误差就是设定位置和测量位置的关系的偏差。

图7-4 右边伺服电机及 IR LED/探测器的比例控制方框图

基础机器人制作与编程

下面是从方框图中归纳出来的方程关系及结果：

Error = Right distance set point – Measured right distance

= 2 – 4 Output adjus = error ⋅ Kp

= –2 ⋅ 35

= – 70

Right servo output = Output adjust + Center pulse width

= – 70 + 750

= 680

通过一些置换，上面三个等式可被简化为一个，提供你相同的结果：

Right servo output = (Right distance set point – Measured right distance) ⋅ Kp + Center pulse width

代入数值，你可以得到一致的结果：

Right servo output = ((2 – 4) ⋅ 35) + 750

= 680

左边的IR LED/探测器以及左边的伺服电机的控制框图如图7-5所示，与右边的运算法则类似。不同的是比例系数Kp的值由35替换为-35。假设与右边的测量值一样，输出修正的脉冲宽度应该为820。下面是该框图的计算等式：

Left servo output = (Left distance set point – Measured left distance) ⋅ Kp + Center pulse width

= ((2 – 4) ⋅ –35) + 750

= 820

这个控制环的值让电机大约以3/4全速逆时针旋转。这个对机器人的左轮来讲是一个向前旋转的脉宽。反馈的意思是，系统的输出被尾随机器人重新采样做另一个距离探测。控制环一次又一次的重复运行，大概每秒40次。

对尾随机器人编程

记住右边伺服电机的输出方称为：

Right servo output = (Right distance set point – Measured right distance) ⋅ Kp + Center pulse width

下面的例子说明如何用PBASIC语言求解上面的方程。右边距离设置为2，测量距离由变量 distanceRight存储，Kp为35，零点脉冲宽度为750：

pulseRight = (2 – distanceRight) * 35 + 750

图7-5 机器人左伺服电机及 IR Led/探测器的比例控制方框图

基础机器人制作与编程

pulseLeft =(2 – distanceLeft) * (-35) + 750

既然数值 -35, 35, 2, 和 750 全都有命名，干脆对这些常数声明如下：

Kpl CON -35 Kpr CON 35 SetPoint CON 2 CenterPulse CON 750

由于程序中有这些常数声明，你可以用Kpl代替-35，Kpr代替35， SetPoint 代替2，CenterPulse 代替750。在常量声明之后，比例控制计算式现在看起来象这样：

pulseLeft = (SetPoint – distanceLeft) * Kpl + CenterPulse pulseRight =(SetPoint – distanceRight) * Kpr + CenterPulse

声明变量后者常量的便利在于，你只需在程序的开始部分对变量做一次改变。程序的开始部分的修改会反映到所有你用到该常量的地方。例如把Kpl CON指令由-35改为-40，那么程序中所有Kpl的值都会由-35更改为-40。对于左、右比例控制系统的调试试验来讲，这是非常有用的。

例程―FollowingRobot.bs2

FollowingRobot.bs2重复刚才讨论过的各个伺服脉冲比例控制环。换句话说，在每个脉冲发送之前，需要测量距离，决定误差信号，然后将误差值乘以比例系数Kp，再将结果加上（或减去）发送到左（或右）伺服电机的脉冲宽度值。

z 输入、保存并运行程序 FollowingRobot.bs2。

z 把大小为8.5×11英寸的纸片置于机器人的前面，就象障碍物墙。机器人应该维持它和 ' FollowingRobot.bs2

' Robot adjusts its position to keep objects it detects in zone 2.

' {$STAMP BS2} ' Stamp directive.

在文檔中基础机器人制作与编程 (頁 91-103)

第七讲 机器人距离探测