CN102359784A - 一种室内移动机器人自主导航避障系统及方法 - Google Patents

Abstract

本发明提供一种室内移动机器人自主导航避障系统及方法，系统包括无线传感器网络和移动机器人，无线传感器网络包括Cricket传感器锚节点、Cricket传感器接收节点和超声波传感器节点，室内环境分为若干网格，超声波传感器节点按分好的网格部署在天花板，同时随机部署大于等于四个Cricket传感器锚节点，移动机器人主控单片机外接通信模块连接Cricket接收节点，采用分段极大似然质心算法对机器人定位，建立三维环境地图和栅格地图，构造路径网格，基于膨胀算法和Dijkstra算法规划全局路径。本发明采用有全局监测能力的传感器网络，将其引入机器人室内、动态环境导航，定位准确、环境建模迅速、路径规划合理。

Description

一种室内移动机器人自主导航避障系统及方法

技术领域

本发明涉及无线传感器网络领域，具体涉及一种基于无线传感器网络的室内移动机器人自主导航避障系统。

背景技术

无线传感器网络利用密集分布的大量节点协同作业，具有动态自组织、高可靠性和以数据为中心等特点，是当前信息领域的研究热点。随着传感器网络和无线技术的发展，传感器网络已经在工业、监控和安全领域获得越来越广泛的应用，研究者开始讨论如何将传感器网络用于机器人导航。基于传感器网络的机器人导航关键问题在于，使机器人准确定位，进行环境建模，避开障碍，顺利到达目标区域。

目前室内定位技术分类较多，但根据定位过程中是否测量实际节点的距离，把定位算法分为：基于距离的算法和距离无关的算法。前者通过测量节点间距或角度计算未知节点的位置；后者无需距离或角度信息，仅依靠网络连通性等信息实现目标定位。基于TDOA的定位是基于距离定位算法中的一种，它在已知两种信号的传输速度的情况下，通过测量两种不同信号的到达时间差来确定距离，定位精度高，且误差不随时间累积。在传感器节点选择方面，基于TDOA定位的典型代表是Active Bat、AHLos、和Cricket，Cricket返回值为直观的距离信息，在实现定位系统过程中价格比Active Bat低，且为无中心构架，实现比较方便；同时相对于AHLos而言Cricket使用的信标节点少，因此受到了广泛的关注。

在环境建模方面，通常采用超声波传感器探测障碍物的信息，通过计算发送脉冲与接收到返回脉冲的时间差可以计算出距离。依靠对环境的定量表征可建立栅格地图，依靠对环境的定性表征可建立拓扑地图。其中，栅格地图法具有较高的精度、建立速度快、信息融合直观、易于整个统计意义上的数据可信度信息；拓扑地图强迫机器人按照一个预先规划好的路径导航。在路径规划方面，可视图法采用直线连接机器人、目标点和障碍物各定点从而形成一张路径图；人工势场法是把机器人在工作环境中的运动视为在一种虚拟的人工力场中的运动，该算法数学描述上简洁，但算法计算量很大，且容易陷入局部极小。

发明内容

针对现有技术存在的不足，本发明提供一种定位准确、环境建模迅速、路径规划合理的基于无线传感器网路的室内移动机器人自主导航避障系统及方法。

本发明的技术方案是：一种室内移动机器人自主导航避障系统包括无线传感器网络和移动机器人，无线传感器网络包括Cricket传感器锚节点、Cricket传感器接收节点和超声波传感器节点，以Cricket传感器和超声波传感器为载体，组成无线传感器网络WSN进行室内机器人定位和避障导航，具体地：将室内环境划分为若干网格，把超声波传感器节点按照划分好的网格均匀部署在天花板上，超声波传感器节点自发自收超声波信号，用于探测环境障碍物信息；同时，在天花板上随机部署大于等于四个Cricket传感器锚节点用于机器人定位，Cricket传感器接收节点用于接收Cricket传感器锚节点发送的信号，移动机器人的主控单片机外接SCI串口通信模块，单片机通过该模块与Cricket传感器接收节点相连，Cricket传感器接收节点收到信息后通过SCI串口通信模块将信息送入单片机中，采用分段极大似然质心算法对移动机器人进行定位，实时探测环境信息，建立三维环境地图和栅格地图，构造路径网格，基于膨胀算法和Dijkstra算法进行全局路径规划。

所述室内移动机器人自主导航避障系统实现自动导航避障的方法包括如下步骤：

步骤一：通过运行模式命令代码MD将Cricket模块设置为Beacon和Listener两种模式。Cricket工作在Beacon模式下，被设置成锚节点；Cricket工作在Listener模式下，被设置成接收节点。其中锚节点的个数大于等于四，接收节点的个数为一。

步骤二：在天花板上随机部署Cricket传感器锚节点，并记录下各个锚节点的坐标值，用于机器人定位。

