[发明专利]一种融合TEB算法和模糊控制的AGV轨迹跟踪控制方法

说明书

本发明公开了一种融合TEB算法和模糊控制的AGV轨迹跟踪控制方法，包括步骤1：建立AGV运动学状态方程，构建TEB算法多目标优化问题；步骤2：设定目标点，构建TEB算法求解模型，每个迭代周期得到优化后的最优轨迹序列；步骤3：基于所述的最优轨迹点序列和反馈的实际轨迹点序列，计算距离误差和角度误差，设计轨迹跟踪模糊控制器，包括输出参数模糊化处理、设计模糊规则和进行模糊推理；步骤4：控制器输出AGV运动的控制量，通过运动学方程转换成差速AGV左右轮的运动参数并传递给受控对象，组成TEB算法规划最优轨迹点、轨迹跟踪模糊控制器、AGV运动学方程和被控对象的闭环控制系统。从而实现对差速AGV的轨迹纠偏，提高算法的控制精度。

技术领域

本发明涉及AGV系统的局部路径规划和轨迹跟踪控制的技术领域，特别涉及一种融合TEB算法和模糊控制的AGV轨迹跟踪控制方法。

背景技术

随着“工业4.0”时代的到来和科技水平不断的进步，国家陆续发布《中国制造2025》、《机器人产业发展规划》和《新一代人工智能发展规划》等一系列促进机器人产业发展的政策。这些政策促进自动导引小车(Automated Guided Vehicle，AGV)技术迅速地发展，如今，AGV已被广泛应用于物流、制造、服务、军事等多种领域。轨迹跟踪控制是AGV运动控制的关键环节之一，其跟踪的精确度很大程度上对AGV是否能够高效快捷、精准智能完成仓储作业任务起着至关重要的作用。

TEB(Timed Elastic Band)算法是由经典的“Elastic Band”算法发展而来，它用时间信息强化了原本的EB算法。与其它局部路径规划算法相比，TEB算法最大的特点是实时性强，适用性广，其可以根据不同环境下的不同需求，添加或删减若干约束条件，具有高度的灵活性。TEB算法是基于稀疏结构的多目标优化问题，Rosmann等提出使用g2o框架解决了基于超图的TEB非线性优化问题。

模糊控制是建立在人工经验基础上的控制方法，即从行为上模仿人的模糊推理和决策过程。该方法首先将熟练的操作人员丰富的实践经验编成模糊规则，然后用模糊数学将其量化，转换为机器可以识别的数学函数完成模糊推理，最终实现对机器的控制。模糊控制抗干扰能力很强，能够解决传统控制理论不能解决的一些问题。模糊控制具有较好的柔性和自适应能力，只需要模拟人类的思维方式和处理问题的习惯，不需要确定的数学模型就能够根据经验解决问题。

发明内容

为了克服现有技术中的不足，本发明旨在将TEB局部路径规划算法和AGV轨迹跟踪模糊控制相结合，提出了一种融合TEB算法和模糊控制的AGV轨迹跟踪控制方法，通过用轨迹跟踪模糊控制器跟踪TEB算法得到的最优轨迹序列，实现对差速AGV的轨迹纠偏，提高算法的控制精度。

为了达到上述发明目的，解决其技术问题所采用的技术方案如下：

一种融合TEB算法和模糊控制的AGV轨迹跟踪控制方法，包括以下步骤：

步骤1：建立AGV运动学状态方程，构建TEB算法多目标优化问题；

步骤2：设定目标点，构建TEB算法求解模型，每个迭代周期得到优化后的最优轨迹序列；

步骤3：基于所述的最优轨迹点序列和反馈的实际轨迹点序列，计算距离误差和角度误差，设计轨迹跟踪模糊控制器，包括输出参数模糊化处理、设计模糊规则和进行模糊推理；

步骤4：控制器输出AGV运动的控制量，通过运动学方程转换成差速AGV左右轮的运动参数并传递给受控对象，组成TEB算法规划最优轨迹点、轨迹跟踪模糊控制器、AGV运动学方程和被控对象的闭环控制系统。

进一步的，所述步骤1中使用的是差速AGV，设AGV小车在t时刻的位姿状态量为S(t)＝(x_t，y_t，θ_t)^T，为位姿状态的导数，可以建立差速AGV的运动学状态方程如下：