一种融合TEB算法和模糊控制的AGV轨迹跟踪控制方法技术

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

【技术实现步骤摘要】
一种融合TEB算法和模糊控制的AGV轨迹跟踪控制方法


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

技术介绍

[0002]随着“工业4.0”时代的到来和科技水平不断的进步，国家陆续发布《中国制造2025》、《机器人产业发展规划》和《新一代人工智能发展规划》等一系列促进机器人产业发展的政策。这些政策促进自动导引小车(Automated Guided Vehicle，AGV)技术迅速地发展，如今，AGV已被广泛应用于物流、制造、服务、军事等多种领域。轨迹跟踪控制是AGV运动控制的关键环节之一，其跟踪的精确度很大程度上对AGV是否能够高效快捷、精准智能完成仓储作业任务起着至关重要的作用。
[0003]TEB(Timed Elastic Band)算法是由经典的“Elastic Band”算法发展而来，它用时间信息强化了原本的EB算法。与其它局部路径规划算法相比，TEB算法最大的特点是实时性强，适用性广，其可以根据不同环境下的不同需求，添加或删减若干约束条件，具有高度的灵活性。TEB算法是基于稀疏结构的多目标优化问题，Rosmann等提出使用g2o框架解决了基于超图的TEB非线性优化问题。
[0004]模糊控制是建立在人工经验基础上的控制方法，即从行为上模仿人的模糊推理和决策过程。该方法首先将熟练的操作人员丰富的实践经验编成模糊规则，然后用模糊数学将其量化，转换为机器可以识别的数学函数完成模糊推理，最终实现对机器的控制。模糊控制抗干扰能力很强，能够解决传统控制理论不能解决的一些问题。模糊控制具有较好的柔性和自适应能力，只需要模拟人类的思维方式和处理问题的习惯，不需要确定的数学模型就能够根据经验解决问题。

技术实现思路

