CN113581388B - 一种环保作业的无人艇控制方法及应用其的无人艇系统 - Google Patents

Abstract

本发明提出一种环保作业的无人艇控制方法及应用其的无人艇系统，其中方法包括以下步骤：根据预设作业点的位置，生成路径规划航线；控制无人艇追踪路径规划航线，同时控制放下清扫装置对途径的水面进行清理，通过水质监测传感器对水体的水质进行探测；无人艇控制采用分离式控制方法，包括外环制导和内环控制；外环制导：根据无人艇实际位置与规划航线的距离误差，计算出期望艏向角ψ_ref；内环控制：根据期望艏向角ψ_ref计算出期望的舵角，计算并输出无人艇两个推进器的油门，通过两个油门的差速控制无人艇的航向和航速；本发明能够保证无人艇的行驶路径，且有效对路径上的垃圾进行清理，并对路径上的水质进行探测，保证水体的清洁干净无污染。

Description

一种环保作业的无人艇控制方法及应用其的无人艇系统

技术领域

本发明涉及无人艇的技术领域，特别是一种环保作业的无人艇控制方法及应用其的无人艇系统。

背景技术

目前城市公园里面都建设有湖泊，湖泊的水质、植物、鱼类动物都需要日常的打理和保护，目前则通过工人在湖面上划船的形式，对湖面的垃圾进行清理，对过长的水草进行切割，若需要对水体进行检测，则需要划船到指定的区域获取水体样本进行检测，这样人工划船的方式不仅需要耗费较大的劳动成本，工人的劳动强度大，而且效率低下，不利于对湖泊打理的发展，市面上的用于水面的无人艇方案不成熟，功能单一，无法达到收集垃圾和收割水槽的功能，也缺乏对水体检测的装置。

发明内容

针对上述缺陷，本发明的目的在于提出一种环保作业的无人艇控制方法及应用其的无人艇系统，解决现有技术人工划船进行垃圾清理和水体检测的劳动成本高、强度高，效率低下的问题。

为达此目的，本发明采用以下技术方案：

一种环保作业的无人艇控制方法，包括以下控制步骤：

根据预设作业点的位置，生成路径规划航线；

控制无人艇追踪路径规划航线，同时控制放下清扫装置对途经的水面进行清理，通过水质监测传感器对水体的水质进行探测；

控制无人艇追踪路径规划航线的方法中，对于无人艇的前进和转向控制采用分离式控制方法，包括外环制导和内环控制；

外环制导：根据无人艇实际位置与规划航线的距离误差，计算出期望艏向角ψ_ref，并作为内环控制的输入；

内环控制：根据期望艏向角ψ_ref计算出期望的舵角，计算并输出无人艇两个推进器的油门T₁和T₂，通过两个油门的差速控制无人艇的航向和航速。

优选的，所述外环制导包括以下方法步骤：

设置参数θ将路径参数化，得到参数化后的路径为P_h＝[x(θ) y(θ)]^T；

定义无人艇在t时刻的位置为P(t)＝[x(t) y(t)]^T；无人艇在t时刻的位置进行参数化后为P(θ)＝[x(θ) y(θ)]^T；

无人艇到路径规划航线的距离误差为e(θ)＝P_h(θ)-P(θ)；

定义参数θ使无人艇到路径规划航线的距离误差最小；定义距离误差代价函数为

当J(θ)取最小值的时候，e(θ)最小，利用梯度下降法，定义θ的迭代公式为：

式中：η为步长，控制θ朝梯度方向下降的速率，结合上式确定无人艇到路径上的最近点：P_d(θ)＝[x_d(θ) y_d(θ)]；

从而获得无人艇路径跟踪的期望艏向角的跟踪制律为：

优选的，所述内环控制由参数

时变矩阵估计算法和航向航速控制算法组成，包括以下方法步骤：

将无人艇平面运动的动力学模型简化为：

公式1中：M为质量矩阵；C为科氏力矩阵；D为阻力矩阵；τ＝[T_x T_y T_z]为控制输入；τ_w为非线性外界扰动；η＝[u v r]^T为无人艇的速度向量；

定义无人艇姿态跟踪误差向量e：

e＝η_d-η——公式2；

公式2中：η_d为期望姿态向量；

对公式2求一阶导后，代入公式1中，得到姿态误差开环动态方程：

将公式3离散化，定义在时刻k，

和η(k)的值是确定的；且M，C(η)，D(η)，τ_w(t)，e(t)均为确定的矩阵，此时，令y(k)＝e(k)，代入公式3得：

定义辅助函数F(k)为：

定义采样时间为T，将

代入公式4得：

记：Δy(k+1)＝y(k+1)-y(k)为相邻两时刻的输出变化；

Δτ(k)＝τ(k)-τ(k-1)为相邻两时刻的输入变化；

定义时变参数矩阵

使公式6系统可以转化为如下的数据模型

其中ΔH(k)＝||[Δy(k) Δτ(k)]^T||≠0，且

对任意时刻k有界；

参照如下控制输入准则函数：