把设置为Beacon模式的传感器锚节点随机部署在天花板上，打开传感器开关。锚节点的个数大于等于四个。以天花板所在平面建立二维平面直角坐标系，选定平面内的一点为坐标原点，记录下在此坐标系下的各个锚节点的坐标值。

步骤三：在天花板上按照划分好的网格均匀部署超声波传感器节点，用于探测障碍物信息。

将室内环境划分为m×n的网格，每个网格的边长为L，把超声波传感器节点按照划分好的网格均匀部署在天花板上，每个网格的中心对应一个传感器节点。打开传感器开关，则超声波传感器与Cricket传感器共同组建成一个无线传感器网络。其中超声波传感器自发自收频率为40KHZ的超声波信号，Cricket传感器由Beacon节点发送超声波信号和无线电信号，由对应的Listener节点接收，因此两种传感器之间不会造成干扰。

步骤四：移动机器人平台放置在地面，Cricket传感器接收节点(未知节点)安放在移动机器人上，通过SCI串口通信模块与移动机器人的主控单片机相连，启动机器人。实时测得的机器人到各锚节点的距离就是该未知节点到各锚节点的距离。

把机器人放置在地面上的起点位置，把设置为Listener模式的Cricket传感器接收节点安放在机器人上，打开传感器开关，则此时接收节点和各个Cricket锚节点之间建立起实时通信，即可实时获得接收节点与各个锚节点之间的距离信息。打开机器人开关，该接收节点随着机器人的移动而移动，因此它的位置就代表机器人所在位置。由于机器人位置实时变动，为未知值，因此也称此接收节点为未知节点。

步骤五：根据机器人自身高度、天花板与地面之间的距离以及步骤二中测得的机器人到锚节点的距离，计算出机器人与各锚节点之间的距离投射在天花板二维平面上的投影。

天花板到地面之间的距离和机器人自身的高度可以通过事先测量得到，二者相减为未知节点到锚节点的垂直距离；未知节点到锚节点的直线距离为Cricket传感器实时反馈的距离信息；则由勾股定理，可求得未知节点到锚节点的水平距离，即机器人与锚节点的直线距离投射在天花板二维平面上的投影。

步骤六：根据无线传感器网络中锚节点个数、各锚节点坐标值以及步骤五中的投影距离，采用分段极大似然质心算法估算出机器人自身位置。

当锚节点个数为4时，根据各锚节点的坐标值和步骤五中计算得到的投影距离，采用三边测量法对机器人进行定位。

当锚节点个数大于4时，根据各锚节点的坐标值和步骤五中计算得到的投影距离，采用极大似然质心算法对机器人进行定位。

设锚节点个数n＞3，设未知节点P坐标为(x_P，y_P)，已知n个锚节点坐标分别为(X₁，Y₁)，(X₂，Y₂)，...，(X_k，Y_k)，到P的距离分别为d₁，d₂，...，d_n。每次从n个锚节点中选取n-1个节点，分别采用极大似然估计法估算待测节点的坐标值，可选取

次，即共进行n次极大似然估计。设产生待测节点P(x_P，y_P)的n次坐标估计值分别为

$P_1(x_{p_1},y_{p_1}),P_2(x_{p_2},y_{p2}),\mathrm{\Λ},P_n(x_{p_n},y_{\mathrm{pn}}),$

以选取锚节点1，2，...，n-1为例，进行第一次极大似然法估算，可得如下方程组：