[0005]为了克服现有技术中的不足，本专利技术旨在将TEB局部路径规划算法和AGV轨迹跟踪模糊控制相结合，提出了一种融合TEB算法和模糊控制的AGV轨迹跟踪控制方法，通过用轨迹跟踪模糊控制器跟踪TEB算法得到的最优轨迹序列，实现对差速AGV的轨迹纠偏，提高算法的控制精度。
[0006]为了达到上述专利技术目的，解决其技术问题所采用的技术方案如下：
[0007]一种融合TEB算法和模糊控制的AGV轨迹跟踪控制方法，包括以下步骤：
[0008]步骤1：建立AGV运动学状态方程，构建TEB算法多目标优化问题；
[0009]步骤2：设定目标点，构建TEB算法求解模型，每个迭代周期得到优化后的最优轨迹序列；
[0010]步骤3：基于所述的最优轨迹点序列和反馈的实际轨迹点序列，计算距离误差和角度误差，设计轨迹跟踪模糊控制器，包括输出参数模糊化处理、设计模糊规则和进行模糊推理；
[0011]步骤4：控制器输出AGV运动的控制量，通过运动学方程转换成差速AGV左右轮的运动参数并传递给受控对象，组成TEB算法规划最优轨迹点、轨迹跟踪模糊控制器、AGV运动学方程和被控对象的闭环控制系统。
[0012]进一步的，所述步骤1中使用的是差速AGV，设AGV小车在t时刻的位姿状态量为S(t)＝(x
t
，y
t
，θ
t
)
T
，为位姿状态的导数，可以建立差速AGV的运动学状态方程如下：
[0013][0014]构建TEB轨迹模型来修饰全局路径，将位姿轨迹模型转换成带时间信息的弹性带模型，AGV的第i个位姿状态可以定义为：S
i
＝(x
i
，y
i
，θ
i
)
T
，AGV在世界坐标系下的位姿序列为：Q＝{S
i
}，i＝1，2，...，n，n个位姿间隔的时间序列为：τ＝{ΔT
i
}，i＝1，2，...，n
‑
1，并将位姿序列与时间序列合并：
[0015]B：＝(Q，τ)＝[S1，ΔT1，S2，ΔT2，...，ΔT
n
‑1，S
n
]T
ꢀꢀꢀ
(2)
[0016]TEB多目标轨迹优化问题的约束条件包括速度约束、加速度约束、运动学约束等，惩罚函数包括最快路径惩罚函数和靠近障碍物的惩罚函数，添加时间最优的约束函数，满足路径上所有时间间隔ΔT之和最小，目标约束函数可表示为：
[0017][0018]添加避开障碍物的约束函数，约束函数以惩罚函数的形式来体现：
[0019][0020]其中，d
min，j
表示第j个障碍物与其最近的TEB轨迹点的距离；表示TEB轨迹点与障碍物之间被允许的最小距离；S为形变因子，n为多项式系数，通常取值为2，ε为边界值附近一个小位移，即偏移因子；
[0021]添加速度与加速度约束：差速AGV移动过程中，其速度和加速度需要满足运动学约束关系，可以根据两个连续位姿S
i
，S
i+1
和两个位姿之间的时间间隔ΔT
i
计算得到平均线速度和角速度：
[0022][0023]整车平均线速度和角速度的惩罚函数分别可表示为：
[0024][0025]线加速度a
i
和角加速度α
i
可由三个连续AGV位姿状态以及相应的两个时间间隔序列表示：
[0026][0027]相应的目标约束函数可表示为：
[0028][0029]添加非完整运动学约束：差速AGV为非完整运动学的机器人，其相邻两个空间位姿S
i
和S
i+1
的移动可近似为曲率不变的弧线运动，曲率不变的弧线运动满足位姿S
i
与运动方向d
i，i+1
的夹角θ
i
应和位姿S
i+1
对应的运动方向角θ
i+1
相等，相应的非完整运动学目标约束函数可表示为：
[0030][0031]进一步的，所述步骤2中，为TEB算法设置一个目标点，并构建TEB算法的求解模型，对所述的目标约束函数按不同权重进行实时优化，通过加权多目标优化获取最优路径，目标函数数学描述可表示为：
[0032][0033]其中，f(B)表示分量f
k
的加权和，f
k
是各种约束目标函数，γ
k
是函数f
k
的权重系数，B
*
表示优化的TEB结果，即每个周期得到优化后的最优轨迹序列；
[0034]上述目标约束函数只与某几个连续的位姿状态有关，而不是整条轨迹上的位姿，这种局部性导致系统稀疏矩阵的产生，因此可以使用解稀疏优化方法—g2o图优化框架来求解非线性优化问题。
[0035]进一步的，所述步骤3中，经实时反馈的轨迹序列与步骤2得到的最优轨迹序列相比较，计算得出误差信号；设计轨迹跟踪模糊控制器，具体包括：将误差信号经过模糊化转换本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种融合TEB算法和模糊控制的AGV轨迹跟踪控制方法，其特征在于，包括以下步骤：步骤1：建立AGV运动学状态方程，构建TEB算法多目标优化问题；步骤2：设定目标点，构建TEB算法求解模型，每个迭代周期得到优化后的最优轨迹序列；步骤3：基于所述的最优轨迹点序列和反馈的实际轨迹点序列，计算距离误差和角度误差，设计轨迹跟踪模糊控制器，包括输出参数模糊化处理、设计模糊规则和进行模糊推理；步骤4：控制器输出AGV运动的控制量，通过运动学方程转换成差速AGV左右轮的运动参数并传递给受控对象，组成TEB算法规划最优轨迹点、轨迹跟踪模糊控制器、AGV运动学方程和被控对象的闭环控制系统。2.根据权利要求1所述的一种融合TEB算法和模糊控制的AGV轨迹跟踪控制方法，其特征在于，所述步骤1中使用的是差速AGV，设AGV小车在t时刻的位姿状态量为S(t)＝(x
t
，y
t
，θ
t
)
Τ
，为位姿状态的导数，可以建立差速AGV的运动学状态方程如下：构建TEB轨迹模型来修饰全局路径，将位姿轨迹模型转换成带时间信息的弹性带模型，AGV的第i个位姿状态可以定义为：S
i
＝(x
i
，y
i
，θ
i
)
Τ
，AGV在世界坐标系下的位姿序列为：Q＝{S
i
}，i＝1，2，...，n，n个位姿间隔的时间序列为：τ＝{ΔT
i
}，i＝1，2，...，n
‑
1，并将位姿序列与时间序列合并：B：＝(Q，τ)＝[S1，ΔT1，S2，ΔT2，...，ΔT
n
‑1，S
n
]
T
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(2)TEB多目标轨迹优化问题的约束条件包括速度约束、加速度约束、运动学约束等，惩罚函数包括最快路径惩罚函数和靠近障碍物的惩罚函数，添加时间最优的约束函数，满足路径上所有时间间隔ΔT之和最小，目标约束函数可表示为：添加避开障碍物的约束函数，约束函数以惩罚函数的形式来体现：其中，d
min，j
表示第j个障碍物与其最近的TEB轨迹点的距离；表示TEB轨迹点与障碍物之间被允许的最小距离；S为形变因子，n为多项式系数，通常取值为2，ε为边界值附近一个小位移，即偏移因子；添加速度与加速度约束：差速AGV移动过程中，其速度和加速度需要满足运动学约束关系，可以根据两个连续位姿S
i
，S
i+1
和两个位姿之间的时间间隔ΔT
i
计算得到平均线速度和角速度：
整车平均线速度和角速度的惩罚函数分别可表示为：线加速度a
i
和角加速度α
i
可由三个连续AGV位姿状态以及相应的两个时间间隔序列表示：相应的目标约束函数可表示为：添加非完整运动学约束：差速AGV为非完整运动学的机器人，其相邻两个空间位姿S
i
和S
i+1
的移动可近似为曲率不变的弧线运动，曲率不变的弧线运动满足位姿S
i
与运动方向d
i，i+1
的夹角θ
i
应和位姿S
i+1
对应的运动方向角θ
i+1
相等，相应的非完整运动学目标约束函数可表示为：3.根据权利要求2所述的一种融合TEB算法和模糊控制的AGV轨迹跟踪控制方法，其特征在于，所述步骤2中，为TEB算法设置一个目标点，并构建TEB算法的求解模型，对所述的目标约束函数按不同权重进行实时优化，通过加权多目标优化获取最优路径，目标函数数学描述可表示为：其中，f(B)表示分量f
k
的加权和，f
k...

【专利技术属性】
技术研发人员：易广政，吴佳峰，马向华，狄逸群，陈浩，
申请(专利权)人：上海应用技术大学，
类型：发明
国别省市：