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

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


背景技术：

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


技术实现要素：

5.为了克服现有技术中的不足，本发明旨在将teb局部路径规划算法和agv轨迹跟踪模糊控制相结合，提出了一种融合teb算法和模糊控制的agv轨迹跟踪控制方法，通过用轨迹跟踪模糊控制器跟踪teb算法得到的最优轨迹序列，实现对差速agv的轨迹纠偏，提高算法的控制精度。
6.为了达到上述发明目的，解决其技术问题所采用的技术方案如下：
7.一种融合teb算法和模糊控制的agv轨迹跟踪控制方法，包括以下步骤：
8.步骤1：建立agv运动学状态方程，构建teb算法多目标优化问题；
9.步骤2：设定目标点，构建teb算法求解模型，每个迭代周期得到优化后的最优轨迹序列；
10.步骤3：基于所述的最优轨迹点序列和反馈的实际轨迹点序列，计算距离误差和角度误差，设计轨迹跟踪模糊控制器，包括输出参数模糊化处理、设计模糊规则和进行模糊推理；
11.步骤4：控制器输出agv运动的控制量，通过运动学方程转换成差速agv左右轮的运动参数并传递给受控对象，组成teb算法规划最优轨迹点、轨迹跟踪模糊控制器、agv运动学方程和被控对象的闭环控制系统。
12.进一步的，所述步骤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得到的最优轨迹序列相比较，计算得出误差信号；设计轨迹跟踪模糊控制器，具体包括：将误差信号经过模糊化转换为模糊语言变量，然后经过模糊推理并通过解模糊转换为控制量；
[0036]
选取轨迹序列的误差e
d
和误差变化率作为控制器的输入，转化后的语言变量分别为e
d
和ec
d
，输出变量为v
x
及ω，其语言变量分别为u
x
和u
ω
，将轨迹序列误差e
d
的基本论域设定在[
‑
0.8m，0.8m]，的实际范围设定为[
‑
0.24m，0.24m]；
[0037]
模糊化处理中，通过量化因子k＝m/|e
max
|将基本论域转换到论域上，定义模糊变量e
d
论域范围为[
‑
6，6]，定义输入ec
d
论域范围为[
‑
3，3]，则其量化因子计算分别如下：
[0038][0039]
对于上述变量e
d
、ec
d
、u
x
以及u
ω
，选择负大(nb)、负中(nm)、负小(ns)、零(zo)、正小(ps)、正中(pm)、正大(pb)这7个变量作为其语言变量，agv以平稳运行为原则，降低误差，因而选择采用高斯型隶属度函数；
[0040]
其中，nb、nm、ns表示参数量的值为规定的负方向，ps、pm、pb表示参数量的值为规定的正方向，zo表示参数量、误差量或变化率为0；
[0041]
根据输入的轨迹序列误差e
d
和误差变化率ec
d
的大小情况，设计轨迹跟踪模糊控制器的控制规则，每条控制规则表示为：if
…
and
…
then
…
的形式，控制器输出控制量通过解模糊环节转换为精确值，选取重心法进行解模糊：
[0042][0043]
通过比例因子k
u
将公式(12)中8转换到对应输出参数的基本论域中：
[0044]
u＝k
u
·
u
ꢀꢀꢀ
(13)
[0045]
对于模糊控制器而言，输出变量u
x
和ω的论域分别为[
‑
6，6]、[
‑
3，3]，可以得到其比例因子分别为：
[0046][0047]
其中，u
n
(x
i
)是第x
i
个语言变量隶属度值；x
i
为输出的模糊量；u为解模糊后的精确值；u是属于被控对象基本论域中的模糊控制器输出量的精确值；k
u
为比例因子。
[0048]
进一步的，所述步骤4中，由步骤3控制器输出变量v
x
和ω经过运动学方程输出v和ω并传递给左右轮，控制agv的速度和转向；假设以向左转弯为例，左轮与右轮的速度可表示为：
[0049][0050]
控制端输入量与两轮速度之间的关系表示为：
[0051][0052]
通过上述步骤，组成teb算法规划最优轨迹点、轨迹跟踪模糊控制器、agv运动学方程和被控对象的闭环控制系统。
[0053]
本发明由于采用以上技术方案，使之与现有技术相比，具有以下的优点和积极效果：
[0054]
本发明提供了一种融合teb算法和模糊控制的agv轨迹跟踪控制方法，通过对teb算法设置目标点，得到一条最优的轨迹序列，并设计轨迹跟踪模糊控制器，包括输入参数模糊化、模糊推理和解模糊来得到agv小车运动的控制量。通过运动学方程转换成差速agv左右轮的运动参数并传递给受控对象，组成teb算法规划最优轨迹点、轨迹跟踪模糊控制器、agv运动学方程和被控对象的闭环控制系统。从而实现对差速agv的轨迹纠偏，提高算法的控制精度。
附图说明
[0055]
为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单的介绍。显而易见，下面描述中的附图仅仅是本发明的一些实施例，对于本领域技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。附图中：
[0056]
图1为本发明实例提供的融合控制方法的流程图；
[0057]
图2是本发明实例提供的差速agv的运动学模型；
[0058]
图3是本发明实例提供的轨迹跟踪模糊控制器的模糊规则。
具体实施方式
[0059]
以下将结合本发明的附图，对本发明实施例中的技术方案进行清楚、完整的描述和讨论，显然，这里所描述的仅仅是本发明的一部分实例，并不是全部的实例，基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明的保护范围。
[0060]
如图1所示，本实施例公开了一种融合teb算法和模糊控制的agv轨迹跟踪控制方法，包括以下步骤：
[0061]
步骤1：建立agv运动学状态方程，构建teb算法多目标优化问题；
[0062]
步骤2：设定目标点，构建teb算法求解模型，每个迭代周期得到优化后的最优轨迹序列；
[0063]
步骤3：基于所述的最优轨迹点序列和反馈的实际轨迹点序列，计算距离误差和角度误差，设计轨迹跟踪模糊控制器，包括输出参数模糊化处理、设计模糊规则和进行模糊推理；
[0064]
步骤4：控制器输出agv运动的控制量，通过运动学方程转换成差速agv左右轮的运动参数并传递给受控对象，组成teb算法规划最优轨迹点、轨迹跟踪模糊控制器、agv运动学方程和被控对象的闭环控制系统。
[0065]
进一步的，所述步骤1中使用的是差速agv，其运动学模型如图2所示，设agv小车在w时刻的位姿状态量为s(t)＝(x
t
，y
t
，θ
t
)
t
，为位姿状态的导数，可以建立差速agv的运动学状态方程如下：
[0066]
[0067]
构建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，并将位姿序列与时间序列合并：
[0068]
b：＝(q，τ)＝[s1，δt1，s2，δt2，...，δt
n
‑1，s
n
]
t
ꢀꢀꢀꢀ
(2)
[0069]
teb多目标轨迹优化问题的约束条件包括速度约束、加速度约束、运动学约束等，惩罚函数包括最快路径惩罚函数和靠近障碍物的惩罚函数，添加时间最优的约束函数，满足路径上所有时间间隔δt之和最小，目标约束函数可表示为：
[0070][0071]
添加避开障碍物的约束函数，约束函数以惩罚函数的形式来体现：
[0072][0073]
其中，d
min，j
表示第j个障碍物与其最近的teb轨迹点的距离；表示teb轨迹点与障碍物之间被允许的最小距离；s为形变因子，n为多项式系数，通常取值为2，ε为边界值附近一个小位移，即偏移因子；
[0074]
添加速度与加速度约束：差速agv移动过程中，其速度和加速度需要满足运动学约束关系，可以根据两个连续位姿s
i
，s
i+1
和两个位姿之间的时间间隔δt
i
计算得到平均线速度和角速度：
[0075][0076]
整车平均线速度和角速度的惩罚函数分别可表示为：
[0077][0078]
线加速度a
i
和角加速度α
i
可由三个连续agv位姿状态以及相应的两个时间间隔序列表示：
[0079][0080]
相应的目标约束函数可表示为：
[0081][0082]
添加非完整运动学约束：差速agv为非完整运动学的机器人，其相邻两个空间位姿s
i
和s
i+1
的移动可近似为曲率不变的弧线运动，曲率不变的弧线运动满足位姿s
i
与运动方向d
i，i+1
的夹角θ
i
应和位姿s
i+1
对应的运动方向角θ
i+1
相等，相应的非完整运动学目标约束函数可表示为：
[0083][0084]
进一步的，所述步骤2中，为teb算法设置一个目标点，并构建teb算法的求解模型，对所述的目标约束函数按不同权重进行实时优化，通过加权多目标优化获取最优路径，目标函数数学描述可表示为：
[0085][0086]
其中，f(b)表示分量f
k
的加权和，f
k
是各种约束目标函数，γ
k
是函数f
k
的权重系数，b
*
表示优化的teb结果，即每个周期得到优化后的最优轨迹序列；
[0087]
上述目标约束函数只与某几个连续的位姿状态有关，而不是整条轨迹上的位姿，这种局部性导致系统稀疏矩阵的产生，因此可以使用解稀疏优化方法—g2o图优化框架来求解非线性优化问题。
[0088]
进一步的，所述步骤3中，经实时反馈的轨迹序列与步骤2得到的最优轨迹序列相比较，计算得出误差信号；设计轨迹跟踪模糊控制器，具体包括：将误差信号经过模糊化转换为模糊语言变量，然后经过模糊推理并通过解模糊转换为控制量；
[0089]
选取轨迹序列的误差e
d
和误差变化率作为控制器的输入，转化后的语言变量分别为e
d
和ec
d
，输出变量为v
x
及ω，其语言变量分别为u
x
和u
ω
，将轨迹序列误差e
d
的基本论域设定在[
‑
0.8m，0.8m]，的实际范围设定为[
‑
0.24m，0.24m]；
[0090]
模糊化处理中，通过量化因子k＝m/|e
max
|将基本论域转换到论域上，定义模糊变量e
d
论域范围为[
‑
6，6]，定义输入ec
d
论域范围为[
‑
3，3]，则其量化因子计算分别如下：
[0091][0092]
对于上述变量e
d
、ec
d
、u
x
以及u
ω
，选择负大(nb)、负中(nm)、负小(ns)、零(zo)、正小(ps)、正中(pm)、正大(pb)这7个变量作为其语言变量，agv以平稳运行为原则，降低误差，因而选择采用高斯型隶属度函数；
[0093]
其中，nb、nm、ns表示参数量的值为规定的负方向，ps、pm、pb表示参数量的值为规定的正方向，zo表示参数量、误差量或变化率为0；
[0094]
根据输入的轨迹序列误差e
d
和误差变化率ec
d
的大小情况，图3所示为轨迹跟踪模
糊控制器的控制规则，每条控制规则表示为：if
…
and
…
then
…
的形式，控制器输出控制量通过解模糊环节转换为精确值，选取重心法进行解模糊：
[0095][0096]
通过比例因子k
u
将公式(12)中u转换到对应输出参数的基本论域中：
[0097]
u＝k
u
·
u
ꢀꢀꢀ
(13)
[0098]
对于模糊控制器而言，输出变量u
x
和ω的论域分别为[
‑
6，6]、[
‑
3，3]，可以得到其比例因子分别为：
[0099][0100]
其中，u
n
(x
i
)是第x
i
个语言变量隶属度值；x
i
为输出的模糊量；u为解模糊后的精确值；u是属于被控对象基本论域中的模糊控制器输出量的精确值；k
u
为比例因子。
[0101]
进一步的，所述步骤4中，由步骤3控制器输出变量v
x
和ω经过运动学方程输出v和ω并传递给左右轮，控制agv的速度和转向；假设以向左转弯为例，左轮与右轮的速度可表示为：
[0102][0103]
控制端输入量与两轮速度之间的关系表示为：
[0104][0105]
通过上述步骤，组成teb算法规划最优轨迹点、轨迹跟踪模糊控制器、agv运动学方程和被控对象的闭环控制系统。
[0106]
以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。