【转】移动机器人控制软件的设计与实现

移动机器人的应用现在非常多，做一个移动机器人似乎也很容易，车体自己可以加工，也可以去买现成的；避障可以用超声阵列；导航可以用激光测距LMS；定位可以用电子地图加LMS加陀螺仪；驱动可以用各种电机及配套驱动器或者自己做；通讯可以去买现成的无线通讯模块，可以是数字的，也有模拟的；大范围定位可以用GPS模块，也是现成的；至于什么红外，蓝牙，甚至计算机视觉都可以去市场上买，但是（然而）为什么做一个移动机器人还是这么难呢？尤其是对一个新手而言。一个老外说过，硬件是现成的，软件算法杂志里有的是，很多可以在网上当，但即使是一个博士生也要花费很长的时间完成一个实际可用的移动机器人。为什么？因为机器人应用的困难在应用软件的设计上。前面那个老外也说过，现在什么都可以在网上当，唯独应用程序不能。有过自己写移动机器人程序的人可能会理解这段话，当然也仅仅是可能，因为不排除有很多机器人大拿一上来就可以写出很棒的移动机器人软件。移动机器人的控制软件开发是和硬件紧密相关的，甚至和机器人的体系结构也密切相关，同样是移动机器人，有的是用PC控制的，有的是用多个嵌入式系统实现的，有的则是多机器人协同工作的，操作系统有人会用DOS，有人会用Windows，有人会用Linux，有人会用Embeded Operation System。硬件平台有的用x86，有的用ARM芯片，有的会用DSP，通讯里面会有串口，TCP/IP网络，无线以太网，红外，蓝牙等，甚至驱动机构也不一样，有的是用腿，有的是用轮子，即使是轮子也有好多种情况，两个轮子，三个轮子（包括两个独立驱动轮一个随动轮，或者两个同步驱动轮一个转向轮等），四个轮子，五个轮子等等，有的是集中控制的体系结构，有的是分布式控制的体系结构。应用也不同，有智能行走的，有人远程控制的，也有人机结合的，诸如此类，这样看来，移动机器人应用程序的开发的确是很复杂，“无法从网上直接当了”，看来只能自己从头开始编写代码了这一条路好走了。的确如此，但自己从头编程序也存在一个问题，那就是如何着手。我们写文章希望有模板，同样写机器人应用程序也希望能有一个模板，按照模板的约定，把自己编写的模块嵌进去，就可以得到一个应用程序了。可惜程序设计里面的模板无法移植到机器人程序设计领域，但是我们可以采用一个框架，在这个框架的规范下，我们写的模块或者利用他人写的模块进行组合就可以得到一个合理的应用程序。框架的概念很广，可大可小，大的可以做到像J2EE或者CORBA那么复杂，做企业级开发，小的则只能在一个CPU上构件一个简单的应用程序。我在开发移动机器人的时候设计了一种简单框架，专门用来给采用PC对机器人进行集中控制的情况下编写应用程序。在我的博士论文（在个人主页里可以找到）里则设计了一种较复杂的框架，可以采用P2P技术实现多机器人或多系统的软件应用开发，而我现在正打算设计一种更通用，更复杂的，可以实现多平台，多CPU，多系统环境下的移动机器人应用快速开发，包括框架和中间件技术。这里只是先介绍一下目前正在使用的那个PC用的软件框架。值的一提的是，国内外已经有很多人在做或者已经做过这方面的工作了，我写在这里的目的只是为了和大家交流。由于最近时间很紧张，每次只能写一小点，所以打算分数次把这部分内容介绍完，主要包括框架的模型，模块的接口定义，模块之间的通讯，动态模块的加载和配置，以及采用该框架实现的移动机器人应用举例，并提供相应的框架的源代码。

1、引言

我们初次编写机器人软件，通常有种“老虎啃天，无处下嘴”的感觉。编写移动机器人程序和普通程序稍有不同，因为通常意义上的移动机器人程序都是多任务，实时的，甚至是分布式的。我们可以很容易的在计算机终端上显示出“Hello，world”，也可以毫不费力的在移动机器人的液晶屏上显示出“Hello, World”，但是一边让机器人行走，一边要避障，同时还要处理通讯、定位等工作就决不是那么容易了。这个时候我们需要的就是框架了。

