一种通信时延下基于人工势场法的AUV编队无死锁避碰方法

一种通信时延下基于人工势场法的auv编队无死锁避碰方法
技术领域
1.本发明属于船舶控制技术领域，具体涉及一种通信时延下基于人工势场法的auv编队无死锁避碰方法。


背景技术：

2.随着海洋任务需求的增加，auv的工作环境就会变得越来越复杂，其作业的难度也随之提高，自身携带的有限资源和本身的能力不足等问题，一般无法独立完成这些任务。近年来基于多auv水下探测系统的研究工作层出不穷，auv编队协同作业替代单个auv作业则可以弥补其自身能力不足的问题，编队具有更高的工作效率，特别是在导航、路径规划、协同数据采集、目标搜索等方面。
3.auv编队协调控制在实际操作过程中的基础是成员之间可以完成信息的交互，即拥有互相通信的能力。但是水声通信的传输速度较慢，这限制了水声通信时的带宽，所以与无线电在陆上传输方式相比，通信时延就不能忽略。所以，通信时延是auv编队协调作业中不可避免的难题。在编队变换的过程中会出现由于避碰算法的控制作用致使auv无法到达期望编队位置的现象。比如auv1和auv2的位置为各自的期望位置，驱动力和势场力共线，并相互抵消，造成死锁现象。
4.对于现有死锁问题的研究成果进行检索，文献(基于匈牙利算法的多auv编队控制研究)提出死锁问题但并未对死锁问题提出相应的解决措施。虽然死锁问题在实际环境中发生的概率较低，但引入避免死锁的分量后可以避免在某些极端环境下编队控制陷入死锁，同时加入避免死锁的分量对编队避碰的效果几乎不会产生影响。


技术实现要素：

