基于自抗扰控制算法的单舵轮AGV控制系统设计

2018-05-16

单片机与嵌入式系统应用 2018年5期

(济南二机床集团有限公司，济南 250022)

引言

随着科学技术的快速提升，工业4.0和智能工厂的概念在工业生产中占有了越来越重要的地位，越来越多的机器人在工业生产中代替人工完成了重复、单调以及危险的工作，节省人力成本的同时，提升了工作效率。压力机行业中，由线首的拆垛机器人、板料传送系统、机器人上料系统、压力机间单元、线末的压力机下料单元以及自动运料AGV小车组成的机器人冲压自动化系统也得到了越来越多企业的认可[1]。AGV(Automated Guided Vehicle)，即自动引导小车，是该系统中的一个重要组成部分，运行中背载或牵引着物料沿预设的导引路线行驶。

在复杂的车间环境中，要求AGV能够：①准确、快速到达指定位置；②遇到障碍物时能够自动停车；③能到达并自动识别不同的站点，然后做出相应的动作。

随着电子技术的不断发展，出现了磁条(辅以RFID站点)、惯性导航或激光导航等自动导引装置[2]，激光导航凭借其不需要地面辅助定位、可随时改变行驶路径的优点，得到了广泛的应用[3]。

本文基于激光SLAM(Simultaneous Localization And Mapping，SLAM)导航原理和自抗扰控制算法，设计了一种AGV自动控制系统，实现了AGV自动路径跟踪控制。该系统只需要通过上层调度软件更新位置坐标信息即可改变AGV行驶路径或停靠站点。所设计的AGV控制系统结构框图如图1所示。

图1 控制系统结构框图

1 单舵轮AGV运动学数学模型

本系统选用的AGV包含一个能控制前进方向的舵轮和两个固定的从动轮[4-5]，依靠前轮电机旋转产生前进的驱动力，并由另一个电机控制前轮转向，从而实现AGV方向控制，即所谓的单舵轮结构形式，其结构如图2所示。

图2 单舵轮AGV结构模型

单舵轮AGV的数学模型主要与舵轮和从动轮的相对位置有关，其中O1和O2分别表示舵轮的回转中心以及从动轮的几何中心，b是AGV回转中心和从动轮几何中心的距离，通常用轴距表示。可通过控制舵机旋转角度β和行进电机转速，实现对AGV的路径跟踪控制。将O2作为AGV的参考点，则AGV的运动数学模型可描述为：

(1)

式中(x,y)为AGV参考点O2在坐标系中的位置，vW为行进电机的线速度，ω为车体回转的角速度，并规定舵轮和车体逆时针旋转为正方向，旋转的中心定位为O′，回转半径为R，θ为坐标系下车体的朝向。在全局坐标系下，AGV的运动姿态可用[x,y,θ]T来表达。

2 AGV控制系统设计

2.1 导航方式

激光SLAM导航不需要在地面或车间做任何定位标记，属于无轨导航，因此非常便于调整或修改AGV运行路径，得到了广泛的应用。SLAM最早由Smith R等专家提出，能让载有激光传感器的机器人在未知环境中和未知的初始位置条件下分析提取环境特征，并通过特征对比感知环境并构建地图，然后再确定本体在地图中的位置[6]。Gmapping是目前2D激光SLAM中普遍使用的算法，其中采用Rao-Blackwellized 粒子滤波(Rao-Blackwellized particle filtering，RBPF)的方法，引入自适应重采样机制解决了SLAM过程中粒子消耗的问题[7]。

如图3所示是本系统基于RBPF和数据融合方法设计的SLAM定位系统工作原理框图，其工作中分为5个步骤：采样、权重计算、扫描匹配、重采样与数据融合、更新地图。系统中选用了Sick公司的NAV350激光雷达测量车间环节的结构化信息，配合9轴运动姿态传感器以及电机编码器构成的里程计进行导航数据融合，从而提供准确的定位数据。同时激光传感器还能提供AGV运行中遇到的静态障碍物和动态障碍物信息，保障AGV的安全性能。

图3 激光定位系统工作原理框图

在实际应用中，可以首先建立车间的导航地图，然后通过上层调度软件在地图中规划运行路线和停靠站点，最后将设定路径的起始点、目标点、目标点的朝向角、路径类型、AGV运行速度以及停靠站点以及停靠时间等信息传送给AGV控制系统即可。

2.2 控制问题分析

考虑到单舵轮AGV的运动性能，将AGV的运动分解为直线行驶(直线运动)和转弯(圆弧运动)两个过程。行驶过程中，vW由行进电机的旋转速度决定，对路径跟踪系统来说是一个常量。直接路径跟踪中的位置偏差计算方法介绍如下。

(1)直线路径跟踪

AGV起始点运动姿态为[x1,y1,θ1]T，目标点期望姿态[x2,y2,θ2]T，当前运动姿态[x,y,θ]T，则运行过程中设定路径的直线方程Ax+By+C=0中各参数为：

(2)

