初始化定义和赋值；
[0065] 2)预测：利用导盲机器人个体的运动过程模型，k-1时刻导盲机器人个体自身位 姿Xk1，误差协方差矩阵Pk1和k-1时刻到k时刻导盲机器人个体的运动控制量Uk来预测k时刻导盲机器人个体的位姿估计Xkik1，导盲机器人个体SLAM状态空间向量X(KIK-1)和误 差协方差矩阵Pkikl，其中： 阳066] Xk|k 1= f (Xk Ilk 1，Uk) + ?k阳067] X(k|k-l) = [Xk|ki，Mki]T [0068] Pk|ki=fxPki(fx)T+fuQk(fjT W例其中，t和f。分别表示机器人运动模型关于SLAM状态向量X(k|k-1)和控制量Uk的雅克比（Jacobian)行列式，
[0070] 3)实际观测：应用导盲机器人个体的数据采集模块获得区域中第i个特征点的观 测值4
[0071] 4)观测预测：根据导盲机器人个体观测模型和步骤2)中预测的k时刻导盲机器 人个体SLAM状态空间向量可得区域中第i个特征点的预测值
[0072]
[0073] 5)匹配：采用最邻近算法将观测到的特征点与最近的已知特征点关联。计算新息 矩阵Sk，
[0074]
阳0巧]并使其对称化
[0076] 将上面新生成的对称化新息矩阵进行化olesky分解使得递归运算过程更加稳 定，公式如下：
[0077] Schol似=Ch〇l(Sne?(k)) 阳07引计算卡尔曼增益
[0079] 6)状态更新：判断导盲机器人个体检测到得特征点是否与k-1时刻的地图匹配， 当匹配的时，则利用卡尔曼增益修正k-1时刻的导盲机器人个体状态空间向量X化|k-i)， 得到k时刻导盲机器人个体状态空间向量X(k)和误差协方差矩阵Pk。
[0080]
阳0川 Pk= (I-K山)Pkiki
[0082] 式中hy表示导盲机器人个体观测模型关于状态空间X化|k-l)的雅克比行列式。
[0083] 7)当与k-1时刻的地图不匹配时，则判断该特征点是否是新的特征点，若不是新 的特征点，则移除该特征点的测量数据。
[0084] 8)地图扩展：当导盲机器人个体观测到特征点是新的特征点时，经过数据关联校 验后，需要把新观测到的特征点的坐标添加到系统状态向量中，实时更新地图，运用扩展模 型g(*)对新的导盲机器人个体状态空间向量和误差协方差矩阵进行更新。新观测到的特 征点mw坐标可表示为： 阳0化]
[0086] 那么包含新特征点的导盲机器人个体状态空间向量沪化）为： 阳〇87] X* 似=[Xk，Mk，mw] 阳08引对应的协方差矩阵環为：
[0091] 式中&和g,是g(*)关于机器人位姿向量和观测向量的雅克比行列式。
[0092] W上所述，仅为本发明较佳的【具体实施方式】。当然，本发明还可有其它多种实施 例，在不背离本发明精神及其实质的情况下，任何熟悉本技术领域的技术人员，当可根据本 发明作出各种相应的等效改变和变形，都应属于本发明所附的权利要求的保护范围。
【主权项】
1. 一种基于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连接，实现导盲机器人个体之间 的信息交互。2. 根据权利要求1所述的基于R0S的多导盲机器人，其特征在于，所述RFID模块（2) 为射频识别模块。3. 根据权利要求1所述的基于R0S的多导盲机器人，其特征在于，所述数据采集模块 (3)为激光雷达。4. 根据权利要求1所述的基于R0S的多导盲机器人，其特征在于，所述车轮组（7)由两 个驱动轮和一个万向轮组成，成三角形安装在所述车身下层的底部。5. -种根据权利要求1所述的基于R0S的多导盲机器人在未知环境中的地图创建方 法，其特征在于，包括以下步骤： (1) 在导盲机器人个体所在处区域设置若干个锚点，即在所处区域中的重要标志性特 征点上设置封装有该位置坐标信息的RFID标签； (2) 启动所有导盲机器人个体中的数据采集模块（3)和RFID模块（2); (3) 手动控制导盲机器人个体在各自所在的区域运行一圈，所述数据采集模块（3)获 取区域环境的障碍物数据信息，所述RFID模块（2)采集区域中预设的锚点信息，即RFID标 签信息，其中存储有标志性特征点的坐标； (4) 将步骤（3)获取的区域环境中的障碍物数据信息采用导盲机器人半径和定位误差 之和进行膨胀，将障碍物尺寸进行扩大； (5) 运用R0S提供的功能包中的EKF-SLAM算法，创建并保存各自区域的带有锚点坐标 的SLAM地图； (6) 导盲机器人个体通过各自笔记本（5)的WiFi连接进行数据交互，即步骤（5)中生 成的带有锚点坐标的SLAM地图的交互。6. 根据权利要求5所述的基于R0S的多导盲机器人在未知环境中的地图创建方法，其 特征在于，步骤（5)所述的EKF-SLAM算法的具体步骤是： 1) 初始化：将导盲机器人个体的SLAM状态空间向量，误差协方差矩阵，导盲机器人个 体过程控制噪声协方差矩阵，导盲机器人个体观测过程噪声协方差矩阵参数进行初始化定 义； 2) 状态预测：利用导盲机器人个体运动过程模型，根据k-Ι时刻的导盲机器人个体位 姿，误差协方差矩阵和k-Ι时刻到k时刻导盲机器人个体的运动控制量来预测k时刻的导 盲机器人个体自身位姿估计，误差协方差矩阵和导盲机器人个体的观测量； 3) 数据关联：在导盲机器人个体的实际观测完成时，建立导盲机器人个体实际从环境 中观测到的特征点的观测值和步骤2)中通过观测模型得到预测值之间的对应关系，计算 卡尔曼增益； 4) 状态更新：利用卡尔曼增益和步骤2)中的预测值，更新k时刻的导盲机器人个体 SLAM状态空间向量和误差协方差矩阵； 5) 地图扩展：当导盲机器人个体观测到新的特征点时，经过数据关联校验后，将新观 测到的特征点的坐标添加到系统状态向量中，实时更新SLAM地图。
【专利摘要】一种基于ROS的多导盲机器人，由至少两台结构相同的导盲机器人个体组成，导盲机器人个体含上中下三层结构的车身，下层设有车轮组，电机，电机驱动器和电源转换模块；中层设有运动控制器，电源和语音识别模块；上层设有RFID模块，数据采集模块和笔记本；笔记本通过USB串口分别与RFID模块，数据采集模块，语音识别模块和运动控制器进行通信；运动控制器输出驱动信号至电机驱动器，电机驱动器驱动电机带动车轮组，电源通过电源转换模块分别给RFID模块，数据采集模块和语音识别模块供电；导盲机器人个体间采用分布式控制结构，通过各自的笔记本之间的WiFi连接，实现导盲机器人个体间的信息交互。本发明还公开了基于ROS的多导盲机器人在未知环境中的地图创建方法。
【IPC分类】G01C21/32
【公开号】CN105403222
【申请号】CN201510896947
【发明人】陈超, 王若怡, 刘志
【申请人】江苏科技大学
【公开日】2016年3月16日
【申请日】2015年12月8日