$\left\{\begin{array}{c}{(x_1-x_{P1})}^2+{(y_1-y_{P1})}^2=d_1^2\\ {(x_2-x_{P1})}^2+{(y_2-y_{P1})}^2=d_2^2\\ M\\ {(x_{n-1}-x_P)}^2+{(y_{n-1}-y_P)}^2=d_{n-1}^2\end{array}\right.$

由此方程组即可求出基于锚节点1，2，...，n-1的待测节点估计值坐标P₁(x_P1，y_P1)。

其他n-1次极大似然估算步骤同上。求n个估计值坐标的质心，即得节点P最终估算坐标(x_P，y_P)：

$\left\{\begin{array}{c}{x}_P=\frac{x_{P_1}+x_{P_2}+\mathrm{\Λ}+x_{P_n}}{n}\\ y_P=\frac{y_{P_1}+y_{P_2}+\mathrm{\Λ}+y_{P_n}}{n}\end{array}\right.$

当锚节点个数n＝4时，上述极大似然质心定位误差较大。因为当n＝4时，每次从4个锚节点中选取n-1＝3个节点来估算待测节点的坐标值，共选取次。3个节点的极大似然估计法即为三边测量法，即先进行4次误差较大的三边测量定位，再进行一次误差更大的质心定位，造成误差累加。为此采用分段思想进行处理，当锚节点个数n＞4时用改进算法进行机器人定位；当锚节点个数n＝4时用三边测量法进行定位。

步骤七：由部署在天花板的各传感器节点返回的距障碍物距离得到障碍物实际高度的离散数据，采用三次样条插值法建立三维环境地图。

部署在天花板上的超声波传感器节点返回的是传感器节点到障碍物的距离，用天花板距水平地面的距离减去上述距离得到障碍物的实际高度。由于超声波传感器节点的部署是离散的，得到的障碍物高度的数据也是离散的，显然不能满足创建地图的需要。为了获得整个活动空间中每个位置处的环境信息，需要根据已获得的信息建立环境模型。所以我们通过三次样条插值的方法估计了没有无线传感器节点的位置处的环境信息，从而构建机器人工作区域的环境模型。三次样条插值是插值方法中较为简单，且具有很好连续性的方法。计算精度较高，不失真，能够通过低代价而获得较好地收敛性。

步骤八：对环境信息即障碍物高度，进行二值化处理，建立网格模型，根据处理的结果对栅格单元赋值，从而得到二值化的栅格地图。

根据移动机器人性能和环境对象的具体特征设置恰当的阈值，作为判断是否为妨碍机器人运动的障碍物的依据。由于根据超声传感器测量值计算出来的是障碍物的高度，故当高度高于阈值时，则认为该位置处障碍物高度对移动机器人构成障碍作用，机器人不能通过，对应的栅格值标为1；否则，认为移动机器人可以安全通过，对应的栅格值标为0。由此完成对环境信息的二值化处理，得到二值化的栅格地图。

步骤九：对步骤八中的栅格地图进行扩张(膨胀)，即把障碍物与其边界所在栅格逐步扩张到相邻的自由栅格区域，直至满足扩张停止条件，从而构造路径网络。

把障碍物与边界所在栅格作为“内核”，逐步扩张到相邻的自由栅格区域。对工作区域内的栅格每进行一遍扩张，障碍物的形状就如同均匀地生长了一层。不断地进行扩大，直到满足扩张停止的条件为止。

障碍物扩张规则：

1)每次生长，障碍物所在栅格都按上下左右四个方向均匀生长。

2)每次障碍物扩张后所生长出来的栅格都被当成新的障碍物。

3)若在某个方向上满足停止条件，则此栅格在下一次扩张时，此方向上停止扩张，其他方向继续扩张。

障碍物扩张停止的条件：

1)考虑到移动机器人的实际直径为(2/3)L，L为每个栅格边长，故两障碍物或障碍物与边界之间的距离为一个栅格时，停止扩张。

2)若三个障碍物，或两个障碍与边界之间只有一个自由栅格时，停止扩张。

3)工作区域不存在自由栅格停止扩张。

满足以上前两个条件中的任意一个，该障碍物在此方向停止扩张，当满足第三个条件时，所有障碍物停止扩张。障碍物扩张后由2个区域交会形成交线，3个或3个以上区域交会形成交点，定义交线为机器人可行走的路径，所有交线的集合为路径网络，而交点则为路径网络的基本节点。

步骤十：对步骤九中的路径网络标号、赋权值，构造带权图。

在步骤九的路径网络中依次标记路径起点、各基本节点和路径终点。对于中间相连的两节点，将该两节点之间栅格的数目定义为该两点之间路径的权值；对于中间没有栅格相连的两节点，将这两点之间路径的权值定义为无穷大，从而得到带权图。

步骤十一：采用Dijkstra算法求得步骤十中带权图从起点到终点的最短路径，此路径即为该系统机器人避障导航的最优路径。

Dijkstra算法按如下步骤进行：

步骤1：从起点出发，若权值W(0，0)＝0，则在起点处标记0，建立已标号点集V，并把起点添加到点集V。

步骤2：从起点出发，找相邻点r使得权数W(0，r)(距离)最小，若W(0，r)＝W(0，0)+d(0，r)，则将W(0，r)标于点r处。将节点0与节点r之间的边变虚线，并把r添加到点集V。

步骤3：从已标号点集V出发，找与点集中节点的相邻点中最小权数(距离)的节点，若W(0，r)＝W(0，0)+d(0，r)，则W(0，r)将标于p处，并把节点r与节点p之间的边变红。此处r为已标号节点下标，p为未标号节点下标。

步骤4：重复上述步骤，直到标记到终点处，从而得出的最优路径。

步骤十二：根据规划出的最优路径和步骤六中采用分段极大似然质心算法估计出的机器人自身位置，由移动机器人自主控制自身的行进，从而实现机器人自主避障导航。

Cricket传感器的Listener节点接收到距离信息后通过SCI串口模块将信息送入机器人主控制单片机中，并通过编程实现分段极大似然质心定位算法，确定机器人自身位置。规划好的最优路径同样送入机器人主控单片机中，对照机器人当前位置和最优路径位置，机器人自动控制转向，沿规划好的路径到达目的地。

有益效果

本发明系统采用具有全局监测能力的传感器网络，将传感器网络引入机器人的室内、动态环境导航，定位准确、环境建模迅速、路径规划合理。

