一种基于卡尔曼滤波的模糊pid控制方法
【专利摘要】本发明公开了一种基于卡尔曼滤波的模糊PID控制方法，首先对AGV视觉定位系统获取到的AGV位姿信息进行卡尔曼滤波处理；然后使用模糊PID控制对卡尔曼滤波处理后的AGV位姿信息进行控制。不同于其他控制方案，本方法结合卡尔曼滤波与模糊PID控制器，既能提高AGV控制系统的性能与稳定性，又可以降低识别误差对系统的影响。
【专利说明】
-种基于卡尔曼滤波的模糊PID控制方法
技术领域
[0001] 本发明设及一种基于卡尔曼滤波的模糊PID控制方法，属于AGV导航系统控制技 术。
【背景技术】
[0002] 基于视觉的AGV(Automated Guided Vehicle,自动导引车）导航系统中视觉定位 系统存在一定识别误差，带有噪声的输入会对AGV控制器产生一定干扰，卡尔曼滤波器可W 根据AGV运动模型对其位姿状态进行预测，根据预测结果与识别结果做综合处理，从而得到 滤波后的AGV位姿状态信息，较大地减少了视觉定位系统的识别噪声对控制系统产生的干 扰。
[0003] 目前在AGV控制系统中应用最广泛的是传统PID控制器，但由于传统PID主要通过 试验得到一组固定参数对系统进行控制，运些参数很难处理好系统动态性能与静态性能之 间的平衡关系，模糊PID控制器的引入可W在一定程度上解决运一问题。模糊PID控制器不 需要对系统建立精确的数学模型，主要通过对获得的有限信息进行"不确定推理"，将相关 经验知识通过一定方法转化为控制策略，实现模糊控制，可W有效地实现对复杂系统的控 制。
【发明内容】
[0004] 发明目的:在AGV导航系统中，传统PID很难兼顾系统的动态性能与静态性能、设定 值与抑制扰动之间的矛盾，为了克服运一问题，本发明运用模糊PID控制技术对其进行改 进，提升系统控制性能；由于AGV视觉定位系统获取到的AGV位姿信息存在一定的误差干扰， 为了消除误差对系统运行稳定性的影响，本发明采用卡尔曼滤波对识别得到的位姿信息进 行滤波处理，将滤波后的位姿信息作为控制器的输入。该方法可W有效提升控制系统的性 能，而且降低了识别误差对系统的影响。
[0005] 技术方案:为实现上述目的，本发明采用的技术方案为：
[0006] -种基于卡尔曼滤波的模糊PID控制方法，包括如下步骤：
[0007] (1)对AGV视觉定位系统获取到的AGV位姿信息进行卡尔曼滤波处理；
[000引（2)使用模糊PID控制对卡尔曼滤波处理后的AGV位姿信息进行控制。
[0009]具体的，所述步骤（1)中，卡尔曼滤波的基本过程为:设k-1时刻AGV位姿的最优估 计为兩J.，通过成J预巧化时刻AGV位姿的预测量命|:&_1，通过AGV视觉定位系统获取k时刻AGV位 姿的观测量成，W秀&对％M进行修正，得到k时刻AGV位姿的最优估计为焉。
[0010]具体的，模糊PID控制是模糊控制和PID控制的复合控制：
[0011]模糊控制的输入变量为误差的模糊量皂和误差变化率的模糊量C，输出变量为模 糊PID参数，模糊PID参数包括模糊比例参数kp、模糊积分参数ki和模糊微分参数kD;对输入 量的误差e和误差变化率C进行离散化，根据离散值在隶属度函数中的位置即可得到对应的 晏、C和隶属度;根据kp的模糊规则表可W得到对模糊量的比例控制作用段.，结合隶属度对 !品运用重屯、法进行反模糊处理，即可得到比例控制作巧裝。的精确量UP;根据ki的模糊规则 表可W得到对模糊量的积分控制作用&，结合隶属度对 <攻运用重屯、法进行反模糊处理，即 可得到积分控制作用&的精确量UI;根据kD的模糊规则表可W得到对模糊量的微分控制作 用掠，结合隶属度对运用重屯、法进行反模糊处理，即可得到微分控制作用【知的精确量 UD;
[0012] PID控制的输入变量为误差e和误差变化率C，输出变量为PID参数，PID参数包括比 例参数kp'、积分参数ki'和微分参数kD' ；根据kp'得到比例控制作用的精确量UP'，根据kr得 到积分控制作用的精确量UI'，根据kD'得到微分控制作用的精确量UD' ；
[0013] 计算复合控制的输出关
[0014] 一般来说，所述模糊PID控制中，使用的隶属度函数为=角函数或正态型函数。
[0015] -般来说，所述模糊PID控制中，模糊规则表按照如下方式制定：
[001W ①根据kp的模糊规则表:若e和C符号相同，贝化P的符号与e和C相反，且e和C绝对值 的差值越大，kp的绝对值越大;若e和C符号相反，贝化P的符号与绝对值较小的相同，且e和C绝 对值的差值越小，kp的绝对值越小；
[0017]②根据ki的模糊规则表:当e与C的和为负时，则ki的符号为负，且e与C的和越大时， ki的绝对值越小；当e与C的和为正时，则ki的符号为正，且e与C的和越大时，ki的绝对值越 大；
[001引③根据kD的模糊规则表:kD的模糊规则主要通过实际环境测试得到，无明显规律。
[0019] 有益效果:本发明提供的基于卡尔曼滤波的模糊PID控制方法，通过卡尔曼滤波器 对识别得到的AGV位姿信息进行滤波处理，同时将模糊控制技术与传统PID控制技术进行融 合，提升了系统的控制性能;本发明利用卡尔曼滤波与模糊PID控制器代替传统PID控制器， 可W较大提高AGV控制系统的性能与稳定性，提升AGV运行效率与安全性能，提高企业劳动 生产率。
【附图说明】
[0020] 图1为本发明的实施流程图；
[0021] 图2为卡尔曼预测示意图；
[0022] 图3为模糊控制系统框图；
[0023] 图4为隶属度函数图；
[0024] 图5为模糊控制系统仿真模型；
[0025] 图6为FIS编辑器中输入变量与输出变量；
[0026] 图7为无扰动时传统PID控制与模糊PID控制响应曲线对比图；
[0027] 图8为模糊PID的编程实现流程；
[002引图9为模糊PID控制与传统PID控制运行路线对比。
【具体实施方式】
[0029]下面结合附图对本发明作更进一步的说明。
[0030] 理论基础:卡尔曼滤波、PID控制、模糊控制。对AGV进行控制的第一步是通过视觉 定位系统获取AGV实时位姿信息，我们首先运用卡尔曼滤波器对位姿信息进行滤波处理，然 后通过模糊PID控制器对AGV进行控制，提升其控制性能。
[0031] 1.卡尔曼滤波器
[0032] 卡尔曼滤波的一个典型实例是从一组有限的、包含噪声的、对物体位置的观察序 列（可能有偏差)预测出物体的位置的坐标及速度。该方法可应用在很多工程应用（如雷达 跟踪、船舶位置预测）中。卡尔曼滤波器提供了一种高效可计算的方法来估计过程的状态， 并使估计的均方差最小，在不知道当前模型的确切性质时，不仅能够估计信号的过去与当 前状态，甚至可W估计将来的状态。
[0033] 卡尔曼滤波器应用AGV导航系统中主要设及如下变量:k时刻AGV位姿的观测量馬， k-1时刻AGV位姿的最优估计为瑪_1，k时刻AGV位姿的预测量％_1;各个变量的详细定义如下 所示：
[0034]
(1)
[003引式中：Xk是k时刻X坐标的观测值;Yk是k时刻Y坐标的观测值；Q k是k时刻角度的观 测值;Xk是k时刻X坐标的最优估计值;yk是k时刻Y坐标的最优估计值；CO k是k时刻角度的最 优估计值;是k-1时刻对k时亥化坐标的预测值;裝是k-1时刻对k时刻Y坐标的预测值； 卸it_i是k-1时刻对k时刻角度的预测值。
[0036] 馬通过视觉定位系统识别得到，但由于视觉定位系统存在一定的误差，且AGV导航 系统是控制周期T为200ms的离散系统，所W识别得到的位姿状态是包含噪声(此噪声被视 为高斯白噪声）的离散量，不能够完全准确地还原行进过程中AGV的位姿状态。现假设Wk-I 时刻AGV位姿的最优估计馬_:1为准，预测k时刻的AGV位姿馬|4_1，同时通过视觉定位系统对AGV 位姿进行观测，得到k时刻AGV的观测量成&，. W观测量7?对预测量為&_.1进行修正，从而得到k 时刻的AGV位姿的最优估计％，运就是卡尔曼滤波的基本思想。
[0037] 1.1对当前状态做预测
[003引已知AGV两个驱动轮的距离为d，k-l时刻左右两轮的转速分别为vl、vr，AGV导航系 统的控制周期T为200ms，由此可W得到左右两轮在该周期内的行程SL、SR，如公式(2)所示。 SL与SR存在两种大小关系，即SL> = SR或者SL< SR,运里W SL< SR情况来说明预测的过程，SL > =SR的计算过程与之类似。
[0039]
[0040] 如图2所示，点(XI，yi)与(X2，y2)分别为AGV在k-1时刻与k时刻时所在位置，用两个 驱动轮的连线表示AGV，即图中两条绿色线段(其中（xi，yi)处线段为k-1时刻AGV位置，（X2, 72)处线段为k时刻AGV位置），其长度为d;分别与两条绿色线段垂直带箭头的线段表示AGV 的方向（其中k-1时刻AGV方向为d) i，k时刻AGV方向为d) 2)。该阶段需要根据k-1时刻AGV状态 最优估计％-._1 .，即（Xi, yi, 4 I)，预测Jk时刻AGV状态尽|i_i，即（X2，y2，4 2)。
[0041 ]图2中，（Xi，yi)与（X2，y2)的连线表示AGV行进的距离D;两条弧线分别表示AGV左驱 动轮行进的路线SL与右驱动轮行进的路线SR。由于k-1时刻至k时刻之间，AGV的速度恒定，红 蓝两条弧线所在的圆为同屯、圆，并可由公式(3)求得圆屯、角(60,行进的距离DW及行进方向 与垂直方向的夹角4 3。
[0042]
'3)
[00创根据行进距离D及行进方向(63可W预巧化时亥IjAGV位姿状态（X2，y2, 4 2)，如公式 (4)所示，从而得到k时刻预测量成。
[0044]
[0045] 1.2对当前状态做更新
[0046] 现已知k-1时亥IjAGV位姿的最优估计为馬_,和k-1时刻对k时亥IjAGV位姿的预测量 %._1，该预测量的高斯噪声的偏差DPik计算方式如公式(5)所示。
[0047]
(5)
[004引式中:DElk-I为k-1时刻最优估计的偏差;化Ik-I为k时刻预测量的不确定度。
[0049]同时通过视觉定位系统对AGV位姿进行观测，得至化时刻AGV的观测量成A，该时刻观 巧慢的偏差记为Dolk。通过预测量的偏差化Ik和观测量偏差Dolk计算卡尔曼增益化alman Ga in)的值Ke,血公击（6)所元。
[(K)加 ]
[0051 ]若k时刻AGV位姿的观测量为Nk，则可W根据公式(7)计算出k时亥IjAGV位姿的最优 估计量4。
[0化2]
讯
[0化3] 2模糊PID控制器
[0054] PID控制的适应性较强，适应各种控制对象，关键问题在于如何实现对参数的整 定。典型的PID通常利用一组固定参数进行控制，而运些参数很难兼顾系统的动态性能与静 态性能、设定值与抑制扰动之间的矛盾。由于AGV识别自身位姿(坐标和角度)时存在一定误 差，反应在控制系统中为一种随机扰动，为改善控制系统对运一扰动的处理效果，本发明提 出将模糊控制与PID控制相结合的模糊PID控制方法。
[0055] 随着自动控制系统被控对象的复杂化，难W建立精确数学模型对系统进行控制。 模糊控制对控制对象获得的有限模糊信息进行"不确定推理"，将相关领域的专家知识和熟 练操作人员的经验，转换成模糊化后的语言规则，通过模糊推理与模糊决策，实现对复杂系 统的控制。运种将操作经验用条件语句(控制规则)加 W描述，再利用模糊理论、模糊语言变 量和模糊逻辑推理，将模糊的控制规则上升为数值运算，让计算机运用程序实现对复杂系 统的控制的思路正是模糊控制的基本思想。
[0056] 模糊控制系统由模糊数据和规则库、模糊器、模糊推理机、解模糊器组成。模糊控 制系统用作控制器时称为模糊控制器。模糊控制系统框图如图3所示，图中r为系统设定值， 是精确量;e和C分别是误差和误差变化率，也为精确量;f和C分别是e和C经过模糊量化处 理后得到的模糊量;是晏和G经模糊控制规则和近似推理处理后得到的对模糊量的控制 作用;U是对模糊量的控制作用g经模糊判决得到模糊控制器输出的精确量控制作用。
[0057] 模糊PID控制器是一种结合了模糊控制算法的非传统式PID控制器，主要为了解决 传统PID在非线性系统、高阶时滞线性系统和无法精确建模的复杂模糊系统中无法良好控 制的问题。
[005引2.1确定模糊语言变量
[0059] 模糊控制的输入变量为误差的模糊量晏和误差变化率的模糊量C，输出变量为模 糊PID参数，模糊PID参数包括模糊比例参数kp、模糊积分参数ki和模糊微分参数kD，输入量 和输出量均用自然语言的形式给出，而不是W数值形式给出，因此它们不是数值变量，而是 语言变量。在应用中常取语言变量的词集为如下7个模糊子集组成的集合：
[0060] {NB，NM，NS，ZO，PS，PM，PB} (8)
[0061 ] 式中NB表不模糊隶属度函数中负大部分，醒表不负中部分，NS表不负小部分，ZO表 示零，PS表示正大部分，PM表示正中部分，TO表示正大部分。
[0062] 对输入量的误差e和误差变化率C进行离散化，根据离散值在隶属度函数中的位置 即可得到对应的巧隶属度。误差e和误差变化率C及模糊PID参数的实际变化范围称为 运些变量的基本论域。假设e的实际范围[a, b]是一个连续变量，为了将其转换到7个模糊子 集中，需要先将e的实际范围离散化到公式(9)所示的离散集中，再进行模糊化处理。
[0063] G = {-n,-n+l ,n-l ,n} (9)
[0064]
[0065] 在公式(9)中取n = 6的划分方式适合本文AGV实时控制系统，运样即完成连续变量 e的离散化。
[0066] G = {-6,-5,-4,-3,-2,-1,0,1,2,3,4,5,6} (11)
[0067] 2.2确定隶属度函数
[0068] 每个模糊子集的赋值可W根据统计资料建立，也可分析定义，在分析定义中，常用 =角形函数或正态型函数作为隶属度函数。由于=角形函数的形状仅与其斜率相关，运算 简单，占用内存空间较小，适合运用在对实时性要求较高的控制系统中。本文选用=角形函 数为语言变量的隶属度函数，如图4所示。
[0069] 根据图4建立运7个模糊子集在量化论域(公式(11)所示)上的赋值表，如表1所示。
[0070] 表1离散变量的隶属度赋值表 LUU/^」往：巧属巧刃0的省略禾与
[0073] 实际应用中，隶属度函数的形状(=角形、梯形或正态型）对控制效果的影响并不 明显，而每个模糊子集对论域覆盖范围的大小更加影响系统性能。通常来讲，每个模糊子集 的宽度选择合适，则控制效果较好;若其宽度较小，导致部分区域没有相应规则覆盖，则收 敛性较差;相反，若宽度较大，导致部分区域出现重叠，则对重叠区域规则相加，相互之间影 响增强，响应速度降低。
[0074] 2.3模糊控制规则
[0075] 模糊控制规则是模糊控制的核屯、。根据逻辑推理，将实际应用过程中操作人员的 大量成功经验，进行加工提炼，转化成输入、输出变量的模糊状态描述，得到如下相应控制 规则。
[0076] 当系统偏差e和偏差变化率C绝对值较大且符号相同（系统起动或停止运行，出现 扰动)时，PID比例参数kp的绝对值也应较大，但符号应该相反，运种取值方法可W加快系统 响应速度;若e和C符号相反，则kp的符号与绝对值较小的相同，且e和C绝对值的差值越小，kp 的绝对值越小；若e和C绝对值相近但符号相反，即误差趋于变小;kp具体模糊规则如表2所 /J、- O
[0077] 表2 kp的模糊规则表 「007?!
[0079] 当系统偏差e和偏差变化率C绝对值较大且符号相同，PID积分项会累计运些误差， 因此参数ki绝对值较大且符号与e和C相同；当系统误差逐渐减小时，为防止系统出现较大 超调，即积分项的绝对值也应相应减小。当e与C的和为负时，贝化I的符号为负，且e与C的和 越大时，ki的绝对值越小；当e与C的和为正时，贝化I的符号为正，且e与C的和越大时，ki的绝 对值越大;具体模糊规则如表3所示。
[0080] 表3 ki的模糊规则表
[0081] 1?~当系统稳定运行时，此时偏差e较小
，为保持系统的稳定性，避免系统在设定路线 附近出现振荡现象，还需要添加微分控制作用，提高系统整定时间及稳定性。相应模糊控制 规则由实际测试环境测试得到，具体模糊规则如表4所示。
[0083] 表4 kD的模糊规则表 「00841
[00化]3仿真实验与测试 [00化]3.1建立仿真模型
[0087] 本发明选择MATLAB/Simulink仿真软件，建立智能AGV控制系统的仿真模型，如图5 所示，其中模糊推理系统为化zzy Logic Controller模块。
[0088] 在Simulink中所建立仿真模型的模糊推理系统主要包括FIS编辑器、隶属度函数 编辑器和模糊规则编辑器=个部分，它们之间是相互连接，动态相关的。在FIS编辑器中分 别调用隶属度函数编辑器和模糊规则编辑器，如图6所示，隶属度函数编辑器主要负责编辑 系统输入、输出变量e，c，kp，ki，kD的隶属度函数，模糊规则编辑器主要负责编辑输入变量与 输出变量之间的模糊规则。
[0089] 3.2仿真结果及分析
[0090] 为验证模糊PID控制器在不同场景下的控制效果，仿真系统选用强度为1的脉冲函 数，分别从上升沿与下降沿观察其响应曲线。
[0091] 当系统在脉冲信号输入下，模糊PID控制器与传统PID控制器的动态响应特性如图 7所示，图中分别给出了脉冲输入信号、模糊PID与常规PID的输出响应对比。从图中可W看 出，在阶跃信号上升沿，传统PID响应曲线的最大超调量达到了 10.6 %，调节时间为1.38s (输出量与稳态值偏差在± 2 % );模糊PID响应曲线的最大超调量仅为1.0%，且调节时间为 0.18s，实验对比结果表明，相对于传统PID控制器，模糊PID控制器能够更迅速平稳地跟踪 上升沿信号。
[0092] 为进一步验证模糊PID控制器对实际应用中可能出现干扰的响应，阶跃信号下降 沿的响应曲线也是重要参考因素。如图7所示，传统PID响应曲线的最大超调量为10.7%，调 节时间为1.36s;而模糊PID控制器未出现超调现象，且调节时间为0.78s，相比传统PID控制 器速度提升了42.6%。结果表明，针对下降沿信号的跟踪，模糊PID控制器的动态性能也得 到了一定的提高。
[0093] 3.3模糊PID控制器的实现
[0094] 本发明运用C++编程语言实现上述模糊PID控制器，编程实现如图8所示。现W比例 控制作用UP的计算过程说明编程思路:首先对系统输入量误差e与误差变化C进行离散化， 并根据离散值在隶属度函数中的位置得到模糊量晏，C与其对应的隶属度;根据kp的模糊规 则表2可W得到比例控制作用UP的模糊量&>，最后通过上面求得的隶属度运用重屯、法对 进行反模糊处理，得到比例控制作用精确量UP。
[00M]同样，在上述流程中将kp的模糊规则替换为ki和kD的模糊规则可W分别求得积分 控制作用UI和微分控制作用加。又已知传统PID控制作用分别为邮'，山'，邮'，贝撮终系统控制 作用U为：
[0096]
[0097] 将上述编程实现的模糊PID控制器应用在AGV控制中，与传统PID控制器的进行对 比，如图9所示。图9中AGV的位置通过其定位系统识别得到，由于定位系统采集的位置存在 一定噪声，所W图中显示的AGV路线存在抖动现象，但实际运行的AGV路线比较平稳光滑。从 图中可W看出，传统PID控制下的AGV路线在规划路线X方向4m至6m处存在一定的震荡，在其 行驶到X方向6m处才较平稳地沿规划路线行驶;而模糊PID控制作用下的AGV路线在X方向4m 处到达规划路线，并且几乎没有出现震荡，直接稳定地跟踪规划路线行驶。实验结果表明， 相比于传统PID控制器，经过卡尔曼滤波的模糊PID控制器能够更加快速和稳定地对规划路 线进行跟踪。
[0098] W上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人 员来说，在不脱离本发明原理的前提下，还可W做出若干改进和润饰，运些改进和润饰也应 视为本发明的保护范围。
【主权项】
1. 一种基于卡尔曼滤波的模糊PID控制方法，其特征在于:包括如下步骤： (1) 对AGV视觉定位系统获取到的AGV位姿信息进行卡尔曼滤波处理； (2) 使用模糊PID控制对卡尔曼滤波处理后的AGV位姿信息进行控制。2. 根据权利要求1所述的基于卡尔曼滤波的模糊PID控制方法，其特征在于:所述步骤 (1) 中，卡尔曼滤波的基本过程为:设k-Ι时刻AGV位姿的最优估计为/^，通过L预测k时刻 AGV位姿的预测量'，通过AGV视觉定位系统获取k时刻AGV位姿的观测量兄，以爲对屯^ 进行修正，得到k时刻AGV位姿的最优估计为<。3. 根据权利要求1所述的基于卡尔曼滤波的模糊PID控制方法，其特征在于:所述步骤 (2) 中，模糊PID控制是模糊控制和PID控制的复合控制，具体为： 模糊控制的输入变量为误差的模糊量f和误差变化率的模糊量€:，输出变量为模糊PID 参数，模糊PID参数包括模糊比例参数kp、模糊积分参数ki和模糊微分参数kD;对输入量的误 差e和误差变化率c进行离散化，根据离散值在隶属度函数中的位置即可得到对应的? 和隶属度;根据kP的模糊规则表可以得到对模糊量的比例控制作用ζ/ρ，结合隶属度对仏运 用重心法进行反模糊处理，即可得到比例控制作用仏的精确量up;根据匕的模糊规则表可 以得到对模糊量的积分控制作用ζ/ρ结合隶属度对&运用重心法进行反模糊处理，即可得 到积分控制作用私的精确量m;根据k D的模糊规则表可以得到对模糊量的微分控制作用 匕，结合隶属度对^运用重心法进行反模糊处理，即可得到微分控制作用匕的精确量UD; PID控制的输入变量为误差e和误差变化率c，输出变量为PID参数，PID参数包括比例参 数kP，、积分参数kr和微分参数kD，；根据kP，得到比例控制作用的精确量up，，根据kr得到积分 控制作用的精确量w，，根据k D，得到微分控制作用的精确量UD，； 计算复合控制的输出为：4. 根据权利要求3所述的基于卡尔曼滤波的模糊PID控制方法，其特征在于:所述模糊 PID控制中，使用的隶属度函数为三角函数或正态型函数。5. 根据权利要求3所述的基于卡尔曼滤波的模糊PID控制方法，其特征在于:所述模糊 PID控制中，模糊规则表按照如下方式制定： ① 根据kp的模糊规则表:若e和c符号相同，则kp的符号与e和c相反，且e和c绝对值的差 值越大，kp的绝对值越大;若e和c符号相反，则kp的符号与绝对值较小的相同，且e和c绝对值 的差值越小，k P的绝对值越小； ② 根据Ia的模糊规则表:当e与c的和为负时，则Ia的符号为负，且6与〇的和越大时，1?的 绝对值越小；当e与c的和为正时，则Ia的符号为正，且e与c的和越大时，1?的绝对值越大； ③ 根据kD的模糊规则表:kD的模糊规则主要通过实际环境测试得到。
【文档编号】G05D1/02GK106020202SQ201610561137
【公开日】2016年10月12日
【申请日】2016年7月15日
【发明人】李新德, 徐叶帆
【申请人】东南大学