CN114326728A - 高安全裕度的单agv智能车库路径跟踪控制系统及方法 - Google Patents

Abstract

本发明提供了一种高安全裕度的单AGV智能车库路径跟踪控制系统及方法，AGV根据规划车速判断AGV处于低速工况还是高速工况，低速工况时，执行基于AGV运动学模型的控制策略，高速工况时，执行基于AGV动力学模型的控制策略，对转向系统进行控制调整，直至完成AGV整体的路径跟踪控制。本发明针对AGV特性选择合适的控制策略，满足不同工况下的路径跟踪控制需求，提高了AGV的整体综合性能；本发明在高速工况下的控制策略中设计前馈控制器，消除路径跟踪过程中横向控制的稳态误差；本发明通过约束AGV整体的实际位置与目标位置之间形成的偏差面积，实现对控制器输出的动态调整，从而使AGV可以达到更高的通过安全裕度。

Description

高安全裕度的单AGV智能车库路径跟踪控制系统及方法

技术领域

本发明属于智能车库中移动机器人自动驾驶技术领域，具体涉及一种高安全裕度的单AGV智能车库路径跟踪控制系统及方法。

背景技术

基于移动机器人(Automated Guided Vehicle，AGV)的智能车库(Robot StereoGarage，RIG)是融入现代化智能控制技术的立体车库，通过AGV搬运车辆在车库内移动，实现人车分离，降低了停车难度，提升了停车过程中的环保性与便利性。AGV路径跟踪控制技术是智能车库中AGV搬运车辆到指定停车位作业的关键技术。目前，AGV运行过程中的实际位置与目标位置的偏差以及AGV尺寸对整体通过性的影响，是智能车库建设中亟待解决的技术难题。

常见智能车库中存在较多的狭窄区域，在通过这些区域时，AGV极易与周围车辆或物体发生碰撞，从而造成整体通过安全裕度的下降，无法安全进入指定停车位。同时，路径跟踪控制中，传统的单一控制算法不能有效协调自主转向系统在不同工况下的控制需求，不同的工况应具有不同的控制目标和侧重点。在AGV行驶过程中存在不同的工况，在空旷区域行驶时，速度相对较快；在停车位附近时，速度需要降低。在低速工况下，AGV的运动学特性较为突出；在高速工况下，AGV的动力学特性对自身的运动状态影响较大。传统的单一路径跟踪控制算法中没有针对不同场景下的AGV特性来建立相应的模型，从而无法使AGV的综合性能达到最优。

发明内容

针对现有技术中存在不足，本发明提供了一种高安全裕度的单AGV智能车库路径跟踪控制系统及方法，提高了AGV的整体综合性能，使AGV对各类工况具有良好的通过性。

本发明是通过以下技术手段实现上述技术目的的。

一种高安全裕度的单AGV智能车库路径跟踪控制方法，具体为：

AGV根据规划车速判断AGV处于低速工况还是高速工况，在低速工况时，执行基于AGV运动学模型的控制策略，在高速工况时，执行基于AGV动力学模型的控制策略；

所述基于AGV运动学模型的控制策略，具体为：以车后轴为切点、AGV纵向车身为切线，通过控制方向盘转角δ，使AGV沿着一条经过目标路径的圆弧行驶；所述方向盘转角δ为：

其中：L为AGV轴距，α为车身与目标路径的夹角，p是调整系数，v_x是为AGV的纵向车速；

所述基于AGV动力学模型的控制策略，具体采用LQR控制器控制，所述LQR控制器的最终控制率为：

U(t)＝-KX(t)＝-R^-1B^TPX(t)

其中：K为状态反馈控制器，X(t)为误差e_rr在t时刻的值，R为控制器的加权矩阵，矩阵

P(t)为Riccati方程PA+A^TP-PBR^-1BP^T+Q＝0的解，C_αf是前轮的侧偏刚度，m为AGV整体质量，a是AGV质心到前轴的距离，I为AGV绕z轴的转动惯量；

所述基于AGV动力学模型的控制策略中设计前馈控制器，使得控制量输出u＝-KX+δ_q，且变量

前馈控制器以δ_q作为控制输出；

其中：b是AGV质心到后轴的距离，C_αr为后轮的侧偏刚度，θ_r为规划航向角，k₃为LQR控制器增益；

