一种全向运动AGV任意运动控制算法制造技术

本发明专利技术公开了一种全向运动AGV任意运动控制算法，包括如下步骤：S1：轨迹生成(预设)；S2：参考位姿计算；S3：控制量计算；S4：运动分解；S5：运动执行命令。本发明专利技术基于全向AGV的路径轨迹规划，AGV运动控制方法，并根据AGV的实时定位信息，计算AGV的运动控制量，使得全向运动AGV可追随任意轨迹，使得全向AGV可以沿任意轨迹、不停滞的进行“柔性”动作，最终沿指定轨迹并达到目标位姿，AGV行驶过程中，通过在线位姿调整和离线位姿规划，在AGV行驶过程中提供希望位姿(参考位姿)，使得AGV实现任意轨迹曲线的跟踪，本发明专利技术提供的参考位姿计算，主要基于当前AGV的实际位姿(定位信息)计算而得，控制量是通过AGV的实际位姿与参考位姿之差作为PID控制输入。PID控制输入。PID控制输入。

【技术实现步骤摘要】
一种全向运动AGV任意运动控制算法


[0001]本专利技术涉及AGV
，具体为一种全向运动AGV任意运动控制算法。

技术介绍

[0002]相对于受限运动AGV(两轮差速、四轮差速、双差速、车型等)，全向移 动AGV(全向轮或麦克纳姆轮驱动)可进行无限制运动，如横移、自旋、斜行 等，适应于狭窄受限空间的货物搬运工作。另一方面，一般的，全向驱动AGV 的控制，主要局限在典型动作中，如直行、横移、自旋、斜行等，因此其轨 迹规划采用拆分的方式，把起点到终点的整体路径划分为典型动作，通过典 型动作的组合完成搬运工作。但是，这种典型动作的组合，造成其轨迹单一 刻板，反而效率低下。例如，一个典型的AGV车头的转弯动作，需要进行直 行
→
自旋
→
直行的动作完成，虽然整个过程无转弯半径，但AGV需要停车两 次，自旋动作耗时较长，因此整个过程较之受限运动AGV的运动效率低得多。

技术实现思路

