CN113534789A - 移动机器人三次多项式速度曲线实时规划方法及装置 - Google Patents

Abstract

本发明公开了一种移动机器人三次多项式速度曲线实时规划方法及装置，所述方法包括：在每一个控制周期，获取移动机器人当前的速度v_t、上一个控制周期的加速度a_t‑1、以及当前距离终点的距离s_t，根据v_t、a_t‑1、s_t、以及预设的加加速度J，进行移动机器人的动作策略判断；根据动作策略判断结果进行移动机器人速度曲线的实时规划。本发明能够为移动机器人实时规划最合适的行走速度，使其运行轨迹满足要求。

Description

移动机器人三次多项式速度曲线实时规划方法及装置

技术领域

本发明涉及物流机器人运动控制技术领域，尤其是涉及一种移动机器人三次多项式速度曲线实时规划方法及装置。

背景技术

在现有技术中，仓储物流自动化应用场景中，自动导引运输车(AGV)或自主移动机器人(AMR)被广泛地用于货物搬运、拣选、传输、分类等。相比传统的人工操作，移动机器人自动化运作大幅提升了物流效率、降低了人工成本。AGV或AMR在工作时，其运载货物的质量、重心往往是变化不定的，为了使移动机器人能够平稳地起步和停止、运输过程中更快速地行走、并适应不同的货物搭载情形，通常为移动机器人的行走路径规划一个适宜的速度S曲线。常用的S曲线有梯形速度S曲线、多项式S曲线、三角函数加加速度S曲线。

在现有技术中，提供了一种物流搬运设备S曲线加减速度规划与控制方法，公开了一种三次多项式S曲线的规划方法和实施过程中移动机器人的运行误差控制方法。该发明专利在移动机器人每次运行前，一次性规划出行走路径的S曲线，以移动机器人的当前位移作为判断依据并对比规划好的S曲线，计算得出应该下发的行走速度，同时结合理论计算的当前位移和实际位移，对速度进行PID控制调整，最终得到实际下发给移动机器人的运行速度。

但是，上述技术方案存在以下技术问题：

1、速度曲线规划所需的计算时间长，尤其是4段S曲线无法解析求解，需要使用二分查找的方式搜索近似解；

2、速度曲线是提前规划好的，不支持在行走过程中任意改变目的地并实时规划速度曲线；

3、固定轨迹的速度曲线规划，不支持在行进过程中实时调整速度并跟随前方的移动障碍物；

4、使用位移判断当前的运行速度，有可能在位移误差较大时计算得到的速度变化较大，导致移动机器人运行不稳定。

发明内容

本发明的目的在于提供一种移动机器人三次多项式速度曲线实时规划方法及装置，旨在解决现有技术中的上述问题。

本发明提供一种移动机器人三次多项式速度曲线实时规划方法，包括：

在每一个控制周期，获取移动机器人当前的速度v_t、上一个控制周期的加速度a_t-1、以及当前距离终点的距离s_t，根据v_t、a_t-1、s_t、以及预设的加加速度J，进行移动机器人的动作策略判断；

根据动作策略判断结果进行移动机器人速度曲线的实时规划。

本发明提供一种移动机器人三次多项式速度曲线实时规划装置，包括：

动作策略判断模块，用于在每一个控制周期，获取移动机器人当前的速度v_t、上一个控制周期的加速度a_t-1、以及当前距离终点的距离s_t，根据v_t、a_t-1、s_t、以及预设的加加速度J，进行移动机器人的动作策略判断；

实时规划模块，用于根据动作策略判断结果进行移动机器人速度曲线的实时规划。

本发明实施例还提供一种移动机器人三次多项式速度曲线实时规划装置，包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时实现上述移动机器人三次多项式速度曲线实时规划方法的步骤。

本发明实施例还提供一种计算机可读存储介质，所述计算机可读存储介质上存储有信息传递的实现程序，所述程序被处理器执行时实现上述移动机器人三次多项式速度曲线实时规划方法的步骤。

采用本发明实施例，能够为移动机器人实时规划最合适的行走速度，使其运行轨迹满足下列要求：

1、在起步阶段有平滑的加速阶段，在停止阶段有平滑的减速阶段，使移动机器人在不同载重情形下能够稳定平稳地变速；

2、在最短的时间内完成加速或减速阶段，尽量缩短移动机器人的速度调整时间，增大其平均行走速度；

3、在行走过程中改变目的地或遇到障碍物，能够快速实时地重新规划速度曲线并平滑地提速运行、限速跟随或尽快停止。

上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，而可依照说明书的内容予以实施，并且为了让本发明的上述和其它目的、特征和优点能够更明显易懂，以下特举本发明的具体实施方式。

附图说明

为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例的移动机器人三次多项式速度曲线实时规划方法的流程图；

图2是本发明实施例的第1种情形的示意图；

图3是本发明实施例的第二种情形的示意图；

图4是本发明实施例的七段S形曲线的v和a曲线的示意图。

图5是本发明实施例的移动机器人三次多项式速度曲线实时规划装置的示意图一；

图6是本发明实施例的移动机器人三次多项式速度曲线实时规划装置的示意图二。

