AGV系统的轨迹跟踪控制方法与流程

本发明属于自动控制技术领域，具体涉及一种具有多个偏心舵轮的AGV系统以及AGV位姿解耦的轨迹跟踪控制方法。

背景技术：

AGV，即Automated Guided Vehicle，通过预设的程序自动将物品从一个位置移动至另一位置，是一种自动化、信息化和智能化设备。目前市场上的AGV系统其结构主要包括差动轮结构、麦克纳姆轮结构以及舵轮结构等。其中差动轮结构简单可靠、易于控制，但车体姿态与前进方向耦合、回转中心必须位于两个差动轮的轴线上，运动灵活性较差；麦克纳姆轮结构克服了差动轮结构的缺点，但也引入了成本高、运动平稳性差等不足；常见的舵轮结构有单舵轮和双舵轮等，其中单舵轮结构本质上与差动轮结构一致但结构更紧凑多用于叉车系统；双舵轮结构可以实现在不改变车体姿态的情况下转变前进方向，兼具运动灵活性高和运动平稳性好的优点，成本也低于麦克纳姆轮结构。理论上以双舵轮为代表的多舵轮AGV系统可以实现沿着指定轨迹前进的同时任意旋转车体姿态，但其运动控制难度较高，目前国内市场上未见有应用案例。此外，为改善传统舵轮中驱动轮位于转向轴的正下方导致高度较高、体积较大的不足，驱动轮远离转向轴的新型偏心舵轮开始出现，这也给其运动控制带来了新的难点。

技术实现要素：

本发明要解决的技术问题是提供一种针对多偏心舵轮AGV系统位姿解耦的轨迹跟踪控制方法，使其可以沿着指定轨迹前进的同时任意旋转车体姿态。通过反步法给出当AGV车体因车轮打滑等原因而偏离预定轨迹时的反馈控制率以得到修正后的运动指令，然后将该运动指令通过位姿解耦运动学求解每个偏心舵轮的转向角和前进速度，偏心舵轮的伺服电机即可按照该转向角和前进速度驱动AGV车体运动。

为解决上述技术问题，本发明采用的技术方案是：

一种AGV系统的轨迹跟踪控制方法，基于具有至少2个偏心舵轮装置的AGV系统，其特征在于，包括以下步骤：

步骤A、以所有偏心舵轮装置中的转向轴位于零位时车体前进方向为X轴、指定任意参考点A为原点建立与AGV车体固连的AGV车体坐标系{A}，计算出每个转向轴的中心Oi在AGV车体坐标系{A}下的坐标值[^Axi,^Ayi]及驱动轮的偏心值bi；

步骤B、建立在AGV系统应用场景下的地面直角坐标系{W}以及上层规划器给出的AGV车体位姿[x,y,α]在地面直角坐标系{W}下的运动轨迹{Path}：其中，t为时间；

步骤C、在AGV系统开始运动后的任意时刻，计算轨迹跟踪偏差以及修正后的运动指令并根据修正后运动指令求解AGV每个偏心舵轮的转向角^Aβi,r和前进速度vi,r，控制偏心舵轮装置按照求解得到的转向角和前进速度驱动AGV系统移动。

本发明的方法针对具有多个偏心舵轮的AGV系统，不但能够在不改变车体姿态的情况下转变前进方向，而且能够在沿着指定轨迹前进的同时任意旋转车体姿态，充分发挥舵轮结构AGV系统运动灵活性高和平稳性好的优点；通过视觉相机和IMU或视觉相机和里程计实现组合导航定位，摆脱了传统方法对专用参照物(如磁条、RFID信标、二维码阵列等)的依赖，只需要已有常见地面参照物(如地板、瓷砖等)，大大降低了现场施工成本、难度与工作量，视觉相机在车体内部向下拍摄地面，不怕人员或障碍物的遮挡(如激光反射板、激光雷达等)，适用于动态非结构环境的应用场合。。

下面结合附图对本发明进行详细说明。

附图说明

图1是本发明中具有多个偏心舵轮装置的AGV系统的结构示意图；

图2是本发明中具有两个偏心舵轮装置的AGV系统的结构示意图；

图3是图2中双舵轮AGV的A-A向剖视图；

图4是本发明中AGV系统沿指定轨迹前进的同时任意旋转车体姿态示意图；

图5是本发明AGV系统中的控制装置的控制示意图；

