CN111679681B - 一种基于滑模鲁棒控制的水下机器人运动路径跟踪方法 - Google Patents

一种基于滑模鲁棒控制的水下机器人运动路径跟踪方法 Download PDF

Info

Publication number
CN111679681B
CN111679681B CN202010559434.0A CN202010559434A CN111679681B CN 111679681 B CN111679681 B CN 111679681B CN 202010559434 A CN202010559434 A CN 202010559434A CN 111679681 B CN111679681 B CN 111679681B
Authority
CN
China
Prior art keywords
data
target
underwater robot
linear
disturbance
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
CN202010559434.0A
Other languages
English (en)
Other versions
CN111679681A (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.)
Nanjing Institute of Technology
Original Assignee
Nanjing Institute of Technology
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 Nanjing Institute of Technology filed Critical Nanjing Institute of Technology
Priority to CN202010559434.0A priority Critical patent/CN111679681B/zh
Publication of CN111679681A publication Critical patent/CN111679681A/zh
Application granted granted Critical
Publication of CN111679681B publication Critical patent/CN111679681B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • 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/04—Control of altitude or depth
    • G05D1/06—Rate of change of altitude or depth
    • G05D1/0692—Rate of change of altitude or depth specially adapted for under-water vehicles

Landscapes

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

Abstract

本发明公开了一种基于滑模鲁棒控制的水下机器人运动路径跟踪方法，属于机器人技术领域，本发明包括将未知海流等外部有界干扰考虑在内，扩展了路径跟踪控制的最新技术，在给定目标姿态测量值的情况下，利用卡尔曼滤波器估计目标的线速度和角速度，采用滑模控制来实现对有界干扰的鲁棒性，通过使用干扰观测器计算估计值来直接补偿干扰，使得水下机器人更快地收敛到运动路径，解决了在水下机器人受到有界干扰和目标速度估计不准确的情况下，保证路径跟踪误差的渐近稳定性的技术问题，在目标速度和环境扰动有界估计误差的情况下，保证了对于水下机器人路径跟踪控制是全局渐近稳定的。

Description

