【www.zhangdahai.com--教学设计】
宋鑫鹏，赵 倩
(曲阜师范大学工学院，山东 日照 276800)
全球新冠感染疫情防控仍不容懈怠，公共场所疫情防控的消杀环节是防控重点。然而，国内外的防疫工作主要采用人工消杀。人工消杀作业过程繁琐、消杀覆盖率低、危险场所无法保障人员安全等弊端逐渐显现。国内外公司均已开发出多款面向疫情防控的消杀防疫机器人[1]。这些机器人虽存在一定的短板，但其扩展性高、路径规划可选择，优点十分明显。
为了简化作业流程、提高消杀覆盖率和保障人员安全，本文设计了1种基于机器人系统(robot operating system,ROS)的无人消杀机器人系统。该系统结合了市场上一些消杀机器人系统的扩展性高和路径规划可选择的特点，可自主调节消杀药液浓度，通过自主导航进行环境消杀，并通过上位机调度软件对机器人进行实时监控，以实现公共场所消杀的无人化和智能化。
基于ROS的无人消杀机器人系统由上位机调度软件、搭建在Jeston nano上的ROS机器人操作系统、STM32系列单片机、激光雷达、九轴惯性测量单元(inertial measurement unit，IMU)、编码器、药液箱和雾化喷射装置组成。上位机调度软件使用Pyside2进行可视化界面设计，并利用Socket与ROS中的调度节点进行通信，以实现数据的双向传输。在ROS中，利用基于粒子滤波的Gmapping算法对未知环境进行地图构建。地图的精度直接影响了在执行消杀任务时的导航精度。Gmapping算法构建的地图精度与里程计的精度有关。为了提高里程计的精度和鲁棒性，本文采用了基于扩展卡尔曼滤波的里程计融合方案，对激光雷达里程计、轮式里程计和IMU进行融合，以提高定位精度。同时，本文通过基于粒子滤波的自适应蒙特卡洛定位(adaptive Monte Carlo localization，AMCL)发布的Map坐标系到Odom坐标系的坐标变换，进一步提高了定位精度。在消杀导航阶段：全局路径规划采用带有启发式搜索的A*算法；
局部路径规划采用动态窗口法(dynamic window approach，DWA)。药液喷洒部分由霍尔流量计、可调速吸泵、射流器和雾化喷头组成。上位机调度软件通过Socket与机器人上位机Jeston nano通信。Jeston nano与机器人下位机STM32单片机进行串口通信，从而实现远程操控机器人的目的。系统结构如图1所示。
图1 系统结构框图
无人消杀系统主要包括任务调度系统、定位系统、地图构建系统、自主导航系统和药液喷洒系统这5个子系统。
2.1 任务调度系统
任务调度系统主要用于监测并控制机器人，通过远程通信观察每个机器人的运行状态和消毒液剩余量，并对不同编号的机器人进行任务分配。使用任务调度系统可达到减少人工参与、降低人工成本、保障工作人员安全的目的。调度软件界面如图2所示。
图2 调度软件界面
调度软件界面采用Pyside2设计。在界面内可观察到当前机器人在线数量，并可选择当前操作机器人的编号、调整消毒液浓度、发送指令使机器人前往指定区域消杀。与机器人通信时，采用Socket编程中的面向连接编程，即传输控制协议/网际协议(transmission control protocol/internet protocal,TCP/IP)。此通信方法比较固定，需要在调度软件上建立服务端、在机器人上建立客户端。在建立服务端的同时，设置监听多客户端，以实现一对多的通信。在每个机器人机载电脑的ROS中分别单独建立1个节点，用于与调度软件的通信。该节点实时监听来自调度软件的指令，通过自定义话题发出控制信息并实时上传机器人的运行状态。
2.2 定位系统
机器人定位系统包括里程计定位与AMCL全局定位。里程计定位用于估计机器人位姿。AMCL全局定位用于减小里程计误差。目前，3种主流里程计分别是基于IMU的里程计、轮式里程计和视觉里程计。基于IMU的里程计是较为理想的估计空间方向的里程计，但在测量时无法抵消测量的重力分量，会随着时间的推移产生里程漂移。轮式里程计[2]被广泛应用于轮式或腿式机器人的运动估计，但在建立机器人模型时经常因为打滑而导致里程不准确。视觉里程计则是运动估计中较优秀的解决方案，适用于不同运动模型的机器人。但视觉里程计受环境影响严重，且计算量较大。除以上3种主流方式外，本文设计还采用了1种效率高、准确度高且适用性强的里程计方案——激光雷达里程计。
本文设计中使用的里程计和AMCL全局定位大致分为5个部分，分别为激光雷达里程计、轮式里程计、轮式里程计标定、扩展卡尔曼滤波和AMCL全局定位。
2.2.1 激光雷达里程计
激光雷达里程计具有快速性和准确性，可为机器人的里程计提供较快的响应和较为准确的信息。本文设计采用的rf2o_laser_odometry是1种基于距离流约束方程的估计2D激光雷达位姿的方法[3]。该方法不依赖于机器人的运动类型，适用于激光雷达的测绘、避障或定位，并且在平移和旋转方面都优于点对点扫描的极轴扫描匹配(polar scan match，PSM)算法和点对线迭代最近点(point to line iterative closest points，PL-ICP)扫描匹配算法。
2.2.2 轮式里程计
激光雷达里程计的原理是利用扫描点建立距离流约束方程。若将机器人放置于长走廊中，激光轮廓scan_match匹配机制就会失效，导致机器人无法精确估计走廊方向的位姿变化。但轮式里程计对环境的依赖程度较小，即便在长走廊环境下也可对机器人位姿进行估计。因此，在里程计方案中引入轮式里程计[4]，可提高里程计的准确性和鲁棒性。轮式里程计由底盘的运动学模型和每个编码器数值计算得到。底盘模型如图3所示。
图3 底盘模型图
2.2.3 轮式里程计标定
理想情况下，激光雷达里程计所估计出的机器人运动状态[vLωL]与轮式里程计计算的机器人运动状态[vω]应相等，则有：
(1)
式中：T为采样周期；
ni为图3中第i号(i=1,2,3,4)轮子编码器采样结果；
γ为编码器每米脉冲数；
μ为原地旋转1周的机器人左右2列轮子编码器累计值差值的绝对值。
假设雷达里程计估计姿态服从高斯分布，则以最小二乘法估计参数γ和μ，以提高里程计计算精度。
2.2.4 扩展卡尔曼滤波
由前文分析可知，激光雷达里程计与轮式里程计可以进行优势互补。轮式里程计本身容易存在较大的累积误差，若标定精度高，则里程计定位精度也会得到大幅提高。本文设计利用扩展卡尔曼滤波对激光雷达里程计、轮式里程计和IMU这3种检测信息进行融合[5]。卡尔曼滤波器是1种状态最优估计算法，采用卡尔曼增益在预测值的基础上进行修正。
robot_localization功能包中的扩展卡尔曼滤波节点以式(2)中的15维状态向量表示机器人运动状态：
(2)
在功能包的配置文件中，可自定义每个被融合信息的输入。融合节点根据配置文件从激光雷达里程计、轮式里程计和IMU中选择相应数据，对扩展卡尔曼滤波预测值进行校正，以实现里程计融合。
2.2.5 AMCL全局定位
导航任务的实现离不开精准的机器人位姿估计。AMCL是移动机器人在二维环境中的概率定位系统[6]，采用粒子滤波器跟踪已知的机器人位姿，根据粒子的分布估计机器人位姿的概率分布，被广泛应用于解决机器人的全局定位问题。AMCL是在蒙特卡罗定位算法的基础上改进而成的，解决了机器人绑架问题和粒子冗余问题。在本文设计中：AMCL在机器人导航阶段提供了Map与Odom坐标系之间的坐标变换关系；
扩展卡尔曼滤波里程计融合器提供了Odom与Base_link之间的坐标变换。在理想情况下，Map坐标系与Odom坐标系时刻保持重合，即里程计与真实值之间无误差。在实际情况下，外部环境及传感器自身存在误差，因此里程计信息往往与真实值之间存在误差。AMCL提供了Map与Odom之间的坐标变换，进一步提高了定位精度，为本文设计的导航功能提供了前提和保障。
2.3 地图构建系统
与Hector_slam算法和Cartographer算法对比，Gmapping算法对激光雷达扫描频率要求更低，且在中小型地图中有着更高的准确度。因此，本文设计采用Gmapping算法构建二维栅格地图。
在同步定位与地图构建(simulteneaus localrzation and mapping,SLAM)时需要解决2个问题，分别是机器人的定位，以及对地图的特征评估。基于Rao-Blackwellized粒子滤波(Rao-Blackwellized particle filters，RBPF)的SLAM方案先解决机器人定位问题，再根据机器人定位解决建图问题。本文设计使用的SLAM方案是基于RBPF的Gmapping算法。该算法基于RBPF，在定位部分完成了2个改进，分别为改进提议分布和选择性重采样[7]。
粒子滤波(particle filters，PF)原理在Gmapping中用于估计轨迹的后验概率。1个粒子需要保存所有历史时刻的机器人位姿和整个地图信息，对机器人位姿有较高要求，并且频繁的重采样会导致粒子耗竭。若机器人的里程计误差较大，那么提议分布与目标分布误差也会较大，将需要更多的粒子才能更好地估计机器人位姿。这会占用更多的内存。RBPF仅依靠里程计数据计算提议分布(位姿的概率分布)。Gmapping利用激光雷达最近1帧的观测值将提议分布限制在1个有效的狭窄区域，得到改进提议分布，再对改进提议分布进行采样。这样可以使用更少的粒子覆盖机器人位姿的概率分布。同时，Gmapping算法通过选择性重采样的方法来减少重采样次数。在重采样步骤前增加一步判断，引入式(3)计算粒子之间的权重差距：
(3)
式中：Neff为权重分散度量值；
W(i)为单个粒子权重。
Neff的值越大，表示粒子之间权重差距越小，有越多的粒子能反应真实值，粒子也越集中。当Neff小于设定阈值时，意味着较少的粒子能反映真实值，此时进行重采样可有效减少重采样次数、避免粒子耗竭。Gmapping在完成上述2种改进后，则更适合在中小型地图场景下解决SLAM问题。
2.4 自主导航系统
无人驾驶系统在获得目的地信息后，首先经过全局路径规划器规划出1条大致可行的路线，而后调用局部路径规划器根据这条全局路线和局部代价地图信息在局部作出运动策略的调整。由于消杀任务的不同，导致需要使用不同的全局路径规划器。在定点消杀任务中使用A*全局路径规划算法能保证规划路径较短，在区域消杀任务中需要使路径尽可能覆盖指定区域。因此，本文选用全覆盖全局路径规划(complete coverage path planning，CCPP)算法。
A*算法的代价函数式为：
F=H+G
(4)
式中：H为从当前节点到目标节点的代价；
G为从开始节点到当前节点的代价。
A*算法通过寻找父节点，以代价函数式计算待检查列表中节点的代价，再作代价比较并寻找新的父节点，不断维护待检查列表与已检查列表，直至父节点为目标点。所有父节点相连即为所规划路径[8]。
局部路径规划对系统能否安全且准确到达目标点有着重要的影响。当局部代价地图中出现障碍物时，局部路径规划器可作出相应的调整，以实现动态避障。此设计采用的局部路径规划器是动态窗口法(dynamic window approach，DWA)。
DWA流程如图4所示。
图4 DWA流程图
DWA实现的大致步骤为速度采样、轨迹采样和评价[9]。在速度采样的过程中，对速度的取样范围进行了限制。假设在初始时刻的车体线速度为v0，在参数配置中设置最大加速度为am，则Δt后速度的范围为v0±amΔt。同理可得角速度的范围为ω0±αmΔt。在所设定的范围内完成取样后，组合不同的线速度和角速度，可以得到不同的运动状态。轨迹采样根据当前速度采样结果对车体的轨迹进行延伸。评价函数用于对得到的多组轨迹进行评价。若目标点为可达目标点，则选择分数最高的轨迹，发布此条轨迹速度信息，从而实现对车体的控制。
在A*全局路径规划器与DWA局部路径规划器的配合下，机器人可完成定点导航的功能。但实际应用时，消杀机器人需定时对环境进行消杀，或对环境进行循环消杀。为了尽可能地覆盖消杀区域，需要使用CCPP算法规划全局路径。因此，在调用全局路径规划器时需先判断当前任务为定点消杀还是区域消杀，再根据任务需求选择A*全局路径规划器或CCPP。路径规划流程如图5所示。
图5 路径规划流程图
为解决全覆盖路径问题，众多学者采用了随机覆盖法，即让机器人在环境中随意移动，检测到障碍物后旋转一定角度并继续移动，由此循环。虽然该方法可行，但覆盖率低，难以满足消杀机器人全覆盖的需求。此外，该方法耗时过长，无法保证消杀效率。基于激励神经网络的全覆盖路径规划算法可用于较为复杂的环境。但该算法会出现路径过长、重复率高、转弯次数多且容易陷入死区的问题。移动规则法可通过对机器人移动方向加以引导，使得全覆盖路径重复率和转弯次数降低，在路径陷入死区后使用A*算法寻找最短路径逃离死区[10]。因此，移动规则法的全覆盖路径规划算法更适用于消杀机器人的全覆盖消杀任务。
2.5 药液喷洒系统
药液喷洒系统由药液喷洒装置与水泵控制系统组成。喷洒装置由可调速水泵、霍尔流量计、射流器和雾化喷头组成[11]。该系统可将纯消毒液和水以一定速度吸出，流经霍尔流量计后在射流器充分混合，在雾化喷头处雾化喷出。
喷洒装置结构如图6所示。
图6 喷洒装置结构图
在不同场景下，消毒液浓度不同。可在上位机调度系统中对机器人的消毒液浓度进行选择，以适应不同的场景。由于水泵控制系统对快速性要求不高，因此可采用闭环控制系统。水泵控制系统结构如图7所示。
图7 水泵控制系统结构图
根据图2所示的调度软件界面，可在右侧调节区域中消毒液浓度栏中设定机器人的期望消毒液浓度。调度系统将与局域网下对应编号的机器人进行Socket通信，将期望消毒液浓度发送至机器人端。STM32单片机接收来自调度系统的控制信息，使用可调速吸泵将药液箱中的药品和水以一定流速吸出。混合后的液体可经过雾化喷头喷出，从而完成消毒液浓度的远程调节。
本文所设计的基于ROS的无人消杀机器人系统具有以下创新点。
①本文系统地设计无人消杀任务的解决方案，利用上位机调度软件、通过Socket通信来远程操控和观察机器人，提高了消杀任务的效率。
②本设计将SLAM技术与消杀任务相结合，减少了消杀作业中的人工参与，节约了人工成本，保障了工作人员的安全。
③本设计通过上位机调度软件与机载电脑的远程通信，调整了消毒液浓度，以适应不同环境。
④本设计根据不同任务类型选用不同全局路径规划器，提高了系统灵活性，保证了消杀区域覆盖率。
⑤本设计设置了定时消杀，可以按要求进行按时消杀。
本文设计的基于ROS和SLAM的地面无人消杀机器人系统，将SLAM与消杀任务有机结合，利用上位机调度软件对机器人的参数进行设置，提高了运作效率，减少了人工参与。SLAM方案采用了基于PF的Gmapping算法，提高了精度，为精准导航提供了前提。在里程计设计方面，本文采用了扩展卡尔曼滤波对激光雷达里程计、轮式里程计和IMU数据进行了融合，并利用自适应蒙特卡洛定位为机器人提供全局定位，有效保障了机器人的定位精度。在点到点任务中，机器人的全局路径规划算法采用A*算法，在区域消杀任务中使用了全覆盖路径规划算法。局部路径规划算法采用了DWA，可靠地提供了机器人的任务路径，保障了消杀任务的覆盖率。药液喷洒装置由STM32单片机进行PID闭环控制，保证了消毒液浓度。机器人与调度软件的协调运作提高了消毒杀菌任务的效率和消杀的覆盖率，保障了人员安全。
猜你喜欢 里程计消杀轮式 室内退化场景下UWB双基站辅助LiDAR里程计的定位方法导航定位学报(2022年5期)2022-10-13疫情防护现代职业安全(2022年4期)2022-06-10校园消杀 迎接开学科教新报(2021年34期)2021-09-30“蜘蛛侠”环球时报(2021-04-22)2021-04-22轮式装备抢救抢修车军民两用技术与产品(2021年10期)2021-03-16执行消杀任务的无人机无人机(2020年2期)2020-07-20车载自主导航系统里程计误差在线标定方法数字通信世界(2019年12期)2020-01-14一种单目相机/三轴陀螺仪/里程计紧组合导航算法中国惯性技术学报(2019年1期)2019-05-21两轮式随动支撑装置控制系统设计制造技术与机床(2018年8期)2018-10-09高通过性轮式无人机平台诞生记中国军转民(2018年1期)2018-06-08本文来源：http://www.zhangdahai.com/jiaoxueziyuan/jiaoxuesheji/2023/0903/648905.html