图6是本发明的方法中位姿解耦运动学的原理示意图；

图7是以采用木地板的地面建立的坐标系示意图；

图8是以采用瓷砖的地面建立的坐标系示意图；

图9是根据参照物自动生成的网格地图示意图；

图10是IMU数据计算得到的相机视野与视觉相机采集到的真实视野的示意图。

在附图中：1是连接支架，2是行走驱动电机，3是舵轮，4是转向驱动电机，5是转向轴，6是链条传动组件，7是万向轮。

具体实施方式

参见附图1，本发明提供了一种具有多个偏心舵轮装置的AGV系统，AGV系统至少有2个用于驱动车体移动的偏心舵轮装置。偏心舵轮装置包括驱动轮、调整驱动轮方向的转向轴以及必要的辅助安装固定组件。偏心舵轮装置的驱动轮与地面接触，驱动AGV车体前进；并且驱动轮不位于转向轴的正下方而是与转向轴之间存在偏心。

具体地，参见附图2和3，AGV系统包括AGV车体，在AGV车体设有万向轮7和用于驱动AGV车体移动的偏心舵轮装置。偏心舵轮装置包括舵轮组件以及驱动舵轮组件偏心旋转的转向驱动组件。万向轮7在车体的底部的四个角分别设置一个或多个。偏心舵轮装置可以根据需要设置2个、3个或者多个。

上述的舵轮组件包括连接支架1、固定在连接支架1上的行走驱动电机2以及与行走驱动电机2的输出轴相连的驱动轮3。

上述的转向驱动组件包括固定在AGV车体上的转向驱动电机4、转向轴5以及设置在转向驱动电机4的输出轴和转向轴5间的链条传动组件6。转向轴5与连接支架1相连并且位于驱动轮3的一侧。转向轴5为套管式的结构，以便于在其中空腔内安装与电机相配套的滑环。

AGV系统中设有与偏心舵轮装置相配套的控制装置，控制装置包括用户操作接口模块、无线通信模块、导航定位模块、上层轨迹规划器模块、伺服电机控制模块、安全避碰模块、系统日志模块等，以及与本发明所述方法相关的轨迹跟踪控制器模块和位姿解耦运动学模块，其流程图如图5所示。轨迹跟踪控制器模块根据上层规划器的轨迹指令及AGV当前位姿等参数求解位姿偏差和修正后运动指令。位姿解耦运动学模块根据修正后运动指令求解AGV每个偏心舵轮的转向角和前进速度，如图6所示。偏心舵轮的伺服电机即可按照该转向角和前进速度驱动AGV车体运动，使AGV可以在沿任意曲线前进的同时任意旋转车体姿态，如图4所示。

参见附图5和6，基于上述的AGV系统，本发明提供了一种多偏心舵轮AGV位姿解耦的轨迹跟踪控制方法，包括建立车体坐标系以及计算偏心舵轮坐标值与偏心值、定义应用场景以及任务轨迹和控制AGV车体跟踪给定任务轨迹三个步骤。首先，AGV控制装置的轨迹跟踪模块根据给定的任务轨迹以及当前AGV位姿数据计算轨迹跟踪偏差并给出修正后的运动指令，然后控制装置的位姿解耦运动学模块将运动指令转化成每个偏心舵轮的转向角和前进速度。如此偏心舵轮装置的伺服电机就可按照求解的转向角和前进速度驱动AGV前进；使其不但能够在不改变车体姿态的情况下转变前进方向，而且能够在沿着指定轨迹前进的同时任意旋转车体姿态，充分发挥舵轮结构AGV系统运动灵活性高和平稳性好的优点。

轨迹跟踪控制方法包括以下步骤：

步骤A、任意指定AGV车体一点为参考点A，以该参考点为原点、以所有偏心舵轮装置中的转向轴5位于零位时车体前进方向为X轴建立与AGV车体固连的AGV车体坐标系{A}，计算出所有偏心舵轮装置转向轴的中心Oi在AGV车体坐标系{A}下的坐标值[^Axi,^Ayi]及驱动轮的偏心值bi。

步骤B、建立在AGV应用场景下的地面直角坐标系{W}以及由上层规划器给出AGV车体位姿[x,y,α]在地面直角坐标系{W}下的运动轨迹{Path}：

其中，t为时间。