具体实施方式

下面将结合实施例对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“长度”、“宽度”、“厚度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”、“顺时针”、“逆时针”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个所述特征。在本发明的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。此外，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。

方法实施例

根据本发明实施例，提供了一种移动机器人三次多项式速度曲线实时规划方法，图1是本发明实施例的移动机器人三次多项式速度曲线实时规划方法的流程图，如图1所示，根据本发明实施例的移动机器人三次多项式速度曲线实时规划方法具体包括：

步骤101，在每一个控制周期，获取移动机器人当前的速度v_t、上一个控制周期的加速度a_t-1、以及当前距离终点的距离s_t，根据v_t、a_t-1、s_t、以及预设的加加速度J，进行移动机器人的动作策略判断；

在步骤101中，根据v_t、a_t-1、s_t、以及预设的加加速度J，进行移动机器人的动作策略判断具体包括：

步骤1，根据公式1计算当前加速度a_t：

a_t＝a_t-1+JΔt 公式1；

其中，Δt为时间差；

步骤2，判断a_t≥a_max是否成立，如果成立，则确认当前动作策略失败，进行下一个动作策略判断，执行步骤3；如果不成立，则根据v_t和a_t计算最短行驶距离s_min，如果s_min<s_t，则接受加速度a_t，根据公式2计算下一个控制周期应该设定的速度v_t+1，否则，拒绝加速度a_t，继续进行下一个动作策略判断，执行步骤3，其中，a_max为预先设置的最大加速度阈值；

v_t+1＝v_t+a_tΔt+JΔt²/2 公式2；

步骤3，根据公式3计算此刻加速度，并根据v_t和a_t计算最短行驶距离s_min，如果s_min<s_t，则接受加速度a_t，根据公式4计算下一个控制周期应该设定的速度v_t+1，否则，拒绝加速度a_t，继续进行下一个动作策略判断，执行步骤4，

a_t＝a_t-1 公式3；

v_t+1＝v_t+a_tΔt 公式4；

步骤4，根据公式5计算此刻加速度a_t，并根据公式6计算下一个控制周期的速度：

a_t＝max{a_t-1-JΔt,-a_max} 公式5；

v_t+1＝v_t+a_tΔt-JΔt²/2 公式6。

在上述处理中，根据v_t和a_t计算最短行驶距离s_min具体包括：

情况1：当a_t>0时，要达到最短距离，向移动机器人先加一个负向的J，让加速度先变成0，然后从达到的最高速v_u开始减速，最后是一段正向J的阶段，使得加速度和速度都将为0，根据J将上述过程分为4个阶段，其中，第一阶段为当前速度v_t达到最高速v_u，且J<0，第二阶段为从最高速v_u减速到v_t，且J<0，第三阶段为v_t减速到v_std，且J＝0，第四阶段为由v_std减速到0，且J>0；根据公式7计算第一阶段的耗时，根据公式8计算达到的最大速度v_u，根据公式9计算第一阶段走过的距离：

t_I＝a_t/J 公式7；

v_u＝v_t+a_tt_I-Jt_I ²/2＝v_t+a_t ²/(2J) 公式8；

s_I＝v_ta_t/J+a_t ³/(3J²) 公式9；

定义一个速度v_std为移动机器人从速度0开始用正向J一直到加速到加速度等于最大值的时速度的取值，要加到最大加速度的时间为a_max/J,根据公式10计算出v_std：

v_std＝a_max ²/(2J) 公式10；

判断v_u>2v_std是否成立，则成立则存在第三阶段，否则不存在第三阶段；

在存在第三阶段的情况下即v_u>2v_std，根据公式11计算第二阶段行走的距离，根据公式12计算第三阶段行走的距离，根据公式13计算第四阶段行走的距离，并根据公式14计算总的最短行走距离s_min：

s_II＝(v_u-v_std)a_max/J+a_max ³/(3J²) 公式11；

s_III＝v_u(v_u-2v_std)/(2a_max) 公式12；

s_IV＝a_max ³/(6J²) 公式13；

s_min＝s_I+s_II+s_III+s_IV 公式14；

在不存在第三阶段的情况下即v_u≤2v_std，有一半的时间在第二阶段，一半的时间在第四阶段，在中点切换的时候的速度为v_u/2，则根据公式15计算第二阶段行走的距离，根据公式16计算第四阶段行走的距离，并根据公

式17计算总的最短行走距离s_min：

_smin＝s_I+s_II+s_IV 公式17；

情况2：当a_t≤0时，设置虚拟阶段，其中，虚拟阶段为加速度为0速度为v_u的时刻到v_t的时刻，根据公式18计算虚拟阶段的虚拟时间，根据公式19计算对应的vu，根据公式20计算虚拟阶段的行走距离：

t_I′＝|a_t|/J 公式18；

v_u＝v_t+a_t ²/(2J) 公式19；

s_I′＝v_t|a_t|/J+|a_t|³/(3J²) 公式20；

根据情况1的v_u之后的第二阶段、第三阶段、第四阶段行走距离的计算结果来计算从v_u这个点到停止所需要行走的距离，将该距离减去sI′得到情况2下的最短行走距离s_min。

