基于有限状态机（FSM）的多传感器信息融合在轮式运动机器人编程中的应用∗

徐 伟

（江汉大学工程训练中心 武汉 430056）

1 引言

智能机器人是集成了环境探测与数据采集、实时动态决策与规划、行为控制与执行等多功能于一体的综合系统。其中包含了传感器技术、信息处理技术、计算机、自动化控制以及人工智能等多学科的专业知识，是机电一体化技术的综合表现形式，是目前科学技术发展最活跃的领域之一［1］。智能移动机器人按移动方式可分为轮式运动机器人、步行移动机器人（单足、双足或多足）、蛇形机器人、履带式移动机器人、爬行机器人等。其中，轮式运动机器人作为一门新兴的综合技术，广泛地应用于社会生产、生活、军事、太空等技术领域，特别是取代人应用于复杂、恶劣的工作环境，具有良好的民用和军用应用前景［2］。

国外科研机构对移动机器人的研究开始于20世纪60 年代。1966 年至1972 年间，美国斯坦福研究院（SRI）的Nils Nilssen 和Charles Rosen 等成功研制出了名为Shakey 的地面自主移动机器人，标志着机器人研究的开始。在我国，移动机器人的研究在“七五”开始起步，国家“863”计划中的遥控驾驶防核化侦察车的立项确定，标志着移动机器人研究的开始［3～4］。本文将以轮式运动机器人为模型展开论述。

2 单个传感器信息处理的编程思路

高度的智能化是机器人技术的必然发展趋势，机器人对外界环境的感知完全通过不同类型的传感器来实现，所以传感器技术则是实现机器人智能化的基础。但是在机器人实际工作过程中，单个传感器获得的信息非常有限，而且还要受到传感器自身性能的影响，因此智能机器人通常装备数量较多的传感器［5～7］。对于多传感器机器人编程的时候，状态罗列的越全，未来机器人的bug 越少。

如果对于多传感器轮式运动机器人的控制采用单个传感器信息处理的方式，那么一般对于传感器触发不同情况可以用条件判断分支流程去予以判断，对于机器人所探知的不同传感器信息，其自身的运动行为也采用相对应的运动方式子程序模块，比如前进、后退、左转、右转、停止等。这时机器人程序的一般思路是：

如果（机器人的某个传感器触发了）

｛机器人的某几个电机做个什么事；做多久；｝

如果（机器人的另外某个传感器触发了）

｛机器人的某几个电机做个什么事；做多久；｝，以此类推。

这种方式是一种包容体系结构（subsumption architecture）。在这种体系结构内，处理器按照程序编写的顺序，轮流处理每个单独的传感器信息，可以人为手工划分传感器等级，甚至某些特定的传感器可以否决或抑制其余的传感器［8］。比如寻光-避障机器人，可以人为地将寻找光源（即确定光源目标）信息作为第一级传感器信息，而障碍探测传感器信息放在其次。但这种方法的问题在于不能很好地处理一些错综复杂的情况，在编程过程不可避免要用到大量的if 语句，以带有2 个传感器的机器人运动判断分支流程为例，程序用C 语言伪码写出来就是：

if｛Sensor（端口a，触发）｝

｛Motor（L，顺）；Motor（R，逆）；Delay（5）；｝

if｛Sensor（端口b，触发）｝

｛Motor（L，逆）；Motor（R，顺）；Delay（5）；｝

else｛Motor（L，停）；Motor（R，停）；｝

由上例可以看出，单个传感器信息处理的编程思路主要有以下弊端。

首先，上面的例子仅仅是两个传感器，仅仅列举了a 传感器和b 传感器单独触发时的运动情况，没有包含如a、b 传感器仅触发其中一个或者两者同时触发的情况，这必然会对后期机器人运动过程产生影响。但是考虑各种情况必然要求在当前if语句中另外增加else if 语句，大量的if 语句使程序的执行效率变得非常低下，不仅程序语言编写复杂、可读性差，而且经常出现识别不灵的情况。

