专利摘要

专利摘要

本发明涉及一种水下航行器轨迹的控制方法和系统，该控制方法包括：接收目标信号，目标信号携带欲达到的目标位置信息；根据当前的位置信息与目标位置信息确定第一状态信号，第一状态信号携带第一方向角度和第一速度；利用第一滤波组件对第一状态信号进行处理得到第二方向角度和第二速度；根据第二速度确定控制力，以使得水下航行器的航行速度在控制力的作用下达到目标速度；根据第二方向角度确定控制力矩，以使得水下航行器的航行方向角度在控制力矩的作用下达到目标方向角度，抑制了高频噪声、外界干扰、参数不确定等情况，增强了系统的鲁棒性。

权利要求

1.一种水下航行器轨迹的控制方法，其特征在于，所述控制方法包括：

接收目标信号，所述目标信号携带欲达到的目标位置信息；

根据当前的位置信息与所述目标位置信息确定第一状态信号，所述第一状态信号携带第一方向角度和第一速度；所述确定第一状态信号还包括，当所述水下航行器为欠驱动状况，根据所述欠驱动状况，确定所述第一状态信号中所述第一方向角度，使得所述第一方向角度作为控制量使所述水下航行器从所述当前的位置到达目标位置；所述欠驱动状况下,水下航行器轨迹的运动学以及动力学方程为：

其中,水下航行器的航向角为θ，位置为P＝[x y]T，控制输入为q＝[v ω]T，v和ω分别为航行器的线速度和角速度,m为水下航行器的质量，I为转动惯量；F为控制力，τ为控制力矩；C1(v)，C2(ω)为科氏矩阵；D1(v)，D2(v)为水动力阻尼矩阵,为位置和姿势向量,设利用第一滤波组件对所述第一状态信号进行处理得到第二方向角度和第二速度；所述第一滤波组件包括一阶低通滤波器；根据所述第二速度确定控制力，以使得水下航行器的航行速度在所述控制力作用下达到目标速度；

根据所述第二方向角度确定控制力矩，以使得所述水下航行器的航行方向角度在所述控制力矩的作用下达到目标方向角度。

2.根据权利要求1所述的方法，其特征在于，所述利用第一滤波组件对所述第一状态信号进行处理得到第二方向角度和第二速度，包括：

通过对所述第一滤波组件设定第一时间常数，并利用所述第一滤波组件对所述第一方向角度进行处理得到第二方向角度；

通过对所述第一滤波组件设定第二时间常数，并利用所述第一滤波组件对所述第一速度进行处理得到所述第二速度。

3.根据权利要求2所述的方法，其特征在于，所述第二方向角度与所述第一时间常数，以及所述第二速度与所述第二时间常数分别满足以下关系：

其中，τ1θ是第一时间常数，τ1v是第二时间常数；是第一方向角度，是第一速度；θ1d是第二方向角度，v1d是第二速度；是的零初始状态；θ1d(0)是θ1d的零初始状态；是的零初始状态；v1d(0)是v1d的零初始状态。

4.根据权利要求1所述的方法，其特征在于，所述根据所述第二速度确定控制力，包括：

根据所述当前的位置信息，所述目标位置信息，以及所述第二速度确定所述控制力。

5.根据权利要求4所述的方法，其特征在于，所述根据所述当前的位置信息，所述目标位置信息，以及所述第二速度确定所述控制力，包括：

计算所述当前的位置信息与所述目标位置信息的位置偏差量；

计算当前的速度与所述第二速度的速度偏差量；

根据所述位置偏差量和所述速度偏差量确定所述控制力。

6.根据权利要求4或5所述的方法，其特征在于，所述第二速度与所述控制力满足以下关系：

其中，F是控制力，m是水下航行器的质量，k1v是第一增益值，S1v是速度偏差量，v1d是第二速度，S1x是当前的位置信息和目标位置信息在x方向的位置偏差量，S1y是当前的位置信息和目标位置信息在y方向的位置偏差量，v是当前的速度，θ1是当前的方向角度，是第一速度，是第一方向角度，C1(v)为科氏矩阵；D1(v)为水动力阻尼矩阵。

