CN115639814A - 一种两轮差速移动机器人的路径跟随方法及系统 - Google Patents

Abstract

本发明涉及一种两轮差速移动机器人的路径跟随方法及系统，包括如下步骤：S1:根据导航系统获取目标路径；S2:更新机器人当前位置，并获取机器人与目标路径距离最近的路径点；S3:计算机器人的前视距离并确定前视点，根据机器人当前位置和前视点计算出曲率；S4:根据路况确定期望的线速度指令，并对期望的线速度指令施加左右轮电机加速度约束产生最终线速度指令；S5:根据最终线速度指令和曲率计算出角速度指令，将线速度指令和角速度指令转换成左右轮电机速度指令并发送至电机；S6:重复步骤S2‑S5，直至机器人达到目标路径的终点。本发明提供的两轮差速移动机器人的路径跟随方法及系统，运动控制效果更流畅，且一定程度上保证了机器人在复杂路径上路径跟随精度。

Description

一种两轮差速移动机器人的路径跟随方法及系统

技术领域

本发明涉及机器人运动控制技术领域，具体涉及一种两轮差速移动机器人的路径跟随方法及系统。

背景技术

现有技术中，两轮差速移动机器人路径跟随的常用方法为：根据经验和测试，手动选定合适的前视距离d,即可确定前视点Z，找到距离前视点Z最近的路径点P，c为路径点P处的曲率，e为前视点与路径点P的距离偏差(参考图1)。

线速度控制指令v与曲率c成反比,从而实现直道加速，弯道减速策略。v的计算公式如下：

v＝k*c(0＜k＜1)

角速度控制指令ω由比例积分微分控制器(PID控制器)计算得出。在离散时间数字控制系统中，ω的计算公式如下：

其中K_P为比例系数；K_I为积分系数；K_D为微分系数。

这些PID控制器系数需要手动调节，以达到满意的效果。通常的调参方法为：先将K_I和K_D初始化为0，然后增大K_P，直到系统开始震荡。再开始调节K_D，直到大部分情况下震荡被衰减掉。最后，调节K_I来消除静态偏差。

以上现有技术方案存在的问题：

1、PID控制器系数需要手动调节，即使经过多次调节和测试来迭代系数，路径跟随的精度依然不是特别高，尤其是在曲率较大的弯道路径处；

2、PID控制器系数与线速度v相关性较高，一套PID系数无法在高中低速度档位上都有较好的路径跟随精度，通常需要为不同的速度档位适配不同的PID系数，大大增加了调试工作量，不易部署使用；

3、没有考虑动力学约束，即没有考虑左右轮电机的动态性能指标(如最大加速度)，在复杂路况下会使机器人的运动控制效果卡顿不流畅、加速度过大等。

发明内容

针对现有技术的不足，本发明提供了一种两轮差速移动机器人的路径跟随方法及系统，运动控制效果更流畅，且一定程度上保证了机器人在复杂路径上路径跟随精度，从而提高了效率。

为了实现本发明的目的，本发明提供一种两轮差速移动机器人的路径跟随方法，包括如下步骤：

S1:根据导航系统获取目标路径；

S2:更新机器人当前位置，并获取机器人与目标路径距离最近的路径点；

S3:计算机器人的前视距离并确定前视点，根据机器人当前位置和前视点计算出曲率；

S4:根据路况确定期望的线速度指令，并对期望的线速度指令施加左右轮电机加速度约束产生最终线速度指令；

S5:根据最终线速度指令和曲率计算出角速度指令，将线速度指令和角速度指令转换成左右轮电机速度指令并发送至电机；

S6:重复步骤S2-S5，直至机器人达到目标路径的终点。

优选的，所述步骤S1具体包括：

根据导航系统获取目标路径，所述目标路径包括多个路径点，其中每个路径点包括：

全局坐标系下的x坐标和y坐标；

全局坐标系下目标路径的朝向，以及目标路径下每一个路径点的曲率。

优选的，所述步骤S3中计算机器人的前视距离并确定前视点的具体步骤包括：

机器人的前视距离计算公式为：