附图说明

图1本发明整体系统框图；

图2本发明软件流程图；

图3本发明实施例天花板上Cricket锚节点的示意图；

图4本发明实施例定位算法精度比较；

图5本发明实施例室内环境抽象图；

图6本发明实施例障碍物高度数据；

图7本发明实施例全局环境三维图；

图8本发明实施例二值化后的栅格地图；

图9本发明实施例障碍物膨胀区域与路径网络；

图10本发明实施例有向的带权图；

图11本发明实施例Dijkstra算法解得的最优路径；

图12本发明实施例栅格图中的最优路径；

图13本发明实施例Dijkstra算法流程图。

具体实施方式

下面结合附图对本发明做进一步的描述。

基于无线传感器网路的室内移动机器人自主导航避障系统包括无线传感器网络和移动机器人，传感器网络包括Cricket传感器锚节点，Cricket传感器接收节点和超声波传感器节点，其中，Cricket传感器锚节点部署在天花板上，发送采集到的无线信号，用于机器人定位，Cricket传感器接收节点用于接收Cricket传感器锚节点发送的信号，移动机器人的主控单片机外接SCI串口通信模块，单片机通过该模块与Cricket传感器接收节点相连，Cricket传感器接收节点收到信息后通过SCI串口通信模块将信息送入单片机中。超声波传感器自发自收超声波信号，返回的是传感器节点到障碍物的距离。以Cricket传感器和超声波传感器为载体，组成无线传感器网络WSN进行室内机器人定位和避障导航，移动机器人的主控单片机外接SCI串口通信模块，通过该模块与Cricket传感器接收节点相连，Cricket传感器接收节点收到信息后通过此串口模块将信息送入单片机中。串口通信模块型号是rs232，通过一个max232芯片实现TTL电平和串口电平的转换，然后一端连单片机的引脚，一边连cricket接收传感器的信息。本发明系统结构如图1所示，整体流程如图2所示。软件环境为WINDOWS XP系统，仿真环境为MATLAB2009。硬件环境是在长10米，宽5米，高5米的室内环境下，部署MIT所开发的Cricket传感器和US100超声波传感器，选用由MC9S12XS128MAA型号单片机主控的移动机器人。

1.Cricket定位

室内环境下的Cricket定位系统由散布在建筑物内位置固定的锚节点(Beacon)和需要定位的人或物体携带的未知节点(Listener)组成。锚节点随机的发射RF射频信号和超声波信号，RF信号中包含该锚节点的位置坐标和ID。未知节点使用TDOA技术测量其与锚节点的距离，当它能够获得4个以上锚节点距离时，采用分段极大似然质心算法进行定位，确定其物理位置。实验中测得Cricket的实际测距误差在3～5cm。

Cricket模块可以通过运行模式命令代码MD来设置为Beacon和Listener两种模式。设置参数1，命令代码为P MD 1<return>，表示Cricket工作在Beacon模式下，被设置成锚节点；设置参数2，命令代码为P MD 2<return>，表示Cricket工作在Listener模式下，被设置成接收节点。

如图3所示，把5个Beacon固定在天花板上，打开传感器开关，以天花板所在平面建立二维平面直角坐标系，选定平面内的一点为坐标原点，记录下在此坐标系下的各个锚节点的坐标值，分别为(X₁，Y₁)，(X₂，Y₂)，(X₃，Y₃)，(X₄，Y₄)，(X₅，Y₅)。把Listener节点安放在机器人上，打开传感器开关，打开机器人开关，该接收节点随着机器人的移动而移动，因此它的位置就代表机器人所在位置，设此未知节点的坐标为(x_P，x_P)。当Listener接收到来自Beacon的信号后，会把当前接收到的Beacon的状态信息和距离信息一起传送到机器人单片机中。由于Beacon的坐标是预先设定的，单片机会通过Listener与多个Beacon的距离信息计算出Listener的坐标位置，此过程采用的是分段极大似然质心算法。

2.距离换算

在某室内环境下，现已测得天花板到地面之间的距离为H、机器人自身的高度为h₀，二者相减为未知节点到各锚节点的垂直距离H-h₀；未知节点到各个锚节点的直线距离D_i为Listener接收到的各Beacon发送的距离信息。由勾股定理，可求得未知节点到锚节点的水平距离，即机器人与锚节点的直线距离投射在天花板二维平面上的投影d_i为：

$d_i=\sqrt{D_i^2-{(H-h_0)}^2}---\left(1\right)$

定位算法中使用的锚节点到未知节点的距离实际上是此投影距离。

3.分段极大似然质心算法

1)极大似然质心算法

锚节点个数n＞3，设未知节点P坐标为(x_P，y_P)，已知n个锚节点坐标分别为(X₁，Y₁)，(X₂，Y₂)，...，(X_k，Y_k)，到P的距离分别为d₁，d₂，...，d_n。每次从n个锚节点中选取n-1个节点，分别采用极大似然估计法估算待测节点的坐标值，可选取

