CN103777522B - 基于模糊pid的无人水面艇直线跟踪方法 - Google Patents

Abstract

本发明公开了一种无人水面艇的直线路径跟踪方法，步骤如下：利用GPS装置动态地获得无人水面艇的航向角和实时经纬度坐标值；采用向量叉积方法计算距离偏差和角度偏差；利用距离偏差和角度偏差与PID参数之间的模糊关系动态地调整PID的参数值；计算出左右电动推进器的控制电压U；利用控制电压施加于电机，推动所述水面艇前进。仿真和实验结果表明，本方法在直线斜率、USV起点及起始角不同的情况下都有很好的跟踪效果，具有较好的适应性，能有效避免大迴转现象的发生，而且直线跟踪性能优于PID控制器。

Description

基于模糊PID的无人水面艇直线跟踪方法

[技术领域]

本发明涉及到智能化无人平台，具体是涉及一种无人水面艇的直线路径跟踪方法。

[背景技术]

无人水面艇是一种可以在复杂海洋环境中执行各种任务的智能化无人平台，它可以代替人类在海洋中完成一些危险任务。20世纪90年代国外关于USV的研究项目大量出现，美国和以色列等国家在这方面处于领先地位，例如美国的“Spartan”，以色列的“Protector”，它们既能自主航行也可遥控操纵，装备有摄像机、导航雷达、GPS等设备。在民用方面，麻省理工(MIT)的AUV实验室在海洋基金项目资助下研发了一系列的水面机器人，用于各种不同的任务。在MIT研究项目的鼓舞下，英国、意大利、葡萄牙等国家已研制出多种水面机器人，应用于海洋环境监测、水文勘测、水上网络组建和水面溢油处理等领域。国内起步比较晚，相对国外差距较大，而且国内的无人水面艇研究大都在遥控的阶段，无人水面艇的智能化关键技术方面还很薄弱，因此在无人水面艇智能化方面的前瞻性技术探索和研究不仅很有必要，而且十分紧迫。无人水面艇的自主路径跟踪是实现无人水面艇智能化的关键技术之一，而直线跟踪是路径跟踪的基础。

在上海市科委的资助下，上海海事大学航运技术与控制工程交通行业重点实验室研制出了一种具有完全自主知识产权的无人水面艇。目前正在进行该无人水面艇自主航行的研究，采用PID控制方法实现该无人水面艇的直线路径跟踪控制，但PID控制器在无人水面艇初始航向角和跟踪路径倾斜角偏差较大时会出现大迴转。

[发明内容]

针对上述问题以及实验对象的特点，本发明采用模糊PID方法，根据输入进行模糊推理， 在线调整PID参数，实验结果表明，该方法能有效避免大迴转现象的发生，而且直线跟踪性能优于PID控制器。

本发明无人水面艇上装有GPS装置，可以动态地获得航向角θ以及当前经纬度坐标值(X，Y)。模糊PID控制器主要分为四部分，分别为距离计算器、角度偏差比较器、PID控制器以及模糊推理模块。

模糊推理模块利用无人水面艇与目标路径的距离偏差e以及航向角偏差在线调整PID参数值，其输出是PID各参数的值。PID控制器利用调整后的PID参数和当前误差，计算出左右电动推进器的控制电压。

基于模糊PID的无人水面艇直线路径跟踪方法具体包括以下步骤：

一.GPS装置动态地获得航向角θ以及实时经纬度坐标值(X，Y)；

二.利用步骤一的数值计算距离偏差e和角度偏差设A、B两点为指定路线的起始点和终止点，O为小艇的位置，A、B、O三点的坐标分别为A(x₁,y₁),B(x₂,y₂),O(x₃,y₃)，θ为无人水面艇航向角，α为直线路径倾斜角，令

e＝(x₂-x₁)(y₃-y₁)-(x₃-x₁)(y₂-y₁)，

三.利用距离偏差e和角度偏差与PID参数之间的模糊关系动态地调整PID的参数值：定义角度偏差的模糊子集为{NB、NS、ZO、PS、PB}，即负大，负小，零，正小，正大，距离偏差e的PID各参数的模糊子集为{NB、NM、NS、ZO、PS、PM、PB}，子集中的元素分别为负大，负中，负小，零，正小，正中，正大，以三角形函数建立隶属度函数，左侧电机调速时的PID参数的推理规则表如以下三个表：