2、面向移动机器人的应用程序框架

2.1、什么是框架？

我的理解中，软件里框架的概念很有点像计算机硬件里面的主板，它提供了系统运行所需要的电源，通讯等手段，并且所有符合总线协议的卡都可以往主板上插，从而实现所需要的功能和性能。例如把数据采集卡查到主板上，计算机就具有了数据采集的能力；把视频卡插到主板上，计算机就具有了压缩视频的功能，此外还有显卡，声卡，网卡，电视卡，运动控制卡，传真卡等等。软件里的框架也是如此，它有点类似中间件的概念，提供了相当程度的代码支持，同时也定义了一些规范，这些规范包括通讯协议，模块的接口定义等。用户可以自己编写模块，也可以使用别人已经写好的模块，通过框架的运行，带动模块的运行，从而实现一些预期的功能。由此可见，框架必定是一种集成工具，其自身是一个完整的应用程序，而且可以通过模块来扩展应用系统的功能和改进其性能；或者说，框架提供了应用程序的入口和出口，并能够根据配置，加载相关的模块实现功能的扩展和性能的提高。其中，功能的扩展通过编写新的模块实现；性能提高则通过对原有模块进行升级实现。框架和体系结构之间存在着微妙的关系，一方面可以说框架和体系结构无关，例如同样用MFC，可以开发出各种体系结构的软件系统来；另一方面，框架和体系结构又结合的那么的紧密，例如J2EE是一种框架，而看到J2EE就令人想起了分布式的体系结构。CORBA亦是如此。主板和体系结构之间的关系就更令人弄不清了。因此我认为执着的追求框架和体系结构的区别是没有意义的，关键在于如何设计框架，使它更好用，效率更高。

2.2、框架的设计

框架设计的好坏对系统性能的影响当然非常重要，应当仔细考虑，全面分析，更重要的是能够在实践的过程中不断的进行改进。目前我设计的这个软件框架比较简单，适合比较简单的应用，当然可以在此基础上进行进一步的完善，使之能够适应更复杂应用的开发。

1、R.A. Brooks的包容体系结构和分层递阶体系结构

首先让我们来看看目前移动机器人所主要采用的体系结构。移动机器人目前所采用控制软件的体系结构可以分为三类：一类是分层递阶式结构，一类是包容体系结构，最后一类是上述两种结构的综合。在分层递阶体系结构中，它把各种模块安排成若干个层次，使不同层次的模块具有不同的工作性能和操作方式，一般说来，位于高层的模块负责复杂的判断、推理等操作，其智能化程度较高；而较低的层次用于与外界交互，具有较强的实时响应能力，因此在底层往往是一些视觉，驱动，传感，伺服等工作模块。其中具有代表性的是Saridis的三级模型，即执行级，协调级和组织管理级等。分层递接体系结构主要特点是层次向上，智能增加，精度降低，实时性降低；层次向下则相反。分层递接结构能够较好的解决智能和控制精度的关系，因此获得了广泛的应用，但也有一些明显的缺陷，最主要的是它的反应性极差，因为处理问题的能力主要由高层解决，需要自下而上然后自上而下的来回过程，从而失去了高度智能性的实时反应能力。为了解决这个问题，R.A.Brooks提出了包容体系结构。包容体系结构结构图如下图所示：与分层递接结构相比，包容体系结构是一种水平分片的结构，它的主要特点是按照“任务和行为”进行分类，把系统分解为若干个子系统，每个子系统不是技术功能模块，而是能够独立产生动作行为的系统；每个子系统都能够直接接收传感器信号，也可以直接产生行为动作；每个子系统平行工作，由一个协调机制负责集成，从而产生总体的行为。该体系结构设计目标为多任务，鲁棒性，可扩展性以及较强的可判断性。其基本设计思想来源于Brooks对智能的理解，即他认为能产生复杂行为的机器内部其控制系统不一定是复杂的，复杂的行为仅仅是复杂环境的反射；一个自主式智能系统必须具有永久的生存能力等。Brooks采用包容式体系结构，构造了多种机器人，这些机器人能够实现其设计目标，表现出较强的智能行为，因此包容式体系结构在智能机器人领域产生了极大的影响，对传统人工智能（基于符号和推理）产生了强烈的冲击。但包容体系结构也存在不足之处，那就是功能层次之间缺乏协调和组织，尽管在局部行为上表现出灵活的反应能力和鲁棒性，但对长远目标和全局性的目标跟踪缺乏主动性；而且限制了对知识的运用，包括人的知识。因此，最近许多研究人员提出了一些新的体系结构，基本上是在包容体系结构的基础上集成了传统人工智能的方法，或者以分层递接为骨架，在每一层次上引入了类似包容体系结构的布置，提高每一层次的响应能力。我把此类体系结构归于第三类。分析分层递接结构与包容体系结构，不难发现，两者都是基于模块的基础之上构建的，不同之处在于分层递接结构中的模块是功能模块，而包容体系结构中的模块是行为模块。但即使是行为模块也是由一些功能模块组合实现的，因此模块实际上是所有体系结构的基础，不同之处就在于如何组织这些模块，使其能够发挥其在整个智能机器人体系中的作用。本项目在借鉴包容体系结构的基础上，结合多智能体技术，提出了一种基于模块协同的体系结构。在此体系结构中，模块既可以是功能模块，也可以是行为模块，对模块的唯一要求是其必须能够主动的对输入的信号进行处理，并产生响应（类似智能体，即Agent）。体系结构如下图所示