7.根据权利要求1所述的方法，其特征在于，所述根据所述第二方向角度确定控制力矩，包括：

计算当前的方向角度与所述第二方向角度的角度偏差量；

根据所述第二方向角度和所述角度偏差量确定所述控制力矩。

8.根据权利要求7所述的方法，其特征在于，所述根据所述第二方向角度和所述角度偏差量确定所述控制力矩，包括：

根据所述第二方向角度和所述角度偏差量计算第二状态信号，所述第二状态信号携带第一角速度；

利用第二滤波组件对所述第二状态信号进行处理得到第二角速度；

根据当前的角速度与所述第二角速度的角速度偏差量，以及所述第二角速度确定所述控制力矩。

9.根据权利要求7或8所述的方法，其特征在于，所述第二速度与所述控制力矩满足以下关系：

其中，τ是控制力矩，I是转动惯量，k2θ是第二增益值，S2θ是角速度偏差量，θ2d是第二角速度，S1θ是角度偏差量，ω为水下航行器角速度，C2(ω)为科氏矩阵；D2(v)为水动力阻尼矩阵。

10.一种系统，其特征在于，所述系统包括权利要求1至权利要求9中所述的第一滤波组件和第二滤波组件，所述第一滤波组件用于对第一状态信号进行处理得到第二方向角度和第二速度，以使得根据第二速度确定目标速度，根据第二方向角度确定目标方向角度；所述系统用于执行权利要求1至权利要求9所述的方法。

说明书

技术领域

本发明涉及航行器领域，尤其涉及一种水下航行器轨迹的控制方法和系统。

背景技术

随着电子技术、测量技术、控制技术等领域的发展，使得水下航行器更容易在恶劣的环境下完成高精度、高质量、高难度的任务。而今，水下航行器已经在水下探测、救援、环境监测、科学研究、军事国防等方面得到了广泛的应用，而对航行轨迹的精确跟踪能力是实现上述应用必须的技术基础。但由于这类航行器通常是欠驱动的，即控制量小于系统的被控制量，并且外界复杂的环境存在噪声、强干扰，系统内部存在建模误差、参数不确定，使得水下航行器的轨迹跟踪控制器设计具有一定的难度，无法实现水下航行器沿给定的轨迹行驶，即无法满足对水下航行器轨迹的精确跟踪。

所以需要寻找新的方案来解决这一问题。

发明内容

本发明实施例提供了一种水下航行器轨迹的控制方法和系统，满足了对水下下航行器轨迹的精确控制，实现了水下航行器沿给定的轨迹行驶。

第一方面，提供了一种水下航行器轨迹的控制方法，该控制方法包括：

接收目标信号，目标信号携带欲达到的目标位置信息；

根据当前的位置信息与目标位置信息确定第一状态信号，第一状态信号携带第一方向角度和第一速度；

利用第一滤波组件对第一状态信号进行处理得到第二方向角度和第二速度；

根据第二速度确定控制力，以使得水下航行器的航行速度在控制力的作用下达到目标速度；

根据第二方向角度确定控制力矩，以使得水下航行器的航行方向角度在控制力矩的作用下达到目标方向角度。

在一个可能的实现中，利用第一滤波组件对第一状态信号进行处理得到第二方向角度和第二速度，包括：

通过对第一滤波组件设定第一时间常数，并利用第一滤波组件对第一方向角度进行处理得到第二方向角度；

通过对第一滤波组件设定第二时间常数，并利用第一滤波组件对第一速度进行处理得到第二速度。

进一步，在一个可能的实现中，第二方向角度与第一时间常数，以及第二速度与第二时间常数分别满足以下关系：

其中，τ1θ是第一时间常数，τ1v是第二时间常数； 是第一方向角度， 是第一速度；θ1d是第二方向角度，v1d是第二速度。

在一个可能的实现中，根据第二速度确定控制力，包括：

根据当前的位置信息，目标位置的信息，以及第二速度确定控制力。

