CN110989526A - 一种双agv协同运载控制系统及方法 - Google Patents

Abstract

本发明公开一种双AGV协同运载控制方法及系统，涉及智能物流运载机器人领域。该方法主要针对大部件，特别是长度方向具备较大尺寸物件的输送需求，结合路径跟踪方法及领航‑跟随方法，以三层拓扑结构的协同规划模型，结合双AGV领航‑跟随策略下的队形偏差及路径跟踪下的路径偏差，获得一种基于领航‑跟随和路径跟踪的运动学控制模型，并采用基于时域滚动预测控制的离散控制模型对该运动学控制模型进行优化，实现双AGV系统稳定可靠的路径跟踪协同运载。

Description

一种双AGV协同运载控制系统及方法

技术领域

本发明涉及一种面向双AGV协同运载系统的控制系统及方法，尤其涉及一种针对大型物件输送的双AGV协同运载控制方法，属于智能化工业机器人领域。

背景技术

随着工业自动化水平的提高，传统采用的长距离AGV自动化物流方式已逐渐无法满足生产制造、物流转运派送等工作。在大型装备的总装，以及码头集装箱运载等场景中，目前仍大多采用龙门吊、塔式起重机等传统输送系统进行大型物件的运送，存在着占用空间大，能耗高，系统可拓展性有限等诸多局限性。如位于上海的C919总装车间，在进行机翼机身等的对接工作时，仍采用位于车间上部的轨道起重机进行机身段及部装机翼的输送和对接。而位于上海洋山港及厦门远海自动化码头的集装箱自动化运送，则采用了增大单台AGV体型的方法实现大型集装箱的自动化物流。

采用多个AGV组成多移动机器人系统，使之在自动化运行的同时，保持固定队形，由此便具备了单个AGV所没有的运载能力。基于多智能体协作的模式，采用多AGV进行物流协同搬运的模式，有着以下优点：1.便于对AGV的管理；2.提高物流运送系统稳定性和鲁棒性；3.拓展AGV的利用领域，使其运载方式更加多样化；4.提高AGV的运载利用率；5.系统具备较高的安全性、鲁棒性及灵活性。多AGV在运行过程中进行协调同步运作和队形保持，实现物料等的多AGV协同搬运，将大大改善目前大体积、大重量物料自动化输送的应用现状，同时进一步提高AGV的利用率，优化物流资源的配置，具备广阔的应用场景。

当下，对于多机器人队形控制方法，国内外学者提出了基于行为法、人工势场法、虚拟结构保持法、分布式控制法、循环法、领航-跟随法等诸多理论方法。其中，领航- 跟随法因可靠性高，扩展性好，成为被广泛采用的一种多机器人队形控制及队形保持方法。基于领航-跟随的方法，国内外众多专家学者对于多机器人系统的导航构建及运行控制领域进行了全方位的拓展和研究。然而，应用于工业现场的多机器人技术需要针对实际设计。目前，设计可用于工业现场多AGV协同搬运系统的可实现方案仍然较少，对于多AGV协同搬运技术的应用仍不完善。公开号为CN10418899A的中国发明专利提出一种双移动机器人柔性连接的协同搬运系统及其复合导航装置，但在该专利中仅涉及了双移动机器人的机械结构，并不涉及控制方法方面的描述。而区别于公开号为 CN109062150A的中国发明专利提出的一种基于AGV的自动搬运系统及其多AGV协作方法中提及的多AGV协作方法，该种协作方法是在单AGV运行系统基础上形成的AGV 运行任务规划、调度管理，单一时刻控制主体依旧为单台AGV，而本发明设计目的为双 AGV协同运载控制方法，在同一时刻的控制主体为双AGV，以此来形成双AGV对同一物体的协作同步运载。

因此，综合上述对多移动机器人应用领域的发展现状及其广阔的应用价值和实用前景，本发明设计人以路径跟踪为基础，结合领航-跟随的多智能体基本架构，提出一种面向双AGV协同运载的控制方法，进一步拓展AGV在产业上的利用价值。