J(τ(k))＝||y^*(k+1)-y(k+1)||²+λ||τ(k)-τ(k-1)||²——公式8

公式8中：λ为一个大于0的权重因子，用于惩罚控制输入量变化过大；τ＝[T_x T_yT_z]为控制输入；y^*(k+1)为期望输出。

由Cauchy微分中值定理得：

将其代入公式8中，对其求关于τ(k)的偏导，并令其等于0，得到航向航速控制律如下：

公式9中：ρ∈(0，1]为步长因子。

优选的，路径规划航线的生成包括以下步骤：

读取用户输入的作业点，标记无人艇起点s，除起点s外所有作业点记入集合U，并将已规划的作业点记入集合S；

比较起点s到集合U中各作业点之间的距离L，取距离最近的作业点记作下一个作业点k，将作业点k加入集合S，从集合U中移除作业点k；

判断集合U是否为空集，若否则比较当前作业点到集合U中剩余作业点之间的距离，取最近的作业点覆盖记作下一作业点k，将作业点k加入集合S，从集合U中移除作业点k；若是则结束路径规划。

优选的，无人艇在行驶中还包括无人艇避障控制方法，包括以下步骤：

激光雷达探测到障碍物，生成从当前无人艇的位置作为起点q-start到终点q-goal的直线连接路径m-line；

无人艇沿着m-line移动，当到达障碍物附近时，记录此刻无人艇的位置Xi；控制无人艇沿着障碍物的轮廓进行轮廓追踪；

当无人艇重新进入路径m-line后，沿着路径m-line到达终点q-goal；

当无人艇回到初始位置Xi时，则判断目标q-goal不可达，通知工作人员进行手动操控。

本申请还提出一种环保作业的无人艇系统，应用有所述环保作业的无人艇控制方法，本无人艇系统包括有：

无人艇本体，具有用于承载物件，并可以在水面上行驶的艇身；

无人艇清扫装置，在无人艇行驶的路径上，用于切割水草并对水面上的垃圾进行清理收集；

水质监测传感器，对在无人艇行驶的路径上，用于对水体的水质进行探测；

定位系统，用于对无人艇的位置进行定位；

推进器，设置在无人艇本体后方的两端，用于驱动无人艇本体，控制无人艇本体前进的航向和航速；

路径生成系统，用于生成无人艇行进的路径航线；

路径追踪系统，包括外环制导模块和内环控制模块，外环制导模块用于计算出期望艏向角ψ_ref，内环控制模块用于根据路径航线通过两个推进器的油门对无人艇的航向和航速进行控制，驱动无人艇本体沿着航线行驶。

优选的，所述外环制导模块包括：

路径参数化模块，用于设置参数θ将路径参数化；

无人艇位置与时间参数化模块，用于对无人艇在t时刻的位置参数化；

距离误差函数模块，用于定义距离误差代价函数；

最近点确认模块，用于获取无人艇到路径上的最近点；

跟踪制律模块，用于获得无人艇路径跟踪的期望艏向角的跟踪制律。

优选的，所述内环控制模块包括：

动力模型模块，用于定义无人艇平面运动的动力学模型；

误差向量模块，定义无人艇姿态跟踪误差向量；

姿态误差模块，用于获得无人艇姿态误差开环动态方程；

离散模块，用于将姿态误差开环动态方程离散化；

辅助函数模块，用于定义辅助函数；

采样时间代入模块，用于定义采样时间，将采样时间代入离散模块运算结果中；

时变参数矩阵模块，用于定义时变参数矩阵，转化数据模型；

控制输入准则函数模块，用于定义控制输入准则函数；

Cauchy微分中值模块，用于运算Cauchy微分中值定理得到相邻两时刻的输出变化；

航向航速控制律模块，用于获得无人艇路径跟踪的航向航速控制律。

优选的，所述路径生成系统包括：

读取模块，用于读取用户输入的作业点；

存储模块，用于存储和标记作业点；

判断模块，用于判断作业点间的距离长短，和判断集合U是否为空集；

执行模块，用于根据判断模块的判断结果，通过无人艇当前的位置计算出下一个距离最近的作业点。

优选的，还包括避障系统，所述避障系统包括：

激光雷达探测模块，用于对无人艇行驶前进的方向进行激光雷达探测；

避障路径规划模块，用于生成起始点和终点的虚拟直线路径；

路径控制模块，用于记录无人艇的位置并判断无人艇的行驶路径；

轮廓跟踪模块，用于控制无人艇沿着雷达探测的障碍物轮廓路径进行跟踪行驶；

上述技术方案中的一个技术方案具有如下优点或有益效果：

通过控制无人艇追踪路径规划航线，利用外环制导计算出无人艇实际位置与规划航线的距离误差，计算出期望艏向角ψ_ref，内环控制根据期望艏向角ψ_ref通过两个推进器实现无人艇的航向和航速的协同控制，从而使得无人艇能够始终沿着路径规划航线进行行驶，即使在水面风浪的情况下无人艇偏离了航线，依然可以通过控制无人艇追踪路径规划航线重新行驶回到航线中，保证无人艇的行驶路径，且能够有效的对路径上的垃圾进行清理，并对路径上的水质进行探测，保证水体的清洁干净无污染。