l＝k*v

其中，k为可调节比例系数，v为机器人当前实际线速度；

以机器人与目标路径距离最近的路径点作为基准，计算出机器人与每一个路径点之间的距离，当计算出机器人与其中一个路径点的距离大于l时，则将该路径点作为前视点。

优选的，所述步骤S3中根据机器人当前位置和前视点计算出曲率具体包括：

根据机器人当前位置和前视点(x，y)的圆弧计算出曲率c，其计算公式：

其中，r为圆弧半径，x为前视点在机器人坐标系下的X轴坐标，y为前视点在机器人坐标系下的Y轴坐标。

优选的，所述步骤S4的具体步骤为：

确定期望的线速度指令，其计算公式为：

v_desire＝min(v₁，v₂，v₃)

通过计算机器人前视方向某一段路径上的所有路径点的曲率，并找出曲率的最大值，其中，线速度指令V₁与最大曲率成反比；

通过计算机器人前视方向某一段路径上的所有路径点与障碍物之间的距离，并找出距离的最小值，其中，线速度指令V₂与最小距离成反比；

通过计算机器人当前位置与目标路径终点之间的距离，其中，线速度指令V₃与该段距离成反比；

并对期望的线速度指令施加左右轮电机加速度约束得出最终线速度指令。

优选的，所述对期望的线速度指令施加左右轮电机加速度约束得出最终线速度指令具体包括：

将步骤S2-S5作为一个控制周期，计算出当前控制周期的线速度指令的取值区间，并对期望的线速度指令施加取值区间约束，即可得出最终线速度指令，其取值区间计算公式为：

其中，下标k为当前控制周期，下标k-1为上一个控制周期，V_k为当前控制周期的线速度指令，a为通过测试左右轮电机的带负载阶跃响应，确定出的左右轮的最大加速度，R为机器人的左轮，L为机器人的右轮，△t代表控制周期。

优选的，所述对期望的线速度指令施加取值区间约束具体包括：

当施加的取值小于区间下限时，则取区间下限值进行约束；

当施加的取值大于区间上限时，则取区间上限值进行约束；

当施加的取值位于区间内时，则保持不变。

优选的，所述步骤S5具体包括：

根据最终线速度指令和曲率计算出角速度指令，所述角速度指令的计算公式为：

ω_cmd＝c*v_cmd

其中，c为曲率，V_cmd为最终线速度指令；

将线速度指令和角速度指令转换成左右轮电机速度指令并发送至电机，其转换公式为：

其中，c为曲率，r为半径，ω_cmd为角速度指令，V_cmd为最终线速度指令，d为轮间距的一半，VR_cmd为右轮线速度指令，VL_cmd为左轮线速度指令。

优选的，本发明还提供了一种两轮差速移动机器人的路径跟随系统，包括：

获取模块：用于获取目标路径、机器人当前位置以及目标路径上每个路径点的坐标；

控制模块：计算机器人的前视距离并确定前视点，根据机器人当前位置和前视点计算出曲率，根据路况确定期望的线速度指令，并对期望的线速度指令施加左右轮电机加速度约束产生最终线速度指令，根据最终线速度指令和曲率计算出角速度指令，将线速度指令和角速度指令转换成左右轮电机速度指令；

驱动模块：接收控制模块所下发的左右轮电机速度指令并对机器人进行驱动。

优选的，在所述控制模块中，计算机器人的前视距离并确定前视点具体包括：

机器人的前视距离计算公式为：

l＝k*v

其中，k为可调节比例系数，v为机器人当前实际线速度；

以机器人与目标路径距离最近的路径点作为基准，计算出机器人与每一个路径点之间的距离，当计算出机器人与其中一个路径点的距离大于l时，则将该路径点作为前视点。

本发明的有益效果为：本发明提供的两轮差速移动机器人的路径跟随方法及系统，运动控制效果更流畅，且一定程度上保证了机器人在复杂路径上路径跟随精度，从而提高了效率。

附图说明

