一种基于改进卡尔曼滤波器的水下机器人姿态控制方法技术

本发明专利技术涉及水下机器人姿态控制的技术领域，公开了一种基于改进卡尔曼滤波器的水下机器人姿态控制方法。本发明专利技术的目的是为了解决桥墩附近出现乱流，小型水下机器人浮游作业受持续扰动影响导致姿态难以控制的问题。本方法首先从模糊PID出发，加入跟踪微分器平滑输入信号，减小系统反馈误差取值，扩大模糊PID适用范围；其次基于最小方差估计准则设计改进卡尔曼滤波器，利用模糊PID控制器离散输出数据对被控对象传感器测量值进行约束；最后为防止算法控制器故障，设计非线性函数并引入非线性组合及扩张观测器作为备选控制器。本发明专利技术所述的方法实现了在水下环境带来扰动不确定性及传感器存在测量误差情况下水下机器人姿态稳定控制。制。制。

【技术实现步骤摘要】
一种基于改进卡尔曼滤波器的水下机器人姿态控制方法


[0001]本专利技术涉及水下机器人姿态控制的
，尤其涉及一种基于改进卡尔曼滤波器的水下机器人姿态控制方法。

技术介绍

[0002]墩台基础是桥梁的主要部件，承担着桥梁上部结构传递的荷载，其质量直接关系到结构物的耐久性和桥梁的承载能力，对桥梁的安全运营至关重要，目前对于桥墩检测最安全可靠的方式为采用水下机器人搭载水下相机检测，对于桥梁检测小型水下机器人，在执行检测任务时，水下墩柱附近易出现乱流现象，流速可达到 4节以上，水下机器人极易受水流影响导致受力不均，因此，其水下姿态稳定控制是高效作业的前提。

技术实现思路