5.本发明的目的在于解决auv编队控制过程中可能发生的碰撞和死锁问题，提供一种通信时延下基于人工势场法的auv编队无死锁避碰方法。
6.一种通信时延下基于人工势场法的auv编队无死锁避碰方法，包括以下步骤：
7.步骤1：对于由一个领航者、n个跟随者组成的auv编队，auv编队存在相同时延τ；获取编队中每个auv的位置信息、姿态信息、速度信息和角速度信息；
8.步骤2：建立auv的非线性模型并对其进行反馈线性化；
9.忽略横摇运动，在船体坐标系下建立全驱动auv的三维数学模型；
[0010][0011]
其中，表示auv在大地坐标系下的位姿向量；表示auv在船体坐标系下的速度向量；m是惯性矩阵，包括附加质量；j(η)是变换矩阵；c(μ)是向心力和科氏力矩阵，包括附加质量产生的向心力和科氏力；d(μ)是水动力阻力和升力力矩；g(η)是恢复力和力矩向量；是在船体坐
标系下的控制器输入向量；是执行机构的参数矩阵，忽略高阶非线性水动力阻尼项和横摇运动对auv的影响；
[0012]
将c(μ)μ、d(μ)μ、g(η)三项合并成一个列向量n(η,μ)表示，auv非线性模型转化为：
[0013][0014]
为了便于模型的线性化，将上式写成如下形式，
[0015][0016]
非线性系统的输出量为位姿向量，则auv的非线性模型为：
[0017][0018]
其中，h(ξ)＝(x,y,z,θ,ψ)
t
；g(ξ)＝m1g
′
；
[0019]
根据lie导数可知：
[0020][0021]
可得一阶lie导数lfh(ξ)，lgh(ξ)具有如下形式：
[0022][0023]
根据二阶lie导数的定义，可知：
[0024][0025]
二阶lie导数lglfh(ξ)具有如下形式：
[0026][0027]
该模型的相对阶之和为：ρ1 ρ2 ρ3 ρ4 ρ5＝10，其中ρ1＝ρ2＝ρ3＝ρ4＝ρ5＝2，即相对阶的和等于系统的阶数10；可知该auv模型可以进行精确反馈线性化，并且有解；选择坐标变化如下：
[0028][0029]
将lie导数代入可得：
[0030][0031]
令那么在新的坐标转换下实际的控制输入为：
[0032][0033]
得到auv线性化数学模型为：
[0034][0035]
编队中所有auv具有相同的非线性模型，所以编队中其他auv可以进行相同的反馈线性化，经过转换后的第i个auv线性化数学模型为：
[0036][0037]
其中，0≤i≤n，i＝0表示领航者auv，i＝1,2,...,n表示第i个跟随者auv；
[0038][0039]
步骤3：设计存在时延下的多auv编队队形一致性控制器；
[0040]
步骤3.1：auv编队存在相同时延τ，考虑编队时延后，多auv编队队形一致性控制器表示为：
[0041]
其中，α表示位置增益；β是速度增益；σ
l
(t)和v
l
(t)分别是编队中领航者的位姿和速度状态；a
ij
(t)表示第i个跟随者auv与第j个跟随者auv之间的通信关系；c
il
(t)表示第i个跟随者auv与领航者auv之间的通信关系；ni为第i个auv的邻域；
[0042][0043]
步骤3.2：解决编队变换过程中的碰撞问题，设计控制器中避碰相关的势场函数；
[0044]
设δ
ij
表示两个auv之间的相对距离，r表示auv的安全距离，r是产生斥力区域的半径，auvi＝(xi(t),yi(t),zi(t))表示第i个auv的坐标，auvj表示第j个auv的坐标信息，具体的势能函数为：
[0045][0046]
其中，kr是势能函数的控制增益，通过调整控制增益kr使auv控制更加的平滑，从而使避碰效果更理想；对势能函数u
ij
求梯度函数得到第i个auv对第j个auv的势场力：
[0047][0048]
其中，pi＝[δxδyδz]
t
，δx＝xi(t)-xj(t)，δy＝yi(t)-yj(t)，δz＝zi(t)-zj(t)，并且则沿x方向的势场力为：沿y方向和z方向的势场力同理可得，那么第i个auv受到的总势场力表示如下：
[0049][0050]
步骤3.3：解决加入避碰分量u
ri
(t)后可能导致的死锁问题；
[0051]
由于加入了避碰分量u
ri
(t)，可能致使驱动形成编队的某一方向控制力与避碰分量u
ri
(t)共线且相互抵消，auv无法达到期望位置，从而导致死锁问题；为避免出现死锁问题，在编队控制器中增加避免死锁的分量u
rj
(t)＝ρu
ri
(t)，其中ρ是一个较小的控制系数；当避碰力u
ri
(t)和驱动形成编队的某一方向控制力共线时，会产生垂直于共线方向一段时间的力u
rj
(t)，从而使得两个auv可以产生绕行的效果，避免死锁问题的发生，总的避碰力为
[0052]urk
(t)＝u
ri
(t) u
rj
(t)
[0053]
把总的避碰力u
rk
(t)加入到步骤3.1设计的控制器中得到最终的控制器为：
[0054][0055]
在该控制器的作用下，可避免在存在通信时延和参数变化扰动条件下auv编队控制过程中可能发生的碰撞和死锁问题。
[0056]
本发明的有益效果在于：
[0057]
本发明基于人工势场法和二阶积分模型，提供了一种通信时延下基于人工势场法的auv编队无死锁避碰方法，解决了auv编队控制过程中可能发生的碰撞和死锁问题，使得编队可以在存在通信时延条件下稳定收敛。
附图说明
[0058]
图1为运动坐标系和固定坐标系示意图。
[0059]
图2为编队队形示意图。
[0060]
图3为无避碰分量控制器作用下auv编队三维轨迹图。
[0061]
图4为无避碰分量控制器作用下auv编队二维轨迹图。
[0062]
图5为无避碰分量控制器作用下auv编队位置信息示意图。
[0063]
图6为无避碰分量控制器作用下auv编队姿态信息示意图。
[0064]
图7为无避碰分量控制器作用下auv编队速度信息示意图。
[0065]
图8为无避碰分量控制器作用下auv编队角速度信息示意图。
[0066]
图9为有避碰分量控制器作用下auv编队三维轨迹图。
[0067]
图10为有避碰分量控制器作用下auv编队二维轨迹图。
[0068]
图11为有避碰分量控制器作用下auv编队位置信息示意图。
[0069]
图12为有避碰分量控制器作用下auv编队姿态信息示意图。
[0070]
图13为有避碰分量控制器作用下auv编队速度信息示意图。
[0071]
图14为有避碰分量控制器作用下auv编队角速度信息示意图。
具体实施方式
[0072]
下面结合附图对本发明做进一步描述。
[0073]
本发明的目的是为了解决auv编队控制过程中可能发生的碰撞和死锁问题而提出的一种基于人工势场法和二阶积分模型相结合的auv编队控制方法，可以使得编队在存在通信时延条件下稳定收敛。
[0074]
首先，建立领航跟随者编队并进行初始化；
[0075]
考虑一个领航者，n个跟随者的auv编队，n为正整数，领航者的状态用0标记，n个跟随者的状态标记为1到n。初始化编队中每个auv的位置信息、姿态信息、速度信息和角速度信息。
[0076]
其次，建立auv的非线性模型并对其进行反馈线性化；
[0077]
为了实现对auv的运动控制，首先建立运动坐标系和固定坐标系，如图1所示，并在船体坐标系下建立全驱动auv的三维数学模型(忽略横摇运动)：
[0078][0079]
式中：表示auv在大地坐标系下的位姿向量；表示auv在船体坐标系下的速度向量；m是惯性矩阵，包括附加质量；j(η)是变换矩阵；c(μ)是向心力和科氏力矩阵，包括附加质量产生的向心力和科氏力；d(μ)是水动力阻力和升力力矩；g(η)是恢复力和力矩向量；是在船体坐标系下的控制器输入向量。是执行机构的参数矩阵，忽略高阶非线性水动力阻尼项和横摇运动对auv的影响。
[0080]
针对本文的研究对象，把模型中c(μ)μ，d(μ)μ，g(η)三项合并成一个列向量n(η,μ)表示，那么，auv非线性模型可转化为：
[0081][0082]
为了便于模型的线性化，将上式(2-15)写成如下形式：
[0083][0084]
非线性系统的输出量为位姿向量，则auv的非线性模型为：
[0085][0086]
且h(ξ)＝(x,y,z,θ,ψ)
t
，g(ξ)＝m1g
′
。
[0087]
根据lie导数可知：
[0088][0089]
可得一阶lie导数lfh(ξ)，lgh(ξ)具有下面形式：
[0090]
[0091]
同理根据二阶lie导数的定义，可知：
[0092][0093]
且二阶lie导数lglfh(ξ)具有下面形式：
[0094][0095]
该模型的相对阶之和为：ρ1 ρ2 ρ3 ρ4 ρ5＝10，其中ρ1＝ρ2＝ρ3＝ρ4＝ρ5＝2，即相对阶的和等于系统的阶数10。可知该auv模型可以进行精确反馈线性化，并且有解。选择坐标变化如下：
[0096][0097]
将lie导数(2-19)代入可得：
[0098][0099]
令那么在新的坐标转换下实际的控制输入为：
[0100][0101]
得到auv线性化数学模型为：
[0102][0103]
编队中所有auv具有相同的非线性模型，所以编队中其他auv可以进行相同的反馈线性化，经过转换后的第i个auv线性化数学模型为：
[0104][0105]
其中，0≤i≤n，
[0106]
然后，设计存在时延下的多auv编队队形一致性控制器；
[0107]
设auv编队存在相同时延τ，考虑编队时延后，多auv编队队形一致性控制器表示为：
[0108][0109]
其中α表示位置增益，β是速度增益，σ
l
(t)和v
l
(t)分别是编队中领航者的位姿和速度状态，a
ij
(t)表示第i个跟随者auv与第j个跟随者auv之间的通信关系，c
il
(t)表示第i个跟随者auv与领航者auv之间的通信关系，ni为第i个auv的邻域。
[0110]
最后，实现基于人工势场法的多auv避碰控制。
[0111]
考虑在编队变换过程中可能发生的碰撞和死锁问题。
[0112]
(1)解决编队变换过程中的碰撞问题：设计控制器中避碰相关的势场函数。设δ
ij
表示两个auv之间的相对距离，r表示auv的安全距离，r是产生斥力区域的半径，auvi＝(xi(t),yi(t),zi(t))表示第i个auv的坐标，auvj表示第j个auv的坐标信息。下面给出具体的势能函数：
[0113][0114]
其中：kr是势能函数的控制增益，通过调整增益可以使auv控制更加的平滑，从而使避碰效果更理想。对势能函数u
ij
求梯度函数可以得到第i个auv对第j个auv的势场力：
[0115][0116]
其中，pi＝[δx δy δz]
t
，δx＝xi(t)-xj(t)，δy＝yi(t)-yj(t)，δz＝zi(t)-zj(t)，并且则沿x方向的势场力为：沿y方向和z方向的势场力同理可得，那么第i个auv受到的总势场力表示如下：
[0117][0118]
将该避碰分量u
ri
(t)加入到步骤3设计的控制器中得到新的控制器为：
[0119][0120]
(2)解决加入避碰分量u
ri
(t)后可能导致的死锁问题
[0121]
由于加入了避碰分量u
ri
(t)，可能致使驱动形成编队的某一方向控制力与避碰分量u
ri
(t)共线且相互抵消，auv无法达到期望位置，从而导致死锁问题。为避免出现死锁问题，在编队控制器中增加避免死锁的分量u
rj
(t)＝ρu
ri
(t)，其中ρ是一个较小的控制系数。当避碰力u
ri
(t)和驱动形成编队的某一方向控制力共线时，会产生垂直于共线方向一段时间的力u
rj
(t)，从而使得两个auv可以产生绕行的效果，避免死锁问题的发生，总的避碰力为：
[0122]urk
(t)＝u
ri
(t) u
rj
(t)
ꢀꢀꢀꢀꢀꢀ
(19)把总的避碰力u
rk
(t)加入到步骤3设计的控制器中得到新的控制器为：
[0123]
[0124]
在式(12)所示控制器的作用下，可避免在存在通信时延和参数变化扰动条件下auv编队控制过程中可能发生的碰撞和死锁问题。
[0125]
以上设计方案，稳定性证明如下：
[0126]
设为auv编队的位姿状态，为auv编队的速度状态，l1为跟随者auv通信拓扑的laplacian矩阵，设l2代表领航者和跟随者的通信状态矩阵，则编队的状态可以表示成：
[0127][0128]
其中，表示编队中各auv势场力的集合，1n表示元素全部都是1的n维列向量即[1 1...1]
t
∈rn。设跟随者相对于领航者的位姿状态σ
l
(t)和速度状态v
l
(t)的状态误差为其中：
[0129][0130]
由laplacian矩阵和克罗内克积可以推导出下式：
[0131][0132]
根据式(20)和式(22)可以将原auv编队的状态方程转换成编队系统的误差方程，具体形式如下式：
[0133][0134]
简化式(23)，得到总的系统误差e(x)：
[0135][0136]
其中，如果满足式(3-26)lmi矩阵不等式，则所有跟随者auv可以在所设计协调控制器的作用下跟踪上领航者位姿和速度，从而形成期望的编队队形并保持一致性。
[0137][0138]
其中，ψ
11
＝2pa 2pb q τa
t
ra-pbm-1bt
p
t
，m是2n维的正定矩阵。
[0139]
考虑以下lyapunov-krasovskii泛函：
[0140][0141]
其中对称矩阵p≥0,q≥0,r≥0，令v1(t)＝e
t
(t)pe(t)，(t)pe(t)，对总的能量函数v(t)求导可得：
[0142][0143][0144][0145]
式(29)中f(c)是关于避碰项c的相关函数，式(27)中同样存在，如果满足避碰条件，则最终势场力趋于0。在式(29)中，由r正定可以得出由牛顿-莱布尼兹公式可得：
[0146][0147]
所以：
[0148][0149]
可以得出，如果auv编队满足式(25)的lmi矩阵不等式则收敛，所以基于反馈线性化的auv编队在满足避碰和lmi等约束条件下，编队可以在通信存在相同时延的情况下稳定收敛到期望队形并保持。
[0150]
实施例1：
[0151]
步骤一：建立领航跟随者编队并进行参数初始化；
[0152]
考虑1个领航者，2个跟随者的auv编队，领航者的状态用0标记，2个跟随者的状态标记为1和2。三个auv保持的队形如图2所示，其中r为10米，β为π/4。选取文献(水下潜航器编队海洋勘探的协调控制方法研究)中的auv模型作为仿真对象，详细模型参数与之相同。
[0153]
设计领航者的轨迹为：
[0154][0155]
两个跟随者被随机布放：xi(0)和yi(0)的初始范围为[0,5]m，zi(0)的初始范围为[-3,0]m，初始姿态θi(0)和艏向角ψi(0)初始范围为[0,2]rad，纵向速度ui(0)初始范围为[0,0.5]m/s，其他速度都初始化为0m/s。
[0156]
步骤二：建立auv的非线性模型并对其进行反馈线性化
[0157]
为了实现对auv的运动控制，首先建立运动坐标系和固定坐标系，如图1所示，并在船体坐标系下建立全驱动auv的三维数学模型(忽略横摇运动)：
[0158][0159]
式中：表示auv在大地坐标系下的位姿向量；表示auv在船体坐标系下的速度向量；m是惯性矩阵，包括附加质量；j(η)是变换矩阵；c(μ)是向心力和科氏力矩阵，包括附加质量产生的向心力和科氏力；d(μ)是水动力阻力和升力力矩；g(η)是恢复力和力矩向量；是在船体坐标系下的控制器输入向量。是执行机构的参数矩阵，忽略高阶非线性水动力阻尼项和横摇运动对auv的影响。
[0160]
针对本文的研究对象，把模型中c(μ)μ，d(μ)μ，g(η)三项合并成一个列向量n(η,μ)表示，那么，auv非线性模型可转化为：
[0161][0162]
为了便于模型的线性化，将上式(2-15)写成如下形式：
[0163][0164]
非线性系统的输出量为位姿向量，则auv的非线性模型为：
[0165][0166]
且h(ξ)＝(x,y,z,θ,ψ)
t
，g(ξ)＝m1g
′
。
[0167]
根据lie导数可知：
[0168][0169]
可得一阶lie导数lfh(ξ)，lgh(ξ)具有下面形式：
[0170][0171]
同理根据二阶lie导数的定义，可知：
[0172][0173]
且二阶lie导数lglfh(ξ)具有下面形式：
[0174][0175]
该模型的相对阶之和为：ρ1 ρ2 ρ3 ρ4 ρ5＝10，其中ρ1＝ρ2＝ρ3＝ρ4＝ρ5＝2，即相对阶的和等于系统的阶数10。可知该auv模型可以进行精确反馈线性化，并且有解。选择坐标变化如下：
[0176][0177]
将lie导数(2-19)代入可得：
[0178][0179]
令那么在新的坐标转换下实际的控制输入为：
[0180][0181]
得到auv线性化数学模型为：
[0182][0183]
编队中所有auv具有相同的非线性模型，所以编队中其他auv可以进行相同的反馈线性化，经过转换后的第i个auv线性化数学模型为：
[0184][0185]
其中，0≤i≤n，
[0186]
步骤三：设计多auv编队队形一致性控制器；
[0187]
设auv编队存在相同时延τ，考虑编队时延后，多auv编队队形一致性控制器表示为：
[0188][0189]
其中α表示位置增益，β是速度增益，σ
l
(t)和v
l
(t)分别是编队中领航者的位姿和速度状态，a
ij
(t)表示第i个跟随者auv与第j个跟随者auv之间的通信关系，c
il
(t)表示第i个跟随者auv与领航者auv之间的通信关系，ni为第i个auv的邻域。
[0190]
步骤四：基于人工势场法的多auv避碰控制
[0191]
考虑在编队变换过程中可能发生的碰撞和死锁问题。
[0192]
(1)解决编队变换过程中的碰撞问题：设计控制器中避碰相关的势场函数。设δ
ij
表示两个auv之间的相对距离，r表示auv的安全距离，r是产生斥力区域的半径，auvi＝(xi(t),yi(t),zi(t))表示第i个auv的坐标，auvj表示第j个auv的坐标信息。下面给出具体的势能函数：
[0193][0194]
其中：kr是势能函数的控制增益，通过调整增益可以使auv控制更加的平滑，从而使避碰效果更理想。对势能函数u
ij
求梯度函数可以得到第i个auv对第j个auv的势场力：
[0195][0196]
其中，pi＝[δx δy δz]
t
，δx＝xi(t)-xj(t)，δy＝yi(t)-yj(t)，δz＝zi(t)-zj(t)，并且则沿x方向的势场力为：沿y方向和z方向的势场力同理可得，那么第i个auv受到的总势场力表示如下：
[0197][0198]
将该避碰分量u
ri
(t)加入到步骤3设计的控制器中得到新的控制器为：
[0199][0200]
(2)解决加入避碰分量u
ri
(t)后可能导致的死锁问题
[0201]
由于加入了避碰分量u
ri
(t)，可能致使驱动形成编队的某一方向控制力与避碰分量u
ri
(t)共线且相互抵消，auv无法达到期望位置，从而导致死锁问题。为避免出现死锁问题，在编队控制器中增加避免死锁的分量u
rj
(t)＝ρu
ri
(t)，其中ρ是一个较小的控制系数。当避碰力u
ri
(t)和驱动形成编队的某一方向控制力共线时，会产生垂直于共线方向一段时间的力u
rj
(t)，从而使得两个auv可以产生绕行的效果，避免死锁问题的发生，总的避碰力为：
[0202]urk
(t)＝u
ri
(t) u
rj
(t)(52)把总的避碰力u
rk
(t)加入到步骤3设计的控制器中得到新的控制器为：
[0203][0204]
在式(53)所示控制器的作用下，可避免在存在通信时延和参数变化扰动条件下auv编队控制过程中可能发生的碰撞和死锁问题。
[0205]
设计控制参数α＝0.06，β＝0.7，延时时间τ＝1s，由于两个跟随者auv分别接收领航者的位姿等信息，两个跟随者auv之间不进行通信。避碰势能函数中的系数kr设为0.5，编队各auv的避碰半径r为8.5米，安全距离r为5米。
[0206]
根据公式(54)和公式(55)分别计算不含避碰分量u
rj
(t)的控制器和含避碰分量u
rj
(t)的控制器：
[0207][0208][0209]
接着根据公式(56)将控制器转换到运动坐标系下：
[0210][0211]
将τ作用在auv非线性模型上，假设在1600秒时，产生队形的变换，auv1和auv2期望的路径做一个交换，使用不含u
rj
(t)的控制器，此时发生了如图3和图4中的死锁现象，严重影响编队控制稳定性。图5和图6为auv编队的位姿信息，图7和图8为auv编队的速度和角速度信息；使用含u
rj
(t)的控制器，如图9和图10所示，两个auv以一种绕过的形式错开，而不是长时间的处于死锁的状态，编队稳定收敛。图11和图12为auv编队的位姿信息，图13和图14为auv编队的速度和角速度信息。
[0212]
以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。