通过附图中所示的本发明优选实施例更具体说明，本发明上述及其它目的、特征和优势将变得更加清晰。在全部附图中相同的附图标记指示相同的部分，且并未刻意按实际尺寸等比例缩放绘制附图，重点在于示出本的主旨。

图1为背景技术中提供的实施例的示意图；

图2为本发明实施例提供的两轮差速移动机器人的路径跟随方法的流程示意图；

图3为本发明实施例提供的关于机器人在目标路径跟随过程中的曲率计算示意图；

图4为本发明实施例提供的两轮差速底板的运动学模型的流程示意图；

图5为本发明实施例提供的两轮差速移动机器人的路径跟随方法的具体流程示意图；

具体实施方式

下面结合附图和具体实施例对本发明技术方案作进一步的详细描述，以使本领域的技术人员可以更好地理解本发明并能予以实施，但所举实施例不作为对本发明的限定。

请参考图2-5，本发明实施例提供一种两轮差速移动机器人的路径跟随方法，包括如下步骤：

S1:根据导航系统获取目标路径；

S2:更新机器人当前位置，并获取机器人与目标路径距离最近的路径点；

S3:计算机器人的前视距离并确定前视点，根据机器人当前位置和前视点计算出曲率；

S4:根据路况确定期望的线速度指令，并对期望的线速度指令施加左右轮电机加速度约束产生最终线速度指令；

S5:根据最终线速度指令和曲率计算出角速度指令，将线速度指令和角速度指令转换成左右轮电机速度指令并发送至电机；

S6:重复步骤S2-S5，直至机器人达到目标路径的终点(将步骤S2-S5作为一个控制周期，下一个控制周期到来时，先通过步骤S2进行机器人位置更新以及获取力目标路径距离最近的路径点，再重复S3-S5步骤计算电机速度指令。循环执行以上控制周期，两轮差速移动机器人即可完成对目标路径的高精度跟随。)。

请参考图2-5，在优选实施例中，先计算出路径的曲率，然后在根据环境确定速度指令，在由公式

ω_cmd＝c*v_cmd

计算出角速度指令，则无论线速度指令是高速还是低速，对于同样的路径，机器人的转弯半径始终是确定的。

本申请提供的两轮差速移动机器人的路径跟随方法，即使面对较为复杂的路径，如大弧度路径，也有很高的路径跟随精度(能够解决现有技术中PID控制器系数需要手动调节，即使经过多次调节和测试来迭代系数，路径跟随的精度依然不是特别高，尤其是在曲率较大的弯道路径处的技术问题)。

本发明提供的两轮差速移动机器人的路径跟随方法，在实际操作过程中，两轮差速移动机器人的导航系统中的规划器向该方法输入一条由离散点组成的目标路径和机器人在世界坐标系下的位姿，通过预先计算出的数据(前视距离来确定前视点Z，预设期望的线速度指令来计算出机器人的最终线速度指令，再根据最终线速度指令算出角速度指令)从而来控制机器人的左右轮进行相应的运动，达到路径跟随的目的。

本发明的有益效果为：通过确定前视点，并根据前视点计算出路径的曲率，然后根据环境确定线速度指令，一定程度上能够使机器人在进行路径跟随时保证其路径跟随的精度。

请参考图2-5，在优选实施例中，所述步骤S1具体包括：

根据导航系统获取目标路径，所述目标路径包括多个路径点(目标路径由一系列离散的路径点组成)，其中每个路径点包括：

全局坐标系下的x坐标和y坐标；

全局坐标系下目标路径的朝向，以及目标路径下每一个路径点的曲率。

请参考图2-3，在优选实施例中，所述步骤S3中计算机器人的前视距离并确定前视点的具体步骤包括：

机器人的前视距离计算公式为：

l＝k*v

其中，k为可调节比例系数(参数k是决定路径跟随精度的唯一关键参数。合适的参数k可以有很高的路径跟随精度，参数少，调参简单，通过合适的可调节比例系数，能够解决现有技术中参数多，调参复杂的技术问题)，v为机器人当前实际线速度；