表1 K_pl模糊推理规则表

表2 K_il模糊推理规则表

表3 K_dl模糊推理规则表

利用以上三个表的推理规则动态调整PID参数；

四.计算出左右电动推进器的控制电压U：

将其离散化可化为：

其中e(k)为k时刻无人水面艇与目标路径的距离误差,与分别表示在k时刻左侧和右侧的电压值，

增量式PID控制算法方程如下：

分别表示k时刻左侧和右侧施加于电机上的电压增量；

五.运用步骤四得到的电压增量分别施加于所述左右电动推进器，推动所述水面艇前进。

本发明的积极进步效果在于：本方法在直线斜率、USV起点及起始角不同的情况下都有很好的跟踪效果，具有较好的适应性，能有效避免大迴转现象的发生，而且直线跟踪性能优于PID控制器。

[附图说明]

图1为本发明的系统结构示意图

图2为本发明的距离及角度偏差计算示意图

图3为不同情况仿真实验结果示意图

图4为大起始航向偏差角情况对比仿真结果示意图

图5为定量分析仿真对比图

图6为PID参数动态变化图

图1中标记如下所示：1为距离计算器、2为模糊推理器、3为角度偏差比较器、4为PID控制器、5为无人艇，6为模糊PID控制器。

[具体实施方式]

为了使本发明实现的技术手段、创作特征、达成目的与功效易于明白了解，下面结合具体图示，进一步阐述本发明：

如图1所示，在无人水面艇上装有GPS装置，可以动态地获得航向角θ以及当前经纬度坐标值(X，Y)。模糊PID控制器主要分为四部分，分别为距离计算器、角度偏差比较器、PID控制器以及模糊推理模块。

图1中R为指定路径，e为无人水面艇与目标路径的距离误差。模糊推理模块利用距离偏差e以及航向角偏差在线调整PID参数值，其输出是PID各参数的值。PID控制器利用调整后的PID参数和当前误差，计算出左右电动推进器的控制电压。

如图2所示，A、B两点为指定路线的起始点和终止点，O为小艇的位置，输出为采用向量叉积方法计算距离误差，以及航向偏差角

设A、B、O三点的坐标分别为A(x₁,y₁),B(x₂,y₂),O(x₃,y₃),

令e＝(x₂-x₁)(y₃-y₁)-(x₃-x₁)(y₂-y₁)，如果A、B、O三点是逆时针分布，则e为正值；若为顺时针分布，则e为负值。可以根据计算结果的符号来判断无人水面艇在路径的左侧或右侧，同时又可以根据偏差绝对值的大小来判断无人水面艇与目标路径的距离误差。

以实际实验为例选取检测到的右边缘线上三点A(600，-228.9279)，O(700，-269.6279)，B(800，-308.3279)，经过计算可得距离偏差e为100，然后通过选取左侧边缘上三点，来计算左侧边缘线的值，若实验选取A(300，-277.4714)，O(400，-291.1214)B(500，-364.7714)，通过计算可知距离偏差e为-3000，这与分析的情况相吻合，可以将面积的范围设定一个阈值范围在此范围内是判断边缘线位置线，若两条边缘线的其中一条位置线及判断该通道为直道。由于没有进行实际的运用，具体阈值大小的设定还需进一步研究。

角度偏差即航向偏差角表示航向角与直线倾斜角的偏差，其计算公式如下：

式中θ为无人水面艇航向角，α为直线路径倾斜角。

本发明利用航向偏差角以及距离偏差e作为模糊控制器的输入语言变量，PID参数值为输出的语言变量。定义航向偏差角的模糊子集为{NB、NS、ZO、PS、PB}，即负大，负小，零，正小，正大。距离偏差e的PID各参数的模糊子集为{NB、NM、NS、ZO、PS、PM、PB}，子集中的元素分别为负大，负中，负小，零，正小，正中，正大。以三角形函数建立隶属度函数。左、右侧电机调速时的PID参数建立推理规则表，如表1、2、3所示。

表1 K_pl模糊推理规则表

表2 K_il模糊推理规则表

表3 K_dl模糊推理规则表

系统正常运行时，利用航向偏差角以及距离偏差e与PID参数之间的模糊关系动态地调整PID的参数值，以获得比较理想的动态及静态特性。

