基于位姿状态的全向运行型AGV路径跟踪优化控制方法

钱晓明　朱立群　楼佩煌　张沪松

(1.南京航空航天大学机电学院， 南京 210016； 2.江苏省精密与微细制造技术重点实验室， 南京 210016)

0　引言

“中国制造2025”战略的实施以及近年来数字化、无人化工厂、物流业和仓储业的迅速发展，使自动导航车(Automated guided vehicles,AGV)的需求量急剧上升。现代制造系统中，各个物流的传输时间占整个生产过程的80%～90%，物料传输与存储费用占整个零部件加工费用的30%～40%[1-2]，AGV在缩短物流传输时间、降低物料传输与存储费用方面起到至关重要的作用。因此，提高AGV路径跟踪的效率和精度对推广AGV的应用具有至关重要的作用。

针对路径跟踪问题[3-9]，文献[8]提出了一种最优控制策略，通过状态反馈得到合适的控制律，从而使系统的性能指标函数达到最小值。但随着差速驱动轮中心速度的改变，其鲁棒性也随之变化。文献[9]针对全方位移动机器人的延时问题，引入了广义预测控制法，同时将多输入多输出的线性模型分解成单输入单输出模型。在最优化控制中，加权矩阵Q、R的选择是一个难以避免的难题，文献[8-9]也没能有效地解决这个难题，而目前主要使用试凑法[10-12]，即将加权矩阵R固定为一个确定的或特殊的矩阵，通过反复实验与仿真从而找到合适的加权矩阵Q。

本文提出一种基于相对位姿状态的优化控制，将连续的状态转换成离散的系统，通过有限的纠偏步骤，使角度偏差和距离偏差同步收敛，同时通过建立Mecanum轮AGV的运动学模型，将目标函数中的加权矩阵求解转换为求解一个有限的控制序列，并且该控制序列是关于控制量的函数，从而根据该有限的控制序列实现对AGV的控制，且控制周期取决于控制步数，同时避开Q、R选择难的问题。

1　Mecanum轮AGV运动学模型

图1　全方位AGV运动学模型Fig.1　Omni-directional AGV kinematic model

本文采用的是视觉导航四轮独立驱动的Mecanum轮AGV小车，其在平面上具有3个自由度，机动性能好[13]。且该形式AGV的加速度性能在各个方向上比较均匀，加速度的大小基本受各个方向的影响[9]。全方位运动AGV运动学模型如图1所示，AGV系统运动的逆运动学方程为

(1)

式中ω1——第1个轮子角速度，rad/s

ω2——第2个轮子角速度，rad/s

ω3——第3个轮子角速度，rad/s

ω4——第4个轮子角速度，rad/s

r——轮子半径，mm

vx——AGV沿X方向的速度，mm/s

vy——AGV沿Y方向的速度，mm/s

ωz——AGV旋转的角速度，rad/s

L1——前后轮距离的一半，mm

L2——左右轮中心距离的一半，mm

对式(1)进行变换可得到系统的正运动学方程为

(2)

式(2)、(1)分别为AGV系统的正、逆运动学方程。由正逆运动学方程可知，当4个Mecanum轮的速度选择恰当时，就可以实现AGV在运动平面上的任意运动。由Rank(A)为3可知变量vx、vy、ωz相互独立，从而将原来的4个控制变量转换为3个变量vx、vy、ωz。其中vx、vy和后续的路径跟踪的距离偏差ed与沿轨迹的切线速度vτ相关，而ωz与路径跟踪的角度偏差eθ相关。

2　基于连续系统离散化的有限步运动预测优化控制

最优控制问题可以简单描述为根据被控对象的数学模型，选择一个容许的控制律，使被控对象按照预定的要求运行，并使给定的性能指标达到最优值。文献[14]通过对控制方程的简化和归一化处理使机器人的控制量Ui规整到一个确定的闭区间上，从而得到系统的时间Bang-Bang最优控制。

近年来，实时嵌入式控制器[13-14]在控制领域中使用越来越多，而一般实时嵌入式控制器都是按步执行的，因此对于控制器而言控制序列即离散系统更能有利于滚动控制[15-16]。为了实现对连续的过程实现嵌入式控制，需要把连续的系统离散化。本节首先通过将连续的系统离散化，再选择合适的目标函数，最后求解合适的有限控制序列使系统达到要求的状态。

2.1　控制系统性能指标的选取及系统离散化

根据最优控制理论，性能指标是指控制系统在容许的控制律作用下，系统所能达到性能程度，性能指标的内容和形式取决于最终的目标[17]。本文选择积分型性能指标J[17-18]，即