在另一个可能的实现中，根据当前的位置信息，目标位置的信息，以及第二速度确定控制力，包括：

计算当前的位置信息与目标位置信息的位置偏差量；

计算当前的速度与第二速度的速度偏差量；

根据位置偏差量和速度偏差量确定控制力。

进一步，在一个可能的实现中，第二速度与控制力满足以下关系：

其中，F是控制力，m是系统质量，k1v是第一增益值，S1v是速度偏差量，v1d是第二速度，S1x是当前的位置信息和目标位置信息在x方向的位置偏差量，S1y是当前的位置信息和目标位置信息在y方向的位置偏差量，v是当前的速度，θ1是当前的方向角度， 是第一速度， 是第一方向角度，C1(v)为科氏矩阵；D1(v)为水动力阻尼矩阵。

在一个可能的实现中，根据第二方向角度确定控制力矩，包括：

计算当前的方向角度与第二方向角度的角度偏差量；

根据第二方向角度和角度偏差量确定控制力矩。

在另一个可能的实现中，根据第二方向角度和角度偏差量确定控制力矩，包括：

根据第二方向角度和角度偏差量计算第二状态信号，第二状态信号携带第一角速度；

利用第二滤波组件对第二状态信号进行处理得到第二角速度；

根据当前的角速度与第二角速度的角速度偏差量，以及第二角速度确定控制力矩。

进一步，在一个可能的实现中，第二速度与控制力矩满足以下关系：

其中，τ是控制力矩，I是转动惯量，k2θ是第二增益值，S2θ是角速度偏差量，θ2d是第二角速度，S1θ是角度偏差量，ω为水下航行器角速度，C2(ω)为科氏矩阵；D2(v)为水动力阻尼矩阵。

第二方面，提供了一种系统，该系统包括第一方面中的第一滤波组件和第二滤波组件，第一滤波组件用于对第一状态信号进行处理得到第二方向角度和第二速度，以使得根据第二速度确定目标速度，根据第二方向角度确定目标方向角度；系统用于执行第一方面的方法。

基于本申请实施例提供的水下航行器轨迹的控制方法和系统，通过在系统中增加滤波组件，对系统中的第一状态信号进行滤波处理，并基于滤波后的第二速度和第二方向角度确定控制力和控制力矩，以使水下航行器在控制力和控制力矩的作用下，航行速度达到目标速度，航行方向角度达到目标方向角度，实现了对水下下航行器轨迹的精确控制，实现了水下航行器沿给定的轨迹行驶。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对本发明实施例中所需要使用的附图作简单地介绍，显而易见地，下面所描述的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例提供的一种水下航行器示意图；

图2为本发明实施例提供的一种水下航行器轨迹的控制方法流程示意图；

图3是本发明实施例提供的一种系统框架示意图；

图4是本发明实施例提供的一种姿态跟踪误差的示意图；

图5是本放实施例提供的一种位置跟踪误差的示意图；

图6是本发明实施例提供的一种平面轨迹示意图；

图7是本发明实施例提供的一种系统结构示意图。

具体实施方式

为了实现水下航行器轨迹的跟踪，当前理论上已证明系统的收敛性，但所设计的控制律往往很复杂，难以实现。其中，控制律是系统形成控制指令的算法，描述受控状态变量与系统输入信号之间的函数关系。其次，没有考虑到信号求导的难实现性，通常需要假设系统内部信号的导数容易获得，而实际中对信号求导往往是近似的，且常对信号求导的方法是在间隔时刻对信号进行差分，当存在噪声、参数不确定、外界干扰时无法精确获得信道的导数；而且采用差分求导也会放大系统噪声。

因此，由于水下环境条件恶劣，存在噪声与强干扰，所以通常的求导方法并不适用。为了减少信号求导对系统稳定性的影响，使得控制律易于实现，本发明实施例提供了一种水下航行器轨迹跟踪的控制方法，提供的控制输入表达式简洁，且易于工程应用；并通过将信号输入滤波器以代替当前信号的求导，以抑制高频噪声，外界干扰、参数不确定，增强了系统的鲁棒性，鲁棒性是指系统在一定的参数变动下，维持其他某些性能的特征。