2、移动机器人控制软件框架的设计

框架的设计一方面考虑了对体系结构的支持，另一方面又打算尽量与体系结构保持一定的独立性，所以对框架做了高度的抽象，整体框架被抽象成了三个部分，即Platform平台、Module模块和Wire通讯连线等。整个框架的结构（部分）如下图所示。
400){this.resized=true;this.style.width=400;}" resized="true">其中Platform类负责模块的加载，对模块的配置以及模块之间通讯的建立，Platform对象是整个应用程序的入口（有Main函数）和出口，并不考虑体系结构和机器人功能的实现，这些都是模块和配置实现的。Module定义了模块应该遵守的接口，和模块要实现的基本功能。模块和Applet类似，有初始化init，开始start，停止Stop和退出Destroy等行为，该接口没有定义模块应该实现哪些功能，这些是模块的具体实现应该考虑的，模块里有两个很重要的变量，分别是该模块的ModuleDoc和端口Ports。ModuleDoc描述了该模块的具体配置信息，例如和硬件通讯时使用的串口名称，或者是网络地址，IP地址，也可能是其他需要传递给具体模块的参数，其中最重要的是模块的名称，用来在系统中唯一的标识模块用的，不能重复；其次是该模块的实现类，供Platform在加载该模块的时候实例化所需。端口Port是模块必须具备的，一个模块至少有一个端口，端口分为输入端口和输出端口，是模块之间通讯的唯一途径。端口也是一个接口，符合该接口的任意实现类都可以作为端口来供模块使用，目前端口是硬编码在模块的代码中的，因此一旦模块编码完毕，使用何种端口也就确定不可更改了。在后续研究中可以考虑把端口与模块的实现分离开来，在系统运行时动态组合，实现更灵活的配置。下图是模块的一个典型配置。模块实现的基本操作是根据输入的数据进行处理后，从输出端口把数据输送出去，当然数据处理的过程可能很复杂，例如可能是图像识别，也可能是障碍物检测，有些还需要进行网络数据库等操作等；有些模块没有输入端口，例如一些人机接口模块（操作面板）；也有些模块没有输出端口，例如图形化显示模块等。整个软件的功能是由模块决定的，确切的说是由模块之间的协作实现的，模块之间的协作，则是利用端口之间的通讯实现的。在本系统中，端口之间通过虚拟的“线”连接起来的，在系统中用WireConfig来表示，线从输出端口引出，可以连接至多个不同的输入端口；不同线之间可能会存在着交点，根据不同的方式，交点有不同的类型，如下图所示。在输入端口的交点称之为抑制连接，即抑制线输入来的信号可以取代原线上的信号输送到输入端口里去，在输出端口的交点称之为禁止连接，即如果禁止信号线上有数据，则会禁止原信号线上的信号输出。这些概念均取自Brooks的包容体系结构。在Brooks的包容体系结构中，模块之间的通讯是异步的，分层的。在同一功能层内模块的数据是直接发送的；在不同层之间数据则是通过抑制（Inhibit）和禁止（Suppress）进行传递的。这种通讯机制最大的优点就是鲁棒性。即上层模块工作时，可以利用抑制取代原有底层模块的输出；当上层模块失效时，抑制功能丧失，下层模块的输出重新有效，从而避免因模块失效带来的整体功能失效，保证了机器人的基本安全。在我们设计的框架中，同样采用了抑制和禁止这两个通讯功能，不同的是没有分层，而是根据设计者的意图来确定哪些通讯直接相连，哪些通讯之间要互相抑制和禁止。通过这三种通讯方式实现模块之间的协作，实现各种自主智能功能。其实在一般使用当中，模块之间基本上都是直接线连，出现交叉的情况很少，这种情况的设计主要是为了有软件鲁棒性考虑的应用设计的。例如模块A、B都连接到模块C上，而B优先级大于A，则B－A连接属于抑制连接，当B有信号的时候，会取代A的信号输入到C中，当B没有信号的时候（例如出现了故障，或者没有输出），A的数据仍然可以输入到C中。2.3 框架的实现前面分析的主要是思想，这部分讲述的是实现方法。由于本人一直在用Java语言做开发，所以这里也选用Java语言作为该框架的实现语言，但原则上讲用C语言来实现更好，但C语言面向对象特性不好，而且编程的难度大，更不能跨平台，而我的项目所剩时间无多，所以最终开始确定用Java开发。事实证明Java开发的效果也还不错（在实验过程出现了响应缓慢的症状，其原因在于导航的算法，而非语言的问题），而且更可以把某些实时性要求比较高的模块用JNI来实现，速度就更有保障了

