基于ROS的多导盲机器人及在未知环境中的地图创建方法技术领域
本发明涉及一种导盲机器人，更具体地说，是涉及一种基于ROS的多导盲机器人及在未
知环境中的地图创建方法，属于导盲机器人技术领域。
背景技术
盲和视力障碍对人类生活质量影响非常严重，视力障碍者一般只能通过听觉和触觉的方
式来感觉外在世界的信息，这给其日常生活带来了极大的不便。因此设计和研发出一款可以
配合视力障碍者的感触特征而运行的导盲机器人，对弥补其视觉信息的缺失来说是十分重要
的。随着机器人技术的飞速发展，研究导盲机器人成为辅助盲人正常生活的最好选择之一。
导盲机器人目前在服务机器人领域逐渐成为了一大研究热点，但单一的导盲机器人功能
比较有限且服务对象较为单一，服务区域相对也比较小。单一的导盲机器人不能完成某些复
杂的任务，并且在复杂环境中的导盲也具有局限性。针对单一导盲机器人的这些不足，多导
盲机器人的研究应运而生。多导盲机器人系统可以完成单个导盲机器人不能完成的复杂任务，
更能适应各种复杂的环境。可以增加系统的鲁棒性，实时性好。因此多导盲机器人的研究成
为当今社会发展的必然趋势。
ROS是能为异质计算机集群提供例如操作系统功能的软件平台。2007年,斯坦福大学人工
智能实验室与机器人技术公司WillowGarage合作开发出了机器人操作系统，2008年以后由
WillowGarage推广。
ROS平台是一个非常大的共享平台，人们可以借由这个系统进行所处领域的代码交流，
因此世界上有非常多的机构在对ROS进行开发和维护。基于Linux操作系统下，ROS系统能
够提供的功能包括硬件抽象描述、执行底层驱动程序管理、程序间共用功能的执行、程序间
的消息传递、程序发行包管理，它也提供一些工具程序和库用于获取、建立、编写和运行多
机集成的程序。世界各地的研究人员在ROS的基础上开发了许多软件包，如定位绘图、并行
规划、感知、仿真和其他高级功能包功能。在机器人研究领域，ROS已经逐渐成为通用性的
软件平台。
发明内容
本发明的目的是针对现有单导盲机器人在面对未知环境时存在的工作能力有限，工作效
率低和智能化程度较差的缺点，解决单一导盲机器人在面对未知环境以及复杂导盲信息时，
由于自身的硬件设备不足或是不能有效获取环境障碍物信息，因而无法创建有效地图的问题，
提出了一种基于ROS的多导盲机器人及在未知环境中的地图创建方法。
为实现上述目的，本发明解决问题所采取的技术方案是：
一种基于ROS的多导盲机器人，由至少两台结构相同的导盲机器人个体组成，所述导盲
机器人个体包括上中下三层结构的车身，车身下层设有车轮组7，电机6，电机驱动器8和电
源转换模块1；中层设有运动控制器9，电源10和语音识别模块4；上层设置有RFID模块2，
数据采集模块3和笔记本5；所述笔记本5通过USB串口分别与所述RFID模块2，数据采集
模块3，语音识别模块4和运动控制器9进行通信；所述运动控制器9输出驱动信号至电机
驱动器8，所述电机驱动器8驱动电机6从而带动车轮组7，所述电源10通过电源转换模
块1分别给RFID模块2，数据采集模块3和语音识别模块4供电；所述导盲机器人个体之间
采用分布式控制结构，通过各自的笔记本之间的WiFi连接，实现导盲机器人个体之间的信息
交互。
其中，上述所述导盲机器人个体都运行机器人操作系统——ROS。
上述所述RFID模块2为射频识别模块，是一种无线通信模块，用来检测并获取环境中
RFID标签的信息。
上述所述数据采集模块3为激光雷达。
上述所述车轮组7由两个驱动轮和一个万向轮组成，成三角形安装在所述车身下层的底
部。
为实现上述目的，本发明解决问题所采取的另一技术方案是：
一种基于ROS的多导盲机器人在未知环境中的地图创建方法，包括以下步骤：
(1)在盲机器人个体所在处区域设置若干个锚点，即在所处区域中的重要标志性特征点
上设置封装有该位置坐标信息的RFID标签；
(2)启动所有导盲机器人个体中的数据采集模块3和RFID模块2；
(3)手动控制导盲机器人个体在各自所在的区域运行一圈，所述数据采集模块3获取区
域环境的障碍物数据信息，所述RFID模块2采集区域中预设的的锚点信息，即RFID标签信
息，其中存储有标志性特征点的坐标；
(4)将步骤3)获取的区域环境中的障碍物数据信息采用导盲机器人半径和定位误差之
和进行膨胀，将障碍物尺寸进行扩大；
(5)运用ROS提供的功能包中的EKF-SLAM算法，创建并保存各自区域的带有锚点坐标
的SLAM地图；
(6)导盲机器人个体通过各自笔记本5的WiFi连接进行数据交互，即步骤5)中生成
的带有锚点坐标的SLAM地图的交互。
上述所述的基于ROS的多导盲机器人在未知环境中的地图创建方法中步骤5)所述的
EKF-SLAM算法的具体步骤是：
1)初始化：将导盲机器人个体的SLAM状态空间向量，误差协方差矩阵，导盲机器人个
体过程控制噪声协方差矩阵，导盲机器人个体观测过程噪声协方差矩阵参数进行初始化定义；
2)状态预测：利用导盲机器人个体运动过程模型，根据k-1时刻的导盲机器人个体位姿，
误差协方差矩阵和k-1时刻到k时刻导盲机器人个体的运动控制量来预测k时刻的导盲机器
人个体自身位姿估计，误差协方差矩阵和导盲机器人个体的观测量；
3)数据关联：在导盲机器人个体的实际观测完成时，建立导盲机器人个体实际从环境中
观测到的特征点的观测值和步骤2)中通过观测模型得到预测值之间的对应关系，计算卡尔曼
增益；
4)状态更新：利用卡尔曼增益和步骤2)中的预测值，更新k时刻的导盲机器人个体SLAM
状态空间向量和误差协方差矩阵；
5)地图扩展：当导盲机器人个体观测到新的特征点时，经过数据关联校验后，将新观测
到的特征点的坐标添加到系统状态向量中，实时更新SLAM地图。
有益效果：本发明与现有技术相比，具有以下的优点和有益效果：
1、采用多导盲机器人，设计和制造多个简单的导盲机器人个体比单个复杂的导盲机器人
个体要容易很多，成本也低很多。
2、导盲机器人个体均采用目前流行机器人操作系统——ROS，由于其本身的开源性，使
得机器人软件系统对环境的适应性更强，同时也提高了软件系统的实时性。
3、使用多台导盲机器人个体可以通过数据共享获取更大范围的环境信息，而不需要直接
观测，提高工作效率和工作范围。
4、导盲机器人个体上使用激光雷达检测环境中的障碍物信息，检测精度高，速度快，抗
干扰能力强。
5、导盲机器人个体上使用RFID模块检测环境中锚点的信息，识别唯一，安全性好，准
确性高。
附图说明
图1为导盲机器人个体结构示意图，
图2为导盲机器人个体控制结构示意框图，
图3为多导盲机器人所处的一种未知环境图，
图4为多导盲机器人个体在未知环境图中的地图创建方法流程框图，
图5为导盲机器人个体创建地图过程中使用的EKF-SLAM算法流程框图。
具体实施方式
下面结合附图，进一步阐述本发明。
本发明涉及一种基于ROS的多导盲机器人及在未知环境中的地图创建方法。该方法中使
用的多导盲机器人包括至少两台导盲机器人个体，每台导盲机器人个体结构相同，可以自主
移动。每台导盲机器人个体都是独立的智能体，能够独立探测自己周围的环境。每台导盲机
器人个体都运行ROS操作系统。导盲机器人个体之间采用分布式控制结构，导盲机器人个体
之间可以通过WiFi连接传输数据，实现导盲机器人个体之间的信息交互。导盲机器人个体之
间的WiFi连接主要是各自笔记本间的WiFi连接。多导盲机器人所处的区域环境中的标志性
特征点上预设有锚点，即RFID标签，其中存储该特征点在全局地图下的坐标，该坐标值是利
用导盲机器人个体半径和定位误差之和进行膨胀，扩大尺寸后的坐标值。启动所有导盲机器
人个体的数据采集模块3和RFID模块2后，手动控制其在区域中运行一圈，通过数据采集模
块3获取区域环境中的障碍物数据信息，RFID模块2采集区域环境中的锚点信息。为保证安
全，将导盲机器人个体上的数据采集模块3获取的区域环境中的障碍物数据信息，利用导盲
机器人个体半径和定位误差之和进行膨胀，将障碍物尺寸进行扩大，运行ROS提供的功能包
中的EKF-SLAM算法，建立并保存各自区域的带有锚点坐标的SLAM地图。导盲机器人个体之
间通过各自的笔记本5的WiFi连接进行生成的带有锚点坐标的SLAM地图的交互。
本发明的导盲机器人个体外形如图1所示，包括车身，所述车身为三层构造，底层设有
车轮组7(车轮组包括两个驱动轮和一个万向轮)，电机6，电机驱动器8和电源转换模块1；
第二层设有运动控制器9，电源10和语音识别模块4；第三层设置有RFID模块2，数据采集
模块3和笔记本5，车身采用分层结构，在每一层间里都合理的布置了相应的模块，在充分
利用导盲机器人车身空间的基础上，避免了各个模块工作时所带来的干扰问题。
本发明的导盲机器人个体控制结构如图2所示，笔记本5通过USB串口分别与RFID模块
2，数据采集模块3，语音识别模块4和运动控制器9进行通信；运动控制器9输出驱动信号
至电机驱动器8，电机驱动器8驱动电机6从而带动驱动轮，电源10通过电源转换模块1分
别给RFID模块2，数据采集模块3和语音识别模块4供电。本发明将整体分割为几个部分，
思路清晰，控制效果显著，各部分功能互相独立，笔记本5和数据采集模块3，运动控制器9，
语音识别模块4和RFID模块2间采用标准接口，整个通信过程稳定、条理清晰、扩展性好，
与盲人之间保持了良好的交互性。
本发明的多导盲机器人所处的未知环境如图3所示，包括三个区域，区域A，区域B和
区域C，区域A内放置有导盲机器人个体A和两个尺寸不同的长方形障碍物，在障碍物的角
点上分别设置一个RFID标签，在区域A的墙角处同样设置一个RFID标签，这些预先设置的
RFID标签中存储有坐标系X1O1Y1下的坐标值，分别为a1(xa1,ya1)，a2(xa2,ya2)，a3(xa3,ya3)；
区域B内放置有导盲机器人个体B和一个长方形的障碍物，区域B内障碍物的角点处设置有
一个RFID标签，在区域B的两个不同的墙角处同样各设置一个RFID标签，标签中存储有坐
标系X2O2Y2下的坐标值，分别为b1(xb1,yb1)，b2(xb2,yb2)，b3(xb3,yb3)；区域C内放置有导盲
机器人个体C和一个长方形的障碍物，区域C内障碍物的角点上设置有一个RFID标签，区域
C中的两个不同的墙角处各设置一个RFID标签，标签中存储有坐标系X3O3Y3下的坐标值，分
别为c1(xc1,yc1)，c2(xc2,yc2)，c3(xc3,yc3)。这些预设的RFID标签称之为锚点。
如图4所示，本发明的基于ROS的多导盲机器人在未知环境中地图创建方法，步骤如下：
(1)在区域A，区域B和区域C中各设置3个锚点，即在所处区域中的重要标志性特征
点上设置封装有该位置坐标信息的RFID标签；
(2)启动导盲机器人个体A，B和C的数据采集模块3和RFID模块2；
(3)手动控制导盲机器人个体A，B和C在各自所在的区域中运行一圈，数据采集模块
3获取区域中的障碍物数据信息，数据采集模块3采用激光雷达，一次扫描可以得到1080个
环境特征点数据(扫描范围270°，扫描体征点间隔0.25°)，这些数据是以激光雷达中心为极
点的极坐标系表示的局部坐标(di,θi)，其中θi＝i*0.25*π/270
第i个特征点在全局坐标下的坐标为：
xi＝xk+dicosθicosαk+disinθisinαk
yi＝yk+dicosθisinαk+disinθicosαk
其中(xk,yk)是k时刻激光雷达在全局坐标系中的坐标，因为激光雷达设置在导盲机器
人个体上，因此认为导盲机器人个体的坐标值即为激光雷达的坐标值，αK为k时刻激光雷达
主轴与全局坐标系X轴的夹角,di表示k时刻激光雷达与第i个特征点之间的距离。
RFID模块2采集区域中的锚点信息，即区域中标志性特征点的坐标，该坐标值是利用导
盲机器人个体半径r和定位误差△之和进行膨胀之后，在各自全局坐标系下的全局坐标值，A
房间：a1(xa1,ya1)，a2(xa2,ya2)，a3(xa3,ya3)；B房间：b1(xb1,yb1)，b2(xb2,yb2)，b3(xb3,yb3)；
C房间：c1(xc1,yc1)，c2(xc2,yc2)，c3(xc3,yc3)；
(4)为保障安全，防止导盲机器人个体距离障碍物过近，发生碰撞，将步骤3)中导盲
机器人个体获取的区域中的障碍物数据信息利用导盲机器人个体半径和定位误差之和进行膨
胀，将障碍物尺寸进行扩大，使得导盲机器人个体与障碍物间始终保持一定的安全距离；
(5)运用ROS提供的功能包中EKF-SLAM算法，创建并保存区域A，区域B和区域C的带
有锚点坐标的SLAM地图，记为SA，SB和SC；
(6)导盲机器人个体A,B,C通过各自笔记本(5)的WiFi连接进行数据交互，即步骤5)
中生成的带有锚点坐标的SLAM地图的交互。即每个导盲机器人个体都拥有三个区域的带有锚
点坐标的SLAM地图。
如图5所示，本发明的多导盲机器人在未知环境中地图创建方法，所采用的EKF-SLAM算
法步骤如下：
在基于特征的导盲机器人个体的实时定位与地图创建中，把导盲机器人个体在运动过程
中的位姿x和地图中所有特征点M存储在一个独立的状态空间向量X中，X可表示为：
X(k)＝[xk，Mk]T
其中M＝(m1，m2，...，mN)表示区域中所有特征点的集合，mi＝(xi，yi)表示地图环
境中第i个特征点的坐标。
导盲机器人个体运动过程模型为：xk＝f(xk-1，uk-1)+ωk
式中非线性函数f(*)将k-1时刻的状态映射到k时刻的状态，xk表示k时刻的导盲机器人
个体的位姿，uk-1表示由k-2时刻到k-1时刻的导盲机器人个体的运动控制量，随机信号ωk表
示导盲机器人个体运动过程中的过程控制噪声，其均值为0，协方差为Qk。
导盲机器人个体的观测模型为：zk＝h(X(k))+vk
式中非线性函数h(*)为导盲机器人个体的观测函数，随机信号vk表示导盲机器人个体观
测过程中的观测噪声，其均值为0，协方差为Rk。
EKF-SLAM算法包括以下步骤：
1)初始化：对导盲机器人个体的SLAM状态空间向量X(0),误差协方差矩阵P0，导盲机器
人个体运动过程噪声协方差矩阵Q0，导盲机器人个体观测噪声协方差矩阵R0，导盲机器人个
体运控过程控制量u0，等参数进行初始化定义和赋值；
2)预测：利用导盲机器人个体的运动过程模型，k-1时刻导盲机器人个体自身位姿xk-1，
误差协方差矩阵Pk-1和k-1时刻到k时刻导盲机器人个体的运动控制量uk来预测k时刻导盲
机器人个体的位姿估计xk|k-1，导盲机器人个体SLAM状态空间向量X(K|K-1)和误差协方差矩
阵Pk|k-1，其中：
xk|k-1＝f(xk-1|k-1，uk)+ωk
X(k|k-1)＝[xk|k-1，Mk-1]T
Pk|k-1＝fxPk-1(fx)T+fuQk(fu)T
其中，fx和fu分别表示机器人运动模型关于SLAM状态向量X(k|k-1)和控制量uk的雅克比
(Jacobian)行列式,
3)实际观测：应用导盲机器人个体的数据采集模块获得区域中第i个特征点的观测值
4)观测预测：根据导盲机器人个体观测模型和步骤2)中预测的k时刻导盲机器人个体
SLAM状态空间向量可得区域中第i个特征点的预测值
z k | k - 1 i = h i ( X ( k | k - 1 ) ) + v k ]]>
5)匹配：采用最邻近算法将观测到的特征点与最近的已知特征点关联。计算新息矩阵Sk，
Sk＝hXPk|k-1(hX)T+Rk
并使其对称化： S n e w ( k ) = ( S K + S k T ) / 2 ]]>
将上面新生成的对称化新息矩阵进行Cholesky分解使得递归运算过程更加稳定，公式如
下：
SChol(k)＝Chol(Snew(k))
计算卡尔曼增益 K k = P k | k - 1 ( h X ) T S C h o l ( k ) - 1 ( S C h o l ( k ) - 1 ) T ]]>
6)状态更新：判断导盲机器人个体检测到得特征点是否与k-1时刻的地图匹配，当匹配
的时，则利用卡尔曼增益修正k-1时刻的导盲机器人个体状态空间向量X(k|k-1)，得到k时
刻导盲机器人个体状态空间向量X(k)和误差协方差矩阵Pk。
X ( k ) = X ( k | k - 1 ) + K k ( z k i - z k | k - 1 i ) ]]>
Pk＝(I-KkhX)Pk|k-1
式中hx表示导盲机器人个体观测模型关于状态空间X(k|k-1)的雅克比行列式。
7)当与k-1时刻的地图不匹配时，则判断该特征点是否是新的特征点，若不是新的特征
点，则移除该特征点的测量数据。
8)地图扩展：当导盲机器人个体观测到特征点是新的特征点时，经过数据关联校验后，
需要把新观测到的特征点的坐标添加到系统状态向量中，实时更新地图，运用扩展模型g(*)对
新的导盲机器人个体状态空间向量和误差协方差矩阵进行更新。新观测到的特征点mi+1坐标
可表示为：
m i + 1 = g ( x k , z k i + 1 ) ]]>
那么包含新特征点的导盲机器人个体状态空间向量X*(k)为：
X*(k)＝[xk，Mk，mi+1]
对应的协方差矩阵为：
P k * = J x , z P k 0 0 R k ( J x , z ) T ]]>
其中, J x , z = I x 0 0 0 I M 0 g x 0 g z ]]>
式中gx和gz是g(*)关于机器人位姿向量和观测向量的雅克比行列式。
以上所述，仅为本发明较佳的具体实施方式。当然，本发明还可有其它多种实施例，在
不背离本发明精神及其实质的情况下，任何熟悉本技术领域的技术人员，当可根据本发明作
出各种相应的等效改变和变形，都应属于本发明所附的权利要求的保护范围。