CN105068543A - 一种基于pid控制的背负型agv两轮同步方法 - Google Patents

Abstract

一种基于PID控制的背负型AGV两轮同步方法，该方法首先将利用驱动单元行走编码器采样得到驱动单元的位置和各自运动的转向角度，利用差分法获取驱动单元的线速度偏差与角速度偏差，进而采用改进型PID控制器使得两个驱动单元同步。本发明根据偏差控制双驱动背负式AGV的两个驱动单元，保证两个驱动单元自动跟踪同步。

Description

一种基于PID控制的背负型AGV两轮同步方法

技术领域

本发明应用于自动导航车控制领域，涉及一种适用于两轮驱动的自动导航车的两轮同步方法。

背景技术

AGV(AutomatedGuidedVehicle，自动导引小车)是现在自动仓储技术发展的趋势，是生产物流自动化先进性的一个重要体现，它具有柔性运输、高效运输、节能环保、安全可靠等功能，并且可以改善工作环境，大幅度节约劳动力成本，对提高生产自动化和生产效率有着重要的意义。

AGV车型可按运载类别、承载质量、驱动型式、导引方式、移载方式分类。比如按导引方式分类有：磁导引，激光导引，超声导引，光反射检测，惯性导航，图像识别与坐标识别等；按移载方式分类有：侧辊式，牵引式，跨鞍式，双侧推挽式，平衡式，双侧叉式，三向叉式，低叉式，双举升式，龙门式，背负式等。

AGV小车主要由车体系统、车载控制系统、动力装置及安全与辅助系统组成。车体系统是AGV的硬件构成，车载控制系统控制AGV的动作执行和导航，动力装置是AGV的运行动力来源。安全与辅助装置提供避障措施，保证AGV的安全和可靠性。

双驱动背负式AGV配备磁地标，可以进行站点停靠，非常适合在空间不宽裕的场合使用，也适用于特定的场合，如:电子厂或电子厂波峰焊流水线运送模板。AGV车载控制系统的主要作用是使驱动单元控制同步。驱动单元同步控制可实现AGV运行过程中保持平衡，具有适应地形变化能力强，移动轨迹灵活易变的特点，并保证驱动功率较小，为电池长时间供电提供了可能，有效地提高物流系统运行的可靠性和运行成本，故AGV中保证两个驱动单元同步的控制器设计尤为关键。

发明内容

为了克服现有双驱动背负式AGV实现的跟踪同步性较差的不足，本发明提供了一种基于PID控制的背负型AGV两轮同步方法，通过调节两个驱动轮的速度与角度姿态，获取两轮速度差与角度姿态差来调整驱动模块的姿态，从而实现跟踪同步。

为了解决上述技术问题本发明采用的技术方案如下：

一种基于PID控制的背负型AGV两轮同步方法，所述方法包括如下步骤：

1)、以T为采样周期，通过驱动单元行走编码器分别获取驱动单元的位置p1，p2和各自运动的转向角度θ1，θ2，其中p1，θ1分别指第一个轮的位置与转向角；p2，θ2分别指第二个轮的位置与转向角；

2)、定义驱动单元的线速度偏差e_{p_v}和角速度偏差e_{θ_v}为

$e_{p\_v}=\left[\begin{array}{c}{e}_{p1\_v}\\ e_{p2\_v}\end{array}\right]=\left[\begin{array}{ccc}{v}_{p1\_d}& -& v_{p1}\\ v_{p2\_d}& -& v_{p2}\end{array}\right],$

$e_{\mathrm{\θ}\_v}=\left[\begin{array}{c}{e}_{\mathrm{\θ}1\_v}\\ e_{\mathrm{\θ}2\_v}\end{array}\right]=\left[\begin{array}{ccc}{v}_{\mathrm{\θ}1\_d}& -& v_{\mathrm{\θ}1}\\ v_{\mathrm{\θ}2\_d}& -& v_{\mathrm{\θ}2}\end{array}\right].---\left(1\right)$