一种基于滑模鲁棒控制的水下机器人运动路径跟踪方法
技术领域
本发明属于机器人技术领域，涉及一种基于滑模鲁棒控制的水下机器人运动路径跟踪方法。
背景技术
水下机器人路径跟踪通常是是在水下空间坐标系下，设定一条理想的几何路径，按照设定控制方法沿着预先指定的几何路径(相对于运动目标框架)进行运动控制，实现其跟踪运动。在水下护航、管道巡检、海底勘测、海洋生物跟踪等方面有着重要的研究意义。但是目前已有的水下机器人路径跟踪方法容易受到水下外部环境干扰，如海流、海底突变地形，严重影响水下机器人的运动。
目前水下机器人路径跟踪控制方面的局限性：
1.需要假设目标框架的姿态和速度是精确已知的；
2.没有考虑外部环境干扰的影响，例如海流、海底突变地形等，从而导致的现有的路径跟踪控制在存在上述干扰和不确定性的情况下表现出严重的性能损失。
发明内容
本发明的目的是提供一种基于滑模鲁棒控制的水下机器人运动路径跟踪方法，解决了在水下机器人受到有界干扰和目标速度估计不准确的情况下，保证路径跟踪误差的渐近稳定性的技术问题。
为实现上述目的，本发明采用如下技术方案：
一种基于滑模鲁棒控制的水下机器人运动路径跟踪方法，包括如下步骤：
步骤1：建立自主水下机器人导航系统、水下机器人控制系统和中心服务器集群，中心服务器集群通过Wi-Fi与自主水下机器人导航系统通信；
自主水下机器人导航系统用于采集水下信息、机器人姿态信息和惯导信息；
水下机器人控制系统用于进行水下机器人的姿态调节、重心调节和运动控制；
在中心服务器集群中建立目标数据采集模块、线性卡尔曼滤波器、滑模鲁棒控制模块、扰动观测器和路径控制模块；
目标数据采集模块用于定时采集自主水下机器人导航系统所传输过来的目标导航数据，目标导航数据包括水下信息、机器人姿态信息和惯导信息，所述惯导信息包括目标航向角数据、线性加速度数据、角加速度数据、采样时间和位置数据；
步骤2：目标数据采集模块定时将目标导航数据传送给线性卡尔曼滤波器，线性卡尔曼滤波器根据目标导航数据对目标进行建模，得到线性模型，其建模公式如下：
其中，ψt是航向角数据，at，αt是从高斯分布中提取的线性加速度和角加速度，分别具有零均值和零协方差 表示目标的线速度；
步骤3：线性卡尔曼滤波器采用零阶保持对线性模型进行离散，得到一个离散的线性时不变模型，离散的线性时不变模型的状态是目标在每个采样时间的位置数据、航向角数据、线性加速度数据和角加速度数据，测量输出是目标位置数据和方位角数据；
线性卡尔曼滤波器存储目标位置数据和方位角数据；
步骤4：线性卡尔曼滤波器根据以下步骤进行滤波：
步骤A1：在得到目标导航数据的瞬间，根据步骤3的方法得到目标位置数据和方位角数据，并对原有数据进行更新；
步骤A2：如果10s内没有收到目标导航数据，则认为pt固定在最后的位置，且测量噪声协方差选择为/>和/>
步骤4：滑模鲁棒控制模块对卡尔曼滤波器滤波后的数据通过粒子滤波鲁棒目标跟踪算法MPF进行处理，其公式为：
其中：
ev为位置误差；
uv控制变量；
d扰动变量；
S斜对称矩阵；
ωv角速度；
dω角速度扰动；
dv线速度扰动；
Δε常数矩阵；
Rv相对于机器人坐标系的旋转矩阵；
vv线速度；
Rt相对于惯性坐标系的旋转矩阵；
γ路径参数；
参考点的位置；
步骤5：扰动观测器对扰动信号进行处理，处理方式如下，当存在大幅度扰动的情况下，在减小控制输入抖振的同时，通过观测器来估计干扰，将估计结果用于控制器中，进行主动补偿扰动；
步骤6：路径控制模块根据步骤4和步骤5的结果，进行路径策划。
优选的，自主水下机器人导航系统通过水下通信系统与目标数据采集模块进行通信。
优选的，自主水下机器人导航系统通过水下机器人自载传感器采集水下信息、机器人姿态信息和惯导信息；
优选的，所述水下信息包括水质数据、流速数据和水底地形数据。
优选的，在执行步骤5时，其具体步骤如下：
步骤C1：建立平移扰动的扰动观测器
其中测量估计误差和水下机器人位置pv，对于正定增益K1，K2，证明估计误差/>在原点处渐近稳定；
步骤C2：建立旋转扰动dω：
其中估计误差定义为和，并且测量水下机器人平面方向ψ，同样，对于正增益Kω1，Kω2，证明估计误差/>在原点渐近稳定。
本发明所述的一种基于滑模鲁棒控制的水下机器人运动路径跟踪方法，解决了在水下机器人受到有界干扰和目标速度估计不准确的情况下，保证路径跟踪误差的渐近稳定性的技术问题，本放利用卡尔曼滤波器估计目标的线速度和角速度，采用滑模控制来实现对有界干扰的鲁棒性，通过使用干扰观测器计算估计值来直接补偿干扰，使得水下机器人更快地收敛到运动路径，在目标速度和环境扰动有界估计误差的情况下，保证了对于水下机器人路径跟踪控制是全局渐近稳定的，通过利用卡尔曼滤波器估计目标的线速度和角速度，采用滑模控制来实现对有界干扰的鲁棒性，通过使用干扰观测器计算估计值来直接补偿干扰，使得水下机器人更快地收敛到运动路径。
附图说明
图1是本发明的流程图；
图2是本发明的MPF速度误差的曲线图；
图3是本发明的线性速度vf的曲线图；
图4是本发明的角速度ωv的曲线图；
图5是本发明的误差修正项gerr的曲线图。
具体实施方式
如图1-图5所示的一种基于滑模鲁棒控制的水下机器人运动路径跟踪方法，包括如下步骤：
步骤1：建立自主水下机器人导航系统、水下机器人控制系统和中心服务器集群，中心服务器集群通过Wi-Fi与自主水下机器人导航系统通信；
自主水下机器人导航系统用于采集水下信息、机器人姿态信息和惯导信息；
水下机器人控制系统用于进行水下机器人的姿态调节、重心调节和运动控制；
在中心服务器集群中建立目标数据采集模块、线性卡尔曼滤波器、滑模鲁棒控制模块、扰动观测器和路径控制模块；
目标数据采集模块用于定时采集自主水下机器人导航系统所传输过来的目标导航数据，目标导航数据包括水下信息、机器人姿态信息和惯导信息，所述惯导信息包括目标航向角数据、线性加速度数据、角加速度数据、采样时间和位置数据；
步骤2：目标数据采集模块定时将目标导航数据传送给线性卡尔曼滤波器，卡尔曼滤波器将数据通过预测/更新方法进行处理，每次采样瞬间，计算目标姿态和速度的新预测，并在从目标机器人接收到包含位置/角度测量的更新惯导信息时计算更新步骤。如果没有收到来自目标机器人的数据超过10s，则认为目标机器人pt固定在最后接收的位置，线性卡尔曼滤波器根据目标导航数据对目标进行建模，得到线性模型，其建模公式如下：
其中，ψt是航向角数据，at，αt是从高斯分布中提取的线性加速度和角加速度，分别具有零均值和零协方差 表示目标的线速度；
步骤3：线性卡尔曼滤波器采用零阶保持对线性模型进行离散，得到一个离散的线性时不变模型，离散的线性时不变模型的状态是目标在每个采样时间的位置数据、航向角数据、线性加速度数据和角加速度数据，测量输出是目标位置数据和方位角数据；
线性卡尔曼滤波器存储目标位置数据和方位角数据；
卡尔曼滤波器负责实现对目标速度的精确估计的工作。
步骤4：线性卡尔曼滤波器根据以下步骤进行滤波：
步骤A1：在得到目标导航数据的瞬间，根据步骤3的方法得到目标位置数据和方位角数据，并对原有数据进行更新；
步骤A2：如果10s内没有收到目标导航数据，则认为pt固定在最后的位置，且测量噪声协方差选择为/>和/>
步骤4：滑模鲁棒控制模块对卡尔曼滤波器滤波后的数据通过粒子滤波鲁棒目标跟踪算法MPF进行处理，在水下机器人受到有界干扰和目标速度估计不准确的情况下，保证了水下机器人位置误差的渐近稳定性，其公式为：
其中：
ev为位置误差；
uv控制变量；
d扰动变量；
S斜对称矩阵；
ωv角速度；
dω角速度扰动；
dv线速度扰动；
Δε常数矩阵；
Rv相对于机器人坐标系的旋转矩阵；
vv线速度；
Rt相对于惯性坐标系的旋转矩阵；
γ路径参数；
参考点的位置；
Δε是一个常数矩阵，其形式为
或者/>
分别适用于平面(n＝2)和三维(n＝3)情况，请注意，始终可以选择，使得是满秩；
向量d是总扰动，在平面情况下，由
步骤5：扰动观测器对扰动信号进行处理，处理方式如下，当存在大幅度扰动的情况下，在减小控制输入抖振的同时，通过观测器来估计干扰，将估计结果用于控制器中，进行主动补偿扰动；
步骤6：路径控制模块根据步骤4和步骤5的结果，进行路径策划。
优选的，自主水下机器人导航系统通过水下通信系统与目标数据采集模块进行通信。
优选的，自主水下机器人导航系统通过水下机器人自载传感器采集水下信息、机器人姿态信息和惯导信息；
优选的，所述水下信息包括水质数据、流速数据和水底地形数据。
优选的，在执行步骤5时，其具体步骤如下：
步骤C1：建立平移扰动的扰动观测器
其中测量估计误差和水下机器人位置pv，对于正定增益K1，K2，证明估计误差/>在原点处渐近稳定；
步骤C2：建立旋转扰动dω：
其中估计误差定义为和，并且测量水下机器人平面方向ψ，同样，对于正增益Kω1，Kω2，证明估计误差/>在原点渐近稳定。
本实施例通过设计制作了一个小型自主水下机器人，在深水实验水池中进行实验，水下机器人比较容易进行信号发送、水下操作和回收作业，自主水下机器人包括通信、导航、控制、操作所需的硬件和软件，一台模拟水下机器人被作为目标，而自主水下机器人则被用于围绕目标执行路径跟踪任务；
目标水下机器人在逆时针方向以60米为半径进行游荡机动，并通过Wi-Fi将其位置(根据GPS测量值计算)连续传输给自主水下机器人，自主水下机器人执行路径跟踪控制方法。水下机器人的理想路径是一个以目标为中心的半径为25米的圆，可以表示为
式中R＝20m。对于路径误差ev的构造，使用了值ε＝[1 0]T。控制器增益矩阵选择为Kp＝diag(0.2,0.2)。误差修正项的增益取Kη＝4，虚点的期望名义速度为
控制信号保持在其驱动限值内，从而导致路径跟踪误差中的实际滑动模式行为。对于路径跟踪方案，路径跟踪误差的范数较小。滑动过程中的动力学(ev＝0)对以ρ为界的扰动是不变的，误差修正项gerr控制水下机器人沿虚点运动路径行进，以使水下机器人更快地收敛。一旦路径跟踪误差开始增大，gerr会减缓γ的演化，有效地等待水下机器人能够沿着路径行驶。
本发明所述的一种基于滑模鲁棒控制的水下机器人运动路径跟踪方法，解决了在水下机器人受到有界干扰和目标速度估计不准确的情况下，保证路径跟踪误差的渐近稳定性的技术问题，本放利用卡尔曼滤波器估计目标的线速度和角速度，采用滑模控制来实现对有界干扰的鲁棒性，通过使用干扰观测器计算估计值来直接补偿干扰，使得水下机器人更快地收敛到运动路径，在目标速度和环境扰动有界估计误差的情况下，保证了对于水下机器人路径跟踪控制是全局渐近稳定的，通过利用卡尔曼滤波器估计目标的线速度和角速度，采用滑模控制来实现对有界干扰的鲁棒性，通过使用干扰观测器计算估计值来直接补偿干扰，使得水下机器人更快地收敛到运动路径。