发明内容

本发明为解决现有技术问题，针对大部件自动化运送的场景，面向双AGV柔性连接前后布局的方式，参考领航-跟随策略，提出了一种双AGV协同运载的控制方法，用于双AGV协同运载系统队形保持及自动化运行的实现。

本发明构建了一种双AGV协同运载控制方法，采用了两台全向移动AGV，结合路径跟踪方法及领航-跟随方法，以三层拓扑结构的协同控制模型，建立一种在路径跟踪下的双AGV协同运载系统的运动学控制模型，并采用了基于时域滚动预测控制的离散控制模型对该运动学模型进行运动控制优化，实现双AGV系统稳定可靠的协同运行。

在本发明中，一种双AGV协同运载控制方法，双AGV为全向移动，前后布局，前者为领航AGV，后者为跟随AGV，该方法包括如下步骤：

步骤1：采集t时刻的领航AGV的路径距离偏差e_x1(t)、路径角度偏差e_θ1(t)、相对于工件的队形角度偏差α₁和车轮转速，以及跟随AGV的路径距离偏差e_x2(t)、路径角度偏差e_θ2(t)、队形距离偏差ΔL(t)、相对于工件的队形角度偏差α₂和车轮转速；

步骤2：根据领航AGV和跟随AGV的车轮转速，求得领航AGV的侧移速度分量v_x1、前进速度分量v_y1及角速度w₁，跟随AGV侧移速度分量v_x2、前进速度分量v_y2及角速度 w₂；

步骤3：令

其中，

将e_x1(t)、e_θ1(t)、 e_x2(t)、e_θ2(t)、ΔL(t)、v_y1、α₁、α₂输入运动学控制模型，得到时域t到t+T时段内的估计输入向量

表达式；

步骤4：根据t时刻领航AGV及跟随AGV的速度状态和偏差状态、估计输入向量

以及偏差变化模型，采用时域滚动预测控制的优化策略，获得优化输入向量

步骤5：将优化输入向量

解算成领航AGV及跟随AGV各轮的转速输出量，并发往驱动器驱动领航AGV及跟随AGV的电机运转，从而进行双AGV协同系统的路径跟踪协同运载运行。

进一步地，步骤3中所述，用于获得估计输入向量

的运动学控制模型为：

L：

F：

进一步地，步骤4中所述的偏差变化模型具有如下形式：

L：

F：

ΔL(t+1)＝ΔL(t)+T(v_y1cosα₁-v_x1sinα₁+v_x2sinα₂+v_y2cosα₂)。

进一步地，步骤6中描述的领航AGV的终端罚函数的形式如下：

代价函数的形式如下：

L(e₁(t)，u₁(t))＝e₁(t)^TQe₁(t)+u₁(t)^TRu₁(t)

其中，e₁(t)＝[e_x1(t) e_θ1(t)]^T，u₁(t)＝[v_x1(t) w_θ1(t)]^T，权值矩阵Q与R则为半正定的对称矩阵；设置代价函数权值矩阵Q和R如下：

且

则，

L(e₁(t)，u₁(t))＝q_x1e_x1(t)²+q_θ1e_θ1(t)²+r_x1v_x1(t)²+r_θ1ω₁(t)²

跟随AGV模型的终端罚函数的形式如下：

代价函数的形式如下：

L(e₂(t)，u₂(t)，ΔL(t))＝q_x2e_x1(t)²+q_e2e_e1(t)²+q₃ΔL(t)²

+rx₂v_x2(t)²+r_θ2ω₂(t)²+ry₂v_y2(t)²

进一步地，在步骤4中，采用时域滚动预测控制的优化策略，于t时刻将求解目标函数H的二次规划问题获得的优化输入向量

作为当前时域(t，t+δ)内的控制量对系统进行控制，具体步骤包括：

步骤4.1：估计输入向量

代入权利要求3所述的偏差变化模型预测t+T时刻的领航偏差e_x1(t+T)及e_θ1(t+T)，和跟随AGV偏差e_x2(t+T)、e_θ2(t+T)及ΔL(t+T)，并代入获得终端罚函数G。