其中，e_{p1_v}和e_{p2_v}分别为第一个和第二个驱动单元线速度偏差，e_{θ1_v}和e_{θ2_v}分别为第一个和第二个驱动单元角速度偏差；v_{p1_d}，v_{p2_d}分别为第一个驱动单元与第二个驱动单元的输入期望线速度，v_{θ1_d}，v_{θ2_d}分别为第一个驱动单元与第二个驱动单元的期望角速度信号；v_p1，v_p2分别为第一个驱动单元与第二个驱动单元的实时输出线速度；v_θ1，v_θ2分别为第一个驱动单元与第二个驱动单元的实时角速度信号，且v_p1，v_p2，v_θ1，v_θ2分别由步骤1)中得到的驱动单元位置p1，p2和各自运动的转向角度θ1，θ2采用差分法得到；v_{p1_d}，v_{p2_d}，v_{θ1_d}，v_{θ2_d}输入期望值是根据系统性能要求预先设定的；

3)、根据驱动单元的线速度偏差e_{p_v}与角速度偏差e_{θ_v}，通过改进的PID控制器确定本地控制率u_v1(k)，u_θ1(k)，u_v2(k)，u_θ2(k)，即定义如下：

e_{v_t}＝v_p1-v_p2(2)

e_{θ_t}＝v_θ1-v_θ2

本地控制率u_v1(k)，u_θ1(k)，u_v2(k)，u_θ2(k)的取值如下：

$\begin{array}{c}{u}_{v1}\left(k\right)=K_{p1}{e}_{p1\_v}\left(k\right)+K_{l1}\underset{j=0}{\overset{k}{\Σ}}{e}_{p1\_v}\left(j\right)+K_{d1}\left(e_{p1\_v}\right(k)-e_{p1\_v}(k-1\left)\right)+\\ K_{p\_t}{e}_{v\_t}\left(k\right)+K_{l\_t}\underset{j=0}{\overset{k}{\Σ}}{e}_{v\_t}\left(j\right)+K_{d\_t}\left(e_{v\_t}\right(k)-e_{v\_t}(k-1\left)\right)\end{array}$

$\begin{array}{c}{u}_{\mathrm{\θ}1}\left(k\right)=K_{p1}^{\′}{e}_{\mathrm{\θ}1\_v}\left(k\right)+K_{l1}^{\′}\underset{j=0}{\overset{k}{\Σ}}{e}_{\mathrm{\θ}1\_v}\left(j\right)+K_{d1}^{\′}\left(e_{\mathrm{\θ}1\_v}\right(k)-e_{\mathrm{\θ}1\_v}(k-1\left)\right)+\\ K_{p\_t}^{\′}{e}_{\mathrm{\θ}\_t}\left(k\right)+K_{l\_t}^{\′}\underset{j=0}{\overset{k}{\Σ}}{e}_{\mathrm{\θ}\_t}\left(j\right)+K_{d\_t}^{\′}\left(e_{\mathrm{\θ}\_t}\right(k)-e_{\mathrm{\θ}\_t}(k-1\left)\right)\end{array}$

$\begin{array}{c}{u}_{v2}\left(k\right)=K_{p2}{e}_{p2\_v}\left(k\right)+K_{l2}\underset{j=0}{\overset{k}{\Σ}}{e}_{p2\_v}\left(j\right)+K_{d2}\left(e_{p2\_v}\right(k)-e_{p2\_v}(k-1\left)\right)\\ u_{\mathrm{\θ}2}\left(k\right)=K_{p2}^{\′}{e}_{\mathrm{\θ}2\_v}\left(k\right)+K_{l2}^{\′}\underset{j=0}{\overset{k}{\Σ}}{e}_{\mathrm{\θ}2\_v}\left(j\right)+K_{d2}^{\′}\left(e_{\mathrm{\θ}2\_v}\right(k)-e_{\mathrm{\θ}2\_v}(k-1\left)\right)\end{array}---\left(3\right)$

其中K_p1,K_l1,K_d1,K_{p_t},K_{l_t},K_{d_t},K'_p1,K′_l1,K'_d1,K'_{p_t},K′_{l_t},K'_{d_t},K_p2,K_l2,K_d2,K'_p2,K′_l2,K'_d2为给定的系统系数。e_{p1_v}(k)和e_{p2_v}(k)分别为当前时刻第一个和第二个主动轮线速度偏差，e_{θ1_v}(k)和e_{θ2_v}(k)分别为当前时刻第一个和第二个主动轮角速度偏差，e_{v_t}(k)为当前时刻第一个和第二个主动轮实时线速度的偏差，e_{θ_t}(k)为当前时刻第一个和第二个主动轮实时角速度的偏差。