步骤102，根据动作策略判断结果进行移动机器人速度曲线的实时规划。在步骤102中，将计算得到的v_t+1进行换算和纠偏处理后下发给左右轮的电机，完成移动机器人速度曲线的实时规划。

以下结合附图，对本发明实施例的上述技术方案进行详细说明。

在本发明实施例中，需要在每一个控制周期做如下计算：输入为当前的速度v_t，上一个控制周期的加速度a_t-1，当前距离终点的距离s_t，预设的加加速度J，然后依次做如下的动作策略判断：

1.计算此刻加速度为a_t＝a_t-1+JΔt(如果a_t≥a_max，直接认定该动作策略失败，进行下一个动作策略判断)，然后以v_t，a_t为输入算出最短行驶距离smin，如果此距离小于s_t，就接受这个加速度a_t，反之拒绝，继续做下一个动作策略判断。

根据上面算出的a_t，计算下一个控制周期应该设定的速度：

v_t+1＝v_t+a_tΔt+JΔ_t ²/2

2.计算此刻加速度为a_t＝a_t-1,然后以v_t，a_t为输入算出最短行驶距离s_min，如果此距离小于s_t，就接受这个a_t，反之拒绝，继续做下一个动作策略判断。

根据上面算出的a_t，计算下一个控制周期的速度：

v_t+1＝v_t+a_tΔt

3.上面两个动作策略判断都失败了，直接计算a_t＝max{a_t-1-JΔt,-a_max}。

根据上面算出的a_t，计算下一个控制周期的速度：

v_t+1＝v_t+atΔt-JΔt²/2

此速度经过换算和纠偏处理后再下发给左右轮的电机。

比较关键的计算，就是在给定的输入下，计算出移动机器人以最快时间停止的最短距离s_min。具体来说，假设知道v_t，a_t，需要计算出在这样的初始条件下移动机器人能停下来的最短距离s_min。

第1种情形：a_t>0

先考虑a_t>0的情况。在这种情况下，要达到最短距离，移动机器人需要先加一个负向的J，让加速度先变成0，然后从达到的最高速v_u开始减速(还是负向J)，有可能会有一段匀减速阶段，最后是一段正向J的阶段，使得加速度和速度都将为0。整个过程如图2所示。

本发明实施例把这个过程分成几个Stage。Stage I和II是J<0的情况，Stage III是J＝0的情况，Stage IV是J>0的情况。有可能Stage III不会出现。

先计算加速度为0时，达到的最大速度v_u。Stage I耗费的时间为：

t_I＝a_t/J

因此：

v_u＝v_t+a_tt_I-Jt_I ²/2＝v_t+a_t ²/(2J)

Stage I走过的距离为：

s_I＝v_ta_t/J+a_t ³/(3J²)

到达了vu后，需要分两种情况考虑。如果vu比较大，可能会有一段匀减速的阶段，反之则没有。这个速度的临界值跟最大加速度a_max有关。定义一个速度v_std为移动机器人从速度0开始用正向J一直到加速到加速度等于最大值的时，速度的取值。要加到最大加速度的时间为a_max/J,所以我们可以计算出v_std为：

v_std＝a_max ²/(2J)

根据对称性，如果v_u>2v_std，则存在Stage III，反之则不存在。

情形1.1：v_u>2v_std

如果存在Stage III，可以计算出Stage II行走的距离为：

s_II＝(v_u-v_std)a_max/J+a_max ³/(3J²)

Stage III的行走距离为：

s_III＝v_u(v_u-2v_std)/(2a_max)

Stage IV的行走距离为：

s_IV＝a_max ³/(6J₂)

这样就可以计算出总的最短行走距离:s_min＝s_I+s_II+s_III+s_IV。

情形1.2：v_u≤2v_std

如果v_u≤2v_std，在这种情况下就不存在Stage III，在这种情况下我们只需要Stage II和Stage IV的距离。根据对称性，我们会有一半的时间在Stage II，一半的时间在Stage IV，在中点切换的时候的速度为v_u/2。

Stage II的行走距离为：

Stage IV的行走距离为：

在这种情况下可以算出最短行走距离：s_min＝s_I+s_II+s_IV。

第二种情形：at≤0

考虑加速度at≤0的情况。在这种情况下，移动机器人已经处于减速阶段，最简单的计算最短行走距离的方法，是时间回溯到加速度＝0的时刻，算出那个时刻的v_u，然后用第一种情形里关于vu之后的Stage II,III,IV的行走距离计算来推导出最短行走距离。如图3所示：加速度已经是负的阶段，我们回溯到加速度为0，速度为v_u的时刻。把这个阶段叫做Stage I'，这是一个虚拟的Stage。

这个虚拟Stage的时间为

t_I′＝|a_t|/J

因此我们可以计算出对应的v_u：

v_u＝v_t+a_t ²/(2J)

所以Stage I'的行走距离为：

s_I′＝v_t|a_t|/J+|a^t|³/(3J²)

在找到vu以后，利用第一种情形的结果来计算从vu这个点到停止所需要行走的距离，然后减去sI′得到第二种情形下的最短行走距离。

