环境干扰下的水下无人航行器航迹跟踪控制优化方法
【技术领域】
[0001] 本发明设及水下无人航行器航迹跟踪控制优化方法。
【背景技术】
[0002] 无人水下航行器扣UV)近年来成为各国研究的热口话题，被应用于水下作业的各 种领域。无论是海洋军事领域还是日常海下应用作业，都离不开对于UUV的应用与控制，传 统UUV航迹控制采用经典PID控制，经典控制理论中的PID控制是历史最久，应用最广，适 应性最强的一种控制方法。是比例积分微分控制的简称，其原理就是对于系统误差的控制。 该里所说的"误差"就是期望值与输出值的差值。将误差值作为PID控制器的输入，PID控 制器表示为击P*误差系数+Ki*误差积分+Kd*误差微分。通过该个式子可W看出只要误差 存在，PID控制器就会对误差进行调整，直至误差变为零。PID控制方法具有W下优点：PID 控制结构简单、工作可靠、稳定性好、调整方便，因而成为实际控制应用的主要控制技术之 一。但是在实际的系统运行中，UUV航行速度不可测，即未考虑UUV航行速度对UUV航迹跟 踪控制的影响，导致UUV航迹跟踪控制的准确率低。
【发明内容】
[0003] 本发明的目的是为了解决现有UUV航迹跟踪控制的准确率低的问题，而提出了环 境干扰下的水下无人航行器航迹跟踪控制优化方法。
[0004] 上述的发明目的是通过W下技术方案实现的：
[0005] 步骤一、建立UUV水平面数学模型；
[0006] 步骤二、在UUV水平面数学模型的基础上对无人航行器进行PID控制，实现UUV航 迹跟踪控制；
[0007] 步骤S、在UUV航迹跟踪控制的基础上对PID控制进行改进，确定非线性控制律及 观测器。
[000引发明效果
[0009] 采用本发明的环境干扰下的水下无人航行器航迹跟踪控制优化方法，
[0010] 根据UUV航行速度不可测，设计非线性控制律一观测器，本发明观测器采用了 Luenberger型观测器，利用反馈的偏差来消除偏差，将Luenberger型观测器应用到控制系 统中非线性部分，针对控制系统中的线性部分设计Luenberger型观测器的增益矩阵，并选 择充分大的增益来补偿系统的非线性部分，只依赖UUV的腊向对UUV进行定点跟踪控制， 观测器对非线性控制系统提出UUV速度的估计值，使UUV的腊向能够逐渐趋近于期望的腊 向，并最终达到误差为零，得到UUV航行速度，非线性控制律一Luenberger型观测器的误 差动态方程包观测器增益、刚体科氏矩阵估计值、水动力阻巧矩阵估计值、未知干扰量估计 值，R( 为旋转矩阵、未知干扰量估计值一阶导数和UUV航行速度，考虑了UUV航行速度 对UUV航迹跟踪控制的影响，提高了UUV的航迹跟踪控制的准确率，使UUV的航迹跟踪控制 的准确率提高了 20-85%。
【附图说明】
[0011] 图1为本发明流程图；
[001引 图2a为PID控制下的UUV跟踪误差图，-----为PID控制下的期望轨迹与实际航 行轨迹误差，横坐标仿真时间单位为秒（S)，纵坐标为米（m);
[0013] 图化为PID控制下的UUV轨迹跟踪图，-----为实际航迹，一一为期望航迹，横 坐标Y为（m)，纵坐标X为米（m);
[0014] 图3a为改进算法后的UUV跟踪误差图，一一为非线性输出反馈控制下的期望轨 迹与实际航行轨迹误差，横坐标为时间，时间单位为秒（S)，纵坐标为行迹误差，纵坐标为航 迹误差，航迹误差单位为rad;
[0015] 图3b为改进算法后的UUV轨迹跟踪图，一一为期望航迹，--为实际航迹，横 坐标Y为（m)，纵坐标X为米（m)。
【具体实施方式】
【具体实施方式】 [0016] 一；结合图1说明本实施方式，环境干扰下的水下无人航行器航迹 跟踪控制优化方法具体是按照W下步骤进行的：
[0017] 步骤一、建立UUV水平面数学模型；
[001引步骤二、在UUV水平面数学模型的基础上对无人航行器进行PID控制，实现UUV航 迹跟踪控制；
[0019] 步骤S、在UUV航迹跟踪控制的基础上对PID控制进行改进，确定非线性控制律及 观测器。
【具体实施方式】 [0020] 二；本实施方式与一不同的是；所述步骤一中建立 UUV水平面数学模型；具体过程为：
[0021] (1)运动学方程的建立：
[0022] 设UUV在大地坐标系下的重屯、位姿矩阵为n= [X，y，iD]t，大地坐标系下的角速 度矩阵为'々二[丈兵，载体坐标系下角速度矩阵为V= [U，V，r]T;
[0023] 根据大地坐标系与载体坐标系之间的转换关系，得出运动学方程：
[0024]
[00巧]用向量形式表示为：
[002引式中，n为UUV的位置矩阵，X为X轴重屯、的位姿，y为y轴重屯、的位姿，1])为UUV 航行的实时腊向，？)为大地坐标系下角速度矩阵，i：为X轴角速度，j为y轴角速度，5/>为1]) 轴角速度，V为载体坐标系下角速度矩阵，U为腊摇角速度，V为纵向角速度，r为纵摇角速 度，T为矩阵转置符号；
[0029] (2)动力学方程的建立；
[0030] UUV刚体受力与力矩满足方程；
[0031]
[003引其中，Mce为刚体的质量矩阵，V为载体坐标系下角速度矩阵，F为载体坐标系下UUV的角速度矩阵的一阶导数，Ceb(V)为刚体的科氏矩阵，Th为总的水动力、水动力矩和执 行机构所产生的力与力矩；
[003引式中，为UUV质量，X(；为UUV重屯、距X轴距离，:r为UUV纵摇角速度，CEC为刚体 科氏参数，E为 1、2、3、4、5、6、7、8 或 9，G为 1、2、3、4、5、6、7、8 或 9 ;
[0039]
[0040] 其中，Th为总的水动力、水动力矩和执行机构所产生的力与力矩，Ma为附加质量 的惯性矩阵，D(V)为水动力阻巧矩阵，Ca(V)为水动力科氏矩阵，g(0)为浮力重力矩阵，T 为执行机构的控制力矩阵；
[00川其中，
[0042]
[0043]
[0044] 要建立精确的UUV水动力，阻力和阻力矩矩阵是一项非常有难度的任务，特别是 速度范围变化比较大的UUV，找到符合任意速度的水动力系数是很难W实现的。而且，当 UUV的纵向速度比较大时，水动力的非线性特征就会表现得十分明显。同时，UUV各个自由 度之间的禪合性也会增强。本文所使用的水动力系数是由水池实验和经验公式得到的，当 UUV速度范围在2-4m/s时，能够确保其正确性。
[0045] 结合上面的论证与简化，对于本文的研究对象，水动力阻巧表达式可表示如下：
[0049] 式中，义^为抓乂纵向角速度在X轴下的惯性参数，为UUV横向角速度在y轴下 的惯性参数，K为UUV腊摇角速度在y轴下的惯性参数，W为UUV横向角速度在北向轴下 的惯性参数，M为UUV腊摇角速度在北向轴下的惯性参数，aQ为水动力科氏参数，Q为1或 2,其中，《2 = }>'+K:'";Di为线性阻巧矩阵，D"i(V)为非线性阻巧矩阵，Du为随着 纵向速度改变而改变的阻巧矩阵，Tu为纵向推力，T。为纵倾力矩，Tt为转腊力矩；
[0050]其中，
[0054] 式中，Xu为UUV纵向角速度在X轴下的线性阻巧参数，Yy为UUV横向角速度在y轴 下的线性阻巧参数，Yf为UUV腊摇角速度在y轴下的线性阻巧参数，Ny为UUV横向角速度在 北向轴下的线性阻巧参数，Nf为UUV腊摇角速度在北向轴下的线性阻巧参数，YW为UUV横 向角速度在y轴下的非线性阻巧参数，Yut为UUV腊摇角速度在y轴下的非线性阻巧参数，Nw为UUV横向角速度在北向轴下的非线性阻巧参数，Nut为UUV腊摇角速度在北向轴下的 非线性阻巧参数，X|u|u为UUV纵向角速度在X轴D。1(V)矩阵内的参数，Y|y|y为UUV横向角速 度在y轴D"i(V)矩阵内的参数，Y|y|t为UUV腊摇角速度在y轴D"i(V)矩阵内的参数，N|t|y为 UUV横向角速度在北向轴D"i(V)矩阵内的参数，％I为UUV腊摇角速度在北向轴D"i(V)矩 阵内的参数，
[0055] m = 40，W，''=2.2，Nv=36，Yuv=〇，^=-1.42，X"r=〇.l，、l=S2，F"r="X,;， 1；二-38.4, 1；=1〇,而，=200,W",二"巧-式），I；二-2.5,Yr=5，Iz=8.0, ％=巧， Nr=5, W,=-8.9，N|r|r= 15;
[0056] 向量的形式表示为：