与现有技术相比，本发明的优点在于：AGV的控制系统中PID控制器的输入分别由两主动轮线速度、角速度的期望值与各自实际值之间的偏差部分，以及两主动轮之间的实际线速度与实际角速度值的偏差部分构成。该方法中利用各自主动轮的线速度偏差与角速度偏差，使得两个主动轮跟踪上各自期望值的同时，连续的动态闭环控制亦能保证双驱动背负式AGV两个主动轮快速稳定自动跟踪同步。

附图说明

图1是AGV结构图。

图2是保证AGV驱动单元跟踪同步控制原理图。

图3是保证AGV驱动单元跟踪同步控制系统结构图。

具体实施方式

为了使本发明的技术方案、设计思路能更加清晰，下面结合附图再进行详尽的描述。

参照图1～图3，一种基于PID控制保证背负式AGV两个驱动单元的同步方法，所述方法包括如下步骤：

1)、以T为采样周期，通过驱动单元行走编码器分别获取驱动单元的位置p1，p2和各自运动的转向角度θ1，θ2，其中p1，θ1分别指第一个轮的位置与转向角；p2，θ2分别指第二个轮的位置与转向角；

2)、定义驱动单元的线速度偏差e_{p_v}和角速度偏差e_{θ_v}为

$e_{p\_v}=\left[\begin{array}{c}{e}_{p1\_v}\\ e_{p2\_v}\end{array}\right]=\left[\begin{array}{ccc}{v}_{p1\_d}& -& v_{p1}\\ v_{p2\_d}& -& v_{p2}\end{array}\right],$

$e_{\mathrm{\θ}\_v}=\left[\begin{array}{c}{e}_{\mathrm{\θ}1\_v}\\ e_{\mathrm{\θ}2\_v}\end{array}\right]=\left[\begin{array}{ccc}{v}_{\mathrm{\θ}1\_d}& -& v_{\mathrm{\θ}1}\\ v_{\mathrm{\θ}2\_d}& -& v_{\mathrm{\θ}2}\end{array}\right].---\left(1\right)$

其中，e_{p1_v}和e_{p2_v}分别为第一个和第二个驱动单元线速度偏差，e_{θ1_v}和e_{θ2_v}分别为第一个和第二个驱动单元角速度偏差；v_{p1_d}，v_{p2_d}分别为第一个驱动单元与第二个驱动单元的输入期望线速度，v_{θ1_d}，v_{θ2_d}分别为第一个驱动单元与第二个驱动单元的期望角速度信号；v_p1，v_p2分别为第一个驱动单元与第二个驱动单元的实时输出线速度；v_θ1，v_θ2分别为第一个驱动单元与第二个驱动单元的实时角速度信号，且v_p1，v_p2，v_θ1，v_θ2分别由步骤1)中得到的驱动单元位置p1，p2和各自运动的转向角度θ1，θ2采用差分法得到；v_{p1_d}，v_{p2_d}，v_{θ1_d}，v_{θ2_d}输入期望值是根据系统性能要求预先设定的；

3)、根据驱动单元的线速度偏差e_{p_v}与角速度偏差e_{θ_v}，通过改进的PID控制器确定本地控制率u_v1(k)，u_θ1(k)，u_v2(k)，u_θ2(k)，即定义如下：

e_{v_t}＝v_p1-v_p2(2)

e_{θ_t}＝v_θ1-v_θ2

本地控制率u_v1(k)，u_θ1(k)，u_v2(k)，u_θ2(k)的取值如下：

$\begin{array}{c}{u}_{v1}\left(k\right)=K_{p1}{e}_{p1\_v}\left(k\right)+K_{l1}\underset{j=0}{\overset{k}{\Σ}}{e}_{p1\_v}\left(j\right)+K_{d1}\left(e_{p1\_v}\right(k)-e_{p1\_v}(k-1\left)\right)+\\ K_{p\_t}{e}_{v\_t}\left(k\right)+K_{l\_t}\underset{j=0}{\overset{k}{\Σ}}{e}_{v\_t}\left(j\right)+K_{d\_t}\left(e_{v\_t}\right(k)-e_{v\_t}(k-1\left)\right)\end{array}$

