CN109693774B - 一种水下航行器轨迹的控制方法和*** - Google Patents

一种水下航行器轨迹的控制方法和*** Download PDF

Info

Publication number
CN109693774B
CN109693774B CN201811641485.7A CN201811641485A CN109693774B CN 109693774 B CN109693774 B CN 109693774B CN 201811641485 A CN201811641485 A CN 201811641485A CN 109693774 B CN109693774 B CN 109693774B
Authority
CN
China
Prior art keywords
speed
direction angle
control
target
angle
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201811641485.7A
Other languages
English (en)
Other versions
CN109693774A (zh
Inventor
王锐
鄢社锋
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Institute of Acoustics CAS
Original Assignee
Institute of Acoustics CAS
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Institute of Acoustics CAS filed Critical Institute of Acoustics CAS
Priority to CN201811641485.7A priority Critical patent/CN109693774B/zh
Publication of CN109693774A publication Critical patent/CN109693774A/zh
Application granted granted Critical
Publication of CN109693774B publication Critical patent/CN109693774B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B63—SHIPS OR OTHER WATERBORNE VESSELS; RELATED EQUIPMENT
    • B63G—OFFENSIVE OR DEFENSIVE ARRANGEMENTS ON VESSELS; MINE-LAYING; MINE-SWEEPING; SUBMARINES; AIRCRAFT CARRIERS
    • B63G8/00—Underwater vessels, e.g. submarines; Equipment specially adapted therefor
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B63—SHIPS OR OTHER WATERBORNE VESSELS; RELATED EQUIPMENT
    • B63C—LAUNCHING, HAULING-OUT, OR DRY-DOCKING OF VESSELS; LIFE-SAVING IN WATER; EQUIPMENT FOR DWELLING OR WORKING UNDER WATER; MEANS FOR SALVAGING OR SEARCHING FOR UNDERWATER OBJECTS
    • B63C11/00—Equipment for dwelling or working underwater; Means for searching for underwater objects
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Mechanical Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Ocean & Marine Engineering (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Feedback Control In General (AREA)

Abstract

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

Description

一种水下航行器轨迹的控制方法和***
技术领域
本发明涉及航行器领域，尤其涉及一种水下航行器轨迹的控制方法和***。
背景技术
随着电子技术、测量技术、控制技术等领域的发展，使得水下航行器更容易在恶劣的环境下完成高精度、高质量、高难度的任务。而今，水下航行器已经在水下探测、救援、环境监测、科学研究、军事国防等方面得到了广泛的应用，而对航行轨迹的精确跟踪能力是实现上述应用必须的技术基础。但由于这类航行器通常是欠驱动的，即控制量小于***的被控制量，并且外界复杂的环境存在噪声、强干扰，***内部存在建模误差、参数不确定，使得水下航行器的轨迹跟踪控制器设计具有一定的难度，无法实现水下航行器沿给定的轨迹行驶，即无法满足对水下航行器轨迹的精确跟踪。
所以需要寻找新的方案来解决这一问题。
发明内容
本发明实施例提供了一种水下航行器轨迹的控制方法和***，满足了对水下下航行器轨迹的精确控制，实现了水下航行器沿给定的轨迹行驶。
第一方面，提供了一种水下航行器轨迹的控制方法，该控制方法包括：
接收目标信号，目标信号携带欲达到的目标位置信息；
根据当前的位置信息与目标位置信息确定第一状态信号，第一状态信号携带第一方向角度和第一速度；
利用第一滤波组件对第一状态信号进行处理得到第二方向角度和第二速度；
根据第二速度确定控制力，以使得水下航行器的航行速度在控制力的作用下达到目标速度；
根据第二方向角度确定控制力矩，以使得水下航行器的航行方向角度在控制力矩的作用下达到目标方向角度。
在一个可能的实现中，利用第一滤波组件对第一状态信号进行处理得到第二方向角度和第二速度，包括：
通过对第一滤波组件设定第一时间常数，并利用第一滤波组件对第一方向角度进行处理得到第二方向角度；
通过对第一滤波组件设定第二时间常数，并利用第一滤波组件对第一速度进行处理得到第二速度。
进一步，在一个可能的实现中，第二方向角度与第一时间常数，以及第二速度与第二时间常数分别满足以下关系：
Figure GDA0001977766360000021
其中，τ1θ是第一时间常数，τ1v是第二时间常数；
Figure GDA0001977766360000022
是第一方向角度，
Figure GDA0001977766360000023
是第一速度；θ1d是第二方向角度，v1d是第二速度。
在一个可能的实现中，根据第二速度确定控制力，包括：
根据当前的位置信息，目标位置的信息，以及第二速度确定控制力。
在另一个可能的实现中，根据当前的位置信息，目标位置的信息，以及第二速度确定控制力，包括：
计算当前的位置信息与目标位置信息的位置偏差量；
计算当前的速度与第二速度的速度偏差量；
根据位置偏差量和速度偏差量确定控制力。
进一步，在一个可能的实现中，第二速度与控制力满足以下关系：
Figure GDA0001977766360000024
Figure GDA0001977766360000025
Figure GDA0001977766360000026
其中，F是控制力，m是***质量，k1v是第一增益值，S1v是速度偏差量，v1d是第二速度，S1x是当前的位置信息和目标位置信息在x方向的位置偏差量，S1y是当前的位置信息和目标位置信息在y方向的位置偏差量，v是当前的速度，θ1是当前的方向角度，
Figure GDA0001977766360000027
是第一速度，
Figure GDA0001977766360000028
是第一方向角度，C1(v)为科氏矩阵；D1(v)为水动力阻尼矩阵。
在一个可能的实现中，根据第二方向角度确定控制力矩，包括：
计算当前的方向角度与第二方向角度的角度偏差量；
根据第二方向角度和角度偏差量确定控制力矩。
在另一个可能的实现中，根据第二方向角度和角度偏差量确定控制力矩，包括：
根据第二方向角度和角度偏差量计算第二状态信号，第二状态信号携带第一角速度；
利用第二滤波组件对第二状态信号进行处理得到第二角速度；
根据当前的角速度与第二角速度的角速度偏差量，以及第二角速度确定控制力矩。
进一步，在一个可能的实现中，第二速度与控制力矩满足以下关系：
Figure GDA0001977766360000031
其中，τ是控制力矩，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也可以称为速度。
可得运动学以及动力学方程为：
Figure GDA0001977766360000041
Figure GDA0001977766360000042
其中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)确定理想方向角度
Figure GDA0001977766360000051
也称为航向角。第一速度是根据当前的位置信息(x1,y1)和目标位置信息(x1d,y1d)确定的理想线速度
Figure GDA0001977766360000052
在一个实施例中，如图3所示，***根据(x,y)和(x1d,y1d)确定
Figure GDA0001977766360000053
和
Figure GDA0001977766360000054
的具体过程如下：
***基于当前的位置信息(x,y)和目标信号中携带的目标位置信息(x1d,y1d)，通过位置动力学模型、位置偏差量计算后得到的第一状态信号，第一状态信号携带第一速度
Figure GDA0001977766360000055
和第一方向角度
Figure GDA0001977766360000056
其中，位置动力学模型用于描述***的位置和运动状态。假设x1＝x，
Figure GDA0001977766360000057
y1＝y，
Figure GDA0001977766360000058
θ1＝θ，θ2＝ω。由式(1)可得位置动力学模型：
Figure GDA0001977766360000059
Figure GDA00019777663600000510
Figure GDA00019777663600000511
以及，
Figure GDA00019777663600000512
Figure GDA00019777663600000513
其中，式(2)可以称为水下航行器的位置子***，式(3)可以称为水下航行器的姿态子***。位置偏差量也可以称为***位置误差，即目标信号pd＝(x1d,y1d)T欲达到的目标位置信息实际值，当前的位置信息是测量值，则认为当前的位置信息和目标位置信息的差值为***位置误差，即位置偏差量。位置连差量包括x方向的位置偏差量和y方向的位置偏差量，即：
S1x＝x1-x1d
S1y＝y1-y1d (4)
其中，S1x是当前的位置信息和目标位置信息在x方向的位置偏差量，S1y是当前的位置信息和目标位置信息在y方向的位置偏差量。
由此可知，则
Figure GDA0001977766360000061
将式(2)代入可得：
Figure GDA0001977766360000062
Figure GDA0001977766360000063
因为***为欠驱动***，仅仅设计v是无法同时跟踪x和y两个方向的位置信息，因此理想的角度信息
Figure GDA0001977766360000064
也要作为控制量，以克服欠驱动问题。控制目标选取为跟踪位置和角度，位置子***
Figure GDA0001977766360000065
与姿态子***
Figure GDA0001977766360000066
间接控制位置变量，设计控制力F使得v跟踪目标速度
Figure GDA0001977766360000067
设计控制力矩τ使得θ1跟踪目标角度
Figure GDA0001977766360000068
因此，设计第一个Lyapunov函数为：
Figure GDA0001977766360000069
可得，
Figure GDA00019777663600000610
定义第一状态信号为：
Figure GDA00019777663600000611
Figure GDA00019777663600000612
其中，k1x，k1y分别是预设定的控制增益值。
将式(8)代入式(5)可得：
Figure GDA00019777663600000613
Figure GDA00019777663600000614
其中，
Figure GDA00019777663600000615
Figure GDA00019777663600000616
结合式(7)可得：
Figure GDA00019777663600000617
第一速度
Figure GDA00019777663600000618
是航行过程中理想的速度，第一方向角度
Figure GDA00019777663600000619
是航行过程中理想的航向角。
根据式(8)可知，
Figure GDA0001977766360000071
将
Figure GDA0001977766360000072
取值范围限制在(-π/2,π/2)，得到满足理想姿态轨迹跟踪的
Figure GDA0001977766360000073
为：
Figure GDA0001977766360000074
Figure GDA0001977766360000075
即上式所求得的
Figure GDA0001977766360000076
为位置控制律所要求的理想角度，如果水下航行器的实际角度θ1与理想角度
Figure GDA0001977766360000077
相等，则理想的轨迹控制律可实现，但实际θ1与
Figure GDA0001977766360000078
不可能完全一致，尤其是控制的初始阶段，这会造成闭环跟踪***的不稳定。
如果按照通常的滑模控制方法，需要对式(12)与式(13)进行求导，以实现轨迹跟踪。分别对式(12)、式(13)求导可得：
Figure GDA0001977766360000079
Figure GDA00019777663600000710
将式(15)、式(15)展开，项数十分复杂；而且，随着求导阶次的增加所得的结果会更加复杂，造成“微分***”。
本发明实施例为了解决上述问题，在***中增加滤波组件，该滤波组件可以是一阶低通滤波器，利用滤波器的输出代替原信号，避免对信号求导。将
Figure GDA00019777663600000711
输入到低通滤波器，即执行S130。需要说明的是，在该实施例中，滤波组件可以包括多个滤波组件。
S130，利用第一滤波组件对第一状态信号进行处理得到第二方向角度和第二速度。
考虑到***的噪声会对信号的求导过程产生强干扰，造成很大的误差，影响***的稳定性，引入滤波组件，将中间信号
Figure GDA00019777663600000712
和
Figure GDA00019777663600000713
进行滤波，得到输出信号第二方向角度θ1d和第二速度v1d。在图3的与滤波组件中设定第一时间常数τ1θ和第二时间常数τ1v，τ1θ用于对中间信号
Figure GDA00019777663600000714
进行滤波，τ1v用于对中间信号
Figure GDA00019777663600000715
进行滤波；τ1θ和τ1v数值的设置会影响到滤波效果。
在一个实施例中，滤波组件满足一下关系：
Figure GDA0001977766360000081
其中τ1θ，τ1v为滤波器的时间常数；
Figure GDA0001977766360000082
为滤波器的输入；θ1d，v1d为滤波器的输出。
S140，根据第二速度确定控制力，以使得水下航行器的航行速度在控制力的作用下达到目标速度。
接着使用经滤波后得到的v1d，以及当前速度与经滤波后得到的v1d的速度偏差量S1v代入设计的控制力F公式：
Figure GDA0001977766360000083
Figure GDA0001977766360000084
Figure GDA0001977766360000085
其中，F是控制力，m是***质量，k1v是第一增益值，S1v是速度偏差量，v1d是第二速度，S1x是当前的位置信息和目标位置信息在x方向的位置偏差量，S1y是当前的位置信息和目标位置信息在y方向的位置偏差量，v是当前的速度，θ1是当前的方向角度，
Figure GDA0001977766360000086
是第一速度，
Figure GDA0001977766360000087
是第一方向角度，C1(v)为科氏矩阵；D1(v)为水动力阻尼矩阵。需要说明的是，这里的k1v是预先设置的增益值，或者称为控制增益值。
求得控制力F，然后再根据位置动力学模型
Figure GDA0001977766360000088
Figure GDA0001977766360000089
Figure GDA00019777663600000810
得到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θ确定第二状态信号，第二状态信号携带第一角速度
Figure GDA0001977766360000091
Figure GDA0001977766360000092
其中，k1θ是预先设置的控制增益值。然后利用第二滤波器组件对第二状态信号进行处理得到第二角速度θ2d，其中，在第二滤波组件对第二状态信号进行处理过程中，设置了时间常数τ2θ，以满足第二滤波组件对第二状态信号的滤波效果。接着，依据当前的角速度θ2＝ω与第二角速度θ2d的角速度偏差量S2θ，以及第二角速度θ2d确定控制力矩τ，具体的将这些量代入设计的控制力矩τ公式：
Figure GDA0001977766360000093
其中，τ是控制力矩，I是转动惯量，k2θ是第二增益值，S2θ是角速度偏差量，θ2d是第二角速度，S1θ是角度偏差量，ω为航行器角速度，C2(ω)为科氏矩阵；D2(v)为水动力阻尼矩阵。需要说明的是，这里的k2θ是预先设置的增益值，或者称为控制增益值。得到控制力矩τ，并根据公式：
Figure GDA0001977766360000094
Figure GDA0001977766360000095
得到θ1。因为θ1＝θ，即得到航行角θ。在该实施例中，航行角θ也是不断被更新的值，当根据当前的θ求得控制力矩τ时，再由控制力矩τ求得θ，以保证能按照预设的航向角θ航行。
整个航行过程中按照预设的航行速度vd以及预设的航行角θd航行，即实现了按照预设的轨迹pd＝(x1d,y1d)T行驶。采用本发明实施例增加滤波器件对中间信号进行滤波处理，避免了查分求导导致***噪声的放大，避免了在存在噪声、参数不确定，外外界干扰等恶劣条件下，无法精确获得信号的导数的问题，抑制高频噪声、外界干扰、参数不确定，增强了***的鲁棒性，提高了***的稳定性，而且使控制律容易实现。且采用设计的控制力和控制力矩的关系式比较简洁，易于用于工程应用。
需要说明的是，在图3所示的***中设置了滤波组件，包括第一滤波组件和第二滤波组件，第一滤波组件和第二滤波组件可以是滤波器，或者是具有滤波功能的设备。在一个实施例中，第一滤波组件和第二滤波组件可以是同一个滤波组件，也可以是分别独立的滤波组件，在该实施例中不作限定。
还需说明的是，在该实施例中，“第一”，“第二”仅仅是为了区分事物，并不对事物本身进行限定。
图3中直接采用设计好的控制力F和控制力矩τ实现了水下航行器轨迹的控制，下面对控制力F和控制力矩τ的设计过程进行描述。
首先，定义位置跟踪误差(或称为速度偏差量)为：
S1v＝v-v1d (17)
由于控制目标为使v跟踪
Figure GDA0001977766360000101
而上述定义的位置跟踪误差是v与v1d之间的误差，换句话讲，也就是v与v1d之间的偏差量。如果
Figure GDA0001977766360000102
那么v跟踪
Figure GDA0001977766360000103
或v1d是等价的。定义
Figure GDA0001977766360000104
与v1d之间的滤波边界层误差为：
Figure GDA0001977766360000105
若能保证S1v→0且
Figure GDA0001977766360000106
显然
Figure GDA0001977766360000107
设计第二个Lyapunov函数为：
Figure GDA0001977766360000108
求导可得：
Figure GDA0001977766360000109
由式(16)可知，
Figure GDA00019777663600001010
代入式(20)展开可得：
Figure GDA00019777663600001011
设计位置控制力为：
Figure GDA00019777663600001012
将式(22)代入式(21)可得：
Figure GDA00019777663600001013
上述是对控制力F的设计，下面描述控制力矩τ的设计过程：
定义姿态跟踪误差(或称为角度偏差量)为：
S1θ＝θ1-θ1d (24)
控制目标为使θ1跟踪
Figure GDA00019777663600001014
而上述定义的姿态跟踪误差是为了使θ1跟踪θ1d，如果
Figure GDA00019777663600001015
那么θ1跟踪
Figure GDA00019777663600001016
与θ1d二者是等价的。定义
Figure GDA00019777663600001017
与θ1d之间的滤波边界层误差为：
Figure GDA0001977766360000111
得
Figure GDA0001977766360000112
设计第三个Lyapunov函数为：
Figure GDA0001977766360000113
求导可得：
Figure GDA0001977766360000114
设计虚拟控制律为：
Figure GDA0001977766360000115
将
Figure GDA0001977766360000116
输入到如下低通滤波器中，
Figure GDA0001977766360000117
其中τ2θ为滤波器的时间常数；
Figure GDA0001977766360000118
为滤波器的输入；θ2d为滤波器的输出。与式(24)和式(25)类似，定义第二组姿态跟踪误差与滤波边界层误差：
S2θ＝θ2-θ2d
Figure GDA0001977766360000119
可得，
Figure GDA00019777663600001110
根据式(16)可知，
Figure GDA00019777663600001111
又根据式(30)并结合式(28)可知，
Figure GDA00019777663600001112
将上述结果代入式(27)可得：
Figure GDA00019777663600001113
设计第四个Lyapunov函数为：
Figure GDA0001977766360000121
求导可得：
Figure GDA0001977766360000122
根据式(29)可知，
Figure GDA0001977766360000123
根据式(28)与式(31)可知，
Figure GDA0001977766360000124
将式(32)与式(35)代入式(34)可得：
Figure GDA0001977766360000125
设计姿态控制力矩为：
Figure GDA0001977766360000126
如前所述设计了控制力F和控制力矩τ，下面对设计的控制力F和控制力矩τ的稳定性进行分析：
将式(38)代入式(37)可得：
Figure GDA0001977766360000127
将式(23)代入式(39)，整理可得：
Figure GDA0001977766360000128
由滤波边界层误差的定义可得：
Figure GDA0001977766360000131
Figure GDA0001977766360000132
Figure GDA0001977766360000133
根据式(8)、式(14)、式(15)可知，存在非负连续函数B1v、B1θ、B2θ满足：
Figure GDA0001977766360000134
Figure GDA0001977766360000135
Figure GDA0001977766360000136
假设1、跟踪目标轨迹pd是有界的，存在正数χ1使得
Figure GDA0001977766360000137
成立，存在正数χ2使得
Figure GDA0001977766360000138
成立。
定理1：对于式(1)所描述的航行器***，采用式(22)以及式(38)所设计的控制力以及控制力矩。在假设1的基础上，当***初始值V4(0)≤c，c为任意正常数。那么可以通过调节控制增益k1x、k1y、k1v、k1θ、k2θ；函数B1v、B1θ、B2θ；时间常数τ1v、τ1θ、τ2θ使航行器的状态信号半全局一致有界，其跟踪误差限定在一个小的残集中。
证：利用杨氏不等式，式(40)满足：
Figure GDA0001977766360000139
在V4(0)≤p成立的基础上，设Ω1和Ω2为紧集，则对它们的表述为：
Figure GDA00019777663600001310
Figure GDA00019777663600001311
可知Ω1×Ω2也是紧集。在V≤c成立时，B1v、B1θ、B2θ在Ω1×Ω2上有最大值，记为：M1v、M1θ、M2θ。因此
Figure GDA00019777663600001312
满足：
Figure GDA0001977766360000141
又因为
Figure GDA0001977766360000142
Figure GDA0001977766360000143
Figure GDA0001977766360000144
将式(46)代入式(45)
Figure GDA0001977766360000145
选取***的控制参数如下：
Figure GDA0001977766360000146
k1θ≥1+r，
Figure GDA0001977766360000147
其中r可以设定***最终的误差，则
Figure GDA0001977766360000148
当V4＝c时，由于B*≤M*(*＝1v,1θ,2θ)，可得
Figure GDA0001977766360000149
因此可知V4≤c为***的不变集；此外，当V4(0)≤c，即对任意t＞0都有V4(t)≤c。
可得
Figure GDA00019777663600001410
求解(48)可得
Figure GDA00019777663600001411
因此闭环***的所有信号是有界的，而且
Figure GDA00019777663600001412
由上式可知，增大参数r可通过调节调节控制增益k1x、k1y、k1v、k1θ、k2θ；函数B1v、B1θ、B2θ；时间常数τ1v、τ1θ、τ2θ来实现，使得V4(t)收敛到一个残集，跟踪误差可以任意小，满足工程需要。
通过控制力输入式(22)可知，所需的信号中需要
Figure GDA0001977766360000151
而当
Figure GDA0001977766360000152
输入滤波器后满足
Figure GDA0001977766360000153
通过滤波器的输入
Figure GDA0001977766360000154
与输出v1d的线性运算避免了对信号的求导；通过控制力矩输入式(38)可知，所需的信号中需要
Figure GDA0001977766360000155
而当
Figure GDA0001977766360000156
输入滤波器后满足
Figure GDA0001977766360000157
通过滤波器的输入
Figure GDA0001977766360000158
与输出θ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具体用于：
通过对第一滤波组件设定第一时间常数，并利用第一滤波组件对第一方向角度进行处理得到第二方向角度；
通过对第一滤波组件设定第二时间常数，并利用第一滤波组件对第一速度进行处理得到第二速度。
可选地，在一个实施例中，第二方向角度与第一时间常数，以及第二速度与第二时间常数分别满足以下关系：
Figure GDA0001977766360000161
其中，τ1θ是第一时间常数，τ1v是第二时间常数；
Figure GDA0001977766360000162
是第一方向角度，
Figure GDA0001977766360000163
是第一速度；θ1d是第二方向角度，v1d是第二速度。
可选地，在一个实施例中，确定单元720具体用于：
根据当前的位置信息，目标位置的信息，以及第二速度确定控制力。
可选地，在一个实施例中，确定单元720具体用于：
计算当前的位置信息与目标位置信息的位置偏差量；
计算当前的速度与第二速度的速度偏差量；
根据位置偏差量和速度偏差量确定控制力。
进一步，在一个实施例中，第二速度与控制力满足以下关系：
Figure GDA0001977766360000164
Figure GDA0001977766360000165
Figure GDA0001977766360000166
其中，F是控制力，m是水下航行器的质量，k1v是第一增益值，S1v是速度偏差量，v1d是第二速度，S1x是当前的位置信息和目标位置信息在x方向的位置偏差量，S1y是当前的位置信息和目标位置信息在y方向的位置偏差量，v是当前的速度，θ1是当前的方向角度，
Figure GDA0001977766360000167
是第一速度，
Figure GDA0001977766360000168
是第一方向角度，C1(v)为科氏矩阵；D1(v)为水动力阻尼矩阵。
可选地，在一个实施例中，根据第二方向角度确定控制力矩，包括：
计算当前的方向角度与第二方向角度的角度偏差量；
根据第二方向角度和角度偏差量确定控制力矩。
在另一个可能的实现中，该控制***还包括第二滤波组件740；
确定单元720用于根据第二方向角度和角度偏差量计算第二状态信号，第二状态信号携带第一角速度；
第二滤波组件740对第二状态信号进行处理得到第二角速度；
确定单元720根据当前的角速度与第二角速度的角速度偏差量，以及第二角速度确定控制力矩。
进一步，在一个实施例中，第二速度与控制力矩满足以下关系：
Figure GDA0001977766360000171
其中，τ是控制力矩，I是转动惯量，k2θ是第二增益值，S2θ是角速度偏差量，θ2d是第二角速度，S1θ是角度偏差量，ω为航行器角速度，C2(ω)为科氏矩阵；D2(v)为水动力阻尼矩阵。
该***700可以实现图2和图3中的方法/步骤，且能达到相同的技术效果，为简洁描述，在此不再赘述。
需要说明的是，在一个实施例中，第一滤波组件和第二滤波组件可以是同一个滤波组件，也可以是独立的滤波组件，在该实施例中不作限定。
专业人员应该还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
结合本文中所公开的实施例描述的方法或算法的步骤可以用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器(RAM)、内存、只读存储器(ROM)、电可编程ROM、电可擦除可编程ROM、寄存器、硬盘、可移动磁盘、CD-ROM、或技术领域内所公知的任意其它形式的存储介质中。
以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种水下航行器轨迹的控制方法，其特征在于，所述控制方法包括：
接收目标信号，所述目标信号携带欲达到的目标位置信息；
根据当前的位置信息与所述目标位置信息确定第一状态信号，所述第一状态信号携带第一方向角度和第一速度；所述确定第一状态信号还包括，当所述水下航行器为欠驱动状况，根据所述欠驱动状况，确定所述第一状态信号中所述第一方向角度，使得所述第一方向角度作为控制量使所述水下航行器从所述当前的位置到达目标位置；所述欠驱动状况下,水下航行器轨迹的运动学以及动力学方程为：
Figure FDA0002621735490000011
Figure FDA0002621735490000012
其中,水下航行器的航向角为θ，位置为P＝[x y]T，控制输入为q＝[v ω]T，v和ω分别为航行器的线速度和角速度,m为水下航行器的质量，I为转动惯量；F为控制力，τ为控制力矩；C1(v)，C2(ω)为科氏矩阵；D1(v)，D2(v)为水动力阻尼矩阵,
Figure FDA0002621735490000013
为位置和姿势向量,设
Figure FDA0002621735490000014
利用第一滤波组件对所述第一状态信号进行处理得到第二方向角度和第二速度；所述第一滤波组件包括一阶低通滤波器；根据所述第二速度确定控制力，以使得水下航行器的航行速度在所述控制力作用下达到目标速度；
根据所述第二方向角度确定控制力矩，以使得所述水下航行器的航行方向角度在所述控制力矩的作用下达到目标方向角度。
2.根据权利要求1所述的方法，其特征在于，所述利用第一滤波组件对所述第一状态信号进行处理得到第二方向角度和第二速度，包括：
通过对所述第一滤波组件设定第一时间常数，并利用所述第一滤波组件对所述第一方向角度进行处理得到第二方向角度；
通过对所述第一滤波组件设定第二时间常数，并利用所述第一滤波组件对所述第一速度进行处理得到所述第二速度。
3.根据权利要求2所述的方法，其特征在于，所述第二方向角度与所述第一时间常数，以及所述第二速度与所述第二时间常数分别满足以下关系：
Figure FDA0002621735490000021
其中，τ1θ是第一时间常数，τ1v是第二时间常数；
Figure FDA0002621735490000022
是第一方向角度，
Figure FDA0002621735490000023
是第一速度；θ1d是第二方向角度，v1d是第二速度；
Figure FDA0002621735490000024
是
Figure FDA0002621735490000025
的零初始状态；θ1d(0)是θ1d的零初始状态；
Figure FDA0002621735490000026
是
Figure FDA0002621735490000027
的零初始状态；v1d(0)是v1d的零初始状态。
4.根据权利要求1所述的方法，其特征在于，所述根据所述第二速度确定控制力，包括：
根据所述当前的位置信息，所述目标位置信息，以及所述第二速度确定所述控制力。
5.根据权利要求4所述的方法，其特征在于，所述根据所述当前的位置信息，所述目标位置信息，以及所述第二速度确定所述控制力，包括：
计算所述当前的位置信息与所述目标位置信息的位置偏差量；
计算当前的速度与所述第二速度的速度偏差量；
根据所述位置偏差量和所述速度偏差量确定所述控制力。
6.根据权利要求4或5所述的方法，其特征在于，所述第二速度与所述控制力满足以下关系：
Figure FDA0002621735490000028
Figure FDA0002621735490000029
Figure FDA00026217354900000210
其中，F是控制力，m是水下航行器的质量，k1v是第一增益值，S1v是速度偏差量，v1d是第二速度，S1x是当前的位置信息和目标位置信息在x方向的位置偏差量，S1y是当前的位置信息和目标位置信息在y方向的位置偏差量，v是当前的速度，θ1是当前的方向角度，
Figure FDA00026217354900000211
是第一速度，
Figure FDA00026217354900000212
是第一方向角度，C1(v)为科氏矩阵；D1(v)为水动力阻尼矩阵。
7.根据权利要求1所述的方法，其特征在于，所述根据所述第二方向角度确定控制力矩，包括：
计算当前的方向角度与所述第二方向角度的角度偏差量；
根据所述第二方向角度和所述角度偏差量确定所述控制力矩。
8.根据权利要求7所述的方法，其特征在于，所述根据所述第二方向角度和所述角度偏差量确定所述控制力矩，包括：
根据所述第二方向角度和所述角度偏差量计算第二状态信号，所述第二状态信号携带第一角速度；
利用第二滤波组件对所述第二状态信号进行处理得到第二角速度；
根据当前的角速度与所述第二角速度的角速度偏差量，以及所述第二角速度确定所述控制力矩。
9.根据权利要求7或8所述的方法，其特征在于，所述第二速度与所述控制力矩满足以下关系：
Figure FDA0002621735490000031
其中，τ是控制力矩，I是转动惯量，k2θ是第二增益值，S2θ是角速度偏差量，θ2d是第二角速度，S1θ是角度偏差量，ω为水下航行器角速度，C2(ω)为科氏矩阵；D2(v)为水动力阻尼矩阵。
10.一种***，其特征在于，所述***包括权利要求1至权利要求9中所述的第一滤波组件和第二滤波组件，所述第一滤波组件用于对第一状态信号进行处理得到第二方向角度和第二速度，以使得根据第二速度确定目标速度，根据第二方向角度确定目标方向角度；所述***用于执行权利要求1至权利要求9所述的方法。
CN201811641485.7A 2018-12-29 2018-12-29 一种水下航行器轨迹的控制方法和*** Active CN109693774B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811641485.7A CN109693774B (zh) 2018-12-29 2018-12-29 一种水下航行器轨迹的控制方法和***

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811641485.7A CN109693774B (zh) 2018-12-29 2018-12-29 一种水下航行器轨迹的控制方法和***

Publications (2)

Publication Number Publication Date
CN109693774A CN109693774A (zh) 2019-04-30
CN109693774B true CN109693774B (zh) 2020-11-03

Family

ID=66233113

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811641485.7A Active CN109693774B (zh) 2018-12-29 2018-12-29 一种水下航行器轨迹的控制方法和***

Country Status (1)

Country Link
CN (1) CN109693774B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111781938B (zh) * 2020-06-23 2021-06-18 中国科学院声学研究所 欠驱动水下航行器及其镇定方法与装置
CN117553787B (zh) * 2024-01-09 2024-03-26 湖南大学无锡智能控制研究院 水下无人航行器的协同导航方法、装置及***

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB1133658A (en) * 1965-05-28 1968-11-13 Litton Industries Inc A method and apparatus for guiding a vehicle automatically and asymptotically into a predetermined path
JP2006313087A (ja) * 2005-05-06 2006-11-16 Mitsui Eng & Shipbuild Co Ltd 水中航走体の検出位置修正方法およびシステム
CN103576693A (zh) * 2013-11-11 2014-02-12 哈尔滨工程大学 基于二阶滤波器的水下机器人三维路径跟踪控制方法
CN103699007A (zh) * 2014-01-10 2014-04-02 大连海事大学 一种船舶动力定位***及其设计方法
CN104076821A (zh) * 2014-06-19 2014-10-01 哈尔滨工程大学 基于模糊自适应观测器的欠驱动水面艇轨迹跟踪控制***
CN105807789A (zh) * 2016-03-24 2016-07-27 哈尔滨工程大学 基于t-s模糊观测器补偿的uuv控制方法
CN106444806A (zh) * 2016-09-27 2017-02-22 哈尔滨工程大学 基于生物速度调节的欠驱动auv三维轨迹跟踪控制方法
CN106873628A (zh) * 2017-04-12 2017-06-20 北京理工大学 一种多无人机跟踪多机动目标的协同路径规划方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107168369B (zh) * 2017-06-13 2019-07-09 中国科学院声学研究所 一种欠驱动无人车的编队控制方法及***

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB1133658A (en) * 1965-05-28 1968-11-13 Litton Industries Inc A method and apparatus for guiding a vehicle automatically and asymptotically into a predetermined path
JP2006313087A (ja) * 2005-05-06 2006-11-16 Mitsui Eng & Shipbuild Co Ltd 水中航走体の検出位置修正方法およびシステム
CN103576693A (zh) * 2013-11-11 2014-02-12 哈尔滨工程大学 基于二阶滤波器的水下机器人三维路径跟踪控制方法
CN103699007A (zh) * 2014-01-10 2014-04-02 大连海事大学 一种船舶动力定位***及其设计方法
CN104076821A (zh) * 2014-06-19 2014-10-01 哈尔滨工程大学 基于模糊自适应观测器的欠驱动水面艇轨迹跟踪控制***
CN105807789A (zh) * 2016-03-24 2016-07-27 哈尔滨工程大学 基于t-s模糊观测器补偿的uuv控制方法
CN106444806A (zh) * 2016-09-27 2017-02-22 哈尔滨工程大学 基于生物速度调节的欠驱动auv三维轨迹跟踪控制方法
CN106873628A (zh) * 2017-04-12 2017-06-20 北京理工大学 一种多无人机跟踪多机动目标的协同路径规划方法

Also Published As

Publication number Publication date
CN109693774A (zh) 2019-04-30

Similar Documents

Publication Publication Date Title
Wang et al. Trajectory tracking control of a 6-DOF quadrotor UAV with input saturation via backstepping
CN106773713B (zh) 针对欠驱动海洋航行器的高精度非线性路径跟踪控制方法
CN108614426B (zh) 一种基于干扰观测器的多移动机器人编队鲁棒控制方法
CN106406325B (zh) 基于模糊扩张状态观测器的四旋翼无人机反馈线性化控制方法
CN105807789B (zh) 基于t-s模糊观测器补偿的uuv控制方法
CN108829123B (zh) 一种四旋翼飞行器控制方法、***和装置
CN111061286B (zh) 一种滤波微分提供阻尼实现飞行器侧向过载控制的方法
CN109693774B (zh) 一种水下航行器轨迹的控制方法和***
CN112631316B (zh) 变负载四旋翼无人机的有限时间控制方法
CN113110511B (zh) 一种基于广义模糊双曲模型的智能船舶航向控制方法
CN113467231A (zh) 基于侧滑补偿ilos制导律的无人艇路径跟踪方法
Duan et al. Attitude tracking control of small-scale unmanned helicopters using quaternion-based adaptive dynamic surface control
CN117389312B (zh) 一种基于模型的水下航行器抗横滚三维跟踪控制方法
CN113608541B (zh) 一种水下航行器姿态控制方法和***
Li et al. L1 adaptive structure-based nonlinear dynamic inversion control for aircraft with center of gravity variations
Cao et al. Robust flight control design using sensor-based backstepping control for unmanned aerial vehicles
CN112034869B (zh) 一种无人机变参神经动力学控制器的设计方法及其应用
Sharma et al. Finite time disturbance observer based geometric control of quadrotors
Tao et al. Control of a flexible air-breathing hypersonic vehicle with measurement noises using adaptive interval type-2 fuzzy logic system
CN116185024A (zh) 一种无人艇的艏向抗扰控制方法
Jensen et al. Basics and guidelines of complementary filters for small UAS navigation
CN112904719B (zh) 一种适用于水下机器人位置环形区域跟踪控制方法
Ma Two modified unscented Kalman filter and acceleration information in unmanned surface vehicle estimation
Peng et al. Vision based target tracking/following and estimation of target motion
Goslinski et al. Introduction of the flying robots into the human environment: An adaptive square-root unscented Kalman filter for a fault tolerant state estimation in a quadrotor

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant