基于模糊PID对万向车台路径引导控制

汤子龙 ，丁志强，丁永强，高世燕

（1.兰州理工大学 电气工程与信息工程学院，兰州 730050；2.甘肃工大舞台技术工程有限公司，兰州 730050；3.甘肃正隆工程招标代理有限责任公司，天水 741000）

万向车台是一种能够自动导引，高能电池组提供动力，以微控制器为控制核心，装有非接触导引装置的无人驾驶舞台机械设备，具有安全保护功能，能够沿预设的路径导引行走，在国内正处于研发测试阶段，尚未得到实际的应用。导引定位功能是万向车台准确行走的关键[1-3]，因此定位技术成为研究的热点。路径导引的一个重要任务就是根据自身的感知系统确定自身的位置，从而按照用户的需求做出正确的行为决策和路径选择。在此，采用激光导引技术作为万向车台的导引方式，该技术具有导引柔性和较高的信息维度特性，在工程中得到了广泛的应用。

实现激光自动导引万向车台控制的基础是路径跟踪偏差值的矫正[4-6]，也是研发万向车台的关键技术。万向车台导引系统的精度由定位精度决定，因PID控制具有可靠、简单、鲁棒性好等优点，在工业控制领域普遍应用，但模糊控制器、模糊PID等控制方法具有更强的适应性，应用时取得了更好的控制效果。因此，文中采取模糊PID控制器实现万向车台的路径导引控制，其稳定性、鲁棒性、易用性更好，是一种最优偏差控制导引路径的方法。

1 激光导引系统定位原理与位姿计算

1.1 激光定位算法原理

要实现准确定位，万向车台在行走的过程中激光扫描传感器同时扫描到反射板不少于3块，根据反射板的位置信息，按照激光定位算法在预设的全局坐标系中确定万向车台的坐标与方位，然后反馈给车台微控制器进行处理。常用的定位算法有三角定位算法和三边定位算法。

1.1.1 三角定位算法

三角定位算法通过对万向车台纵轴与反光板间的夹角测量来完成定位。如图1（a）所示，测得∠AOB和∠BOC，利用外接圆的知识作2个外接圆。由外接圆的性质得知，三点不在一条直线上只有1个外接圆，推出车台一定位于2个圆相交的其中一个交点处，另一个交点处是第2个反光板。该算法对反光板的布设位置及其相互之间的角度准确度要求较高，且两圆圆心距离满足一定的条件，对测量信号的角度分辨率要求极高，因此难以获取精确的外置。该算法有一定的局限性。

1.1.2 三边定位算法

三边定位算法通过测量万向车台与全局坐标中预制的反光板之间的距离来实现定位。如图1（b）所示，测量车体到3个反光板A，B，C的距离分别为R1，R2，R3；以 R1，R2，R3为半径，分别以 3 个反光板A，B，C的中心为圆心作圆，由圆的性质得出车体的位置在3个圆的交点处。三边定位方法与三角定位方法相比，实现更加容易、精确，且对硬件环境条件要求低，在GPS定位中成功的应用，故文中采用三边定位算法。

图1 常用定位算法Fig.1 Common location algorithm

1.2 位姿计算

为尽量减小系统误差，在舞台面周围的墙壁上安装多块反光板可以提高定位的精度。以舞台面为参考，建立全局坐标系XOY，行走过程中万向车台上的激光扫描仪转动1周，可以检测到m个反光板1，2，…，i，…，m（m≥3），得到在全局坐标系 XOY 中与反光板匹配的信息坐标分别为（x1，y1），（x2，y2），…，（xi，yi），…，（xm，ym），对应生成 m 个圆，可知万向车台的位置一定在m个圆的交点处，如图2所示。

图2 反光板的动态匹配Fig.2 Dynamic matching of reflective plate

构建的m个圆方程为

任意选取方程（1）中的2个圆方程，相减得到1个线性方程（xo，yo）；将方程（1）两两相减可得个线性方程，组成1个超定方程组，则出现了方程的冗余；用最小二乘法解此方程组，得到万向车台在全局坐标系中的位置坐标为（xo，yo）。

万向车台位姿确定包括位置和姿态角2部分的确定，即车体前进的方向与全局坐标系中X轴的正向夹角就是车台相对于全局坐标系的方向角αo。系统中，全局坐标系与反光板的局部坐标系存在的关系如图3所示。

