system: [SYSTEM]

user: 3. 虚拟场景的构建 3.1. SLAM地图建建立 针对实验环境地图构建的方法上，我们采用即时定位与地图构建(Simultaneous Localization and Mapping， SLAM)技术 [ 2 ]，该技术目前被广泛的应用于地图构建。SLAM的核心思想是根据机器人的观测值 z 1 : t (地图中的点)和里程计测量序列 u 1 : t − 1 (机器人轨迹)来估计机器人轨迹分布 x 1 : t 和地图m，以此获得联合后验概率函数 P ( x 1 : t ， m | z 1 : t ， u 1 : t − 1 ) ，而为了简化计算，往往将其拆分成位姿状态估计和地图状态估计，即： P ( x 1 : t ， m | z 1 : t ， u 1 : t − 1 ) = P ( m | x 1 : t ， z 1 : t ) P ( x 1 : t | z 1 : t ， u 1 : t − 1 ) 这一举措即是RBPF(粒子滤波算法)的思想，而我们采用的Gmapping算法技术在RBPF基础上，针对RBPF所有粒子多和频繁执行采样问题提出了改进提议分布和选择性采样，优化了常规RBPF算法，使得实现SLAM建图更加准确。 常规RBPF粒子滤波算法总体可概括为采样、权重计算、重采样、地图估计。而Gmapping在此基础上提出改进提议分布，在激光雷达的帮助下获取比里程计更准确的环境数据，得到改进后的建议分布 [ 3 ]： P ( x t | m t − 1 ( i ) ， x t − 1 ( i ) ， z t ， u t − 1 ) = P ( z t | m t − 1 ( i ) ， x t ) P ( x t | x t − 1 ( i ) ， u t − 1 ) P ( z t | m t − 1 ( i ) ， x i − 1 ( i ) ， u t − 1 ) 且其计算权重时的公式为： w t ( i ) = P ( x 1 : t ( i ) | z 1 : t ， u 1 : t − 1 ) π ( x 1 : t ( i ) | z 1 : t ， u 1 : t − 1 ) 使用贝叶斯加全概率展开等变换，可以得权重计算近似为： w t ( i ) ∝ w t − 1 ( i ) ⋅ P ( z t | m t − 1 ( i ) ， x i − 1 ( i ) ， u t − 1 ) 而随着粒子不断的迭代，在选择性采样的协助下，低权重粒子被舍去，高权重粒子被采纳，在重采样的过程中，以设定阀值 N e f f 的方式来评估粒子权重分散程度，即： N e f f = 1 ∑ i = 1 M ( φ ( i ) ) 2 其中 φ ( i ) 表示粒子归一化的权重，在粒子数目为M的情况下，只有在 N e f f < 1 2 时，系统才会进行重采样 [ 4 ]。 在SLAM的实现上，算法上ROS提供了实现Gamaping算法的功能包，在完成功能包中的相关参数配置以及激光雷达、里程计的配置后，即可实现定位与建图的功能。 图3是机器人执行建图时当前系统运行节点关系图： 图3. SLAM建图节点关系图 图3中可直观的观察到建图时系统涉及到的节点，以及节点间发布订阅话题的关系。其中/slam_gampping作为Gmapping算法的核心节点分别订阅了来自/hawkbot_lds节点发布的激光雷达/scan话题、/robot_state_publisher的底盘与里程计原点坐标变换/tf_static话题、/imu_complementary_filer的IMU提供的相对定位信息/tf话题、/base_link_to_laser的底座与激光雷达坐标变换/tf话题，此外/teleop则提供键盘控制话题，最终所有的输出都将作为/robot_pose_ekf节点的输入来控制机器人的移动。 在启动/slam_gmapping节点的同时，一并启动Rviz组件，地图将在Rviz中建立。完成二维地图构建后，在命令行终端使用指令： 将建立好的地图保存后，文件名即为map_1，保存的二维地图(图4)和真实的实验场景(图5)： 图4. 机器人构建的二维地图 图5. 机真实实验场景图 3.2. ROS地图到Unity场景 3.2.1. ROS地图的尺寸计算 根据上文所述，在Gmapping建图过程中，使用ROS指令可以保存的构建的地图文件，其中会生成一个map_1.yaml和一个map_1.pgm文件，地图图像保存在map_1.pgm文件中，map_1.yaml文件则是对地图的参数的说明，而map_1.pgm文件可以通过Ubuntu中的GIMP图片编辑软件转成.png格式用于Unity加工，根据map_1.yaml文件中的resolution(分辨率)参数，其代表的是每个像素对应的实际距离(0.05 m/pixel)，再利用GIMP测量工具(图6)，测量出地图轮廓的像素值，再乘以分辨率即可知实际距离。 图6. GIMP测量图 实际实验场地尺寸计算公式即为： 实际距离=测量像素值×分辨率(resolution) 以下是对图5地图中每一条黑线(边界)的像素尺寸的测量值列表1： Table 1 Experiment of map size measuremen 边编号 第一次测量(/像素) 第二次测量(/像素) 第三次测量(/像素) 平均像素(/像素) 实际尺寸(/m) ① 43.6 42.4 44.2 43.40 2.17 ② 28.3 27.8 29.1 28.40 1.42 ③ 31.3 32.7 31.9 31.97 1.60 ④ 44.9 45.1 45.6 45.2 2.26 ⑤ 53.7 52.3 54.3 53.43 2.67 表1. 地图尺寸测量实验 由此我们可以在没有人员到实际场景进行手工测量的情况下便可实现远程尺寸测量。 3.2.2. Unity虚拟场景构建与导航网格烘焙 结合Unity中像素与距离的关系，对导入的地图文件进行适当放缩，即可达到尺寸匹配的效果。经过查阅Unity中场景元素的属性，基础单位为米，参照导入Unity的ROS地图创建并调节出对应尺寸的GameObject对象“plane”，以该对象为载体调用C#脚本代码计算出plane对象的尺寸即求出ROS地图在Unity虚拟环境中的尺寸，关键代码如下： 以宽度 w 虚 为例，为plane添加MeshFilter组件，以此获取原始宽度default_w，再将原始宽度乘以放缩比即可得实际宽度map_w，长度 l 虚 以此类推，数据取整，计算出虚拟场景得： l 虚 × w 虚 = 10 × 13 与对于④号边与⑤号边实际长度的比值k分别为： k 1 = w 虚 l 4 = 4.425 ， k 2 = l 虚 l 5 = 4.869 ， k _ = ( k 1 + k 2 ) ÷ 2 = 4.647 则虚拟机器人与显示机器人的速度关系即根据该虚实交互比值来限定，以此提高两者的同步性能。 导入Unity后，为了更加体现出本项目的实用性，我们利用Unity在ROS地图基础上模拟构建了一个工业生产场景(如图7)。 在虚拟机器人的运动控制方面，我们采用Unity的导航功能，模拟虚拟机器人将物料配送到入料口的过程，该功能需要采用的Unity的Navigation导航网格 [ 5 ] 组件进行网格烘焙，同时为虚拟机器人添加上NavMeshAgent组件，而导航网格的作用是使得添加了NavMeshAgent组件的物体能够在导航网格上以最优路径移动到设定的终点，由此可实现自动配送物料的功能。 图7. Unity添加工业模型并导航烘焙后的虚拟场景 3.2.3. Unity虚拟场景操作界面 为了测试便利以及交互性的提高，我们使用Unity的GUI界面技术 [ 6 ] 在Unity客户端设计了用于人机交互的操作界面(如图8)，同时在PC端连接了外接摄像头，该摄像头处于实验场景，可将真实场景画面传送至Unity操作界面。在点击运行后，Unity会同时运行两个程序线程，主线程完成加载客户端界面，副线程则完成与ROS的网络链接。 界面上的操作区，空白处用于显示提示信息，“开启信号灯识别”按钮用于Unity接收识别信号灯的控制信号，如遇红灯则提示“通行状态如下：红灯停车等待中”，无信号灯或者为绿灯则提示“通行状态如下：可通行”。而“前进”、“左转”、“停止”、“右转”、“后退”按钮则可手动控制ROS机器人移动。“启动导航”按钮则是进行虚实交互控制的启动按钮，当按下该按钮后，提示“机器人已启动”，虚拟机器人首先移动，Unity发布控制信息给ROS机器人使之随动。具体的操作流程如图9。 图8. Unity初始化操作界面 图9. Unity界面操作流程图 3.3. 虚实交互方式 3.3.1. Unity导航对ROS机器人运动控制 针对Unity与ROS机器人的运动交互上，我们运用到了Unity导航功能中的NavMeshPath类型组件，通过定义该类型的变量path，将导航起始点坐标作为输入，path作为输出，调用NavMesh.CalculatePath()函数即可计算出导航的路径信息并保存在变量path中，path中所包含的Corners数组则存储了导航路径的所有关键点的坐标，因为Unity导航路线是由Corners关键点连接起来的(如图10所示)，通过比较相邻两点的坐标，可以很容易得到两点间的距离和方向转角。 图10. Unity界面操作流程图 如图设CD近似与轴重合(即此时机器人的正方向为CD)，故CD与轴夹角为，同时设 D ( x d ， y d ) ， E ( x e ， y e ) ，D、E点与轴的夹角分别为 θ 1 、 θ 2 ，若机器人从C点移动到D点，在D点停止时机器人的正向仍为CD，则以此时状态移动到E点的过程有： tan θ 1 = x e − x d y e − y d ， θ 1 = arctan x e − x d y e − y d ， Δ θ 1 = θ 1 − θ 0 即为机器人C到D的末态方向与D到E的初态方向夹角，同理可得E到F点的运动方向夹角为： Δ θ 2 = θ 2 − θ 1 ，同时DE的运动距离为： 或 y e − y d cos Δ θ 1 以上述方法可计算出任意相邻关键点的距离与转角，并将其转化为控制信息即可对ROS现实机器人进行虚实交互控制。 3.3.2. 机器视觉识别交通信号灯 在现代工业场景中，为了更好的协调各个车辆工作的有序进行，必然会存在指示灯这类设施。于是，我们基于机器视觉为机器人设计了，针对识别交通信号灯控制其移动与停止的功能。 我们以一广角摄像头作为视觉交互设备。使用OpenCV开源计算机视觉库 [ 7 ] 实现视觉识别算法，并利用ROS所提供的CV_bridge视觉框架将OpenCV和ROS结合，实现通过ROS平台控制机器人调用视觉识别功能。ROS本身并不能直接使用OpenCV开源库函数，而在ROS提供的CV_bridge的帮助下可将两者紧密地联系起来，通过其内部函数，可将OpenCV返回的Mat类型图像数据转换为ROS内部通讯机制能够识别的sensor_msgs/image消息格式进行传输，也可逆向转换。而交通信号灯的识别，则是基于OpenCV的颜色空间模型 [ 7 ] 来实现颜色识别的。 在颜色模型选择上，我们使用RGB和HSV。RGB是最为常用的颜色空间模型，其显示效果同人眼所见最为相近，但是对于机器来说，RGB很难较好的反应图像中的具体信息。而HSV模型包含的色调(H)、饱和度(S)、亮度(V)三个参量，能够实现图像的特征调节凸显所要识别的颜色，可极大提高颜色间的对比效率。 由于基于ROS平台运行，摄像头读入的RGB信息转换为的是ROS的图像数据，通过话题发布此消息，经CV_bridge将该消息转换为OpenCV图像数据，再经一系列图像处理(如图10)，即可识别出特定颜色。针对交通信号灯，主要是对红色与绿色的识别，经过操作界面(如图12)调节得红色的三参数取值范围分别为H[0，10]，S[43，255]，V[46，255]，绿色的三参数取值范围分别是H[35，77]，S[43，255]，V[46，255]。 识别算法的实现过程如图11，同时红色识别并框选如图13，绿色类似。 图11. 算法流程图 图12. HSV操作界面 图13. 红色识别框选效果 而在虚实交互上，识别到红色指示和识别到绿色指示时，该节点会发布/traffic_deal话题，该话题会发送红色与绿色的对应数据，Unity通过订阅此话题即可让虚拟机器人知道此时是红灯还是绿灯。