以机器人与目标路径距离最近的路径点作为基准(从距离机器人最近的路径点开始，向前搜索)，计算出机器人与每一个路径点之间的距离，当计算出机器人与其中一个路径点的距离大于l时，则将该路径点作为前视点Z。

请参考图3，在进一步的优选实施例中，所述步骤S3中根据机器人当前位置和前视点计算出曲率具体包括：

根据机器人当前位置(以机器人的中心为准)和前视点Z(x，y)(前视点Z在全局坐标系下的坐标)的圆弧计算出曲率c(根据几何关系计算出连接机器人中心和前视点Z的圆弧的曲率，圆弧所在的圆为机器人瞬时圆周运动的圆，该圆与机器人坐标系的X轴相切，故该圆唯一确定，由正弦定理，可计算出曲率c)，其计算和推导公式：

其中，r为圆弧半径，x为前视点在机器人坐标系下的X轴坐标，y为前视点在机器人坐标系下的Y轴坐标。

请参考图2-5，在进一步的优选实施例中，所述步骤S4的具体步骤为：

确定期望的线速度指令，其计算公式为：

v_desire＝min(v₁，v₂，v₃)

通过计算机器人前视方向某一段路径上的所有路径点的曲率，并找出曲率的最大值，其中，线速度指令V₁与最大曲率成反比(从而实现直道加速，弯道减速的策略)；

通过计算机器人前视方向某一段路径上的所有路径点与障碍物之间的距离，并找出距离的最小值，其中，线速度指令V₂与最小距离成反比(在靠近障碍物时降低速度，保证安全)；

通过计算机器人当前位置与目标路径终点之间的距离，其中，线速度指令V₃与该段距离成反比(从而使机器人能在终点处停下来)；

通过计算目标路径上每个路径点的曲率、以及路径点与障碍物之间的距离和机器人当前位置与目标路径终点之间的距离，能够提高机器人的精准度，同时还能够提高机器人的效率。

并对期望的线速度指令施加左右轮电机加速度约束得出最终线速度指令(能够确保机器人能在较为复杂的路径中更好进行运动)。

请参考图2-5，在优选实施例中，所述对期望的线速度指令施加左右轮电机加速度约束得出最终线速度指令具体包括：

将步骤S2-S5作为一个控制周期，计算出当前控制周期的线速度指令的取值区间，并对期望的线速度指令施加取值区间约束(通过施加取值区间，能够确保机器人执行机构(左右轮电机)运动控制效果更流畅、不卡顿且不会使施加的速度过大)，即可得出最终线速度指令，其取值区间计算公式为：

其中，下标k为当前控制周期，下标k-1为上一个控制周期，V_k为当前控制周期的线速度指令，a为通过测试左右轮电机的带负载阶跃响应，确定出的左右轮的最大加速度，R为机器人的左轮，L为机器人的右轮，△t代表控制周期。

当前控制周期的线速度指令V_k需要满足左右轮电机的加速度约束,(推导公式基于图4中的两轮差速底盘的运动学模型上进行推导，具体可参考图4对应的推导公式)其推导过程(区间计算公式)如下：

解上述不等式，即可得到满足左右轮电机加速度约束的线速度指令V_k的可取值区间。

请参考图2-5，在进一步的优选实施例中，所述对期望的线速度指令施加取值区间约束具体包括：

当施加的取值小于区间下限时，则取区间下限值进行约束；

当施加的取值大于区间上限时，则取区间上限值进行约束；

当施加的取值位于区间内时，则保持不变。

请参考图2-5，在优选实施例中，所述步骤S5具体包括：

根据最终线速度指令和曲率计算出角速度指令，所述角速度指令的计算公式为：

ω_cmd＝c*v_cmd

其中，c为曲率，V_cmd为最终线速度指令；

请参考图4，机器人进行圆周运动的线速度计算公式为：

v＝ω*r

通过角速度和路径的曲率可求出角速度，其计算公式为

通过角速度和线速度可计算出左右两轮的速度，其计算公式为：

其中，ω和v分别为角速度和线速度，V_L和V_R分别为左右轮的速度，d为轮间距的一半，r为瞬时圆周运动的半径，c为瞬时圆周运动的曲率。