其次，轮式机器人多传感器的综合，状态数量按2 的N 次幂增加，即在3 个传感器的情况下，不同传感器触发的组合有23=8；4个传感器的情况下，不同传感器触发的组合有24=16；5个传感器的情况下，不同传感器触发的组合有25=32……。如果按照传统思路对各传感器信息进行分析，这样一方面使机器人程序编译的时所需要考虑的状态呈几何级数增加；另一方面对机器人控制器硬件的要求也相应提高，造成成本和资源的增加。

第三，使各传感器之间丧失了整体联系，丢失了经组合后蕴含的有关环境整体特征，造成机器人运行过程中的失控。

3 基于有限状态机思想的机器人编程算法

3.1 传感器信息融合和有限状态机思想的原理

传感器信息融合（sensor fusion）可有效地解决上述问题。传感器信息融合综合运用自动化控制、信息处理、人工智能算法和统计等方面的理论技术，利用信息互补，对各个传感器的信息进行有机地整合，降低或者排除干扰因素，处理那些由不同传感器发出的甚至是可能冲突的信号［9～10］。虽然不同的环境信息非常繁杂性，其特征各不相同，但经过智能系统的处理，将信息进行及时有效地融合后，是完全能够比较精准地反映出某个时刻或某个状态下环境的属性特点［11］。本文将用有限状态机（FSM）的思想来进行传感器数据信息融合。

有限状态机（Finite-state machine）简称FSM，表示有限个状态以及在这些状态之间的转移和动作等行为的数学模型，它反映从系统开始到现在时刻的输入变化的状态，它存储的是关于过去的信息，指示状态变更的是相对应的动作，并且用满足确定使转移发生的条件来描述它［12］。它把复杂的控制逻辑分解成有限个稳定状态，在每个状态上判断事件。由于有限状态机有有限个状态，因此可以在实际中实现［13］。有限状态机的思想可以广泛地应用于机器人多个传感器触发组合状态的程序编程判断中，大大提高检测效率。仍然以带有2 个传感器的机器人在不同状态中，传感器返回值来说明情况，见表1。

表1 传感器返回值转换成二进制数值

由上表容易看出，只要知道传感器的触发状态，就可以利用一定的算法，将不同状态下传感器的信息进行整合成相关的序号信息，就可以按照相对应的序号，利用如switch 语句按图索骥，使机器人能够根据自己接收到的传感器信息组合值采取相应的动作。

其具体算法如下：

首先设置一个变量s，初值为0，用来存储传感器的二进制状态序号，即上表中十进制结果的值。

然后分别有序读取每个传感器阵脚的数值，用左移位“＜＜”运算将每一位值左移，空出来的位用0填补。即控制器先读取的某一传感器的状态数值，用移位运算移动到第二位，空位用“0”进行补足；再读取另一传感器状态数值，两个信息数值间作“或”运算。比如表1 中两个传感器均触发的情况：1 号传感器读取（真）并左移0位，得“01”；2号传感器读取（真）并左移1 位，得“10”；将“01”和“10”作“或”运算，即得“11”，结果即为状态“3”。

算法程序伪码语句如下：

s=0；

for（i=0；i＜2；i++）

｛s=s|digitalRead（pin［i］）＜＜i；｝

switch（s）

｛case 0x00：｛动作0；｝break；

case 0x01：｛动作1；｝break；

case 0x02：｛动作2；｝break；

case 0x03：｛动作3；｝break；｝

3.2 有限状态机思想的多传感器机器人的实例应用

以基于Arduino单片机的躲避悬崖机器人的实例应用来说明。Arduino 单片机是一个开源的单片机电子设计硬件平台，是全球非电子类专业人员所喜欢用来开发的一款单片机硬件控制板。其硬件部分由Atmel AVR 单片机、I/O 接口及相关电路组成。软件部分则包括标准的程序编译器和程序下载器。作为一种新型的集成开发环境，其采用C++或Java语言编程，简单易用且适用面广［14～15］。Arduino 可以直接处理各种传感器接收的电子信号，并将其转换为计算机可以识别的数字信号“0”或者“1”，同样也可以将计算机的数字信号输入到相应的引脚输出口，是指成为驱动输出元件动作的电子信号［16］。