$\begin{array}{c}{u}_{\mathrm{\θ}1}\left(k\right)=K_{p1}^{\′}{e}_{\mathrm{\θ}1\_v}\left(k\right)+K_{l1}^{\′}\underset{j=0}{\overset{k}{\Σ}}{e}_{\mathrm{\θ}1\_v}\left(j\right)+K_{d1}^{\′}\left(e_{\mathrm{\θ}1\_v}\right(k)-e_{\mathrm{\θ}1\_v}(k-1\left)\right)+\\ K_{p\_t}^{\′}{e}_{\mathrm{\θ}\_t}\left(k\right)+K_{l\_t}^{\′}\underset{j=0}{\overset{k}{\Σ}}{e}_{\mathrm{\θ}\_t}\left(j\right)+K_{d\_t}^{\′}\left(e_{\mathrm{\θ}\_t}\right(k)-e_{\mathrm{\θ}\_t}(k-1\left)\right)\end{array}$

$\begin{array}{c}{u}_{v2}\left(k\right)=K_{p2}{e}_{p2\_v}\left(k\right)+K_{l2}\underset{j=0}{\overset{k}{\Σ}}{e}_{p2\_v}\left(j\right)+K_{d2}\left(e_{p2\_v}\right(k)-e_{p2\_v}(k-1\left)\right)\\ u_{\mathrm{\θ}2}\left(k\right)=K_{p2}^{\′}{e}_{\mathrm{\θ}2\_v}\left(k\right)+K_{l2}^{\′}\underset{j=0}{\overset{k}{\Σ}}{e}_{\mathrm{\θ}2\_v}\left(j\right)+K_{d2}^{\′}\left(e_{\mathrm{\θ}2\_v}\right(k)-e_{\mathrm{\θ}2\_v}(k-1\left)\right)\end{array}---\left(3\right)$

其中K_p1,K_l1,K_d1,K_{p_t},K_{l_t},K_{d_t},K'_p1,K′_l1,K'_d1,K'_{p_t},K′_{l_t},K'_{d_t},K_p2,K_l2,K_d2,K'_p2,K′_l2,K'_d2为给定的系统系数。e_{p1_v}(k)和e_{p2_v}(k)分别为当前时刻第一个和第二个主动轮线速度偏差，e_{θ1_v}(k)和e_{θ2_v}(k)分别为当前时刻第一个和第二个主动轮角速度偏差，e_{v_t}(k)为当前时刻第一个和第二个主动轮实时线速度的偏差，e_{θ_t}(k)为当前时刻第一个和第二个主动轮实时角速度的偏差。

如图1所示，AGV整体结构以主控单元为核心，控制移载、安全辅助、导向、驱动、站点识别六大单元。本专利主要是应用于驱动单元。

如图2所示，从编码器采集当前信号输出至控制器，控制器判断驱动单元是否同步，若未同步，控制量输入相应伺服电机，实现驱动单元同步；若同步继续运行。

如图3所示，本发明考虑的对象是背负式AGV，在测试PID方法的有效性时，包括如下步骤：

步骤1)分别给定各驱动单元的期望速度值与角速度值，并采集AGV各驱动单元的实际速度值与角速度值，获取相应偏差e_{p1_v}，e_{p2_v}，e_{θ1_v}，e_{θ2_v}，e_{v_t}，e_{θ_t}作为控制器的输入。

步骤2)得到的偏差信号经过如式(4)所示的线性组合可计算出各控制量u_v1(k),u_θ1(k),u_v2(k),u_θ2(k)。

