CN102854878B - 红外光电导向agv装置及其控制方法 - Google Patents

Abstract

本发明涉及红外光电导向AGV装置及控制方法，在车体底部的四端角位置各安装一从动轮，车体底部的中间部位沿纵向安装有一组呈对称分布的驱动轮，驱动轮均与无刷直流电机驱动连接，每只驱动轮的内侧设有速度检测装置，车体底部的中间部位沿横向安装有一组呈对称分布的红外光电导向装置，红外光电导向装置的内侧安装有红外标志检测装置，车体底部的四端角位置还各安装一避障传感器。采用测量位置偏移量和偏转角的红外位置检测方法，二次线型最优控制和卡尔曼滤波器相结合导引路径跟踪策略，很好地实现沿着预定轨迹自动巡航，并能够保证AGV路径跟踪的快速性以及高精度。

Description

红外光电导向AGV装置及其控制方法

技术领域

本发明涉及用于工业自动化车间的无人车的自动导向系统及其控制方法，尤其涉及红外光电导向AGV装置及其控制方法，属于移动机器人技术领域。

背景技术

自动导引车辆(Automated Guided Vehicle，简称AGV)是现代物流技术的关键设备，随着自动导引车辆新兴市场的发展，对廉价且控制精度高的AGV产生了巨大的需求，车辆建模和控制策略的研究已经发展成为当前研究课题的热点，AGV作为自动运输设备，在自动化车间中的沿着预定轨迹运行的精度直接影响到整个车间的正常运行。尤其是AGV的导向控制精度作为决定AGV性能的关键因素更是吸引了极大的关注。红外自动导引车AGV是一种以红外光电作为导引方式的自动导向车辆，控制系统是AGV的核心内容，而路径跟踪控制方法是保证AGV控制精度的关键所在。

专利申请号为201110306984.2的中国专利公开了一种车道识别偏离检测方法，采用图像处理对AGV车道识别偏离的检测方法，首先对获取的车道图像进行预处理，然后对预处理之后的车道图像进行边缘检测，得到车道边缘图像，根据得到的车道边缘图像基于卡尔曼滤波器的车道跟踪方法，确定出车道的位置，并在点集优化后的基础上提取车道参数，然后根据得到的车道参数，拟合提取车道线，最后根据提取的车道线判别AGV车体所在车道的具体状况，可以实现对车道状况的检测。

专利申请号为201110054045.3的中国专利公开了AGV可编程电磁导引方法和装置，通过改变同一根导引线上的导引信号的频率，而各种信号的频率分别代表不同的动作意义，随时控制AGV的动作，动作包括前进、后退、停止、左转、右转等，AGV根据检测到的导引频率的执行不同的动作。

专利申请号为201110029304.7的中国专利公开了一种AGV自动导向系统和控制方法，系统由检测模块和控制模块组成，检测模块由最为AGV车体位置传感器的编码器和作为AGV位姿传感器的激光扫描仪构成；控制模块是由伺服驱动单元和工控机组成，设置控制模块由内位置环和外位置环构成的双位置闭环控制模块；内位置环为电机转角位置环，外位置环是以激光扫描读取AGV位置信号，将外位置环中反馈的AGV位姿信号计算AGV位置的路径跟踪误差，将路径跟踪误差作为控制系统的输入量。

发明内容

本发明的目的是克服现有技术存在的不足，提供一种红外光电导向AGV装置及导向控制方法。

本发明的目的通过以下技术方案来实现：

红外光电导向AGV装置，包括车体、驱动轮和从动轮以及铺设于地面的导引反光带，特点是：在车体底部的四端角位置各安装一从动轮，车体底部的中间部位沿纵向安装有一组呈对称分布的驱动轮，驱动轮均与无刷直流电机驱动控制连接，每只驱动轮的内侧设有速度检测装置，车体底部的中间部位沿横向安装有一组呈对称分布的红外光电导向装置，红外光电导向装置的内侧安装有红外标志检测装置，车体底部的四端角位置还各安装一避障传感器。

进一步地，上述的红外光电导向AGV装置，两驱动轮之间的轴距为900mm。

更进一步地，上述的红外光电导向AGV装置，所述避障传感器为GP2Y0A21红外测距传感器。

再进一步地，上述的红外光电导向AGV装置，所述红外光电导向装置包含红外发射器、红外接收器、缓冲驱动器和主控制单元，红外接收器经电压比较器接入至主控制单元，电压比较器上设有电压调节电阻，主控制单元上连接译码器，译码器连接缓冲驱动器，缓冲驱动器上设有光强可调电阻，缓冲驱动器经扫描指示灯连接红外发射器，主控制单元上设有通信串口。

本发明红外光电导向AGV装置的控制方法，按照预设路径的轨迹在地面铺设导引反光带，在需要铺设动作标志的位置设置动作标志；

(1)AGV装置运行中，车体相对导引路径的偏移信息通过红外光电导向装置进行检测，红外光电导向装置发射红外光线，红外光线经过导引反光带的发射得到车体相对导引反光带的位置信息；位置信息的检测步骤是：在AGV相对导引路线所在的运动线路上，定义实际位置和目标位置之间的误差为位置误差，将AGV的运动位置表示为(d，α)，其中d为AGV偏移导引路线的法相位置误差，α为AGV的方位角误差，所述的方位角为AGV的当前行进路线与预期轨迹也就是导引路线之间的夹角；要求AGV沿着预定的导轨路线循迹前进，设置所述的AGV的目标位置为(0，0)，由红外光电导向装置检测AGV起点的位置是(0，0)，根据路径的规划，预先设置两个驱动轮之间的距离是L，前后两个位置检测装置之间的距离是D；

目标位置有两种情况，当沿着指导路径循迹时，目标位置为(0，0)；当在一些特定转弯动作标志位置时，AGV的目标位置为(d，α)；

在需要转弯的情况下导引车的控制采用前馈控制，在检测到动作标志时，加入对应动作的前馈控制量，当行驶过动作标志进入循迹过程时，恢复正常循迹；

根据AGV车体运行时的相对位置的几何平面，测量AGV的导轨跟踪误差的具体步骤是：AGV的前后两端各配置一个红外光电导向装置，当车辆行驶时，红外光电导向装置的发射器按照固定的频率发射红外光，接收光电二极管采集反光导引路线反射回来的红外光点；

两个红外光电导向装置之间的距离固定为D；

预处理AGV车体行驶过程中在同一时刻采集到的从导引路线反射回来的红外光点序列，接收光电二极管之间的距离固定，根据接收到的发射光点序列得到偏移距离，则前后两个红外光电导向装置采集到的偏移距离分别是D1，D2；设定，当AGV车体偏向左边时候，测得偏移量是负值，当AGV的车体偏向右边时候，测得偏移量是正值，当车体现行轨迹与导引路线的夹角偏向导引路线左侧为逆时针的时候，角度为负值，当偏向右侧顺时针的时候，角度为正值；