次，即共进行n次极大似然估计。设产生待测节点P(x_P，y_P)的n次坐标估计值分别为

$P_1(x_{p_1},y_{p_1}),P_2(x_{p_2},y_{p2}),\mathrm{\&Lambda;},P_n(x_{p_n},y_{\mathrm{pn}}),$

以选取锚节点1，2，...，n-1为例，进行第一次极大似然法估算，可得如下方程组：

$\left\{\begin{array}{c}{(x_1-x_{P1})}^2+{(y_1-y_{P1})}^2=d_1^2\\ {(x_2-x_{P1})}^2+{(y_2-y_{P1})}^2=d_2^2\\ M\\ {(x_{n-1}-x_P)}^2+{(y_{n-1}-y_P)}^2=d_{n-1}^2\end{array}\right.$

由此方程组即可求出基于锚节点1，2，...，n-1的待测节点估计值坐标P₁(x_P1，y_P1)。

其他n-1次极大似然估算步骤同上。求n个估计值坐标的质心，即得节点P最终估算坐标(x_P，y_P)：

$\left\{\begin{array}{c}{x}_P=\frac{x_{P_1}+x_{P_2}+\mathrm{\&Lambda;}+x_{P_n}}{n}\\ y_P=\frac{y_{P_1}+y_{P_2}+\mathrm{\&Lambda;}+y_{P_n}}{n}\end{array}\right.---\left(2\right)$

2)分段思想

当锚节点个数n＝4时，上述极大似然质心定位误差较大。因为当n＝4时，每次从4个锚节点中选取n-1＝3个节点来估算待测节点的坐标值，共选取

次。3个节点的极大似然估计法即为三边测量法，即先进行4次误差较大的三边测量定位，再进行一次误差更大的质心定位，造成误差累加。为此采用分段思想进行处理，当锚节点个数n＞4时用改进算法进行机器人定位；当锚节点个数n＝4时用三边测量法进行定位。

三边测量法是根据已知三个锚节点的坐标信息，确定未知节点的位置。设未知节点P坐标为(x_P，y_P)，已知A、B、C三个锚节点坐标分别为(x_A，y_A)，(x_B，y_B)，(x_C，y_C)，到P的距离分别为d_A，d_B，d_C，则可得如下方程组：

$\left\{\begin{array}{c}{(x-x_A)}^2+{(y-y_A)}^2=d_A^2\\ {(x-x_B)}^2+{(y-y_B)}^2=d_B^2\\ {(x-x_C)}^2+{(y-y_C)}^2=d_C^2\end{array}\right.$

由此方程组即求出P点的坐标(x_P，y_P)。

3)实验分析

在10m×10m的传感区域内随机部署n个锚节点和1个待测节点，n取值分别为4，8，12，16，20。根据测得的未知节点与各个锚节点间的距离，分别采用质心法、三边测量法、极大似然估计法和本发明中的分段极大似然质心算法估计待测节点的位置。分别计算4种算法估算出来的坐标值与待测节点的真实坐标值之间的差值作为算法误差。如图4所示，随着锚节点个数的增加，三边测量法的定位精度变化不大，而极大似然估计和分段极大似然质心算法的定位精度都有提高。定位精度方面，本发明中的定位算法高于极大似然估计算法高于三边测量法。

4.超声波传感器探测障碍物

1)超声波传感器测距原理

超声波测距是借助超声波脉冲回波渡越时间法来实现的。设超声波脉冲由传感器发出到接收所经历的时间为t。超声波在空气中的传播速度为c，则从传感器到目标物体的距离D可用下式求出：

D＝ct/2        (3)

超声波传感器探测障碍物的基本原理是：经发射器发射出长约6mm，频率为40KHZ的超声波信号。此信号被障碍物反射回来由接收头接收，接收头实质上是一种压电效应的换能器，它接收到信号后产生mV级的微弱电压信号。

2)超声波传感器部署

如图5所示，已知室内环境为7.2m×4m的矩形，环境中存在若干障碍物。已知移动机器人的最大直径为24cm，考虑到机器人的实际直径和机器人行进时与障碍物保持的安全距离，将室内环境划分为20×11的网格，每个网格的边长为36.19cm。把超声波传感器节点按照划分好的网格均匀部署在天花板上，每个网格的中心对应一个传感器节点。打开传感器开关，则超声波传感器与Cricket传感器共同组建成一个无线传感器网络。其中超声波传感器自发自收频率为40KHZ的超声波信号，Cricket传感器由Beacon节点发送超声波信号和无线电信号，由对应得listener节点接收，因此两种传感器之间不会造成干扰。

5.建立三维环境地图

1)障碍物高度

部署在天花板上的超声波传感器返回的是传感器节点到障碍物的距离。通过以下公式将返回的值转化成障碍物的实际高度：