附图说明

图1是本发明提出的面向环保的无人艇控制方法中一个实施例的总体方法流程示意图；

图2是本发明提出的面向环保的无人艇控制方法中一个实施例的外环制导中无人艇路径参数化的极坐标系示意图；

图3是本发明提出的面向环保的无人艇控制方法中一个实施例的内环控制的运算流程示意图；

图4是本发明提出的面向环保的无人艇控制方法中一个实施例的生成路径规划航线的流程示意图；

图5是本发明提出的面向环保的无人艇控制方法中一个实施例的无人艇避障控制方法的流程示意图；

图6是本发明提出的面向环保的无人艇中一个实施例的结构示意图；

图7是本发明提出的面向环保的无人艇中一个实施例的无人艇清扫装置的结构示意图。

具体实施方式

下面详细描述本发明的实施方式，实施方式的示例在附图中示出，其中，相同或类似的标号自始至终表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施方式是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。

在本发明的实施方式的描述中，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个所述特征。在本发明的实施方式的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。

在本发明的实施方式的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明的实施方式中的具体含义。

在本发明的实施方式中，除非另有明确的规定和限定，第一特征在第二特征之“上”或之“下”可以包括第一和第二特征直接接触，也可以包括第一和第二特征不是直接接触而是通过它们之间的另外的特征接触。而且，第一特征在第二特征“之上”、“上方”和“上面”包括第一特征在第二特征正上方和斜上方，或仅仅表示第一特征水平高度高于第二特征。第一特征在第二特征“之下”、“下方”和“下面”包括第一特征在第二特征正下方和斜下方，或仅仅表示第一特征水平高度小于第二特征。

下文的公开提供了许多不同的实施方式或例子用来实现本发明的实施方式的不同结构。为了简化本发明的实施方式的公开，下文中对特定例子的部件和设置进行描述。当然，它们仅仅为示例，并且目的不在于限制本发明。此外，本发明的实施方式可以在不同例子中重复参考数字和/或参考字母，这种重复是为了简化和清楚的目的，其本身不指示所讨论各种实施方式和/或设置之间的关系。此外，本发明的实施方式提供了的各种特定的工艺和材料的例子，但是本领域普通技术人员可以意识到其他工艺的应用和/或其他材料的使用。

一种环保作业的无人艇控制方法，包括以下控制步骤：

根据预设作业点的位置，生成路径规划航线；

控制无人艇追踪路径规划航线，同时控制放下清扫装置对途经的水面进行清理，通过水质监测传感器对水体的水质进行探测；

控制无人艇追踪路径规划航线的方法中，对于无人艇的前进和转向控制采用分离式控制方法，包括外环制导和内环控制；

外环制导：根据无人艇实际位置与规划航线的距离误差，计算出期望艏向角ψ_ref，并作为内环控制的输入；

内环控制：根据期望艏向角ψ_ref计算出期望的舵角，计算并输出无人艇两个推进器的油门T₁和T₂，通过两个油门的差速控制无人艇的航向和航速。

具体的，在本实施例中，通过控制无人艇追踪路径规划航线，利用外环制导计算出无人艇实际位置与规划航线的距离误差，计算出期望艏向角ψ_ref，内环控制根据期望艏向角ψ_ref通过两个推进器实现无人艇的航向和航速的协同控制，从而使得无人艇能够始终沿着路径规划航线进行行驶，即使在水面风浪的情况下无人艇偏离了航线，依然可以通过控制无人艇追踪路径规划航线重新行驶回到航线中，保证无人艇的行驶路径，且能够有效的对路径上的垃圾进行清理，并对路径上的水质进行探测，保证水体的清洁干净无污染，例如应用在公园湖泊中，能够利用无人艇对湖面进行有效的清洁，降低人工划船进行清理的劳动强度，且能够有效提高工作效率，同时可以对水质进行探测，保证水体的干净，避免水体污染对湖泊中植物和鱼类动物造成伤害。

进一步的是，所述外环制导包括以下方法步骤：

设置参数θ将路径参数化，得到参数化后的路径为P_h＝[x(θ) y(θ)]^T；

定义无人艇在t时刻的位置为P(t)＝[x(t) y(t)]^T；无人艇在t时刻的位置进行参数化后为P(θ)＝[x(θ) y(θ)]^T；

无人艇到路径规划航线的距离误差为e(θ)＝P_h(θ)-P(θ)；

定义参数θ使无人艇到路径规划航线的距离误差最小；定义距离误差代价函数为

当J(θ)取最小值的时候，e(θ)最小，利用梯度下降法，定义θ的迭代公式为：

式中：η为步长，控制θ朝梯度方向下降的速率，结合上式确定无人艇到路径上的最近点：P_d(θ)＝[x_d(θ) y_d(θ)]；

从而获得无人艇路径跟踪的期望艏向角的跟踪制律为：

