一种空间受限环境下双轮差速机器人的位置调整算法

A Position Adjustment Algorithm for Two-Wheeled Differential Drive Robots in Constrained Spaces

摘要: 针对在空间受限的环境下，双轮差速机器人如何有效地进行位置调整，以避开障碍物并达到目标位置的问题，本文提出一种简易高效的位置调整算法。本文首先详细阐述了算法的实现过程，基于Gazebo和ROS2的仿真实验平台，并设计了四种最常见的窄巷道场景，通过仿真实验验证了算法的有效性和实用性。在详细阐述该算法时，我们介绍了其两步核心策略。首先，机器人通过原地旋转的方式，对各个方向进行采样，以确定在不发生碰撞的前提下可实现的最大旋转角度范围。其次，基于该旋转角度范围，机器人通过遍历不同角度的直行路径，在考虑障碍物的情况下，生成一系列潜在的运动轨迹。为确定最优轨迹，我们采用了一种基于目标点接近程度和用时的评分机制，在所有潜在运动轨迹中，选择最符合要求的路径作为控制目标路径。

Abstract: Addressing the question of how a two-wheeled differential robot can effectively make position adjustments to avoid obstacles and reach a target position in a space-constrained environment, this paper proposes a simple and efficient position adjustment algorithm for two-wheeled differential drive robots to effectively navigate and avoid obstacles in constrained environments, ultimately reaching a target position. The implementation process of the algorithm is elaborated in detail, leveraging the Gazebo and ROS2 simulation platforms. Four common narrow corridor scenarios are designed to validate the effectiveness and practicality of the algorithm through simulation experiments. The algorithm comprises two core strategies. Firstly, the robot rotates in place to sample various directions, determining the maximum achievable rotation angle range without colliding with obstacles. Secondly, based on this rotation angle range, the robot generates a series of potential motion trajectories by traversing straight paths at different angles, taking into account the presence of obstacles. To determine the optimal trajectory, a scoring mechanism is adopted, which considers both the proximity to the target point and the time required. Among all potential motion trajectories, the path that best meets the requirements is selected as the control target path.

1. 引言

在机器人技术领域中，双轮差速机器人因其结构简单、运动灵活而广泛应用于各种场景。在受限空间中，当双轮差速机器人在靠近导航终点，但是又与终点有一定的横向距离时，由于机器人无法横向移动，现有的算法，如动态窗口法(Dynamic Window Approach, DWA) [1]、时间弹性带(Time Elastic Band, TEB) [2]、线性二次型调节器(Linear Quadratic Regulator，LQR) [3] [4] [5]、模型预测控制(Model Predictive Control, MPC)和纯跟随算法[6]等，存在一定的局限性。当机器人在靠近终点的起点出发，行驶到终点并调整好朝向的这一过程，下文称为位置微调。

DWA虽然能够在一定程度上实现机器人的局部路径规划，但在靠近终点时，DWA微调效果往往不佳，容易出现振荡现象，即机器人反复摆动而无法稳定地接近目标点。

TEB算法有很强的前瞻性，能够动态避障，表现较好。MPC是一种先进的控制策略，起源于20世纪70年代。它通过建立系统的数学模型(主要包括状态方程和输出方程)，利用系统模型和当前状态信息，进行状态预测，在预测范围内对系统的未来行为进行优化，以实现对动态系统的轨迹跟踪。虽然根据文献[7]的结论MPC可以得到较好的控制效果，但是TEB和MPC均存在计算复杂、参数配置较为困难的问题。

LQR算法是基于现代控制理论的最优控制方法，用于解决线性系统最优控制问题，其基本原理是将问题转化为求解Riccati方程。通过选择权重矩阵Q和R可得到最优状态反馈控制律，实现系统稳定、快速和精确控制。但LQR只适用于线性系统，性能依赖权重矩阵选择，模型不确定性可能影响控制性能。

直接解方程的方法，如基于优化理论的路径规划算法，“算法的收敛性很难得到保障；一般只能得到最优解；需显示处理障碍物约束；非凸问题的优化较为困难”[8]。在实际应用中，由于环境中随机障碍物的存在，使得这类方法难以处理复杂的约束条件，同时此类算法的计算量往往不小。

