差速轮底盘机器人自适应路径跟随方法及其相关设备

摘要

本申请属于导航技术领域，公开了一种差速轮底盘机器人自适应路径跟随方法及其相关设备，方法包括获取目标路径的路径点的路径点位置，在跟随路径更新周期开始的时刻，则获取当前时刻差速轮底盘机器人的机器人全局坐标、偏航角度、线速度和角速度，根据预设的期望线速度和预设的期望前视距离计算跟随路径的总时间，根据前面获得的数据确定四阶贝塞尔曲线形的跟随路径的五个控制点的控制点全局坐标，根据五个控制点全局坐标生成跟随路径的显示函数，并根据显示函数生成在当前的跟随路径更新周期内的每个控制周期的控制指令，以控制差速轮底盘机器人运动；从而能够提高路径跟随精度、减小位置震荡和更充分地发挥差速轮底盘的转弯性能优势。

权利要求

1.一种差速轮底盘机器人自适应路径跟随方法，用于对差速轮底盘机器人进行控制；

其特征在于，包括步骤：

A1.获取目标路径的路径点在世界坐标系下的路径点位置；

A2.若当前时刻为跟随路径更新周期开始的时刻，则获取当前时刻所述差速轮底盘机器人在世界坐标系下的机器人全局坐标、偏航角度、线速度和角速度；

A3.根据预设的期望线速度和预设的期望前视距离，计算跟随路径的总时间；

A4.根据所述路径点位置、所述总时间以及当前时刻的所述机器人全局坐标、所述偏航角度、所述线速度和所述角速度，确定四阶贝塞尔曲线形的跟随路径的五个控制点在世界坐标系下的控制点全局坐标；

A5.根据五个所述控制点在世界坐标系下的所述控制点全局坐标生成跟随路径的显示函数，并根据所述显示函数生成在当前的所述跟随路径更新周期内的每个控制周期的控制指令，以控制所述差速轮底盘机器人运动；

所述四阶贝塞尔曲线为：

4 3 2 2 3 4

P(τ)＝P0(1‑τ) +4P1τ(1‑τ) +6P2τ(1‑τ) +4P3τ(1‑τ)+P4τ；

其中，τ为归一化时间，t为时间，P(τ)为τ时刻的路径位置，P0、P1、P2、P3、P4为五个所述控制点的控制点坐标，T为所述总时间；

步骤A4包括：

A401.以当前时刻的所述差速轮底盘机器人的位置点作为第一个所述控制点，获取第一个所述控制点在世界坐标系下的控制点全局坐标；

A402.根据当前时刻的所述线速度计算第二个所述控制点在世界坐标系下的控制点全局坐标；

A403.根据当前时刻的所述角速度计算第三个所述控制点在世界坐标系下的控制点全局坐标；

A404.根据第三个所述控制点的所述控制点全局坐标，确定所述目标路径的一个路径点作为第四个所述控制点，获取第四个所述控制点在世界坐标系下的控制点全局坐标；

A405.根据第四个所述控制点的所述控制点全局坐标，确定所述目标路径的一个路径点作为第五个所述控制点，获取第五个所述控制点在世界坐标系下的控制点全局坐标。

2.根据权利要求1所述的差速轮底盘机器人自适应路径跟随方法，其特征在于，步骤A402包括：根据以下公式计算第二个所述控制点在机器人坐标系下的控制点局部坐标：B

P1x＝v0/4；

B

P1y＝0；

B B

其中，P1x为第二个所述控制点在机器人坐标系下的控制点局部坐标的横坐标值，P1y为第二个所述控制点在机器人坐标系下的控制点局部坐标的纵坐标值，v0为当前时刻的所述线速度；

通过坐标转换把第二个所述控制点在机器人坐标系下的控制点局部坐标转换为在世界坐标系下的所述控制点全局坐标。

3.根据权利要求1所述的差速轮底盘机器人自适应路径跟随方法，其特征在于，步骤A403包括：根据以下公式计算第三个所述控制点在机器人坐标系下的控制点局部坐标：B

P2y＝ω0/12；

B B

其中，P2x为第三个所述控制点在机器人坐标系下的控制点局部坐标的横坐标值，P2y为第三个所述控制点在机器人坐标系下的控制点局部坐标的纵坐标值，ω0为当前时刻的所述角速度，ld为所述期望前视距离；

通过坐标转换把第三个所述控制点在机器人坐标系下的控制点局部坐标转换为在世界坐标系下的所述控制点全局坐标。

4.根据权利要求1所述的差速轮底盘机器人自适应路径跟随方法，其特征在于，步骤A404包括：B1.从前到后依次以各所述路径点为第一备选点，循环执行以下步骤：计算所述第一备选点与第三个所述控制点的第一距离l1；

计算所述目标路径在所述第一备选点处的第一曲率r1；

若r1＜ld/2，则使第一参考距离L1等于r1/2，若r1≥ld/2，则使第一参考距离L1等于ld/4；

ld为所述期望前视距离；

如果l1≥L1，则停止循环，并执行步骤B2；

B2.把最后的所述第一备选点作为第四个所述控制点，获取第四个所述控制点在世界坐标系下的所述控制点全局坐标。

5.根据权利要求1所述的差速轮底盘机器人自适应路径跟随方法，其特征在于，步骤A405包括：C1.从前到后依次以各所述路径点为第二备选点，循环执行以下步骤：计算所述第二备选点与第四个所述控制点的第二距离l2；

计算所述目标路径在所述第二备选点处的第二曲率r2；

若r2＜ld/2，则使第二参考距离L2等于r2/2，若r2≥ld/2，则使第二参考距离L2等于ld/4；

ld为所述期望前视距离；

如果l2≥L2，则停止循环，并执行步骤C2；

C2.把最后的所述第二备选点作为第五个所述控制点，获取第五个所述控制点在世界坐标系下的所述控制点全局坐标。

6.一种差速轮底盘机器人自适应路径跟随装置，用于对差速轮底盘机器人进行控制；

其特征在于，包括：

第一获取模块，用于获取目标路径的路径点在世界坐标系下的路径点位置；

第二获取模块，用于在当前时刻为跟随路径更新周期开始的时刻的时候，获取当前时刻所述差速轮底盘机器人在世界坐标系下的机器人全局坐标、偏航角度、线速度和角速度；

第一计算模块，用于根据预设的期望线速度和预设的期望前视距离，计算跟随路径的总时间；

控制点选取模块，用于根据所述路径点位置、所述总时间以及当前时刻的所述机器人全局坐标、所述偏航角度、所述线速度和所述角速度，确定四阶贝塞尔曲线形的跟随路径的五个控制点在世界坐标系下的控制点全局坐标；

第一控制模块，用于根据五个所述控制点在世界坐标系下的所述控制点全局坐标生成跟随路径的显示函数，并根据所述显示函数生成在当前的所述跟随路径更新周期内的每个控制周期的控制指令，以控制所述差速轮底盘机器人运动；

所述四阶贝塞尔曲线为：

4 3 2 2 3 4

P(τ)＝P0(1‑τ) +4P1τ(1‑τ) +6P2τ(1‑τ) +4P3τ(1‑τ)+P4τ；