[0003]本专利技术的目的在于提供一种全向运动AGV任意运动控制算法，以解决上 述
技术介绍
中提出的问题。
[0004]为实现上述目的，本专利技术提供如下技术方案：一种全向运动AGV任意运 动控制算法，包括如下步骤：
[0005]S1：轨迹生成(预设)；
[0006]S2：参考位姿计算；
[0007]S3：控制量计算；
[0008]S4：运动分解；
[0009]S5：运动执行命令。
[0010]优选的，所述S1步骤具体步骤如下：AGV沿圆弧运动，其中圆弧半径为 r，AGV旋转的圆心角为φ(φ≤90
°
)，首先生成AGV的希望运动轨迹，在 某一时刻，AGV在圆弧上运动，其位置对应的圆心角为θ，整段圆弧(终点) 对应的圆心角为φ，逆时针旋转为正，r为圆弧半径，全局坐标系下的原点 坐标如上图所示；
[0011]对应时间和速度，假设时间索引为t，AGV在进入小圆弧时按照车型运动， 运动方向始终为圆弧切线，相对小圆弧的角速度为ω
T
，那么AGV的位姿(x， y，θ)的参数方程为：
[0012][0013]将整个圆弧切割成m个等分，利用m等分的Hermite差值，对于每一等 分来说，Hermite差值可以表达成三阶多项式的形式：
[0014]其中
[0015]已知x和y的参数方程，那么x和y在起点t＝0和终点的 值和一阶导数的值均可以算出，计算结果如下：
[0016]对于x:x
_coeff
＝[a
x，i
，b
x，i
，c
x，i
，d
x，i
]，
[0017][0018]x
_coeff
＝T
x
‑1*β
x
[0019]对于y:y
_coeff
＝[a
y，i
，b
y，i
，c
y，i
，d
y，i
]，
[0020][0021]y
_coeff
＝T
y
‑1*β
y
其中：θ0和θ1分别对应该段小圆弧起点和终点的圆心角。
[0022]优选的，所述S2步骤具体步骤如下：
[0023]根据AGV定位信息，所知AGV当前位姿(x,y,θ
AGV
)，计算垂足：
[0024]S1：根据x和y，计算当前对应的圆心角：θ＝atan2(y
‑
O
y
，x
‑
O
x
)；
[0025]S2:根据θ，得出AGV现在处于m个小圆弧中的那一段，从而得出上一 章提及的θ0和θ1；
[0026]S3：判断当前AGV位姿是否与终点位置足够远，如果是，那么进行计算 时间参数t_curr＝(θ
‑
θ0)；
[0027]S4：根据t_curr，计算参考点的x_ref和y_ref；
[0028]S5:给定参考位姿的角度为：θ
ref
＝atan2(y_ref，x_ref)；
[0029]End：参考位姿：(x_ref，y_ref,θ
ref
)。
[0030]优选的，所述S3控制量计算包括以下步骤：
[0031]S3.1：前馈控制计算；
[0032]S3.2:反馈控制计算；
[0033]S3.3：控制量输出；
[0034]S3.4：电机速度保护；
[0035]S3.5：电机加速度保护。
[0036]优选的，所述S3.1具体步骤如下：
[0037]在电机不过载的情况下，前馈控制为参考位姿的切线速度：
[0038][0039]θ为当前AGV坐标相对于上图模板对应的圆心角： θ＝atan2(y
‑
O
y
，x
‑
O
x
)。
[0040]优选的，所述S3.2具体步骤如下：
[0041]分别计算控制命令的三个误差，然后进行比例控制：
[0042][0043]优选的，所述S3.3计算步骤如下：
[0044][0045]优选的，所述S3.4具体步骤如下：
[0046]根据全向AGV的运动学方程(以麦克纳姆轮为例，小棍子的斜角为45
°
)：
[0047][0048]其中R
W
、W、L均为与AGV尺寸相关的参数。
[0049]按照电机的最高转速，判断ω1、ω2、ω3和ω4中是否有超限，如有，那么 所有转速按照同比例压制。
[0050]优选的，所述S3.5具体步骤如下：
[0051]计算电机本拍和上一拍的差，进行加速度限制。
[0052]与现有技术相比，本专利技术的有益效果是：
[0053]该全向运动AGV任意运动控制算法，本方法基于全向AGV的路径轨迹规划，AGV运动控制方法，并根据AGV的实时定位信息，计算AGV的运动控制量，使 得全向运动AGV可追随任意轨迹，使得全向AGV可以沿任意轨迹、不停滞的 进行“柔性”动作，最终沿指定轨迹并达到目标位姿。AGV行驶过程中，本方 法遵循环境限制，通过在线位姿调整和离线位姿规划，在AGV行驶过程中提 供希望位姿(参考位姿)，使得AGV实现任意轨迹曲线的跟踪，本专利技术提供 的参考位姿计算，主要基于当前AGV的实际位姿(定位信息)计算而得，控 制量是通过AGV的实际位姿与参考位姿之差作为PID控制输入。
附图说明
[0054]图1为本专利技术计算流程示意图；
[0055]图2为本专利技术运行轨迹示意图；。
具体实施方式
[0056]下面将结合本专利技术实施例中的附图，对本专利技术实施例中的技术方案进行 清楚、完整地描述，显然，所描述的实施例仅仅是本专利技术一部分实施例，而 不是全本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种全向运动AGV任意运动控制算法，其特征在于，包括如下步骤：S1：轨迹生成(预设)；S2：参考位姿计算；S3：控制量计算；S4：运动分解；S5：运动执行命令。2.根据权利要求1所述的一种全向运动AGV任意运动控制算法，其特征在于：所述S1步骤具体步骤如下：AGV沿圆弧运动，其中圆弧半径为r，AGV旋转的圆心角为φ(φ≤90
°
)，首先生成AGV的希望运动轨迹，在某一时刻，AGV在圆弧上运动，其位置对应的圆心角为θ，整段圆弧(终点)对应的圆心角为φ，逆时针旋转为正，r为圆弧半径，全局坐标系下的原点坐标如上图所示；对应时间和速度，假设时间索引为t，AGV在进入小圆弧时按照车型运动，运动方向始终为圆弧切线，相对小圆弧的角速度为ω
T
，那么AGV的位姿(x，y，θ)的参数方程为：将整个圆弧切割成m个等分，利用m等分的Hermite差值，对于每一等分来说，Hermite差值可以表达成三阶多项式的形式：其中已知x和y的参数方程，那么x和y在起点t＝0和终点的值和一阶导数的值均可以算出，计算结果如下：对于x：x
_coeff
＝[a
x，i
，b
x，i
，c
x，i
，d
x，i
]，x
_coeff
＝T
x
‑1*β
x
对于y：y
_coeff
＝[a
y，i
，b
y，i
，c
y，i
，d
y，i
]，y
_coeff
＝T
y
‑1*β
y
其中：θ0和θ1分别对应该段小圆弧起点和终点的圆心角。3.根据权利要求1所述的一种全向运动AGV任意运动控制算法，其特征在于：所述S2步骤具体步骤如下：
根据AGV定位信息，所知AGV当前位姿(x，y，θ
AGV
)，计算垂足：S1：根据x和y，计算当前对应的圆心角：θ＝atan2(y

【专利技术属性】
技术研发人员：邓光荣，
申请(专利权)人：上海睿瞻智能科技有限公司，
类型：发明
国别省市：