具体的，在本实施例中，无人艇导航所用的NED坐标系转换为极坐标系，将路径进行极坐标的参数化，得到无人艇在坐标中的位置与路径规划航线的距离误差，计算出无人艇距离路径规划航线的最近点，从而快速计算得到无人艇路径跟踪的期望艏向角，运算简单且快速，提高无人艇的反应速度，使得无人艇能够以最短的路程和时间快速移动至规划航线上，有效提高工作效率。

进一步的是，所述内环控制由参数

时变矩阵估计算法和航向航速控制算法组成，包括以下方法步骤：

将无人艇平面运动的动力学模型简化为：

公式1中：M为质量矩阵；C为科氏力矩阵；D为阻力矩阵；τ＝[T_x T_y T_z]为控制输入；τ_w为非线性外界扰动；η＝[u v r]^T为无人艇的速度向量；

定义无人艇姿态跟踪误差向量e：

e＝η_d-η——公式2；

公式2中：η_d为期望姿态向量；

对公式2求一阶导后，代入公式1中，得到姿态误差开环动态方程：

将公式3离散化，定义在时刻k，

和η(k)的值是确定的；且M，C(η)，D(η)，τ_w(t)，e(t)均为确定的矩阵，此时，令y(k)＝e(k)，代入公式3得：

定义辅助函数F(k)为：

定义采样时间为T，将

代入公式4得：

对于公式6所示的系统，满足下面的两个条件：

1)除有限时刻点外，y(k+1)关于y(k)，τ(k)的偏导数连续；

2)除有限时刻点外，系统满足Lipschitz条件，即对任意k₁≠k₂，k₁、k₂＞0，τ₁≠τ₂：

|y(k₁+1)-y(k₂+1)|≤b|τ(k₁)-τ(k₂)|

记：Δy(k+1)＝y(k+1)-y(k)为相邻两时刻的输出变化；

Δτ(k)＝τ(k)-τ(k-1)为相邻两时刻的输入变化；

对满足条件1)和条件2)的非线性系统，当ΔH(k)＝||[Δy(k) Δτ(k)]^T||≠0，定义时变参数矩阵

使公式6系统可以转化为如下的数据模型

且

对任意时刻k有界；

参照控制输入准则函数得：

J(τ(k))＝||y^*(k+1)-y(k+1)||²+λ||τ(k)-τ(k-1)||²——公式7

公式7中：λ为一个大于0的权重因子，用于惩罚控制输入量变化过大；τ＝[T_x T_yT_z]为控制输入；y^*(k+1)为期望输出。

由Cauchy微分中值定理得：

将其代入公式7中，对其求关于τ(k)的偏导，并令其等于0，得到航向航速控制律如下：

公式8中：ρ∈(0，1]为步长因子。

具体的，在本实施例中，将无人艇的动力学模型简化，定义无人艇姿态跟踪误差向量后得到无人艇姿态误差开环动态方程，将姿态误差开环动态方程离散化后，将采样时间T对应的

代入离散化后的方程中运算，通过Cauchy微分中值定理得到Δy(k+1)代入控制输入准则函数中，并对其关于的偏导，并令其等于0，从而得到航向航速控制律τ(k)，可以通过以上运算获得两个推进器油门T₁和T₂，通过两个推进器控制无人艇的移速，同时可以通过对两个推进器的差速控制，对无人艇的航向进行控制，使得无人艇能够以最短的路程和时间快速移动至规划航线上，有效提高工作效率。

进一步的是，路径规划航线的生成包括以下步骤：

读取用户输入的作业点，标记无人艇起点s，除起点s外所有作业点记入集合U，并将已规划的作业点记入集合S；

比较起点s到集合U中各作业点之间的距离L，取距离最近的作业点记作下一个作业点k，将作业点k加入集合S，从集合U中移除作业点k；

判断集合U是否为空集，若否则比较当前作业点到集合U中剩余作业点之间的距离，取最近的作业点覆盖记作下一作业点k，将作业点k加入集合S，从集合U中移除作业点k；若是则结束路径规划。

具体实施例：假设集合U中包含作业点a、b、c，将起点s到作业点a之间的距离记为长度Lsa＝(s,a)，当起点s和作业点a为不相邻的两个作业点时，则将起点s到作业点a之间的距离记为长度Lsa＝∞，同理将各作业点的到起点s的距离分别记为Lsa、Lsb、Lsc，比较Lsa、Lsb、Lsc三者的大小，若Lsa为长度最短的，则记作业点a为下一个作业点k，将作业点a加入集合S，从集合U中移除作业点a，此时S包括s和a，U包括b和c；

判断U中还包含作业点b、c，则计算作业点a到作业点b之间的距离Lab和作业点a到作业点c之间的距离Lac，比较Lab和Lac的长度，假设Lab为长度较短的，则记作业点b为下一个作业点k，将作业点b加入集合S，从集合U中移除作业点b，此时S包括s、a和b，U包括c；