其中，τ为归一化时间，t为时间，P(τ)为τ时刻的路径位置，P0、P1、P2、P3、P4为五个所述控制点的控制点坐标，T为所述总时间；

控制点选取模块在根据所述路径点位置、所述总时间以及当前时刻的所述机器人全局坐标、所述偏航角度、所述线速度和所述角速度，确定四阶贝塞尔曲线形的跟随路径的五个控制点在世界坐标系下的控制点全局坐标的时候，执行：以当前时刻的所述差速轮底盘机器人的位置点作为第一个所述控制点，获取第一个所述控制点在世界坐标系下的控制点全局坐标；

根据当前时刻的所述线速度计算第二个所述控制点在世界坐标系下的控制点全局坐标；

根据当前时刻的所述角速度计算第三个所述控制点在世界坐标系下的控制点全局坐标；

根据第三个所述控制点的所述控制点全局坐标，确定所述目标路径的一个路径点作为第四个所述控制点，获取第四个所述控制点在世界坐标系下的控制点全局坐标；

根据第四个所述控制点的所述控制点全局坐标，确定所述目标路径的一个路径点作为第五个所述控制点，获取第五个所述控制点在世界坐标系下的控制点全局坐标。

7.一种电子设备，其特征在于，包括处理器和存储器，所述存储器存储有所述处理器可执行的计算机程序，所述处理器执行所述计算机程序时，运行如权利要求1‑5任一项所述的差速轮底盘机器人自适应路径跟随方法中的步骤。

8.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时运行如权利要求1‑5任一项所述的差速轮底盘机器人自适应路径跟随方法中的步骤。

说明书

差速轮底盘机器人自适应路径跟随方法及其相关设备

技术领域

[0001] 本申请涉及导航技术领域，具体而言，涉及一种差速轮底盘机器人自适应路径跟随方法及其相关设备。

背景技术

[0002] 复合机器人通常由底盘、执行机构和传感器等组合而成，其中底盘用于安装其他构件并驱动机器人整体空间移动，常见的底盘包括阿克曼底盘、差速轮底盘、腿足式底盘以及麦克纳姆轮底盘等。其中差速轮底盘具有良好的载重能力和越野性能，而且相较于阿克曼底盘，由于四轮差速轮底盘依靠轮子转速差实现转弯，能够实现小半径转弯甚至原地掉头，因此灵活性也更加优秀，更加适用于园区巡检机器人。

[0003] 为了提高机器人的运动精度，在实际导航过程中，路径规划以及跟随控制的算法需要与底盘的运动学、动力学模型相匹配，来最大的发挥底盘的优势性能。一般地，在规划生成由起始点到目标点的路径后，由路径跟随算法根据路径点的位置和机器人实时位姿生成实时的控制指令来让机器人沿规划的路径运动，实现对规划的路径的跟踪。

[0004] 目前常用的路径跟随算法包括：纯跟踪（PurePursuit），Stanley Method前轴控制，PID控制，MPC控制等。其中PurePursuit 是基于阿克曼小车模型提出的，只考虑在位置上的跟随，会导致到达跟随点后小车方向与路径切向不一致，同时对于四轮差速机器人而言，PurePursuit的最小转弯半径与前视距离正相关，若前视距离过小，机器人总体沿路径进行S型行进，前视距离过大则不能将四轮差速机器人可以实现0转弯半径的特性进行发挥。PID控制模型结构简单，调整方便，得到了广泛应用，但在实际应用过程中，如果机器人与路径出现了较大的位置偏差或路径存在转弯半径较小、连续转弯等情况，容易出现实际控制过程中位置震荡，同时PID 不能保证在一定距离或时间范围内一定到达路径点上。MPC控制以优化的方式求解在约束下最优行进路径，由于建立的模型与真实系统存在误差，同时真实系统中存在扰动，因此MPC仅执行输出序列的第一个控制输出，由于MPC求解优化问题计算量较大，因此在机器人系统上以较高频率进行计算会占用大量计算资源，实时性不够好。

[0005] 为此，需要寻求一种适用于差速轮底盘机器人（即具有差速轮底盘的机器人）的自适应路径跟随方法，以提高跟随精度、减小位置震荡和更充分地发挥差速轮底盘的转弯性能优势。

发明内容

[0006] 本申请的目的在于提供一种差速轮底盘机器人自适应路径跟随方法及其相关设备，能够提高路径跟随精度、减小位置震荡和更充分地发挥差速轮底盘的转弯性能优势。

[0007] 第一方面，本申请提供了一种差速轮底盘机器人自适应路径跟随方法，用于对差速轮底盘机器人进行控制；包括步骤：

[0008] A1.获取目标路径的路径点在世界坐标系下的路径点位置；

[0009] A2.若当前时刻为跟随路径更新周期开始的时刻，则获取当前时刻所述差速轮底盘机器人在世界坐标系下的机器人全局坐标、偏航角度、线速度和角速度；

[0010] A3.根据预设的期望线速度和预设的期望前视距离，计算跟随路径的总时间；

[0011] A4.根据所述路径点位置、所述总时间以及当前时刻的所述机器人全局坐标、所述偏航角度、所述线速度和所述角速度，确定四阶贝塞尔曲线形的跟随路径的五个控制点在世界坐标系下的控制点全局坐标；

[0012] A5.根据五个所述控制点在世界坐标系下的控制点全局坐标生成跟随路径的显示函数，并根据所述显示函数生成在当前的所述跟随路径更新周期内的每个控制周期的控制指令，以控制所述差速轮底盘机器人运动。

[0013] 每隔一个跟随路径更新周期即更新跟随路径，并只在该跟随路径更新周期内根据更新后的跟随路径生成控制指令，如此循环，能够有效减小扰动对跟随效果的影响，从而提高路径跟随精度；通过四阶贝塞尔曲线的控制点的自适应选取，可实现线速度和角速度的连续变化，减小位置震荡，并在存在小转弯半径和原地调头等情况时能够充分发挥差速轮底盘优势。

[0014] 优选地，所述四阶贝塞尔曲线为：

[0015] ；

[0016] ；

[0017] 其中， 为归一化时间，为时间， 为 时刻的路径位置， 、 、 、 、为五个所述控制点的控制点坐标， 为所述总时间。

[0018] 优选地，步骤A4包括：

[0019] A401.以当前时刻的所述差速轮底盘机器人的位置点作为第一个所述控制点，获取第一个所述控制点在世界坐标系下的控制点全局坐标；

[0020] A402.根据当前时刻的所述线速度计算第二个所述控制点在世界坐标系下的控制点全局坐标；

[0021] A403.根据当前时刻的所述角速度计算第三个所述控制点在世界坐标系下的控制点全局坐标；

[0022] A404.根据第三个所述控制点的所述控制点全局坐标，确定所述目标路径的一个路径点作为第四个所述控制点，获取第四个所述控制点在世界坐标系下的控制点全局坐标；

[0023] A405.根据第四个所述控制点的所述控制点全局坐标，确定所述目标路径的一个路径点作为第五个所述控制点，获取第五个所述控制点在世界坐标系下的控制点全局坐标。