(3)

式中x(t)——系统状态量

u(t)——系统控制量

L(x(t),u(t),t)——x(t)、u(t)和t的连续函数

文献[10-12,18]利用LQR优化方法，将目标函数具体化为

(4)

式中Q、R——加权矩阵

文献[5]认为Q的选取不宜过大，否则系统控制器的输出会趋向饱和，同时会使系统对噪声比较敏感，系统的抗干扰能力下降。文献[10]认为距离偏差不能过大，否则会产生角度超调，并且前轮的转角会超过机械机构的极限。本节考虑约束条件

(5)

式中vxmax——X方向最大速度

vymax——Y方向最大速度

ωzmax——AGV最大旋转角速度

amax——系统的最大加速度

a——系数的加速度

将当前的状态偏差分为有限步进行消除，在最优控制中通过求解控制量的一个有限最优序列，将Q的试凑难题转换为求解控制量序列，此时将连续的系统转换为一个离散的系统，不仅避免了Q的求解，也有利于嵌入式控制器实时滚动控制。

由上述可知，vx、vy、ωz相互独立，其中vx、vy和路径跟踪的距离偏差ed与沿轨迹的切线速度vτ相关，ωz与路径跟踪的角度偏差eθ相关。因此，在系统性能满足的前提下，在确定的控制步数内，总可以选择合适的角度控制量Δω使角度偏差同步于距离偏差收敛于零。又当AGV进行路径跟踪时，希望AGV沿路径的切线速度vτ是一定的，可知vx、vy之间存在一定的关系，所以式(4)可以简化为只包含控制量u的形式，且控制量中只包含一个控制量vx。所以，可以将系统的目标函数离散为

(6)

式中k——系统控制步数

2.2　基于位姿状态的控制模型的建立

本文研究的是基于位姿状态的视觉导引四轮独立驱动的Mecanum轮路径跟踪问题[19-21]，其结构简图如图2所示。AGV在路径跟踪过程中，相对于路径有一定的位姿状态，在某一具体时刻该位姿状态主要表现为CCD摄像机实时采集到的角度偏差和距离偏差，而路径跟踪的实质是在产生角度偏差和距离偏差时，如何快速、平稳的消除偏差。本文通过控制vx和ωz来消除这两种偏差，调整位姿状态。

图2　Mecanum轮AGV结构简图Fig.2　Structure diagram of Mecanum wheel AGV 1.Mecanum轮　2.视野范围　3.CCD摄像机

假设AGV的控制周期为Ts，当前位姿状态为k时的角度偏差eθ(k)，经过一个周期的控制后，到达下一个状态k+1时，由前面的分析可知vx、vy、ωz相互独立，所以在AGV的角度偏差变化中可以单独控制ωz使角度偏差均匀且同步于距离偏差收敛于零，则k+1时的角度偏差为

eθ(k+1)=eθ(k)+ΔωzTs

(7)

如图3所示，k时刻距离偏差为lOA，经过Ts到k+1时距离偏差的减少量为lBC，并且此时的偏差为lCE，则

lCE=lOA+lDE-lBC

(8)

图3　AGV偏差几何解析Fig.3　Geometric analysis of AGV

当控制周期很短且AGV的速度值为有限值时，圆弧OC可近似地看成直线，可视AGV移动的距离为vTs，即lOC=vTs。由图4可知vx、vy、vτ和v存在关系

(9)

图4　AGV运行轨迹模型Fig.4　AGV running trajectory model

在RtΔAED中，距离的增量lDE为

lDE=lDAtaneθ=lOCcosθtaneθ

(10)

当角度偏差很小时，由式(8)～(10)可得距离偏差ed(k+1)近似为

(11)

从而得到系统的运动学模型即状态方程为

(12)

引入拉格朗日乘子函数

λ(k+1)=[λ1(k+1)λ2(k+1)]T

构造系统的广义离散泛函

(13)

又离散泛函取得极小值的条件为

(14)

由式(13)和式(14)可得

(15)

式中c——常数

又当控制步数N确定时，有

(16)

由式(15)和式(16)可得

(17)

将式(15)～(17)代入状态方程(12)中可得

(18)

又系统应满足纠偏目标

(19)

即经过N步控制后，偏差同步收敛于零。

由式(18)和式(19)可得

(20)

显然，c是一个只与初始偏差状态控制步数以及控制周期有关的常数。因此可得到最终速度和角速度控制量的解析表达式为

(21)

2.3　有限步数N的确定

速度控制量vx(k)的大小及变化率必须满足速度和加速度的约束条件，即式(5)的约束，由式(21)可知

