[0057] T=[Tu TJT
[0058]其中，T为执行机构的控制力矩阵，Tu为纵向推力，T。为纵倾力矩，Tt为转腊 力矩；
[0059] 得出UUV水平面数学模型，其微分方程组的形式如下式：
[0064] 式中，A为质量参数，B为附加质量参数，mi。为UUV质量，I为0、1、2、3、4、5、6、7、 8 或 9，Q为 0、1、2、3、4、5、6、7、8 或 9，dE。为水动力阻巧参数，E为 1、2、3、4、5、6、7、8 或 9，G 为 1、2、3、4、5、6、7、8 或 9,Cec为刚体科氏参数，E为 1、2、3、4、5、6、7、8 或 9,G为 1、2、3、4、 5、6、7、8 或 9 ;
[0065] 其它步骤及参数与【具体实施方式】一相同。
【具体实施方式】 [0066] 本实施方式与一或二不同的是：所述所述步骤二 中在UUV水平面数学模型的基础上对无人航行器进行PID控制，实现UUV航迹跟踪控制；具 体过程为：
[0067] PID控制算法；
[0068] it
[006引式中，1^为实时期望腊向，是通过编程人为设定的期望值，为抓￥航行时的实 际腊向值，为腊向误差；
[0070] UUV腊向的PID控制规律为；I// =A'" .I//, + 占,.JI//,. +V1/少
[0071] 式中，kp为比例增益，ki为积分增益，kd为微分增益，5/>为的一阶导数，均是通 过调节时人为给定的参数。
[0072] 其它步骤及参数与【具体实施方式】一或二相同。
【具体实施方式】 [0073] 四：本实施方式与一、二或=不同的是：所述步骤= 中在UUV航迹跟踪控制的基础上对PID控制进行改进，设计非线性控制律及观测器；具体过 程为：
[0074] 非线性控制律的确定；
[00巧]定义误差变量：
[0076]
[0077] 式中的Zi为腊向角的误差，e为腊向角误差的积分变量，a1为镇定函数，r为角 速度；Z,为镇定腊摇角差；
[0078] UUV航迹跟踪控制的误差动态方程为：
[0079]
[0080] 式中，"^^为e的一阶导数，知为21的一阶导数，！2为22的一阶导数，/^为!"的一阶 导数，〇1为镇定函数，加为〇1的一阶导数，^/>为iD的一阶导数；
[0081] 选取镇定函数：
[0082] 曰 1=-kEe-kziZi
[0083] k。为腊相角积分误差设计参数ku为腊相角设计参数；
[0084] 可知，
[0085] di二ke台一hih
[0086] 得到了非线性控制律的表达式为：
[0087] IV二一W++M^ch-Zl-后2Z2
[008引式中，kz2为设计参数，W为未知的扰动量的估计值，Ma为附加质量的惯性矩阵，D 为线性阻巧矩阵，Tr为转腊力矩；
[008引观测器的确定；
[0090] 设UUV位置与腊向角的估计值为=巧J'，^)，（为V的估计值；
[0091]其中，7为UUV位置与腊向角的估计值，去为估计的X轴位姿，少为估计的y轴位 姿，?^为估计的UUV航行的实时腊向；
[0092] 则定义UUV的速度与位置观测误差为：
[0093]
[0094] 式中，巧为位置观测误差，V为速度观测误差，n为UUV的位置矩阵；
[0095] 根据UUV跟踪控制系统设计Luenberger型观测器；
[0096] 所述，Luenberger型观测器为隆伯格型观测器；
[0097]
[009引式中，RU)为旋转矩阵，Cw(V)为刚体的科氏矩阵，D(V)为水动力阻巧矩阵，Ca(V)为水动力科氏矩阵，^为时间常数，W为未知扰动，X为纵向速度的旋转矩阵， Fw vUUV纵向角速度，T。为水动力矩，W为未知扰动量，W为扰动量一阶导数；
[0099] 可W得到观测器的误差动态方程：
[0100]
A
[0101] 式中，Ki为观测器增益，K2为一是时间常数，义为纵向速度的旋转矩阵估计值，i) fw 为纵向速度估计值，Gw(巧:为刚体科氏矩阵估计值，。(巧为水动力阻巧矩阵估计值，诗，为 未知干扰量估计值，R(i]〇为旋转矩阵，占为未知干扰量估计值一阶导数，f为纵向速度的 旋转矩阵误差为扰动波长，r，为扰动角。
[0102] 其它步骤及参数与【具体实施方式】一、二或=相同。
[0103]采用W下实施例验证本发明的有益效果：
[0104] 实施例1
[0105] 环境干扰下的水下无人航行器航迹跟踪控制优化方法具体是按照W下步骤进行 的：
[0106] 步骤一、UUV模型的建立；
[0107] 步骤二、在UUV模型的基础上进行PID控制实现UUV航迹跟踪控制；
[010引 PID控制算法；
[010引 i]5e= 广1]5
[0110] 式中，1^为实时期望腊向，是通过编程人为设定的期望值，为UUV航行时的实 际腊向值，1]^6为腊向误差；
[01川UUV腊向的PID控制规律为淨=、?於+&,.
[0112] 式中，kp为比例增益，ki为积分增益，kd为微分增益，？/>为1]^的一阶导数，均是通 过调节时人为给定的参数；kp= 8,k1= 0. 5,kd= 25 ;
[0113] 步骤=、在UUV航迹跟踪控制的基础上对PID控制进行改进，即带观测器的非线性 输出反馈控制；
[0114] 非线性控制律的确定：
[0115] 定义误差变量：
[0116]
[0117] 式中的zi为腊向角的误差，e为腊向角误差的积分变量，a1为镇定函数，r为角 速度；Z,为镇定腊摇角差；
[0118] UUV航迹跟踪控制的误差动态方程为：
[0119]
[0120] 式中，心为e的一阶导数，A为Zi的一阶导数，扣为Z2的一阶导数，r为r的一阶 导数，a1为镇定函数，化为a1的一阶导数，^为iD的一阶导数；
[0121] 选取镇定函数：
[0122] a1=-kee-kziZi
[0123] ke为腊相角积分误差设计参数，k,i为腊相角设计参数；取k。= 3.45，k,i= 6.8， Ki= 10,K2= 0. 56 ；
[0124] 可知，
[0125]
[0126] 得到了非线性控制律的表达式为：
[0127]
[012引式中，k,2为设计参数，布为未知的扰动量的估计值，Ma为附加质量的惯性矩阵，D 为线性阻巧矩阵，Tr为转腊力矩；
[0129] 观测器的确定；
[0130] 设UUV位置与腊向角的估计值为I々=托知八，V为V的估计值；
[0131]其中，巧;为UUV位置与腊向角的估计值，X为估计的X轴位姿，少为估计的y轴位 姿，|/>为估计的UUV航行的实时腊向；
[0132] 则定义UUV的速度与位置观测误差为：
[0133]
[0134] 式中，巧为位置观测误差，P为速度观测误差，n为UUV的位置矩阵；
[01巧]根据UUV跟踪控制系统设计Luenberger型观测器；
[0136] 所述，Luenberger型观测器为隆伯格型观测器；
[0137]
[013引式中，RU)为旋转矩阵，Cw(V)为刚体的科氏矩阵，D(V)为水动力阻巧矩阵，Ca(V)为水动力科氏矩阵，^为时间常数，W为未知扰动，乂为纵向速度的旋转矩阵， Tw VUUV纵向角速度，T。为水动力矩，W为未知扰动量，W为扰动量一阶导数；
[0139] 可W得到观测器的误差动态方程：
[0140]
[01川式中，Ki为观测器增益，K2为一是时间常数，Ki= 10,K2= 0. 56，义为纵向速度 !Tw 的旋转矩阵估计值，f为纵向速度估计值，CM巧为刚体科氏矩阵估计值，。快)为水动力阻 巧矩阵估计值，w;为未知干扰量估计值，R(i]〇为旋转矩阵，为未知干扰量估计值一阶导 数，i.为纵向速度的旋转矩阵误差，A为扰动波长，r，为扰动角；
[0142] 得到【附图说明】中图2 (a)、图2化）、图3 (a)和图3化）的仿真结果，从仿真结果可W 看出带观测器的非线性控制规律同样可W达到PID的控制效果，即UUV成功跟踪上一条我 所设定的直线轨迹，并达到误差为零