本发明中的PID控制器输入为无人水面艇与目标路径的距离误差e，利用模糊控制器调整PID参数，计算出左右电动推进器的控制电压U。PID控制器时域输出如式(3)所示：

其中e(t)为t时刻无人水面艇与目标路径的距离误差，K_pl,K_il,K_dl分别为左侧电机调速时所用的PID各参数值；K_pr,K_ir,K_dr分别为右侧电机调速时所用的PID各参数值，U(t)_l,U(t)_r分别表示在t时刻左侧和右侧电机的电压值。

将其离散化可化为：

其中e(k)为k时刻无人水面艇与目标路径的距离误差,与分别表示在k时刻左侧和右侧的电压值。

增量式PID控制算法方程如下：

分别表示k时刻左侧和右侧施加于电机上的电压增量。

有益效果

本发明进行了仿真实例实验。为了验证本发明所述方法的性能，分别取直线斜率为正、为负、无穷大和0四种情况，在每种情况下无人水面艇起点分别在直线两侧，并设起始位置的分别为0、仿真结果如图3所示,图中A、B分别为无人水面艇在直线两侧的起点。

以上仿真可看出，本方法在直线斜率、USV起点及起始角不同的情况下都有很好的跟踪效果，表明本算法具有较好的适应性。

在起始航向偏差角较大的情况下对本发明方法和PID方法进行仿真对比，图4是在无人水面艇起始点(2，8)，为时跟踪路径2，以及为时跟踪路径1的仿真对比图。

结果显示PID控制器在无人水面艇起始航向偏差角较大时出现大迴转现象，本文方法避免了这一现象发生。

在前面实验的基础上对PID以及本文算法进行定量分析，对比仿真如图5所示。

图5中无人水面艇初始位置为(2,28)，分别为时的跟踪路线，性能指标对比如表4所示。

表4 性能指标对比

由表中的定量分析可看出模糊PID方法与PID方法相比降低了超调量并且缩短了调节时间。

探究本文所述方法之所以优于PID方法的原因，模糊PID能根据实时的情况，利用模糊规则来动态整定PID参数。而PID算法各项参数固定，不能实时调整。以图5中航向偏差角度为 的路径跟踪仿真为例，其PID各参数的动态变化如图6所示。由于模糊PID方法可以在线调整PID参数，与PID方法相比它可以适应更多的环境，并获得更优化的效果。

以上显示和描述的是本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等同物界定。

Claims (1)

1.一种基于模糊PID的无人水面艇直线路径跟踪方法，所述无人水面艇安装有GPS装置和模糊PID控制器，所述模糊PID控制器包括距离计算器、角度偏差比较器、PID控制器以及模糊推理模块，其特征在于：所述基于模糊PID的无人水面艇直线路径跟踪方法包括以下步骤：

一.所述GPS装置动态地获得航向角θ以及实时经纬度坐标值(X，Y)；

二.利用步骤一的数值计算距离偏差e和角度偏差：设A、B两点为指定路线的起始点和终止点，O为小艇的位置，A、B、O三点的坐标分别为A(x₁,y₁),B(x₂,y₂),O(x₃,y₃)，θ为无人水面艇航向角，α为A到B的直线路径的倾斜角，令

e＝(x₂-x₁)(y₃-y₁)-(x₃-x₁)(y₂-y₁)，

三.利用所述距离偏差e和所述角度偏差与PID参数之间的模糊关系动态地调整PID的参数值：定义所述角度偏差的模糊子集为{NB、NS、ZO、PS、PB}，即负大，负小，零，正小，正大，所述距离偏差e的PID各参数的模糊子集为{NB、NM、NS、ZO、PS、PM、PB}，子集中的元素分别为负大，负中，负小，零，正小，正中，正大，以三角形函数建立隶属度函数，左、右侧电机调速时的PID参数的推理规则表如以下三个表：

表1 K_pl模糊推理规则表

表2 K_il模糊推理规则表

表3 K_dl模糊推理规则表

利用以上三个表的推理规则动态调整PID参数；

四.计算出左右电动推进器的控制电压U：

将其离散化可化为：

其中e(k)为所述距离偏差e在k时刻的值,与分别表示在k时刻左侧和右侧的电压值，增量式PID控制算法方程如下：

分别表示k时刻左侧和右侧施加于电机上的电压增量；

五.运用步骤四得到的电压增量分别施加于所述左右电动推进器，推动所述水面艇前进。