其中y2≠y1，x2≠x1,从而得到AGV到直线路径上期望位置的偏差为：

errdist=(Ax+By+C)×cos(atan2(A,-B))

(3)

(2)圆弧路径跟踪

当AGV的行进速度vW和舵角β不变时，AGV会沿中心点O'做半径为R回转运动。因此，为了让AGV沿指定的圆弧运动，需要根据起始姿态和目标姿态计算AGV的舵角初始值。因起始位置和目标位置是圆弧上的两个点，且圆心处于目标位置的垂线上，根据几何知识可以得到经过圆心和目标位置的直线方程为：

-tan(θ2+pi/2)x+y+tan(θ2+pi/2)x2-y2=0

(4)

根据起始点、目标点以及圆心三者构成的三角形，可以计算出回转半径R和圆心坐标[xcir,ycir]，然后可计算出AGV沿圆弧行进中的位置偏差：

(5)

2.3 控制算法设计

控制算法的设计目标是让运行过程中位置以及朝向角偏差趋向于0。PID控制算法不依赖于被控对象精确的数学模型，在目前工业控制中占有非常重要的地位，但在激光SLAM导航方式中，容易因为地图更新不及时或外部扰动出现AGV定位信息的波动，PID算法中的微分环节会将这种扰动放大，非常容易引起震荡。自抗扰控制算法除继承了PID算法的优点外，增加了跟踪微分器环节使给定输入变得更平滑，另外，还通过状态观测器补偿和消除系统中未建模的部分以及运行中收到的内部扰动和外部扰动，进而得到了较好的控制效果[8]。

由AGV数学模型公式可知，该系统是一个关于舵角β的二阶系统，而舵机运动过程是一个滞后环节，AGV运动过程中的负载变化以及激光SLAM位置抖动等都会给系统带来干扰，常规的线性系统控制算法难以取得优良的控制效果。因此考虑充分利用自抗扰控制器的能自动补偿和消除扰动的优点实现AGV的自动路径跟踪控制，本项目中设计的控制系统结构如图4所示。

图4 路径跟踪自抗扰控制器结构

该控制系统由AGV行进速度和路径跟踪两个子系统组成。行进速度控制器实现AGV的行进电机转速的自抗扰控制，而路径跟踪控制器则通过跟踪路径位置偏差不断修正转舵角度，进而保证AGV沿设定路线行驶。离散化的自抗扰控制算法如下所示：

(6)

式中,r0称为快速因子，与电机功率有关;h为算法采样时间，非线性函数fhan(·)以及fal(·)的表示形式可参考相关文献。需要调整的参数有r0、α1、α2、β01、β02、β03、b0，可按照分离性原理逐个调试。

2.4 电路设计与实现

该系统以STM32F767为核心控制器，采用模块化的设计方法，各模块基于车载无线路由器实现数据的交互。STM32F767是意法半导体公司生产的Cortex-M7内核的32位ARM控制器，其CPU运行频率高达216 MHz，内部带有双精度浮点数字单元，能满足系统运行控制算法的需求。具备多个串口、CAN控制器、USB接口、以太网控制器以及TFT控制器等片内外设资源，外围设备运行速度独立于CPU频率。STM32F767通过LAN8720以太网接口电路和无线路由器获取激光SLAM的定位数据以及避碰信息。但为了防止激光SLAM对复杂多变的工业现场环境处理不及时，增加了实时性更高的碰撞传感器作为避碰急停保护，从而保护现场人员、环境以及车体的安全。为了便于现场人员管理和操作AGV，还设计了以无线遥控电路以及触摸屏构成的人机接口单元电路。限于篇幅，仅给出控制器部分电路原理如图5所示。

3 调试及运行结果

为了验证本文设计的AGV控制系统的有效性，首先在仿真环境下调整了控制器参数，验证了控制器对外部扰动及内部数学模型不确定性的控制性能，然后将算法移植到设计的硬件平台，并在车间环境下进行了实验验证。

图5 系统电路原理图

图6 仿真环境下AGV控制性能测试

3.1 仿真验证

仿真测试中，选用了转弯时典型的“直线-圆弧-直线”路径。初始仿真条件以及目标点信息设置如下：

(7)

通过图6可以看出，AGV在初始位置开始运动时会冲出设定路径，这是由单舵轮结构和初始朝向角误差较大原因引起的。而随后AGV能迅速运动到所设定的路线上，然后沿所设定的路线运动，运行过程误差以及超调都非常小，说明所设计的系统能够实现AGV的自动控制。

3.2 车间测试

为了测试该系统在实际应用中的控制性能，在车间环境下，通过调度软件给AGV设计了一条圆环形的路线，来测试AGV控制器的控制精度和重复性。所设计的路径中直线部分长度为20 m，4个拐角分别设计了半径为1.5 m、1 m以及2 m的圆弧路径，循环运行5圈为一个周期，行驶速度设为0.4 m/s。然后通过遥控器将AGV小车移动到三个不同的初始位置进行测试。