Claims (4)

1.一种基于滑模鲁棒控制的水下机器人运动路径跟踪方法，其特征在于：包括如下步骤：
步骤1：建立自主水下机器人导航系统、水下机器人控制系统和中心服务器集群，中心服务器集群通过Wi-Fi与自主水下机器人导航系统通信；
自主水下机器人导航系统用于采集水下信息、机器人姿态信息和惯导信息；
水下机器人控制系统用于进行水下机器人的姿态调节、重心调节和运动控制；
在中心服务器集群中建立目标数据采集模块、线性卡尔曼滤波器、滑模鲁棒控制模块、扰动观测器和路径控制模块；
目标数据采集模块用于定时采集自主水下机器人导航系统所传输过来的目标导航数据，目标导航数据包括水下信息、机器人姿态信息和惯导信息，所述惯导信息包括目标航向角数据、线性加速度数据、角加速度数据、采样时间和位置数据；
步骤2：目标数据采集模块定时将目标导航数据传送给线性卡尔曼滤波器，线性卡尔曼滤波器根据目标导航数据对目标进行建模，得到线性模型，其建模公式如下：
其中，ψt是航向角数据，at，αt是从高斯分布中提取的线性加速度和角加速度，分别具有零均值和零协方差表示目标的线速度；
步骤3：线性卡尔曼滤波器采用零阶保持对线性模型进行离散，得到一个离散的线性时不变模型，离散的线性时不变模型的状态是目标在每个采样时间的位置数据、航向角数据、线性加速度数据和角加速度数据，测量输出是目标位置数据和方位角数据；
线性卡尔曼滤波器存储目标位置数据和方位角数据；
步骤4：线性卡尔曼滤波器根据以下步骤进行滤波：
步骤A1：在得到目标导航数据的瞬间，根据步骤3的方法得到目标位置数据和方位角数据，并对原有数据进行更新；
步骤A2：如果10s内没有收到目标导航数据，则认为Pt固定在最后的位置，且测量噪声协方差选择为/>和/>
步骤4：滑模鲁棒控制模块对卡尔曼滤波器滤波后的数据通过粒子滤波鲁棒目标跟踪算法MPF进行处理，其公式为：
其中：
ev为位置误差；
uv控制变量；
d扰动变量；
S斜对称矩阵；
ωv角速度；
dω角速度扰动；
dv线速度扰动；
Δε常数矩阵；
Rv相对于机器人坐标系的旋转矩阵；
vv线速度；
Rt相对于惯性坐标系的旋转矩阵；
γ路径参数；
参考点的位置；
步骤5：扰动观测器对扰动信号进行处理，处理方式如下，当存在大幅度扰动的情况下，在减小控制输入抖振的同时，通过观测器来估计干扰，将估计结果用于控制器中，进行主动补偿扰动；
在执行步骤5时，其具体步骤如下：
步骤C1：建立平移扰动的扰动观测器
其中测量估计误差和水下机器人位置pv，对于正定增益K1，K2，证明估计误差/>在原点处渐近稳定；
步骤C2：建立旋转扰动dω：
其中估计误差定义为和，并且测量水下机器人平面方向ψ，同样，对于正增益Kω1，Kω2，证明估计误差/>在原点渐近稳定；
步骤6：路径控制模块根据步骤4和步骤5的结果，进行路径策划。
2.如权利要求1所述的一种基于滑模鲁棒控制的水下机器人运动路径跟踪方法，其特征在于：自主水下机器人导航系统通过水下通信系统与目标数据采集模块进行通信。
3.如权利要求1所述的一种基于滑模鲁棒控制的水下机器人运动路径跟踪方法，其特征在于：自主水下机器人导航系统通过水下机器人自载传感器采集水下信息、机器人姿态信息和惯导信息。
4.如权利要求1所述的一种基于滑模鲁棒控制的水下机器人运动路径跟踪方法，其特征在于：所述水下信息包括水质数据、流速数据和水底地形数据。
CN202010559434.0A 2020-06-18 2020-06-18 一种基于滑模鲁棒控制的水下机器人运动路径跟踪方法 Active CN111679681B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010559434.0A CN111679681B (zh) 2020-06-18 2020-06-18 一种基于滑模鲁棒控制的水下机器人运动路径跟踪方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010559434.0A CN111679681B (zh) 2020-06-18 2020-06-18 一种基于滑模鲁棒控制的水下机器人运动路径跟踪方法