400){this.resized=true;this.style.width=400;}">。（1）框架的结构框架由一个平台对象Platform和一个系统配置文件构成，这两者之间的关系，打个比方，配置文件好比一个设计蓝图，描述了整个框架里应该加载多少模块，每个模块的加载信息，以及模块之间的通讯线路等；而Platform对象就像一个装配车间，按照图纸的要求，把模块加载进来，然后建立模块之间的通讯线路。其中，Platform的编程比较容易，因为Java虚拟机能够实现类的动态加载，而且读取XML文档的能力也很强。实现模块的动态加载代码如下（有删改）：

/**
* loading module from xml configuration
* @param md - ModuleDoc, the robot configuration file
* @throws Exception - Something wrong, check the message
* @return the Module Object that has just been generated
*/
protected Module loadModule(ModuleDoc md) throws Exception
{
String cn = md.getClassName(); //读取XML文件中的类名称
if(cn == null || cn.length() == 0)
{
throw new Exception("Wrong Configuration for Module");
}
Object m = Class.forName(cn).newInstance(); //创建模块对象
if(m instanceof Module)
{
return (Module) m;
}else{
throw new Exception("Failed loading module: " + cn);
}
}

加载模块的启动如下（有删改）

/**
* booting the platform,
* install all the modules and make the system working.
*
* @throws Exception
* @see ＃c onfig(String)
*/
public void booting() throws Exception
{
Iterator iterator = confdoc.getModuleDocs();
while(iterator.hasNext())
{
//Loading modules...
ModuleDoc md = (ModuleDoc) iterator.next();
Module m = loadModule(md);
//initialize the modules
m.init(md);
//put it in the module hash
module_hash.put(m.getName(), m);
}

//start the modules
iterator = module_hash.values().iterator();
while(iterator.hasNext())
{
Module m = (Module) iterator.next();
m.start();
logit("Module " + m.getName() + " has been started.");
}
}