纯跟随算法的实现简单、参数易于调整、计算要求低，且在低速下性能良好，但是在高速行驶时跟踪精度下降，对初始偏差较大的路径跟踪能力有限，且对路径曲率有限制，无法处理过于曲折的路径。

本文提出了一种适用于空间受限环境下双轮差速机器人的位置调整算法。该算法通过原地旋转采样和直行路径规划相结合的方式，能够在不依赖复杂数学模型和大量计算资源的情况下，实现机器人的稳定位置调整。算法的核心思想在于，首先通过原地旋转确定机器人在当前环境下能够安全旋转的最大角度范围，然后基于这一范围进行直行路径规划，通过遍历不同角度的直行路径，找到最优的轨迹作为控制目标。该算法简单、易于理解和实现，并且能够在实时性要求较高的情况下，实现机器人在受限空间中的稳定位置调整。

2. 位置调整算法设计

根据文献[9]中的两轮差速机器人移动底盘运动学模型，在空间受限的情况下，虽然理论上双轮差速机器人通过曲线行走进行微调是效率最优的，但微调的时候机器人与终点距离较近，且空间受限，因此曲线行走的跟随效果不一定能保证，再者曲线行走相比走直线段带来的效率提升并不显著。为了简单起见，本文设计的位置调整算法令双轮差速机器人只执行两个动作：原地旋转和直行(包括前进和后退)。算法的目标是找出由旋转和直行组成的一系列动作，使得机器人在尽可能短的时间内到达终点并调整好车头朝向。

2.1. 算法总流程

机器人的偏差可以分为三种：左右偏差、前后偏差、角度偏差。角度偏差即机器人朝向与目标朝向的角度差。左右偏差和前后偏差需要进行规定。这里的“左右”“前后”是针对于目标朝向而言的，如图1所示。

Figure 1. Left-right deviation and forward-backward deviation of the robot

图1. 机器人的左右偏差和前后偏差

算法的总流程如图2所示。机器人微调过程由上述三类偏差调整过程组成。首先，对三类偏差的大小进行判断，根据三类偏差的误差阈值决定当前需要先调整哪类偏差。显然，前后偏差和角度偏差对于双轮差速机器人来说，是容易消除的，因此三类偏差的调整优先级从高到低分别是：左右偏差、前后偏差、角度偏差。

Figure 2. Overall flowchart of the algorithm

图2. 算法总流程图

对于前后偏差的调整，车子只需摆正好角度，然后在前后方向进行直行即可。对于角度偏差的调整，车子只需根据角度误差的方向，选择顺时针还是逆时针方向以最小的旋转量旋转到目前朝向即可。

左右偏差的调整则较为复杂，该算法为整个调整算法的核心，在下文进行介绍。

需要注意的是在某些特殊情况下，若按上述流程无法找到可到的路径，那么考虑进行执行一定的恢复动作(例如，前进或后退一段距离)后，再重新开始微调流程。

2.2. 左右偏差调整算法

由于机器人无法像全向底盘“实现任意方位的灵活运动”[10]，特别是左右横移，因此算法中最复杂的部分是左右偏差的调整。左右偏差调整算法的目标是找出由旋转和直行不断交替执行的一系列动作，使能机器人通过“旋转，直行，旋转，直行……”一系列动作，将左右偏差减少到设定阈值内。

左右偏差调整算法分为四个步骤：一是确定原地旋转的允许角度范围；二是规划直行路径；三是对可行路径进行评分，选取“最优”路径；四是执行“最优”路径。

2.2.1. 确定原地旋转角度范围

机器人首先进行原地旋转的角度采样，如图3所示，点A为车体中心，三角形为障碍物。算法以预设的采样精度，在逆时针和顺时针方向上进行角度采样，同时判断在该角度下车子是否发生碰撞。在采样过程中，记录未发生碰撞的角度范围。这一步骤的目的是获取机器人在当前环境下能够安全旋转的最大角度，为后续的直行路径规划提供输入。

Figure 3. Range of in-place rotation angles

图3. 原地旋转角度范围

2.2.2. 规划直行路径

在确定了旋转角度范围后，机器人开始规划直行路径。对于上一步骤得到的角度范围中的每一个角度，机器人分别进行前进和后退两个方向的直行计算，生成对应的直行路径。

