[发明专利]双舵轮自动导引车轨迹追踪控制方法及系统

权利要求书

1.双舵轮自动导引车轨迹追踪控制方法，其特征是，包括：

建立双舵轮自动导引车AGV运动学模型：双舵轮AGV转弯时，AGV车体四角各为一个从动轮，沿中线前后并置两轮为独立驱动的驱动轮；由于车体运动通过动态调整前后舵轮来实现的，因此，在不影响运动学建模分析的情况下，将AGV的运动模型简化为前后驱动轮的运动模型；

基于上述模型，建立双舵轮前后舵轮的位置坐标即AGV车体位置坐标，基于车体位置坐标，车体运动过程中，结合舵轮运动速度及航向角，得其角速度；

建立平面直角坐标系{XOY}，O_f、O_b分别为前后驱动轮与地面接触点；轴距为L；考虑AGV车身处于转弯圆弧轨迹时，由于圆弧轨迹各处曲率相同，因此行进过程中前后驱动轮转角相同；前驱动轮与后驱动轮行进方向与机器人体坐标系{X’CY’}X轴夹角为θ_f、θ_b，V_f、V_b为前后轮行进速度，转弯半径为R；从后驱动轮进入转弯圆弧开始计时，经过t₁时间，前轮走过的轨迹弧长为2Rθ_f；则有：

即驱动轮行进速度V_f恒定的前提下，车体转角2θ_f与其行进时间t₁成正比；根据转弯轨迹三角关系，可知：

双舵轮AGV定位算法：车体几何中心(X₀,Y₀)为前舵轮中点O_f和后舵轮中点O_b的连线中点，在负载均匀的情况下，认为几何中心(X₀,Y₀)即为AGV车体质心，其初始值为(X_0c,Y_0c)；θ_f、θ_b分别为AGV前后轮转角角度V_f、V_b为前后轮线速度；O点为小车转弯时瞬时运动轨迹圆心，则R_f、R_b为前后轮转弯过程中的轨迹半径；则其前后舵轮的位置坐标可表示为：

其中，质心坐标C＝(X₀,Y₀)可以表示为：

即为AGV车体位置坐标；

质心在全局坐标系下的线速度状态量为：

即线速度v_c可表示为：

其航向角θ为：

考虑刚体运动特性，车体绕O点运动时，前后舵轮中心点及车体质心C运动的在地面平动的角速度一致；车体运动过程中，结合舵轮运动速度及航向角，根据正弦定理得其角速度为：

将轮式自动导引车系统位姿状态分别表示车体质心在全局坐标系下的位置坐标，将双舵轮自动导引车自身运动线速度与角速度作为控制输入量，以车体质心线速度和角速度表示，获得双舵轮自动导引车运动学方程；

当应用模型预测控制纠正双舵轮AGV轨迹路径时，将轮式自动导引车系统位姿状态记为X＝[X₀ Y₀ θ]^T＝[x y θ]^T，x，y分别表示车体质心在全局坐标系{XOY}下的位置坐标；将双舵轮自动导引车自身运动线速度与角速度作为控制输入量u，以车体质心线速度和角速度表示，记为u＝[v_c ω_c]^T＝[v ω]^T；

车轮与地面纯滚动接触，即AGV在平面运动时没有滑动，可以给出系统运动学模型如下：

进一步，双舵轮自动导引车运动学方程如式(11)所示：

求解双舵轮自动导引车运动学方程：将最优控制问题的求解转化为实时数值解有约束优化问题，通过计算得到控制量作用于双舵轮自动导引车，实现双舵轮AGV轨迹跟踪控制；

双舵轮AGV轨迹跟踪误差模型：为规划AGV跟踪路径，设立运行参考轨迹，参考轨迹任意点均满足状态空间表达式(12)，即：

表示参考轨迹上任意点的状态空间表达式；

其中参考轨迹状态量为X_r＝[x_r y_r θ_r]^T，其参考输入控制量u_r＝[v_r,w_r]^T，(x_r,u_r)为参考轨迹车辆质心坐标，θ_r为参考航向角；为线性化预测控制模型，将式(12)在参考轨迹点(x_r,u_r)处进行泰勒展开，忽略高阶项，保留一阶项，获得双舵轮移动机器人线性化误差模型，其中，(13)式为参考轨迹保留一阶项的泰勒级数展开；线性误差模型为(16)式：

即：

结合状态空间表达式，可得：

将式(13)与式(12)做差，可得：

表示连续误差模型的状态空间表达式；

其中，x′＝x-x_r，表示车体实际运动位置与参考位置的状态误差；u′＝u-u_r则是控制输入量的误差；式(16)为连续模型，通过使用前向差分近似x′，得到以下AGV运动线性时变离散误差模型：

其中，线性时变矩阵分别为：

其中，T为采样周期，k为采样时间；

轨迹跟踪控制器模型：离散化误差模型在系统k时刻增量式表达式为：

Δu(k)＝u(k)-u(k-1) (20)

则运动学模型的状态空间表达式为：

其中，

这里有限预测时域为p，在时域[τ,τ+p]内对系统状态进行预测；控制时域为c，在时域[τ,τ+c]内生成系统控制序列；所以预测时域内系统的预测方程为：

Y(k)＝φ_pψ(k)+Q_cΔU(k) (23)

其中，Y(k)为系统输出量；φ_p为状态量参数；Q_c为控制量参数；ΔU(k)为控制增量序列，用公式表达为：

在系统预测方程中，建立准确的目标函数求解控制增量ΔU(k)，从而得到控制时域[τ,τ+c]内合适的控制量序列，这里建立目标函数为：

目标函数既保证了模型精准跟踪目标轨迹，又保证控制量符合AGV实际性能限制；Q为预测时域权重因子；R为控制时域权重因子；_ρ为权重系数；γ为松弛变量；

AGV实际运行中，考虑AGV行驶可行域、舵轮转角和行进速度现实约束条件，轨迹跟踪控制器模型可描述为：

轨迹跟踪控制器模型算法求解：为获取系统最优控制序列，将上述最优化轨迹跟踪问题转化为带约束线性二次规划问题进行求解；其形式如下：

将预测方程式(23)代入目标函数式(26)，经过整理可得：

其中G为控制增量系数矩阵，E_k为预测时域[τ,τ+p]内轨迹跟踪误差；针对带有线性约束的二次实函数，采用具有通用性的内点法求解，求得[τ,τ+c]内控制输入增量为：

ΔU_t＝[Δu_t,Δu_t+1,…,Δu_t+c-1]^T (29)

将该控制序列中第一个元素作为实际输入的控制量增量，通过计算得到控制量作用于实际系统，即：

u(t)＝u(t-1)+Δu_t (30)

当系统进入下一采样周期后，系统重新计算控制输入增量，循环迭代实现双舵轮AGV轨迹跟踪控制。