$\left\{\begin{array}{c}{u}_{v1}\left(k\right)=K_{p1}{e}_{p1\_v}\left(k\right)+K_{l1}\underset{j=0}{\overset{k}{\Σ}}{e}_{p1\_v}\left(j\right)+K_{d1}\left(e_{p1\_v}\right(k)-e_{p1\_v}(k-1\left)\right)+\\ K_{p\_t}{e}_{v\_t}\left(k\right)+K_{l\_t}\underset{j=0}{\overset{k}{\Σ}}{e}_{v\_t}\left(j\right)+K_{d\_t}\left(e_{v\_t}\right(k)-e_{v\_t}(k-1\left)\right)\\ u_{\mathrm{\θ}1}\left(k\right)=K_{p1}^{\′}{e}_{\mathrm{\θ}1\_v}\left(k\right)+K_{l1}^{\′}\underset{j=0}{\overset{k}{\Σ}}{e}_{\mathrm{\θ}1\_v}\left(j\right)+K_{d1}^{\′}\left(e_{\mathrm{\θ}1\_v}\right(k)-e_{\mathrm{\θ}1\_v}(k-1\left)\right)+\\ K_{p\_t}^{\′}{e}_{\mathrm{\θ}\_t}\left(k\right)+K_{l\_t}^{\′}\underset{j=0}{\overset{k}{\Σ}}{e}_{\mathrm{\θ}\_t}\left(j\right)+K_{d\_t}^{\′}\left(e_{\mathrm{\θ}\_t}\right(k)-e_{\mathrm{\θ}\_t}(k-1\left)\right)\\ u_{v2}\left(k\right)=K_{p2}{e}_{p2\_v}\left(k\right)+K_{l2}\underset{j=0}{\overset{k}{\Σ}}{e}_{p2\_v}\left(j\right)+K_{d2}\left(e_{p2\_v}\right(k)-e_{p2\_v}(k-1\left)\right)\\ u_{\mathrm{\θ}2}\left(k\right)=K_{p2}^{\′}{e}_{\mathrm{\θ}2\_v}\left(k\right)+K_{l2}^{\′}\underset{j=0}{\overset{k}{\Σ}}{e}_{\mathrm{\θ}2\_v}\left(j\right)+K_{d2}^{\′}\left(e_{\mathrm{\θ}2\_v}\right(k)-e_{\mathrm{\θ}2\_v}(k-1\left)\right)\end{array}\right.---\left(4\right)$

其中K_p1,K_l1,K_d1,K_{p_t},K_{l_t},K_{d_t},K'_p1,K′_l1,K'_d1,K'_{p_t},K′_{l_t},K'_{d_t},K_p2,K_l2,K_d2,K'_p2,K′_l2,K'_d2为给定的系统系数。e_{p1_v}(k)和e_{p2_v}(k)分别为当前时刻第一个和第二个驱动单元线速度偏差，e_{θ1_v}(k)和e_{θ2_v}(k)分别为当前时刻第一个和第二个驱动单元角速度偏差，e_{v_t}(k)为当前时刻第一个和第二个驱动单元实时线速度的偏差，e_{θ_t}(k)为当前时刻第一个和第二个驱动单元轮实时角速度的偏差。一个周期内作用到各自主动轮伺服电机上的控制量实际上是由两部分构成，一部分是主动轮的线速度期望值与实际值的偏差，和角速度期望值与实际值的偏差计算得到的控制量，另一部分是主动轮之间的实时线速度值偏差和实时角速度偏差计算得到的控制量。

Claims (1)

1.一种基于PID控制的背负型AGV两轮同步方法，其特征在于：所述方法包括如下步骤：

1)、以T为采样周期，通过驱动单元行走编码器分别获取驱动单元的位置p1，p2和各自运动的转向角度θ1，θ2，其中p1，θ1分别指第一个轮的位置与转向角；p2，θ2分别指第二个轮的位置与转向角；

2)、定义驱动单元的线速度偏差e_{p_v}和角速度偏差e_{θ_v}为

$e_{p\_v}=\left[\begin{array}{c}{e}_{p1\_v}\\ e_{p2\_v}\end{array}\right]=\left[\begin{array}{ccc}{v}_{p1\_d}& -& v_{p1}\\ v_{p2\_d}& -& v_{p2}\end{array}\right],$

$e_{\mathrm{\θ}\_v}=\left[\begin{array}{c}{e}_{\mathrm{\θ}1\_v}\\ e_{\mathrm{\θ}2\_v}\end{array}\right]=\left[\begin{array}{ccc}{v}_{\mathrm{\θ}1\_d}& -& v_{\mathrm{\θ}1}\\ v_{\mathrm{\θ}2\_d}& -& v_{\mathrm{\θ}2}\end{array}\right].---\left(1\right)$