(2)检测得到的数据通过红外光电导向装置的标准通信串口和主控单元MCU通信，将位置信息输入主控单元MCU进行处理；

位置信息的处理通过以下步骤实现：

根据几何关系推算出；

$L_1=\frac{{\mathrm{DD}}_1}{D_1-D_2}$

$L_2=\frac{{\mathrm{DD}}_2}{D_1-D_2}$

$\mathrm{\θ}=\arctan \frac{D_2}{L_2}=\arctan \frac{D_1}{L_1}$

d₁＝sinθ×D₁

d₂＝sinθ×D₂

其中，L1是前端红外光电导向装置沿两个红外光电导向装置中心线到导引反光带之间距离，L2是后端红外光电导向装置沿两个红外光电导向装置中心线到反光带之间距离，θ为偏转角；d₁是导引线到车体两个红外光电导向装置的中心点的垂直距离，同理，d₂是导引线到车体两个红外光电导向装置中心点的垂直距离；

(3)将得到的偏移量和偏转角通过卡尔曼滤波器进行滤波，去除高频噪声，进一步修正AGV与导引路线的误差；

(4)AGV运行的过程中，在一些特定位置要求AGV执行特定的动作，在这些特定位置设置动作标志，动作标志是间隔性的等距反光带，由宽度相同的反光带或者非反光带按顺序排列组成；

所述标志检测通过以下步骤；

根据红外标志检测装置检测到的标志的数字信号通过串口输入MCU，主控装置判断动作标志代表的意义然后发出对应的动作控制命令，AGV做出相对应的动作，在AGV运行的过程中，在一些特定位置要求AGV执行特定的动作，在特定位置设置动作标志，当光线发射到反光带上时，接收管接收到反射光，判断为1，相反，判断为0；

当AGV沿着导引路径以固定的速度v行进时，经过每一个小段反光带的时间是相同的t，则根据在每一段时间t内检测到的高低电平的不同对标志做出判断；

标志检测装置检测到动作标志的信号是01101010，所代表的动作是启动；

将启动动作信号发送到主控MCU，主控板发出启动命令；

在有具体动作控制位置，AGV的导向控制器与直线循迹控制器相同，控制器的前端设有前馈控制；

使AGV的目标位置改变，以左转动，目标位置为(-d，-α)，AGV将以目标位置为终点前进；

根据检测到的不同的动作标志信号，代表不同的动作意义，将检测到的动作标志通过串口发送到MCU主控制器，

主控制单元经过处理之后得到控制量，将控制量发送到驱动，驱动车体向目标位置运行；

当需要改变特定位置的动作时候，改变动作标志，或者通过人机界面对标志所代表的动作的意义加以改变；

(5)由避障传感器通过红外检测到的距离的远近产生大小不同的模拟量信号，通过AD转换接口与MCU相连接，主控制装置将模拟信号转换为对应的数字信号也就是距离信息，根据距离的远近，然后发出转弯、减速或者刹车的命令到驱动轮控制器，使AGV系统避开障碍物减小损害；

(6)速度检测装置为脉冲信号检测装置，对单位时间内接收到的脉冲信号个数进行统计，并通过SPI接口发送到主控制单元，MCU计算出电机的实际转速。

再进一步地，上述的红外光电导向AGV装置的控制方法，轨迹跟踪控制策略采用二次线型最优控制和卡尔曼滤波器控制相结合的LQG控制策略，在AGV运动所在的平面直角坐标系中，所述的位置检测装置检测到的偏移导引路径的偏移量d和偏转角α；

所述路径跟踪控制方法步骤如下；

控制系统的建模：

AGV在初始状态的时没有偏差，由于外部干扰的影响，AGV的路径随着时间的推移出现略微的偏差，偏差的角度为参考轨迹和小车移动的中心线的夹角，偏移的距离由两个轮子中心点和参考轨迹确定，左驱动轮的速度是V_L，右边驱动轮的速度是V_R，在一个非常短的时间Δt内，速度的大小和方向都没有变化，由几何关系推导出以下信息；

$V_c=\frac{V_R+V_L}{2}$

$\mathrm{\Δ\θ}=\frac{(V_R-V_L)\×\mathrm{\Δt}}{L}$

$\mathrm{\Δd}=\frac{V_R+V_L}{2}\×\mathrm{\Δt}\×\sin \mathrm{\θ}$

在上述公式中，L是两个驱动轮之间的距离，当Δt接近0的时候，得到偏移距离和偏移角的微分方程；

$\mathrm{d\θ}=\frac{V_R-V_L}{L}\mathrm{dt}$

$\mathrm{dd}=\frac{V_R+V_L}{2}\sin \mathrm{\θdt}$

通过积分和拉普拉斯变换，上述公式化为；

$\mathrm{\θ}\left(s\right)=\frac{(V_R-V_L)}{\mathrm{Ls}}$

$d\left(s\right)=\frac{(V_R+V_L)\mathrm{\θ}}{2s}$

当公式中的角度足够小时，公式近似化为；

$d\left(s\right)=\frac{(V_R+V_L)\mathrm{\θ}}{2s}$

AGV控制系统的数学模型中，两个轮子由两个直流无刷电机分别驱动，假定电机和轮子以相同的速度n运行，轮半径是r，电机的控制输入是U，电机的负载时间常数是Tm，左右电机的参数分别是Tmr和Tml，左右电机的传递函数简化为：

$n_R=\frac{k_R}{1+T_{\mathrm{mR}}s}{U}_R$

$n_L=\frac{k_L}{1+T_{\mathrm{mL}}s}{U}_L$

在上述公式中，K_L和K_R是与电机的反电动势相关的常数，因两电机的参数相同，所以

T_mR＝T_mL＝T_m

此外，考虑到轮子的线速度和角速度之间的关系，得到左右轮的线速度如下所示，

$V_R=\frac{k}{1+T_{m}s}{U}_R$

$V_L=\frac{k}{1+T_{m}s}{U}_L$

系统包含θ与V_R+V_L相乘因子，系统属于非线性系统，AGV是在一种固定的规则下运行，控制输入视为一个固定速度控制输入和一个小的循迹控制控制输入量，因此，该非线性系统采用小误差系统的方法，将非线性系统转换成线性系统并进行设计；

无外部的干扰，左轮和右轮的给定信号是相同的，V_R＝V_L＝V_c，AGV的速度V_c是由常量U_c产生的，视为速度为常量的匀速直线运动，当AGV离开预定的轨迹，给定信号将增加ΔU调节循迹；