模块之间的通讯信息则存储在对象WireConfig的集合中，该类存储了从源模块某端口出发的通讯线路，描述了该线路如何与其他模块的什么端口进行连接，这样在模块发送数据时，可以动态的到该集合中搜索与自己关联的通讯线路，并根据该线路的描述，寻找到可以写入的模块和端口。这些工作都是由端口Port完成的，模块本身无需干预。输出端口发送数据的实现代码：

public void setValue(Module module, Object v, long duration)
{
//{{{1
/*judge if output is disabled*/
if(i_flag && ((System.currentTimeMillis() - timestamp)

　


400){this.resized=true;this.style.width=400;}" resized="true">

(2) 通讯模块的设计和控制原理

在远程控制中，通讯模块的设计非常重要，一方面所有的数据都要通过通讯模块在客户端和服务器之间传递，另一方面在网络控制中的时延会对控制系统的稳定性造成一定的影响，应该尽量予以消除。为了提高远程控制系统的应用性，我们在该系统中采用了TCP/IP协议，该协议是Internet上的通用协议，使用该协议，理论上就可以通过任何接入Internet的计算机来控制移动机器人。为了进一步提高它的开放性，我们采用了HTTP协议，利用超文本来传递数据和控制命令。使用HTTP协议的另一优点是目前几乎所有的网关代理都支持HTTP代理，这样使得该系统的应用范围更广。



a)客户机/服务器模式的通讯模块设计

为了简化网络通讯的实现，增加代码的重用性，在本系统中我们采用了系统架构中模块的思想，构建了网络数据通讯模块。该模块分客户端和服务器两个模块，遵循客户端/服务器架构，可以根据需要配合使用。通讯模块的工作流程如上图所示。该模块是一个通用的网络数据传输模块，使用的时候只需要在其模块的Data端口上写入要发送的对象数据，或者从Data端口读入欲获得的对象（下图）。在使用该模块的时候需要在配置文件中配置好该模块所使用的网络端口号，或者欲连接的网络IP地址和端口号等参数。



b)基于XML技术的数据描述和命令封装

在上一部分中，对象能够通过网络通讯模块在不同的主机之间进行传送，这里的对象主要是从传感器获得的数据对象，或是从客户端发送的控制命令。这些对象或者命令都必须要转化成网络能够传递的文本或者字节流才能够通过网络进行传递。由于我们所使用的是HTTP（超文本传输协议），这种协议最适合文本数据的传输；此外，XML具有良好的数据表达能力和数据标准化，以及其开放型等优点，因此在数据传输的时候我们使用XML技术来实现对对象的文本序列化。

序列化是面向对象编程的一种技术，指的是对象能够在需要永久保存或者传输的时候转化成相应的数据，之后可以从该数据中恢复出该对象。对于一个对象而言，其方法是所有对象都拥有的，因此只有其属性代表的数据才能够确定对象的唯一性，所谓的序列化实质上就是研究对象的属性数据的保存和读取的一种方法。XML具有很强的数据表达能力，能够轻而易举的描述任何对象的数据，因而可以用来做为对象的序列化媒介。

利用XML实现对象序列化的方法很简单，我们只需要在所需要进行序列化的对象里添加一个方法toXMLDoc()，并在该方法中根据对象该时刻数据生成一个XML文档即可。关于如何实现XML文档的生成可以参考有关XML编程的书籍。从XML文档中恢复对象包括两个步骤：首先根据XML文档的描述创建所需要的对象，然后利用该对象的fromXMLDoc()方法恢复数据。

例如LMS200激光雷达系统扫描得到的数据对象经过序列化后可以得到的XML文本为：
[code]

35-34-35-56-78-98-120-......-380

