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

本发明专利技术公开了一种移动机器人三次多项式速度曲线实时规划方法及装置，所述方法包括：在每一个控制周期，获取移动机器人当前的速度v

【技术实现步骤摘要】
移动机器人三次多项式速度曲线实时规划方法及装置


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

技术介绍

[0002]在现有技术中，仓储物流自动化应用场景中，自动导引运输车(AGV)或自主移动机器人(AMR)被广泛地用于货物搬运、拣选、传输、分类等。相比传统的人工操作，移动机器人自动化运作大幅提升了物流效率、降低了人工成本。AGV或AMR在工作时，其运载货物的质量、重心往往是变化不定的，为了使移动机器人能够平稳地起步和停止、运输过程中更快速地行走、并适应不同的货物搭载情形，通常为移动机器人的行走路径规划一个适宜的速度S曲线。常用的S曲线有梯形速度S曲线、多项式S曲线、三角函数加加速度S曲线。
[0003]在现有技术中，提供了一种物流搬运设备S曲线加减速度规划与控制方法，公开了一种三次多项式S曲线的规划方法和实施过程中移动机器人的运行误差控制方法。该专利技术专利在移动机器人每次运行前，一次性规划出行走路径的S曲线，以移动机器人的当前位移作为判断依据并对比规划好的S曲线，本文档来自技高网...

【技术保护点】

【技术特征摘要】
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Δt2/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Δt2/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
t
t
I
‑
Jt
I2
/2＝v
t
+a
t2
/(2J)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
公式8；s
I
＝v
t
a
t
/J+a
t3
/(3J2)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
公式9；定义一个速度v
std
为移动机器人从速度0开始用正向J一直到加速到加速度等于最大值
的时速度的取值，要加到最大加速度的时间为a
max
/J,根据公式10计算出v
std
：v
std
＝a
max2
/(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
max3
/(3J2)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
公式11；s
III
＝v
u
(v
u
‑
2v
std
)/(2a
max
)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
公式12；s
IV
＝a
max3
/(6J2)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
公式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
t2
/(2J)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
公式19；s
I
′
＝v
t
|a
t
|/J+|a
t
|3/(3J2)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
公式20；根据情况1的v
u
之后的第二阶段、第三阶段、第四阶段行走距离的计算结果来计算从v
u
这个点到停止所需要行走的距离，将该距离减去sI
′
得到情况2下的最短行走距离s
min
。5.一种移动机器人三次多项式速度曲线实时规划装置，其特征在于，包括：动作策略判断模块，用于在每一个控制周期，获取移动机器人当前的速度v
t...

【专利技术属性】
技术研发人员：李光毅，朱礼君，刘衡，周铠，
申请(专利权)人：杭州壹悟科技有限公司，
类型：发明
国别省市：