从上述描述可以看出，在本发明实施例中：

为了让移动机器人行走更稳定，不产生抖动，用一个S形速度曲线来规划移动机器人的前进速度。在S形曲线的控制下，移动机器人的加速度不会有跳变，所以行走可以更稳定。

假设移动机器人的加速度不能突变，定义一个加加速度(Jerk：J)。在Jerk的作用下，加速度会随着时间增加/减少:

a_t＝a_t-1+JΔt

S型速度曲线的假设就是，只有三种不同的动作策略：正的J、负的J、J＝0。在加速的阶段，这就分别对应于加加速、减加速、和匀加速。

在一个S形曲线的假设下，如果目的地距离足够长，可以把整个速度曲线分解成7段：加加速、匀加速、减加速、匀速、加减速、匀减速、减减速。如果距离不是足够长，有可能会没有中间的匀速阶段，甚至(在很短距离的情况下)会出现连匀加速和匀减速阶段都没有的情况。

一个七段S形曲线的v和a曲线如图4所示：在S形速度曲线的假设下，在每一个控制周期其实只有三个动作策略：正向的J、负向的J、J＝0。不考虑J随时间缓慢变化的情况。在每一个控制周期，输入有：当前距离目的地的距离(s_t)，当前的速度(v_t)，上一个控制周期的加速度(a_t-1)。需要根据这些输入算出这一个周期的加速度(a_t)和下一个时刻的速度(v_t+1)，并且把这个新的速度(经过纠偏处理以后)下发给电机。只需要在给定的输入的情况下，判断到底应该用正向的J、负向的J还是J＝0这三个动作策略，就可以计算出对应的所需的输出。

在每一个控制周期，只需要最多做三次动作策略判断：

1.判断是否能用正向的J，如果使用正向的J，然后后面用行进最短路程的方式能够在终点之前停下，说明还可以使用正向的J，那么就直接用正向的J计算输出。

2.如果1判断失败了，那么判断一下是否能用J＝0这一个动作策略，也就是说保持上一轮的加速度。如果做了这个选择后，后面能在终点前停下来，就用J＝0。

3.如果上面两个动作策略都失败了，就只能用负向的J。也判断一下是否能到终点，如果超过终点，需要做一些特殊处理。

当然，在一些区域如果已经到了最大加速度，那么就不需要判断1。在移动机器人已经达到了最高速的阶段，也不需要判断1。任何情况下，正向的J都是能让移动机器人走更远的，其次是J＝0，再其次是负向的J。所以只要正向J的动作策略后移动机器人能在终点前停下来，这个动作策略就是可行的，而且是能让移动机器人在最短时间到达终点的动作策略。

从上述技术方案可以看出，本发明实施例无需规划整条速度S曲线，仅仅关注当前时刻的速度和加速度变化，将轨迹规划问题转化为简单的动作决策问题；此外，本发明实施例支持运动过程中任意位移、速度、加速度条件下目的地改变后的实时速度规划，保证运行轨迹满足三次多项式速度曲线的约束。

综上所述，采用于本发明实施例的技术方案具有以下有益效果：

1、将速度曲线规划问题转化为加速度变化的决策问题，计算时间缩短；

2、每次都是在任意速度、加速度、位移的条件下重新实时规划，消除累计误差对当前速度规划的影响，无需前进方向的PID控制就能准确达到终点；

3、能够在运动过程中改变移动机器人的目的地，并且无缝平滑地衔接当前运动状态进行速度的重新规划。

装置实施例一

根据本发明实施例，提供了一种移动机器人三次多项式速度曲线实时规划装置，图5是本发明实施例的移动机器人三次多项式速度曲线实时规划装置的示意图，如图5所示，根据本发明实施例的移动机器人三次多项式速度曲线实时规划装置具体包括：

动作策略判断模块50，用于在每一个控制周期，获取移动机器人当前的速度v_t、上一个控制周期的加速度a_t-1、以及当前距离终点的距离s_t，根据v_t、a_t-1、s_t、以及预设的加加速度J，进行移动机器人的动作策略判断；

动作策略判断模块50具体包括：

第一计算子模块，根据公式1计算当前加速度a_t：

a_t＝a_t-1+JΔt 公式1；

其中，Δt为时间差；

第二计算子模块，判断a_t≥a_max是否成立，如果成立，则确认当前动作策略失败，进行下一个动作策略判断，调用第三计算子模块；如果不成立，则根据v_t和a_t计算最短行驶距离s_min，如果s_min<s_t，则接受加速度a_t，根据公式2计算下一个控制周期应该设定的速度v_t+1，否则，拒绝加速度a_t，继续进行下一个动作策略判断，调用第三计算子模块，其中，a_max为预先设置的最大加速度阈值；

v_t+1＝v_t+a_tΔt+JΔt²/2 公式2；

第三计算子模块，根据公式3计算此刻加速度，并根据v_t和a_t计算最短行驶距离s_min，如果s_min<s_t，则接受加速度a_t，根据公式4计算下一个控制周期应该设定的速度v_t+1，否则，拒绝加速度a_t，继续进行下一个动作策略判断，调用第四计算子模块，