h_ij＝H-s_ij，(i＝1，2，Λ11 j＝1，2，Λ20)     (4)

公式中H为传感器节点到地面的距离，i×j表示11×20个网格，s_ij为表示网格对应的传感器返回的测量值，h_ij则表示网格对应障碍物的高度值，障碍物的高度数据如图8所示。

2)建立全局三维地形图

由图6可见，探测的障碍物高度数据都是离散，显然不能满足创建地图的需要，为了获得整个活动空间中每个位置处的环境信息，需要根据已获得的信息建立环境模型。所以通过三次样条插值的方法估计没有无线传感器节点的位置处的环境信息，从而构建机器人工作区域的环境模型。三次样条插值是插值方法中较为简单，且具有很好连续性的方法，其计算精度较高，不失真，能够通过低代价而获得较好地收敛性。插值后得到的全局三维地形图如图7所示。

6.建立栅格地图

为了创建便于移动机器人导航使用的栅格地图，根据重建出的三维环境模型，对环境信息进行二值化处理。根据移动机器人性能和环境对象的具体特征设置恰当的阈值，作为判断是否为妨碍机器人运动的障碍物的依据。根据超声传感器测量值计算出来的是障碍物的高度h_ij，当此高度高于阈值时，则认为该位置处障碍物高度对移动机器人构成障碍作用，机器人不能通过；否则，认为移动机器人可以安全通过。经过反复试验，所采用的机器人的越障能力为不超过3cm高的台阶。阈值化合栅格赋值用公式表示如下：

$f(x,y)=\left\{\begin{array}{cc}1& \mathrm{if}(h(x,y)\&GreaterEqual;h_0)\\ 0& \mathrm{else}\end{array}\right.---\left(5\right)$

公式中，(x，y)为栅格单元坐标，h(x，y)为栅格(x，y)处传感信息经过计算得出的障碍物的高度值；h₀为所设定的阈值，h₀＝3；f(x，y)为栅格(x，y)处环境地图的取值，是一个布尔变量，为1表示存在障碍，为0表示空置。这样就可以得到高度的栅格地图，如图8所示。

7.全局路径规划

a)膨胀算法构造路径网络

把障碍物与边界所在栅格作为“内核”，逐步扩张到相邻的自由栅格区域。对工作区域内的栅格每进行一遍扩张，障碍物的形状就如同均匀地生长了一层。不断地进行扩大，直到满足扩张(膨胀)停止的条件为止。障碍物扩张规则：

1)每次生长，障碍物所在栅格都按上下左右四个方向均匀生长；

2)每次障碍物扩张后所生长出来的栅格都被当成新的障碍物；

3)若在某个方向上满足停止条件，则此栅格在下一次扩张时，此方向上停止扩张，其他方向继续扩张。

障碍物扩张停止的条件：

1)考虑到移动机器人的实际直径为(2/3)L，故两障碍物或障碍物与边界之间的距离为一个栅格时，停止膨胀；

2)若三个障碍物，或两个障碍与边界之间只有一个自由栅格时(如图11中的节点7)，停止膨胀。

3)工作区域不存在自由栅格停止膨胀。

满足以上前两个条件中的任意一个，该障碍物在此方向停止扩张，当满足第三个条件时，所有障碍物停止膨胀。

障碍物膨胀后由2个区域交会形成交线，3个或3个以上区域交会形成交点，定义交线为机器人可行走的路径，所有交线的集合为路径网络，而交点则为路径网络的基本节点。在图8栅格化地图的基础上进行障碍物膨胀得到图9。在图8中，黑色区域为障碍物的区域，图9中，比图8中多出的黑色区域为障碍物膨胀的区域，斜线区域为机器人可以安全行走的路径网络，数字1～10标记的是网络基本节点。

b)Dijkstra算法求最优路径

在已经构造好的路径网络的基础上，通过选择最优的网络节点来构成最优路径。在图9中，设节点0为路径起点，节点11为路径终点。最优路径就是从起点0开始，最终到达终点11的最短路径，即移动机器人经过的栅格总数最少。对于中间相连的两节点，将该两节点之间栅格的数目定义为该两点之间路径的权值；对于中间没有栅格相连的两节点，将这两点之间路径的权值定义为无穷大，这样，将该节点图看成是一个带权图，如图10所示，连同它边上的权称为赋权图，而为了解决规划最优路径问题，则不应重复经过同一节点，也不应与终点背道而驰，所以图10为有向的赋权图。而对于带权图求固定节点之间的最短距离可以用图论中的Dijkstra算法来实现，本例中我们讨论的是从网络中的点0到点11的最短路。

采用Dijkstra算法求得带权图从起点到终点的最短路径，此路径即为该系统机器人避障导航的最优路径，算法流程如图13所示，算法步骤如下：

步骤1：从起点出发，若权值W(0，0)＝0，则在起点处标记0，建立已标号点集V，并把起点添加到点集V。