控制模块输出u^*＝γu+(1-γ)θ，对转向系统进行控制调整，直至完成AGV整体的路径跟踪控制；其中

k为路径曲率，θ是最优航向角。

进一步地，所述最优航向角是通过以下方式求得的：

令S’＝0，通过比较极值点和端点处函数值大小，得到e_θ的值，进而求得偏差面积最小情况下的最优航向角θ；

其中：目标函数

g(x)为全局坐标系下目标路径表达式，e_d为AGV实际位置与目标位置之间的横向偏差，e_θ为AGV实际位置与目标位置之间的航向角偏差。

进一步地，所述目标函数是根据AGV整体的实际位置和目标位置偏差面积与横向误差和航向角误差之间的数学关系建立的。

进一步地，所述AGV运动学模型为：

其中：θ为AGV航向角，(x，y)是AGV的质心，v是AGV的速度。

进一步地，所述AGV动力学模型为：

其中：v_x、v_y分别为AGV的纵向速度和横向速度，a_y为AGV的纵向加速度。

一种高安全裕度的单AGV智能车库路径跟踪控制系统，包括：

路径规划模块，基于车辆信息和车库环境信息，通过五次多项式生成目标路径；

控制策略切换模块，根据规划车速判断AGV处于低速工况还是高速工况；

控制模块，根据低速工况和高速工况执行控制策略；

误差计算模块，获取目标路径与实际路径的横向误差、纵向误差和航向角误差。

上述技术方案，还包括智能车库云端，用于存储各类车辆信息和车库环境信息。

上述技术方案，还包括信息接收模块，所述信息接收模块安装在AGV上，接收智能车库云端传输的信息，并发送给路径规划模块。

上述技术方案，还包括传感器模块，用于确定各时刻AGV的位置及车身姿态。

本发明的有益效果为：

(1)本发明针对不同工况选择合适的AGV控制策略，有效协调了自主转向系统在不同工况下的控制需求，在AGV低速工况下，执行基于AGV运动学模型的纯跟踪控制策略；在AGV高速工况下，执行基于AGV运动学模型的LQR控制策略，使得AGV整体综合性能达到最优；

(2)本发明在高速工况下的控制策略中设计了前馈控制器，消除了路径跟踪过程中横向控制的稳态误差，使得路径跟踪过程中可以达到更好的控制精度；

(3)本发明考虑行驶过程中AGV整体尺寸对通过性造成的影响，通过约束AGV整体的实际位置与目标位置之间形成的偏差面积，实现对控制器输出的动态调整，从而使AGV可以达到更高的通过安全裕度。

附图说明

图1为本发明所述高安全裕度的单AGV智能车库路径跟踪控制系统结构框图；

图2为本发明所述AGV运动学模型示意图；

图3为本发明所述AGV动力学模型示意图；

图4为本发明所述纯跟踪算法示意图；

图5为本发明所述AGV实际位置与目标位置的偏差面积示意图。

具体实施方式

下面结合附图以及具体实施例对本发明作进一步的说明，但本发明的保护范围并不限于此。

如图1所示，本发明一种高安全裕度的单AGV智能车库路径跟踪控制系统，包括智能车库云端、信息接收模块、路径规划模块、传感器模块、控制策略切换模型、控制模块和误差计算模块。

智能车库云端存储各类车辆信息，包括车辆尺寸信息、质心位置、轴距和车辆总质量，还存储智能车库实时更新的环境信息，为AGV搬运车辆进入指定车库的路径规划提供信息。

信息接收模块安装在AGV上，接收来自智能车库云端传输的信息。

路径规划模块基于信息接收模块传输的信息，通过五次多项式生成目标路径(目标路径提供了规划的AGV横向位置、规划的AGV纵向位置、规划的AGV航向角、路径各处的曲率、规划的AGV速度和规划的AGV加速度)。

传感器模块包含多种传感器，如速度传感器、加速度传感器、定位装置及可以感知AGV所搬运车辆尺寸及位姿的传感器，通过传感器采集的数据，确定各时刻AGV的位置及车身姿态(具体包括实际的AGV横向位置、实际的AGV纵向位置、实际的AGV航向角、实际的AGV速度和实际的AGV加速度)。