a_t＝a_t-1 公式3；

v_t+1＝v_t+a_tΔt 公式4；

第四计算子模块，根据公式5计算此刻加速度a_t，并根据公式6计算下一个控制周期的速度：

a_t＝max{a_t-1-JΔt,-a_max} 公式5；

v_t+1＝v_t+a_tΔt-JΔt²/2 公式6。

所述第二计算子模块、第三计算子模块具体用于：

情况1：当a_t>0时，要达到最短距离，向移动机器人先加一个负向的J，让加速度先变成0，然后从达到的最高速v_u开始减速，最后是一段正向J的阶段，使得加速度和速度都将为0，根据J将上述过程分为4个阶段，其中，第一阶段为当前速度v_t达到最高速v_u，且J<0，第二阶段为从最高速v_u减速到v_t，且J<0，第三阶段为v_t减速到v_std，且J＝0，第四阶段为由v_std减速到0，且J>0；根据公式7计算第一阶段的耗时，根据公式8计算达到的最大速度v_u，根据公式9计算第一阶段走过的距离：

t_I＝a_t/J 公式7；

v_u＝v_t+a_tt_I-Jt_I ²/2＝v_t+a_t ²/(2J) 公式8；

s_I＝v_ta_t/J+a_t ³/(3J²) 公式9；

定义一个速度v_std为移动机器人从速度0开始用正向J一直到加速到加速度等于最大值的时速度的取值，要加到最大加速度的时间为a_max/J,根据公式10计算出v_std：

v_std＝a_max ²/(2J) 公式10；

判断v_u>2v_std是否成立，则成立则存在第三阶段，否则不存在第三阶段；

在存在第三阶段的情况下即v_u>2v_std，根据公式11计算第二阶段行走的距离，根据公式12计算第三阶段行走的距离，根据公式13计算第四阶段行走的距离，并根据公式14计算总的最短行走距离s_min：

s_II＝(v_u-v_std)a_max/J+a_max ³/(3J²) 公式11；

s_III＝v_u(v_u-2v_std)/(2a_max) 公式12；

s_IV＝a_max ³/(6J²) 公式13；

s_min＝s_I+s_II+s_III+s_IV 公式14；

在不存在第三阶段的情况下即v_u≤2v_std，有一半的时间在第二阶段，一半的时间在第四阶段，在中点切换的时候的速度为v_u/2，则根据公式15计算第二阶段行走的距离，根据公式16计算第四阶段行走的距离，并根据公

式17计算总的最短行走距离s_min：

_smin＝s_I+s_II+s_IV 公式17；

情况2：当a_t≤0时，设置虚拟阶段，其中，虚拟阶段为加速度为0速度为v_u的时刻到v_t的时刻，根据公式18计算虚拟阶段的虚拟时间，根据公式19计算对应的vu，根据公式20计算虚拟阶段的行走距离：

t_I′＝|a_t|/J 公式18；

v_u＝v_t+a_t ²/(2J) 公式19；

s_I′＝v_t|a_t|/J+|a_t|³/(3J²) 公式20；

根据情况1的v_u之后的第二阶段、第三阶段、第四阶段行走距离的计算结果来计算从v_u这个点到停止所需要行走的距离，将该距离减去sI′得到情况2下的最短行走距离s_min。

实时规划模块52，用于根据动作策略判断结果进行移动机器人速度曲线的实时规划。

实时规划模块52具体用于：

将计算得到的v_t+1进行换算和纠偏处理后下发给左右轮的电机，完成移动机器人速度曲线的实时规划。

本发明实施例是与上述方法实施例对应的装置实施例，各个模块的具体操作可以参照方法实施例的描述进行理解，在此不再赘述。

装置实施例二

本发明实施例提供一种移动机器人三次多项式速度曲线实时规划装置，如图6所示，包括：存储器60、处理器62及存储在所述存储器60上并可在所述处理62上运行的计算机程序，所述计算机程序被所述处理器62执行时实现如方法实施例中所述的步骤。

装置实施例三

本发明实施例提供一种计算机可读存储介质，所述计算机可读存储介质上存储有信息传输的实现程序，所述程序被处理器62执行时实现如方法实施例中所述的步骤。

本实施例所述计算机可读存储介质包括但不限于为：ROM、RAM、磁盘或光盘等。

上述对本说明书特定实施例进行了描述。其它实施例在所附权利要求书的范围内。在一些情况下，在权利要求书中记载的动作或步骤可以按照不同于实施例中的顺序来执行并且仍然可以实现期望的结果。另外，在附图中描绘的过程不一定要求示出的特定顺序或者连续顺序才能实现期望的结果。在某些实施方式中，多任务处理和并行处理也是可以的或者可能是有利的。