需要说明的是，本实施例中的系统是指水下航行器轨迹的控制系统。下文提到的水下航行器也可以称为航行器。

下面结合附图对本发明实施例的方案进行描述。

水下航行器在x方向与y方向分别具有一个推进器，由螺旋桨驱动产生推理，通过调节推力的大小产生控制力矩，实现转向的目的，如图1所示，定义水下航行器的航向角为θ，位置为P＝[x y]T，控制输入为为q＝[v ω]T，v和ω分别为航行器的线速度和角速度。在下面各个实施例的描述中，v也可以称为速度。

可得运动学以及动力学方程为：

其中m为水下航行器的质量，I为转动惯量；F为控制力，τ为控制力矩；C1(v)，C2(ω)为科氏矩阵；D1(v)，D2(v)为水动力阻尼矩阵。由上式(1)可见，该模型共有2个自由度，模型输出为3个变量，为欠驱动系统，只能实现2个变量的主动跟踪，剩余的变量为随动或镇定状态。通过设计控制力F以及控制力矩τ实现移动航行器对参考速度pd的跟踪，夹角θ的随动。

图2是本发明实施例提供的一种水下航行器轨迹控制方法的流程示意图。该方法的执行主体是水下航行器，该水下航行器也称为系统，如图2所示，该方法可以包括以下步骤：

S110，接收目标信号，目标信号携带欲达到的目标位置信息。

系统接收期望达到的目标位置(x1d,y1d)的目标信号pd＝(x1d,y1d)T，如图3所示。其中，pd＝(x1d,y1d)T用于描述系统航行的目标位置，以及目标运动状态，其中，目标运动状态包括系统航行的目标速度，系统航行的目标方向角度等状态信息。图3是本发明实施例提供的一种统框架示意图，该系统框架示意图示出了控制水下航行器轨迹的内部实现过程。

S120，根据当前的位置信息与目标位置信息确定第一状态信号，第一状态信号携带第一方向角度和第一速度。

第一方向角度是根据当前的位置信息(x,y)和目标位置信息(x1d,y1d)确定理想方向角度 也称为航向角。第一速度是根据当前的位置信息(x1,y1)和目标位置信息(x1d,y1d)确定的理想线速度

在一个实施例中，如图3所示，系统根据(x,y)和(x1d,y1d)确定 和 的具体过程如下：

系统基于当前的位置信息(x,y)和目标信号中携带的目标位置信息(x1d,y1d)，通过位置动力学模型、位置偏差量计算后得到的第一状态信号，第一状态信号携带第一速度 和第一方向角度

其中，位置动力学模型用于描述系统的位置和运动状态。假设x1＝x， y1＝y， θ1＝θ，θ2＝ω。由式(1)可得位置动力学模型：

以及，

其中，式(2)可以称为水下航行器的位置子系统，式(3)可以称为水下航行器的姿态子系统。位置偏差量也可以称为系统位置误差，即目标信号pd＝(x1d,y1d)T欲达到的目标位置信息实际值，当前的位置信息是测量值，则认为当前的位置信息和目标位置信息的差值为系统位置误差，即位置偏差量。位置连差量包括x方向的位置偏差量和y方向的位置偏差量，即：

S1x＝x1-x1d

S1y＝y1-y1d(4)

其中，S1x是当前的位置信息和目标位置信息在x方向的位置偏差量，S1y是当前的位置信息和目标位置信息在y方向的位置偏差量。

由此可知，则 将式(2)代入可得：

因为系统为欠驱动系统，仅仅设计v是无法同时跟踪x和y两个方向的位置信息，因此理想的角度信息 也要作为控制量，以克服欠驱动问题。控制目标选取为跟踪位置和角度，位置子系统 与姿态子系统 间接控制位置变量，设计控制力F使得v跟踪目标速度 设计控制力矩τ使得θ1跟踪目标角度

因此，设计第一个Lyapunov函数为：

可得，