其中，e_{p1_v}和e_{p2_v}分别为第一个和第二个驱动单元线速度偏差，e_{θ1_v}和e_{θ2_v}分别为第一个和第二个驱动单元角速度偏差；v_{p1_d}，v_{p2_d}分别为第一个驱动单元与第二个驱动单元的输入期望线速度，v_{θ1_d}，v_{θ2_d}分别为第一个驱动单元与第二个驱动单元的期望角速度信号；v_p1，v_p2分别为第一个驱动单元与第二个驱动单元的实时输出线速度；v_θ1，v_θ2分别为第一个驱动单元与第二个驱动单元的实时角速度信号，且v_p1，v_p2，v_θ1，v_θ2分别由步骤1)中得到的驱动单元位置p1，p2和各自运动的转向角度θ1，θ2采用差分法得到；v_{p1_d}，v_{p2_d}，v_{θ1_d}，v_{θ2_d}输入期望值是根据系统性能要求预先设定的；

3)、根据驱动单元的线速度偏差e_{p_v}与角速度偏差e_{θ_v}，通过改进的PID控制器确定本地控制率u_v1(k)，u_θ1(k)，u_v2(k)，u_θ2(k)，即定义如下：

e_{v_t}＝v_p1-v_p2(2)

e_{θ_t}＝v_θ1-v_θ2

本地控制率u_v1(k)，u_θ1(k)，u_v2(k)，u_θ2(k)的取值如下：

$u_{v1}\left(k\right)=K_{p1}{e}_{p1\_v}\left(k\right)+K_{l1}\underset{j=0}{\overset{k}{\Σ}}{e}_{p1\_v}\left(j\right)+K_{d1}\left(e_{p1\_v}\right(k)-e_{p1\_v}(k-1\left)\right)+$

$K_{p\_t}{e}_{v\_t}\left(k\right)+K_{l\_t}\underset{j=0}{\overset{k}{\Σ}}{e}_{v\_t}\left(j\right)+K_{d\_t}\left(e_{v\_t}\right(k)-e_{v\_t}(k-1\left)\right)$

$u_{\mathrm{\θ}1}\left(k\right)=K_{p1}^{\′}{e}_{\mathrm{\θ}1\_v}\left(k\right)+K_{l1}^{\′}\underset{j=0}{\overset{k}{\Σ}}{e}_{\mathrm{\θ}1\_v}\left(j\right)+K_{d1}^{\′}\left(e_{\mathrm{\θ}1\_v}\right(k)-e_{\mathrm{\θ}1\_v}(k-1\left)\right)+$

$K_{p\_t}^{\′}{e}_{\mathrm{\θ}\_t}\left(k\right)+K_{l\_t}^{\′}\underset{j=0}{\overset{k}{\Σ}}{e}_{\mathrm{\θ}\_t}\left(j\right)+K_{d\_t}^{\′}\left(e_{\mathrm{\θ}\_t}\right(k)-e_{\mathrm{\θ}\_t}(k-1\left)\right)$

$u_{v2}\left(k\right)=K_{p2}{e}_{p2\_v}\left(k\right)+K_{l2}\underset{j=0}{\overset{k}{\Σ}}{e}_{p2\_v}\left(j\right)+K_{d2}\left(e_{p2\_v}\right(k)-e_{p2\_v}(k-1\left)\right)$

$u_{\mathrm{\θ}2}\left(k\right)=K_{p2}^{\′}{e}_{\mathrm{\θ}2\_v}\left(k\right)+K_{l2}^{\′}\underset{j=0}{\overset{k}{\Σ}}{e}_{\mathrm{\θ}2\_v}\left(j\right)+K_{d2}^{\′}\left(e_{\mathrm{\θ}2\_v}\right(k)-e_{\mathrm{\θ}2\_v}(k-1\left)\right)---\left(3\right)$

其中K_p1,K_l1,K_d1,K_{p_t},K_{l_t},K_{d_t},K'_p1,K′_l1,K'_d1,K'_{p_t},K′_{l_t},K'_{d_t},K_p2,K_l2,K_d2,K'_p2,K′_l2,K′_d2为给定的系统系数。e_{p1_v}(k)和e_{p2_v}(k)分别为当前时刻第一个和第二个主动轮线速度偏差，e_{θ1_v}(k)和e_{θ2_v}(k)分别为当前时刻第一个和第二个主动轮角速度偏差，e_{v_t}(k)为当前时刻第一个和第二个主动轮实时线速度的偏差，e_{θ_t}(k)为当前时刻第一个和第二个主动轮实时角速度的偏差。