AGV车体位姿包括车体位置和姿态。其中，AGV车体位置是AGV车体坐标系{A}的原点即参考点A在地面直角坐标系{W}下的坐标值[x,y]；AGV车体姿态是AGV车体坐标系{A}的X轴正向与地面直角坐标系{W}的X轴正向之间的夹角α。AGV参考点A处的位置平动速度矢量与AGV车体坐标系{A}的X轴正向之间的夹角为AGV车体的转向角，记为^Aβ；与地面直角坐标系{W}的X轴正向之间的夹角为AGV车体的前进方向角，记为β；二者之间满足β＝^Aβ+α。

运动轨迹{Path}规定了AGV车体位姿随着时间t的变化关系，并且AGV车体位置和姿态之间相互独立。

步骤C、在AGV开始运动后的任意时刻(如图4所示)，由轨迹跟踪控制器模块根据上层规划器给出的轨迹指令以及当前时刻AGV实际位姿计算轨迹跟踪偏差以及修正后的运动指令即位置平动速度指令和姿态转动速度指令vα,r；然后由位姿解耦运动学模块根据修正后的运动指令计算所有偏心舵轮的转向角^Aβi,r和前进速度vi,r。如此，偏心舵轮装置的伺服电机就可按照求解的转向角和前进速度驱动AGV移动。

其中，轨迹跟踪控制器模块计算轨迹跟踪偏差以及修正后的运动指令包括以下步骤：

S1：由AGV车体参考点处运动轨迹{Path}给出的目标位姿[xt,yt,αt]和当前时刻AGV车体的实际位姿[xa,ya,αa]以及当前时刻AGV车体的转向角^Aβa计算轨迹跟踪偏差[xe,ye,αe]：

S2：由AGV车体参考点处运动轨迹{Path}给出的位置平动速度矢量姿态转动速度vα,t以及当前时刻AGV车体的实际姿态转动速度vα,a和轨迹跟踪偏差[xe,ye,αe]计算修正后的运动指令即位置平动速度指令和姿态转动速度指令vα，r：

其中，k1和k2为增益系数，经验值。该公式由经典反馈控制方法反步法推导得出。

参考附图6，前述位姿解耦运动学模块根据修正后的运动指令计算所有偏心舵轮的转向角^Aβi,r和前进速度vi,r包括以下步骤：

P1：首先，计算AGV车体运动的速度瞬心。AGV车体沿轨迹{Path}的位置平动与姿态转动的复合运动为平面刚体运动，根据平面刚体运行性质由位置平动速度和姿态转动速度vα,r按照如下公式求解其刚体运动瞬心O＝[xO,yO]^T：

公式中，为从运动瞬心O指向AGV车体参考点A的平面向量，可由平面刚体运动性质求解得到：其中，‘×’表示向量叉积。

P2：将转向轴中心Oi在AGV车体坐标系{A}下的坐标值[^Axi,^Ayi]转换到地面直角坐标系{W}下的[xi,yi]，然后计算偏心舵轮转向中心Oi处的平动速度矢量

因为偏心舵轮装置的转向轴5与AGV车体固连，其与AGV车体一起满足平面刚体运动性质。对于平面刚体运动，由AGV车体运动的速度瞬心和姿态转动速度即可求解AGV车体上任意固连点处的速度，包括大小和方向；由速度方向就可计算偏心舵轮装置的转向角。则可由转向轴中心Oi处的平面运动速度矢量得到该偏心舵轮装置的转向角^Aβi,r。计算偏心舵轮装置中转向轴处速度方向之前，需要先将转向轴中心Oi在AGV车体坐标系{A}下的坐标值[^Axi,^Ayi]转换到地面直角坐标系{W}下的[xi,yi]：

然后，计算转向轴中心Oi处的平面运动速度矢量

其中，‘×’表示向量叉积；为从运动瞬心O指向转向轴中心Oi的平面向量，

P3：计算偏心舵轮的转向角^Aβi,r和偏心舵轮的前进速度vi,r。

其中，偏心舵轮的转向角偏心舵轮的前进速度