如图4所示，选择终点坐标系作为计算坐标系，终点为原点O，x轴方向为终点朝向，y轴方向为终点左边，点A为车子中心位置，三角形为障碍物。直行路径规划过程分为三个步骤：首先，根据预设的可直行距离，则可在车子前进方向生成一段直线段，即线段AB；然后，对线段AB进行离散采样，并对采样点进行碰撞检测，截去碰撞的路径，得到无碰撞线段AC；最后，在线段AC上查找距离终点左右偏差最小的点D作为线段终点，则线段AD即为直行路径。同理，对于车子的后退方向也进行上述步骤的规划。

2.2.3. 评价可行路径

为了评估不同路径的优劣，本文引入了目标点接近程度和动作用时评分机制。目标点接近程度用于

Figure 4. Straight path planning

图4. 直行路径规划

评估机器人与目标点的左右偏差大小；动作用时评分则考虑了机器人完成动作所需的时间。通过综合这两个指标，机器人可以对每一条路径进行评分，并选择评分最高的路径作为本次控制目标轨迹。整体代价函数(评分值)如下：

$f\left(t,d,w1,w2\right)=w1\times t+w2\times d$

其中，其中w1和w2是权重(或称为系数)，t是动作时间，d是左右距离偏差。在实际应用中，也可根据需要修改或加入另外的评分机制。

2.2.4. 执行最优路径

上一步骤获得的“最优”路径是一个旋转角度加一段直行距离，车子只需按照角度进行旋转和直行即可。需要注意的是，由于车子周围环境是动态变化的，所以在旋转或直行的过程中有一定的概率会出现避障并中断动作的情况。动作中途出现避障时，车子需要停下来，再重新进行位置微调。

3. 实验结果与分析

本实验使用的小车和窄巷道场景如图5所示。

Figure 5. The narrow corridor and the vehicle within it

图5. 窄巷道和小车

下面我们将基于以下四个场景，分别验证机器人在不同障碍物分布位置调整过程。

• 机器人周围除通道限制外，没有额外的障碍物，目标点在机器人右侧；

• 机器人周围除通道限制外，正前方存在障碍物，目标点在机器人右侧；

• 机器人周围除通道限制外，左前方存在障碍物，目标点在机器人右侧；

• 机器人周围除通道限制外，右前方存在障碍物，目标点在机器人右下侧。

上述场景中的目标点朝向都为正上方。由于机器人的前后动作、左右动作具有对称性，因此场景中障碍物的三种位置基本可以代表实际应用的障碍物情况。

上述四个场景的位置调整运动轨迹如图6所示。图中红色点为雷达数据，表示障碍物，绿色框为小车轮廓。从车中心出发的绿色线段为微调轨迹，绿色箭头方向即为车子朝向。

图6(a)中小车通过顺时针旋转，前进，逆时针旋转，后退到达目标点。

图6(b)中小车无法前进，所以通过逆时针旋转、后退，顺时针旋转，前进到达目标点。

图6(c)中小车无法逆时针旋转，所以小车通过顺时针旋转，前进，逆时针旋转，后退到达目标点。

图6(d)中小车无法顺时针旋转，所以小车通过逆时针旋转，后退，顺时针旋转，前进到达目标点。

(a) 没有额外的障碍物 (b) 正前方存在障碍物

(c) 左前方存在障碍物 (d) 右前方存在障碍物

Figure 6. Adjusted motion trajectories for positioning in four scenarios

图6. 四个场景的位置调整运动轨迹

上述实验结果验证了本文提出的位置调整算法的有效性和可行性。该算法在不同场景下均能够较好地实现机器人的位置调整任务。

4. 结论与展望

本文提出了一种适用于空间受限环境的双轮差速机器人的位置调整算法。实验结果表明，该算法能够有效解决机器人在受限环境下的位置调整问题。但是该算法也还存在一定的局限性，比如，只适用于双轮差速底盘的机器人，调整轨迹不是全局最优解，在远距离、复杂环境中可能效果不佳。未来，我们将进一步改进和拓展该算法，例如，探索与机器学习算法的结合[11]，提高其在不同环境下的适应性和通用性。