本发明提供一种基于惩罚式小波网络的无人潜航器位姿控制装置及方法，通过集成惩罚式小波网络控制参数整定系统生成新的控制器参数，并通过控制器得到控制信息，将其送入动力推进系统进行指令分配，产生纵向推力、横向推力以及力矩，实现UUV的位姿控制。本发明首次采用野值剔除与卡尔曼滤波平滑相结合的方式实现传感器量测量的优化，并且首次提出在控制器的前端加入了集成惩罚式小波网络的控制参数整定系统，结合了小波分析和神经网络的优点，而且引入惩罚项优化网络泛化能力，解决小波网络泛化性能不强的问题，从而能够自适应生成合理的控制器参数，实现无人潜航器的位姿控制。本发明对今后无人潜航器水下作业、运动控制等领域的发展有着积极意义。
1.一种基于惩罚式小波网络的无人潜航器位姿控制方法，其特征在于：装置由信息采集单元、数据处理单元、海图信息系统、控制参数整定系统、控制器以及动力推进系统组成，信息采集单元包括位置参考系统和艏向传感器，用于采集位置与航向信息并转化为北东坐标系信息与艏向角信息；数据处理单元包括野值消除系统和无迹卡尔曼滤波器，用于剔除传感器采集过程中的不确定干扰并进行滤波平滑；海图信息系统接收数据处理单元输出的北东坐标系信息与航向信息，并解算生成期望位置与航向信息；控制参数整定系统内部采用惩罚式小波网络方法，基于控制误差信息训练和更新网络权值，进而生成一组新的控制器参数；控制器由PID控制器组成，并接收控制参数整定系统的输出参数，生成在线控制量并计算出保持UUV位置所需的作用力；动力推进系统接收控制器传来的指令并分配给各推进器，从而产生抵抗干扰力和力矩的纵向，实现UUV的位姿控制；
步骤1：信息采集单元的位置参考系统以定速率和精度采集UUV实际位置信息并转化为北东坐标系信息X、Y；艏向传感器由陀螺罗经构成并利用陀螺特性寻北采集航向信息ψ；
步骤2：数据处理单元的野值消除系统通过野值的辨识方法剔除传感器采集过程中的不确定干扰，并传送至无迹卡尔曼滤波器；无迹卡尔曼滤波器将无野值的位置与航向信息进行滤波平滑，进一步削弱干扰影响；
数据处理单元中的野值消除系统采用切比雪夫多项式回归方法辨识并剔除系统野值，当采样值为yk时，根据切比雪夫多项式回归概念，计算当前时刻k时n维切比雪夫多项式，根据牛顿插值法得到切比雪夫多项式系数Ci(i＝0,1,2,...,n)，递推得到一组多项式回归式为：其中：Ci为多项式系数，Ti(τ)为采样频率，k为采样时刻，X(t)为拟合曲线上对应的数据，根据X(t)和采样值yk求解每个时刻拟合数据的更新率K，并用当前时刻k的X(t)进行回归拟合，计算当前平均斜率 定义野值斜率辨识范围是 并判断K是否在的区间内，α的取值在区间[2,4]范围内：当 时，采样值yk不是
野值，无需消除；当 时，则认为此时的采样值yk为野值，并根据上述计算估计值将yk替换为 以达到消除野值的目的；
所述卡尔曼滤波器用于平滑无野值的位置与航向信息，对采样数据进行处理，根据野值消除系统的输出结果可得到系统状态量和量测量为：Y(k)＝[n(k) e(k) ψ(k)]T＝H(k)X(k)
其中：T为包含时间常数的三维对角矩阵，H(k)为3×3单位矩阵，Γ(k-1)为3×3的系统过程噪声输入矩阵，W(k-1)为3维的系统随机过程噪声序列，n(k)、e(k)和ψ(k)分别为北东坐标系下的位置与航向，u、v和r分别为UUV船体坐标系下的纵向、横向速度与回转速率；Γ(k,k-1)为过程噪声输入矩阵，X(k)和Y(k)分别表示系统的状态量和量测量；
步骤3：海图信息系统接收数据处理单元输出的北东坐标系信息与航向信息，并将其与海图信息进行计算，产生北东坐标系的动态北向期望信息Xd，东向期望信息Yd以及航向角期望信息ψd；
步骤4：控制参数整定系统集成惩罚式小波网络方法的误差输入、训练与更新以及参数输出过程，控制参数整定系统将接收的期望信息与实际信息比较并生成误差信息，并根据误差信息训练和更新网络权值，进而生成一组新的控制器参数；
步骤5：控制系统接收控制参数整定系统的输出参数，通过控制器的积分项补偿海流的作用，并将控制信息送入动力推进系统进行指令分配，从而产生纵向推力、横向推力以及力矩，实现UUV的位姿控制。
2.根据权利要求1所述的一种基于惩罚式小波网络的无人潜航器位姿控制方法，其特征在于：步骤4中采用优化后的误差函数：其中： 为平方误差函数，λ为惩罚系数， 为惩罚项，wi,j为连
接两层网络的权值；w表示网络权值系数；er为误差，
网络采用单隐层结构，时刻n的输出层权值wKJ(n)为：
其中：vJ(n)为J层的诱导局部域， 为隐层函数；
其中：Kw＝wKJ(0)-λsgn(wKJ(n-1))，vJ＝ηJ(n)β(n-1)kJ为输出层诱导局部域，f(·)为sigmoid函数，ηK(n)为自适应学习率，k＝[k(vJ(1),vJ(n)),k(vJ(2),vJ(n)),...,k(vJ(n-
1) ,vJ(n))]T为小波核函数，α(n)＝e(n)f '(ηK(n-1)αT(n-1)kK)，kK、kJ为对应的核函数；
表示在高等数学、机器学习以及优化理论中一个很小的数，xK(n)为每一个网络节点的输出信息，vK(n)为相应层的诱导局部域，核函数k(vJ(p),vJ(n))可根据下式求得：
一种基于惩罚式小波网络的无人潜航器位姿控制方法
技术领域
[0001] 本发明涉及一种基于惩罚式小波网络的无人潜航器位姿控制装置及方法，属于无人水下潜航器位姿控制技术领域，可以用于无人潜航器水下作业、运动控制等领域。
背景技术
[0002] 无人潜航器(UUV)是一种可长期潜入水下，依靠自带能源、自推进、遥控或自主控制，通过配置任务载荷执行作战或作业任务，能回收和反复使用的潜器。传统的UUV主要用于侦察、勘测、搜索、投送和跟踪等，其运动方式为巡航。并通过控制面，即舵来进行运动控制。此类UUV必须以一定速度运动才能保证自身的稳定，因此无法完成定点悬停的动作。然而，随着人类海洋开发与探索的深度、广度不断加强，无人潜航器的研究与发展也不断进步，其功能已经不仅仅局限于在水下低速或高速航行，而且需要具有悬停、旋转及爬潜等更高的机动性能，来完成远程水下通讯中继、水下设施检查等作业任务，因此对UUV提出了精确定位控制的要求，使其能够抵抗环境扰动，保持在水中的位置和姿态，实现位姿控制。
[0003] 目前位姿控制研究方法主要有多变量优化控制、H∞控制、模糊控制等。Balchen等人结合卡尔曼滤波，提出了基于多变量优化的自适应控制方法，实现了基于模型的位姿控制，并成为位姿控制领域里程碑式的成果。Craven等人在前人基础上，考虑了更多不同的线性与非线性控制控制技术。类似的，增益调度控制、模糊控制、鲁棒控制以及多变量抗饱和控制等先进控制方法也逐渐被研究应用在位姿控制研究中。
[0004] 然而实际的UUV系统的全局数学模型具有很大的不确定性，难以用数学模型来描述，，为此近年来更多的学者重新将目光放到PID控制器，利用PID不需要数学模型且调节方式简单的特点，采用不同算法实现对控制器的参数整定。小波神经网络(简称小波网络)是建立在小波分析理论基础上的新型神经网络，结合了小波分析和神经网络的优点，具有很强的学习能力，适合用于在线整定控制器参数。但是，小波网络存在着过分强调克服学习错误而导致泛化性能不强等问题。已有研究表明，在满足训练精度的基础上，采用尽量小的网络规模是优化网络泛化能力的有效途径，为此本发明基于UUV的位姿控制原理，在满足航行器物理性能上的各种约束条件的基础上，通过一种新型小波网络在线整定PID控制器实现UUV 的位姿控制，实现无人潜航器在在更多领域中的工程应用。
发明内容
[0005] 为了在系统模型不完备的条件下，通过推进器推力，使UUV能够自适应地保持位置和航向(固定位置或预定轨迹)，本发明的目的是提出一种基于惩罚式小波网络的UUV位姿控制方法，该方法能够利用惩罚式小波网络方法更好的收敛性与自适应性，基于前序状态训练权值结果，对当前状态进行对控制器进行实时调整的参数整定，从而实现控制器在不同状况下的自适应调节，解决系统模型不完备条件下的控制约束问题。一种基于惩罚式小波网络的无人潜航器位姿控制装置及方法。
[0006] 本发明的目的是这样实现的：一种基于惩罚式小波网络的无人潜航器位姿控制装置，由信息采集单元、数据处理单元、海图信息系统、控制参数整定系统、控制器以及动力推进系统组成，信息采集单元包括位置参考系统和艏向传感器，用于采集位置与航向信息并转化为北东坐标系信息与艏向角信息；数据处理单元包括野值消除系统和无迹卡尔曼滤波器，用于剔除传感器采集过程中的不确定干扰并进行滤波平滑；海图信息系统接收数据处理单元输出的北东坐标系信息与航向信息，并解算生成期望位置与航向信息；控制参数整定系统内部采用惩罚式小波网络方法，基于控制误差信息训练和更新网络权值，进而生成一组新的控制器参数；控制器由PID控制器组成，并接收控制参数整定系统的输出参数，生成在线控制量并计算出保持UUV位置所需的作用力；动力推进系统接收控制器传来的指令并分配给各推进器，从而产生抵抗干扰力和力矩的纵向，实现UUV的位姿控制。
[0007] 本发明还包括这样一些结构特征：
[0008] 1.一种基于惩罚式小波网络的无人潜航器位姿控制方法，包括所述的基于惩罚式小波网络的无人潜航器位姿控制装置，步骤如下：
[0009] 步骤1：信息采集单元的位置参考系统以定速率和精度采集UUV实际位置信息并转化为北东坐标系信息X、Y；艏向传感器由陀螺罗经构成并利用陀螺特性寻北采集航向信息ψ；
[0010] 步骤2：数据处理单元的野值消除系统通过野值的辨识方法剔除传感器采集过程中的不确定干扰，并传送至无迹卡尔曼滤波器；无迹卡尔曼滤波器将无野值的位置与航向信息进行滤波平滑，进一步削弱干扰影响；
[0011] 步骤3：海图信息系统接收数据处理单元输出的北东坐标系信息与航向信息，并将其与海图信息进行计算，产生北东坐标系的动态北向期望信息Xd，东向期望信息Yd以及航向角期望信息ψd；
[0012] 步骤4：控制参数整定系统集成惩罚式小波网络方法的误差输入、训练与更新以及参数输出过程，控制参数整定系统将接收的期望信息与实际信息比较并生成误差信息，并根据误差信息训练和更新网络权值，进而生成一组新的控制器参数；
[0013] 步骤5：控制系统接收控制参数整定系统的输出参数，通过控制器的积分项补偿海流的作用，并将控制信息送入动力推进系统进行指令分配，从而产生纵向推力、横向推力以及力矩，实现UUV的位姿控制。
[0014] 2.步骤2具体为：
[0015] 数据处理单元中的野值消除系统采用切比雪夫多项式回归方法辨识并剔除系统野值，当采样值为yk时，根据切比雪夫多项式回归概念，计算当前时刻k时n维切比雪夫多项式，根据牛顿插值法得到切比雪夫多项式系数Ci(i＝0,1,2,...,n)，递推得到一组多项式回归式为：
[0016]
[0017] 其中：Ci为多项式系数，Ti(τ)为采样频率，k为采样时刻，X(t)为拟合曲线上对应的数据，根据X(t)和采样值yk求解每个时刻拟合数据的更新率K，并用当前时刻k的X(t)进行回归拟合，计算当前平均斜率Kp；定义野值斜率辨识范围是 并判断K是否在的区间内，α的取值在区间[2,4]范围内：当 时，采样值yk不是野值，无需消除；当 时，则认为此时的采样值yk为野值，并根据上述计算估计值
[0018]
[0019] 将yk替换为 以达到消除野值的目的；
[0020] 所述卡尔曼滤波器用于平滑无野值的位置与航向信息，对采样数据进行处理，根据野值消除系统的输出结果可得到系统状态量和量测量为：
[0021]
[0022] Y(k)＝[n(k) e(k) ψ(k)]T＝H(k)X(k)
[0023] 其中：T为包含时间常数的三维对角矩阵，H(k)为3×3单位矩阵，Γ(k-1)为3×3的系统过程噪声输入矩阵，W(k-1)为3维的系统随机过程噪声序列，n(k)、e(k)和ψ(k)分别为北东坐标系下的位置与航向，u、v和r分别为UUV船体坐标系下的纵向、横向速度与回转速率。
[0024] 3.步骤4中采用优化后的误差函数：
[0025]
[0026] 其中： 为平方误差函数，λ为惩罚系数， 为惩罚项，wi,j为连接两层网络的权值；
[0027] 网络采用单隐层结构，时刻n的输出层权值wKJ(n)为：
[0028]
[0029] 网络输出zK(n)为：
[0030]
[0031] 其中：Kw＝wKJ(0)-λsgn(wKJ(n-1))，vJ＝ηJ(n)βT(n-1)kJ为输出层诱导局部域，f(·)为 sigmoid函数，ηK(n)为自适应学习率，k＝[k(vJ(1),vJ(n)),k(vJ(2),vJ(n)),...,k(vJ(n-1),vJ(n))]T为小波核函数，α(n)＝e(n)f'(ηK(n-1)αT(n-1)kK)，ηK(n) 为：
[0032]
[0033] 核函数k(vJ(p),vJ(n))可根据下式求得：
[0034]
[0035] 其中ai为实数。
[0036] 与现有技术相比，本发明的有益效果是：本发明一方面在数据处理单元中采用野值消除系统和卡尔曼滤波器的串行处理方式。首先野值消除系统能够辨识出偏离正常范围的量测值，通过切比雪夫多项式回归方法自适应地检测并求得偏离点的合理量测解，并引入卡尔曼滤波平滑无野值的位置与航向信息，对采样数据进行进一步处理。目前无人潜航器的数据平滑过程多采用最小二乘直线拟合的方法进行滤波处理，而卡尔曼滤波是克服最小二乘直线拟合的苛刻噪声条件而提出的，不要求信号和噪声都是平稳过程，并且采用更直观的时间域语言，更符合实际观测数据的特点。同时由于仅对数据进行平滑处理，同时位姿控制过程通常保持低速行进，采用线性滤波方法即可，因此与其他非线性滤波方法相比，运算量被大大降低，更适合实际观测数据的平滑处理。另一方面，本发明在控制器的前端加入了集成惩罚式小波网络的控制参数整定系统，结合了小波分析和神经网络的优点，而且引入惩罚项优化网络泛化能力，解决了小波网络泛化性能不强的问题，从而能够自适应生成合理的控制器参数，实现无人潜航器的位姿控制。
附图说明
[0037] 图1是无人潜航器位姿控制系统总体结构图。
[0038] 图2是无人潜航器在北东坐标系与船体坐标系中的表示，其中北东坐标系η下的线速度与角速度分别为 与[φ θ ψ]T，船体坐标系ν下的线速度与角速度分别为[u v w]与 [p q r]。
[0039] 图3是本发明的控制系统原理框图。
[0040] 图4是本发明的UUV位姿控制性能仿真。
具体实施方式
[0041] 下面结合附图与具体实施方式对本发明作进一步详细描述。
[0042] 如图1示出本发明的基于惩罚式小波网络的无人潜航器位姿控制装置。包括1无人潜航器，2位置参考系统，3艏向传感器，4信息采集单元，5野值消除系统，6卡尔曼滤波器，7 数据处理单元，8海图信息系统，9控制参数整定系统，10控制系统，11动力推进系统。
[0043] 本发明在于将一种基于惩罚式小波网络PID控制的控位方法应用于无人潜航器的位姿控制方法，使其相对于目标物的位置保持不变，具有能够抵抗环境扰动的位姿控制能力。
[0044] 1.信息采集单元
[0045] 信息采集单元4包括位置参考系统2与艏向传感器3。位置参考系统以一定的速率和精度采集UUV实际位置信息并转化为北东坐标系信息X，Y。艏向传感器由陀螺罗经构成，利用陀螺特性寻北采集航向信息ψ。
[0046] 2.数据处理单元
[0047] 数据处理单元7主要包括野值消除系统5和卡尔曼滤波器6。野值消除系统通过野值的辨识方法剔除传感器采集过程中的不确定干扰，并传送至卡尔曼滤波器。滤波器将无野值的位置与航向信息进行滤波平滑并削弱干扰影响。其中野值消除系统采用切比雪夫多项式回归方法辨识并剔除系统野值，并传送至卡尔曼滤波器。当采样值为yk时，根据切比雪夫多项式回归概念，计算当前时刻k时n维切比雪夫多项式，根据牛顿插值法计算切比雪夫多项式系数 Ci，递推计算出一组多项式回归式：
[0048]
[0049] 其中Ci为多项式系数，Ti(τ)为采样频率，k为采样时刻，X(t)为拟合曲线上对应的数据，根据X(t)和采样值yk求解每个时刻拟合数据的更新率K，并用当前时刻k的X(t)进行回归拟合，计算当前平均斜率Kp。定义野值斜率辨识范围是 并判断K是否在的区间内，α的取值在区间[2,4]范围内：当 时，则认为此时的采样值yk为野值，并根据式(1)计算估计值 将yk替换为 以达到消除野值的目的。
[0050]
[0051] 卡尔曼滤波器用于平滑无野值的位置与航向信息，对采样数据进行进一步处理。根据野值消除系统的输出结果可得到系统状态量和量测量：
[0052]
[0053] Y(k)＝[n(k) e(k) ψ(k)]T＝H(k)X(k) (4)
[0054] 其中T为包含时间常数的三维对角矩阵，H(k)为3×3单位矩阵，Γ(k-1)为3×3的系统过程噪声输入矩阵，W(k-1)为3维的系统随机过程噪声序列，n(k)、e(k)和ψ(k)分别为北东坐标系下的位置与航向，u、v和r分别为UUV船体坐标系下的纵向、横向速度与回转速率。由于仅对数据进行平滑处理，同时位姿控制过程通常保持低速行进，因此只需采用线性系统的卡尔曼滤波方法进行平滑。卡尔曼滤波方法进行平滑处理过程由表1所示：
[0055] 表1卡尔曼平滑处理过程
[0056]
[0057] 3.海图信息系统
[0058] 海图信息系统8接收数据处理单元输出的北东坐标系信息与航向信息，并将其与海图信息进行计算，产生北东坐标系的动态北向期望信息Xd，东向期望信息Yd以及航向角期望信息ψd。
[0059] 4.控制参数整定系统
[0060] 控制参数整定系统9集成惩罚式小波网络方法的误差输入、训练与更新以及参数输出过程，控制参数整定系统将接收的期望信息与实际信息比较并生成误差信息，并根据误差信息训练和更新网络权值，进而生成一组新的控制器参数。其特征在于控制参数整定系统集成惩罚式小波网络方法的误差输入、训练与更新以及参数输出过程。
[0061] 本发明在传统误差函数中引入一衡量网络结构复杂程度的惩罚项，通过间接修剪神经网络结构而提高网络的泛化能力。优化后的误差函数表示为：
[0062]
[0063] 其中 为平方误差函数，λ为惩罚系数， 为惩罚项，wj,k为连接两层网络的权值。网络采用单隐层结构，时刻n的输出层权值wKJ(n)为：
[0064]
[0065] 网络输出zK(n)为：
[0066]
[0067] 其中Kw＝wKJ(0)-λsgn(wKJ(n-1))，vJ＝ηJ(n)βT(n-1)kJ为输出层诱导局部域，f(·)为sigmoid函数，ηK(n)为自适应学习率，k＝[k(vJ(1),vJ(n)),k(vJ(2),vJ(n)),...,k(vJ(n-1),vJ(n))]T为小波核函数，α(n)＝e(n)f'(ηK(n-1)αT(n-1)kK)，ηK(n)的由(8)式表示：
[0068]
[0069] 核函数定义为：
[0070]
[0071] ηK(n)用于隐层与输出层间权值的更新，对于隐层与输入层之间的自适应学习率ηJ(n)来说，由于隐层权值的更新以输出层更新为基础，根据更新规则可知每个输出层的神经元在隐层更新中均有相同的贡献。因此本发明在输出层学习率ηK(n)自适应调节的基础上，对隐层的每个神经元j均采用相同的学习率：
[0072]
[0073] 其中ηj表示隐层第j个神经元的学习率，K为输出层神经元个数。
[0074] 5.控制系统
[0075] 控制系统10由PID控制器组成，并接收控制参数整定系统的输出参数，由于UUV在水下运动，风浪对其影响不会太大，主要通过控制器的积分项补偿海流的作用，并将控制信息送入动力推进系统11进行指令分配，从而产生纵向推力、横向推力以及力矩，实现UUV的位姿控制。
[0076] 由于UUV的位姿控制属于水平面控制，本发明令ν＝[u,v,r]T和η＝[n,e,ψ]T，即忽略垂荡、横摇与纵摇运动相关的动力学特性，因此对于水平运动的运动学方程而言，旋转矩阵可化简为：
[0077]
[0078] 假设UUV质量为均匀分布，若船体坐标系的原点位于UUV的中心线上，同时附加质量的中心与重心一致，存在M＝MT，C(ν)＝-CT(ν)，因此有刚体动力学矩阵：
[0079]
[0080]
[0081] 其中X，Y和N分别为北东方向的力和转艏力矩。对于系统惯性矩阵，纵荡方向的线性阻尼与横荡和艏摇方向解耦，即有：
[0082]
[0083] 由于低速应用中，可认为阻尼呈线性，进而UUV位姿控制的模型可表示为：
[0084]
[0085]
[0086] 其中B为动力推进器配置的控制矩阵，u为控制输入。在控制设计上，本发明的纵向推力、横向推力以及艏向控制力矩均采用增量式PID控制算法，即：
[0087] Δu(n)＝Kp[e(n)-e(n-1)]+Kie(n)+Kd[e(n)-2e(n-1)+e(n-2)] (17)[0088] 其中控制系数Kp，Ki和Kd分别通过惩罚式小波网络参数整定系统中网络的输出得到。
[0089] 以上所述，仅为本发明中的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉该技术的人在本发明所揭露的技术范围内，可理解想到的变换或替换，都应涵盖在本发明的权利要求书的保护范围之内。
[0090] 综上，本发明是一种基于惩罚式小波网络的无人潜航器位姿控制方法及装置。以一定的速率和精度采集并转换UUV实际位置与航向信息，通过剔除采集过程中的不确定干扰与滤波平滑，削弱干扰影响。通过集成惩罚式小波网络控制参数整定系统生成新的控制器参数，并通过控制器得到控制信息，将其送入动力推进系统进行指令分配，产生纵向推力、横向推力以及力矩，实现UUV的位姿控制。本发明首次采用野值剔除与卡尔曼滤波平滑相结合的方式实现传感器量测量的优化，并且首次提出在控制器的前端加入了集成惩罚式小波网络的控制参数整定系统，结合了小波分析和神经网络的优点，而且引入惩罚项优化网络泛化能力，解决了小波网络泛化性能不强的问题，从而能够自适应生成合理的控制器参数，实现无人潜航器的位姿控制。该方法简单可靠，易于实现，计算量小，实时性较好，提高了UUV位姿控制的可行性与实用性，对今后无人潜航器水下作业、运动控制等领域的发展有着积极意义。