控制策略切换模块根据规划车速判断AGV处于低速工况还是高速工况，在低速工况时(速度≤0.8m/s)，执行基于AGV运动学模型的控制策略，在高速工况时(速度＞0.8m/s)，执行基于AGV动力学模型的控制策略。

低速工况时执行基于AGV运动学模型的情况下，控制模块采用纯跟踪算法控制；高速工况时执行基于AGV动力学模型的情况下，控制模块采用LQR控制算法，其中控制模块还包含为消除高速时横向稳态误差而设计的前馈控制器。

误差计算模块通过传感器模块反馈的数据与路径规划模块中的数据进行比对计算，得到横向误差、纵向误差和航向角误差。

本发明一种高安全裕度的单AGV智能车库路径跟踪控制方法，具体包括如下步骤：

步骤(1)，AGV搬运车辆前的准备

步骤(1.1)，智能车库云端接收待停车辆信息与车库环境信息，并将信息传输给AGV信息接收模块；

步骤(1.2)，AGV信息接收模块获得车库环境信息及车辆信息(车辆信息包括车辆尺寸信息、质心位置、轴距和车辆总质量)；

步骤(1.3)，AGV进入待搬运车辆车底，使得AGV的质心(x，y)与车辆质心(X_o，Y_o)在横、纵向的位置重合，即x＝X_o、y＝Y_o。

步骤(2)，AGV搬运车辆

步骤(2.1)，路径规划模块生成目标路径

根据步骤(1.2)接收的车库环境信息进行目标路径规划，规划的目标路径含有以下信息：全局坐标系下AGV在水平方向与垂直方向的位置、目标路径各处的曲率、AGV速度、AGV加速度和AGV航向角。

步骤(2.2)，建立AGV运动学模型

将AGV结构简化为自行车模型，即AGV两个前轮拥有相同的角度和转速，同样两个后轮也是如此。AGV运动学模型基础是建立在阿克曼转向几何理论上，理论假设：AGV转向时，四个车轮沿同一个转向圆心走圆弧，车轮处于纯滚动状态且没有侧向加速度，并且AGV在行驶过程中没有垂直方向的运动。基于运动学自行车模型，给定一个时刻的控制输入后，求得AGV的状态信息(坐标、航向角和速度)。因为低速条件下，认为AGV不会发生侧向滑动，因此可假设AGV横摆角近似等于AGV航向角。基于上述假设，建立起二自由度AGV运动学模型，如图2所示，并得到如下数学关系：

其中，L为AGV轴距，δ为AGV的方向盘转角。

步骤(2.3)，建立AGV动力学模型

忽略AGV的垂向运动、侧倾运动、俯仰运动，并且假设AGV的侧向加速度限定在0.4g以下，轮胎侧偏特性处于线性范围，AGV左右车轮侧偏角相同，建立AGV二自由度动力学微分方程：

其中，v_x、v_y分别为AGV的纵向速度和横向速度，I为AGV绕z轴的转动惯量，C_αf、C_αr分别为前轮和后轮的侧偏刚度，m为AGV整体质量，a、b分别为AGV质心到前后轴的距离，a_y为AGV的纵向加速度。AGV动力学模型参见图3。

步骤(2.4)，根据传感器模块获取行驶过程中AGV的实际位姿信息(包括实际的AGV横向位置、实际的AGV纵向位置、实际的AGV航向角、实际的AGV速度和实际的AGV加速度)。

步骤(2.5)，控制模块判断执行基于AGV运动学模型的控制策略还是基于AGV运动学模型的控制策略。

步骤(2.6)，低速工况下，控制模块执行基于AGV运动学模型的纯跟踪算法

从自行车模型出发，纯跟踪算法以车后轴为切点、AGV纵向车身为切线，通过控制方向盘转角δ，使AGV可以沿着一条经过目标路径的圆弧行驶，如图4所示，推导出下列转换式：

其中l_d为AGV的预瞄距离(l_d≥2.5m)，R是在给定的转向角下后轴遵循的圆的半径，α为车身与目标路径的夹角；化简公式(3)，得到：

由于方向盘转角δ近似等于α，所以：

其中L为AGV轴距；