再判断U中只含作业点c，则作业点c就会作为最后的作业点，于是就能得到无人艇的路线为s→a→b→c；从而将路径规划发送至无人艇，控制无人艇根据规划路线逐个作业点依次进行作业，使得无人艇的路径轨迹尽可能短距离。

本路径规划航线中从一个作业点规划到另一个作业点时，通过上述方法选取距离最近的作业点作为下一个作业点，依次选取作业点，直至所有作业点被选取，从而形成一条路径相对较短的行驶航线，保证无人艇行驶至每一个作业点进行垃圾清理和水质监测，耗费较短的时间行驶所有作业点所需要的时间，提高无人艇对所有作业点清理的效率。

进一步的是，无人艇在行驶中还包括无人艇避障控制方法，包括以下步骤：

激光雷达探测到障碍物，生成从当前无人艇的位置作为起点q-start到终点q-goal的直线连接路径m-line；

无人艇沿着m-line移动，当到达障碍物附近时，记录此刻无人艇的位置Xi；控制无人艇沿着障碍物的轮廓进行轮廓追踪；

当无人艇重新进入路径m-line后，沿着路径m-line到达终点q-goal；

当无人艇回到初始位置Xi时，则判断目标q-goal不可达，通知工作人员进行手动操控。

具体的，在本实施例中，在无人艇沿着路径规划航线行驶的途中，航线上有可能会出现无法预见的障碍物，通过激光雷达可以探测出障碍物的轮廓，生成的路径m-line为无人艇原本行驶的距离最近的航线，若出现了障碍物，就控制无人艇沿着障碍物的轮廓进行轮廓跟踪行驶，直到无人艇重新进入路径m-line，然后行驶至终点，当无人艇回到最初遇到障碍物的位置Xi时，则会通知工作人员寻求方法，提高无人艇的灵活性，避免无人艇与障碍物碰撞造成无人艇的损坏，有效保护无人艇。

本申请还提出一种环保作业的无人艇系统，应用有上述的无人艇控制方法，包括：

无人艇本体，具有用于承载物件，并可以在水面上行驶的艇身；

无人艇清扫装置，在无人艇行驶的路径上，用于切割水草并对水面上的垃圾进行清理收集；

水质监测传感器，对在无人艇行驶的路径上，用于对水体的水质进行探测；

定位系统，用于对无人艇的位置进行定位；

推进器，设置在无人艇本体后方的两端，用于驱动无人艇本体，控制无人艇本体前进的航向和航速；

路径生成系统，用于生成无人艇行进的路径航线；

路径追踪系统，包括外环制导模块和内环控制模块，外环制导模块用于计算出期望艏向角ψ_ref，内环控制模块用于根据路径航线通过两个推进器的油门对无人艇的航向和航速进行控制，驱动无人艇本体沿着航线行驶。

具体的，通过路径生成系统生成无人艇行进的路径航线，然后通过推进器驱动无人艇本体在水面上行驶，同时降下无人艇清扫装置，在无人艇行驶的路径上进行切割水草和对水面上的垃圾进行清理收集，并利用水质监测传感器对水质进行探测，利用定位系统对无人艇的位置进行定位，通过路径追踪系统的外环制导模块运算，根据无人艇实际位置与规划航线的距离误差，计算出期望艏向角ψ_ref，并作为内环控制的输入，路径追踪系统的内环控制模块，根据期望艏向角计算出期望的舵角，计算并输出无人艇两个推进器的油门T1和T2，通过两个油门的差速控制无人艇的航向和航速；从而使得无人艇能够始终沿着路径规划航线进行行驶，即使在水面风浪的情况下无人艇偏离了航线，依然可以通过路径追踪系统重新行驶回到航线中，保证无人艇的行驶路径，能够有效的通过无人艇清扫装置对路径上的垃圾进行清理，并对路径上的水质进行探测，保证水体的清洁干净无污染，例如应用在公园湖泊中，能够利用无人艇对湖面进行有效的清洁，降低人工划船进行清理的劳动强度，且能够有效提高工作效率，同时可以对水质进行探测，保证水体的干净，避免水体污染对湖泊中植物和鱼类动物造成伤害。

进一步的是，所述外环制导模块包括：

路径参数化模块，用于设置参数θ将路径参数化；

无人艇位置与时间参数化模块，用于对无人艇在t时刻的位置参数化；

距离误差函数模块，用于定义距离误差代价函数；

最近点确认模块，用于获取无人艇到路径上的最近点；

跟踪制律模块，用于获得无人艇路径跟踪的期望艏向角的跟踪制律。

具体的，在本实施例中，路径参数化模块设置参数θ将路径参数化，得到参数化后的路径为P_h＝[x(θ) y(θ)]^T，无人艇位置与时间参数化模块定义无人艇在t时刻的位置为P(t)＝[x(t) y(t)]^T；将无人艇在t时刻的位置进行参数化后得到P(θ)＝[x(θ) y(θ)]^T，距离误差函数模块定义无人艇到路径规划航线的距离误差为e(θ)＝P_h(θ)-P(θ)；最近点确认模块定义参数θ使无人艇到路径规划航线的距离误差最小；定义距离误差代价函数为