图3 坐标系之间的关系Fig.3 Relationship between coordinate systems

由图可见，1块反光板在全局坐标系中的坐标位置为（xi，yi，αi），在局部坐标系中的坐标为（xi′，yi′，αi′），万向车台导引参考坐标（全局坐标）（xo，yo，αo），其相互之间存在的关系为

由式（2）可得，第1块反光板的方向角α1为

即

式中：α1，…，αi为根据第 1，…，i块反光板得到万向车台的方向角；θ1，…，θi为检测到第 1，…，i块反光板在万向车台局部坐标系中的的方位角；（x1，y1），…，（xi，yi）为第 1，…，i块反光板在全局坐标系中的坐标值；（xo，yo）为万向车台上安装的激光扫描仪在全局坐标系中的坐标值。

由式（4）可得到的万向车台相对全局坐标系的方位角αo为

由式（1）和式（5）可得行走过程中万向车台确定的位姿（xo，yo，αo）。

2 模糊PID控制系统的设计

2.1 控制系统结构

模糊PID参数自整定控制系统在运行的过程中对一些不确定的因素，如参数、干扰、延迟等进行检测分析，运用模糊推理的策略对PID的3个参数在线自整定，即具备常规PID控制系统的使用方便、鲁棒性较强、原理简单等优点，又具备适应性、灵活性、精确性等优点。

万向车台的控制器由常规PID控制器与模糊控制器2部分组成。模糊系统的输入量为万向车台的位置偏差e与方向偏差θ；输出量为PID的3个参数变化值。应用预先确定的模糊控制规则对PID的各个参数进行在线修改，结构如图4所示。

图4 控制系统Fig.4 Control system

2.2 常规PID控制器设计

常规PID控制器可用以下算法表示为

从系统的响应速度、稳态性、稳态误差、超调量等条件考虑，比例系数kp，积分系数ki，微分系数kd具有的作用分别为kp能够提高系统的调节精度，改变响应速度；ki能够控制系统的稳态误差；kd能够控制系统的动态特性。

PID参数整定是对PID的 3个参数 kp，ki，kd与车台的位置偏差e及方向偏差θ之间关系的模糊处理。在万向车台行走的过程中实时检测e与θ的变化， 对 3个参数 Δkp，Δki，Δkd按照模糊控制规则进行在线修改，当e与θ不同时，满足控制参数的不同要求，达到良好的控制性能。

2.3 模糊PID控制器设计

确定其变量的基本论域非常关键，关系到车体位置偏差e及方向偏差θ太大时，万向车台就失去了自动导引控制。在此，取

e 基本论域为［-15，15］；

θ 基本论域为［-15°，15°］；

kp基本论域为［-20，20］；

ki基本论域为［-5，5］；

kd基本论域为［-0.5，0.5］。

基本论域均为 7 个量化等级，均取｛-3，-2，-1，0，1，2，3｝，并设其子集为｛NB，NM，ZO，PS，PM，BP｝。假设，万向车台车体的纵轴线越过路径中心线，在右边时e为正，在左边时e为负，α以顺时针转动方向为负。

模糊控制器的设计方法有理论法与试错法2类。实际应用中结合这2种方法能够得到更好的控制器。隶属度函数采用灵敏度高的三角形函数，如图5所示。

图5 隶属度函数Fig.5 Membership function

模糊控制器的设计核心是根据实际操作经验与专业技术知识，建立标准的模糊规则表。考虑Δkp，Δki，Δkd这 3 个参数之间的相互影响，模糊推理规则运用Mamdani方法，为两输入三输出的模糊控制系统，运用if-then控制规则——if（e is NB）and（θ is NM）then（Kp is PB）（Ki is NB）（Kd is PS），得出49条模糊语句，其规则见表1～3。

按照 Δkp，Δki，Δkd模糊控制规则表的信息，运用Mamdani组合推理知识[7]，推出相应的模糊关系，假设 R1，R2，R3，…，Rk，因此总的控制规则所对应的模糊关系R分别为

表1 Δkp的模糊控制规则Tab.1 Δkpfuzzy control rules