定义第一状态信号为：

其中，k1x，k1y分别是预设定的控制增益值。

将式(8)代入式(5)可得：

其中，

结合式(7)可得：

第一速度 是航行过程中理想的速度，第一方向角度 是航行过程中理想的航向角。

根据式(8)可知， 将 取值范围限制在(-π/2,π/2)，得到满足理想姿态轨迹跟踪的 为：

即上式所求得的 为位置控制律所要求的理想角度，如果水下航行器的实际角度θ1与理想角度 相等，则理想的轨迹控制律可实现，但实际θ1与 不可能完全一致，尤其是控制的初始阶段，这会造成闭环跟踪系统的不稳定。

如果按照通常的滑模控制方法，需要对式(12)与式(13)进行求导，以实现轨迹跟踪。分别对式(12)、式(13)求导可得：

将式(15)、式(15)展开，项数十分复杂；而且，随着求导阶次的增加所得的结果会更加复杂，造成“微分爆炸”。

本发明实施例为了解决上述问题，在系统中增加滤波组件，该滤波组件可以是一阶低通滤波器，利用滤波器的输出代替原信号，避免对信号求导。将 输入到低通滤波器，即执行S130。需要说明的是，在该实施例中，滤波组件可以包括多个滤波组件。

S130，利用第一滤波组件对第一状态信号进行处理得到第二方向角度和第二速度。

考虑到系统的噪声会对信号的求导过程产生强干扰，造成很大的误差，影响系统的稳定性，引入滤波组件，将中间信号 和 进行滤波，得到输出信号第二方向角度θ1d和第二速度v1d。在图3的与滤波组件中设定第一时间常数τ1θ和第二时间常数τ1v，τ1θ用于对中间信号 进行滤波，τ1v用于对中间信号 进行滤波；τ1θ和τ1v数值的设置会影响到滤波效果。

在一个实施例中，滤波组件满足一下关系：

其中τ1θ，τ1v为滤波器的时间常数； 为滤波器的输入；θ1d，v1d为滤波器的输出。

S140，根据第二速度确定控制力，以使得水下航行器的航行速度在控制力的作用下达到目标速度。

接着使用经滤波后得到的v1d，以及当前速度与经滤波后得到的v1d的速度偏差量S1v代入设计的控制力F公式：

其中，F是控制力，m是系统质量，k1v是第一增益值，S1v是速度偏差量，v1d是第二速度，S1x是当前的位置信息和目标位置信息在x方向的位置偏差量，S1y是当前的位置信息和目标位置信息在y方向的位置偏差量，v是当前的速度，θ1是当前的方向角度， 是第一速度， 是第一方向角度，C1(v)为科氏矩阵；D1(v)为水动力阻尼矩阵。需要说明的是，这里的k1v是预先设置的增益值，或者称为控制增益值。

求得控制力F，然后再根据位置动力学模型

得到v和x1，y1，该v和x1，y1在本发明实施中是个不断被更新的值，当根据当前的速度v求得F后，根据F求得新的v，进一步积分得到x1和y1，并更新当前的速度v，以保证航行器能按照预设的v航行，从而使航行器位置坐标x1，y1能够跟踪目标信号pd＝(x1d,y1d)T。

S150，根据第二方向角度确定控制力矩，以使得水下航行器的航行方向角度在控制力矩的作用下达到目标方向角度。

系统使用经滤波后得到的θ1d，以及当前方向角度θ1与经滤波后得到的θ1d的角度偏差量S1θ确定第二状态信号，第二状态信号携带第一角速度

其中，k1θ是预先设置的控制增益值。然后利用第二滤波器组件对第二状态信号进行处理得到第二角速度θ2d，其中，在第二滤波组件对第二状态信号进行处理过程中，设置了时间常数τ2θ，以满足第二滤波组件对第二状态信号的滤波效果。接着，依据当前的角速度θ2＝ω与第二角速度θ2d的角速度偏差量S2θ，以及第二角速度θ2d确定控制力矩τ，具体的将这些量代入设计的控制力矩τ公式：

