[发明专利]一种全向运动AGV任意运动控制算法

说明书

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

技术领域

本发明涉及AGV技术领域，具体为一种全向运动AGV任意运动控制算法。

背景技术

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

发明内容

本发明的目的在于提供一种全向运动AGV任意运动控制算法，以解决上 述背景技术中提出的问题。

为实现上述目的，本发明提供如下技术方案：一种全向运动AGV任意运 动控制算法，包括如下步骤：

S1：轨迹生成(预设)；

S2：参考位姿计算；

S3：控制量计算；

S4：运动分解；

S5：运动执行命令。

优选的，所述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]，