当J(θ)取最小值的时候，e(θ)最小，利用梯度下降法，定义θ的迭代公式为：

式中：η为步长，控制θ朝梯度方向下降的速率，确定无人艇到路径上的最近点为P_d(θ)＝[x_d(θ) y_d(θ)]；最后通过跟踪制律模块获得无人艇路径跟踪的期望艏向角的跟踪制律为

快速计算得到无人艇路径跟踪的期望艏向角，运算简单且快速，提高无人艇的反应速度，使得无人艇能够以最短的路程和时间快速移动至规划航线上，有效提高工作效率。

进一步的是，所述内环控制模块包括：

动力模型模块，用于定义无人艇平面运动的动力学模型；

误差向量模块，定义无人艇姿态跟踪误差向量；

姿态误差模块，用于获得无人艇姿态误差开环动态方程；

离散模块，用于将姿态误差开环动态方程离散化；

辅助函数模块，用于定义辅助函数；

采样时间代入模块，用于定义采样时间，将采样时间代入离散模块运算结果中；

时变参数矩阵模块，用于定义时变参数矩阵，转化数据模型；

控制输入准则函数模块，用于定义控制输入准则函数；

Cauchy微分中值模块，用于运算Cauchy微分中值定理得到微分方程；

航向航速控制律模块，用于获得无人艇路径跟踪的航向航速控制律。

具体的，在本实施例中，动力模型模块将无人艇平面运动的动力学模型简化为

通过误差向量模块定义无人艇姿态跟踪误差向量e＝η_d-η——公式2；通过姿态误差模块对公式2求一阶导后，代入公式1中，得到姿态误差开环动态方程：

离散模块将将公式3离散化，定义在时刻k，

和η(k)的值是确定的；且M，C(η)，D(η)，τ_w(t)，e(t)均为确定的矩阵，此时，令y(k)＝e(k)，代入公式3得：

通过辅助函数模块定义辅助函数F(k)为：

通过采样时间代入模块定义采样时间为T，将

代入公式4得：

时变参数矩阵模块，定义时变参数矩阵

使公式6系统可以转化为如下的数据模型

控制输入准则函数模块定义控制输入准则函数为：J(τ(k))＝||y^*(k+1)-y(k+1)||²+λ||τ(k)-τ(k-1)||²——公式8；通过Cauchy微分中值模块运算得微分方程

最后通过航向航速控制律模块将微分方程代入公式8中，对其求关于τ(k)的偏导，并令其等于0，得到航向航速控制律：

通过以上运算得到期望的航向航速控制律，对于本文所使用的双推进无人艇，采用差速转向。输出为两个推进器油门T₁和T₂，对无人艇的航向进行姿态的控制，使得无人艇能够以最短的路程和时间快速移动至规划航线上，有效提高工作效率。

进一步的是，所述路径生成系统包括：

读取模块，用于读取用户输入的作业点；

存储模块，用于存储和标记作业点；

判断模块，用于判断作业点间的距离长短，和判断集合U是否为空集；

执行模块，用于根据判断模块的判断结果，通过无人艇当前的位置计算出下一个距离最近的作业点。

具体的，在本实施例中，通过读取模块读取用户输入作业点，通过存储模块标记无人艇起点s，除起点s外所有作业点记入集合U，并将已规划的作业点记入集合S；通过判断模块比较当前作业点到集合U中剩余各作业点之间的距离L，通过执行模块取距离最近的作业点记作下一个作业点k，将作业点k加入集合S，从集合U中移除作业点k；通过判断模块集合U是否为空集来结束路径规划；通过上述方法选取距离最近的作业点作为下一个作业点，依次选取作业点，直至所有作业点被选取，从而形成一条路径相对较短的行驶航线，保证无人艇行驶至每一个作业点进行垃圾清理和水质监测，耗费较短的时间行驶所有作业点所需要的时间，提高无人艇对所有作业点清理的效率。

进一步的是，还包括避障系统，所述避障系统包括：

激光雷达探测模块，用于对无人艇行驶前进的方向进行激光雷达探测；

避障路径规划模块，用于生成起始点和终点的虚拟直线路径；

路径控制模块，用于记录无人艇的位置并判断无人艇的行驶路径；

轮廓跟踪模块，用于控制无人艇沿着雷达探测的障碍物轮廓路径进行跟踪行驶；

具体的，在本实施例中，激光雷达探测模块探测前进路径上是否存在障碍物，通过避障路径规划模块生成从当前无人艇的位置作为起点q-start到终点q-goal的直线连接路径m-line，通过路径控制模块控制无人艇沿着m-line移动，当到达障碍物附近时，记录此刻无人艇的位置Xi；通过轮廓跟踪模块控制无人艇沿着障碍物的轮廓进行轮廓追踪；当无人艇重新进入路径m-line后，沿着路径m-line到达终点q-goal；当无人艇回到初始位置Xi时，则判断目标q-goal不可达，通知工作人员进行手动操控；提高无人艇的灵活性，避免无人艇与障碍物碰撞造成无人艇的损坏，有效保护无人艇。