其中，τ是控制力矩，I是转动惯量，k2θ是第二增益值，S2θ是角速度偏差量，θ2d是第二角速度，S1θ是角度偏差量，ω为航行器角速度，C2(ω)为科氏矩阵；D2(v)为水动力阻尼矩阵。需要说明的是，这里的k2θ是预先设置的增益值，或者称为控制增益值。得到控制力矩τ，并根据公式：

得到θ1。因为θ1＝θ，即得到航行角θ。在该实施例中，航行角θ也是不断被更新的值，当根据当前的θ求得控制力矩τ时，再由控制力矩τ求得θ，以保证能按照预设的航向角θ航行。

整个航行过程中按照预设的航行速度vd以及预设的航行角θd航行，即实现了按照预设的轨迹pd＝(x1d,y1d)T行驶。采用本发明实施例增加滤波器件对中间信号进行滤波处理，避免了查分求导导致系统噪声的放大，避免了在存在噪声、参数不确定，外外界干扰等恶劣条件下，无法精确获得信号的导数的问题，抑制高频噪声、外界干扰、参数不确定，增强了系统的鲁棒性，提高了系统的稳定性，而且使控制律容易实现。且采用设计的控制力和控制力矩的关系式比较简洁，易于用于工程应用。

需要说明的是，在图3所示的系统中设置了滤波组件，包括第一滤波组件和第二滤波组件，第一滤波组件和第二滤波组件可以是滤波器，或者是具有滤波功能的设备。在一个实施例中，第一滤波组件和第二滤波组件可以是同一个滤波组件，也可以是分别独立的滤波组件，在该实施例中不作限定。

还需说明的是，在该实施例中，“第一”，“第二”仅仅是为了区分事物，并不对事物本身进行限定。

图3中直接采用设计好的控制力F和控制力矩τ实现了水下航行器轨迹的控制，下面对控制力F和控制力矩τ的设计过程进行描述。

首先，定义位置跟踪误差(或称为速度偏差量)为：

S1v＝v-v1d(17)

由于控制目标为使v跟踪 而上述定义的位置跟踪误差是v与v1d之间的误差，换句话讲，也就是v与v1d之间的偏差量。如果 那么v跟踪 或v1d是等价的。定义 与v1d之间的滤波边界层误差为：

若能保证S1v→0且 显然

设计第二个Lyapunov函数为：

求导可得：

由式(16)可知， 代入式(20)展开可得：

设计位置控制力为：

将式(22)代入式(21)可得：

上述是对控制力F的设计，下面描述控制力矩τ的设计过程：

定义姿态跟踪误差(或称为角度偏差量)为：

S1θ＝θ1-θ1d(24)

控制目标为使θ1跟踪 而上述定义的姿态跟踪误差是为了使θ1跟踪θ1d，如果 那么θ1跟踪 与θ1d二者是等价的。定义 与θ1d之间的滤波边界层误差为：

得 设计第三个Lyapunov函数为：

求导可得：

设计虚拟控制律为：

将 输入到如下低通滤波器中，

其中τ2θ为滤波器的时间常数； 为滤波器的输入；θ2d为滤波器的输出。与式(24)和式(25)类似，定义第二组姿态跟踪误差与滤波边界层误差：

S2θ＝θ2-θ2d

可得，

根据式(16)可知， 又根据式(30)并结合式(28)可知，

将上述结果代入式(27)可得：

设计第四个Lyapunov函数为：

求导可得：

根据式(29)可知，

根据式(28)与式(31)可知，

将式(32)与式(35)代入式(34)可得：

设计姿态控制力矩为：

如前所述设计了控制力F和控制力矩τ，下面对设计的控制力F和控制力矩τ的稳定性进行分析：

将式(38)代入式(37)可得：

将式(23)代入式(39)，整理可得：

由滤波边界层误差的定义可得：

根据式(8)、式(14)、式(15)可知，存在非负连续函数B1v、B1θ、B2θ满足：

假设1、跟踪目标轨迹pd是有界的，存在正数χ1使得 成立，存在正数χ2使得 成立。