步骤4.2：由t时刻领航AGV偏差状态e_x1(t)及e_θ1(t)和跟随AGV相关偏差状态e_x2(t)、 e_θ2(t)及ΔL(t)，结合估计输入向量

获得代价函数L。

步骤4.3：设置目标函数H＝G+L，求解H关于参数k_i的二次规划问题，将求解获得的k_i代入获得领航AGV与跟随AGV的优化输入向量

步骤4.4：在控制时段(t，t+δ)内，令真实输入向量

其中0<δ≤T；

步骤4.5：当双AGV系统运行到t+δ时刻后，将时刻t更新为t+δ，重复步骤1到步骤5。

基于上述方法的双AGV协同运载控制系统，包括传感通讯层、数据融合处理层和运动控制层；所述运动控制层包括领航AGV控制器和跟随AGV控制器，领航AGV控制器和跟随AGV控制器分别连接领航AGV和跟随AGV的车轮电机驱动器；所述传感通讯层用于监测双AGV的路径偏差、队形偏差和车轮转速，并将所监测信息传送给数据融合处理层，数据融合处理层对信息进行融合解算处理，得到领航AGV及跟随AGV 的速度及角速度输入量，并将领航AGV及跟随AGV的速度及角速度输入量分别发送给领航AGV控制器和跟随AGV控制器；所述领航AGV控制器和跟随AGV控制器分别对所接收的速度及角速度输入量进行反解，得到各车轮转速，并将转速命令发送至电机驱动器。

进一步地，领航AGV与跟随AGV之间为运载工件；领航AGV与运载工件的连接处安装有角度传感器用于测量领航AGV与工件之间的队形角度偏差，跟随AGV与运载工件的连接处安装有角度传感器与位移传感器测量跟随AGV与工件之间的队形角度偏差与队形距离偏差；领航AGV和跟随AGV车身中部分别安装有垂直向下的视觉识别模块，用于测量路径角度偏差及路径距离偏差；领航AGV和跟随AGV车轮处分别安装有编码器采集车轮转速信息。

有益效果：本发明提供了一种双AGV协同运载控制方法，应用于前后布局的双AGV系统进行路径跟踪的协同运作场合。以双AGV前后安置为基本布局，在领航-跟随模式的基础上结合视觉导引路径跟踪的方式，双AGV系统协同运作的同时保证了自动化路径跟踪的稳定性。引入基于时域滚动预测控制的方法，对双AGV的时间离散化控制序列进行优化，得到了更优化的双AGV协同路径跟踪运行的控制。本发明提供的方法，具有纠偏收敛快，路径跟踪平稳，队形保持稳定等优点。是为一种面向双AGV前后布局结构的协同运载控制方法，进一步拓展了AGV的应用场合和配置优化，打开了多AGV 协同运载方式的新思路。

附图说明

图1为本发明所述的一种双AGV协同运载控制方法对应的双AGV布局结构简图。

图2为本发明所述的三层拓扑结构协同控制模型。

图3为本发明所述的传感通讯层星形布局通讯网络。

图4为本发明所述的总体控制方案流程图。

图5为本发明所述的基于时域滚动预测时间离散示意图。

具体实施方式

下面结合附图对本发明的具体实施方式作进一步的说明。

一种双AGV协同运载控制方法，由两台全向移动AGV前后布局组成，在参考现有的多智能体编队控制方法的基础上，采用异构的领航-跟随方法，结合视觉导引路径跟踪方法，构建三层拓扑结构的协同控制模型，在路径偏差和队形偏差的基础上建立一种在路径跟踪及领航-跟随下的双AGV运动学控制模型，并采用了基于时域滚动预测控制的离散控制模型对该运动学控制模型进行优化，实现双AGV系统稳定可靠的协同运行。