[0024] 根据机器人位姿、线速度、角速度计算前三个控制点的位置能够使更新的跟随路径的角速度和线速度连续，通过在规划生成的目标路径上选取后两个控制点，可保证机器人在经过预设的期望前视距离时到达要跟随的路径上，且当后两个控制点都在路径点上时，能保证机器人前进速度方向与目标路径切向方向近似，不会出现S型行进或大幅度震荡。

[0025] 优选地，步骤A402包括：

[0026] 根据以下公式计算第二个所述控制点在机器人坐标系下的控制点局部坐标：

[0027] ；

[0028] ；

[0029] 其中， 为第二个所述控制点在机器人坐标系下的控制点局部坐标的横坐标值， 为第二个所述控制点在机器人坐标系下的控制点局部坐标的纵坐标值， 为当前时刻的所述线速度；

[0030] 通过坐标转换把第二个所述控制点在机器人坐标系下的控制点局部坐标转换为在世界坐标系下的所述控制点全局坐标。

[0031] 优选地，步骤A403包括：

[0032] 根据以下公式计算第三个所述控制点在机器人坐标系下的控制点局部坐标：

[0033] ；

[0034] ；

[0035] 其中， 为第三个所述控制点在机器人坐标系下的控制点局部坐标的横坐标值， 为第三个所述控制点在机器人坐标系下的控制点局部坐标的纵坐标值， 为当前时刻的所述角速度， 为所述期望前视距离；

[0036] 通过坐标转换把第三个所述控制点在机器人坐标系下的控制点局部坐标转换为在世界坐标系下的所述控制点全局坐标。

[0037] 通过上述方式计算第三个控制点的控制点全局坐标，有利于使后续生成的指令速度接近期望速度，避免指令速度过大。

[0038] 优选地，步骤A404包括：

[0039] B1.从前到后依次以各所述路径点为第一备选点，循环执行以下步骤：

[0040] 计算所述第一备选点与第三个所述控制点的第一距离 ；

[0041] 计算所述目标路径在所述第一备选点处的第一曲率 ；

[0042] 若 ，则使第一参考距离 等于 ，若 ，则使第一参考距离 等于 ； 为所述期望前视距离；

[0043] 如果 ，则停止循环，并执行步骤B2；

[0044] B2.把最后的所述第一备选点作为第四个所述控制点，获取第四个所述控制点在世界坐标系下的所述控制点全局坐标。

[0045] 优选地，步骤A405包括：

[0046] C1.从前到后依次以各所述路径点为第二备选点，循环执行以下步骤：

[0047] 计算所述第二备选点与第四个所述控制点的第二距离 ；

[0048] 计算所述目标路径在所述第二备选点处的第二曲率 ；

[0049] 若 ，则使第二参考距离 等于 ，若 ，则使第二参考距离 等于 ； 为所述期望前视距离；

[0050] 如果 ，则停止循环，并执行步骤C2；

[0051] C2.把最后的所述第二备选点作为第五个所述控制点，获取第五个所述控制点在世界坐标系下的所述控制点全局坐标。

[0052] 第二方面，本申请提供了一种差速轮底盘机器人自适应路径跟随装置，用于对差速轮底盘机器人进行控制；包括：

[0053] 第一获取模块，用于获取目标路径的路径点在世界坐标系下的路径点位置；

[0054] 第二获取模块，用于在当前时刻为跟随路径更新周期开始的时刻的时候，获取当前时刻所述差速轮底盘机器人在世界坐标系下的机器人全局坐标、偏航角度、线速度和角速度；

[0055] 第一计算模块，用于根据预设的期望线速度和预设的期望前视距离，计算跟随路径的总时间；

[0056] 控制点选取模块，用于根据所述路径点位置、所述总时间以及当前时刻的所述机器人全局坐标、所述偏航角度、所述线速度和所述角速度，确定四阶贝塞尔曲线形的跟随路径的五个控制点在世界坐标系下的控制点全局坐标；

[0057] 第一控制模块，用于根据五个所述控制点在世界坐标系下的控制点全局坐标生成跟随路径的显示函数，并根据所述显示函数生成在当前的所述跟随路径更新周期内的每个控制周期的控制指令，以控制所述差速轮底盘机器人运动。

[0058] 每隔一个跟随路径更新周期即更新跟随路径，并只在该跟随路径更新周期内根据更新后的跟随路径生成控制指令，如此循环，能够有效减小扰动对跟随效果的影响，从而提高路径跟随精度；通过四阶贝塞尔曲线的控制点的自适应选取，可实现线速度和角速度的连续变化，减小位置震荡，并在存在小转弯半径和原地调头等情况时能够充分发挥差速轮底盘优势。

[0059] 第三方面，本申请提供了一种电子设备，包括处理器和存储器，所述存储器存储有所述处理器可执行的计算机程序，所述处理器执行所述计算机程序时，运行如前文所述的差速轮底盘机器人自适应路径跟随方法中的步骤。

[0060] 第四方面，本申请提供了一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时运行如前文所述的差速轮底盘机器人自适应路径跟随方法中的步骤。

[0061] 有益效果：本申请提供的差速轮底盘机器人自适应路径跟随方法及其相关设备，每隔一个跟随路径更新周期即更新跟随路径，并只在该跟随路径更新周期内根据更新后的跟随路径生成控制指令，如此循环，能够有效减小扰动对跟随效果的影响，从而提高路径跟随精度；通过四阶贝塞尔曲线的控制点的自适应选取，可实现线速度和角速度的连续变化，减小位置震荡，并在存在小转弯半径和原地调头等情况时能够充分发挥差速轮底盘优势。

附图说明

[0062] 图1为本申请实施例提供的差速轮底盘机器人自适应路径跟随方法的流程图。

[0063] 图2为本申请实施例提供的差速轮底盘机器人自适应路径跟随装置的结构示意图。

[0064] 图3为本申请实施例提供的电子设备的结构示意图。

[0065] 图4为世界坐标系与机器人坐标系的示意图。

[0066] 标号说明：1、第一获取模块；2、第二获取模块；3、第一计算模块；4、控制点选取模块；5、第一控制模块；301、处理器；302、存储器；303、通信总线。

具体实施方式

[0067] 下面将结合本申请实施例中附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。通常在此处附图中描述和示出的本申请实施例的组件可以以各种不同的配置来布置和设计。因此，以下对在附图中提供的本申请的实施例的详细描述并非旨在限制要求保护的本申请的范围，而是仅仅表示本申请的选定实施例。基于本申请的实施例，本领域技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。

[0068] 应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。同时，在本申请的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

[0069] 请参照图1，图1是本申请一些实施例中的一种差速轮底盘机器人自适应路径跟随方法，用于对差速轮底盘机器人进行控制；包括步骤：

[0070] A1.获取目标路径的路径点在世界坐标系下的路径点位置；

[0071] A2.若当前时刻为跟随路径更新周期开始的时刻，则获取当前时刻差速轮底盘机器人在世界坐标系下的机器人全局坐标、偏航角度、线速度和角速度；

[0072] A3.根据预设的期望线速度和预设的期望前视距离，计算跟随路径的总时间；