步骤2：从起点出发，找相邻点r使得权数W(0，r)(距离)最小，若W(0，r)＝W(0，0)+d(0，r)，则将W(0，r)标于点r处。将节点0与节点r之间的边边虚线，并把r添加到点集V。

步骤3：从已标号点集V出发，找与点集中节点的相邻点中最小权数(距离)的节点，若W(0，r)＝W(0，0)+d(0，r)，则W(0，r)将标于p处，并把节点r与节点p之间的边变虚线。此处r为已标号节点下标，p为未标号节点下标。

步骤4：重复上述步骤，直到标记到终点处，从而得出的最优路径。

图10中有向边上的黑色数字为权值，黑色方框为网络中的基本节点，图11中虚线途经的节点旁的数字为步骤3中的W(0，p)，即节点p到节点0的最小权数之和。根据Dijkstra算法，求得从节点0到节点11的最优路径为0→3→5→6→10→11，如图11虚线边所示，权值之和为8+5+3+4+3＝23最小。而图12为最优路径反映到实际的栅格图当中的效果，带浮点栅格为根据Dijkstra算法求得的移动机器人从节点0到节点11的最优路径。

综上，Cricket传感器的Listener节点接收到距离信息后送入机器人主控制单片机中，并通过编程实现分段极大似然质心定位算法，确定机器人自身位置。规划好的最优路径同样送入机器人主控单片机中，对照机器人当前位置和最优路径位置，机器人自动控制转向，沿规划好的路径到达目的地。整个系统实现了基于WSN的室内移动机器人自主导航避障。

Claims (4)

1.一种室内移动机器人自主导航避障系统，其特征在于：包括无线传感器网络和移动机器人，无线传感器网络包括Cricket传感器锚节点、Cricket传感器接收节点和超声波传感器节点，以Cricket传感器和超声波传感器为载体，组成无线传感器网络WSN进行室内机器人定位和避障导航，具体地：将室内环境划分为若干网格，把超声波传感器节点按照划分好的网格均匀部署在天花板上，超声波传感器节点自发自收超声波信号，用于探测环境障碍物信息；同时，在天花板上随机部署大于等于四个Cricket传感器锚节点用于机器人定位，Cricket传感器接收节点用于接收Cricket传感器锚节点发送的信号，移动机器人的主控单片机外接SCI串口通信模块，单片机通过该模块与Cricket传感器接收节点相连，Cricket传感器接收节点收到信息后通过SCI串口通信模块将信息送入单片机中，采用分段极大似然质心算法对移动机器人进行定位，实时探测环境信息，建立三维环境地图和栅格地图，构造路径网格，基于膨胀算法和Dijkstra算法进行全局路径规划。

2.一种室内移动机器人自主导航避障方法，其特征在于：按如下步骤实现自动导航避障：

步骤一：通过运行模式命令代码MD将Cricket模块设置为Beacon和Listener两种模式：Cricket工作在Beacon模式下，被设置成锚节点；Cricket工作在Listener模式下，被设置成接收节点，其中锚节点的个数大于等于四，接收节点的个数为一；

步骤二：在天花板上随机部署Cricket传感器锚节点，并记录下各个锚节点的坐标值，用于机器人定位；

步骤三：在天花板上按照划分好的网格均匀部署超声波传感器节点，用于探测障碍物信息；

将室内环境划分为m×n的网格，每个网格的边长为L，把超声波传感器节点按照划分好的网格均匀部署在天花板上，每个网格的中心对应一个传感器节点，打开传感器开关，则超声波传感器与Cricket传感器共同组建成一个无线传感器网络，其中超声波传感器自发自收超声波信号，Cricket传感器由Beacon节点发送超声波信号和无线电信号，由对应的Listener节点接收；

步骤四：移动机器人平台放置在地面，Cricket传感器接收节点安放在移动机器人上，通过SCI串口通信模块与移动机器人的主控单片机相连，启动机器人，实时测得的机器人到各锚节点的距离就是该未知节点到各锚节点的距离；

步骤五：根据机器人自身高度、天花板与地面之间的距离以及步骤二中测得的机器人到锚节点的距离，计算出机器人与各锚节点之间的距离投射在天花板二维平面上的投影；

步骤六：根据无线传感器网络中锚节点个数、各锚节点坐标值以及步骤五中的投影距离，采用分段极大似然质心算法估算出机器人自身位置；

步骤七：由部署在天花板的各传感器节点返回的距障碍物距离得到障碍物实际高度的离散数据，采用三次样条插值法建立三维环境地图；

步骤八：对环境信息即障碍物高度，进行二值化处理，建立网格模型，根据处理的结果对栅格单元赋值，从而得到二值化的栅格地图；

步骤九：对步骤八中的栅格地图进行扩张，即把障碍物与其边界所在栅格逐步扩张到相邻的自由栅格区域，直至满足扩张停止条件，从而构造路径网络；