显然vx(k)是(N-k)2的线性函数，分析可知，当k=0时，vx(k)得最大值且

控制周期Ts一定，当|vx(k+1)-vx(k)|得最大值时，则加速度也最大，而

显然加速度|a(k)|是(2N-2k-1)的线性函数，所以加速度的最大值|a(k)|max也是在k=0时得到，且

故控制步数N确定公式为

(22)

根据式(22)即可确定2个N值，取大者为最优控制有限步数的最小值，则只要控制步数大于该最小值即可，一般控制步数不宜过大，过大会造成系统的延时严重。同时控制步数反映了路径跟踪过程中两种偏差消除的快慢程度，即纠偏的效率，显然步数越小系统的纠偏效率也就越高，实时性也就越好。

2.4　基于连续系统离散化的有限步数运动预测的路径跟踪优化控制

文献[9-11,17]通过试凑加权矩阵Q、R来获得系统的增益系数，而求得的增益系数是在某一特定速度下获得的，当AGV运行速度改变时，系统的鲁棒性就会变差，并且增益系数的获得是通过反复迭代求解Raccati方程，计算难度大且不利于实时控制器的控制。

本文将系统及目标函数离散化，在系统的约束条件(式(5))下，通过寻求有限步的速度控制序列和角速度控制序列，从而得到系统的最优控制。且该控制序列能保证角度偏差和距离偏差同步快速的收敛。同时通过离散化的控制序列更加有利于实时控制器的滚动控制。由式(21)可知，该控制序列只与偏差初始值、控制步数、控制周期、运行速度vτ有关。一般情况下，系统的控制周期是不变的，在初始偏差一定的情况下，控制步数只取决于运行速度，且角度的控制量与k正相关，vx(k)是(N-k)2的线性函数，其计算量小，控制方便。

3　系统仿真与实验分析

本文的第1节推导出来了AGV小车的运动学模型，第2节在第1节的基础上推导了离散系统的最优控制模型，得到了状态方程的有限步控制序列式(18)以及速度和角速度控制量解析表达式式(21)。为了验证该控制方法的有效性和可行性，本节在前面模型的基础上对该模型进行数字仿真和实验分析。

由于在式(11)的推导过程中，作了近似代替，而此处近似代替的前提条件是角度偏差较小，故本文讨论的角度偏差的范围为|eθ(k)|≤8°。这里将对不同的角度和距离偏差以及速度进行数字仿真，其参数选择如表1所示，为了满足系统的运动约束，给定了系统的最大容许速度和加速度。

表1　低速初始位姿状态Tab.1　Initial pose state of low-velocity

表1是低速下各个参数的取值，表2是高速下各个参数的取值。

图5是vτ=0.3 m/s时，初始偏差eθ(0)=5°,ed(0)=0.01 m下的Matlab仿真结果。

表2　高速初始位姿状态Tab.2　Initial pose state of high-velocity

图5　低速纠偏仿真结果Fig.5　Simulation process of rectifying deviation with low-velocity

图6是vτ=0.5 m/s时，初始偏差eθ(0)=5°,ed(0)=0.01 m下的仿真结果。由图5和图6可知，在满足约束条件的前提下，控制步数越大，距离偏差在开始控制的前几步内越容易产生超调，因此不宜选择过大的控制步数。由图5a、6a或图5b、6b可知，在控制步数等其他条件相同的条件下，过大的线速度会造成一定距离偏差的超调，但经过几步后，距离偏差很快收敛。由图知，无论何种情况下，距离偏差和角度偏差均同步的收敛于目标值，且通过有限的步数，均能将偏差降低至零，控制过程比较平稳、快速、协调。

图6　高速纠偏仿真结果Fig.6　Simulation process of rectifying deviation with high-velocity

为验证本算法的有效性和可行性以及在全向运动型AGV路径跟踪中的实际效果，在仿真的基础上，采用如图7所示自主研发的视觉导引全向运动型AGV进行了路径跟踪实验。运动控制器是基于ARM并移植了uC/OSII的实时操作控制系统，系统的控制框图如图8所示。

图7　全向运动型AGV路径跟踪实验Fig.7　Experiment of omni-directional motion AGV path tracking

图8　系统控制框图Fig.8　System control block diagram