[0073] A4.根据路径点位置、总时间以及当前时刻的机器人全局坐标、偏航角度、线速度和角速度，确定四阶贝塞尔曲线形的跟随路径的五个控制点在世界坐标系下的控制点全局坐标；

[0074] A5.根据五个控制点在世界坐标系下的控制点全局坐标生成跟随路径的显示函数，并根据显示函数生成在当前的跟随路径更新周期内的每个控制周期的控制指令，以控制差速轮底盘机器人运动。

[0075] 每隔一个跟随路径更新周期即更新跟随路径，并只在该跟随路径更新周期内根据更新后的跟随路径生成控制指令，如此循环，能够有效减小扰动对跟随效果的影响，从而提高路径跟随精度；通过四阶贝塞尔曲线的控制点的自适应选取，可实现线速度和角速度的连续变化，减小位置震荡，并在存在小转弯半径和原地调头等情况时能够充分发挥差速轮底盘优势。

[0076] 其中，目标路径是从起始点通往目标点的路径，可通过现有技术的任意路径规划方法规划得到，再对规划得到的目标路径进行离散处理即得到多个路径点在世界坐标系下的路径点位置。

[0077] 其中，跟随路径更新周期可根据实际需要设置，一般可取为控制周期的n倍，n为正整数，可根据实际需要设置，从而，每根据最新的跟随路径生成n个控制指令控制差速轮底盘机器人运动后则更新一次跟随路径。

[0078] 其中，当前时刻差速轮底盘机器人在世界坐标系下的机器人全局坐标、偏航角度、线速度和角速度可通过设置在差速轮底盘机器人上的定位模块和传感器实时测得。

[0079] 其中，跟随路径的总时间实际上是使差速轮底盘机器人从当前位置回到目标路径上所需的期望时间，因此，期望线速度和期望前视距离实际上是对该期望时间的两个约束参数，期望线速度和期望前视距离可根据实际需要设置。

[0080] 具体地，步骤A3包括：

[0081] 根据以下公式计算总时间：

[0082] ；

[0083] 其中，为总时间， 为期望前视距离， 为期望线速度。

[0084] 在本实施例中，四阶贝塞尔曲线为：

[0085]  （1）；

[0086] ；

[0087] 其中， 为归一化时间，为时间（该时间以当前的跟随路径更新周期开始的时刻为零点）， 为 时刻的路径位置（即跟随路径的路径点的位置）， 、 、 、 、 为五个控制点的控制点坐标（ 为第一个控制点的控制点坐标， 为第二个控制点的控制点坐标， 为第三个控制点的控制点坐标， 为第四个控制点的控制点坐标， 为第五个控制点的控制点坐标）；其中， 、 、 、 、 、 均包括两个横坐标值和纵坐标值。

[0088] 通过对该公式（1）求导，并使 为0，可得到：

[0089]  （2）；

[0090]  （3）；

[0091] 其中， 为 时 的一阶导数， 为 时 的二阶导数。

[0092] 在一些优选实施方式中，步骤A4包括：

[0093] A401.以当前时刻的差速轮底盘机器人的位置点作为第一个控制点，获取第一个控制点在世界坐标系下的控制点全局坐标；

[0094] A402.根据当前时刻的线速度计算第二个控制点在世界坐标系下的控制点全局坐标；

[0095] A403.根据当前时刻的角速度计算第三个控制点在世界坐标系下的控制点全局坐标；

[0096] A404.根据第三个控制点的控制点全局坐标，确定目标路径的一个路径点作为第四个控制点，获取第四个控制点在世界坐标系下的控制点全局坐标；

[0097] A405.根据第四个控制点的控制点全局坐标，确定目标路径的一个路径点作为第五个控制点，获取第五个控制点在世界坐标系下的控制点全局坐标。

[0098] 根据机器人位姿（包括机器人全局坐标和偏航角度）、线速度、角速度计算前三个控制点的位置能够使更新的跟随路径的角速度和线速度连续，通过在规划生成的目标路径上选取后两个控制点，可保证机器人在经过预设的期望前视距离时到达要跟随的路径上，且当后两个控制点都在路径点上时，能保证机器人前进速度方向与目标路径切向方向近似，不会出现S型行进或大幅度震荡。

[0099] 参考图4，在跟随路径更新周期开始的时刻，机器人坐标系B相对世界坐标系w具有一个位置偏移量（即两个坐标系原点之间的位置偏差）和一个旋转角度（即两个坐标系的横坐标之间的夹角）；其中机器人坐标系B的原点为差速轮底盘机器人的位置点（也是更新后的跟随路径的起始点），因此该位置偏移量实际上等于差速轮底盘机器人在世界坐标系下的机器人全局坐标；其中机器人坐标系B的横坐标正方向为差速轮底盘机器人的线速度方向，因此该旋转角度等于差速轮底盘机器人的线速度方向与机器人坐标系B的横坐标正方向之间的夹角，该夹角即为在跟随路径更新周期开始的时刻的差速轮底盘机器人在世界坐标系下的偏航角度。

[0100] 在步骤A401中，当前时刻（此时为跟随路径更新周期开始的时刻）的差速轮底盘机器人的位置点（也是更新后的跟随路径的起始点）即当前时刻差速轮底盘机器人在世界坐标系下的机器人全局坐标对应的位置点，因此，第一个控制点在世界坐标系下的控制点全局坐标等于当前时刻差速轮底盘机器人在世界坐标系下的机器人全局坐标。见图4，此时的第一个控制点 为机器人坐标系B的原点。

[0101] 在本实施例中，步骤A402包括：

[0102] 根据以下公式计算第二个控制点在机器人坐标系下的控制点局部坐标：

[0103]  （4）；

[0104]  （5）；

[0105] 其中， 为第二个控制点在机器人坐标系下的控制点局部坐标的横坐标值，为第二个控制点在机器人坐标系下的控制点局部坐标的纵坐标值， 为当前时刻的线速度；

[0106] 通过坐标转换把第二个控制点在机器人坐标系下的控制点局部坐标转换为在世界坐标系下的控制点全局坐标。

[0107] 根据公式（2）有：

[0108] ；

[0109] 其中， 为 时 的一阶导数， 为机器人坐标系下的 时刻的路径位置， 为第二个控制点的在机器人坐标系下的控制点局部坐标， 为 的横坐标值， 为 的纵坐标值， 为第一个控制点的在机器人坐标系下的控制点局部坐标， 为 的横坐标值， 为 的纵坐标值；

[0110] 由于第一个控制点 为机器人坐标系的原点，因此 和 均为零，又由于机器人坐标系的横坐标正方向为差速轮底盘机器人的当前时刻的线速度方向，因此有， 为当前时刻差速轮底盘机器人的线速度，所以上式转换为：

[0111]  （6）；

[0112] 根据该公式（6）即可得到公式（4）和（5）；从而，第二个控制点实际上为当前时刻的机器人坐标系横坐标上的点，且该第二个控制点与机器人坐标系原点的距离等于差速轮底盘机器人当前时刻的线速度大小（如图4所示）。

[0113] 其中，可通过以下公式把第二个控制点在机器人坐标系下的控制点局部坐标转换为在世界坐标系下的控制点全局坐标：