[0003]本专利技术公开一种基于改进卡尔曼滤波器的水下机器人姿态控制方法，使得水下机器人姿态控制在存在外部和内部时变扰动下仍能够快速收敛，稳定精准的追踪期望姿态。
[0004]本专利技术的上述任务目的是通过以下技术方案得以实现的：
[0005]一种基于改进卡尔曼滤波器的水下机器人姿态控制方法，包括：
[0006]S1：根据国际水池会议推荐的造船与轮机工程师学会术语公报的体系和刚体运动的牛顿
‑
欧拉方程，在小冲角情况下建立所述水下机器人垂直面俯仰角角速度模型；
[0007]S2：利用模糊逻辑并根据一定的模糊规则建立模糊PID；
[0008]S3：为保证S2所建立的模糊PID具有广泛的实用性，在系统输入后添加跟踪微分器，减小系统反馈误差取值；
[0009]S4：为保证水下机器人抗扰能力，基于最小方差估计准则设计改进卡尔曼滤波器，联合S2所述的模糊PID组成控制器1对水下机器人姿态进行控制；
[0010]S5：为防止S4所述的的控制器1故障导致水下机器人推进器等执行器负载极限提升，进而影响水下机器人正常运动轨迹，甚至造成设备损坏，设计非线性函数，进行利用该非线性函数设计非线性组合及扩张观测器作为控制器2；
[0011]S6：根据S4所设计的控制器1，引入外部和内部时变扰动以检验控制器1的快速稳定性；
[0012]S7：模拟S4所述的控制器1出现故障导致误差无限增大，验证系统故障补偿。
[0013]优选地，所述的模糊PID包括：
[0014]S8：确定输入隶属度函数，对模糊PID输入角速度偏差e及其偏差变化率ec进行模糊化；
[0015]S9：根据领域内专家及熟练操作人员的经验总结编写模糊控制规则表，以便模糊推理；
[0016]S10：确定输出隶属度函数，进行解模糊化。
[0017]优选地，所述的改进卡尔曼滤波器设计包括：
[0018]S11：利用S2所述的模糊PID控制器离散输出数据以线性动态方式融合估计数据，对水下机器人姿态观测数据进一步约束。
[0019]优选地，所述的改进卡尔曼滤波器的水下机器人姿态控制方法由控制器1和控制器2共同组成，控制器2作为备用控制器对控制器1故障时进行补偿。
[0020]有益效果
[0021]1.利用模糊PID控制器离散输出数据以线性动态方式融合卡尔曼滤波器估计数据，对水下机器人姿态观测数据进一步约束，提高了系统在对抗扰动时的稳定性；
[0022]2.在输入目标姿态后加入跟踪微分器平滑输入信号，减小了反馈误差取值，增强了模糊PID适用性；
[0023]3.为防止算法控制器故障，设计非线性函数设计非线性组合及扩张观测器作为备选控制器，提高了水下机器人姿态控制的稳定性。
附图说明
[0024]图1是本专利技术提供的水下机器人姿态控制方法结构图；
[0025]图2是本专利技术提供的水下机器人控制器1姿态控制方法数据流程图；
[0026]图3是本专利技术提供的所述方法在不同方差白噪声内部扰动下控制效果图及其局部放大；
[0027]图4是本专利技术提供的所述方法在多段阶跃拟合曲线下跟踪效果图；
[0028]图5是本专利技术提供的所述方法在时变不规则曲线下跟踪效果图；
[0029]图6是本专利技术提供的模拟控制器1出现故障效果图；
[0030]图7是本专利技术提供的所述方法故障补偿效果图。
具体实施方式
[0031]以下结合附图对本专利技术作进一步详细说明。
[0032]本专利技术提供一种基于改进卡尔曼滤波器的水下机器人姿态控制方法，其控制结构如图1所示，包括如下步骤：
[0033]S1：根据国际水池会议推荐的造船与轮机工程师学会术语公报的体系和刚体运动的牛顿
‑
欧拉方程，在小冲角情况下建立所述水下机器人垂直面俯仰角角速度模型；
[0034]S2：利用模糊逻辑并根据一定的模糊规则建立模糊PID；
[0035]S3：为保证S2所建立的模糊PID具有广泛的实用性，在系统输入后添加跟踪微分器，减小系统反馈误差取值；
[0036]S4：为保证水下机器人抗扰能力，基于最小方差估计准则设计改进卡尔曼滤波器，联合S2所述的模糊PID组成控制器1对水下机器人姿态进行控制；
[0037]S5：为防止S4所述的的控制器1故障导致水下机器人推进器等执行器负载极限提升，进而影响水下机器人正常运动轨迹，甚至造成设备损坏，设计非线性函数，进行利用该非线性函数设计非线性组合及扩张观测器作为控制器2；
[0038]S6：根据S4所设计的控制器1，引入外部和内部时变扰动以检验控制器1的快速稳定性；
[0039]S7：模拟S4所述的控制器1出现故障导致误差无限增大，验证系统故障补偿。
[0040]本专利技术提供一种基于改进卡尔曼滤波器的水下机器人姿态控制方法，进一步的，具体实施步骤如下：
[0041]S1：根据国际水池会议推荐的造船与轮机工程师学会术语公报的体系和刚体运动的牛顿
‑
欧拉方程，在小冲角情况下建立所述水下机器人垂直面俯仰角角速度模型可以表示如下：
[0042][0043]S2：利用模糊逻辑并根据一定的模糊规则建立模糊PID，实现对Kp、Ki、Kd三个参数分别进行实时优化具体步骤如下：
[0044]首先惯性元件会采集到机器人当前姿态信息，经过滤波处理之后会得到与预设定值的偏差e，以及当前偏差和上次偏差的变化(差值)ec两个值。
[0045]e(k)＝r1(k)
‑
x(k|k)
ꢀꢀꢀ
(2)
[0046]ec(k)＝r2(k)
‑
[e(k)
‑
e(k
‑
1)]/T
ꢀꢀꢀ
(3)
[0047]其次要对这两个值进行模糊化，选择描述输入输出的变量语言的模糊集为7个隶属度：
[0048]{NB,NM,NS,Z,PS,PM,PB}
ꢀꢀꢀ
(4)
[0049]选择模糊集合的论域为：
[0050]e,ec＝[
‑
3,
‑
2,
‑
1,0,1本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于改进卡尔曼滤波器的水下机器人姿态控制方法，其特征在于，包括：S1：根据国际水池会议推荐的造船与轮机工程师学会术语公报的体系和刚体运动的牛顿
‑
欧拉方程，在小冲角情况下建立所述水下机器人垂直面俯仰角角速度模型；S2：利用模糊逻辑并根据一定的模糊规则建立模糊PID；S3：为保证S2所建立的模糊PID具有广泛的实用性，在系统输入后添加跟踪微分器，减小系统反馈误差取值；S4：为保证水下机器人抗扰能力，基于最小方差估计准则设计改进卡尔曼滤波器，联合S2所述的模糊PID组成控制器1对水下机器人姿态进行控制；S5：为防止S4所述的控制器1故障导致水下机器人推进器等执行器负载极限提升，进而影响水下机器人正常运动轨迹，甚至造成设备损坏，设计非线性函数，进行利用该非线性函数设计非线性组合及扩张观测器作为控制器2；S6：根据S4所设计的控制器1，引入外部和内部时变扰动以检验控制器1的快速稳定性；S7：...

【专利技术属性】
技术研发人员：齐晓轩，陶九明，吴宝举，韩晓微，
申请(专利权)人：沈阳大学，
类型：发明
国别省市：