结合附图1，本发明所述的双AGV协同运载控制方法中，采用的AGV为全向移动AGV，双AGV为前后布局，以系统前AGV作为领航AGV(7)，后AGV作为跟随AGV (1)，领航AGV与跟随AGV之间为运载工件(3)；领航AGV与运载工件之间采用具备转动副的转动柔性连接(8)，运载工件与跟随AGV之间采用具备转动副和移动副的组合柔性连接(4)；在领航AGV与运载工件的转动柔性连接(8)处安装有角度传感器用于测量领航AGV与工件之间的队形角度偏差，在跟随AGV与运载工件组合柔性连接处安装有角度传感器与位移传感器测量跟随AGV与工件之间的队形角度偏差与队形距离偏差；领航AGV车身中部有垂直向下的视觉识别模块(6)识别路径(5)，跟随 AGV车身中部安装有垂直向下的视觉识别模块(4)识别路径；领航AGV车轮处安装有编码器(9)采集车轮转速信息，跟随AGV车轮处安装有编码器(10)采集车轮转速信息；领航AGV的主要功能为进行路径跟踪前进以及与上位机系统交换指令信息的功能，跟随AGV在路径跟踪前进的过程中根据传感器的数据及领航AGV反馈的数据进行实时调整控制。

本发明所述的双AGV协同运载控制方法，包括下述步骤：

步骤1：采集t时刻的领航AGV的路径距离偏差e_x1(t)、路径角度偏差e_θ1(t)、相对于工件的队形角度偏差α₁和车轮转速，以及跟随AGV的路径距离偏差e_x2(t)、路径角度偏差e_θ2(t)、队形距离偏差ΔL(t)、相对于工件的队形角度偏差α₂和车轮转速；

步骤2：根据领航AGV和跟随AGV的车轮转速，求得领航AGV的侧移速度分量v_x1、前进速度分量v_y1及角速度w₁，跟随AGV侧移速度分量v_x2、前进速度分量v_y2及角速度 w₂；

步骤3：令

其中，

将e_x1(t)、e_θ1(t)、 e_x2(t)、e_θ2(t)、ΔL(t)、v_y1、α₁、α₂输入运动学控制模型，得到时域t到t+T时段内的估计输入向量

表达式；

步骤4：根据t时刻领航AGV及跟随AGV的速度状态和偏差状态、估计输入向量

以及偏差变化模型，采用时域滚动预测控制的优化策略，获得优化输入向量

步骤5：将优化输入向量

解算成领航AGV及跟随AGV各轮的转速输出量，并发往驱动器驱动领航AGV及跟随AGV的电机运转，从而进行双AGV协同系统的路径跟踪协同运载运行。

结合附图各步骤展开详细叙述。

结合附图4总体方案控制流程图，在步骤1中，t时刻，通过地面安置的色带标识路径，各AGV上的视觉模块识别路面的色带获得领航AGV相对于路径的路径距离偏差e_x1和路径角度偏差e_θ1，跟随AGV相对于路径的路径距离偏差e_x2和路径角度偏差e_θ2；通过安装于工件与领航AGV连接处的角度传感器获得领航AGV相对于工件的队形角度偏差α₁，通过安装于工件与跟随AGV连接处的角度传感器与位移传感器获得跟随AGV 相对于工件的队形角度偏差α₂和队形距离偏差ΔL；通过安装于领航AGV及跟随AGV 驱动轮处的速度编码器获得双AGV各轮的转速。

在步骤2中，由双AGV各轮的转速，通过麦克纳姆轮全向移动机器人的正运动学方程获得当前时刻领航AGV速度v_x1、v_y1及角速度w₁，跟随AGV速度v_x2、v_y2及角速度w₂。求解模型如下：

R为麦克纳姆轮的半径；L为轮子到AGV中心车长方向的距离，即为车长的1/2；W 为轮子车体中心车宽方向的距离，即为车宽的1/2。

在步骤3中，于融合解算层进行当前偏差及AGV速度角速度状态到双AGV控制量的转化，令

其中，

将e_x1(t)、e_θ1(t)、e_x2(t)、e_θ2(t)、ΔL(t)、v_y1、α₁、α₂输入运动学控制模型，得到时域t到t+T时段内的估计输入向量

表达式。

采用的运动学控制模型如下：

L：

F：

在步骤4中，本发明对应的双AGV系统，采用领航AGV路径跟踪，跟随AGV进行路径跟踪并伴随双AGV系统内队形偏差补偿的总体方案，具有如下偏差变化模型：

L：

F：

ΔL(t+1)＝ΔL(t)+T(v_y1cosa₁-v_x1sina₁+v_x2sinα₂+v_y2cosa₂) (6)

将输入向量

代入上述偏差变化模型计算预测t+T时刻的领航AGV偏差 e_x1(t+T)及e_θ1(t+T)，和跟随AGV偏差e_x2(t+T)、e_θ2(t+T)及ΔL(t+T)。并代入获得终端罚函数G；由t时刻领航AGV偏差状态e_x1(t)及e_θ1(t)和跟随AGV相关偏差状态e_x2(t)、e_θ2(t)及ΔL(t)，结合估计输入向量

获得代价函数L。

在步骤4中，设置目标函数H为终端罚函数G及代价函数L的和，则有H＝G+L；求解H的二次规划问题，获得优化参数k_i，将求解获得的ki代入运动学控制模型获得领航AGV与跟随AGV的优化输入向量

在步骤4中，采用了时域滚动预测控制的优化策略，于t时刻将求解目标函数H的二次规划问题获得的优化输入向量

作为当前时域(t，t+δ)内的控制量对系统进行控制，具体步骤包括：

步骤4.1：估计输入向量

代入权利要求3所述的偏差变化模型预测t+T时刻的领航偏差e_x1(t+T)及e_θ1(t+T)，和跟随AGV偏差e_x2(t+T)、e_θ2(t+T)及ΔL(t+T)，并代入获得终端罚函数G。

步骤4.2：由t时刻领航AGV偏差状态e_x1(t)及e_θ1(t)和跟随AGV相关偏差状态e_x2(t)、 e_θ2(t)及ΔL(t)，结合估计输入向量

获得代价函数L。

步骤4.3：设置目标函数H＝G+L，求解H关于参数k_i的二次规划问题，将求解获得的k_i代入获得领航AGV与跟随AGV的优化输入向量

步骤4.4：在控制时段(t，t+δ)内，令真实输入向量

其中0<δ≤T；

步骤4.5：当双AGV系统运行到t+δ时刻后，将时刻t更新为t+δ，重复步骤1到步骤5。

下面结合附图4及附图5，对控制过程中的步骤4.1至4.3展开详细叙述，在预测控制中，滚动优化输入向量的目的在于使罚函数G(e_i(t+T))与代价函数L(e_i(t)，u_i(t))取和的目标函数H取到最小值，罚函数G(e_i(t+T))的输入e_i(t+T)由预测阶段得到，罚函数G应当为连续可微的正定函数。

参考上述方法，以领航AGV的运动学控制模型为例进行分析，可选择终端罚函数的形式如下：

相应地，设置代价函数如下：

L(e₁(t)，u₁(t))＝e₁(t)^TQe₁(t)+u₁(t)^TRu₁(t) (8)

其中，e₁(t)＝[e_x1(t) e_θ1(t)]^T，u₁(t)＝[v_x1(t) w_θ1(t)]^T，权值矩阵Q与R则为半正定的对称矩阵。设置代价函数权值矩阵Q和R如下：

且

则可得，

L(e₁(t)，u₁(t))＝q_x1e_x1(t)²+q_θ1e_θ1(t)²+r_x1v_x1(t)²+r_θ1ω₁(t)² (9)

问题转化为关于参数k_i的目标函数H＝G(e₁(t+T))+L(e₁(t)，u₁(t))的二次规划求解问题。式(2)中可得，偏差向量e₁(t+T)为与偏差向量e₁(t)与输入向量u₁(t)有关的值，t时刻偏差向量e₁(t)＝[e_x1(t) e_θ1(t)]^T则由该时刻传感器测量数据解析得到，为已知值。在(5)中，输入向量的表达式又是经由反馈控制系数k₁，k₂，以及系统t时刻偏差向量e₁(t)与速度v_y1(t)可得。因此，最终的优化目的变为寻求系数k₁，k₂的在函数H可获得最小值的最优解。最后，把解得的系数k₁，k₂代入得到输入向量

并用于(t，t+δ)时间段内的控制。

而对于跟随AGV模型，类似地设置终端罚函数：

设置代价函数：

L(e₂(t)，u₂(t)，ΔL(t))＝q_x2e_x1(t)²+q_θ2e_θ1(t)²+q₃ΔL(t)² (11)

+r_x2v_x2(t)²+r_θ2ω₂(t)²+r_y2v_y2(t)²

同样地求解优化解k₃，k₄，k₅，并代入(6)得到Follower的控制输入向量

并作用于时间段(t，t+δ)内。等到达时刻t+δ后重新上述步骤，并更新反馈控制系数k₃， k₄，k₅。

至此，上述内容即为双AGV协同运载系统内领航AGV及跟随AGV基于滚动时域预测控制的控制输入量求解过程。

结合附图4，在步骤5中，于运动控制层处，将优化输入向量

解算成领航AGV及跟随AGV各轮的转速输出量，并发往驱动器驱动领航AGV及跟随AGV的电机运转，从而进行双AGV协同系统的路径跟踪协同运载运行。在运动控制层中，领航 AGV及跟随AGV的控制器接收来自数据解算层的运动控制量，通过全向AGV逆运动学方程解算成领航AGV及跟随AGV各轮的转速输出量，并发往驱动器驱动领航AGV 及跟随AGV的电机运转，从而进行双AGV协同系统的路径跟踪协同运载运行。通过基于麦克纳姆轮的全向移动AGV运动学模型，将前述数据解算层获得的领航AGV的速度、角速度真实输入向量

转化为各轮的转速输入量，并通过控制器发往驱动器从而驱动领航AGV的电机运行；将前述数据解算层获得的跟随AGV速度、角速度真实输入向量

转化为各轮的转速输入量，并通过控制器发往驱动器从而驱动跟随AGV的电机运行。转化模型如下：

R为麦克纳姆轮的半径；L为轮子到AGV中心车长方向的距离，即为车长的1/2； W为轮子车体中心车宽方向的距离，即为车宽的1/2；

为领航AGV的y方向(即前进方向)的速度分量，系统运行时设定为固定值。

为AGV(i＝1为领航AGV，i＝2为跟随AGV)的各轮转速控制量。

为适用于双AGV协同运行构型的控制方法，本发明设计了三层拓扑结构模型。结合附图2，对本发明中的三层拓扑结构协同控制模型进行叙述。

在该模型中，以各传感器及通讯模块构成传感通讯层。其中，在各AGV上由CCD 摄像头及DSP处理器形成的视觉模块用于识别路径并分别获得领航AGV及跟随AGV 的路径角度偏差及路径距离偏差。由编码器获得领航AGV及跟随AGV各轮转速，每个AGV共四个编码器，分别安置在驱动四个麦克纳姆轮的电机输出轴处。由角度传感器获得领航AGV与工件之间的队形角度偏差以及跟随AGV与工件之间的队形角度偏差，由位移传感器获得队形距离偏差。由无线通讯模块及串口通讯方式构建双AGV协同运载系统内部的信息传输网络。其中，以领航AGV的无线通讯模块作为服务器端，跟随AGV、上位机及融合解算中心的无线通讯模块作为客户端接入，形成星形布局的通讯网络。三层拓扑结构模型中，以数据融合解算中心构成数据融合处理层，领航AGV 的路径偏差(距离、角度)、队形角度偏差及领航AGV速度、角速度信息由通讯层传送至融合解算中心，跟随AGV的路径偏差(距离、角度)、队形角度偏差、队形距离偏差及跟随AGV各速度、角速度信息由传感器采集并接收进融合解算中心，数据融合处理得到领航AGV及跟随AGV的速度及角速度输入量。以领航AGV及跟随AGV的控制器、电机、驱动器等执行机构构成运动控制层，通过解算得到的各AGV速度角速度输入量根据麦克纳姆轮全向AGV的运动学反解得到领航AGV及跟随AGV的各轮转速，发送速度指令经由驱动器控制电机驱动。以上述传感通信层、数据解算层及运动控制层为基本框架构成三层拓扑结构的协同控制模型。

在传感通信层中，通过系统内部构建的星形布局通信网络，如附图3所示，网络内部存在信息交互，其中实线箭头表示通过无线传输的数据流，虚线箭头表示通过串行总线传输的数据流。双AGV系统内的交互信息主要有：

(1)领航AGV与跟随AGV之间的AGV避障及启停信息交互，直接通过连接于各AGV控制器上的无线模块进行通讯，达到双AGV系统同步启停的功能；

(2)领航AGV与融合解算中心之间的领航AGV速度信息、偏差信息及控制量信息交互。领航AGV当时刻运行速度、路径偏差(角度、距离)及队形角度偏差，经由领航AGV控制器与融合解算中心之间的无线通讯发往融合解算中心；融合解算中心经过数学模型解算得到领航AGV的速度及角速度控制量，并经由无线通讯发往领航AGV 控制；

(3)领航AGV与上位机系统之间的状态及指令信息交互。领航AGV控制器发往上位机系统的信息为双AGV系统运行的状态信息，包括：双AGV系统的运行速度、避障信息、运行状态。上位机系统往领航AGV控制器发送指令信息和任务信息，用于控制整个双AGV系统的启停动作及任务接收执行；

(4)跟随AGV与融合解算中心之间存在信息交互。跟随AGV将当前运行的速度角速度通过串行总线发往融合结算中心；融合结算中心将解算后的跟随AGV速度、角速度控制量发往跟随AGV。

在附图2的数据融合处理层中，通过各AGV的路径角度偏差、路径距离偏差、队形角度偏差、队形距离偏差等信息，融合当前时刻领航AGV及跟随AGV的运动速度及角速度，在融合解算中心通过运动学模型获得双AGV速度及角速度控制量；引入基于时域滚动优化预测控制对双AGV运动学的时间离散控制进行优化，获得离散时刻的运动控制输入量并发往运动控制层的领航AGV控制器及跟随AGV控制器。

以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (7)

1.一种双AGV协同运载控制方法，其特征在于，所述双AGV为全向移动，前后布局，前者为领航AGV，后者为跟随AGV，所述方法包括如下步骤：

步骤1：采集t时刻的领航AGV的路径距离偏差e_x1(t)、路径角度偏差e_θ1(t)、相对于工件的队形角度偏差α₁和车轮转速，以及跟随AGV的路径距离偏差e_x2(t)、路径角度偏差e_θ2(t)、队形距离偏差ΔL(t)、相对于工件的队形角度偏差α₂和车轮转速；

步骤2：根据领航AGV和跟随AGV的车轮转速，求得领航AGV的侧移速度分量v_x1、前进速度分量v_y1及角速度w₁，跟随AGV侧移速度分量v_x2、前进速度分量v_y2及角速度w₂；

步骤3：令

其中，

将e_x1(t)、e_θ1(t)、e_x2(t)、e_θ2(t)、ΔL(t)、v_y1、α₁、α₂输入运动学控制模型，得到时域t到t+T时段内的估计输入向量

表达式；

步骤4：根据t时刻领航AGV及跟随AGV的速度状态和偏差状态、估计输入向量

以及偏差变化模型，采用时域滚动预测控制的优化策略，获得优化输入向量

步骤5：将优化输入向量

解算成领航AGV及跟随AGV各轮的转速输出量，并发往驱动器驱动领航AGV及跟随AGV的电机运转，从而进行双AGV协同系统的路径跟踪协同运载运行。

2.根据权利要求1所述的一种双AGV协同运载控制方法，其特征在于，所述运动学控制模型为：

3.根据权利要求1所述的一种双AGV协同运载控制方法，其特征在于，所述偏差变化模型为：

ΔL(t+1)＝ΔL(t)+T(v_y1cosα₁-v_x1sinα₁+v_x2sinα₂+v_y2cosα₂)。

4.根据权利要求1所述的一种双AGV协同运载控制方法，其特征在于，领航AGV的终端罚函数的形式如下：

代价函数的形式如下：

L(e₁(t),u₁(t))＝e₁(t)^TQe₁(t)+u₁(t)^TRu₁(t)

其中，e₁(t)＝[e_x1(t) e_θ1(t)]^T，u₁(t)＝[v_x1(t) w_θ1(t)]^T，权值矩阵Q与R则为半正定的对称矩阵；设置代价函数权值矩阵Q和R如下：

且

则，

L(e₁(t),u₁(t))＝q_x1e_x1(t)²+q_θ1e_θ1(t)²+r_x1v_x1(t)²+r_θ1ω₁(t)²

跟随AGV模型的终端罚函数的形式如下：

代价函数的形式如下：

L(e₂(t),u₂(t),ΔL(t))＝q_x2e_x1(t)²+q_θ2e_θ1(t)²+q₃ΔL(t)²+r_x2v_x2(t)²+r_θ2ω₂(t)²+r_y2v_y2(t)²。

5.根据权利要求1所述的一种双AGV协同运载控制方法，其特征在于，步骤4中，采用时域滚动预测控制的优化策略，于t时刻将求解目标函数H的二次规划问题获得的优化输入向量

作为当前时域(t，t+δ)内的控制量对系统进行控制，具体步骤包括：

步骤4.1：估计输入向量

代入权利要求3所述的偏差变化模型预测t+T时刻的领航偏差e_x1(t+T)及e_θ1(t+T)，和跟随AGV偏差e_x2(t+T)、e_θ2(t+T)及ΔL(t+T)，并代入获得终端罚函数G。

步骤4.2：由t时刻领航AGV偏差状态e_x1(t)及e_θ1(t)和跟随AGV相关偏差状态e_x2(t)、e_θ2(t)及ΔL(t)，结合估计输入向量

获得代价函数L。

步骤4.3：设置目标函数H＝G+L，求解H关于参数k_i的二次规划问题，将求解获得的k_i代入获得领航AGV与跟随AGV的优化输入向量

步骤4.4：在控制时段(t,t+δ)内，令真实输入向量

其中0<δ≤T；

步骤4.5：当双AGV系统运行到t+δ时刻后，将时刻t更新为t+δ，重复步骤1到步骤5。

6.基于权利要求1所述方法的双AGV协同运载控制系统，其特征在于，包括传感通讯层、数据融合处理层和运动控制层；所述运动控制层包括领航AGV控制器和跟随AGV控制器，领航AGV控制器和跟随AGV控制器分别连接领航AGV和跟随AGV的车轮电机驱动器；所述传感通讯层用于监测双AGV的路径偏差、队形偏差和车轮转速，并将所监测信息传送给数据融合处理层，数据融合处理层对信息进行融合解算处理，得到领航AGV及跟随AGV的速度及角速度输入量，并将领航AGV及跟随AGV的速度及角速度输入量分别发送给领航AGV控制器和跟随AGV控制器；所述领航AGV控制器和跟随AGV控制器分别对所接收的速度及角速度输入量进行反解，得到各车轮转速，并将转速命令发送至电机驱动器。

7.根据权利要求6所述的一种双AGV协同运载控制系统，其特征在于，领航AGV与跟随AGV之间为运载工件；领航AGV与运载工件的连接处安装有角度传感器用于测量领航AGV与工件之间的队形角度偏差，跟随AGV与运载工件的连接处安装有角度传感器与位移传感器测量跟随AGV与工件之间的队形角度偏差与队形距离偏差；领航AGV和跟随AGV车身中部分别安装有垂直向下的视觉识别模块，用于测量路径角度偏差及路径距离偏差；领航AGV和跟随AGV车轮处分别安装有编码器采集车轮转速信息。

Images