在本说明书的描述中，参考术语“一个实施方式”、“一些实施方式”、“示意性实施方式”、“示例”、“具体示例”或“一些示例”等的描述意指结合所述实施方式或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施方式或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施方式或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施方式或示例中以合适的方式结合。

流程图中或在此以其他方式描述的任何过程或方法描述可以被理解为，表示包括一个或更多个用于实现特定逻辑功能或过程的步骤的可执行指令的代码的模块、片段或部分，并且本发明的优选实施方式的范围包括另外的实现，其中可以不按所示出或讨论的顺序，包括根据所涉及的功能按基本同时的方式或按相反的顺序，来执行功能，这应被本发明的实施例所属技术领域的技术人员所理解。

在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理模块的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。

就本说明书而言，"计算机可读介质"可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。计算机可读介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(控制方法)，便携式计算机盘盒(磁装置)，随机存取存储器(RAM)，只读存储器(ROM)，可擦除可编辑只读存储器(EPROM或闪速存储器)，光纤装置，以及便携式光盘只读存储器(CDROM)。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。

应当理解，本发明的实施方式的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。

本技术领域的普通技术人员可以理解实现上述实施例方法携带的全部或部分步骤是可以通过程序来指令相关的硬件完成，所述的程序可以存储于一种计算机可读存储介质中，该程序在执行时，包括方法实施例的步骤之一或其组合。

此外，在本发明的各个实施例中的各功能单元可以集成在一个处理模块中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个模块中。上述集成的模块既可以采用硬件的形式实现，也可以采用软件功能模块的形式实现。所述集成的模块如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。

上述提到的存储介质可以是只读存储器，磁盘或光盘等。

尽管上面已经示出和描述了本发明的实施方式，可以理解的是，上述实施方式是示例性的，不能理解为对本发明的限制，本领域的普通技术人员在本发明的范围内可以对上述实施实施进行变化、修改、替换和变型。

Claims (8)

1.一种环保作业的无人艇控制方法，其特征在于：包括以下步骤：

根据预设作业点的位置，生成路径规划航线；

控制无人艇追踪路径规划航线，同时控制放下清扫装置对途经的水面进行清理，通过水质监测传感器对水体的水质进行探测；

控制无人艇追踪路径规划航线的方法中，对于无人艇的前进和转向控制采用分离式控制方法，包括外环制导和内环控制；

外环制导：根据无人艇实际位置与规划航线的距离误差，计算出期望艏向角ψ_ref，并作为内环控制的输入；

内环控制：根据期望艏向角ψ_ref计算出期望的舵角，计算并输出无人艇两个推进器的油门T₁和T₂，通过两个油门的差速控制无人艇的航向和航速；

所述外环制导包括以下方法步骤：

设置参数θ将路径参数化，得到参数化后的路径为P_h＝[x(θ) y(θ)]^T；

定义无人艇在t时刻的位置为P(t)＝[x(t) y(t)]^T；无人艇在t时刻的位置进行参数化后为P(θ)＝[x(θ) y(θ)]^T；

无人艇到路径规划航线的距离误差为e(θ)＝P_h(θ)-P(θ)；

定义参数θ使无人艇到路径规划航线的距离误差最小；定义距离误差代价函数为

当J(θ)取最小值的时候,e(θ)最小，利用梯度下降法，定义θ的迭代公式为:

式中：η为步长，控制θ朝梯度方向下降的速率，结合上式确定无人艇到路径上的最近点：P_d(θ)＝[x_d(θ) y_d(θ)]；

从而获得无人艇路径跟踪的期望艏向角的跟踪制律为：

2.根据权利要求1所述的一种环保作业的无人艇控制方法，其特征在于：所述内环控制由参数

时变矩阵估计算法和航向航速控制算法组成，包括以下方法步骤：

将无人艇平面运动的动力学模型简化为：

公式1中：M为质量矩阵；C为科氏力矩阵；D为阻力矩阵；τ＝[T_x T_y T_z]为控制输入；τ_w为非线性外界扰动；η＝[u v r]^T为无人艇的速度向量；

定义无人艇姿态跟踪误差向量e：

e＝η_d-η——公式2；

公式2中：η_d为期望姿态向量；

对公式2求一阶导后，代入公式1中，得到姿态误差开环动态方程：

将公式3离散化，定义在时刻k，

和η(k)的值是确定的；且M,C(η),D(η),τ_w(t),e(t)均为确定的矩阵，此时，令y(k)＝e(k)，代入公式3得：

定义辅助函数F(k)为：

定义采样时间为T，将

代入公式4得：

记：Δy(k+1)＝y(k+1)-y(k)为相邻两时刻的输出变化；

Δτ(k)＝τ(k)-τ(k-1)为相邻两时刻的输入变化；

定义时变参数矩阵

使公式6系统可以转化为如下的数据模型

其中ΔH(k)＝||[Δy(k)Δτ(k)]^T||≠0，且

