一种AUV轨迹跟踪控制器设计方法与系统

一种auv轨迹跟踪控制器设计方法与系统
技术领域
1.本发明涉及控制技术领域，具体是涉及一种auv轨迹跟踪控制器设计方法与系统。


背景技术：

2.蒋云彪硕士名称为“欠驱动auv自适应轨迹跟踪控制研究”中提出的利用rbf神经网络与积分滑模相结合来实现欠驱动auv的轨迹跟踪控制。首先构建常规欠驱动auv的运动建模，包括运动学模型、动力学模型、干扰参数等，随后采用动态面技术、积分滑模来设计轨迹跟踪控制器，并引入rbf神经网络对auv本体建模未知参数以及外界不确定干扰进行实时补偿。本方案对本身不确定参数变化和外部环境的适应能力强，但在控制量幅度输出过大，容易出现自主水下机器人拐点的超调，以及对扰动输入饱和没有进一步的处理和优化，在消除抖振方面只是引入外部约束函数，导致该算法过于复杂，不容易应用于实际工程。


技术实现要素：

3.发明目的：针对以上缺点，本发明提供一种auv轨迹跟踪控制器设计方法，能够消除自主水下机器人在拐点控制量输入过大，容易超调的问题并且消除内部扰动和输入饱和对控制性能的影响。本发明还提供一种auv轨迹跟踪控制器系统，能够提升自主水下机器人的轨迹跟踪能力。
4.技术方案：为解决上述问题，本发明采用一种auv轨迹跟踪控制器设计方法，包括以下步骤：
5.(1)构建全驱动自主水下机器人的运动学数学模型、动力学数学模型；
6.(2)针对运动学数学模型采集相关的auv设备信息，包括auv设备的纵向线速度、横向线速度、垂向线速度、俯仰角速度、艏向角速度；进行自适应的虚拟向导设计，获得auv轨迹误差方程；
7.(3)设计运动学控制器，具体包括：根据auv轨迹误差获得期望纵倾角θ
ed
和期望艏向角ψ
ed
；将艏向角θ和纵倾角ψ作为虚拟控制量输入，设计俯仰角速度q
ed
和艏向角速度控制律r
ed
为：
[0008][0009]
式中，k2和k3为正定常数，θ表示俯仰角，ψ表示艏向角；
[0010]
(4)设计动力学控制器，具体包括：
[0011]
(4.1)将运动学控制器输出通过一阶滤波器进行滤波，引入二阶非线性微分跟踪器并根据二阶非线性微分跟踪器的输出定义误差跟踪速度为：
[0012]
[ue,qe,re]
t
＝[u-v1,q-q
ed
,r-r
ed
]
t
[0013]
式中，ue表示镇定纵向速度误差，qe表示俯仰控制力，re表示艏向控制力；u表示纵向线速度，q表示俯仰角速度，r表示艏向角速度；v1和v2为二阶微分跟踪器的输出信号；
[0014]
(4.2)设计纵向线速度、俯仰角速度、艏向角速度的误差积分滑模面，并对误差积
分滑模面进行求导；引入指数趋近律对误差积分滑模面进行补偿并采用径向基神经网络对auv中未知的不确定项进行估计逼近，最终获得auv的动力学控制律，通过auv的动力学控制律将当前auv设备的纵向线速度、俯仰角速度、艏向角速度反馈至auv设备，循环步骤(2)至步骤(4)直至auv轨迹误差为0。
[0015]
进一步的，步骤(4.3)中auv的动力学控制律为：
[0016][0017]
式中，τu表示纵向动力学控制律，τq表示俯仰角动力学控制律，τr表示艏向动力学控制律；fu表示外界水流扰动产生的纵向力矩，fq表示外界水流扰动产生的俯仰角力矩，fr表示外界水流扰动产生的艏向力矩；m
ii
(1,2,3,4,5)表示为auv的惯性水动力为水下机器人在加速过程中受到周围水流惯性产生的力；为q
ed
通过一阶滤波器获得的值，为r
ed
通过一阶滤波器获得的值；s1为镇定纵向速度的误差积分滑模面，s2为俯仰角速度的误差积分滑模面，s3为艏向角速度的误差积分滑模面；均为auv中未知的不确定项通过神经网络得到的逼近值；
[0018]fu
＝m
22
vr-m
33
wq-xuu-x
μμ∣
u|u|
[0019][0020]fw
＝m
11
uq-m
22
vp-zww-z
w|w|
w|w|
[0021][0022]fr
＝(m
11-m
22
)uv+(m
44-m
55
)pq-nrr-n
r|r|
r|r|
[0023]
式中，u,v,w,q,r分别表示为纵向线速度、横向线速度、垂向线速度、俯仰角速度、艏向角速度；x
u|u|
,y
v|v|
,z
w|w|
,m
q|q|
,n
r|r|
为流体黏性引起的非线性阻尼项，向量为重心与浮心之间的距离。
[0024]
进一步的，还包括步骤(4.3)：设计辅助动态系统降低系统饱和对控制性能有较大影响，辅助系统为：
[0025][0026]
式中，χi(i＝u,q,r)是设计的辅助系统状态值；bi》0(i＝u,q,r)表示系数；gi(i＝u,q,r)，gu＝1/m
11
，gq＝1/m
44
，gr＝1/m
55
；δτi为系统饱和值；δτi＝τ
i-sat(τi)(i＝u,q,r)；li(i＝u,q,r)是正定常数；
[0027]
实际动力学控制器输出可设计为：
[0028][0029]
式中，a1、a2、a3为常数，l1＝lu，l2＝lq，l3＝lr。
[0030]
通过该辅助系统的设计减少内部扰动和输入饱和对auv的影响。
[0031]
进一步的，步骤(3)还包括设计虚拟向导和自适应控制律：通过将虚拟向导的运动速度作为机器人载体坐标系中x轴的输入并赋值于f，定义选取关于f
x
的李雅普诺夫函数v1为：
[0032][0033]
对v1求导得到：
[0034][0035]
根据李雅普诺夫稳定理论获得虚拟向导和自适应控制律为：
[0036][0037]
其中，xe表示auv路径跟踪误差在单位向量上t的投影；θe表示纵倾角误差；ψe表示艏向角误差；k为常数；k1表示可调参数；f为慢时变信号，为f的估计值。对于运动学控制器的设计进一步增加了一个虚拟向导和自适应控制律，能够更好的控制虚拟向导的运动速度
[0038]
进一步的，步骤(3)中根据auv轨迹误差获得期望纵倾角θ
ed
和期望艏向角ψ
ed
包括：
[0039]
(3.1)根据auv路径跟踪误差获取更新前的期望纵倾角θ
l
、期望艏向角ψ
l
：
[0040][0041]
式中，ye表示auv路径跟踪误差在单位向量上n的投影，ze表示auv路径跟踪误差在单位向量上b的投影；δ
θ
表示纵倾角变化的常数，δ
ψ
表示艏向角的变化的常数；
[0042]
(3.2)结合实际纵倾角和艏向角；获得更新后期望纵倾角θ
ed
和期望艏向角ψ
ed
为：
[0043][0044]
式中，θf为auv实际纵倾角，ψf为auv实际艏向角。
[0045]
进一步的，步骤(4.2)中所述的趋近律为：
[0046][0047]
式中，ε为滑模控制切换增益,l为指数趋近律，且ε》0,l》0。
[0048]
进一步的，步骤(4.2)中神经网络的函数为：
[0049][0050]
式中，为神经网络的权值；hi为神经网络的输入向量。
[0051]
此外，本发明采用一种auv轨迹跟踪控制器系统，包括：
[0052]
模型构建模块，用于构建全驱动自主水下机器人的运动学数学模型、动力学数学模型；
[0053]
auv轨迹误差获取模块，用于针对运动学数学模型采集相关的auv设备信息，包括auv设备的纵向线速度、横向线速度、垂向线速度、俯仰角速度、艏向角速度；进行自适应的虚拟向导设计，获得auv轨迹误差方程；；
[0054]
运动学控制器设计模块，用于设计运动学控制器，具体包括：根据auv轨迹误差获得期望纵倾角θ
ed
和期望艏向角ψ
ed
；将艏向角θ和纵倾角ψ作为虚拟控制量输入，设计俯仰角速度q
ed
和艏向角速度控制律r
ed
为：
[0055][0056]
式中，k2和k3为正定常数，θ表示俯仰角，ψ表示艏向角；
[0057]
动力学控制器设计模块，用于设计动力学控制器，具体包括：将运动学控制器输出通过一阶滤波器进行滤波，引入二阶非线性微分跟踪器并根据二阶非线性微分跟踪器的输出定义误差跟踪速度为：
[0058]
[ue,qe,re]
t
＝[u-v1,q-q
ed
,r-r
ed
]
t
[0059]
式中，ue表示镇定纵向速度误差，qe表示俯仰控制力，re表示艏向控制力；u表示纵向线速度，q表示俯仰角速度，r表示艏向角速度；v1和v2为二阶微分跟踪器的输出信号；设计纵向线速度、俯仰角速度、艏向角速度的误差积分滑模面，并对误差积分滑模面进行求导；引入指数趋近律误差积分滑模面进行补偿并采用径向基神经网络对auv中未知的不确定项进行估计逼近，最终获得auv的动力学控制律，通过auv的动力学控制律将当前auv设备的纵向线速度、俯仰角速度、艏向角速度反馈至auv设备；
[0060]
循环操作模块，用于循环操作auv轨迹误差获取模块、动力学控制器设计模块、动力学控制器设计模块直至auv轨迹误差获取模块中获取的轨迹误差为0。
[0061]
此外，本发明提供一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述方法的步骤。此外，本发明提供一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现上述方法的步骤。
[0062]
有益效果：本发明所述方法相对于现有技术，其显著优点是：1、引入二阶微分器，通过二阶微分器输出信号定义误差跟踪速度，进而设计误差积分滑模面克服了在拐点容易出现控制器超调的问题。2、设计辅助动态系统，通过辅助饱和在线函数的设计减少内部扰动和输入饱和对auv的影响。本发明所述系统相对于现有技术，其显著优点是：自主水下机器人具有更好的轨迹跟踪能力。
附图说明
[0063]
图1所示为本发明所述auv轨迹跟踪控制器设计方法与系统的原理图；
[0064]
图2所示为本发明所述auv轨迹跟踪控制器设计方法中虚拟向导轨迹示意图；
具体实施方式
[0065]
下面结合附图对本发明所述的技术方案作进一步的说明。
[0066]
如图1所示，本发明采用一种auv轨迹跟踪控制器设计方法，包括以下步骤：
[0067]
一、构建全驱动自主水下机器人的运动学数学模型、动力学数学模型；
[0068]
(1)auv水下机器人运动学模型表示为：
[0069][0070]
式中，x,y,z是auv在惯性坐标系的坐标位置，ψ,θ分别表示auv的横滚角、俯仰角、艏向角，u,v,w,q,r分别表示为纵向线速度、横向线速度、垂向线速度、俯仰角速度、艏向角速度；
[0071]
(2)auv水下机器人动力学模型表示为：
[0072][0073]
式中，m
ii
(1,2,3,4,5)表示为auv的惯性水动力为水下机器人在加速过程中受到周围水流惯性产生的力，具体为围水流惯性产生的力，具体为其中其中m表示水下机器人的重量；g表示速度和加速度多元矩阵函数，i
x
,iy,iz分别是绕载体坐标系x轴、y轴和z轴的惯性矩；xu,yu,zw,k
p
,mq和nr是线性水动力系数，w,b分比为auv的重力和浮力；x
u|u|
,y
v|v|
,z
w|w|
,m
q|q|
,n
r|r|
为流体黏性引起的非线性阻尼项；
[0074]
向量为重心与浮心之间的距离；fu、fv、fw、fq、fr为外界水流扰动产生的力矩，du、dv、dw、dq、dr为不同方向上的模型不确定项，具体表示为：
[0075]
[0076]
式中，为m
ii
对应的参数摄动。
[0077]
二、采集auv设备信息，针对运动学数学模型进行自适应的虚拟向导设计，获得auv轨迹误差方程；
[0078]
如图2所示，为auv空间曲线轨迹跟踪示意图，其中：i、b、f为分别为固定坐标系、载体坐标系、serret—frenet坐标系，o表示auv的重心，auv的重心与载体坐标系原点o重合，f坐标系原点为auv期望轨迹中的任意一个虚拟参考点，相对于auv的速度为vp，原点p为水下机器人期望轨迹上的“虚拟向导”。
[0079]
(1)坐标系的换算
[0080]
将固定坐标系{i}绕η轴旋转ψ度，在绕ζ轴旋转θ度，然后平移固定坐标系中点e与运动坐标系p相重合，serret-frenet坐标系框架相对于固定坐标系ψ、θ旋转的角度为：
[0081][0082]
其中表示在serret-frenet{f}坐标系下x,y,z方向下的方向向量导数；
[0083]
假设auv在水下航行时，纵向速度u恒不为0，考虑冲角α和漂角β的影响，其中α＝arctan(w/u)。流体坐标系{w}定义为将坐标系{b}分别沿y轴方向旋转角度β，接下来在沿着z轴方向旋转角度α，且坐标系{w}的x轴方向与合成向量u方向一致；所以auv运动方程在坐标系{i}下可以重新表示为：
[0084][0085]
式中，χq＝γ
β-β，γq＝γ
α-α；在坐标系{i}中e点位置向量可表示为：e＝[ξ,η,ζ]
t
和e点速度为：表示auv的坐标系空间合成向量。
[0086]
(2)获取serret-frenet坐标系下的轨迹跟踪误差模型
[0087]
auv质心o到{f}坐标系点p的位移ε为auv路径跟踪误差，在向量t、n、b上的投影由xe,ye,ze来表示，记为ε＝[xe,ye,ze]
t
,相对于期望路径上的任何虚拟向导，沿期望路径上的切线方向的单位向量t和沿期望路径的法线方向单位法向向量n，且一定存在b＝t*n；则流体坐标系{w}与坐标系{f}之间p点和o的相对速度可以表示为流体坐标系{w}与坐标系{f}的旋转矩阵定义为e(γ
α
,γ
β
),可以表示为：
[0088][0089]
流体坐标系{w}与serret—frenet坐标系{f}之间旋转的矩阵为d(γ
α
,γ
β
)，根据向量合成公式，可得出：点o在serret—frenet坐标系{f}的速度向量(d(γ
α
,γ
β
)(dq/dt)w为：(dp/dt)f+wf×
ε+(dε/dt)f[0090]
根据旋转矩阵和速度向量可以得到轨迹跟踪误差模型，表示为：
[0091][0092]
式中，参数s描述期望路径的曲线，又因为serret—frenet坐标系上原点瞬时速度的对应关系，其虚拟向导上的运动速度可表示为：c1(s)和c2(s)分别表示为期望路径上的曲率及挠率，其设定的期望路径参数s是连续且可导的。
[0093]
三、三维轨迹跟踪控制器设计
[0094]
(1)运动学控制器设计
[0095]
(1.1)虚拟向导和自适应控制律设计：通过将虚拟向导的运动速度作为机器人载体坐标系中x轴的输入并赋值于f，定义选取关于f
x
的李雅普诺夫函数v1为：
[0096][0097]
对v1求导得到：
[0098][0099]
根据李雅普诺夫稳定理论获得虚拟向导和自适应控制律为：
[0100][0101]
其中，xe表示auv路径跟踪误差在单位向量上t的投影；θe表示纵倾角误差；ψe表示艏向角误差；k为常数；k1表示可调参数；f为慢时变信号，为f的估计值。
[0102]
(1.2)俯仰角速度和艏向角速度控制律设设计
[0103]
(1.2.1)设计auv轨迹跟踪制导律，引入los曲线路径跟踪法，引入如下导引律为：
[0104][0105]
式中，ye表示auv路径跟踪误差在单位向量上n的投影，ze表示auv路径跟踪误差在单位向量上b的投影；δ
θ
表示纵倾角变化的常数，δ
ψ
表示艏向角的变化的常数；
[0106]
(1.2.2)结合实际纵倾角和艏向角；获得更新后期望纵倾角
ed
和期望艏向角ψ
ed
为：
[0107][0108]
式中，θf为auv实际纵倾角，ψf为auv实际艏向角。
[0109]
(1.2.3)将艏向角和纵倾角ψ作为虚拟控制量输入，设计俯仰角速度q
ed
和艏向角速度控制律r
ed
为：
[0110][0111]
式中，k2和k3为正定常数，表示俯仰角，ψ表示艏向角。
[0112]
四、设计动力学控制器
[0113]
由于在水下轨迹跟踪时横滚角及横滚角速度对auv的影响效果不大可以忽略不计，简化为六自由度数学模型，六自由度动力学方程可简化为：
[0114][0115]fu
＝m
22
vr-m
33
wq-xuu-x
μμ∣
u|u|
[0116][0117]fw
＝m
11
uq-m
22
vp-zww-z
w|w|
w|w|
[0118][0119]fr
＝(m
11-m
22
)uv+(m
44-m
55
)pq-nrr-n
r|r|
r|r|
[0120]
(1)采用动态面的思想简化控制器，来提高跟踪响应效率和高质量的控制输入，将运动学控制器输出通过一阶滤波器进行滤波。构造的数学表达式为：
[0121][0122]
其中，其中，和为一阶低通滤波器所求得值，kq和kr为滤波器时间常数；
[0123]
(2)引入二阶非线性微分跟踪器
[0124][0125]
式中，ud为被跟踪信号，即为auv期望的纵向速度期望输入值，r为跟踪参数，v1和v2为二阶微分跟踪器的输出信号。
[0126]
根据二阶非线性微分跟踪器的输出定义误差跟踪速度为：
[0127]
[ue,qe,re]
t
＝[u-v1,q-q
ed
,r-r
ed
]
t
[0128]
式中，ue表示镇定纵向速度误差，qe表示俯仰控制力，re表示艏向控制力；u表示纵向线速度，q表示俯仰角速度，r表示艏向角速度；
[0129]
(3)设计纵向线速度s1、俯仰角速度s2、艏向角速度s3的误差积分滑模面，表示为：
[0130][0131]
其中，λi(i＝u,q,r)为控制器相关的参数；对误差积分滑模面进行求导；
[0132][0133]
(4)由于auv本身存在参数摄动项和外界洋流的干扰下，等效控制的控制效果并不能收敛逼近，故引入指数趋近律进行补偿，有效抑制滑模带来的抖振问题，使其auv在大角度转弯时会产生的连续输出信号，进而提高auv的运动效果，进一步提高了系统的鲁棒性。选取如下趋近律：
[0134][0135]
式中，ε为滑模控制切换增益,l为指数趋近律，且ε》0,l》0。
[0136]
(5)采用径向基神经网络对auv中未知的不确定项进行估计逼近；
[0137]
(5.1)神经网络的函数为：
[0138][0139]
式中，为神经网络的权值，是最优权值的估计值；hi为神经网络的输入向量；i取u、q、r。
[0140]
(5.2)选取李雅普诺夫函数为：
[0141][0142]
其中为待设计参数，为纵向速度跟踪误差，为径向基神经网络不确定项估值误差；求导可得：
[0143][0144]
式中，εu为纵向推力神经网络逼近误差。
[0145]
则根据可设计神经网络自适应律为：
[0146]
[0147]
式中，k4大于零。
[0148]
参考纵向控制控制律可推出俯仰动力学控制器和艏向动力学控制器，选取李雅普诺夫函数为：
[0149][0150]
求导的
[0151][0152]
式中，εq为俯仰推力神经网络逼近误差，εr为艏向推力神经网络逼近误差；
[0153]
恒大于零，由上式可设计神经网络自适应律为：
[0154][0155][0156]
式中，k5、k6大于零。
[0157]
(6)获得auv的动力学控制律为：
[0158][0159]
式中，τu表示纵向动力学控制律，τq表示俯仰角动力学控制律，τr表示艏向动力学控制律；fu表示外界水流扰动产生的纵向力矩，fq表示外界水流扰动产生的俯仰角力矩，fr表示外界水流扰动产生的艏向力矩；m
ii
(i＝1,2,3,4,5)表示为auv的惯性水动力为水下机器人在加速过程中受到周围水流惯性产生的力；为q
ed
通过一阶滤波器获得的值，为r
ed
通过一阶滤波器获得的值；s1为镇定纵向速度的误差积分滑模面，s2为俯仰角速度的误差积分滑模面，s3为艏向角速度的误差积分滑模面；均为auv中未知的不确定项通过神经网络得到的逼近值。
[0160]
(7)设计辅助动态系统降低系统饱和对控制性能有较大影响，辅助系统为：
[0161][0162]
式中，χi(i＝u,q,r)是设计的辅助系统状态值；bi》0(i＝u,q,r)表示系数；gi(i＝u,q,r)，gu＝1/m
11
，gq＝1/m
44
，gr＝1/m
55
；δτi为系统饱和值；δτi＝τ
i-sat(τi)(i＝u,q,r)；li(i＝u,q,r)是正定常数；
[0163]
实际最终动力学控制器输出可设计为：
[0164][0165]
式中，a1、a2、a3为常数。
[0166]
(8)最终获得auv的动力学控制律τu、τq、τr反馈至auv设备，循环步骤二至步骤四直至轨迹误差为0。
[0167]
此外，本发明采用一种auv轨迹跟踪控制器系统，包括：
[0168]
模型构建模块，用于构建全驱动自主水下机器人的运动学数学模型、动力学数学模型；
[0169]
auv轨迹误差获取模块，用于针对运动学数学模型采集相关的auv设备信息，包括auv设备的纵向线速度、横向线速度、垂向线速度、俯仰角速度、艏向角速度；进行自适应的虚拟向导设计，获得auv轨迹误差方程；；
[0170]
运动学控制器设计模块，用于设计运动学控制器，具体包括：根据auv轨迹误差获得期望纵倾角θ
ed
和期望艏向角ψ
ed
；将艏向角θ和纵倾角ψ作为虚拟控制量输入，设计俯仰角速度q
ed
和艏向角速度控制律r
ed
为：
[0171][0172]
式中，k2和k3为正定常数，θ表示俯仰角，ψ表示艏向角；
[0173]
动力学控制器设计模块，用于设计动力学控制器，具体包括：将运动学控制器输出通过一阶滤波器进行滤波，引入二阶非线性微分跟踪器并根据二阶非线性微分跟踪器的输出定义误差跟踪速度为：
[0174]
[ue,qe,re]
t
＝[u-v1,q-q
ed
,r-r
ed
]
t
[0175]
式中，ue表示镇定纵向速度误差，qe表示俯仰控制力，re表示艏向控制力；u表示纵向线速度，q表示俯仰角速度，r表示艏向角速度；v1和v2为二阶微分跟踪器的输出信号；设计纵向线速度、俯仰角速度、艏向角速度的误差积分滑模面，并对误差积分滑模面进行求导；引入指数趋近律误差积分滑模面进行补偿并采用径向基神经网络对auv中未知的不确定项进行估计逼近，最终获得auv的动力学控制律，通过auv的动力学控制律将当前auv设备的纵向线速度、俯仰角速度、艏向角速度反馈至auv设备；
[0176]
循环操作模块，用于循环操作auv轨迹误差获取模块、动力学控制器设计模块、动力学控制器设计模块直至auv轨迹误差获取模块中获取的轨迹误差为0。
[0177]
此外，本发明提供一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述方法的步骤。此外，本发明提供一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现上述方法的步骤。

技术特征：
1.一种auv轨迹跟踪控制器设计方法，其特征在于，包括以下步骤：(1)构建全驱动自主水下机器人的运动学数学模型、动力学数学模型；(2)针对运动学数学模型采集相关的auv设备信息，包括auv设备的纵向线速度、横向线速度、垂向线速度、俯仰角速度、艏向角速度；进行自适应的虚拟向导设计，获得auv轨迹误差方程；(3)设计运动学控制器，具体包括：根据auv轨迹误差获得期望纵倾角θ
ed
和期望艏向角ψ
ed
；将艏向角θ和纵倾角ψ作为虚拟控制量输入，设计俯仰角速度q
ed
和艏向角速度控制律r
ed
为：式中，k2和k3为正定常数，θ表示俯仰角，ψ表示艏向角；(4)设计动力学控制器，具体包括：(4.1)将运动学控制器输出通过一阶滤波器进行滤波，引入二阶非线性微分跟踪器并根据二阶非线性微分跟踪器的输出定义误差跟踪速度为：[u
e
，q
e
，r
e
]
t
＝[u-v1，q-q
ed
，r-r
ed
]
t
式中，u
e
表示镇定纵向速度误差，q
e
表示俯仰控制力，r
e
表示艏向控制力；u表示纵向线速度，q表示俯仰角速度，r表示艏向角速度；v1和v2为二阶微分跟踪器的输出信号；(4.2)设计纵向线速度、俯仰角速度、艏向角速度的误差积分滑模面，并对误差积分滑模面进行求导；引入指数趋近律对误差积分滑模面进行补偿并采用径向基神经网络对auv中未知的不确定项进行估计逼近，最终获得auv的动力学控制律，通过auv的动力学控制律将当前auv设备的纵向线速度、俯仰角速度、艏向角速度反馈至auv设备，循环步骤(2)至步骤(4)直至auv轨迹误差为0。2.根据权利要求1所述的auv轨迹跟踪控制器设计方法，其特征在于，步骤(4.2)中auv的动力学控制律为：式中，τ
u
表示纵向动力学控制律，τ
q
表示俯仰角动力学控制律，τ
r
表示艏向动力学控制律；f
u
表示外界水流扰动产生的纵向力矩，f
q
表示外界水流扰动产生的俯仰角力矩，f
r
表示外界水流扰动产生的艏向力矩；m
ii
(1，2，3，4，5)表示为auv的惯性水动力为水下机器人在加速过程中受到周围水流惯性产生的力；为q
ed
通过一阶滤波器获得的值，为r
ed
通过一阶滤波器获得的值；s1为镇定纵向速度的误差积分滑模面，s2为俯仰角速度的误差积分滑模面，s3为艏向角速度的误差积分滑模面；均为auv中未知的不确定项通过神经网络得到的逼近值；f
u
＝m
22
vr-m
33
wq-x
u
u-x
μμ|
u|u|
f
w
＝m
11
uq-m
22
vp-z
w
w-z
w|w|
w|w|f
r
＝(m
11-m
22
)uv+(m
44-m
55
)pq-n
r
r-n
r|r|
r|r|式中，u表示纵向线速度、v表示横向线速度、w表示垂向线速度、q表示俯仰角速度、r表示艏向角速度；x
u|u|
，y
v|v|
，z
w|w|
，m
q|q|
，n
r|r|
为流体黏性引起的非线性阻尼项；向量为重心与浮心之间的距离。3.根据权利要求2所述的auv轨迹跟踪控制器设计方法，其特征在于，还包括步骤(4.3)：设计辅助动态系统的，辅助系统为：式中，χ
i
(i＝u，q，r)是设计的辅助系统状态值；b
i
＞0(i＝u，q，r)表示系数；g
i
(i＝u，q，r)，g
u
＝1/m
11
，g
q
＝1/m
44
，g
r
＝1/m
55
；δτ
i
为系统饱和值；δτ
i
＝τ
i-sat(τ
i
)(i＝u，q，r)；l
i
(i＝u，q，r)是正定常数；s
i
(i＝u，q，r)为积分滑膜；实际动力学控制器输出为：式中，a1、a2、a3为常数，l1＝l
u
，l2＝l
q
，l3＝l
r
。4.根据权利要求1所述的auv轨迹跟踪控制器设计方法，其特征在于，步骤(3)还包括设计虚拟向导和自适应控制律：通过将虚拟向导的运动速度作为机器人载体坐标系中x轴的输入并赋值于f，定义选取关于f
x
的李雅普诺夫函数v1为：对v1求导得到：根据李雅普诺夫稳定理论获得虚拟向导和自适应控制律为：其中，x
e
表示auv路径跟踪误差在单位向量上t的投影；θ
e
表示纵倾角误差；ψ
e
表示艏向角误差；k为常数；k1表示可调参数；f为慢时变信号，为f的估计值。5.根据权利要求1所述的auv轨迹跟踪控制器设计方法，其特征在于，步骤(3)中根据auv轨迹误差获得期望纵倾角θ
ed
和期望艏向角ψ
ed
包括：
(3.1)根据auv路径跟踪误差获取更新前的期望纵倾角θ
l
、期望艏向角ψ
l
：式中，y
e
表示auv路径跟踪误差在单位向量上n的投影，z
e
表示auv路径跟踪误差在单位向量上b的投影；δ
θ
表示纵倾角变化的常数，δ
ψ
表示艏向角的变化的常数；(3.2)结合实际纵倾角和艏向角；获得更新后期望纵倾角θ
ed
和期望艏向角ψ
ed
为：式中，θ
f
为auv实际纵倾角，ψ
f
为auv实际艏向角。6.根据权利要求1所述的基于神经网络的auv轨迹跟踪控制器设计方法，其特征在于，步骤(4.2)中所述的趋近律为：式中，ε为滑模控制切换增益，l为指数趋近律，且ε＞0，l＞0。7.根据权利要求1所述的基于神经网络的auv轨迹跟踪控制器设计方法，其特征在于，步骤(4.2)中神经网络的函数为：式中，为神经网络的权值；h
i
为神经网络的输入向量。8.一种auv轨迹跟踪控制器系统，其特征在于，包括：模型构建模块，用于构建全驱动自主水下机器人的运动学数学模型、动力学数学模型；auv轨迹误差获取模块，用于针对运动学数学模型采集相关的auv设备信息，包括auv设备的纵向线速度、横向线速度、垂向线速度、俯仰角速度、艏向角速度；进行自适应的虚拟向导设计，获得auv轨迹误差方程；运动学控制器设计模块，用于设计运动学控制器，具体包括：根据auv轨迹误差获得期望纵倾角θ
ed
和期望艏向角ψ
ed
；将艏向角θ和纵倾角ψ作为虚拟控制量输入，设计俯仰角速度q
ed
和艏向角速度控制律r
ed
为：式中，k2和k3为正定常数，θ表示俯仰角，ψ表示艏向角；动力学控制器设计模块，用于设计动力学控制器，具体包括：将运动学控制器输出通过一阶滤波器进行滤波，引入二阶非线性微分跟踪器并根据二阶非线性微分跟踪器的输出定义误差跟踪速度为：[u
e
，q
e
，r
e
]
t
＝[u-v1，q-q
ed
，r-r
ed
]
t
式中，u
e
表示镇定纵向速度误差，q
e
表示俯仰控制力，r
e
表示艏向控制力；u表示纵向线速度，q表示俯仰角速度，r表示艏向角速度；v1和v2为二阶微分跟踪器的输出信号；设计纵向线速度、俯仰角速度、艏向角速度的误差积分滑模面，并对误差积分滑模面进行求导；引入指数趋近律误差积分滑模面进行补偿并采用径向基神经网络对auv中未知的不确定项进行估计逼近，最终获得auv的动力学控制律，通过auv的动力学控制律将当前auv设备的纵向线
速度、俯仰角速度、艏向角速度反馈至auv设备；循环操作模块，用于循环操作auv轨迹误差获取模块、动力学控制器设计模块、动力学控制器设计模块直至auv轨迹误差获取模块中获取的轨迹误差为0。9.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1至权利要求7所述方法的步骤。10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1至权利要求7所述的方法的步骤。

技术总结
本发明公开了一种AUV轨迹跟踪控制器设计方法与系统，包括步骤：(1)构建全驱动自主水下机器人的运动学数学模型、动力学数学模型；(2)采集AUV设备信息，针对运动学数学模型进行自适应的虚拟向导设计，获得AUV轨迹误差方程；(3)设计运动学控制器；(4)设计动力学控制器。本发明能够消除自主水下机器人在拐点控制量输入过大，容易超调的问题并且消除内部扰动和输入饱和对控制性能的影响。输入饱和对控制性能的影响。输入饱和对控制性能的影响。


技术研发人员：戴晓强 马洪潮 丁建军 曾庆军
受保护的技术使用者：江苏科技大学
技术研发日：2022.02.09
技术公布日：2022/5/25