将线速度指令和角速度指令转换成左右轮电机速度指令并发送至电机，其转换公式为：

其中，c为曲率，r为(瞬时圆周运动)半径，o为圆心，ω_cmd为角速度指令，V_cmd为最终线速度指令，d为轮间距(左轮和右轮之间)的一半，V_Rcmd为右轮线速度指令，V_Lcmd为左轮线速度指令。在特殊复杂路径下，如S弯道，上式中两个区间可能没有交集。这说明电机的动态性能指标(最大加速度a)无法满足复杂路况需求。这时，可以更换性能更强的左右轮电机，或减小机器人负载，或导航系统的规划器在规划路径时降低路径的复杂度。

请参考图2-5，在优选实施例中，本发明还提供了一种两轮差速移动机器人的路径跟随系统，包括：

获取模块：用于获取目标路径、机器人当前位置以及目标路径上每个路径点的坐标；

控制模块：计算机器人的前视距离并确定前视点，根据机器人当前位置和前视点计算出曲率，根据路况确定期望的线速度指令，并对期望的线速度指令施加左右轮电机加速度约束产生最终线速度指令，根据最终线速度指令和曲率计算出角速度指令，将线速度指令和角速度指令转换成左右轮电机速度指令；

驱动模块：接收控制模块所下发的左右轮电机速度指令并对机器人进行驱动。

请参考图2-5，在优选实施例中，在所述控制模块中，计算机器人的前视距离并确定前视点具体包括：

机器人的前视距离计算公式为：

l＝k*v

其中，k为可调节比例系数，v为机器人当前实际线速度；

以机器人与目标路径距离最近的路径点作为基准，计算出机器人与每一个路径点之间的距离，当计算出机器人与其中一个路径点的距离大于l时，则将该路径点作为前视点。

本申请中的两轮差速机器人，两轮差速底盘由于结构简单，成本低，被广泛应用于移动机器人领域，如清洁机器人(扫地机器人、消毒机器人)，服务机器人(配送机器人、引领机器人)。

本发明提供的用于两轮差速移动机器人导航系统的路径跟随算法，符合运动学约束和动力学约束，可以以很高的精度，流畅跟随目标路径，且经过长时间真机实测，具有较强的鲁棒性，能够适应直道和复杂弯道等各种路径。

本申请提供的两轮差速移动机器人的路径跟随方法及系统，主要解决现有技术中路径跟随技术存在的技术问题：如路径跟随精度不高、需要为不同速度档位适配参数、调参复杂、耗时费力；运控控制效果卡顿不流畅、加速度过大等。

本发明的有益效果为：本发明提供了一种两轮差速移动机器人的路径跟随方法及系统，运动控制效果更流畅，且一定程度上保证了机器人在复杂路径上路径跟随精度，从而提高了效率。

以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (10)

1.一种两轮差速移动机器人的路径跟随方法，其特征在于，包括如下步骤：

S1:根据导航系统获取目标路径；

S2:更新机器人当前位置，并获取机器人与目标路径距离最近的路径点；

S3:计算机器人的前视距离并确定前视点，根据机器人当前位置和前视点计算出曲率；

S4:根据路况确定期望的线速度指令，并对期望的线速度指令施加左右轮电机加速度约束产生最终线速度指令；

S5:根据最终线速度指令和曲率计算出角速度指令，将线速度指令和角速度指令转换成左右轮电机速度指令并发送至电机；

S6:重复步骤S2-S5，直至机器人达到目标路径的终点。

2.如权利要求1所述的路径跟随方法，其特征在于，所述步骤S1具体包括：

根据导航系统获取目标路径，所述目标路径包括多个路径点，其中每个路径点包括：

全局坐标系下的x坐标和y坐标；

全局坐标系下目标路径的朝向，以及目标路径下每一个路径点的曲率。

3.如权利要求1所述的路径跟随方法，其特征在于，所述步骤S3中计算机器人的前视距离并确定前视点的具体步骤包括：