对任意时刻k有界；

参照如下控制输入准则函数：

J(τ(k))＝||y^*(k+1)-y(k+1)||²+λ||τ(k)-τ(k-1)||²——公式8；

公式8中：λ为一个大于0的权重因子，用于惩罚控制输入量变化过大；τ＝[T_x T_y T_z]为控制输入；y^*(k+1)为期望输出；

由Cauchy微分中值定理得：

将其代入公式8中，对其求关于τ(k)的偏导，并令其等于0，得到航向航速控制律如下：

公式9中：ρ∈(0,1]为步长因子。

3.根据权利要求2所述的一种环保作业的无人艇控制方法，其特征在于：路径规划航线的生成包括以下步骤：

读取用户输入的作业点，标记无人艇起点s，除起点s外所有作业点记入集合U，并将已规划的作业点记入集合S；

比较起点s到集合U中各作业点之间的距离L，取距离最近的作业点记作下一个作业点k，将作业点k加入集合S，从集合U中移除作业点k；

判断集合U是否为空集，若否则比较当前作业点到集合U中剩余作业点之间的距离，取最近的作业点覆盖记作下一作业点k，将作业点k加入集合S，从集合U中移除作业点k；若是则结束路径规划。

4.根据权利要求1所述的一种环保作业的无人艇控制方法，其特征在于：无人艇在行驶中还包括无人艇避障控制方法，包括以下步骤：

激光雷达探测到障碍物，生成从当前无人艇的位置作为起点q-start到终点q-goal的直线连接路径m-line；

无人艇沿着m-line移动，当到达障碍物附近时，记录此刻无人艇的位置Xi；控制无人艇沿着障碍物的轮廓进行轮廓追踪；

当无人艇重新进入路径m-line后，沿着路径m-line到达终点q-goal；

当无人艇回到初始位置Xi时，则判断目标q-goal不可达，通知工作人员进行手动操控。

5.一种环保作业的无人艇系统，其特征在于：应用有如权利要求3-4任意一项所述环保作业的无人艇控制方法，本无人艇系统包括有：

无人艇本体，具有用于承载物件，并可以在水面上行驶的艇身；

无人艇清扫装置，在无人艇行驶的路径上，用于切割水草并对水面上的垃圾进行清理收集；

水质监测传感器，对在无人艇行驶的路径上，用于对水体的水质进行探测；

定位系统，用于对无人艇的位置进行定位；

推进器，设置在无人艇本体后方的两端，用于驱动无人艇本体，控制无人艇本体前进的航向和航速；

路径生成系统，用于生成无人艇行进的路径航线；

路径追踪系统，包括外环制导模块和内环控制模块，外环制导模块用于计算出期望艏向角ψ_ref，内环控制模块用于根据路径航线通过两个推进器的油门对无人艇的航向和航速进行控制，驱动无人艇本体沿着航线行驶；

所述外环制导模块包括：

路径参数化模块，用于设置参数θ将路径参数化；

无人艇位置与时间参数化模块，用于对无人艇在t时刻的位置参数化；

距离误差函数模块，用于定义距离误差代价函数；

最近点确认模块，用于获取无人艇到路径上的最近点；

跟踪制律模块，用于获得无人艇路径跟踪的期望艏向角的跟踪制律。

6.根据权利要求5所述的一种环保作业的无人艇系统，其特征在于：所述内环控制模块包括：

动力模型模块，用于定义无人艇平面运动的动力学模型；

误差向量模块，定义无人艇姿态跟踪误差向量；

姿态误差模块，用于获得无人艇姿态误差开环动态方程；

离散模块，用于将姿态误差开环动态方程离散化；

辅助函数模块，用于定义辅助函数；

采样时间代入模块，用于定义采样时间，将采样时间代入离散模块运算结果中；

时变参数矩阵模块，用于定义时变参数矩阵，转化数据模型；

控制输入准则函数模块，用于定义控制输入准则函数；

Cauchy微分中值模块，用于运算Cauchy微分中值定理得到微分方程；

航向航速控制律模块，用于获得无人艇路径跟踪的航向航速控制律。

7.根据权利要求6所述的一种环保作业的无人艇系统 ，其特征在于：所述路径生成系统包括：

读取模块，用于读取用户输入的作业点；

存储模块，用于存储和标记作业点；

判断模块，用于判断作业点间的距离长短，和判断集合U是否为空集；

执行模块，用于根据判断模块的判断结果，通过无人艇当前的位置计算出下一个距离最近的作业点。

8.根据权利要求5所述的一种环保作业的无人艇系统，其特征在于：还包括避障系统，所述避障系统包括：

激光雷达探测模块，用于对无人艇行驶前进的方向进行激光雷达探测；

避障路径规划模块，用于生成起始点和终点的虚拟直线路径；

路径控制模块，用于记录无人艇的位置并判断无人艇的行驶路径；

轮廓跟踪模块，用于控制无人艇沿着雷达探测的障碍物轮廓路径进行跟踪行驶。

Images