一种多无人艇编队避碰的优化控制器及其结构、设计方法

1.本发明涉及水面无人艇自主控制领域，尤其涉及一种多无人艇编队避碰的优化控制器及其结构、设计方法。


背景技术：

2.典型的多无人艇集群控制方法有领航
‑
跟随法、基于行为法、虚拟结构法、人工势能法、图论法；基本控制方法也有滑模控制、参数自适应、输出反馈、神经网络、模糊逻辑、连通保持、抗扰控制、约束控制、避障避碰等方法；在控制器的设计方面主要有反步法、动态面、跟踪微分器、指令调节器等方法。
3.但是现有的多无人艇编队控制方法仍存在着以下不足：
4.第一，针对复杂的海洋环境，现有的基于势场函数的无人艇避碰方法往往缺乏考虑系统的输入约束问题，使得无人艇在初始时刻和控制过程中容易出现输入饱和。
5.第二，基于模型预测控制的无人艇避碰方法，在线计算量较大，因而大多数模型预测算法只适用于具有高性能计算机的环境，大大限制了其应用。
6.第三，现有的控制方法通常基于稳定、成熟的控制系统上，难以进行再次开发、更新，若想优化其中某些性能，就需要对其进行重新设计，耗费大量人力物力。


技术实现要素：

7.本发明提供一种多无人艇编队避碰的优化控制器及其结构、设计方法，以克服上述技术问题。
8.本发明一种多无人艇编队避碰的优化控制器设计方法，包括：
9.建立无人艇编队中无人艇的运动学模型；所述无人艇编队包括：多艘无人艇、至少一艘领导者无人艇及无人艇之间相互通信的通信网络；
10.根据所述无人艇的运动学模型通过所述通信网络获得无人艇编队和本艇的位置信息；根据所述位置信息计算得到本艇与无人艇编队运动保持一致所需的期望速度；
11.根据从本艇获得的当前速度计算得到无人艇不确定性的估计值；
12.根据所述期望速度、当前速度及不确定性的估计值，计算得到本艇与无人艇编队运动保持一致所需的速度控制率；
13.为所述优化控制模块提供本艇的避碰和避障约束条件；
14.根据所述速度控制率、不确定性的估计值及避碰和避障约束条件计算得到优化后的速度控制率；
15.根据所述优化后的速度控制率计算得到动力学控制律，将所述动力学控制律输出至本艇的动力控制系统，使得本艇与无人艇编队运动保持一致，避免发生互碰。
16.进一步地，所述无人艇的运动学模型，表示为：
[0017][0018]
同时，无人艇满足式(2)的动力学模型：
[0019][0020]
式中，p
i
表示无人艇在地球坐标系下位置，q
i
表示无人艇在地球坐标系下的速度，q
ix
，q
iy
表示无人艇在地球坐标系下x轴和y轴方向的速度，ψ
i
表示无人艇航向，r
i
表示无人艇艏摇角速度，f
ix
(
·
)，f
iy
(
·
)，f
ir
(
·
)表示无人艇在x轴，y轴以及角速度方向的不确定性，和表示船舶质量，τ
iu
，τ
ir
表示船舶控制输入。
[0021]
一种多无人艇编队避碰的优化控制器，包括：
[0022]
编队制导模块、速度控制律模块、扰动观测模块、控制闸函数模块、优化控制模块及动力学控制律模块；
[0023]
所述编队制导模块，用于获得无人艇编队和本艇的位置信息，根据所述位置信息计算得到本艇与无人艇编队运动保持一致所需的期望速度，并输出至所述速度控制律模块；
[0024]
所述速度控制律模块，用于根据所述期望速度、从本艇获得的当前速度及所述扰动观测模块输出的不确定性的估计值，计算得到本艇与无人艇编队运动保持一致所需的速度控制率，并输出至所述优化控制模块；
[0025]
所述扰动观测模块，用于根据从本艇获得的当前速度计算得到无人艇不确定性的估计值，将所述不确定性的估计值输出至所述优化控制模块和速度控制律模块；
[0026]
所述控制闸函数模块，用于为所述优化控制模块提供本艇的避碰和避障约束条件；
[0027]
所述优化控制模块，用于根据所述速度控制率、不确定性的估计值及避碰和避障约束条件，计算得到优化后的速度控制率，并输出至所述动力学控制律模块；
[0028]
所述动力学控制律模块，用于根据所述优化后的速度控制率计算得到动力学控制律，将所述动力学控制律输出至本艇的动力控制系统，使得本艇与无人艇编队运动保持一致，避免发生互碰。
[0029]
进一步地，所述无人艇编队包括：多艘无人艇、至少一艘领导者无人艇及无人艇之间相互通信的通信网络；所述位置信息包括：本艇的位置信息p
i
、领导者无人艇在地球坐标系下的位置信息p
kr
、从所述通讯网络中获得的邻居无人艇的位置信息p
j
；
[0030]
所述编队制导模块表示为：
[0031][0032]
其中：
[0033][0034]
式中，表示期望速度，e
i
为包含操纵控制误差；若a
ij
＝1代表第j艘无人艇将信息传给第i艘无人艇，若a
ik
＝1代表第k个领导者无人艇将信息传给第i艘无人艇，否则a
ij
＝0、a
ik
＝0；k
ig
为设计参数；v
s
表示路径更新速度，为一个常数；表示路径对θ
k
的偏导数，表示第i艘无人艇的邻居无人艇集合。
[0035]
进一步地，所述扰动观测模块，表示为：
[0036][0037]
式中，为无人艇在地球坐标系下的估计速度，q
i
表示无人艇在地球坐标系下的速度；k
i1
、k
i2
为设计参数；为估计不确定信号；τ
iu
，τ
ir
表示船舶控制输入；和表示船舶质量；ψ
i
表示无人艇航向。
[0038]
进一步地，所述速度控制律模块，表示为：
[0039][0040]
式中，k
ic
为给定参数，为速度控制律，τ
ix
，τ
iy
为无人艇在x轴，y轴的速度分量，为编队制导输出，即期望速度。
[0041]
进一步地，所述控制闸函数模块，表示为：
[0042][0043]
式中，h
ij
为第i艘船和第j艘船之间的避碰目标函数；p
i
表示无人艇在地球坐标系下位置，p
ij
＝p
i
‑
p
j
；q
ij
＝q
i
‑
q
j
；α
i
和α
j
分别为第i艘船和第j艘船的前向速度方向的最大加速度；d
s
为极限避碰距离；p
ijt
表示第i艘无人艇和第j艘无人艇的距离的转置。
[0044]
进一步地，所述优化控制模块，表示为：
[0045][0046][0047][0048][0049]
[0050]
其中：
[0051][0052]
式中，γ为常数，o为障碍物数量，为优化中间变量，为第i艘船舶到障碍物距离的转置，为定义的不等式约束条件，为速度在x轴，y轴的分量。
[0053]
进一步地，所述动力学控制律模块，表示为：
[0054][0055]
其中：
[0056][0057]
式中，r
i
为角速度，为角速度的估计值，为角速度不确定性的偏导数，ψ
ie
＝ψ
i
‑
ψ
ic
，r
ie
＝r
i
‑
r
ic
，k
ic
，k
i3
，k
i4
为给定参数。
[0058]
一种多无人艇编队避碰的优化控制器结构，包括：
[0059]
所述编队制导模块与通讯网络相连接；所述编队制导模块的两个输入端口分别与外部输入信号端口和本艇的动力控制系统的输出端口相连接，输出端口与所述速度控制律模块相连接；所述速度控制律模块的输入端与所述编队制导模块、本艇的动力控制系统及扰动观测器的输出端口相连接，输出端口与所述优化控制器模块相连接；所述扰动观测模块输入端口与本艇的动力控制系统相连接，输出端口与所述优化控制模块相连接；所述控制闸函数模块的输出端口与所述优化控制模块相连接；所述优化控制器模块的输出端口与所述动力学控制律模块相连接；所述动力学控制律模块与本艇的动力控制系统相连接。
[0060]
本发明的控制器考虑了输入受限问题，使其满足输入控制约束，保证任何情况下的控制输入均为有界，避免了因输入饱和带来的一些问题。通过优化控制闸函数以及避碰约束函数，极大的简化了计算量，使其能够更加快速的达到稳定，同时大大节约了成本。在原有控制系统的基础上优化了控制模块，保留原有的基本控制功能，同时改进性能，节约重新设计的成本。
附图说明
[0061]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作以简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图；
[0062]
图1是多无人艇编队避碰的优化控制器结构示意图；
[0063]
图2是无人艇运动轨迹仿真图；
[0064]
图3是无人艇编队误差仿真图；
[0065]
图4是一号无人艇的避碰距离和避障距离结果仿真图；
[0066]
图5是一号无人艇力矩优化前后仿真图；
[0067]
图6是二号无人艇力矩优化前后仿真图；
[0068]
图7是多无人艇编队避碰的优化控制器设计方法流程图。
具体实施方式
[0069]
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0070]
如图7所示，本实施例提供了一种多无人艇编队避碰的优化控制器设计方法，包括：
[0071]
101、建立无人艇编队中无人艇的运动学模型；无人艇编队包括：多艘无人艇、至少一艘领导者无人艇及无人艇之间相互通信的通信网络；
[0072]
具体而言，所述无人艇的运动学模型，表示为：
[0073][0074]
同时，无人艇满足式(2)的动力学模型：
[0075][0076]
式中，p
i
表示无人艇在地球坐标系下位置，q
i
表示无人艇在地球坐标系下的速度，q
ix
，q
iy
表示无人艇在地球坐标系下x轴和y轴方向的速度，ψ
i
表示无人艇航向，r
i
表示无人艇艏摇角速度，f
ix
(
·
)，f
iy
(
·
)，f
ir
(
·
)表示无人艇在x轴，y轴以及角速度方向的不确定性，和表示船舶质量，τ
iu
，τ
ir
表示船舶控制输入。
[0077]
102、根据无人艇的运动学模型通过通信网络获得无人艇编队和本艇的位置信息；根据位置信息计算得到本艇与无人艇编队运动保持一致所需的期望速度；
[0078]
103、根据从本艇获得的当前速度计算得到估计出无人艇不确定性的估计值；
[0079]
104、根据期望速度、当前速度及不确定性的估计值，计算得到本艇与无人艇编队运动保持一致所需的速度控制率；
[0080]
105、为优化控制模块提供本艇的避碰和避障约束条件；
[0081]
106、根据所述速度控制率、不确定性的估计值及避碰和避障约束条件计算得到优化后的速度控制率；
[0082]
107、根据优化后的速度控制率计算得到动力学控制律，将动力学控制律输出至本艇的动力控制系统，使得本艇与无人艇编队运动保持一致，避免发生互碰。
[0083]
如图1所示，本实施例提供一种多无人艇编队避碰的优化控制器，包括：编队制导模块、速度控制律模块、扰动观测模块、控制闸函数模块、优化控制模块及动力学控制律模块；
[0084]
编队制导模块，用于获得无人艇编队和本艇的位置信息，根据位置信息计算得到本艇与无人艇编队运动保持一致所需的期望速度，并输出至速度控制律模块；
[0085]
具体而言，无人艇编队包括：多艘无人艇、至少一艘领导者无人艇及无人艇之间相互通信的通信网络；位置信息包括：本艇的位置信息p
i
、领导者无人艇在地球坐标系下的位置信息p
kr
、从通讯网络中获得的邻居无人艇的位置信息p
j
；编队制导模块表示为：
[0086][0087]
其中：
[0088][0089]
式中，表示期望速度，e
i
为包含操纵控制误差；若a
ij
＝1代表第j艘无人艇将信息传给第i艘无人艇，若a
ik
＝1代表第k个领导者无人艇将信息传给第i艘无人艇，否则a
ij
＝0、a
ik
＝0；k
ig
为设计参数；v
s
表示路径更新速度，为一个常数；表示路径对θ
k
的偏导数，表示第i艘无人艇的邻居无人艇集合。
[0090]
扰动观测模块，用于根据从本艇获得的当前速度计算得到无人艇不确定性的估计值，将不确定性的估计值输出至优化控制模块和速度控制律模块；
[0091]
具体而言，扰动观测模块，表示为：
[0092][0093]
式中，为无人艇在地球坐标系下的估计速度，q
i
表示无人艇在地球坐标系下的速度；k
i1
、k
i2
为设计参数；为估计不确定信号；τ
iu
，τ
ir
表示船舶控制输入；和表示船舶质量；ψ
i
表示无人艇航向。
[0094]
速度控制律模块，用于根据期望速度、从本艇获得的当前速度及扰动观测模块输出的不确定性的估计值，计算得到本艇与无人艇编队运动保持一致所需的速度控制率，并输出至优化控制模块；
[0095]
具体而言，速度控制律模块，表示为：
[0096][0097]
式中，k
ic
为给定参数，为速度控制律，τ
ix
，τ
iy
为无人艇在x轴，y轴的速度分量，为编队制导输出，即期望速度。
[0098]
控制闸函数模块，用于为优化控制模块提供本艇的避碰和避障约束条件；
[0099]
具体而言，控制闸函数模块，表示为：
[0100][0101]
式中，h
ij
为第i艘船和第j艘船之间的避碰目标函数；p
i
表示无人艇在地球坐标系下位置，p
ij
＝p
i
‑
p
j
；q
ij
＝q
i
‑
q
j
；α
i
和α
j
分别为第i艘船和第j艘船的前向速度方向的最大加速度；d
s
为极限避碰距离；p
ijt
表示第i艘无人艇和第j艘无人艇的距离的转置。
[0102]
优化控制模块，用于根据速度控制率、不确定性的估计值及避碰和避障约束条件，计算得到优化后的速度控制率，并输出至动力学控制律模块；
[0103]
具体而言，优化控制模块，表示为：
[0104][0105][0106][0107][0108][0109]
其中：
[0110][0111]
式中，γ为常数，o为障碍物数量，为优化中间变量，为第i艘船舶到障碍物距离的转置，为定义的不等式约束条件，为速度在x轴，y轴的分量。
[0112]
动力学控制律模块，用于根据优化后的速度控制率计算得到动力学控制律，将动力学控制律输出至本艇的动力控制系统，使得本艇与无人艇编队运动保持一致，避免发生互碰。
[0113]
具体而言，动力学控制律模块，表示为：
[0114][0115]
其中：
[0116][0117]
式中，r
i
为角速度，为角速度的估计值，为角速度不确定性的偏导数，ψ
ie
＝ψ
i
‑
ψ
ic
，r
ie
＝r
i
‑
r
ic
，k
ic
，k
i3
，k
i4
为给定参数。
[0118]
如图1所示，本实施例提供一种多无人艇编队避碰的优化控制器结构，包括：编队制导模块与通讯网络相连接；编队制导模块的两个输入端口分别与外部输入信号端口p
kr
和本艇的动力控制系统的输出端口相连接，输出端口与速度控制律模块相连接；速度控制律模块的输入端与编队制导模块、本艇的动力控制系统及扰动观测器的输出端口相连接，输出端口与优化控制器模块相连接；扰动观测模块输入端口与本艇的动力控制系统相连接，输出端口与优化控制模块相连接；控制闸函数模块的输出端口与优化控制模块相连接；优化控制器模块的输出端口与动力学控制律模块相连接；动力学控制律模块与本艇的动力控制系统相连接。
[0119]
仿真试验如下：
[0120]
无人艇模型的具体参数选择如下：
[0121][0122][0123]
具体控制参数选择如下：
[0124]
v
s
＝1，γ＝0.2，d
s
＝10，
[0125]
k
i1
＝20，k
i2
＝100，k
i3
＝20，k
i4
＝100
[0126][0127]
仿真结果如图2
‑
图6所示。如图2所示，两边的黑线为虚拟领导者运动轨迹，中间的两条粗黑线为两艘无人艇的运动轨迹，可以看出受控无人艇能够自行保持连通并且在遇到障碍物时自行绕开，之后继续保持队形；如图3所示，两艘无人艇的不同方向上的避碰误差都能在较短时间内收敛到零附近；如图4所示，极限避障距离为障碍物的半径加2，这里的障碍物半径设置为4，所以极限避障距离为6，极限避碰距离为10，可以看出无人艇的避碰距离始终大于极限避碰距离，无人艇的避障距离也始终大于极限避障距离；如图5、图6所示，可以看出优化后的力矩相比于优化前更加快速的达到稳定，并且优化后的力矩始终在约束上界和约束下界范围内。
[0128]
最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依
然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。