建筑机器人融合改进的A*与TEB算法的运动规划研究
徐定明，李子信，张怡俊
（上海工程技术大学 机械与汽车工程学院，上海 201620）
目前，建筑业正积极推行智能化、对象集约化等新型建造方式，智能建造环境下移动作业机器人就是该领域中较好的解决方案之一，涉及到避障、路径规划、定位、导航、传感和通信等关键技术。其中，运动控制和路径规划问题被认为是复杂的任务［1］。
Zhu 等人［2］提出了基于深度学习的方法和导航框架。张贺等人［3］提出一种基于速度斥力场改进人工势场方法的全向移动机器人自主动态避障方法，并验证了自主动态避障算法的有效性和实用性。胡杰等人［4］改进了传统人工势场法在路径规划中存在的缺陷，将引力函数分段，避免引力过大而碰到障碍物的情况。Kashyap 等人［5］概述并实现一种新型的仿人机器人混合控制器来绘制最优路径，采用人工势场和飞蛾火焰优化混合方法对仿人机器人进行导航。刘子毅等人［6］提出了一种应用BIM（Building Information Model）信息与激光雷达获取的现实环境信息进行导航并保证路径最优的方法。Agarwal 等人［7］基于黏菌的改进群智能算法在Matlab 2020a 验证出在生成最佳无碰撞路径上花费的时间和精力更少。李承远［8］对TEB 算法进行改进，提出了减少优化变量的数量，使用参数三次样条插值的方法将路径点插值成一条平滑的路径曲线的方法，有效地降低了动态环境下TEB 算法的计算成本，解决了算法实时性能较低的问题。Mugarza 等人［9］提出了一种基于改进D*的交通控制器，用于动态确定AGV 的无碰撞轨迹。劳彩莲等人［10］根据温室环境下移动机器人作业的实时路径规划要求，提出一种基于改进A*算法与动态窗口法相结合的温室机器人路径规划算法，跟踪误差保持在0.22 m以内、定位误差不大于0.28 m，能够满足实际需求。柴红杰等人［11］在障碍物边缘放置虚拟障碍物形成缓冲区来改进A*算法，减少机器人碰撞几率，提高路径规划效率和机器人稳定性。Qi 等人［12］提出的MOD-RRT*可以生成更高质量的初始路径。Lyu 等人［13］提出了一种新的基于图的机器人路径优化算法，其中关键是动态分配权值的Floyd 算法，并证明该方法的有效性和鲁棒性。
综上，本文通过减少扩建节点方向改变和次数，以及根据障碍物占比率和复杂程度优化启发式函数。改进的A*算法融合TEB 算法进行建筑机器人的运动控制，高效完成建筑机器人近距离停靠在平行于打孔位置的墙面的任务，验证可行性。
1.1 传统A*算法
传统A*算法流程如图1 所示。图1 中，栅格化环境地图的节点信息全部加入到queue队列中，初始化所有的节点信息。分别设置起始点到当前点的真实代价值G（n）和当前点到目标点的估算代价值H（n），随后进行小顶堆排序，得到总代价值F（n）最小节点坐标的索引。判断当前索引值是否等于目标坐标点的索引值，如果相等，到达终点，可以结束迭代求解。如果不等，继续求解当前节点相邻4 个方向节点的F（n）值，更新容器的节点代价值，排序找到最小值循环求解，直至到达目标点为止。在此基础上根据已经扩展好的节点值，从目标点反向依次找到F（n）最小的值，也就找到了一条新路径。
图1 A*算法流程图Fig. 1 Flowchart of A* algorithm
1.2 局部路径规划TEB 算法
A*算法生成全局路径，TEB 路径规划期接收全局路径，同时激光雷达负责定位以及获得障碍物信息，以一定的频率进行局部路径规划，计算控制指令并下发给飒智机器人下位机控制器。
1.2.1 机器人运动模型
图2 为机器人运动学模型。由图2 可知，6 个轮中2 个驱动轮分别位于两侧中心处，（Xw，Yw，Zw）为全局坐标系，（Xr，Yr，Zr）为机器人基坐标系，坐标中心点在两驱动轮连线的中心轴线中点处，机器人前轮虚拟转向角等于中心转向角φ，为机器人姿态变化值，可得其求解方式如下所示：
图2 机器人运动学模型Fig. 2 Kinematic model of the robot
从上式得出，给定机器人输入u ＝（v（t），ω（t））T，即可得到机器人姿态变化以及求解出左右驱动轮编码器的控制指令ωL（t）和ωR（t），从而驱动小车到达目标位置。
1.2.2 TEB 轨迹模型
TEB 模型示意如图3 所示。由图3 可知，世界坐标系下每个机器人的位姿可表示为Si ＝［xi，yi，φi］，一系列的位姿点组成了弹性带C，并可由下式进行数学描述：
位姿Si到Si＋1的时间为ΔTi，其数学表述为：
机器人从位姿Si经过时间ΔTi运动到Si＋1便组成了时间弹性带，详见图3。
图3 TEB 模型示意图Fig. 3 Schematic diagram of TEB model
为了优化运动轨迹，假设轨迹信息T：＝（C，Δτ），对轨迹加一定约束，由两大部分组成。第一部分包括速度、角速度、线加速度、角加速度的约束以及自身运动学的限制；
第二部分则是与障碍物的距离以及与全局路径的偏离程度。Si处的速度约束求解方程分别见式（1）：
加速度相应的约束是由2 个连续速度的3 个位姿求解而来，可表示为：
非完整约束的差分驱动下相邻的2 个机器人位姿可以看成弧线运动，如图4 所示，Si与Si＋1在曲率恒定的圆弧上。
图4 差分驱动求解图Fig. 4 Differential drive solution diagram
图4 中，［xi，yi，φi］、［xi＋1，yi＋1，φi＋1］ 是位姿；
di，i＋1是圆的弦长；
φi与φi＋1分别是全局坐标系下的转角。由此推出运动学约束的代价函数为：
飒智机器人的转弯半径约束在最小半径之上，此处需用到的公式为：
此外，还有障碍物代价函数、时间最优的目标函数等在机器人行走过程中起约束作用，不再赘述。
1.2.3 算法求解
算法求解流程如图5 所示，对全局路径｛Zj｝ 做离散化处理，成为带有时间信息的位姿序列T（C，τ），每次迭代删除求解过的位姿和插入新的状态点。建立起T（C，τ）与目标函数以及约束函数之间的关系，求解最优轨迹T*，通过最小化一个非线性最小二乘代价函数求得：
图5 TEB 求解流程图Fig. 5 Flowchart of TEB solution
其中，fP表示速度、加速度等惩罚函数；
fT表示障碍物、路径等目标函数；
σk表示配置的权重；
T／｛S1，Sn｝ 表示起始和终止位姿S1与Sn不受优化。TEB 优化问题可以转化成超图问题。求解出来的结果T*，可以计算输入的控制变量u ＝（v（t），ω（t））T，相应地求解出左右驱动轮编码器的控制指令ωL（t）和ωR（t），从而驱动小车到达目标位置。
2.1 改进A*算法
2.1.1 A*算法改进策略
首先，从流程图可以看出，在循环找到目标点的过程中，寻找扩展节点是找到F（n）最小的扩展方向，扩展方向的频繁改变会导致生成的路径曲折，机器人运行卡顿，对工业应用机器人效率影响十分严重。由于是搜索queue容器里所有节点最小值，搜索过程中可能有不同的路径方向，所以只针对同一路径条件下扩展方向进行研究。这里对每个转向节点进行判断，如果最小节点方向与原路径方向下一节点代价值相差不大，则认定没有必要进行转向。
其次，为了扩大工业上移动机器人的运行效率，配合上文对启发式函数H（n）的计算方法进行改进。3 种启发式函数的计算方式，研究后可知，曼哈顿距离最大，欧几里得距离次之，切比雪夫距离最小。H（n）值越大，则搜索路径的目的性越强，但环境地图障碍物太多会导致搜索路径频频碰壁，效率反而不高，因此计算合适的启发式函数值是必要的。
为了合理优化H（n），更好地描述障碍物状态，引入障碍物占比系数K和离散率系数D，分别表示障碍物栅格数占环境地图总栅格数比重和障碍物分散程度。2 个系数可以很好地表征环境地图障碍物多少以及复杂程度等信息。相应数学公式具体如下：
其中，（xs，ys）是起始点；
（xt，yt）是目标点；
N表示障碍物栅格数。H（n）选用曼哈顿距离，评价函数F（n）相应发生变化。此处用到的数学公式可写为：
由式（13）、式（14）可知，障碍物越多、越分散，则D的值越小，其栅格数N也越大，K值越大，1-lnK变小，将会扩展更多搜索方向，适应复杂环境。
2.1.2 仿真验证对比分析
对机器人的路径规划算法进行仿真，验证算法的有效性。根据国家标准中机器人移动能力算法测评要求建立仿真环境场景。仿真试验场景如图6 所示。地面为水平地面，包含4 面墙，墙内面积为2 500 m2，墙高度为2 500 mm，包含天花板，天花板高度为2 500 mm，颜色为白色，包含4 根圆柱和A、B、C、D、E、F、G、H共8 个物体，4 根圆柱的位置固定，A -H坐标非固定，这8 个物体相对均匀地分布于4 面墙内，且彼此之间并不接触。圆柱及其它物体规格见表1。
图6 仿真场景模拟图Fig. 6 Simulation scene simulation diagram
表1 仿真场景物体尺寸表Tab.1 Objects size table of simulation scene
栅格化环境地图如图7 所示。在图7 中，模拟实际导航，机器人长1 000 mm，且可将之看成机器人中心点的移动，障碍物需膨胀500 mm 长度，膨胀部分称为代价地图。仿真计算结果见表2。从仿真结果来看，改进A*算法相对于传统A*算法在计算时间和拐点数上都有较大的改善。从表2 中的实验仿真数据可以看出，改进A*算法有较好的实时性，路径平滑性更好，便于机器人较好地沿着路径轨迹移动到目标点。需通过实验做进一步验证。
图7 仿真结果图Fig. 7 Simulation results diagram
表2 仿真计算结果统计表Tab.2 Statistical table of simulation calculation results
2.2 TEB 参数优化
为了机器人导航避障效果更好，进行参数的一系列优化。研究可知，机器人加载了一系列的参数文件，其中teb_local_planner_params.yaml 等参数文件要着重考虑。并对机器人运行轨迹、机器人本体参数、打孔位置约束以及障碍物约束等参数进行修正。移动操作建筑机器人部分运动参数设置如下：最大线速度为0.5 m／s，最大角速度为0.6 rad／s，最大线加速度为0.5 m／s2，最大角加速度为0.5 rad／s2，转角容许误差为0.05 m，与障碍物间的最小距离为0.25 m，障碍物膨胀半径为0.5 m。
2.3 融合策略
机器人采用精度较高的北阳UTS-10LX 激光雷达，使用cartographer 算法功能包进行环境栅格建图，并通过雷达准确获取周围环境的深度信息，定位到自己的位置，结合时间弹性带算法进行实时的局部路径规划，达到避障的目的，获取机器人运动的最佳路径。将改进的A*算法与TEB 相融合，以保证动态工作路径的实时性和时效性。
根据已知的当前实验室环境建立地图，进行全局路径规划，从规划的全局路径中截取当前位置的一段，将之离散化为带有时间信息的点，通过自身运动以及与障碍物代价函数的约束，求解到时间最优的位姿序列轨迹，并根据当前计算的速度以及角速度值，下发指令到AGV 左右驱动轮的编码器，驱动小车高效到达目标位置。融合算法流程如图8 所示。
图8 融合算法流程图Fig. 8 Flow chart of the fusion algorithm
3.1 实验准备
在机器人上进行实验验证，使用平台自带的里程计以及北阳UTS-10LX 激光雷达感知周围环境。且小车配有处理器为i7 八代的工控机作为主机，个人笔记本作为从机，从机配有i7 十代的虚拟机，主机和从机虚拟机都安装有ubantu18.04 的操作系统和ROS（melodic）。在实验室内网下，主机、从机以及从机虚拟机可通过IP 互相通信，从机和从机虚拟机可实现对主机的远程控制。
在所搭建的实验场景中，选用cartographer 算法功能包进行环境地图的建模，运行程序节点如图9所示。由图9 可知，启动teleop_twist_joy 为手柄控制节点以及cmdvel2can 为速度控制等节点，建立环境地图。运行包内local.launch，通过对子图的扫描匹配来实现定位，做好算法验证前的建图和定位准备。环境建图如图10 所示。
图9 融合算法程序运行节点图Fig. 9 Running nodes graph of the fusion algorithm program
图10 cartographer 环境建图Fig. 10 cartographer environment construction
3.2 实验验证
本文的融合算法适用于大范围非结构化建筑环境，选择实验环境对算法可行性进行验证。利用移动操作机器人分别对传统A*算法和改进A*算法进行实验。
3.2.1 传统A*算法实验验证
现场实验验证图如图11 所示。图11 中，白色点云是小车定位轮廓与环境地图匹配点，1 指的黑色圆角矩形为移动机器人本体，2 指的黑色线条为改进A*算法生成的全局路径，3 指的红色线条为实时截取全局路径生成的局部路径。融合算法的实验步骤描述如前文融合算法内容所述，加载使用调参cartographer 建好的栅格地图，给定目标点位的坐标和方向，机器人生成全局路径，TEB 算法实时截取全局路径生成局部路径，放置的桌子为4，可认为是建筑环境承重水泥柱，5 为打孔位置，实际验证机器人绕过建筑环境水泥柱并近距离停靠在平行于墙面的打孔位置，观察并记录移动机器人到达后的位姿数据信息以及导航避障情况。
图11 现场实验验证图Fig. 11 Field experiment verification diagram
给定打孔位点，机器人运动规划过程如图12 所示。图12 中，1 表示机器人本体，2 表示全局路径，3表示局部路径，4 表示水泥柱，5 表示墙面打孔位置。其中，采用柜子模拟建筑环境中的水泥承重柱。经过10 组实验发现，实时生成的全局路径局部有许多小齿状拐点，影响机器人的运行效率。
图12 建筑机器人运动规划验证实验图Fig. 12 Experimental diagram of construction robots motion planning verification
3.2.2 改进A*算法实验验证
同理，消除实验数据的偶然性，共做10 组实验。改进A*算法生成的全局路径较传统A*算法来说，减少了许多拐点，路径更为平滑。到达的目标点和所需要的方向满足建筑机器人位置和方向的要求，验证该改进算法策略的可行性。
（1）提出改进的A*算法融合TEB 算法进行建筑机器人的运动控制，并进行了实验验证。高效完成建筑机器人近距离停靠在平行于打孔位置墙面的任务，验证了建筑环境下该策略的有效性。其中，提出改进的A*算法，比传统A*算法效率更高，路径平滑性更好，便于建筑机器人生成较好的全局路径轨迹。
（2）系统分析建筑机器人导航避障，提出进一步改进策略：可以通过优化建图定位参数、比如环境子地图匹配频率等、减少小车定位变动次数，以减少小车导航避障过程中方向摆动，提高运动效率；
也可通过研发算法继续改进全局与局部路径规划算法，补偿机器人运动误差，以达到更好的导航效果。