在20世纪30年代，对于一个技术的改进可以很明显地区分是硬件上的改进(例如，对二极管、晶体管、开关等电路结构的改进)还是软件上的改进(对于方法流程的改进)。然而，随着技术的发展，当今的很多方法流程的改进已经可以视为硬件电路结构的直接改进。设计人员几乎都通过将改进的方法流程编程到硬件电路中来得到相应的硬件电路结构。因此，不能说一个方法流程的改进就不能用硬件实体模块来实现。例如，可编程逻辑器件(Programmable Logic Device，PLD)(例如现场可编程门阵列(Field Programmable GateArray，FPGA))就是这样一种集成电路，其逻辑功能由用户对器件编程来确定。由设计人员自行编程来把一个数字系统“集成”在一片PLD上，而不需要请芯片制造厂商来设计和制作专用的集成电路芯片。而且，如今，取代手工地制作集成电路芯片，这种编程也多半改用“逻辑编译器(logic compiler)”软件来实现，它与程序开发撰写时所用的软件编译器相类似，而要编译之前的原始代码也得用特定的编程语言来撰写，此称之为硬件描述语言(Hardware Description Language，HDL)，而HDL也并非仅有一种，而是有许多种，如ABEL(Advanced Boolean Expression Language)、AHDL(Altera Hardware DescriptionLanguage)、Confluence、CUPL(Cornell University Programming Language)、HDCal、JHDL(Java Hardware Description Language)、Lava、Lola、MyHDL、PALASM、RHDL(RubyHardware Description Language)等，目前最普遍使用的是VHDL(Very-High-SpeedIntegrated Circuit Hardware Description Language)与Verilog。本领域技术人员也应该清楚，只需要将方法流程用上述几种硬件描述语言稍作逻辑编程并编程到集成电路中，就可以很容易得到实现该逻辑方法流程的硬件电路。

控制器可以按任何适当的方式实现，例如，控制器可以采取例如微处理器或处理器以及存储可由该(微)处理器执行的计算机可读程序代码(例如软件或固件)的计算机可读介质、逻辑门、开关、专用集成电路(Application Specific Integrated Circuit，ASIC)、可编程逻辑控制器和嵌入微控制器的形式，控制器的例子包括但不限于以下微控制器：ARC 625D、Atmel AT91SAM、Microchip PIC18F26K20以及Silicone Labs C8051F320，存储器控制器还可以被实现为存储器的控制逻辑的一部分。本领域技术人员也知道，除了以纯计算机可读程序代码方式实现控制器以外，完全可以通过将方法步骤进行逻辑编程来使得控制器以逻辑门、开关、专用集成电路、可编程逻辑控制器和嵌入微控制器等的形式来实现相同功能。因此这种控制器可以被认为是一种硬件部件，而对其内包括的用于实现各种功能的装置也可以视为硬件部件内的结构。或者甚至，可以将用于实现各种功能的装置视为既可以是实现方法的软件模块又可以是硬件部件内的结构。

上述实施例阐明的系统、装置、模块或单元，具体可以由计算机芯片或实体实现，或者由具有某种功能的产品来实现。一种典型的实现设备为计算机。具体的，计算机例如可以为个人计算机、膝上型计算机、蜂窝电话、相机电话、智能电话、个人数字助理、媒体播放器、导航设备、电子邮件设备、游戏控制台、平板计算机、可穿戴设备或者这些设备中的任何设备的组合。

为了描述的方便，描述以上装置时以功能分为各种单元分别描述。当然，在实施本说明书实施例时可以把各单元的功能在同一个或多个软件和/或硬件中实现。

本领域内的技术人员应明白，本说明书一个或多个实施例可提供为方法、系统或计算机程序产品。因此，本说明书一个或多个实施例可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本说明书可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本说明书是参照根据本说明书实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

在一个典型的配置中，计算设备包括一个或多个处理器(CPU)、输入/输出接口、网络接口和内存。

内存可能包括计算机可读介质中的非永久性存储器，随机存取存储器(RAM)和/或非易失性内存等形式，如只读存储器(ROM)或闪存(flash RAM)。内存是计算机可读介质的示例。

计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(PRAM)、静态随机存取存储器(SRAM)、动态随机存取存储器(DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD-ROM)、数字多功能光盘(DVD)或其他光学存储、磁盒式磁带，磁带磁磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括暂存电脑可读媒体(transitory media)，如调制的数据信号和载波。

还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、商品或者设备中还存在另外的相同要素。

本说明书一个或多个实施例可以在由计算机执行的计算机可执行指令的一般上下文中描述，例如程序模块。一般地，程序模块包括执行特定任务或实现特定抽象数据类型的例程、程序、对象、组件、数据结构等等。也可以在分布式计算环境中实践本说明书的一个或多个实施例，在这些分布式计算环境中，由通过通信网络而被连接的远程处理设备来执行任务。在分布式计算环境中，程序模块可以位于包括存储设备在内的本地和远程计算机存储介质中。

本说明书中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于系统实施例而言，由于其基本相似于方法实施例，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。

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

Claims (10)

1.一种移动机器人三次多项式速度曲线实时规划方法，其特征在于，包括：

在每一个控制周期，获取移动机器人当前的速度v_t、上一个控制周期的加速度a_t-1、以及当前距离终点的距离s_t，根据v_t、a_t-1、s_t、以及预设的加加速度J，进行移动机器人的动作策略判断；

根据动作策略判断结果进行移动机器人速度曲线的实时规划。