障碍物扩张规则：

1)每次生长，障碍物所在栅格都按上下左右四个方向均匀生长；

2)每次障碍物扩张后所生长出来的栅格都被当成新的障碍物；

3)若在某个方向上满足停止条件，则此栅格在下一次扩张时，此方向上停止扩张，其他方向继续扩张；

障碍物扩张停止的条件：

1)考虑到移动机器人的实际直径为(2/3)L，L为每个栅格边长，故两障碍物或障碍物与边界之间的距离为一个栅格时，停止扩张；

2)若三个障碍物，或两个障碍与边界之间只有一个自由栅格时，停止扩张；

3)工作区域不存在自由栅格停止扩张；

满足以上前两个条件中的任意一个，该障碍物在此方向停止扩张，当满足第三个条件时，所有障碍物停止扩张，障碍物扩张后由2个区域交会形成交线，3个或3个以上区域交会形成交点，定义交线为机器人可行走的路径，所有交线的集合为路径网络，而交点则为路径网络的基本节点；

步骤十：对步骤九中的路径网络标号、赋权值，构造带权图；

步骤十一：采用Dijkstra算法求得步骤十中带权图从起点到终点的最短路径，此路径即为该系统机器人避障导航的最优路径；

步骤十二：根据规划出的最优路径和步骤六中采用分段极大似然质心算法估计出的机器人自身位置，移动机器人自主控制自身的行进，从而实现机器人自主避障导航。

3.根据权利要求2所述的室内移动机器人自主导航避障方法，其特征在于：步骤六中所述的分段极大似然质心算法具体是：

当锚节点个数为4时，根据各锚节点的坐标值和步骤五中计算得到的投影距离，采用三边测量法对机器人进行定位；

当锚节点个数大于4时，根据各锚节点的坐标值和步骤五中计算得到的投影距离，采用极大似然质心算法对机器人进行定位；

设锚节点个数n＞3，设未知节点P坐标为(x_P，y_P)，已知n个锚节点坐标分别为(X₁，Y₁)，(X₂，Y₂)，...，(X_k，Y_k)，到P的距离分别为d₁，d₂，...，d_n，每次从n个锚节点中选取n-1个节点，分别采用极大似然估计法估算待测节点的坐标值，可选取

次，即共进行n次极大似然估计，设产生待测节点P(x_P，y_P)的n次坐标估计值分别为

$P_1(x_{p_1},y_{p_1}),P_2(x_{p_2},y_{p2}),\mathrm{\&Lambda;},P_n(x_{p_n},y_{\mathrm{pn}}),$

以选取锚节点1，2，...，n-1为例，进行第一次极大似然法估算，可得如下方程组：

$\left\{\begin{array}{c}{(x_1-x_{P1})}^2+{(y_1-y_{P1})}^2=d_1^2\\ {(x_2-x_{P1})}^2+{(y_2-y_{P1})}^2=d_2^2\\ M\\ {(x_{n-1}-x_P)}^2+{(y_{n-1}-y_P)}^2=d_{n-1}^2\end{array}\right.$

由此方程组即可求出基于锚节点1，2，...，n-1的待测节点估计值坐标P₁(x_P1，y_P1)；

其他n-1次极大似然估算步骤同上，求n个估计值坐标的质心，即得节点P最终估算坐标(x_P，y_P)：

$\left\{\begin{array}{c}{x}_P=\frac{x_{P_1}+x_{P_2}+\mathrm{\&Lambda;}+x_{P_n}}{n}\\ y_P=\frac{y_{P_1}+y_{P_2}+\mathrm{\&Lambda;}+y_{P_n}}{n}\end{array}\right.$

当锚节点个数n＝4时，上述极大似然质心定位误差较大，因为当n＝4时，每次从4个锚节点中选取n-1＝3个节点来估算待测节点的坐标值，共选取

次，3个节点的极大似然估计法即为三边测量法，即先进行4次误差较大的三边测量定位，再进行一次误差更大的质心定位，造成误差累加。

4.根据权利要求2所述的室内移动机器人自主导航避障方法，其特征在于：步骤十一所述Dijkstra算法按如下步骤进行：

步骤1：从起点出发，若权值W(0，0)＝0，则在起点处标记0，建立已标号点集V，并把起点添加到点集V；

步骤2：从起点出发，找相邻点r使得权数W(0，r)(距离)最小，若W(0，r)＝W(0，0)+d(0，r)，则将W(0，r)标于点r处，将节点0与节点r之间的边变虚线，并把r添加到点集V；

步骤3：从已标号点集V出发，找与点集中节点的相邻点中最小权数(距离)的节点，若W(0，r)＝W(0，0)+d(0，r)，则W(0，r)将标于p处，并把节点r与节点p之间的边变红，此处r为已标号节点下标，p为未标号节点下标；

步骤4：重复上述步骤，直到标记到终点处，从而得出的最优路径。

Images