一种四旋翼水下航行器前进运动状态估计方法技术

本发明专利技术涉及水下航行器与机器人控制领域，旨在提供一种四旋翼水下航行器前进运动状态估计方法。包括：以仿真或实验方法建立四旋翼水下航行器的六自由度动力学与运动学模型；设定运动状态参数，对六自由度模型做简化处理，得到水下航行器的前进运动方程；设定水下航行器在前进运动中的控制律，确定控制量与状态量之间的对应关系；对水下航行器的受力情况与匀速前进运动的动力学模型进行分析，得到匀速前进时倾角与速度之间的函数关系；建立卡尔曼滤波方程，设定对应不同运动状态下的卡尔曼滤波器系数；设定上浮时间段，通过比较实际位置与状态估计位置之间的差别，对参数进行调整。本发明专利技术步骤简便，成本较低；稳定可靠、结果准确，应用范围广。应用范围广。应用范围广。

【技术实现步骤摘要】
一种四旋翼水下航行器前进运动状态估计方法


[0001]本专利技术涉及水下航行器与机器人控制领域，尤其涉及一种四旋翼水下航行器前进运动状态估计方法。

技术介绍

[0002]水下航行器是一种集成微电子、计算机、控制、能源、通信等高新技术的海洋智能装备。与传统的鱼雷形水下航行器相比，四旋翼水下航行器具有悬停性能好、能耗低、结构简单等优势，在水下侦查、探测等方面有极高的应用价值；但由于其特殊的运动模式，在控制系统的设计要求上也更为严格。具体表现为：1)欠驱动性。四旋翼水下航行器有四个独立控制的输入变量与六个运动自由度，其自由度个数大于控制输入个数，是一种典型的欠驱动系统。欠驱动特性可以降低系统的设计和制造难度，但会使控制设计更加复杂。2)强耦合。由动力学模型与运动学模型可知，控制输入力矩直接作用于水下航行器的姿态，当水下航行器姿态发生变化时，会导致转换矩阵发生变化，从而进一步改变水下航行器的位置。3)静不稳定。当控制输入等于零时，水下航行器无法保持在平衡点。4)非线性。水下航行器动力学方程不满足叠加性，是一种典型的非线性系统。
[0003]状态量的观测与估计是控制系统的重要组成部分之一。对于一般的传感系统来说，潜入水下后就无法通过GPS等定位系统得到位置数据，而如多普勒计程仪等水下专用的速度传感器又有着价格昂贵、体积与重量巨大等缺陷。虽然可以靠惯性传感器所得到的加速度积分来获取速度，并进一步估计位移，但仅依靠精度较低的惯性传感器所到的数据，对于控制设计要求更为复杂的四旋翼水下航行器来说无疑是不够准确的。
[0004]随着对海洋发展战略的重视与水下航行器技术的发展，目前迫切需要一种能够有效且便利的四旋翼水下航行器前进运动状态估计方法，以减少设计成本，提高航行器的工作效率。

技术实现思路