实验时将位姿状态以周期Ts=0.1 s通过串口不断地反馈给计算机，根据计算机实时采集到的距离偏差和角度偏差可得到全方位运动AGV的实际路径跟踪过程，如图9所示，图中从第10步到第20步为本算法的纠偏过程，同步快速地消除了角度偏差和距离偏差，第20步到第30步为AGV稳定运行阶段，该阶段角度偏差和距离偏差有一定的小波动，但距离偏差控制在2 mm以内，角度偏差在2°以内。实验表明了该优化算法在全方位运动型AGV路径跟踪的有效性和可行性。

图9　路径跟踪实验结果Fig.9　Experimental results of path tracking

对比前面的仿真结果可知，实际实验的位姿状态的纠偏趋势和仿真结果是相同的，但实际的过程中存在一些小的波动，造成这些波动的可能原因是系统存在一定的滞后，这些滞后包括DSP图像的采集以及处理的滞后，同时伺服电机驱动器的响应也具有一定的滞后性，另外，运动控制器的任务调度和管理是通过每个任务调用延时函数自动实现的，此时延时函数的延时时间不合理也会造成整个系统的滞后。

表3列举了不同控制方法的路径跟踪精度和达到稳定的时间，全方位运动AGV的路径跟踪误差为：ed≤2 mm，eθ≤2°，达到稳定的时间为0.8～1.2 s,显然，本文提出的路径跟踪算法具有较高的路径跟踪精度，且由仿真和实验结果可知，两种偏差的消除具有同步性和平稳性。

表3　不同控制方法的精度与效率Tab.3　Accuracy and efficiency of different control methods

4　结束语

基于以上的应用背景，本文首先对四轮独立驱动的Mecanum轮建立运动学模型，推导出四轮独立驱动的Mecanum轮AGV的正、逆学运动方程。基于AGV运动轨迹的分析，建立了AGV运动学模型，在最优控制理论的基础上，对目标函数进行简化，为了避免传统加权矩阵Q、R的选择难题，本文将系统离散化，通过有限的控制步数，在纠偏目标和运动约束的条件下，寻求最优的速度和角速度控制序列，将角度偏差和距离偏差同步消除，经过理论的推导，得到最终的角速度和速度的解析表达式，该算法计算量较小，且有限的控制序列有利于实时控制系统的实时滚动控制。通过以上仿真和实验可以看出，不同的速度和偏差下，该控制算法均有很好的控制效果，且速度偏差和角度偏差均能同步消除，当系统稳定时角度偏差|eθ|≤2°，距离偏差|ed|≤2 mm，系统达到稳定时的控制时间为0.8～1.2 s，因此本文提出的算法具有较高的路径跟踪精确和效率，同时具有较好的协调性和平稳性。

1WU Xing,YU Jun,LOU Peihuang,et al.Agent-oriented embedded control system design and development of a vision-based automated guided vehicle[J]. International Journal of Advanced Robotic Systems, 2012, 9(2): 1-13.

2WU Xing,LOU Peihuang,YU Jun,et al. Intersection recognition andguide-path selection for avision-based AGV in a bidirectional flow network[J]. International Journal of Advanced Robotic Systems,2014,11(3):1-17.

3武星,楼佩煌,唐敦兵.自动导引车路径跟踪和伺服控制的混合运动控制[J].机械工程学报, 2011, 47(3): 43-48.

WU Xing,LOU Peihuang,TANG Dunbing. Integrated motion control of path tracking and servo control for an automated guided vehicle [J]. Journal of Mechanical Engineering,2011,47(3):43-48.(in Chinese)

4WU Xing,LOU Peihuang,SHEN Ke,et al.Precise transshipment control of an automated magnetic-guided vehicle using optics positioning[J]. International Journal on Smart Sensing and Intelligent Systems, 2014, 7(1): 48-71.

5喻俊，楼佩煌，钱晓明，等.自动导引车视觉导引路径的识别及精确测量[J].华南理工大学学报：自然科学版，2012,40(3):143-149.

YU Jun,LOU Peihuang,QIAN Xiaoming,et al.Recognition and accurate measurement of vision-guided path of automatic guided vehicle[J].Journal of South China University of Technology:Natural Science Edition,2012,40(3):143-149.(in Chinese)

6王皖君，张为公，龚宗洋.自动导引车路径跟踪控制[J].测控技术，2009,28(9):49-51.

WANG Wanjun, ZHANG Weigong,GONG Zongyang.Path tracking control of automatic guided vehicle[J].Measurement & Control Technology,2009,28(9):49-51.(in Chinese)

7孟文俊,刘忠强.视觉导引AGV的路径跟踪控制研究[J].控制工程，2014,21(3):321-325.

MENG Wenjun,LIU Zhongqiang.Research on the path tracking control for vision-guided AGV[J].Control Engineering of China,2014,21(3):321-325.(in Chinese)