2.根据权利要求1所述的方法，其特征在于，根据v_t、a_t-1、s_t、以及预设的加加速度J，进行移动机器人的动作策略判断具体包括：

步骤1，根据公式1计算当前加速度a_t：

a_t＝a_t-1+JΔt 公式1；

其中，Δt为时间差；

步骤2，判断a_t≥a_max是否成立，如果成立，则确认当前动作策略失败，进行下一个动作策略判断，执行步骤3；如果不成立，则根据v_t和a_t计算最短行驶距离s_min，如果s_min<s_t，则接受加速度a_t，根据公式2计算下一个控制周期应该设定的速度v_t+1，否则，拒绝加速度a_t，继续进行下一个动作策略判断，执行步骤3，其中，a_max为预先设置的最大加速度阈值；

v_t+1＝v_t+a_tΔt+JΔt²/2 公式2；

步骤3，根据公式3计算此刻加速度，并根据v_t和a_t计算最短行驶距离s_min，如果s_min<s_t，则接受加速度a_t，根据公式4计算下一个控制周期应该设定的速度v_t+1，否则，拒绝加速度a_t，继续进行下一个动作策略判断，执行步骤4，

a_t＝a_t-1 公式3；

v_t+1＝v_t+a_tΔt 公式4；

步骤4，根据公式5计算此刻加速度a_t，并根据公式6计算下一个控制周期的速度：

a_t＝max{a_t-1-JΔt,-a_max} 公式5；

v_t+1＝v_t+a_tΔt-JΔt²/2 公式6。

3.根据权利要求2所述的方法，其特征在于，根据动作策略判断结果进行移动机器人速度曲线的实时规划具体包括：

将计算得到的v_t+1进行换算和纠偏处理后下发给左右轮的电机，完成移动机器人速度曲线的实时规划。

4.根据权利要求2所述的方法，其特征在于，根据v_t和a_t计算最短行驶距离s_min具体包括：

情况1：当a_t>0时，要达到最短距离，向移动机器人先加一个负向的J，让加速度先变成0，然后从达到的最高速v_u开始减速，最后是一段正向J的阶段，使得加速度和速度都降为0，根据J将上述过程分为4个阶段，其中，第一阶段为当前速度v_t达到最高速v_u，且J<0，第二阶段为从最高速v_u减速到v_t，且J<0，第三阶段为v_t减速到v_std，且J＝0，第四阶段为由v_std减速到0，且J>0；根据公式7计算第一阶段的耗时，根据公式8计算达到的最大速度v_u，根据公式9计算第一阶段走过的距离：

t_I＝a_t/J 公式7；

v_u＝v_t+a_tt_I-Jt_I ²/2＝v_t+a_t ²/(2J) 公式8；

s_I＝v_ta_t/J+a_t ³/(3J²) 公式9；

定义一个速度v_std为移动机器人从速度0开始用正向J一直到加速到加速度等于最大值的时速度的取值，要加到最大加速度的时间为a_max/J,根据公式10计算出v_std：

v_std＝a_max ²/(2J) 公式10；

判断v_u>2v_std是否成立，则成立则存在第三阶段，否则不存在第三阶段；

在存在第三阶段的情况下即v_u>2v_std，根据公式11计算第二阶段行走的距离，根据公式12计算第三阶段行走的距离，根据公式13计算第四阶段行走的距离，并根据公式14计算总的最短行走距离s_min：

s_II＝(v_u-v_std)a_max/J+a_max ³/(3J²) 公式11；

s_III＝v_u(v_u-2v_std)/(2a_max) 公式12；

s_IV＝a_max ³/(6J²) 公式13；

s_min＝s_I+s_II+s_III+s_IV 公式14；

在不存在第三阶段的情况下即v_u≤2v_std，有一半的时间在第二阶段，一半的时间在第四阶段，在中点切换的时候的速度为v_u/2，则根据公式15计算第二阶段行走的距离，根据公式16计算第四阶段行走的距离，并根据公式17计算总的最短行走距离s_min：

smin＝s_I+s_II+s_IV 公式17；

情况2：当a_t≤0时，设置虚拟阶段，其中，虚拟阶段为加速度为0速度为v_u的时刻到v_t的时刻，根据公式18计算虚拟阶段的虚拟时间，根据公式19计算对应的vu，根据公式20计算虚拟阶段的行走距离：

t_I′＝|a_t|/J 公式18；

v_u＝v_t+a_t ²/(2J) 公式19；

s_I′＝v_t|a_t|/J+|a_t|³/(3J²) 公式20；

根据情况1的v_u之后的第二阶段、第三阶段、第四阶段行走距离的计算结果来计算从v_u这个点到停止所需要行走的距离，将该距离减去sI′得到情况2下的最短行走距离s_min。

5.一种移动机器人三次多项式速度曲线实时规划装置，其特征在于，包括：

动作策略判断模块，用于在每一个控制周期，获取移动机器人当前的速度v_t、上一个控制周期的加速度a_t-1、以及当前距离终点的距离s_t，根据v_t、a_t-1、s_t、以及预设的加加速度J，进行移动机器人的动作策略判断；