在这里设定的躲避悬崖场景，并不同于传统的防跌落机器人。传统的防跌落机器人，对于机器人而言可能有很大的运动空间，机器人可以毫无顾忌地进行掉头、转向等动作，机器人在平台上的运动可以说是漫无方向性的，所需要顾忌的仅仅是在平台边缘的时候才需躲避边缘、防止跌落。而长堤型赛道由于宽度和机器人自身宽度相差仅为10mm，换言之机器人两边与赛道各相距5mm。当机器人行走在长堤型跑道上时，一方面要按照跑道的方向前进，另一方面还要随时对于运动情况作出判断，采取相应的动作姿态。因为前一个状态的调整过程都有可能使机器人在某一方向上落于跑道下面，使机器人在行驶调姿过程中就跌落。所以机器人需要时刻通过传感器的探测和数据采集，不断调整机器人行驶状态，使机器人防止走空摔落。

3.2.1 机器人行走原理

其运动结构是前轮驱动方式，后两轮是从动轮。其探测感知悬崖边缘原理是在机器人前后左右轮四个方向上都安装探测传感器，当机器人在实地上行走时，传感器所反映的逻辑信号量为“1”；反之，当机器人某个方向上处于悬空状态时，传感器所反映的逻辑信号量为“0”，这样就能通过逻辑信号量的不同而知道机器人目前所处的环境状态。

3.2.2 机器人状态列表分析

在这里设置I3、I4、I5、I6 四个逻辑信号拟输入接口分别对应右前、左前、右后、左后四个方向上的传感器。传感器触发情况、机器人小车行驶状态及对应行为策略表见表2。

3.2.3 主程序设计

int pin［4］=｛6，5，4，3｝；

byte value；

void setup（）｛

for（int i=0；i＜3；i++）

｛pinMode（pin［i］，INPUT）；｝ //对6，4，5，3 四个输入引脚进行定义

pinMode（5，OUTPUT）；

pinMode（6，OUTPUT）；

pinMode（9，OUTPUT）；

pinMode（10，OUTPUT）；｝ //对5，6，9，10 四个输出引脚进行定义；5，6引脚和9，10引脚分别控制左右两个直流电机

void loop（）｛

value=0；

for（int i=0；i＜3；i++）

｛value|=（digital Read（pin［i］）＜＜i）；｝//依次读取相关引脚值，并左移，再与新值作“或”运算

switch（value）

｛case 0x00：

stop（）；

break；

case 0x01：

while（！digitalRead（pin［2］）&&！digitalRead（pin［3］））

｛right（）；｝

break；

…… //具体各对应行为以及直行、左转、右转、停止、后退等子程序就不冗述

case 0x0F：

forward（）；

break；

default：

stop（）；｝｝

表2 传感器触发情况、机器人小车行驶状态及对应行为策略表

4 结语

在设定的长堤型赛道场景下，由于未知环境随机性太强，无法完整获得环境信息，采用有限状态机思想，通过多个传感器信息整合后，精确感知各种情况，使机器人根据相应的判断，采取相应的智能化动作，不仅将分布在不同位置、处于不同状态的多传感器所提供的局部的、不完整的信息量加以综合，能够实现机器人多个传感器触发组合状态的判断，大大提高检测效率；而且使多传感器机器人程序编译相对简化，增强程序可读性。

另外，有限状态机（FSM）思想方法不仅适用于文中所例举的数字量传感器，而且可以将其使用范围延伸到模拟量传感器的信息融合，可以用“C++语言”的“map（）”函数，将模拟量传感器所检测的较大范围内的数值映射到比较小的区间和比较接近的数量（比如无线遥控手柄位置、颜色等，可以用map（）函数映射得到的数值0、1、2来表示），这就可以通过模拟量和数字量的结合产生不同的触发组合状态，只不过情况更加复杂一点，可以在算法上面做一些修改，仍然可以应用有限状态机的思想原理，在这里就不再冗述了。