[0114] ；

[0115] 其中， 为第二个控制点在世界坐标系下的控制点全局坐标， 为第一个控制点在世界坐标系下的控制点全局坐标（等于当前时刻差速轮底盘机器人在世界坐标系下的机器人全局坐标）， 为机器人坐标系和世界坐标系之间的转换矩阵，且有：

[0116] ； 为机器人坐标系相对世界坐标系的旋转角度（等于当前时刻差速轮底盘机器人在世界坐标系下的偏航角度）。

[0117] 在本实施例中，步骤A403包括：

[0118] 根据以下公式计算第三个控制点在机器人坐标系下的控制点局部坐标：

[0119]  （7）；

[0120]  （8）；

[0121] 其中， 为第三个控制点在机器人坐标系下的控制点局部坐标的横坐标值，为第三个控制点在机器人坐标系下的控制点局部坐标的纵坐标值， 为当前时刻的角速度；

[0122] 通过坐标转换把第三个控制点在机器人坐标系下的控制点局部坐标转换为在世界坐标系下的控制点全局坐标。

[0123] 通过上述方式计算第三个控制点的控制点全局坐标，有利于使后续生成的指令速度（即线速度指令对应的速度）接近期望速度，避免指令速度过大。

[0124] 根据公式（2）、（3）、（6）有：

[0125]  （9）；

[0126] 其中， 为 时 的二阶导数， 为 的横坐标值， 为的纵坐标值， 为第三个控制点在机器人坐标系下的控制点局部坐标。

[0127] 根据曲线曲率计算公式、角速度计算公式和公式（6）、（9）可以得到：

[0128] （10）；

[0129] 其中， 为 时机器人坐标系下的角速度， 为 的横坐标值， 为 的纵坐标值， 为 的横坐标值， 为 的纵
坐标值。

[0130] 根据该公式（10）可以得到公式（7）。

[0131] 通过试验证明，当采用公式（8）计算第三个控制点在机器人坐标系下的控制点局部坐标的横坐标值时，有利于使后续生成的指令速度接近期望速度。

[0132] 其中，可通过以下公式把第三个控制点在机器人坐标系下的控制点局部坐标转换为在世界坐标系下的控制点全局坐标：

[0133] ；

[0134] 其中， 为第三个控制点在世界坐标系下的控制点全局坐标， 为第三个控制点在机器人坐标系下的控制点局部坐标。图4中， 点为第三个控制点。

[0135] 在本实施例中，步骤A404包括：

[0136] B1.从前到后依次以各路径点为第一备选点，循环执行以下步骤：

[0137] 计算第一备选点与第三个控制点的第一距离 （此处为了便于与其它距离做区分，把第一备选点与第三个控制点的距离称为第一距离）；

[0138] 计算目标路径在第一备选点处的第一曲率 （此处为了便于与其它曲率做区分，把目标路径在第一备选点处的曲率称为第一曲率）；

[0139] 若 ，则使第一参考距离 等于 ，若 ，则使第一参考距离 等于 ；

[0140] 如果 ，则停止循环，并执行步骤B2；

[0141] B2.把最后的第一备选点作为第四个控制点，获取第四个控制点在世界坐标系下的控制点全局坐标。

[0142] 其中，两个点之间的距离和路径在某一路径点上的曲率的计算方法为现有技术，此处不对其进行详述。

[0143] 具体地，先把第一个路径点作为第一备选点，然后计算该第一备选点对应的 和，然后确定 值，若满足 ，则把第一个路径点作为第四个控制点，否则把下一个路径点作为第一备选点，然后重复上述步骤，如此循环，直到 ，假设在把第k个路径点作为第一备选点时满足该条件，则把第k个路径点作为第四个控制点，并提取该路径点在世界坐标系下的路径点位置作为第四个控制点在世界坐标系下的控制点全局坐标。

[0144] 在本实施例中，步骤A405包括：

[0145] C1.从前到后依次以各路径点为第二备选点，循环执行以下步骤：

[0146] 计算第二备选点与第四个控制点的第二距离 （此处为了便于与其它距离做区分，把第二备选点与第四个控制点的距离称为第二距离）；

[0147] 计算目标路径在第二备选点处的第二曲率 （此处为了便于与其它曲率做区分，把目标路径在第二备选点处的曲率称为第二曲率）；

[0148] 若 ，则使第二参考距离 等于 ，若 ，则使第二参考距离 等于 ；

[0149] 如果 ，则停止循环，并执行步骤C2；

[0150] C2.把最后的第二备选点作为第五个控制点，获取第五个控制点在世界坐标系下的控制点全局坐标。

[0151] 具体地，先把第一个路径点作为第二备选点，然后计算该第二备选点对应的 和，然后确定 值，若满足 ，则把第一个路径点作为第五个控制点，否则把下一个路径点作为第二备选点，然后重复上述步骤，如此循环，直到 ，假设在把第k个路径点作为第二备选点时满足该条件，则把第k个路径点作为第五个控制点，并提取该路径点在世界坐标系下的路径点位置作为第五个控制点在世界坐标系下的控制点全局坐标。

[0152] 步骤A5中，根据公式（1）生成跟随路径的显示函数，该显示函数为：

[0153] ；

[0154] 其中， 为世界坐标系下 时刻的路径位置， 、 、 、 、 为五个控制点在世界坐标系下的控制点全局坐标。

[0155] 其中，步骤A5中生成的控制指令包括线速度指令和角速度指令。

[0156] 在生成当前的跟随路径更新周期内的各个控制周期的控制指令时，根据各控制周期的时间（该时间以当前的跟随路径更新周期开始的时刻为零点）计算归一化时间 ，其中，计算公式为：

[0157] ；

[0158] 然后在各控制周期的归一化时间 处对显示函数进行链式求导和曲线微分计算（具体计算过程为现有技术，此处不对其进行详述），可得到各控制周期的线速度指令和角速度指令。

[0159] 例如，若每个跟随路径更新周期等于n个控制周期，每个控制周期为△T，则生成t=0、t=△T、t=2△T、...、t=（n‑1）△T共n个时刻的控制指令，分别用于在t=0、t=△T、t=2△T、...、t=（n‑1）时刻作为控制输入来控制差速轮底盘机器人运动。从而，由于每次更新跟随路径后只根据更新后的跟随路径进行n个控制指令的生成和使用，即再次进行跟随路径更新，如此循环，当工作过程中受到干扰，也能够有效减小该干扰对跟随效果的影响。

[0160] 实际上，当差速轮底盘机器人移动到接近目标点（即目标路径的终点）的时候容易出现搜索出的第四个控制点和第五个控制点重合或非常接近的情况，此时，若继续依据四阶贝塞尔曲线形的跟随路径生成控制指令以控制差速轮底盘机器人运动，会导致差速轮底盘机器人绕圈接近目标点，从而延迟到达目标点的时间。为了避免差速轮底盘机器人最后绕圈接近目标点，在一些优选实施方式中，步骤A4之后和步骤A5之前还包括步骤：

[0161] A6.计算第四个控制点和第五个控制点之间的控制点距离；