定理1：对于式(1)所描述的航行器系统，采用式(22)以及式(38)所设计的控制力以及控制力矩。在假设1的基础上，当系统初始值V4(0)≤c，c为任意正常数。那么可以通过调节控制增益k1x、k1y、k1v、k1θ、k2θ；函数B1v、B1θ、B2θ；时间常数τ1v、τ1θ、τ2θ使航行器的状态信号半全局一致有界，其跟踪误差限定在一个小的残集中。

证：利用杨氏不等式，式(40)满足：

在V4(0)≤p成立的基础上，设Ω1和Ω2为紧集，则对它们的表述为：

可知Ω1×Ω2也是紧集。在V≤c成立时，B1v、B1θ、B2θ在Ω1×Ω2上有最大值，记为：M1v、M1θ、M2θ。因此 满足：

又因为

将式(46)代入式(45)

选取系统的控制参数如下： k1θ≥1+r， 其中r可以设定系统最终的误差，则

当V4＝c时，由于B*≤M*(*＝1v,1θ,2θ)，可得 因此可知V4≤c为系统的不变集；此外，当V4(0)≤c，即对任意t＞0都有V4(t)≤c。

可得

求解(48)可得

因此闭环系统的所有信号是有界的，而且

由上式可知，增大参数r可通过调节调节控制增益k1x、k1y、k1v、k1θ、k2θ；函数B1v、B1θ、B2θ；时间常数τ1v、τ1θ、τ2θ来实现，使得V4(t)收敛到一个残集，跟踪误差可以任意小，满足工程需要。

通过控制力输入式(22)可知，所需的信号中需要 而当 输入滤波器后满足 通过滤波器的输入 与输出v1d的线性运算避免了对信号的求导；通过控制力矩输入式(38)可知，所需的信号中需要 而当 输入滤波器后满足 通过滤波器的输入 与输出θ2d的线性运算避免了对信号的求导；增强了系统对噪声干扰、参数不确定的鲁棒性。

在一个具体实施例中，例如，水下航行器的质量为m＝10Kg，转动惯量为I＝4Kg·m2。控制目标为pd＝(-5+t,sin(0.5t)+1)Tm；控制增益为：k1x＝5，k1y＝5，k1v＝5，k1θ＝5，k2θ＝5；滤波器时间常数为：τ1v＝0.02，τ1θ＝0.02，τ2θ＝0.02；初始状态为p(0)＝(-6,-2)Tm，θ(0)＝0°，通过本发明实施例提供的水下航行器轨迹的控制方法得到图4至图6的示意图。

其中，图4给出了姿态跟踪误差S1θ的示意图，即S1θ的角度(rad)与时间t(s)的关系示意图。如图4所示，S1θ的角度值在很短时间内趋于零，并保持稳定，证明实际航行的航行角θ1与θ1d的角度相等，即使用该方法可以很好的跟踪水下航行器的航行轨迹，即保证按照预设航行角θ航行。

图5给出了系统位置跟踪误差的示意图，即系统位置随时间t(s)的关系示意图。如图5所示，大约3s内就能实现x方向位置误差S1x和y方向位置误差S1y均为零，者表明实际航行的位置信息与设定的航行的位置信息一致，同样表明，使用该方法可以很好的跟踪水下航行器的航行轨迹，即保证按照预设位置航行。

图6示出了谁下航行器航行的平面轨迹示意图，如图6所示，实际航行位置P与期望达到的航行位置Pd几乎重合，也表明，使用该方法可以很好的跟踪水下航行器航行的位置轨迹，保证水下航行器按照设定的位置航行。

本发明实施例还提供了一种水下航行器轨迹的控制系统，如图7所示，该控制系统700包括：

接收模块710，用于接收目标信号，目标信号携带欲达到的目标位置信息；

确定模块720，用于根据当前的位置信息与目标位置信息确定第一状态信号，第一状态信号携带第一方向角度和第一速度；

第一滤波组件730，用于对第一状态信号进行处理得到第二方向角度和第二速度；