根据公式(4)、(5)，得到纯跟踪算法控制量的最终表达式为：

将预瞄距离l_d表示成AGV纵向速度的线形函数，即方向盘转角：

其中调整系数p＝0.1；方向盘转角即控制输出。

步骤(2.7)，高速工况下，控制模块执行基于AGV动力学模型的控制策略

①建立基于全局坐标系的误差状态空间方程

将AGV实际位置与目标位置之间的偏差代入AGV二自由度动力学微分方程，并将结果改写成状态空间方程形式，得到误差状态空间方程：

矩阵

误差

矩阵

矩阵

其中，e_d为AGV实际位置与目标位置之间的横向偏差，e_θ为AGV实际位置与目标位置之间的航向角偏差，

和

分别为e_d和e_θ对时间的一阶导数，θ_r为规划航向角，u为控制量输出，且u＝δ，AGV转向系统接收到输入信号执行转向操作，进而对AGV的横向误差进行调节。e_d和e_θ均由误差计算模块通过规划的目标路径与实际位姿信息计算得到(图5中，e_d为AGV实际位置与目标路径上投影点的直线长度)。

②设计横向LQR控制器

LQR即线性二次型调节器，其对象是现代控制理论中以状态空间形式给出的线性系统，目标函数为对象状态和控制输入的二次型函数。LQR最优设计指设计出的状态反馈控制器要使二次型目标函数J取最小值，K由权矩阵Q与R唯一决定。LQR可得到状态线性反馈的最优控制规律，易于构成闭环最优控制且计算量小于常用的模型预测控制MPC，提高了AGV的控制效率。

根据①得到的全局坐标系下的误差状态空间方程，设计LQR控制器的系统性能指标函数：

其中：Q和R为控制器的加权矩阵，Q为半正定矩阵，R为正定矩阵，矩阵各项数值表示对积分项的不同重视程度；X(t)为e_rr在t时刻的值，U(t)为t时刻控制量输出值。

设计控制律使性能指标函数值达到最小。构造哈密顿函数，即：

其中λ为参数；

对公式(9)求导并求极值，得：

U*(t)＝R^-1Bλ(t) (10)

λ(t)＝-P(t)X(t) (11)

其中P(t)为Riccati方程PA+A^TP-PBR^-1BP^T+Q＝0的解。

LQR控制器的最终控制率为：

U(t)＝-KX(t)＝-R^-1B^TPX(t) (12)

控制器增益K＝[k₁，k₂，k₃，k₄]，对应控制器对e_d、

e_θ、

的重视程度。

③设计前馈控制器，消除稳态误差

将②得到的最优控制率U＝-KX代入误差状态空间方程，得

可见，系统存在稳态误差

因此，设计前馈控制器，消除横向控制中的稳态误差，使u＝-KX+δ_q，此时

系统稳定后，计算误差，得：

由上式可知：当变量

时，横向误差e_d＝0。前馈控制器以δ_q作为控制输出，消除路径跟踪过程中的横向稳态误差。

步骤(2.8)，建立AGV整体的实际位置和目标位置偏差面积(图5)与横向误差和航向角误差之间的数学关系，目标函数为：

其中g_t(x_t)表示t时刻全局坐标系下目标路径表达式；

为了简化计算，作以下近似：sine_θ＝e_θ，cos e_θ＝1，θ_e≈g′(-a)，经以下坐标系转换：

目标函数转换为：

其中：(x_t，y_t)为目标路径下AGV整体质心的水平方向和垂直方向，g(x)为全局坐标系下目标路径表达式。

最优航向角计算模块中，令S’＝0，通过比较极值点和端点处函数值大小，得到e_θ的值，进而求得偏差面积最小情况下的最优航向角θ。

步骤(2.9)，对最优航向角θ与低速工况或高速工况的输出进行加权处理，此时控制模块输出为u^*＝γu+(1-γ)θ，其中

且γ∈[0，1]，k为路径曲率。

步骤(2.10)，执行机构对AGV驱动系统以及制动系统执行加速或减速的操作，使AGV以目标车速行驶。

步骤(2.11)，根据步骤(2.9)中的控制器模块输出对转向系统进行控制调整，降低AGV行驶过程中的横向误差。