机器人的前视距离计算公式为：

l＝k*v

其中，k为可调节比例系数，v为机器人当前实际线速度；

以机器人与目标路径距离最近的路径点作为基准，计算出机器人与每一个路径点之间的距离，当计算出机器人与其中一个路径点的距离大于l时，则将该路径点作为前视点。

4.如权利要求1所述的路径跟随方法，其特征在于，所述步骤S3中根据机器人当前位置和前视点计算出曲率具体包括：

根据机器人当前位置和前视点(x，y)的圆弧计算出曲率c，其计算公式：

其中，r为圆弧半径，x为前视点在机器人坐标系下的X轴坐标，y为前视点在机器人坐标系下的Y轴坐标。

5.如权利要求1所述的路径跟随方法，其特征在于，所述步骤S4中根据路况确定期望的线速度指令的具体步骤为：

确定期望的线速度指令，其计算公式为：

v_desire＝min(v₁，v₂，v₃)

通过计算机器人前视方向某一段路径上的所有路径点的曲率，并找出曲率的最大值，其中，线速度指令V₁与最大曲率成反比；

通过计算机器人前视方向某一段路径上的所有路径点与障碍物之间的距离，并找出距离的最小值，其中，线速度指令V₂与最小距离成反比；

通过计算机器人当前位置与目标路径终点之间的距离，其中，线速度指令V₃与该段距离成反比。

6.如权利要求1所述的路径跟随方法，其特征在于，所述步骤S4中对期望的线速度指令施加左右轮电机加速度约束产生最终线速度指令具体包括：

将步骤S2-S5作为一个控制周期，计算出当前控制周期的线速度指令的取值区间，并对期望的线速度指令施加取值区间约束，即可得出最终线速度指令，其取值区间计算公式为：

其中，下标k为当前控制周期，下标k-1为上一个控制周期，V_k为当前控制周期的线速度指令，a为通过测试左右轮电机的带负载阶跃响应，确定出的左右轮的最大加速度，R为机器人的左轮，L为机器人的右轮，△t代表控制周期。

7.如权利要求6所述的路径跟随方法，其特征在于，所述对期望的线速度指令施加取值区间约束具体包括：

当施加的取值小于区间下限时，则取区间下限值进行约束；

当施加的取值大于区间上限时，则取区间上限值进行约束；

当施加的取值位于区间内时，则保持不变。

8.如权利要求1所述的路径跟随方法，其特征在于，所述步骤S5具体包括：

根据最终线速度指令和曲率计算出角速度指令，所述角速度指令的计算公式为：

ω_cmd＝c*v_cmd

其中，c为曲率，V_cmd为最终线速度指令；

将线速度指令和角速度指令转换成左右轮电机速度指令并发送至电机，其转换公式为：

其中，c为曲率，r为半径，ω_cmd为角速度指令，V_cmd为最终线速度指令，d为轮间距的一半，VR_cmd为右轮线速度指令，VL_cmd为左轮线速度指令。

9.一种两轮差速移动机器人的路径跟随系统，其特征在于，包括：

获取模块：用于获取目标路径、机器人当前位置以及目标路径上每个路径点的坐标；

控制模块：计算机器人的前视距离并确定前视点，根据机器人当前位置和前视点计算出曲率，根据路况确定期望的线速度指令，并对期望的线速度指令施加左右轮电机加速度约束产生最终线速度指令，根据最终线速度指令和曲率计算出角速度指令，将线速度指令和角速度指令转换成左右轮电机速度指令；

驱动模块：接收控制模块所下发的左右轮电机速度指令并对机器人进行驱动。

10.如权利要求9所述的路径跟随系统，其特征在于，在所述控制模块中，计算机器人的前视距离并确定前视点具体包括：

机器人的前视距离计算公式为：

l＝k*v

其中，k为可调节比例系数，v为机器人当前实际线速度；

以机器人与目标路径距离最近的路径点作为基准，计算出机器人与每一个路径点之间的距离，当计算出机器人与其中一个路径点的距离大于l时，则将该路径点作为前视点。

Images