Publications (2)

Publication Number Publication Date
CN111679681A CN111679681A (zh) 2020-09-18
CN111679681B true CN111679681B (zh) 2023-11-03

Family

ID=72436480

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010559434.0A Active CN111679681B (zh) 2020-06-18 2020-06-18 一种基于滑模鲁棒控制的水下机器人运动路径跟踪方法

Country Status (1)

Country Link
CN (1) CN111679681B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112286227B (zh) * 2020-11-03 2022-03-18 西北工业大学 基于微分跟踪器和扰动观测器的水下机器人轨迹控制方法及系统
CN114019797B (zh) * 2021-10-29 2023-12-22 湖北工业大学 无时滞名义模型下污水处理设备臭氧浓度滑模抗扰控制方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106292287A (zh) * 2016-09-20 2017-01-04 哈尔滨工程大学 一种基于自适应滑模控制的uuv路径跟踪方法
CN107957727A (zh) * 2016-10-17 2018-04-24 江苏舾普泰克自动化科技有限公司 水下机器人控制系统及动力定位方法
CN108594716A (zh) * 2018-05-25 2018-09-28 福州大学 一种浅水级超小型rov控制系统及控制方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106292287A (zh) * 2016-09-20 2017-01-04 哈尔滨工程大学 一种基于自适应滑模控制的uuv路径跟踪方法
CN107957727A (zh) * 2016-10-17 2018-04-24 江苏舾普泰克自动化科技有限公司 水下机器人控制系统及动力定位方法
CN108594716A (zh) * 2018-05-25 2018-09-28 福州大学 一种浅水级超小型rov控制系统及控制方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
"基于深度学习的单目视觉水下机器人目标跟踪方法研究";陈国军等;《机床与液压》;20191231;全文 *