实时规划模块，用于根据动作策略判断结果进行移动机器人速度曲线的实时规划。

6.根据权利要求5所述的装置，其特征在于，动作策略判断模块具体包括：

第一计算子模块，根据公式1计算当前加速度a_t：

a_t＝a_t-1+JΔt 公式1；

其中，Δt为时间差；

第二计算子模块，判断a_t≥a_max是否成立，如果成立，则确认当前动作策略失败，进行下一个动作策略判断，调用第三计算子模块；如果不成立，则根据v_t和a_t计算最短行驶距离s_min，如果s_min<s_t，则接受加速度a_t，根据公式2计算下一个控制周期应该设定的速度v_t+1，否则，拒绝加速度a_t，继续进行下一个动作策略判断，调用第三计算子模块，其中，a_max为预先设置的最大加速度阈值；

v_t+1＝v_t+a_tΔt+JΔt²/2 公式2；

第三计算子模块，根据公式3计算此刻加速度，并根据v_t和a_t计算最短行驶距离s_min，如果s_min<s_t，则接受加速度a_t，根据公式4计算下一个控制周期应该设定的速度v_t+1，否则，拒绝加速度a_t，继续进行下一个动作策略判断，调用第四计算子模块，

a_t＝a_t-1 公式3；

v_t+1＝v_t+a_tΔt 公式4；

第四计算子模块，根据公式5计算此刻加速度a_t，并根据公式6计算下一个控制周期的速度：

a_t＝max{a_t-1-JΔt,-a_max} 公式5；

v_t+1＝v_t+a_tΔt-JΔt²/2 公式6。

7.根据权利要求5所述的装置，其特征在于，所述实时规划模块具体包括：

将计算得到的v_t+1进行换算和纠偏处理后下发给左右轮的电机，完成移动机器人速度曲线的实时规划。

8.根据权利要求6所述的装置，其特征在于，所述第二计算子模块、第三计算子模块具体用于：

情况1：当a_t>0时，要达到最短距离，向移动机器人先加一个负向的J，让加速度先变成0，然后从达到的最高速v_u开始减速，最后是一段正向J的阶段，使得加速度和速度都将为0，根据J将上述过程分为4个阶段，其中，第一阶段为当前速度v_t达到最高速v_u，且J<0，第二阶段为从最高速v_u减速到v_t，且J<0，第三阶段为v_t减速到v_std，且J＝0，第四阶段为由v_std减速到0，且J>0；根据公式7计算第一阶段的耗时，根据公式8计算达到的最大速度v_u，根据公式9计算第一阶段走过的距离：

t_I＝a_t/J 公式7；

v_u＝v_t+a_tt_I-Jt_I ²/2＝v_t+a_t ²/(2J) 公式8；

s_I＝v_ta_t/J+a_t ³/(3J²) 公式9；

定义一个速度v_std为移动机器人从速度0开始用正向J一直到加速到加速度等于最大值的时速度的取值，要加到最大加速度的时间为a_max/J,根据公式10计算出v_std：

v_std＝a_max ²/(2J) 公式10；

判断v_u>2v_std是否成立，则成立则存在第三阶段，否则不存在第三阶段；

在存在第三阶段的情况下即v_u>2v_std，根据公式11计算第二阶段行走的距离，根据公式12计算第三阶段行走的距离，根据公式13计算第四阶段行走的距离，并根据公式14计算总的最短行走距离s_min：

s_II＝(v_u-v_std)a_max/J+a_max ³/(3J²) 公式11；

s_III＝v_u(v_u-2v_std)/(2a_max) 公式12；

s_IV＝a_max ³/(6J²) 公式13；

s_min＝s_I+s_II+s_III+s_IV 公式14；

在不存在第三阶段的情况下即v_u≤2v_std，有一半的时间在第二阶段，一半的时间在第四阶段，在中点切换的时候的速度为v_u/2，则根据公式15计算第二阶段行走的距离，根据公式16计算第四阶段行走的距离，并根据公式17计算总的最短行走距离s_min：

smin＝s_I+s_II+s_IV 公式17；

情况2：当a_t≤0时，设置虚拟阶段，其中，虚拟阶段为加速度为0速度为v_u的时刻到v_t的时刻，根据公式18计算虚拟阶段的虚拟时间，根据公式19计算对应的vu，根据公式20计算虚拟阶段的行走距离：

t_I′＝|a_t|/J 公式18；

v_u＝v_t+a_t ²/(2J) 公式19；

s_I′＝v_t|a_t|/J+|a_t|³/(3J²) 公式20；

根据情况1的v_u之后的第二阶段、第三阶段、第四阶段行走距离的计算结果来计算从v_u这个点到停止所需要行走的距离，将该距离减去sI′得到情况2下的最短行走距离s_min。

9.一种移动机器人三次多项式速度曲线实时规划装置，其特征在于，包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述计算机程序被所述处理器执行时实现如权利要求1至4中任一项所述的移动机器人三次多项式速度曲线实时规划方法的步骤。

10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有信息传递的实现程序，所述程序被处理器执行时实现如权利要求1至4中任一项所述的移动机器人三次多项式速度曲线实时规划方法的步骤。

Images