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

权利要求书

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

其中：θ₀和θ₁分别对应该段小圆弧起点和终点的圆心角。

3.根据权利要求1所述的一种全向运动AGV任意运动控制算法，其特征在于：所述S2步骤具体步骤如下：

根据AGV定位信息，所知AGV当前位姿(x，y，θ_AGV)，计算垂足：

S1：根据x和y，计算当前对应的圆心角：θ＝atan2(y-O_y，x-O_x)；

S2：根据θ，得出AGV现在处于m个小圆弧中的那一段，从而得出上一章提及的θ₀和θ₁；

S3：判断当前AGV位姿是否与终点位置足够远，如果是，那么进行计算时间参数t_curr＝(θ-θ₀)；

S4：根据t_curr，计算参考点的x_ref和y_ref；

S5：给定参考位姿的角度为：θ_ref＝atan2(y_ref，x_ref)；

End：参考位姿：(x_ref，y_ref，θ_ref)。

4.根据权利要求1所述的一种全向运动AGV任意运动控制算法，其特征在于：所述S3控制量计算包括以下步骤：

S3.1：前馈控制计算；

S3.2：反馈控制计算；

S3.3：控制量输出；

S3.4：电机速度保护；

S3.5：电机加速度保护。

5.根据权利要求1所述的一种全向运动AGV任意运动控制算法，其特征在于：所述S3.1具体步骤如下：

在电机不过载的情况下，前馈控制为参考位姿的切线速度：

θ为当前AGV坐标相对于上图模板对应的圆心角：θ＝atan2(y-O_y，x-O_x)。

6.根据权利要求1所述的一种全向运动AGV任意运动控制算法，其特征在于：所述S3.2具体步骤如下：

分别计算控制命令的三个误差，然后进行比例控制：