所述确定单元720，还用于根据第二速度确定控制力，以使得水下航行器的航行速度在控制力作用下达到目标速度；

所述确定单元720，还用于根据第二方向角度确定控制力矩，以使得水下航行器的航行方向角度在控制力矩的作用下达到目标方向角度。

可选地，在一个实施例中，第一滤波组件730具体用于：

通过对第一滤波组件设定第一时间常数，并利用第一滤波组件对第一方向角度进行处理得到第二方向角度；

通过对第一滤波组件设定第二时间常数，并利用第一滤波组件对第一速度进行处理得到第二速度。

可选地，在一个实施例中，第二方向角度与第一时间常数，以及第二速度与第二时间常数分别满足以下关系：

其中，τ1θ是第一时间常数，τ1v是第二时间常数； 是第一方向角度， 是第一速度；θ1d是第二方向角度，v1d是第二速度。

可选地，在一个实施例中，确定单元720具体用于：

根据当前的位置信息，目标位置的信息，以及第二速度确定控制力。

可选地，在一个实施例中，确定单元720具体用于：

计算当前的位置信息与目标位置信息的位置偏差量；

计算当前的速度与第二速度的速度偏差量；

根据位置偏差量和速度偏差量确定控制力。

进一步，在一个实施例中，第二速度与控制力满足以下关系：

其中，F是控制力，m是水下航行器的质量，k1v是第一增益值，S1v是速度偏差量，v1d是第二速度，S1x是当前的位置信息和目标位置信息在x方向的位置偏差量，S1y是当前的位置信息和目标位置信息在y方向的位置偏差量，v是当前的速度，θ1是当前的方向角度， 是第一速度， 是第一方向角度，C1(v)为科氏矩阵；D1(v)为水动力阻尼矩阵。

可选地，在一个实施例中，根据第二方向角度确定控制力矩，包括：

计算当前的方向角度与第二方向角度的角度偏差量；

根据第二方向角度和角度偏差量确定控制力矩。

在另一个可能的实现中，该控制系统还包括第二滤波组件740；

确定单元720用于根据第二方向角度和角度偏差量计算第二状态信号，第二状态信号携带第一角速度；

第二滤波组件740对第二状态信号进行处理得到第二角速度；

确定单元720根据当前的角速度与第二角速度的角速度偏差量，以及第二角速度确定控制力矩。

进一步，在一个实施例中，第二速度与控制力矩满足以下关系：

其中，τ是控制力矩，I是转动惯量，k2θ是第二增益值，S2θ是角速度偏差量，θ2d是第二角速度，S1θ是角度偏差量，ω为航行器角速度，C2(ω)为科氏矩阵；D2(v)为水动力阻尼矩阵。

该系统700可以实现图2和图3中的方法/步骤，且能达到相同的技术效果，为简洁描述，在此不再赘述。

需要说明的是，在一个实施例中，第一滤波组件和第二滤波组件可以是同一个滤波组件，也可以是独立的滤波组件，在该实施例中不作限定。

专业人员应该还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

结合本文中所公开的实施例描述的方法或算法的步骤可以用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。

以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

一种水下航行器轨迹的控制方法和系统专利购买费用说明

Q：办理专利转让的流程及所需资料

A：专利权人变更需要办理著录项目变更手续，有代理机构的，变更手续应当由代理机构办理。

1：专利变更应当使用专利局统一制作的“著录项目变更申报书”提出。

2：按规定缴纳著录项目变更手续费。

3：同时提交相关证明文件原件。

4：专利权转移的，变更后的专利权人委托新专利代理机构的，应当提交变更后的全体专利申请人签字或者盖章的委托书。

Q:专利著录项目变更费用如何缴交

A：（1）直接到国家知识产权局受理大厅收费窗口缴纳,（2）通过代办处缴纳，（3）通过邮局或者银行汇款,更多缴纳方式

Q：专利转让变更，多久能出结果

A：著录项目变更请求书递交后，一般1-2个月左右就会收到通知，国家知识产权局会下达《转让手续合格通知书》。