Also Published As

Publication number Publication date
CN111679681A (zh) 2020-09-18

Similar Documents

Publication Publication Date Title
WO2020253854A1 (zh) 移动机器人姿态角解算方法
CN111024064B (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN107168312B (zh) 一种补偿uuv运动学和动力学干扰的空间轨迹跟踪控制方法
CN107957727B (zh) 水下机器人控制系统及动力定位方法
CN109857124B (zh) 基于速度与航向双重制导的无人船精确路径跟踪控制方法
EP2856273B1 (en) Pose estimation
CN109141475B (zh) 一种dvl辅助sins鲁棒行进间初始对准方法
CN111679681B (zh) 一种基于滑模鲁棒控制的水下机器人运动路径跟踪方法
CN112068440B (zh) 基于模型预测控制的auv回收对接动力定位控制方法
CN116147624B (zh) 一种基于低成本mems航姿参考系统的船舶运动姿态解算方法
CN111024074A (zh) 一种基于递推最小二乘参数辨识的惯导速度误差确定方法
CN113467231A (zh) 基于侧滑补偿ilos制导律的无人艇路径跟踪方法
CN111307114B (zh) 基于运动参考单元的水面舰船水平姿态测量方法
CN114964222A (zh) 一种车载imu姿态初始化方法、安装角估计方法及装置
CN114967714A (zh) 一种自主式水下机器人抗扰运动控制方法及系统
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
Forte et al. Reference tracking of a nonholonomic mobile robot using sensor fusion techniques and linear control
CN115993089B (zh) 基于pl-icp的在线四舵轮agv内外参标定方法
CN114252073B (zh) 一种机器人姿态数据融合方法
CN116338719A (zh) 基于b样条函数的激光雷达-惯性-车辆融合定位方法
CN116820081A (zh) 基于无模型自适应滑模的欠驱动无人艇路径跟踪控制方法
CN114111840A (zh) 一种基于组合导航的dvl误差参数在线标定方法
CN111025908B (zh) 基于自适应机动加速度扩展卡尔曼滤波器的航姿参考系统
CN112945245A (zh) 基于条件数理论在多auv协同导航系统中可观测性分析方法
CN114594675B (zh) 一种改进型pid的四旋翼飞行器控制系统及方法

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