[0005]本专利技术要解决的技术问题是，克服现有技术中的不足，提供一种四旋翼水下航行器前进运动状态估计方法。
[0006]为解决技术问题，本专利技术所采取的技术方案是：
[0007]提供一种四旋翼水下航行器前进运动状态估计方法，包括以下步骤：
[0008](1)以仿真或实验方法建立四旋翼水下航行器的六自由度动力学与运动学模型；
[0009](2)设定运动状态参数，对六自由度模型做简化处理，得到水下航行器的前进运动方程；
[0010](3)设定水下航行器在前进运动中的控制律，确定控制量与状态量之间的对应关系；
[0011](4)对水下航行器的受力情况与匀速前进运动的动力学模型进行分析，得到匀速前进时倾角与速度之间的函数关系；
[0012](5)建立卡尔曼滤波方程，设定对应不同运动状态下的卡尔曼滤波器系数；
[0013](6)让水下航行器按设定时间段上浮，通过比较实际位置与状态估计位置之间的差别，对参数进行调整。
[0014]与现有技术相比，本专利技术的有益效果在于：
[0015]1、本专利技术步骤简便，成本较低。本专利技术针对水下无法使用GPS且速度传感器较为昂贵的问题，给出一种仅依靠角度与加速度传感器即可得到速度与位移的状态估计器。
[0016]2、本专利技术稳定可靠，结果准确。本专利技术同时参考角度与加速度的观测值，并能依据运动情况自适应调节卡尔曼系数，通过反复校准参数，最终能得到较为准确的结果。
[0017]3、本专利技术应用范围广。本专利技术不但可以应用于四旋翼水下航行器的前进运动，还可应用于依靠姿态变换实现前进或侧移运动的任意旋翼型机器人，包括但不限于水下与空中机器人。
附图说明
[0018]图1是本专利技术的整体流程示意图；
[0019]图2是十字形四旋翼水下航行器的坐标系图；
[0020]图3是水下航行器匀速前进运动中的受力分析图；
[0021]图4是本专利技术的状态估计流程示意图。
具体实施方式
[0022]首先需要说明的是，本专利技术涉及水下机器人技术、水动力建模技术、控制技术、导航技术和定位技术。申请人认为，如在仔细阅读申请文件、准确理解本专利技术的实现原理和专利技术目的以后，在结合现有公知技术的情况下，本领域技术人员完全可以运用其掌握的软件编程技能和算法修改实现本专利技术。在本专利技术的实现过程中，会涉及到多旋翼水下机器人的应用，水下机器人安装的旋翼推进器数量可在3～16个之间；前述水下机器人包括但不限于：水下机器人、水下航行器、潜水器、潜航器、深潜器、水下自走式航行器等；前述滤波方程包括但不限于：卡尔曼滤波、扩展卡尔曼滤波、粒子滤波、最小二乘滤波、无色卡尔曼滤波、无迹卡尔曼滤波、自适应卡尔曼滤波器、贝叶斯滤波等，凡本专利技术申请文件提及的均属此范畴，申请人不再一一列举。
[0023]本专利技术提供的四旋翼水下航行器前进运动状态估计方法，包括以下步骤：
[0024](1)以仿真或实验方法建立四旋翼水下航行器的六自由度动力学与运动学模型；
[0025]动力学与运动学模型的具体实现方式如下：
[0026](1.1)在水下航行器的运动中引入机体坐标系与大地坐标系；其中，机体坐标系与水下航行器本体绑定，原点为浮心，随着水下航行器运动而运动；大地坐标系则与惯性系固连，位置方向均不发生变化；
[0027]在机体坐标系中表示水下航行器的受力与运动情况，x、y、z轴上的线速度依次定义为u、v、w，角速度依次定义为p、q、r，则速度矩阵定义为V＝[u，v，w，p，q，r]T
，上标T代表矩阵的转置，下同；在地坐标系中表示其位置与姿态角，x、y、z轴上的位移依次定义为x、y、z，姿态角依次定义为θ、ψ，则位姿矩阵定义为
[0028](1.2)在六自由度动力学与运动学模型中，总的动力学方程表示为：
[0029][0030]其中，M
T
为总质量矩阵，为V的微分，即点标代表微分，下同；C(V)为总科里奥利力矩阵；D
L
为线性阻力系数矩阵，D
NL
为非线性阻力系数矩阵；G(η)为恢复力矩阵，F为输入力矩阵；
[0031]总质量矩阵M
T
由刚体质量矩阵M
RB
与附加质量矩阵M
A
构成，通过下式计算得到：
[0032]M
T
＝M
RB
+M
A
[0033][0034][0035]其中，m为质量；假设重心的x轴与y轴坐标均为0，z
g
为重心的z轴坐标，I
x
/I
y
/I
z
依次为xyz坐标轴上的转动惯量，依次为xyz轴平移自由度上的附加质量系数，依次为xyz轴旋转自由度上的附加质量系数，又称附加转动惯量系数；
[0036]类似地，总科里奥利力矩阵C(V)由C
RB
与C
A
组成，通过下式计算得到：
[0037]C(V)＝C
RB
+C
A
[0038][0039][0040]线性阻力系数矩阵D<本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种四旋翼水下航行器前进运动状态估计方法，其特征在于，包括以下步骤：(1)以仿真或实验方法建立四旋翼水下航行器的六自由度动力学与运动学模型；(2)设定运动状态参数，对六自由度模型做简化处理，得到水下航行器的前进运动方程；(3)设定水下航行器在前进运动中的控制律，确定控制量与状态量之间的对应关系；(4)对水下航行器的受力情况与匀速前进运动的动力学模型进行分析，得到匀速前进时倾角与速度之间的函数关系；(5)建立卡尔曼滤波方程，设定对应不同运动状态下的卡尔曼滤波器系数；(6)让水下航行器按设定时间段上浮，通过比较实际位置与状态估计位置之间的差别，对参数进行调整。2.根据权利要求1所述的方法，其特征在于，所述步骤(1)中，动力学与运动学模型的具体实现方式如下：(1.1)在水下航行器的运动中引入机体坐标系与大地坐标系；其中，机体坐标系与水下航行器本体绑定，原点为浮心，随着水下航行器运动而运动；大地坐标系则与惯性系固连，位置方向均不发生变化；在机体坐标系中表示水下航行器的受力与运动情况，x、y、z轴上的线速度依次定义为u、v、w，角速度依次定义为p、q、r，则速度矩阵定义为V＝[u，v，w，p，q，r]
T
，上标T代表矩阵的转置，下同；在地坐标系中表示其位置与姿态角，x、y、z轴上的位移依次定义为x、y、z，姿态角依次定义为θ、ψ，则位姿矩阵定义为(1.2)在六自由度动力学与运动学模型中，总的动力学方程表示为：其中，M
T
为总质量矩阵，为V的微分，即点标代表微分，下同；C(V)为总科里奥利力矩阵；D
L
为线性阻力系数矩阵，D
NL
为非线性阻力系数矩阵；G(η)为恢复力矩阵，F为输入力矩阵；总质量矩阵M
Y
由刚体质量矩阵M
RB
与附加质量矩阵M
A
构成，通过下式计算得到：M
T
＝M
RB
+M
AA
其中，m为质量；假设重心的x轴与y轴坐标均为0，z
g
为重心的z轴坐标，I
x
/I
y
/I
z
依次为xyz坐标轴上的转动惯量，依次为xyz轴平移自由度上的附加质量系数，依次为xyz轴旋转自由度上的附加质量系数，又称附加转动惯量系数；类似地，总科里奥利力矩阵C(V)由C
RB
与C
A
组成，通过下式计算得到：C(V)＝C
RB
+C
A
线性阻力系数矩阵D
L
和非线性阻力系数矩阵D
NL
，通过下式计算得到：D
L
＝diag[X
u
，Y
v
，Z
w
，K
p
，M
q
，N
r
]其中，X
u
/Y
v
/Z
w
依次为xyz轴平移自由度上的线性阻力系数，K
p
，M
q
，N
r
依次为xyz轴旋转自由度上的线性阻力系数；依次为xyz轴平移自由度上的非线性阻力系数，依次为xyz轴旋转自由度上的非线性阻力系数；恢复力矩阵G(η)，通过下式计算得到：其中，G为重力与B为浮力，s＝sin，c＝cos；输入力矩阵F，通过下式计算得到：其中，F
z
表示旋翼在垂直方向上的力，表示旋翼产生的横摇力矩，M
θ
表示旋翼产生的纵倾力矩，M
ψ
表示旋翼产生的俯仰力矩；(1.3)在六自由度动力学与运动学模型中，运动学方程表示为：(1.3)在六自由度...

【专利技术属性】
技术研发人员：冀大雄，王睿，
申请(专利权)人：东海实验室，
类型：发明
国别省市：