因为偏心舵轮装置的驱动轮3可以围绕其转向轴旋转，所以其与AGV车体之间不满足平面刚体运动性质。驱动轮3与地面接触点相对于AGV车体是变化的(在AGV车体上投影为以转向轴为中心、以偏心值为半径的圆)，所以偏心舵轮装置的驱动轮3速度不等于转向轴中心Oi的平动速度矢量，而是存在一个偏心补偿量。该偏心补偿量有两个部分组成，一部分为姿态转动速度vα,r以及位置平动速度矢量的转向角速度ωOi,r与偏心bi相互作用的补偿量；另一部分为偏心舵轮从上一时刻的转向角运动到当前时刻的转向角而引起的偏心补偿量。而在计算转向角速度ωOi,r之前需要先计算转向轴处的平动加速度矢量

由AGV偏心舵轮转向轴处的平动速度矢量和平动加速度矢量计算其平动速度矢量的转向速度ωOi,r：

上述的轨迹跟踪控制方法可以适用于由任意多个偏心舵轮构成的AGV系统，对任意给定的AGV轨迹，分别求解每个偏心舵轮的转向角^Aβi,r和前进速度vi,r，使AGV可以在沿任意曲线前进的同时任意旋转车体姿态，从而实现AGV位置与姿态的解耦(即姿态随时间的变化是独立的，位置与姿态之间不存在耦合)。

在AGV系统的底部设有用于检测地面规则参照物的视觉相机以及相应的补光灯。视觉相机拍摄地面信息以得到AGV系统在地面坐标系下的位姿(位置与姿态)。在AGV系统上设有IMU或里程计，用于采集AGV系统在移动过程中的移动数据。由此，通过上述的视觉相机和IMU或者视觉相机和里程计，本发明还提供了一种组合导航定位方法，基于具有规则参照物的地面，适用于如舞台等应用场景。其具有规则参照物的地面是指规则地铺设于地面的参照物如木地板、瓷砖贴纸等任意可拼接材质或可构成直线网格的喷涂性、粘贴性的标记，如附图7和8所示。

组合导航定方法包括以下步骤：

步骤U、建立坐标系：将步骤B所建立的平面直角坐标系{W}(以所要应用场景中的地面的任意一点为原点建立)划分为整备区和工作区两个部分。在整备区粘贴一系列具有位姿信息的二维码。上述的二维码自身信息及其对应的地面平面直角坐标系{W}下的位姿信息[xq,yq,αq]均保存在AGV中控制器的配置文件。

步骤V、设置配置文件：将规则参照物的尺寸信息输入至AGV的控制器的配置文件。

参见附图9，将地面参照物(如地板、瓷砖等)的长宽[lf,wf]以及长度方向(人为指定)与地面平面直角坐标系{W}的X轴方向夹角αf保存到控制装置的配置文件；AGV控制装置根据该配置文件自动生成网格地图，即对应于地面网格线的直线族：{M}＝{LX}∪{LY}。

步骤W、AGV初始化：AGV上电后手动操控其经过二维码标识，视觉相机扫描二维码标识并通过控制装置计算得到AGV在地面平面直角坐标系中的绝对位置信息完成AGV初始化。

具体地，AGV在整备区内上电后，由操作员手动操控AGV经过粘贴有二维码的区域，视觉相机扫描到二维码后控制装置通过配置文件自动计算AGV在地面平面直角坐标系{W}中的绝对位置信息[xa,ya,αa]，完成AGV的初始化。

步骤X、组合导航定位：参见附图10，AGV在初始化完成后的运动过程中，视觉相机采集参照物信息，控制装置将采集到的参照物信息利用平面几何关系求解此时AGV位姿数据、并将得到的AGV位姿数据与IMU或里程计数据通过卡尔曼滤波算法进行融合，完成组合导航定位。

AGV初始化完成后的一段短暂运动过程中，在视觉相机的采集不到有效地图信息的短时间隔内，由IMU或里程计负责此时AGV的导航定位。

AGV初始化完成后的运动过程中，当视觉相机采集到有效地图信息(如地板或瓷砖的缝隙、网格线等)后，控制装置利用平面几何关系求解此时AGV可能的位姿数据，并将其与IMU或里程计数据通过卡尔曼滤波算法进行融合，完成组合导航定位。

具体地，控制装置将采集到的参照物信息利用平面几何关系求解AGV位姿数据包括以下步骤：

X11：读取视觉相机采集到的地面图像。

X12：经过灰度化、去噪、增强等处理后检测图像中的直线，如果检测到直线存在，通过相机内参单应矩阵H求出图像中所有直线在视觉相机坐标系{C}下的方程{Li}＝{H·si,1+t·(H·si,1-H·si,2)|t∈R}；其中si,1和si,2是检测出来的第i条直线的两个端点，t为实数形参。如果没有检测到直线存在，则说明此时是AGV处于初始化完成后的短暂运动过程，由IMU或里程计负责此时AGV的导航定位。

