近年来，新冠病毒疫情反复，严重影响了社会正常秩序与经济稳定发展，在疫情防控的诸多场景中智能机器人得到大量应用[1]．机器人在完成起点到目标点的导航过程中，路径规划技术起着关键性作用[2]，因此开展对算法纵深、应用场景及全新规划算法3个方面内容研究[3]．算法纵深研究通过改进算法的内在原理或者结合各种优化算法，得到性能更优越的混合算法，如迭代加深A*算法[4]、终身规划A*[5]算法或D*算法[6]等，从只能解决静态规划问题逐渐进步到解决动态规划问题，性能不断优化．在算法结合方面，基于参数化曲线规划轨迹[7-8]，利用优化算法进行曲线优化的方法[9-10]被很多学者采用．算法场景研究通过使用优化算法来改进规划性能，以A*算法作为全局算法引导动态窗口算法[11]应用广泛，主要采用改进的A*、稀疏A*和动态A*算法来解决3种不同场景下的路径规划问题[12]．也有学者通过引入蚁群算法与A*算法相融合，开展无人艇巡逻的路径规划研究[13]．全新规划算法需要原理和逻辑的全面创新，随着计算单元的不断优化，使得原先没有表现出良好性能的算法重新受到关注，如通过引入深度学习提高路径规划算法的智能化和泛化能力[14]，具有比较好的发展优势和潜力．综合来看，A*算法结合时间弹性带算法(timed elastic band，TEB)协同的方案得到广泛采用，但是在实际应用中，一方面，A*算法所规划路径存在距离障碍物较近的问题，出现无法完成的穿越障碍物现象，且路径不平滑；另一方面，TEB算法在计算得到机器人控制指令时，存在指令波动．因此，本研究通过对A*和TEB算法进行优化，增大A*算法所得路径与障碍物之间的距离，对连接路径的邻接点进行平滑处理．将PID (proportional-integral-differential)与TEB算法融合来提高稳定性，提出多任务导航调度算法，提高机器人工作效率．1 路径规划算法全局路径规划要解决的问题是为其找到起点到目标点的可通行路径的过程，这个过程只须给出路径的基本信息，此外，还会构建一系列优化目标，包括路径最短、提前躲避障碍物等．机器人只有在全局路径规划算法下确定了路径信息后，局部路径规划算法才会在局部范围内不断更新自身的运动信息，如速度、角速度及加速度等，从而完成路径规划的全部任务．1.1　改进A*算法的全局路径规划算法A*算法通过搜寻最小代价栅格来完成路径规划，未考虑所得最小代价栅格的周围环境信息，造成穿越障碍物、距离障碍物过近及路径不平滑等问题，使得路径规划的结果与机器人的真实路径之间存在较大差异．针对以上问题，本研究从增加路径点与障碍物之间距离和路径平滑两个方面进行改进．1.1.1　增加规划路径点与障碍物之间的距离建立路径点相邻节点的列表，并获得各路径节点在代价地图中的代价值，将所有节点代价值与所设阈值进行比较，若大于阈值，则所对应节点不能作为路径点，具体实现过程如图1所示．10.13245/j.hust.230206.F001图1A*算法改进原理建立节点列表的目的是以一个节点为中心，将其周围节点作为获取代价值的目标节点．利用代价值获取函数，得到列表中所有节点的代价值并与所设阈值进行比较，若节点代价值大于阈值，则最初被选中的中心节点不能作为路径点．1.1.2　平滑路径函数获取连续的三个路径点坐标，并计算相邻两坐标的角度偏差，设立角度偏差阈值，若角度之差的最大绝对值大于阈值，则可用前后两点的平均中点作为中间点坐标，从而实现路径的有效平滑．建立角度标准化函数，在地图坐标系中，将相邻两点所得角度整合到(-π，π)的范围，以便在进行角度比较时统一度量．获取当前时刻与上一时刻的路径点坐标并进行距离计算．若所得距离大于阈值，则在两路径点之间插入新的路径点，坐标为两路径点中点坐标，以此来增加路径点的密度．建立路径平滑函数，获取相邻三个路径点的坐标，然后计算相邻两坐标之间的角度值，通过计算分析角度标准化函数获取两个角度标准化偏差，若角度偏差的最大插值大于阈值，则将中间位置的坐标点用前后位置坐标点的中值替换．通过不断循环迭代该过程，实现整个路径的全局优化，如图2所示．10.13245/j.hust.230206.F002图2A*算法平滑路径原理1.2　改进TEB算法的局部路径规划算法TEB算法传输给机器人的控制指令为速度和角速度，机器人在得到可行驶路径后须要轨迹跟踪算法来满足其动力学约束并进一步优化路径，通过结合控制算法来实现更好的控制效果[15]．针对TEB算法输出指令波动的问题，结合PID算法提出一种融合规划的TEB优化算法，u(t)=Kpe(t)+Ki∫0te(t)dt+Kdde(t)dt，(1)式中：u(t)为控制量；e(t)为目标值与真实值之差；t为时间；Kp，Ki和Kd分别为比例、积分和微分增益．为实现机器人运行过程中的平顺性，在输出参数后加入参数控制模块，经PID调整后输出给机器人．以速度控制参数为例，TEB算法融合PID算法原理如图3所示．在操作系统(robot operating system，ROS)中获取机器人速度vx和vy，利用欧几里得范数表示当前状态下的速度vc，获取TEB算法中输出的速度指令，根据路径规划算法配置文件中的控制指令发布频率得到控制周期dt，并通过邻接的速度指令得到周期内的平均速度，同样将其用欧几里得范数表示，作为PID算法的目标速度．10.13245/j.hust.230206.F003图3TEB算法融合PID算法原理为了将目标控制速度控制在最大约束范围内，计算目标速度与实际速度之间的偏差，得到速度反馈偏差，计算一个周期内的积分项(Idt)与微分项(ddt)，Idt=evdt；(2)ddt=(ev-ev^)/dt，(3)式中：ev为速度反馈偏差值；ev^为ev前一时刻的速度偏差．综合，得到一个周期内的PID算法综合校正值Icr=Kpev+KiIdt+Kdddt．(4)实际控制的目标速度值可以利用校正值和当前值计算得到；同时，须通过判断模块确定是否超出运动学自身约束，将实际速度控制值约束在最大和最小速度之间．根据试验结果，确定PID算法具体参数为Kp=10，Ki=0.25，Kd=10．2 多任务导航调度算法2.1　多任务导航调度算法组成多任务导航调度算法主要包含话题通信、目标点输入、导航列表构建和处理及代价值计算和比较四部分．话题通信算法通过位姿估计模块获取机器人在地图中的坐标，并将坐标信息发布到运动控制模块，以便进行下层控制指令的发布．多目标点输入主要考虑到每次目标数量的不同，设计人机交互模块，对机器人的初始位姿进行定义，在地图中选择目标点，整个初始化过程在控制终端提示下操作．导航列表构建和处理通过引入导航列表，将交互过程中的目标点存入列表中，得到目标点与机器人起始位置的距离，选择最优的目标点，到达一个目标后将其移出导航列表，然后对剩下的目标进行重新选优，直至导航列表为空．代价值计算和比较通过引入代价值计算模块，来衡量每个目标点的远近，为减小算法的计算量，避免过多浮点数运算，引入曼哈顿距离作为评价标准，所得距离越小，优先级越高，导航算法会优先考虑作为第一目标点．2.2　多任务导航调度算法原理多任务导航调度算法流程如图4所示．首先，利用结合可视化平台发布相应数量的目标点坐标信息，将目标信息构建为导航列表，使用曼哈顿距离来衡量每个目标点的路径代价值．然后，将提取导航列表中代价值最小的目标作为本次导航的目标，确定目标后将此目标点从导航列表中移出，防止重复导航，完成第一目标点的导航之后，对列表是否为空列表进行判断：若不为空，则重复以上操作；若为空，则选择是否继续导航．最后，当全部目标点导航完成，通过人机交互模块来继续发布导航任务：若继续导航，则重新输入目标点构建导航列表，以便完成下一次任务；若放弃导航，则导航结束．10.13245/j.hust.230206.F004图4多任务导航调度算法流程3 实验与结果3.1　实验平台为验证路径规划算法，以Turtlebot2机器人为硬件平台，上位机选用智能移动终端，下位机选用英伟达公司TX2计算模块，搭配思岚科技公司的Rplidar A2单线激光雷达．嵌入式系统是Ubuntu 16.04 LTS系统，并搭配了ROS-Kinetic系统[16]．机器人所允许的最大速度为0.4 m/s，最大角速度为0.3 rad/s，设定机器人到达距离目标点0.2 m范围内即可认定为导航成功．为验证机器人在应用场景下路径规划算法的实用性，在所搭建环境中，将障碍物之间的距离设置为机器人直径的2～4倍，同时障碍物边界不规则，以便验证规划算法在路径不平直情况下的规划能力，实验场景如图5所示．通过同步定位与建图技术得到实验环境的栅格地图，并将其作为ROS系统中的背景地图，以此作为实验过程中人机交互和实时数据监控的基础．10.13245/j.hust.230206.F005图5实验场景3.2　实验结果3.2.1　A*与TEB融合算法实验图6为融合A*与TEB算法自主导航实验结果，图6(a)表示位姿初始化之后的机器人状态，绿色粒子为定位粒子，离散程度越小定位越精准．待定位粒子收敛后开展导航过程，确定好图6(b)的目标点，机器人开始导航，图6(b)和(c)中绿色较粗路径为全局路径，红色较细路径为局部优化路径，最终机器人跟踪无碰撞路径到达指定位置点，如图6(d)所示，导航成功．10.13245/j.hust.230206.F006图6自主导航实验过程在该实验场景下，机器人的导航目标点为(-4.23，4.59)，最终停止点为(-4.24，4.57)，停止点与目标点的距离为0.022 m，最终距离小于0.2 m，故机器人成功到达目标点误差允许范围．利用相邻两路路径点的绝对角度来衡量路径的弯折程度，如图7所示，图中：n为路径点；θ为方位角．目标路径主要为平滑转弯过程，得到的角度在45°范围内均匀波动，因此可知路径并未出现弯折过大的问题，实验证明优化后的A*算法能够有效平滑路径．10.13245/j.hust.230206.F007图7自主导航实验转弯角度实验过程中，机器人的规划任务主要为绕过实验场景中的弯角，此弯角产生的工况对应到真实环境中为直角转弯场景，实际速度(v)和角速度(ω)指令如图8所示．10.13245/j.hust.230206.F008图8控制指令对比曲线图由于路径较长，且环境较复杂，机器人在运动过程中的路径为小转弯较多的平滑曲线，造成速度和角速度变化频率较高，速度指令和角速度指令在(0，20)阶段为机器人启动初试阶段，此阶段TEB算法下发的控制指令波动较大，而融合PID的TEB算法波动较小．另外在速度和角速度指令存在偏差较大点或微小波动的情况下，如速度指令在(40，80)和角速度指令在(100，110)阶段，融合PID的TEB算法能够过滤细小波动并降低较大偏差，保障机器人平稳的运行．在实验过程中机器人的运行顺畅，在曲线尾部阶段，机器人接近目标点，控制指令逐渐向0附近波动，最终机器人到达目标点并停止．实验结果证明：经改进后的A*算法能够较好地平滑全局路径，将路径中所有相邻路径点间的方位角控制在45°之内，从而有效加大路径与障碍之间的距离，避免穿越障碍物现象的发生．在导航过程中，除主要波动阶段外，其余阶段控制指令相对平顺，因此主要对状态过渡阶段的数据进行数值分析，利用计算方差来衡量数据的波动情况，如表1所示．融合PID控制的TEB算法能够明显减小控制指令在状态过渡阶段时的波动范围，速度和角速度方差分别减小了47.1%和18.2%；同时，改进后算法还能够过滤细小波动和去除偏差过大指令，使机器人运转流畅．10.13245/j.hust.230206.T001表1自主导航实验数值参数TEB算法本文算法方差变化/%速度方差0.0340.01147.1角速度方差0.0180.00918.23.2.2　多任务导航调度算法实验多目标导航实验的验证目标有两点：一是验证机器人实际导航过程中是否能够按照远近关系进行任务调度；二是验证机器人运转是否顺畅，能否成功到达各个目标点，并返回消耗时间和经过距离．多目标导航任务调度实验所设置的三个目标点如图9(a)所示，三个目标点到机器人位置由近至远依次为A，B，C．机器人获取三个目标点坐标之后开始自动导航，算法同时实现全局路径和局部路径的规划，绿色较粗路线表示全局规划的路径，红色较细路线表示局部规划的路径，运动底盘不断接收控制指令，向位置最近的A点前进．当到达第一个目标点之后，即到达图9(b)图所示位置，考虑到机器人实际应用场景中往往须要在固定目标点完成一定任务，设定机器人原地等待10 s．完成之后，机器人开始导航前往第二近的目标点，即B点，到达B点后继续等待10 s，再次重复此过程，机器人到达第三目标点，即终点C点，如图9(d)所示．10.13245/j.hust.230206.F009图9多任务导航实验过程整个过程中，机器人按照远近关系依次到达图中A，B，C三点，并且在机器人运行过程中，规划算法所得路径平滑无障碍，较好地帮助机器人完成多目标导航任务，多目标导航任务成功完成．通过实时监听终端输出信息，得到导航结果，实验位姿信息如表2所示，机器人到达三个停止点与三个目标点的距离分别为0.054 m，0.063 m，0.180 m，均小于0.200 m，故机器人成功到达目标点允许误差范围．10.13245/j.hust.230206.T002表2实验位姿信息位置点坐标距离/m目标点1(-2.92，4.52)0.054停止点1(-2.94，4.47)目标点2(0.38，3.78)0.063停止点2(0.36，3.72)目标点3(0.12，0.36)0.180停止点3(0.18，0.53)实验场景中，确定3个目标点后，机器人按照远近关系进行任务调度，将导航过程分为三段，即：起点—A，A—B，B—C，三段所经历的距离和时间由终端直接输出结果，多任务导航调度算法能够按照远近关系进行任务发布，机器人与目标点的距离按照由近至远的原则依次为A，B，C．由表3可知：在实验场景下，到达A，B，C三点经过的距离分别为5.5，14.6，26.9 m，同样符合由近至远的调度原则，完成导航的实际总距离为26.9 m，总消耗时间66 s．10.13245/j.hust.230206.T003表3实验结果分析表参数起点—AA—BB—C总计距离/m5.59.112.326.9时间/s8253366实验结果证明：多任务导航调度算法能够实现目标数量的任意设定，在实验场景中能够按照目标远近进行任务调度，并且在完成一次导航任务后自动进入下一次导航初始化中，设想功能实现．4 结论a．以机器人为研究对象，着重考虑算法应用的可行性和实用性，对A*算法进行了改进，通过增大障碍物邻接栅格点的代价值，即认为其为不可达点，从而加大了算法所规划路径与障碍物之间的距离．直接连接方位角较大的相邻路径点，完成了路径的平滑处理．b．提出一种融合PID控制的TEB算法，通过将局部规划算法下发的控制指令进行再处理，减小了控制指令的波动范围，使得机器人运动顺畅；提出的多任务导航调度算法通过人机交互操作获得目标点数量和位置，按照由近及远的原则实现机器人的多目标任务调度与导航．c．以Turtlebot2机器人作为控制对象，结合ROS-Kinetic系统搭建实验平台．使用改进后的A*算法与TEB算法规划出可行驶的平滑路径，所得路径能够规避穿越障碍的现象并将邻接点方位角控制在45°之内，且机器人运行平稳顺畅，并根据实验中的速度和角速度信息进行了规划评价，在实验场景下，使速度和角速度方差分别减小47.1%和18.2%．在多目标导航实验中，通过交互操作验证了自主输入导航目标点数量与位置坐标功能的正常运转，机器人能够按照由近至远的原则进行任务调度与导航，且停止在距离目标点0.2 m的预设范围之内．