U_R＝U_C+ΔU

U_L＝U_C-ΔU

电机线性速度的相应输出是：

V_R＝V_c+ΔV

V_L＝V_c-ΔV

在正常条件下AGV的平均运行速度是一个常数，假设状态变量分别是X₁＝ΔV，X₂＝θ，X₃＝d得状态函数如下：

$\left[\begin{array}{c}{\stackrel{\·}{X}}_1\\ {\stackrel{\·}{X}}_2\\ {\stackrel{\·}{X}}_3\end{array}\right]=\left[\begin{array}{ccc}-\frac{1}{T_m}& 0& 0\\ \frac{1}{L}& 0& 0\\ 0& V_c/2& 0\end{array}\right]\left[\begin{array}{c}{X}_1\\ X_2\\ X_3\end{array}\right]+\left[\begin{array}{c}k\\ 0\\ 0\end{array}\right]\mathrm{\ΔU}$

控制器的设计通过以下步骤实现：

采用LQI控制方法进行导向控制，LQI引入输出误差的积分作为额外的状态变量，以加强系统的跟踪性能，则扩展的系统为：

$\frac{d}{\mathrm{dt}}\left[\begin{array}{c}x\left(t\right)\\ x_r\left(t\right)\end{array}\right]=\left[\begin{array}{cc}A& 0\\ -C& 0\end{array}\right]\left[\begin{array}{c}x\left(t\right)\\ x_r\left(t\right)\end{array}\right]+\left[\begin{array}{c}B\\ 0\end{array}\right]u\left(t\right)+\left[\begin{array}{c}0\\ I\end{array}\right]r\left(t\right)$

式中

$A=\left[\begin{array}{ccc}-\frac{1}{T_m}& 0& 0\\ \frac{1}{L}& 0& 0\\ 0& \frac{V_c}{2}& 0\end{array}\right]B=\left[\begin{array}{c}k\\ 0\\ 0\end{array}\right]C=\left[|\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\end{array}\right]$

基于二次型目标函数的相对于系统的最优控制问题，最优性能指标值为；

$J={\∫}_0^{\∞}[x_a^{T}Q{x}_a\left(t\right)+u^T\left(t\right)\mathrm{Ru}\left(t\right)]\mathrm{dt}$

式中 $x_a=\left[\begin{array}{cc}A& 0\\ -C& 0\end{array}\right],$ Q和R为对应的加权对角矩阵；

状态反馈增益和控制输入为；

$u\left(t\right)=-F_{1}x\left(t\right)-F_2{x}_r\left(t\right)=-F_{1}x\left(t\right)-F_2{\∫}_0^{t}e\left(t\right)\mathrm{dt},$

F＝[F₁ F₂]＝R^-1B^#TP