X13：由IMU或里程计数据确定网格地图中网格线的直线族{M}＝{LX}∪{LY}中位于视觉相机视野内且距离其镜头原点最近的两条直线{Lx}和{Ly}。

具体求解过程如下：

网格线的直线族{LX}为平行线，由以下参数确定：平行线中任意一条直线上的一点s；平行线的方向向量v；平行线之间的距离w。设定与方向向量v垂直的向量为V，则距离点s为w的整数倍且与点s连线垂直于v的点s+n·w·V一定位于平行线族{LX}中的某条直线上。

平面上任意点p与平行线族{LX}的距离可表示为：d＝(p-s)·V/|p-s|，其中等号右侧为向量计算。

如此，距离的平方d²是关于整数n的二次曲线即开口向上的抛物线，由抛物线只存在一个顶点的性质可得到d²的最小值以及其对应的整数n，进而得到点s+n·w·V，该点与方向向量v组成的直线就是直线族{LX}中距离点p最近的直线。如果上述点p为视觉相机原点，即可求解地图网格线直线族{M}＝{LX}∪{LY}中距离相机原点最近的直线{Lx}和{Ly}。

X14：求解所有直线方程{Li}在视觉相机镜头坐标系{C}下的斜率θi与截距di；以及直线{Lx}和{Ly}在视觉相机镜头坐标系{C}下的斜率θx、θy与截距dx、dy。

X15：按照设定的阈值θt和dt将所有直线分成两组并排除错误直线，得到两组分别对应于{Lx}和{Ly}的检测直线{Lxi|i＝1,2,…,m}和{Lyi|i＝1,2,…,n}，

其中，Li∈{Lxi}，if |θi-θx|≤θt∩|di-dx|≤dt

Li∈{Lyi}，if |θi-θy|≤θt∩|di-dy|≤dt。

X16：对每个方向上检测出的直线以斜率差和截距差的加权和si分配每条直线的权重qi：

si＝p×|θi-θx|+(1-p)×|di-dx|

X17：对每个方向上检测出的直线以斜率的加权均值和截距的加权均值作为该方向上最终的检测直线{Lx_d}和{Ly_d}。

X18：由齐次变换关系计算出由视觉相机确定的AGV当前位姿观测值其中，^ATC为相机{C}在车体{A}上的安装位姿。

视觉相机观测得到的AGV位姿数据与IMU或里程计数据通过卡尔曼滤波算法进行融合包括以下步骤：

X21：以IMU或里程计数据作为状态量X，X＝[x,y,α]^T。

X22：一阶牛顿运动学作为状态方程：Xk＝Xk-1+Vk·ΔT+ωk，其中，Vk为第k个周期的AGV速度，ΔT为周期时长，ωk为过程噪声，其协方差矩阵为Q，初值状态X0由二维码检测给出。

X23：建立观测方程(视觉相机拍摄地面图像并求解AGV位姿数据的过程是对状态量的观测)：Zk＝Xk+υk，其中，υk为观测噪声，其协方差矩阵为R；上述步骤将数据融合问题以状态空间模型(State-space Model)的形式在状态空间中进行描述。

X24：利用状态方程来预测下一时刻的状态，即一步预测：

其中，是一步预测结果的协方差矩阵，描述了的误差分布，其初值P0由经验值给出。

X25：然后，利用传感器对系统状态的观测值对系统状态的预测值进行纠正，即状态更新：

其中，Kg为卡尔曼增益，

通过将视觉相机安装在AGV车体内部并向下拍摄地面，采集二维码以及地板、瓷砖等地面图像信息；控制装置处理二维码信息得到AGV的初始位姿；在视觉相机两次采集地面信息的间隔时间内，由IMU或里程计负责处理AGV短时局部导航定位；当视觉相机采集到有效地面信息后，控制装置通过卡尔曼滤波算法融合视觉与IMU或里程计数据纠正定位误差，完成组合导航定位。

最后应当说明的是：以上实施例仅用以说明本发明的技术方案而非对其限制；尽管参照较佳实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者对部分技术特征进行等同替换；而不脱离本发明技术方案的精神，其均应涵盖在本发明请求保护的技术方案范围当中。