基于改进模糊PID的全向搬运机器人路径跟踪控制研究
2021-07-06李长勇
张 品 李长勇
(新疆大学机械工程学院，新疆 乌鲁木齐 830049)
在食品工厂生产流程中，食品原料的转运需要耗费很多时间，对食品生产的效率和工厂效益产生一定影响。采用自动化的搬运机器人[1-4]能有效降低食品原料转运耗费的时间，并且节省大量人力物力。全向机器人[5]是一种动作灵活的移动机器人，全向机器人可以执行物品拿取，仓库整理，地面清扫，来客识别等仓库管理任务。路径跟踪控制是全向机器人应用的核心技术之一，路径跟踪研究主要是控制机器人运动的实际路径与期望路径的误差，对机器人高效、稳定工作具有重要意义。
在移动机器人控制研究中，学者们经常使用PID方法[6-8]来控制机器人，普通PID方法控制效果不确定，参数无法自适应调整，整定困难。模糊逻辑控制[9-11]也经常被应用至机器人控制中，其具有稳定性高，抗扰动能力强等特点，但是控制效果依赖于专家经验和控制规则优劣，缺少积分作用，在工作点附近稳态误差较大。尹奇辉等[12]在设计控制器时加入模糊神经网络，并用LQR对位姿偏差耦合关系解耦，加快了纠偏速度。陈军章等[13]通过重新定义公式改善了人工鱼群算法，并应用改进算法降低了跟踪误差。周仁芳等[14]通过分析实时位置和理想路径，设计了去除时间因素的跟踪控制器，取得比纯追踪更好的效果。朱欣华等[15]在理想路径周围设计一种带有位置信息的场，能迅速纠正机器人的偏离。白国星等[16]通过减少控制步数优化非线性模型预测控制，提高了农业机器人控制精度。由上可知，PID控制、模糊控制、群智算法等在路径跟踪控制中改进或组合应用具有良好效果。
试验拟基于一种三轮全向搬运机器人，利用装置的角度、速度等传感器作为机器人感知系统，提出一种改进模糊PID路径跟踪控制方法，引入论域放缩因子优化模糊控制器，同时加入反向学习策略和权重因子改进麻雀算法，以模糊PID的参数作为麻雀位置进行寻优。通过MATLAB软件对改进麻雀算法进行测试，对改进模糊PID控制器进行模拟试验，分析其跟踪不同路径的效果。
1 全向机器人运动学建模
1.1 模型说明
为了说明机器人的运动状态和便于计算，作出以下假设：全向机器人行走轮与地面不产生相互滑动，机器人自身不产生俯仰倾覆等状况。
1.2 全向机器人运动学模型
设全向机器人状态变量和控制变量为
(1)
(2)
式中：
X——状态方程；
U——控制方程；
x、y——机器人的位置坐标。
将机器人在行走过程中的状态表示为图1的示意图，跟踪过程中的参数由表1说明。对驱动轮的角速度和速度进行分析，得到以下坐标速度和驱动轮速度关系：
表1 全向机器人跟踪参数
图1 全向机器人跟踪示意图
(3)
设环境坐标系为XOY，机器人自身坐标系为xoy，在环境坐标系中的位姿表达为(XYθ)。
由两坐标系关系可求得环境坐标系与自身坐标系间的表达式为：
(4)
式中：
Vwx、Vwy——机器人在环境坐标中的线速度，m/s；
Ww——机器人在环境坐标中的角速度，rad/s。
进一步推导驱动轮转速与机器人在环境中的速度变换关系：
(5)
控制机器人的过程中，为了不时刻进行坐标系的转换运算，假设自身坐标系与环境坐标系重合，则θ=0，式(5)可化简为：
(6)
将矩阵展开化简得：
(7)
将ψ=120°代入式(7)，故全向机器人的运动学模型为：
(8)
2 模糊PID控制器
2.1 PID控制器设计
PID控制器只以系统的输出误差为输入，不依赖数学模型，设计方便，鲁棒性高。通过组合底盘电机的不同动作，机器人就可以按照设计路线行走。电动机的转速与其输入电压呈比例，设期望路径与实际路径的垂直距离为距离偏差h(t)，以距离偏差h(t)为PID控制器的输入，以电机的输入电压u(t)为控制输出。PID控制公式如下：
(9)
为了便于工程实现，确定2 ms为取样时间，将样点输入控制器处理，表示为：
Δh(t)=h(t)-h(t-1)。
(10)
2.2 模糊控制器设计
设计一个模糊控制器。以机器人的位姿偏差为输入，控制器的参数为输出。在机器人行走的过程中，不断将位姿偏差传入，将控制参数传出，调整机器人的路径。位姿误差分为距离偏差h，方向偏差θ。输出参数为kp、ki、kd。
2.2.1 输入输出变量模糊化 依据控制参数选取规则，通过试验初步测定，确定了变量的论域并划分了模糊子集。
输入的模糊子集和论域为：
(11)
输出的模糊子集和论域为：
(12)
2.2.2 模糊规则设计 根据PID控制规律，当距离偏差或者方向偏差比较大时，应当尽快缩小偏差，缩短响应时间，此时kp值应该较大。当响应时间较小，但系统稳定时偏差仍然较大，应当适当增大ki的值，使响应值达到设定值。当响应时间较小，系统稳定时响应值在设定值附近大幅震荡时，应该增大kd，以减小稳态误差。
隶属函数有多种类型，高斯函数过渡性好，控制器工作波动小，如图2所示。
图2 距离偏差隶属度函数
根据试验测试和PID参数规律，制定表2所示参数kp、ki、kd的模糊规则表。
表2 kp、ki、kd模糊规则表
工作过程中，将论域放缩因子α、β引入控制器，在机器人工作状况发生变化时，对论域自我调节。放缩因子设计主要有两种方式，一种是模糊推理型，另一种是函数型，函数型放缩因子参数选定困难，且随着工况变化控制效果难以保证。研究选用模糊推理型放缩因子，基于专家经验和前期试验，模糊PID控制器两个输入参数e和ec的放缩因子α模糊子集为{Z,S,M,B,MB}，输出参数kp、ki、kd的β模糊子集为{Z,S,M,B,MB}。
故输入输出的模糊论域X与Y记为：
(13)
α、β选取如表3。
表3 α、β模糊规则表
3 麻雀算法
3.1 基本麻雀算法
麻雀算法由Xue等[17]提出。该算法仿照麻雀种群的行为，将发现者位置更新公式定义为：
(14)
式中：
t——当前代数；
Tmax——最大迭代数；
φ——一个属于(0,1]的随机数；
E——警惕值，E∈[0,1]；
TS——安全阈值，TS∈[0.5,1]；
Q——服从正态分布的随机数；
L——一行多列矩阵且元素是1。
当E
一部分麻雀随着发现者行动作为跟随者，在发现者找到食物时靠近争夺食物，其中一小部分跟随者因为食物太少，会飞往别处寻找食物，以补充能量。其位置更新公式如下：
(15)
式中：
Np——种群数；
Xp——目前所找到的最好食源；
Xworst——当前全局最差食源；
A——元素随机赋值为1或-1的1×d矩阵；
A+——AT(AAT)-1。
还有一部分麻雀作为警戒者，时刻注意着周围是否有捕食者出现，感知危险时会放弃食物，带领种群飞往其他位置。其位置更新公式：
(16)
式中：
β——步长调节因子；
K——分布在[-1,1]的随机数；
fi——当前个体适应度值；
fg、fw——全局最高、最低适应度值；
ε——极小常数。
3.2 改进麻雀算法
开始创造麻雀群时，麻雀个体随机分布在寻优空间，分布不均匀会延长找到最优质食源的时间。若麻雀群均布在空间，靠近最优质食源的麻雀飞向食源的距离和时间更短。利用对立解和中间解综合考虑创造原始麻雀群。
首先随机生成n个麻雀个体，再通过个体的对立点生成新的麻雀个体，反向解生成公式：
(17)
式中：
ub、lb——寻优空间上界和下界。
种群的初始解和反向解也不能完全均布在搜索空间内，故再加入初始解和反向解的中间解来丰富种群的多样性。中间解的表达式为：
(18)
种群总数量n*的表达式为：
(19)
依据个体的适应度，将总n*个体重新排列。取前n个麻雀作为开始寻优前的麻雀群，并舍弃排列在后面的麻雀。
由于跟随者在察觉发现者找到食物后全部向发现者靠近，种群趋向单一区域，影响搜寻更优食源，容易围绕次优食源盘旋。文中提出一种余弦权重因子ω，在寻优初期ω较小，一小部分跟随者靠近找到食物的发现者，其他跟随者保持原状，避免陷入局部最优。寻优后期ω较大，大量跟随者靠近发现者争夺食物，加快收敛速度。
(20)
式中：
i——当前代数；
imax——最大迭代数；
m——调节系数。
图3是ω在迭代中的变化趋势。
图3 ω递增图
则改进的跟随者位置公式：
(21)
对改进的麻雀算法进行性能测试仿真试验，参数为麻雀数量30，最大迭代数500次。分别选用单峰测试函数、多峰测试函数、固定维数测试函数进行测试。选用的函数如式(22)所示。
(22)
由于部分测试函数收敛快，对结果图横坐标进行了放大，仿真结果如图4所示。
由图4可知：在单峰函数测试下，改进麻雀算法收敛速度快于麻雀算法；在多峰函数测试下，改进的麻雀算法收敛速度比麻雀算法更快，且遇到次优作出反应迅速跳出，比原算法找到的食源更优；在固定维数函数测试下，改进麻雀算法先于原算法找到最优食源，并且寻优过程波动比麻雀算法小。
图4 3种测试函数仿真结果
4 仿真试验及结果分析
4.1 试验过程
改进模糊PID控制器工作过程：
(1)创造原始麻雀群，并且通过策略提升质量。设置麻雀数量、最大迭代数、安全阈值、搜索空间等约束条件。
(2)依据式(14)、式(15)、式(21)，每个麻雀个体完成飞行动作，对自己的信息进行更新。
(3)适应度计算。设置ITAE准则为适应度函数，并将麻雀的3个维度值赋值给模糊PID控制器的3个参数。在simulink中进行仿真，将被控系统产生的稳态误差代入ITAE函数计算适应度值。
(4)对比传入麻雀算法和设定的适应度值，前者小于后者或迭代结束进行下一步，否则返回第二步。
(5)将找到的最好食源信息作为控制参数传入仿真模型，得到结果。
仿真环境为MATLAB2018a/simulink，初始化麻雀群数量30，迭代数500代，仿真步长0.005 s，用改进模糊PID控制器搭建仿真模型。图5是控制结构图。
图5 控制系统结构图
4.2 结果及分析
为了对改进的模糊PID控制器控制全向机器人路径跟踪的效果进行验证，先对控制器进行阶跃仿真，然后选用直线、曲线路径进行跟踪仿真。
由图6可知，两种控制器最终都能让系统收敛，并且上升时间差别不大。但是改进的模糊PID到达设定值的时间小得多，并且超调量很小，达到输出稳态值后几乎不再波动。
图6 阶跃信号响应图
设机器人原位姿为(0,0，π/3)，理想路径y=x+1。图7、图8为直线跟踪图和直线误差图。
图7 直线跟踪
图8 直线误差
由以上试验结果可以得出，机器人跟踪直线路径时较为快速，并且到达期望路径时间较短，到达期望路径后波动很小，e(θ)最大值0.38，跟踪误差较小。
设机器人初始状态分别为(0,2，π/3)，理想路径y=1+2sin(0.9x)。图9、图10为曲线跟踪图和曲线误差图。
图9 曲线跟踪
图10 曲线误差
由试验结果可知，跟踪曲线路径时，y和θ误差有一定波动，但是很快也接近于0，机器人快速向目标路径靠近，总体跟踪效果较好，满足机器人工作需求。
5 结论
针对在复杂食品原料仓库中搬运机器人跟踪控制问题，提出了一种应用于全向搬运机器人的路径跟踪控制方法，设计了模糊推理型论域放缩因子自动调节论域，通过综合对立解和中间解的麻雀群生成方法提高了原始群质量，加入权重因子控制跟随者靠近食源的比例。仿真结果显示改进麻雀算法相比原算法在几种测试函数下寻优精度和收敛速度有所提升，改进控制器在直线和曲线跟踪时误差较小，到达期望路径较快，满足全向搬运机器人工作要求。