[0162] A7.若该控制点距离不小于预设的距离阈值（可根据实际需要设置），则执行步骤A5；

[0163] A8.若该控制点距离小于预设的距离阈值，则令差速轮底盘机器人原地旋转至正前方对准目标点后控制差速轮底盘机器人沿直线移动至该目标点。

[0164] 由上可知，该差速轮底盘机器人自适应路径跟随方法，通过获取目标路径的路径点在世界坐标系下的路径点位置，若当前时刻为跟随路径更新周期开始的时刻，则获取当前时刻差速轮底盘机器人在世界坐标系下的机器人全局坐标、偏航角度、线速度和角速度，根据预设的期望线速度和预设的期望前视距离，计算跟随路径的总时间，根据路径点位置、总时间以及当前时刻的机器人全局坐标、偏航角度、线速度和角速度，确定四阶贝塞尔曲线形的跟随路径的五个控制点在世界坐标系下的控制点全局坐标，根据五个控制点全局坐标生成跟随路径的显示函数，并根据显示函数生成在当前的跟随路径更新周期内的每个控制周期的控制指令，以控制差速轮底盘机器人运动；从而能够提高路径跟随精度、减小位置震荡和更充分地发挥差速轮底盘的转弯性能优势。

[0165] 参考图2，本申请提供了一种差速轮底盘机器人自适应路径跟随装置，用于对差速轮底盘机器人进行控制；包括：

[0166] 第一获取模块1，用于获取目标路径的路径点在世界坐标系下的路径点位置；

[0167] 第二获取模块2，用于在当前时刻为跟随路径更新周期开始的时刻的时候，获取当前时刻差速轮底盘机器人在世界坐标系下的机器人全局坐标、偏航角度、线速度和角速度；

[0168] 第一计算模块3，用于根据预设的期望线速度和预设的期望前视距离，计算跟随路径的总时间；

[0169] 控制点选取模块4，用于根据路径点位置、总时间以及当前时刻的机器人全局坐标、偏航角度、线速度和角速度，确定四阶贝塞尔曲线形的跟随路径的五个控制点在世界坐标系下的控制点全局坐标；

[0170] 第一控制模块5，用于根据五个控制点在世界坐标系下的控制点全局坐标生成跟随路径的显示函数，并根据显示函数生成在当前的跟随路径更新周期内的每个控制周期的控制指令，以控制差速轮底盘机器人运动。

[0171] 每隔一个跟随路径更新周期即更新跟随路径，并只在该跟随路径更新周期内根据更新后的跟随路径生成控制指令，如此循环，能够有效减小扰动对跟随效果的影响，从而提高路径跟随精度；通过四阶贝塞尔曲线的控制点的自适应选取，可实现线速度和角速度的连续变化，减小位置震荡，并在存在小转弯半径和原地调头等情况时能够充分发挥差速轮底盘优势。

[0172] 其中，目标路径是从起始点通往目标点的路径，可通过现有技术的任意路径规划方法规划得到，再对规划得到的目标路径进行离散处理即得到多个路径点在世界坐标系下的路径点位置。

[0173] 其中，跟随路径更新周期可根据实际需要设置，一般可取为控制周期的n倍，n为正整数，可根据实际需要设置，从而，每根据最新的跟随路径生成n个控制指令控制差速轮底盘机器人运动后则更新一次跟随路径。

[0174] 其中，当前时刻差速轮底盘机器人在世界坐标系下的机器人全局坐标、偏航角度、线速度和角速度可通过设置在差速轮底盘机器人上的定位模块和传感器实时测得。

[0175] 其中，跟随路径的总时间实际上是使差速轮底盘机器人从当前位置回到目标路径上所需的期望时间，因此，期望线速度和期望前视距离实际上是对该期望时间的两个约束参数，期望线速度和期望前视距离可根据实际需要设置。

[0176] 具体地，第一计算模块3在根据预设的期望线速度和预设的期望前视距离，计算跟随路径的总时间的时候，执行：

[0177] 根据以下公式计算总时间：

[0178] ；

[0179] 其中，为总时间， 为期望前视距离， 为期望线速度。

[0180] 在本实施例中，四阶贝塞尔曲线为：

[0181]  （1）；

[0182] ；

[0183] 其中， 为归一化时间，为时间（该时间以当前的跟随路径更新周期开始的时刻为零点）， 为 时刻的路径位置（即跟随路径的路径点的位置）， 、 、 、 、 为五个控制点的控制点坐标（ 为第一个控制点的控制点坐标， 为第二个控制点的控制点坐标， 为第三个控制点的控制点坐标， 为第四个控制点的控制点坐标， 为第五个控制点的控制点坐标）；其中， 、 、 、 、 、 均包括两个横坐标值和纵坐标值。

[0184] 通过对该公式（1）求导，并使 为0，可得到：

[0185]  （2）；

[0186]  （3）；

[0187] 其中， 为 时 的一阶导数， 为 时 的二阶导数。

[0188] 在一些优选实施方式中，控制点选取模块4在根据路径点位置、总时间以及当前时刻的机器人全局坐标、偏航角度、线速度和角速度，确定四阶贝塞尔曲线形的跟随路径的五个控制点在世界坐标系下的控制点全局坐标的时候，执行：

[0189] 以当前时刻的差速轮底盘机器人的位置点作为第一个控制点，获取第一个控制点在世界坐标系下的控制点全局坐标；

[0190] 根据当前时刻的线速度计算第二个控制点在世界坐标系下的控制点全局坐标；

[0191] 根据当前时刻的角速度计算第三个控制点在世界坐标系下的控制点全局坐标；

[0192] 根据第三个控制点的控制点全局坐标，确定目标路径的一个路径点作为第四个控制点，获取第四个控制点在世界坐标系下的控制点全局坐标；

[0193] 根据第四个控制点的控制点全局坐标，确定目标路径的一个路径点作为第五个控制点，获取第五个控制点在世界坐标系下的控制点全局坐标。

[0194] 根据机器人位姿（包括机器人全局坐标和偏航角度）、线速度、角速度计算前三个控制点的位置能够使更新的跟随路径的角速度和线速度连续，通过在规划生成的目标路径上选取后两个控制点，可保证机器人在经过预设的期望前视距离时到达要跟随的路径上，且当后两个控制点都在路径点上时，能保证机器人前进速度方向与目标路径切向方向近似，不会出现S型行进或大幅度震荡。

[0195] 参考图4，在跟随路径更新周期开始的时刻，机器人坐标系B相对世界坐标系w具有一个位置偏移量（即两个坐标系原点之间的位置偏差）和一个旋转角度（即两个坐标系的横坐标之间的夹角）；其中机器人坐标系B的原点为差速轮底盘机器人的位置点（也是更新后的跟随路径的起始点），因此该位置偏移量实际上等于差速轮底盘机器人在世界坐标系下的机器人全局坐标；其中机器人坐标系B的横坐标正方向为差速轮底盘机器人的线速度方向，因此该旋转角度等于差速轮底盘机器人的线速度方向与机器人坐标系B的横坐标正方向之间的夹角，该夹角即为在跟随路径更新周期开始的时刻的差速轮底盘机器人在世界坐标系下的偏航角度。