该文本描述了激光雷达一次扫描后获得的全部前方0～180度范围内测量的距离信息。该信息在程序内被封装到了一个对象中（RangeData），该对象序列化后得到该XML文本。同样，通过生成一个空的RangeData对象，然后利用该XML文本即可重新恢复出该对象。c) 局部自主智能的实现由于我们在移动机器人上安装了激光雷达系统和超声传感器阵列，并对相应的避障和导航算法等单元技术进行了研究，因此该移动机器人已经具备了一定的智能（例如能够判断障碍物的距离实现急停，局部避障等功能）。但是由于人工智能的发展现状以及机器人所处环境的复杂性，完全依赖机器的智能在现实环境中自主行走就目前而言还没有可靠的技术保证。因此在实际应用中还不能避免人工的干预，这就要求我们从人机结合的角度来看待这个问题，尽量能够开发出人机协同控制移动机器人的环境。在我们开发的软件框架内，所有模块的输出最终都通过影响电机的驱动模块来体现出来（例如防碰撞，局部避障等），而人（远程操作者）对机器人的控制同样也是通过影响电机的驱动模块而实现的，这与所有模块的工作原理是一样的。因此，通过对每个行为模块指定不同的优先级别，就可以在一定程度上实现对移动机器人行为的人机协同控制。在本项目中，我们把远程命令的处理模块赋予较高的等级（高于避障模块，但是低于防碰撞模块，这样可以解决操作过程中的误操作导致的碰撞过程），当有远程控制命令过来的时候，机器人由远程控制人员进行操纵；当远程命令失效（例如1秒内没有新的命令进入）后，本地避障、漫游等模块的控制命令就可以对机器人进行控制，在控制的同时，用户仍然可以通过远程计算机进行对机器人的运行状态进行监视并随时切入控制。下图显示的就是机器人在遥操作过程中客户端显示的激光雷达数据（可视化）和机器人运行状态数据以及控制命令输入窗口。
400){this.resized=true;this.style.width=400;}">

3.2 基于激光雷达的移动机器人避障漫游

（1）避障算法避障算法不是这里研究的重点，所以简单提一些。激光测距雷达相对于超声波具有很多优越之处，它的测量精度高，稳定性好，方向性强，受到的干扰小等等，因此利用激光雷达进行避障要比超声波可靠的多。在本项目中我们使用了较常用的虚拟力算法（VFF），利用雷达获得的数据实现机器人的自主避障和漫游。VFF方法是在势场法(Potential field method，PFM)的基础上建立起来的。该方法认为，障碍物会对机器人施加一定虚拟的排斥力(Virtual repulsive force)，目标对机器人也会产生一定的吸引力，这两个力的合力决定机器人的速度和方向。漫游机器人的软件配置如下图所示。其中比较重要的几个模块包括防碰撞模块（collide），虚拟力计算模块（force），避障模块（Runaway）等。防碰撞模块有一个参数threashold，用来定义危险距离，当障碍物与机器人的距离低于此值则会产生急停的命令；虚拟力计算模块有两个参数threashold_near和threashold_far，分别用来定义两个边界值。虚拟力的计算函数是一个分段函数，当距离大于threashold_far的时候该值被忽略，即计算得到的力值为0，当距离小于threashold_near的时候，计算得到的力被平方，即增大近距离障碍物在力场中的分量。Runaway模块则根据计算出来的虚拟力的大小，并参考目标点的位置来生成下一步运动的方向和速度等命令。

(2) 漫游实验我们在办公室环境下进行了自主漫游的实验。实验结果标明，移动机器人能够顺利的在无障碍的情况下沿着走廊行走，自动修正行走的方向；当走廊中存在较小障碍物时能够顺利的绕过障碍物继续前进；当走廊中障碍物较多时，就会停在障碍物附近或绕障碍物运动并且寻求操作者帮助。点击这里可以下载当时拍摄的一段录像。

4、总结

总结没什么好说的了，虽然写完了，但是感觉还有好多问题没有说清楚，源代码我会整理一下传上来

400){this.resized=true;this.style.width=400;}">，有什么问题也尽管问，这样也可以帮我做一些改进工作。

5、版权问题

版权所有，严禁转载原文链接：http://www.roboticfan.com/blog/user_2005/73/archives/2006/2006226152239.shtml