8陈无畏，孙海涛，李碧春,等.基于标识线导航的自动导引车跟踪控制[J].机械工程学报，2006,42(8):164-170.

CHEN Wuwei, SUN Haitao, LI Bichun, et al.Tracking control of automatic guided vehicle based on lane marker navigation[J].Chinese Journal of Mechnical Engineering, 2006,42(8):164-170.(in Chinese)

9郭旭，熊蓉，胡协和.全方位移动机器人的运动预测控制[J].电机与控制，2007,11(1)：79-87.

GUO Xu,XIONG Rong,HU Xiehe.Motion predictive control of omni-directional mobile robot[J].Electric Machines and Control,2007,11(1):79-87.(in Chinese)

10王荣本，李兵，徐友春，等.基于视觉导航的智能车辆自主导航最优控制器设计[J].汽车工程，2001,21(2)：97-100.

WANG Rongben, LI Bing,XU Youchun,et al.Optimal controller design for an intelligent vehicle based on vision navigation[J].Automotive Engineering,2001,21(2):97-100.(in Chinese)

11陈无畏，李碧春，孙海涛,等.基于视觉导航的AGV模糊-最优控制研究[J].中国机械工程，2006,17(24)：2546-2550.

CHEN Wuwei,LI Bichun,SUN Haitao,et al. Study on fuzzy-optimal control of vision navigation for an AGV[J].China Mechanical Engineering,2006,17(24):2546-2550.(in Chinese)

12周俊，姬长英.基于视觉导航的轮式移动机器人横向最优控制[J].机器人，2002,24(3)：209-212.

ZHOU Jun,JI Changying.Lateral optimal control for wheeled mobile robot navigated by machine vision[J].Robot,2002,24(3):209-212.(in Chinese)

13刘洲，吴洪涛.Mecanum四轮全方位移动机构运动分析与仿真[J].中国制造业信息化，2011,40(5):43-46.

14熊蓉, 张翮, 褚健, 等. 四轮全方位移动机器人的建模和最优控制[J]. 控制理论与应用, 2006, 23(1): 93-98.

XIONG Rong,ZHANG He,CHU Jian,et al.Modeling and optimal control of omni-ditectional mobile robots[J].Control Theory & Applications,2006,23(1):93-98.(in Chinese)

15LOZOVA C, MARTI P, VELASCO M, et al. Simulation study of a remote wireless path tracking control with delay estimation for an autonomous guided vehicle[J]. The International Journal of Advanced Manufacturing Technology, 2011, 52(5-8): 751-761.

16SOYSAL B. Real-time control of an automated guided vehicle using a continuous mode of sliding mode control[J]. Turkish Journal of Electrical Engineering and Computer Science, 2014, 22(5): 1298-1306.

17胡寿松，王执铨，胡维礼.最优控制理论与系统[M].3版.北京：科学出版社，2017.

18张锋.线性二次型最优控制问题的研究[D].天津:天津大学,2009.

19孟庆宽, 张漫, 杨耿煌，等. 自然光照下基于粒子群算法的农业机械导航路径识别[J/OL]. 农业机械学报, 2016, 47(6): 11-20. http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20160602&flag=1.DOI: 10.6041 /j.issn.1000-1298.2016.06.002.

MEMG Qingkuan，ZHANG Man，YANG Genghuang, et al. Guidance line recognition of agricultural machinery based on particle swarm optimization under natural illumination[J/OL]. Transactions of the Chinese Society for Agricultural Machinery, 2016, 47(6):11-20. (in Chinese)

20武星, 沈伟良, 楼佩煌，等. 视觉导引AGV鲁棒特征识别与精确路径跟踪研究[J/OL]. 农业机械学报, 2016, 47(7): 48-56. http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?file_no=20160708&flag=1.DOI:10.6041/j.issn.1000-1298.2016.07.008.

WU Xing, SHEN Weiliang, LOU Peihuang, et al. Robust feature recognition and precise path tracking for vision-guided AGV[J/OL]. Transactions of the Chinese Society for Agricultural Machinery, 2016, 47(7): 48-56. (in Chinese)

21杨旭, 楼佩煌, 武星，等. 基于光照约束的AGV视觉导引非均匀光照增强方法[J]. 机械科学与技术, 2013, 32(12): 1752-1757.

YANG Xu, LOU Peihuang, WU Xing, et al. A method for enhancing non-uniform illumination of vision-guided automatic guided vehicle with illumination constraints[J]. Mechanical Science and Technology for Aerospace Engineering, 2013,32(12):1752-1757. (in Chinese)