[0196] 其中，当前时刻（此时为跟随路径更新周期开始的时刻）的差速轮底盘机器人的位置点（也是更新后的跟随路径的起始点）即当前时刻差速轮底盘机器人在世界坐标系下的机器人全局坐标对应的位置点，因此，第一个控制点在世界坐标系下的控制点全局坐标等于当前时刻差速轮底盘机器人在世界坐标系下的机器人全局坐标。见图4，此时的第一个控制点 为机器人坐标系B的原点。

[0197] 在本实施例中，控制点选取模块4在根据当前时刻的线速度计算第二个控制点在世界坐标系下的控制点全局坐标的时候，执行：

[0198] 根据以下公式计算第二个控制点在机器人坐标系下的控制点局部坐标：

[0199]  （4）；

[0200]  （5）；

[0201] 其中， 为第二个控制点在机器人坐标系下的控制点局部坐标的横坐标值，为第二个控制点在机器人坐标系下的控制点局部坐标的纵坐标值， 为当前时刻的线速度；

[0202] 通过坐标转换把第二个控制点在机器人坐标系下的控制点局部坐标转换为在世界坐标系下的控制点全局坐标。

[0203] 根据公式（2）有：

[0204] ；

[0205] 其中， 为 时 的一阶导数， 为机器人坐标系下的 时刻的路径位置， 为第二个控制点的在机器人坐标系下的控制点局部坐标， 为 的横坐标值， 为 的纵坐标值， 为第一个控制点的在机器人坐标系下的控制点局部坐标， 为 的横坐标值， 为 的纵坐标值；

[0206] 由于第一个控制点 为机器人坐标系的原点，因此 和 均为零，又由于机器人坐标系的横坐标正方向为差速轮底盘机器人的当前时刻的线速度方向，因此有， 为当前时刻差速轮底盘机器人的线速度，所以上式转换为：

[0207]  （6）；

[0208] 根据该公式（6）即可得到公式（4）和（5）；从而，第二个控制点实际上为当前时刻的机器人坐标系横坐标上的点，且该第二个控制点与机器人坐标系原点的距离等于差速轮底盘机器人当前时刻的线速度大小（如图4所示）。

[0209] 其中，可通过以下公式把第二个控制点在机器人坐标系下的控制点局部坐标转换为在世界坐标系下的控制点全局坐标：

[0210] ；

[0211] 其中， 为第二个控制点在世界坐标系下的控制点全局坐标， 为第一个控制点在世界坐标系下的控制点全局坐标（等于当前时刻差速轮底盘机器人在世界坐标系下的机器人全局坐标）， 为机器人坐标系和世界坐标系之间的转换矩阵，且有：

[0212] ； 为机器人坐标系相对世界坐标系的旋转角度（等于当前时刻差速轮底盘机器人在世界坐标系下的偏航角度）。

[0213] 在本实施例中，控制点选取模块4在根据当前时刻的角速度计算第三个控制点在世界坐标系下的控制点全局坐标的时候，执行：

[0214] 根据以下公式计算第三个控制点在机器人坐标系下的控制点局部坐标：

[0215]  （7）；

[0216]  （8）；

[0217] 其中， 为第三个控制点在机器人坐标系下的控制点局部坐标的横坐标值，为第三个控制点在机器人坐标系下的控制点局部坐标的纵坐标值， 为当前时刻的角速度；

[0218] 通过坐标转换把第三个控制点在机器人坐标系下的控制点局部坐标转换为在世界坐标系下的控制点全局坐标。

[0219] 通过上述方式计算第三个控制点的控制点全局坐标，有利于使后续生成的指令速度接近期望速度，避免指令速度过大。

[0220] 根据公式（2）、（3）、（6）有：

[0221] （9）；

[0222] 其中， 为 时 的二阶导数， 为 的横坐标值， 为的纵坐标值， 为第三个控制点在机器人坐标系下的控制点局部坐标。

[0223] 根据曲线曲率计算公式、角速度计算公式和公式（6）、（9）可以得到：

[0224] （10）；

[0225] 其中， 为 时机器人坐标系下的角速度， 为 的横坐标值， 为 的纵坐标值， 为 的横坐标值， 为 的纵
坐标值。

[0226] 根据该公式（10）可以得到公式（7）。

[0227] 通过试验证明，当采用公式（8）计算第三个控制点在机器人坐标系下的控制点局部坐标的横坐标值时，有利于使后续生成的指令速度接近期望速度。

[0228] 其中，可通过以下公式把第三个控制点在机器人坐标系下的控制点局部坐标转换为在世界坐标系下的控制点全局坐标：

[0229] ；

[0230] 其中， 为第三个控制点在世界坐标系下的控制点全局坐标， 为第三个控制点在机器人坐标系下的控制点局部坐标。图4中， 点为第三个控制点。

[0231] 在本实施例中，控制点选取模块4在根据第三个控制点的控制点全局坐标，确定目标路径的一个路径点作为第四个控制点，获取第四个控制点在世界坐标系下的控制点全局坐标的时候，执行：

[0232] B1.从前到后依次以各路径点为第一备选点，循环执行以下步骤：

[0233] 计算第一备选点与第三个控制点的第一距离 （此处为了便于与其它距离做区分，把第一备选点与第三个控制点的距离称为第一距离）；

[0234] 计算目标路径在第一备选点处的第一曲率 （此处为了便于与其它曲率做区分，把目标路径在第一备选点处的曲率称为第一曲率）；

[0235] 若 ，则使第一参考距离 等于 ，若 ，则使第一参考距离 等于 ；

[0236] 如果 ，则停止循环，并执行步骤B2；

[0237] B2.把最后的第一备选点作为第四个控制点，获取第四个控制点在世界坐标系下的控制点全局坐标。

[0238] 其中，两个点之间的距离和路径在某一路径点上的曲率的计算方法为现有技术，此处不对其进行详述。

[0239] 具体地，先把第一个路径点作为第一备选点，然后计算该第一备选点对应的 和，然后确定 值，若满足 ，则把第一个路径点作为第四个控制点，否则把下一个路径点作为第一备选点，然后重复上述步骤，如此循环，直到 ，假设在把第k个路径点作为第一备选点时满足该条件，则把第k个路径点作为第四个控制点，并提取该路径点在世界坐标系下的路径点位置作为第四个控制点在世界坐标系下的控制点全局坐标。

[0240] 在本实施例中，控制点选取模块4在根据第四个控制点的控制点全局坐标，确定目标路径的一个路径点作为第五个控制点，获取第五个控制点在世界坐标系下的控制点全局坐标的时候，执行：

[0241] C1.从前到后依次以各路径点为第二备选点，循环执行以下步骤：

[0242] 计算第二备选点与第四个控制点的第二距离 （此处为了便于与其它距离做区分，把第二备选点与第四个控制点的距离称为第二距离）；

[0243] 计算目标路径在第二备选点处的第二曲率 （此处为了便于与其它曲率做区分，把目标路径在第二备选点处的曲率称为第二曲率）；

[0244] 若 ，则使第二参考距离 等于 ，若 ，则使第二参考距离 等于 ；