表2 Δki的模糊控制规则Tab.2 Δkifuzzy control rules

表3 Δkd的模糊控制规则Tab.3 Δkdfuzzy control rules

利用模糊推理合成规则，推出对应的修正参数代入

式中：分别为 PID 控制器参数 kp，ki，kd的初始参数。按照常规算法得到，在线运行不断检测系统的输出值，实时计算其位置偏差与角度偏差，模糊化后得到e与θ；由模糊控制算法采用重心法去模糊化的修正参数与的和来确定 kp，ki，kd的调整量，实现控制器的参数调整。

3 引导路径控制算法

万向车台驱动机构采用直流永磁电机，通过系统辨识将控制对象的传递函数简化为二阶系统，其传递函数为

通过MatLab的Simulink模块在Windows 7系统环境下，根据定义的模糊推理控制规则，编写模糊PID控制算法，并建立模糊PID控制器模型。

输入信号选择阶跃信号，设定采样时间为1 ms，同时加入常规闭环控制器与常规PID控制器进行比较。其中的摸糊PID控制器仿真模型如图6所示，则导引路径跟踪控制器MatLab/Simulink的仿真模型如图7所示。

图6 控制器模型Fig.6 Controller model

图7 导引路径跟踪模型Fig.7 Guidance path tracking model

模糊PID控制器的控制增益可调，而常规PID控制器增益固定，其模糊PID控制系统输出响应仿真结果如图8所示。在t=5 s时设置一个随机干扰信号，其输出响应及误差响应的仿真结果如图9所示。

图8 模糊PID控制系统输出结果Fig.8 Output results of fuzzy PID control system

图9 设置干扰信号模糊PID控制系统的输出结果Fig.9 Set the output results of fuzzy PID control system for interference signals

由仿真输出结果得知，模糊PID控制器系统阶跃输出响应结果及误差输出响应结果与常规PID、常规闭环控制系统输出响应结果相比，具有控制更加稳定，抗干扰能力强，调节时间短，超调相对较小，控制性能较好。

4 路径导引性能测试

路径导引性能直接决定着万向车台行走的准确性，为了测试万向车台导引纠偏能否达到舞台应用的标准，进行模拟在剧场环境中路径的特点，预设了一段试验路径，以预设路径的中心点为定位误差记录零点，进行测试。将预设的路径与试验导引行走后的实际路径取其中5点进行比较，测量出偏差量，假设在预设路径内的偏差为负，则在预设路径外的偏差为正。通过多次测试求平均偏差，对万向车台路径导引误差分析，实现路径导引性能测试。测试所得数据见表4，万向车台预设路径与试验第1次导引行走路径如图10所示。

表4 测试数据Tab.4 Test data

图10 预设与导引行走路径Fig.10 Presupposition and guided walking path

5 结语

由万向车台预设的路径与实际导引行走路径比较可得，万向车台在导引行走的过程中，通常在拐弯出现抖动偏离预设路径，在直道上基本能按照预设的路径正常运行，当偏离预设路径时，系统会进行快速的纠偏。通过详细的试验数据计算，该万向车台系统导引定位性能满足舞台应用的要求标准。

[1] 王文深，张淼淼，卢怡．基于模糊PID算法的MPD仿真控制研究[J]．石油机械，2017，45（6）：36-39．

[2] 陈泽宇，赵广耀，翟丽，等．基于模糊PID算法的双侧电传动履带车辆转向控制策略研究[J]．中国机械工程学报，2013，24（3）：410-425．

[3] 郑伟勇，李艳玮．基于前馈-反馈的移动机器人轨迹跟踪控制[J]．计算机工程与控制，2017，38（2）：539-543．

[4] 任柯燕，顾幸生，黄敬平，等．基于模糊PID对AGV的纠偏控制[J]．控制工程，2006，13（S1）：54-57．

[5] 倪振．激光导引四轮差动全方位移动AGV关键技术研究研究[D]．重庆：重庆大学，2013．

[6] 丁永强，丁志强．一种万向车台驱动轮及运动学的研究[J]．机械研究与应用，2017，30（3）：57-60．

[7] Salehpour S，Pourasad Y，Taheri S H．Vehicle path tracking by integrated chassis control[J]．J Cent South Univ，2015，22（4）：1378-1388． ■