$B^{\#}=\left[\begin{array}{c}B\\ 0\end{array}\right],$ P为对应的代数里卡蒂方程的解；

在上述最优控制器中，虽然传感器测量到偏移量和偏转角，但是在此过程中，由于外界的干扰，电压的波动，反光导引带的起伏因素引起的扰动，卡尔曼滤波器对中间的噪声进行处理；

卡尔曼滤波器的设计通过以下步骤实现；

对于偏移量和偏转角两个变量，传感器直接测量得到，而在轨迹跟踪的过程中，实际的AGV系统具有一系列不确定性，主要包括路面的随机干扰输入、传感器的测量噪声以及模型的不确定性，各种过程噪声和观测噪声的干扰，将使状态变量的测量结果出现偏差，卡尔曼滤波器使用的条件是系统噪声和测量噪声均为白噪声，卡尔曼滤波器进行滤波；

根据分离定理，对系统的状态变量X进行估计，表达式为

$\stackrel{\·}{\hat{X}}=A\hat{X}+\mathrm{BU}+K_1(Y-C\hat{X})$

为状态量的估计，K₁为滤波器增益

$K_1=P_l{B}^T{R}_f^{-1}$

式中，Pl为对应的代数利卡迪方程的解，Rf为加权矩阵；

根据分离原理，将线性二次最优控制器和卡尔曼滤波器相结合，即为线性二次高斯LQG控制器；

根据设计的控制器，将位置检测装置测量到的相对轨迹的偏移量和偏移角进行处理，MCU主控制器根据反馈变量计算相应的控制器输出，并将控制量发送到驱动单元对AGV进行纠偏处理；

以AGV速度是1m/s的速度行走，让AGV从某种初始状态出发跟踪设定的直线路径，初始误差设置为d＝0，角度为θ＝0；根据实验结果，得到较佳的实验结果；

根据实验结果得出，AGV能够在一个较短的时间内达到稳定状态，且在随后的行驶过程中，误差和控制量在一个较小的范围内变化；

保证AGV的控制精度，对控制器的设计采用高速和低速进行切换控制，误差在高速和低速时候均控制在比较小的范围之内。

本发明技术方案突出的实质性特点和显著的进步主要体现在：

本发明采用测量位置偏移量和偏转角的红外位置检测方法，导引路径跟踪策略、二次线型最优控制和卡尔曼滤波器相结合控制，很好地实现沿着预定轨迹自动巡航，并能够保证AGV路径跟踪的快速性以及高精度，且能够明显降低车辆的成本。

附图说明

下面结合附图对本发明技术方案作进一步说明：

图1：本发明装置部件的布置示意图；

图2：AGV车体运行时的相对位置的几何平面示意图；

图3：标志反光带含义示意图；

图4：AGV转向系统示意图；

图5：线性二次高斯LQG控制器的示意图。

具体实施方式

如图1所示，红外光电导向AGV装置，包括车体、传感器、驱动轮和从动轮以及铺设于地面的导引反光带，在车体底部的四端角位置各安装一从动轮，即第一从动轮11、第二从动轮12、第三从动轮13和第四从动轮14，从动轮具有辅助转向和承重的作用，为了保证AGV在行驶的过程中，驱动轮一直和地面保持接触，对从动轮的设计采用了加装弹簧器和采用牛眼轮的方法，从动轮的顶端支撑轴上加装阻尼系数可调的弹簧器，根据行驶条件和加载重量的不同可以对弹簧的阻尼系数进行调节，另外牛眼轮的采用可以减小转向过程中机械摩擦的作用，使转向更加平滑迅速；车体底部的中间部位沿纵向安装有一组呈对称分布的驱动轮，即第一驱动轮31和第二驱动轮32，第一驱动轮31和第二驱动轮32之间的轴距为900mm，第一驱动轮31与第一无刷直流电机61驱动控制连接，第二驱动轮32与第二无刷直流电机62驱动控制连接，第一驱动轮31的内侧设有第一速度检测装置71，第二驱动轮32的内侧设有第二速度检测装置72，驱动轮通过无刷直流电机驱动，通过调节无刷直流电机电压各控制一个轮子以不同的速度转动从而实现差速转向控制；车体底部的中间部位沿横向安装有一组呈对称分布的红外光电导向装置，即第一红外光电导向装置41和第二红外光电导向装置42，第一红外光电导向装置41的内侧安装有第一红外标志检测装置51，第二红外光电导向装置42的内侧安装有第二红外标志检测装置52，红外光电导向装置的安装高度对车体位置信息的采集精度有很大影响，太高将影响发射线的强度，而且会出现反射噪声，因此将红外导向装置的安装高度尽量调低约50mm，红外导向传感器对车体相对导引带中心的位置信息进行检测；车体底部的四端角位置还各安装一避障传感器，即第一避障传感器21、第二避障传感器22、第三避障传感器23和第四避障传感器24，四个避障传感器安装在四个端角对AGV周围障碍物实现全面的检测，避障传感器采用夏普公司的GP2Y0A21红外测距传感器，测距范围可以达到20-150cm。

铺于地面的导引反光带采用银色且表面光滑平整的锡箔材料，可以反射回大部分的光线，红外光电导向装置的宽度为导引反光带的2倍，使可测量的偏移量的范围达到3个导引反光带的宽度。

第一红外光电导向装置41和第二红外光电导向装置42均包含红外发射器、红外接收器、缓冲驱动器和主控制单元，红外接收器经电压比较器接入至主控制单元，电压比较器上设有电压调节电阻，主控制单元上连接译码器，译码器连接缓冲驱动器，缓冲驱动器上设有光强可调电阻，缓冲驱动器经扫描指示灯连接红外发射器，主控制单元上设有通信串口，主控制单元采用型号为STC11F04E的芯片。

采用带有卡尔曼滤波器的状态反馈控制系统，由车体位置检测单元和导向控制单元组成，位置检测是由作为指导路径的导引反光带和检测AGV位置偏移量的红外光电位置检测装置构成。作为指导路径的反射光带是银白色锡箔纸构成，具有表面光滑平整，反光强，质量轻薄，易于布置等特点。

导引反光带按照预设路径的轨迹铺设在地面，在需要铺设动作标志的位置将动作标志加上。红外光电位置检测装置由两组组红外发射管、一组红外接收光电二极管和主控制单片机组成，红外接收二极管接收到的红外光点序列与反光导引路径反射光点是一一对应，经主控制单片机推算出偏移量，将所测偏移量发送到控制单元的MCU进行处理。

控制单元由无刷伺服直流电机驱动单元和MCU主控单元构成，设置控制结构为内位置环和外位置环构成的双闭环控制系统，其中内位置环为电机转速控制环，内位置环速度检测装置检测转速信号，并将电机转速信号反馈给MCU主控制单元；外位置环是红外光电位置测量装置和主控制装置，位置检测装置测量AGV相对导引路径的位置信息并发送到主控单元MCU，经过一系列的几何运算可以推算出AGV的偏移量和偏转角，将AGV的偏移量和偏转角经过控制策略的算法处理之后得到路径跟踪误差的控制量，将路径跟踪误差的控制量和转速目标值一起作为内环控制系统的输入量，计算出最终的电机控制量并发送到伺服电机驱动单元，电机运转驱动AGV运行。

内位置环用来控制电机的精确运行，由于电气控制部分与AGV执行机构相对独立，可以采用PID控制，系统易于整定，响应快，保证系统的动态特性；可以通过调节电机的各个增益参数，使电机的性能达到最佳。

在外位置环中，位置检测系统采用红外光电扫描装置，将红外扫描装置安装在AGV的车体前后两端的中心位置，通过红外扫描装置不断发射和接收红外线，可以根据红外接收光点序列推算出AGV的位置偏移信息。外位置环主要用于稳态误差方面的控制，用于保证系统的跟随性。由于控制系统是非线性系统，因此控制精度主要靠路径跟踪控制方法保证。以外置环中反馈的AGV的位置偏移信息计算AGV当前位置的路径跟踪误差，将路径跟踪误差作为控制系统的输入量。

红外光电导向AGV装置的控制方法，按照预设路径的轨迹在地面铺设导引反光带，在需要铺设动作标志的位置设置动作标志；

(1)AGV装置运行中，车体相对导引路径的偏移信息通过红外光电导向装置进行检测，红外光电导向装置发射红外光线，红外光线经过导引反光带的发射得到车体相对导引反光带的位置信息；位置信息的检测步骤是：在AGV相对导引路线所在的运动线路上，定义实际位置和目标位置之间的误差为位置误差，将AGV的运动位置表示为(d，α)，其中d为AGV偏移导引路线的法相位置误差，α为AGV的方位角误差，所述的方位角为AGV的当前行进路线与预期轨迹也就是导引路线之间的夹角；要求AGV沿着预定的导轨路线循迹前进，设置所述的AGV的目标位置为(0，0)，由红外光电导向装置检测AGV起点的位置是(0，0)，根据路径的规划，预先设置两个驱动轮之间的距离是L，前后两个位置检测装置之间的距离是D；

目标位置有两种情况，当沿着指导路径循迹时，目标位置为(0，0)；当在一些特定转弯动作标志位置时，AGV的目标位置为(d，α)；

在需要转弯的情况下导引车的控制采用前馈控制，在检测到动作标志时，加入对应动作的前馈控制量，当行驶过动作标志进入循迹过程时，恢复正常循迹；

根据AGV车体运行时的相对位置的几何平面，如图2所示，测量AGV的导轨跟踪误差的具体步骤是：AGV的前后两端各配置一个红外光电导向装置，当车辆行驶时，红外光电导向装置的发射器按照固定的频率发射红外光，接收光电二极管采集反光导引路线反射回来的红外光点；

两个红外光电导向装置之间的距离固定为D；

预处理AGV车体行驶过程中在同一时刻采集到的从导引路线反射回来的红外光点序列，接收光电二极管之间的距离固定，根据接收到的发射光点序列得到偏移距离，则前后两个红外光电导向装置采集到的偏移距离分别是D1，D2；设定，当AGV车体偏向左边时候，测得偏移量是负值，当AGV的车体偏向右边时候，测得偏移量是正值，当车体现行轨迹与导引路线的夹角偏向导引路线左侧为逆时针的时候，角度为负值，当偏向右侧顺时针的时候，角度为正值；

(2)检测得到的数据通过红外光电导向装置的标准通信串口和主控单元MCU通信，将位置信息输入主控单元MCU进行处理；

位置信息的处理通过以下步骤实现：

根据几何关系推算出；

$L_1=\frac{{\mathrm{DD}}_1}{D_1-D_2}$

$L_2=\frac{{\mathrm{DD}}_2}{D_1-D_2}$

$\mathrm{\θ}=\arctan \frac{D_2}{L_2}=\arctan \frac{D_1}{L_1}$

d₁＝sinθ×D₁

d₂＝sinθ×D₂

其中，L1是前端红外光电导向装置沿两个红外光电导向装置中心线到导引反光带之间距离，L2是后端红外光电导向装置沿两个红外光电导向装置中心线到反光带之间距离，θ为偏转角；d₁是导引线到车体两个红外光电导向装置的中心点的垂直距离，同理，d₂是导引线到车体两个红外光电导向装置中心点的垂直距离；

(3)将得到的偏移量和偏转角经卡尔曼滤波器进行滤波，去除高频噪声，进一步修正AGV与导引路线的误差；

(4)AGV运行的过程中，在一些特定位置要求AGV执行特定的动作，在这些特定位置设置动作标志，动作标志是间隔性的等距反光带，由宽度相同的反光带或者非反光带按一定顺序排列组成；

所述标志检测通过以下步骤；

根据红外标志检测装置检测到的标志的数字信号通过串口输入MCU，主控装置判断动作标志代表的意义然后发出对应的动作控制命令，AGV做出相对应的动作，在AGV运行的过程中，在一些特定位置要求AGV执行特定的动作，在特定位置设置动作标志，当光线发射到反光带上时，接收管接收到反射光，判断为1，相反，判断为0，标志反光带检测的数字信号的意义分别如图3所示，

当AGV沿着导引路径以固定的速度v行进时，经过每一个小段反光带的时间是相同的t，则根据在每一段时间t内检测到的高低电平的不同对标志做出判断；

标志检测装置检测到动作标志的信号是01101010，所代表的动作是启动；

将启动动作信号发送到主控MCU，主控板发出启动命令；

在有具体动作控制位置，AGV的导向控制器与直线循迹控制器相同，控制器的前端设有前馈控制；

使AGV的目标位置改变，以左转动，目标位置为(-d，-α)，AGV将以目标位置为终点前进；

根据检测到的不同的动作标志信号，代表不同的动作意义，将检测到的动作标志通过串口发送到MCU主控制器，

主控制单元经过处理之后得到控制量，将控制量发送到驱动，驱动车体向目标位置运行；

当需要改变特定位置的动作时候，改变动作标志，或者通过人机界面对标志所代表的动作的意义加以改变；

(5)由避障传感器通过红外检测到的距离的远近产生大小不同的模拟量信号，通过AD转换接口与MCU相连接，主控制装置将模拟信号转换为对应的数字信号也就是距离信息，根据距离的远近，然后发出转弯、减速或者刹车的命令到驱动轮控制器，使AGV系统避开障碍物减小损害；

(6)速度检测装置为脉冲信号检测装置，对单位时间内接收到的脉冲信号个数进行统计，并通过SPI接口发送到主控制单元，MCU计算出电机的实际转速。

上述的红外光电导向AGV装置的控制方法，轨迹跟踪控制策略采用二次线型最优控制和卡尔曼滤波器控制相结合的LQG控制策略，在AGV运动所在的平面直角坐标系中，所述的位置检测装置检测到的偏移导引路径的偏移量d和偏转角α；

所述路径跟踪控制方法步骤如下；

控制系统的建模：

AGV在初始状态的时没有偏差，由于外部干扰的影响，AGV的路径随着时间的推移出现略微的偏差，偏差的角度为参考轨迹和小车移动的中心线的夹角，偏移的距离由两个轮子中心点和参考轨迹确定，左驱动轮的速度是V_L，右边驱动轮的速度是V_R，在一个非常短的时间Δt内，速度的大小和方向都没有变化，由几何关系推导出以下信息；

$V_c=\frac{V_R+V_L}{2}$

$\mathrm{\Δ\θ}=\frac{(V_R-V_L)\×\mathrm{\Δt}}{L}$

$\mathrm{\Δd}=\frac{V_R+V_L}{2}\×\mathrm{\Δt}\×\sin \mathrm{\θ}$

在上述公式中，L是两个驱动轮之间的距离，当t接近0的时候，采用微分算法修正AGV的位置偏移误差和方位角误差，获得修正法相位置误差和修正方位角误差分别是；

$\mathrm{d\θ}=\frac{V_R-V_L}{L}\mathrm{dt}$

$\mathrm{dd}=\frac{V_R+V_L}{2}\sin \mathrm{\θdt}$

通过微分和拉普拉斯变换，上述公式化为；

$d\left(s\right)=\frac{(V_R+V_L)\mathrm{\θ}}{2s}$

当公式中的角度θ足够小时，公式近似化为；

$d\left(s\right)=\frac{(V_R+V_L)\mathrm{\θ}}{2s}$

AGV控制系统的数学模型中，两个轮子由两个直流无刷电机分别驱动，假定电机和轮子以相同的速度n运行，轮半径是r，电机的控制输入是U，电机的负载时间常数是Tm，左右电机的参数分别是Tml和Tmr，左右电机的传递函数简化为：

$n_R=\frac{k_R}{1+T_{\mathrm{mR}}s}{U}_R$

$n_L=\frac{k_L}{1+T_{\mathrm{mL}}s}{U}_L$

在上述公式中，K_L和K_R是与电机的反电动势相关的常数，因两电机的参数相同，所以

T_mR＝T_mL＝T_m

此外，考虑到轮子的线速度和角速度之间的关系，得到左右轮的线速度如下所示，如图4所示；

$V_R=\frac{k}{1+T_{m}s}{U}_R$

$V_L=\frac{k}{1+T_{m}s}{U}_L$

系统包含θ与V_R+V_L相乘因子，系统属于非线性系统，AGV是在一种固定的规则下运行，控制输入视为一个固定速度控制输入和一个小的循迹控制输入量，因此，该非线性系统采用小误差系统的方法，将非线性系统转换成线性系统并进行设计；

无外部的干扰，左轮和右轮的给定信号是相同的，V_R＝V_L＝V_c，AGV的速度V_c是由常量U_c产生的，视为速度为常量的匀速直线运动，当AGV离开预定的轨迹，给定信号将增加ΔU调节循迹；

电机线性速度的相应输出是：

U_R＝U_C+ΔU

U_L＝U_C-ΔU

V_R＝V_c+ΔV

V_L＝V_c-ΔV

在正常条件下AGV的平均运行速度是一个常数，假设状态变量分别是X₁＝ΔV，X₂＝θ，X₃＝d得状态函数如下：

$\left[\begin{array}{c}{\stackrel{\·}{X}}_1\\ {\stackrel{\·}{X}}_2\\ {\stackrel{\·}{X}}_3\end{array}\right]=\left[\begin{array}{ccc}-\frac{1}{T_m}& 0& 0\\ \frac{1}{L}& 0& 0\\ 0& V_c/2& 0\end{array}\right]\left[\begin{array}{c}{X}_1\\ X_2\\ X_3\end{array}\right]+\left[\begin{array}{c}k\\ 0\\ 0\end{array}\right]\mathrm{\ΔU}$

控制器的设计通过以下步骤实现：

采用LQI控制方法进行导向控制，LQI伺服控制系统QI伺服控制系统引入输出误差的积分作为额外的状态变量，增强系统的跟踪性能，扩展系统为；

$\frac{d}{\mathrm{dt}}\left[\begin{array}{c}x\left(t\right)\\ x_r\left(t\right)\end{array}\right]=\left[\begin{array}{cc}A& 0\\ -C& 0\end{array}\right]\left[\begin{array}{c}x\left(t\right)\\ x_r\left(t\right)\end{array}\right]+\left[\begin{array}{c}B\\ 0\end{array}\right]u\left(t\right)+\left[\begin{array}{c}0\\ I\end{array}\right]r\left(t\right)$

式中

$A=\left[\begin{array}{ccc}-\frac{1}{T_m}& 0& 0\\ \frac{1}{L}& 0& 0\\ 0& \frac{V_c}{2}& 0\end{array}\right]B=\left[\begin{array}{c}k\\ 0\\ 0\end{array}\right]C=\left[|\begin{array}{ccc}0& 1& 0\\ 0& 0& 1\end{array}\right]$

基于二次型目标函数的相对于系统的最优控制问题，最优性能指标值为；

$J={\∫}_0^{\∞}[x_a^{T}Q{x}_a\left(t\right)+u^T\left(t\right)\mathrm{Ru}\left(t\right)]\mathrm{dt}$

式中 $x_a=\left[\begin{array}{cc}A& 0\\ -C& 0\end{array}\right],$ Q，R为加权对角矩阵，阶数分别与x_a和u对应；

状态反馈增益和控制输入可表示为；

$u\left(t\right)=-F_{1}x\left(t\right)-F_2{x}_r\left(t\right)=-F_{1}x\left(t\right)-F_2{\∫}_0^{t}e\left(t\right)\mathrm{dt}$

F＝[F₁ F₂]＝R^-1B^#TP

$B^{\#}=\left[\begin{array}{c}B\\ 0\end{array}\right],$ P为对应的代数里卡蒂方程的解；

在上述最优控制器中，虽然传感器测量到偏移量和偏转角，但是在此过程中，由于外界的干扰，电压的波动，反光导引带的起伏因素引起的扰动，采用卡尔曼滤波器对中间的噪声进行处理；

卡尔曼滤波器的设计通过以下步骤实现；

对于偏移量和偏转角两个变量，传感器直接测量得到，而在轨迹跟踪的过程中，实际的AGV系统具有一系列不确定性，主要包括路面的随机干扰输入、传感器的测量噪声以及模型的不确定性，各种过程噪声和观测噪声的干扰，将使状态变量的测量结果出现偏差，卡尔曼滤波器使用的条件是系统噪声和测量噪声均为白噪声，卡尔曼滤波器进行滤波；

根据分离定理，对系统的状态变量X进行估计，表达式为

$\stackrel{\·}{\hat{X}}=A\hat{X}+\mathrm{BU}+K_1(Y-C\hat{X})$

为状态量的估计，K₁为滤波器增益

$K_1=P_l{B}^T{R}_f^{-1}$

式中P_l为对应代数利卡迪方程的解，R_f对应的加权矩阵。

根据分离原理，将线性二次最优控制器和卡尔曼滤波器相结合，即为线性二次高斯LQG控制器，其结构框图如图5所示；

根据设计的控制器，将位置检测装置测量到的相对轨迹的偏移量和偏移角进行处理计算相应的控制量，MCU主控制器并将控制量发送到驱动单元对AGV进行纠偏处理；

以AGV速度是1m/s的速度行走，让AGV从某种初始状态出发跟踪设定的直线路径，初始误差设置为d＝0，角度为θ＝0；根据实验结果，得到较佳的实验结果；

根据实验结果得出，AGV能够在一个较短的时间内达到稳定状态，且在随后的行驶过程中，误差和控制量在一个较小的范围内变化；

保证AGV的控制精度，对控制器的设计采用高速和低速进行切换控制，误差在高速和低速时候均控制在比较小的范围之内。

综上所述，本发明红外光电位置检测装置实现对车体相对导引路线的偏移量和偏转角的测量，利用红外光电位置检测装置可以实现对偏移量和偏转角的非接触式测量，且红外光电传感器的精度高，测量成本低，受外界的干扰较小。

导引路径的改变比较方便，只要铺设一条反光导引带作为导引路径就可以实现导向控制，改变控制路径只需要移动导引路径就可以实现。

在车辆行进的过程中，在一些特殊的位置需要AGV做出一些特殊动作，例如加速，减速，左转，右转等，在这些有特殊动作要求的位置，可以采用反光点序列不同的反光带进行标志，红外标志检测装置根据检测到的不同的标志信号执行不同的动作，根据此方法可以在任意不同的位置设置不同的动作标志从而实现不同的动作，且在某个位置如果需要改变动作，只要将标志改变或者通过AGV的人机交互装置对动作标志代表的动作意思进行重新设置，均可以实现动作的改变。

AGV轨迹跟踪控制系统包括两个部分，位置的检测和轨迹的跟踪。系统控制策略的设计采用现代控制策略，将卡尔曼滤波器和二次线型最优控制(LQI)相结合的控制方法，二次线型最优控制根据最优控制指标改变控制量，使AGV的性能最优化，而卡尔曼滤波器的引入可以去除轨迹测量过程中采集到的波动和干扰，使最终控制效果最佳。

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

Claims (2)

1.红外光电导向AGV装置的控制方法，红外光电导向AGV装置包括车体、驱动轮和从动轮以及铺设于地面的导引反光带，在车体底部的四端角位置各安装一从动轮，车体底部的中间部位沿纵向安装有一组呈对称分布的驱动轮，驱动轮均与无刷直流电机驱动控制连接，每只驱动轮的内侧设有速度检测装置，车体底部的中间部位沿横向安装有一组呈对称分布的红外光电导向装置，红外光电导向装置的内侧安装有红外标志检测装置，车体底部的四端角位置还各安装一避障传感器，其特征在于：按照预设路径的轨迹在地面铺设导引反光带，在需要铺设动作标志的位置设置动作标志；

(1)AGV装置运行中，车体相对导引路径的偏移信息通过红外光电导向装置进行检测，红外光电导向装置发射红外光线，红外光线经过导引反光带的发射得到车体相对导引反光带的位置信息；位置信息的检测步骤是：在AGV相对导引路线所在的运动线路上，定义实际位置和目标位置之间的误差为位置误差，将AGV的运动位置表示为(d,α)，其中d为AGV偏移导引路线的法相位置误差，α为AGV的方位角误差，所述的方位角为AGV的当前行进路线与预期轨迹也就是导引路线之间的夹角；要求AGV沿着预定的导轨路线循迹前进，设置所述的AGV的目标位置为(0,0),由红外光电导向装置检测AGV起点的位置是(0,0)，根据路径的规划，预先设置两个驱动轮之间的距离是L，前后两个位置检测装置之间的距离是D；

目标位置有两种情况，当沿着指导路径循迹时，目标位置为(0,0)；当在一些特定转弯动作标志位置时，AGV的目标位置为(d,α)；

在需要转弯的情况下导引车的控制采用前馈控制，在检测到动作标志时，加入对应动作的前馈控制量，当行驶过动作标志进入循迹过程时，恢复正常循迹；

根据AGV车体运行时的相对位置的几何平面，测量AGV的导轨跟踪误差的具体步骤是：AGV的前后两端各配置一个红外光电导向装置，当车辆行驶时，红外光电导向装置的发射器按照固定的频率发射红外光，接收光电二极管采集反光导引路线反射回来的红外光点；

两个红外光电导向装置之间的距离固定为D；

预处理AGV车体行驶过程中在同一时刻采集到的从导引路线反射回来的红外光点序列，接收光电二极管之间的距离固定，根据接收到的发射光点序列得到偏移距离，则前后两个红外光电导向装置采集到的偏移距离分别是D1，D2；设定，当AGV车体偏向左边时候，测得偏移量是负值，当AGV的车体偏向右边时候，测得偏移量是正值，当车体现行轨迹与导引路线的夹角偏向导引路线左侧为逆时针的时候，角度为负值，当偏向右侧顺时针的时候，角度为正值；

(2)检测得到的数据通过红外光电导向装置的标准通信串口和主控单元MCU通信，将位置信息输入主控单元MCU进行处理；

位置信息的处理通过以下步骤实现：

根据几何关系推算出；

$L_1=\frac{D{D}_1}{D_1-D_2}$

$L_2=\frac{D{D}_2}{D_1-D_2}$

$\mathrm{\θ}=\arctan \frac{D_2}{L_2}=\arctan \frac{D_1}{L_1}$

d₁＝sinθ×D₁

d₂＝sinθ×D₂

其中，L1是前端红外光电导向装置沿两个红外光电导向装置中心线到导引反光带之间距离，L2是后端红外光电导向装置沿两个红外光电导向装置中心线到反光带之间距离，θ为偏转角；d₁是导引线到车体两个红外光电导向装置的中心点的垂直距离，同理，d₂是导引线到车体两个红外光电导向装置中心点的垂直距离；

(3)将得到的偏移量和偏转角经卡尔曼滤波器进行滤波，得到平滑的变化曲线，进一步修正AGV与导引路线的误差；

(4)AGV运行的过程中，在一些特定位置要求AGV执行特定的动作，在这些特定位置设置动作标志，动作标志是间隔性的等距反光带，由一系列宽度相同的反光带或者非反光带按顺序排列组成；

所述标志检测通过以下步骤；

根据红外标志检测装置检测到的标志的数字信号通过串口输入主控单元MCU，主控装置判断动作标志代表的意义然后发出对应的动作控制命令，AGV做出相对应的动作，在AGV运行的过程中，在一些特定位置要求AGV执行特定的动作，在特定位置设置动作标志，当光线发射到反光带上时，接收管接收到反射光，判断为1，相反，判断为0；

当AGV沿着导引路径以固定的速度v行进时，经过每一个小段反光带的时间是相同的t，则根据在每一段时间t内检测到的高低电平的不同对标志做出判断；

标志检测装置检测到动作标志的信号是011 01010，所代表的动作是启动；

将启动动作信号发送到主控单元MCU，主控板发出启动命令；

在有具体动作控制位置，AGV的导向控制器与直线循迹控制器相同，控制器的前端设有前馈控制；

使AGV的目标位置改变，以左转动，目标位置为(-d,-α),AGV将以目标位置为终点前进；

根据检测到的不同的动作标志信号，代表不同的动作意义，将检测到的动作标志通过串口发送到主控单元MCU，

主控制单元经过处理之后得到控制量，将控制量发送到驱动，驱动车体向目标位置运行；

当需要改变特定位置的动作时候，改变动作标志，或者通过人机界面对标志所代表的动作的意义加以改变；

(5)由避障传感器通过红外检测到的距离的远近产生大小不同的模拟量信号，通过AD转换接口与主控单元MCU相连接，主控制装置将模拟信号转换为对应的数字信号也就是距离信息，根据距离的远近，然后发出转弯、减速或者刹车的命令到驱动轮控制器，使AGV系统避开障碍物减小损害；

(6)速度检测装置为脉冲信号检测装置，对单位时间内接收到的脉冲信号个数进行统计，并通过SPI接口发送到主控制单元，主控单元MCU计算出电机的实际转速。

2.根据权利要求1所述的红外光电导向AGV装置的控制方法，其特征在于：轨迹跟踪控制策略采用二次线型最优控制和卡尔曼滤波器控制相结合的LQG控制策略；在AGV运动所在的平面直角坐标系中，所述的位置检测装置检测到的偏移导引路径的偏移量d和偏转角α；

所述路径跟踪控制方法步骤如下；

控制系统的建模：

AGV在初始状态时没有偏差，由于外部干扰的影响，AGV的路径随着时间的推移出现略微的偏差，偏差的角度为参考轨迹和小车移动的中心线的夹角，偏移的距离由两个轮子中心点和参考轨迹确定，左驱动轮的速度是V_L，右边驱动轮的速度是V_R，在一个非常短的时间△t内，速度的大小和方向都没有变化，由几何关系推导出以下信息；

$V_c=\frac{V_R+V_L}{2}$

$\mathrm{\Δ\θ}=\frac{(V_R-V_L)\×\mathrm{\Δt}}{L}$

$\mathrm{\Δd}=\frac{V_R+V_L}{2}\×\mathrm{\Δt}\×\sin \mathrm{\θ}$

在上述公式中，L是两个驱动轮之间的距离，当t接近0的时候，得到偏移距离和偏移角度的微分方程：

$\mathrm{d\θ}=\frac{V_R-V_L}{L}\mathrm{dt}$

$\mathrm{dd}=\frac{V_R+V_L}{2}\sin \mathrm{\θdt}$

对上式进行拉普拉斯变换，上述公式化为；

$d\left(s\right)=\frac{(V_R+V_L)\sin \left(\mathrm{\θ}\right)}{2s}$

$\mathrm{\θ}\left(s\right)=\frac{V_R-V_L}{\mathrm{Ls}}$

当公式中的角度θ足够小时，公式近似化为；

$d\left(s\right)=\frac{(V_R+V_L)\mathrm{\θ}}{2s}$

AGV控制系统的数学模型中，两个轮子由两个直流无刷电机分别驱动，假定电机和轮子以相同的速度n运行，轮半径是r，电机的控制输入是U，电机的负载时间常数是Tm，左右电机的参数分别是T_mL和T_mR，左右电机的传递函数简化为：

$n_R=\frac{k_R}{1+T_{\mathrm{mR}}s}{U}_R$

$n_L=\frac{k_L}{1+T_{\mathrm{mL}}s}{U}_L$

在上述公式中，k_L和k_R是与电机的反电动势相关的常数，因两电机的参数相同，所以

T_mR＝T_mL＝T_m

此外，考虑到轮子的线速度和角速度之间的关系，得到左右轮的线速度如下所示，

$V_R=\frac{k}{1+T_{m}s}{U}_R$

$V_L=\frac{k}{1+T_{m}s}{U}_L$

系统包含θ与V_R+V_L相乘因子，系统属于非线性系统，AGV是在一种固定的规则下运行，控制输入视为一个固定速度控制输入和一个小的循迹控制输入量，因此，该非线性系统采用小误差系统的方法，将非线性系统转换成线性系统并进行设计；

无外部的干扰，左轮和右轮的给定信号是相同的，V_R＝V_L＝V_c，AGV的速度V_c是由常量U_c产生的，视为速度为常量的匀速直线运动，当AGV离开预定的轨迹，给定信号将增加△U调节循迹；

U_R＝U_C+△U

U_L＝U_C-△U

电机线性速度的相应输出是：

V_R＝V_c+△V

V_L＝V_c-△V

在正常条件下AGV中心位置速度Vc是一个常数，假设状态变量分别是X₁＝△V,X₂＝θ,X₃＝d得状态函数如下：

$\left[\begin{array}{c}{\stackrel{.}{X}}_1\\ {\stackrel{.}{X}}_2\\ {\stackrel{.}{X}}_3\end{array}\right]=\left[\begin{array}{ccc}-\frac{1}{T_m}& 0& 0\\ \frac{1}{L}& 0& 0\\ 0& V_c/2& 0\end{array}\right]\left[\begin{array}{c}{X}_1\\ X_2\\ X_3\end{array}\right]+\left[\begin{array}{c}k\\ 0\\ 0\end{array}\right]\mathrm{\ΔU}$

控制器的设计通过以下步骤实现：

采用LQI控制方法进行导向控制，LQI伺服控制系统引入输出误差的积分作为额外的状态变量，扩展系统为；

$\frac{d}{\mathrm{dt}}\left[\begin{array}{c}x\left(t\right)\\ x_r\left(t\right)\end{array}\right]=\left[\begin{array}{cc}A& 0\\ -C& 0\end{array}\right]\left[\begin{array}{c}x\left(t\right)\\ x_r\left(t\right)\end{array}\right]+\left[\begin{array}{c}B\\ 0\end{array}\right]u\left(t\right)+\left[\begin{array}{c}0\\ I\end{array}\right]r\left(t\right)$

式中

$A=\left[\begin{array}{ccc}-\frac{1}{T_m}& 0& 0\\ \frac{1}{L}& 0& 0\\ 0& \frac{V_c}{2}& 0\end{array}\right]$ $B=\left[\begin{array}{c}k\\ 0\\ 0\end{array}\right]$ C＝[0 0 1]

基于二次型目标函数的相对于系统的最优控制问题，最优性能指标值为；

$J={\∫}_0^{\∞}[x_a^{T}Q{x}_a\left(t\right)+u^T\left(t\right)\mathrm{Ru}\left(t\right)]\mathrm{dt}$

式中 $x_a=\left[\begin{array}{cc}A& 0\\ -C& 0\end{array}\right],$ Q，R为加权对角矩阵，阶数分别与x_a和u对应；

用状态反馈增益及控制输入表示为；

$u\left(t\right)=-F_{1}x\left(t\right)-F_2{x}_r\left(t\right)=-F_{1}x\left(t\right)-F_2{\∫}_0^{t}e\left(t\right)\mathrm{dt}$

F＝[F₁ F₂]＝R^-1B^#TP

$B^{\#}=\left[\begin{array}{c}B\\ 0\end{array}\right],$ P为对应的代数里卡蒂方程的解；

在上述最优控制器中，虽然传感器测量到偏移量和偏转角，但是在此过程中，由于外界的干扰，电压的波动，反光导引带的起伏因素引起的扰动，采用卡尔曼滤波器对中间的噪声进行处理；

卡尔曼滤波器的设计通过以下步骤实现；

对于偏移量和偏转角两个变量，传感器直接测量得到，而在轨迹跟踪的过程中，实际的AGV系统具有一系列不确定性，包括路面的随机干扰输入、传感器的测量噪声以及模型的不确定性，各种过程噪声和观测噪声的干扰，将使状态变量的测量结果出现偏差，卡尔曼滤波器使用的条件是系统噪声和测量噪声均为白噪声，卡尔曼滤波器进行滤波；

根据分离定理，对系统的状态变量X进行估计，表达式为

$\stackrel{.}{\hat{X}}=A\hat{X}+\mathrm{BU}+K_1(Y-C\hat{X})$

为状态量的估计，K₁为滤波器增益

$K_1=P_l{B}^T{R}_f^{-1}$

式中P_l为对应代数利卡迪方程的解，R_f对应的加权矩阵；

根据分离原理，将线性二次最优控制器和卡尔曼滤波器相结合，即为线性二次高斯LQG控制器；

根据设计的控制器，将位置检测装置测量到的相对轨迹的偏移量和偏移角进行处理，计算相应的控制量，并将控制量发送到驱动单元对AGV进行纠偏处理；

以AGV速度是1m/s的速度行走，让AGV从某种初始状态出发跟踪设定的直线路径，初始误差设置为d＝0，角度为θ＝0；根据实验结果，得到较佳的结果；

根据结果得出，AGV在一个较短的时间内达到稳定状态，且在随后的行驶过程中，误差和控制量在一个较小的范围内变化。