[0245] 如果 ，则停止循环，并执行步骤C2；

[0246] C2.把最后的第二备选点作为第五个控制点，获取第五个控制点在世界坐标系下的控制点全局坐标。

[0247] 具体地，先把第一个路径点作为第二备选点，然后计算该第二备选点对应的 和，然后确定 值，若满足 ，则把第一个路径点作为第五个控制点，否则把下一个路径点作为第二备选点，然后重复上述步骤，如此循环，直到 ，假设在把第k个路径点作为第二备选点时满足该条件，则把第k个路径点作为第五个控制点，并提取该路径点在世界坐标系下的路径点位置作为第五个控制点在世界坐标系下的控制点全局坐标。

[0248] 第一控制模块5根据公式（1）生成跟随路径的显示函数，该显示函数为：

[0249] ；

[0250] 其中， 为世界坐标系下 时刻的路径位置， 、 、 、 、 为五个控制点在世界坐标系下的控制点全局坐标。

[0251] 其中，第一控制模块5生成的控制指令包括线速度指令和角速度指令。

[0252] 在生成当前的跟随路径更新周期内的各个控制周期的控制指令时，根据各控制周期的时间（该时间以当前的跟随路径更新周期开始的时刻为零点）计算归一化时间 ，其中，计算公式为：

[0253] ；

[0254] 然后在各控制周期的归一化时间 处对显示函数进行链式求导和曲线微分计算（具体计算过程为现有技术，此处不对其进行详述），可得到各控制周期的线速度指令和角速度指令。

[0255] 例如，若每个跟随路径更新周期等于n个控制周期，每个控制周期为△T，则生成t=0、t=△T、t=2△T、...、t=（n‑1）△T共n个时刻的控制指令，分别用于在t=0、t=△T、t=2△T、...、t=（n‑1）时刻作为控制输入来控制差速轮底盘机器人运动。从而，由于每次更新跟随路径后只根据更新后的跟随路径进行n个控制指令的生成和使用，即再次进行跟随路径更新，如此循环，当工作过程中受到干扰，也能够有效减小该干扰对跟随效果的影响。

[0256] 实际上，当差速轮底盘机器人移动到接近目标点（即目标路径的终点）的时候容易出现搜索出的第四个控制点和第五个控制点重合或非常接近的情况，此时，若继续依据四阶贝塞尔曲线形的跟随路径生成控制指令以控制差速轮底盘机器人运动，会导致差速轮底盘机器人绕圈接近目标点，从而延迟到达目标点的时间。为了避免差速轮底盘机器人最后绕圈接近目标点，在一些优选实施方式中，该差速轮底盘机器人自适应路径跟随装置还包括：

[0257] 第二计算模块，用于计算第四个控制点和第五个控制点之间的控制点距离；

[0258] 第一执行模块，用于在该控制点距离不小于预设的距离阈值（可根据实际需要设置）时，令第一控制模块执行其功能；

[0259] 第二执行模块，用于在该控制点距离小于预设的距离阈值时，令差速轮底盘机器人原地旋转至正前方对准目标点后控制差速轮底盘机器人沿直线移动至该目标点。

[0260] 由上可知，该差速轮底盘机器人自适应路径跟随装置，通过获取目标路径的路径点在世界坐标系下的路径点位置，若当前时刻为跟随路径更新周期开始的时刻，则获取当前时刻差速轮底盘机器人在世界坐标系下的机器人全局坐标、偏航角度、线速度和角速度，根据预设的期望线速度和预设的期望前视距离，计算跟随路径的总时间，根据路径点位置、总时间以及当前时刻的机器人全局坐标、偏航角度、线速度和角速度，确定四阶贝塞尔曲线形的跟随路径的五个控制点在世界坐标系下的控制点全局坐标，根据五个控制点全局坐标生成跟随路径的显示函数，并根据显示函数生成在当前的跟随路径更新周期内的每个控制周期的控制指令，以控制差速轮底盘机器人运动；从而能够提高路径跟随精度、减小位置震荡和更充分地发挥差速轮底盘的转弯性能优势。

[0261] 请参照图3，图3为本申请实施例提供的一种电子设备的结构示意图，本申请提供一种电子设备，包括：处理器301和存储器302，处理器301和存储器302通过通信总线303和/或其他形式的连接机构（未标出）互连并相互通讯，存储器302存储有处理器301可执行的计算机程序，当电子设备运行时，处理器301执行该计算机程序，以执行上述实施例的任一可选的实现方式中的差速轮底盘机器人自适应路径跟随方法，以实现以下功能：获取目标路径的路径点在世界坐标系下的路径点位置，若当前时刻为跟随路径更新周期开始的时刻，则获取当前时刻差速轮底盘机器人在世界坐标系下的机器人全局坐标、偏航角度、线速度和角速度，根据预设的期望线速度和预设的期望前视距离，计算跟随路径的总时间，根据路径点位置、总时间以及当前时刻的机器人全局坐标、偏航角度、线速度和角速度，确定四阶贝塞尔曲线形的跟随路径的五个控制点在世界坐标系下的控制点全局坐标，根据五个控制点全局坐标生成跟随路径的显示函数，并根据显示函数生成在当前的跟随路径更新周期内的每个控制周期的控制指令，以控制差速轮底盘机器人运动。

[0262] 本申请实施例提供一种计算机可读存储介质，其上存储有计算机程序，计算机程序被处理器执行时，执行上述实施例的任一可选的实现方式中的差速轮底盘机器人自适应路径跟随方法，以实现以下功能：获取目标路径的路径点在世界坐标系下的路径点位置，若当前时刻为跟随路径更新周期开始的时刻，则获取当前时刻差速轮底盘机器人在世界坐标系下的机器人全局坐标、偏航角度、线速度和角速度，根据预设的期望线速度和预设的期望前视距离，计算跟随路径的总时间，根据路径点位置、总时间以及当前时刻的机器人全局坐标、偏航角度、线速度和角速度，确定四阶贝塞尔曲线形的跟随路径的五个控制点在世界坐标系下的控制点全局坐标，根据五个控制点全局坐标生成跟随路径的显示函数，并根据显示函数生成在当前的跟随路径更新周期内的每个控制周期的控制指令，以控制差速轮底盘机器人运动。其中，计算机可读存储介质可以由任何类型的易失性或非易失性存储设备或者它们的组合实现，如静态随机存取存储器（Static Random Access Memory, 简称SRAM），电可擦除可编程只读存储器（Electrically Erasable Programmable Read‑Only Memory, 简称EEPROM），可擦除可编程只读存储器（Erasable Programmable Read Only Memory, 简称EPROM），可编程只读存储器（Programmable Red‑Only Memory, 简称PROM），只读存储器（Read‑Only Memory, 简称ROM），磁存储器，快闪存储器，磁盘或光盘。

[0263] 在本申请所提供的实施例中，应该理解到，所揭露装置和方法，可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，又例如，多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些通信接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

[0264] 另外，作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，既可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

[0265] 再者，在本申请各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。

[0266] 在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。

[0267] 以上所述仅为本申请的实施例而已，并不用于限制本申请的保护范围，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。