步骤(2.13)，重复上述步骤，完成AGV整体的路径跟踪控制过程。

所述实施例为本发明的优选的实施方式，但本发明并不限于上述实施方式，在不背离本发明的实质内容的情况下，本领域技术人员能够做出的任何显而易见的改进、替换或变型均属于本发明的保护范围。

Claims (9)

1.一种高安全裕度的单AGV智能车库路径跟踪控制方法，其特征在于：

AGV根据规划车速判断AGV处于低速工况还是高速工况，在低速工况时，执行基于AGV运动学模型的控制策略，在高速工况时，执行基于AGV动力学模型的控制策略；

所述基于AGV运动学模型的控制策略，具体为：以车后轴为切点、AGV纵向车身为切线，通过控制方向盘转角δ，使AGV沿着一条经过目标路径的圆弧行驶；所述方向盘转角δ为：

其中：L为AGV轴距，α为车身与目标路径的夹角，p是调整系数，v_x是为AGV的纵向车速；

所述基于AGV动力学模型的控制策略，具体采用LQR控制器控制，所述LQR控制器的最终控制率为：

U(t)＝-KX(t)＝-R^-1B^TPX(t)

其中：K为状态反馈控制器，X(t)为误差e_rr在t时刻的值，R为控制器的加权矩阵，矩阵

P(t)为Riccati方程PA+A^TP-PBR^-1BP^T+Q＝0的解，C_αf是前轮的侧偏刚度，m为AGV整体质量，a是AGV质心到前轴的距离，I为AGV绕z轴的转动惯量；

所述基于AGV动力学模型的控制策略中设计前馈控制器，使得控制量输出u＝-KX+δ_q，且变量

前馈控制器以δ_q作为控制输出；

其中：b是AGV质心到后轴的距离，C_αr为后轮的侧偏刚度，θ_r为规划航向角，k₃为LQR控制器增益；

控制模块输出u^*＝γu+(1-γ)θ，对转向系统进行控制调整，直至完成AGV整体的路径跟踪控制；其中

k为路径曲率，θ是最优航向角。

2.根据权利要求1所述的高安全裕度的单AGV智能车库路径跟踪控制方法，其特征在于，所述最优航向角是通过以下方式求得的：

令S’＝0，通过比较极值点和端点处函数值大小，得到e_θ的值，进而求得偏差面积最小情况下的最优航向角θ；

其中：目标函数

g(x)为全局坐标系下目标路径表达式，e_d为AGV实际位置与目标位置之间的横向偏差，e_θ为AGV实际位置与目标位置之间的航向角偏差。

3.根据权利要求2所述的高安全裕度的单AGV智能车库路径跟踪控制方法，其特征在于，所述目标函数是根据AGV整体的实际位置和目标位置偏差面积与横向误差和航向角误差之间的数学关系建立的。

4.根据权利要求1所述的高安全裕度的单AGV智能车库路径跟踪控制方法，其特征在于，所述AGV运动学模型为：

其中：θ为AGV航向角，(x，y)是AGV的质心，v是AGV的速度。

5.根据权利要求4所述的高安全裕度的单AGV智能车库路径跟踪控制方法，其特征在于，所述AGV动力学模型为：

其中：v_x、v_y分别为AGV的纵向速度和横向速度，a_y为AGV的纵向加速度。

6.一种根据权利要求1-5任一项所述的高安全裕度的单AGV智能车库路径跟踪控制方法的控制系统，其特征在于，包括：

路径规划模块，基于车辆信息和车库环境信息，通过五次多项式生成目标路径；

控制策略切换模块，根据规划车速判断AGV处于低速工况还是高速工况；

控制模块，根据低速工况和高速工况执行控制策略；

误差计算模块，获取目标路径与实际路径的横向误差、纵向误差和航向角误差。

7.根据权利要求6所述的控制系统，其特征在于，还包括智能车库云端，用于存储各类车辆信息和车库环境信息。

8.根据权利要求7所述的控制系统，其特征在于，还包括信息接收模块，所述信息接收模块安装在AGV上，接收智能车库云端传输的信息，并发送给路径规划模块。

9.根据权利要求6所述的控制系统，其特征在于，还包括传感器模块，用于确定各时刻AGV的位置及车身姿态。

Images