一种水下自主式潜航器全自由度姿态控制方法和系统技术方案

本发明专利技术提供一种水下自主式潜航器全自由度姿态控制方法和系统，潜航器包括8个喷嘴，8个喷嘴为分别位于入水口的左右两侧的两对喷嘴对以及位于系统后部的两对喷嘴对；AUV中央处理器通过转换矩阵将地球固定坐标系下的坐标转换为机身固定框架坐标系下的坐标，并进一步地构建四个喷嘴对的动力学模型，然后通过构建转换矩阵的直接余弦矩阵，进而实时更新姿态传感器所采集到的潜航器的坐标信息，进而实现各种姿态动作的组合方式运动，具有真正意义上的全自由度运动能力，可很好的解决由于空间局限不能自由改变航线的情况，在浅水区域，具有更小的回转半径，极大的提高了设备的灵活性，可有效提高设备对各种区域的可达性，提高工作效率。效率。效率。

【技术实现步骤摘要】
一种水下自主式潜航器全自由度姿态控制方法和系统


[0001]本专利技术属于水下自主式潜航器
，具体涉及一种水下自主式潜航器全自由度姿态控制方法和系统。

技术介绍

[0002]目前，水下自主式水下潜航器(AUV)水下运动姿态调节主要包括以下几种：
[0003](1)通过尾部的转向舵机来控制姿态；
[0004](2)通过在两侧对称安装两个或多个螺旋桨推进器来调整姿态；
[0005](3)通过在AUV机身设置水平、垂直方向的一个或多个洞穿型孔位并内置旋桨式推进器来实现姿态方向控制；
[0006]现有技术当中的水下自主式潜航器(AUV)的缺点和不足如下：
[0007](1)旋桨式推进器体积较大，设备前后各方向安装多个推进器，会一定程度破坏设备的流体力学结构，增大设备的运行阻力；(2)推进器运行过程中会产生较大的浪花及噪声，在一定程度破坏了AUV设备的隐蔽性设计，造成安全问题；(3)采用尾部转向舵的方式，设备转弯半径较大，不利于高机动性精准控制需求；(4)螺旋桨式推进器在浅水处应用受限，不利于设备近岸工作等。

技术实现思路

[0008]本专利技术针对上述缺陷，提供一种具有真正意义上的全自由度运动能力，可很好的解决由于空间局限不能自由改变航线的情况，相对旋桨式推进器具有更高的控制能力和精确度；在浅水区域，本系统相比于螺旋桨式结构，具有更小的回转半径，极大的提高了设备的灵活性，对各种区域的可达性的水下自主式潜航器全自由度姿态控制方法和系统。
[0009]本专利技术提供如下技术方案：一种水下自主式潜航器全自由度姿态控制方法，所述潜航器包括8个喷嘴，所述8个喷嘴为分别位于入水口的左右两侧的两对喷嘴对以及位于系统后部的两对喷嘴对；入水口的左右两侧的两个喷嘴对为第一喷嘴对和第三喷嘴对，所述位于系统后部的两个喷嘴对：第二喷嘴对和第四喷嘴对；所述四个喷嘴对形成在空间上的四个方向的力以及在地球固定坐标系{E}下三个方向上的扭矩，四个方向的力组成合力U1，三个方向上的扭矩分别为U2、U3和U4；所述方法包括以下步骤：
[0010]S1：所述姿态传感器采集所述潜航器于地球固定坐标系{E}下的坐标ξ，ξ＝[x,y,z]T
且将所采集到的的坐标ξ传输至AUV中央处理器；
[0011]S2：所述AUV中央处理器将地球固定坐标系{E}下的坐标ξ通过转换矩阵R
T
转换为机身固定框架坐标系{Q}下的坐标η，η＝[φ,θ,ψ]T
且转换计算公式如下：
[0012]{Q}
T
＝R
T
×
{E}
T
；
[0013]S3：所述AUV中央处理器构建四个喷嘴对的动力学模型：
[0014][0015][0016]其中，Ω
d
是四个喷嘴对产生的总动力速度，J
r
是四个喷嘴对产生的动力惯性，所述I是四个喷嘴对的惯性矩；，E
z
为所述地球固定坐标系{E}中z轴的向量矩阵，定义为[0,0,1]T
；
[0017]S4：所述AUV中央处理器，通过构建转换矩阵R
T
的直接余弦矩阵并利用所述S3步骤的四个喷嘴对的动力学模型，进而实时更新所述姿态传感器所采集到的所述潜航器的坐标信息，并发出运动控制决策指令，本姿态控制系统驱动控制模块接收到所述运动控制决策指令后控制电磁阀控制指令输出，电磁阀开关执行动作实现左转、右转、俯冲、仰进、左平移、右平移、上平移、下平移、原地左右回转、上下回转、以及其他各种姿态动作的组合方式运动；
[0018]S5：当所述潜航器状态达到调整目标后，则所述AUV中央处理器发送停止命令，各方向喷嘴开关关闭，系统停止动力输出。
[0019]进一步地，所述转换矩阵R
T
的计算公式如下：
[0020][0021]进一步地，所述S3步骤中的惯性矩I为一个3
×
3的对角矩阵：
[0022][0023]其中I
xx
为所述地球固定坐标系{E}下的x轴的惯性矩元素，I
yy
为所述地球固定坐标系{E}下的y轴的惯性矩元素，I
zz
为所述地球固定坐标系{E}下的z轴的惯性矩元素。
[0024]进一步地，根据所述S3步骤中的动力学模型，四个喷嘴对的平动动力学模型和转动动力学模型：
[0025]平动动力学模型为：
[0026][0027][0028][0029]转动动力学模型为：
[0030][0031][0032][0033]其中，τ为与四个喷嘴对形成的科里奥利扭矩总和τ
c
、陀螺力矩τ
g
以及支撑四个喷嘴对的体框架扭矩τ
a
相关的扭矩参数。
[0034]进一步地，所述S4步骤包括以下步骤：
[0035]S41：对于小角度旋转，sinφ趋近于φ，sinθ趋近于θ，sinφ趋近于φ，通过忽略角的乘积，构建斜对称形式的直接余弦矩阵
[0036][0037]S42：采用S41步骤构建的直接余弦矩阵构建所述姿态传感器的更新直接余弦矩阵计算模型，进而实时更新所述姿态传感器所采集到的所述潜航器的坐标信息，所述更新直接余弦矩阵计算模型为：
[0038][0039]S43：所述AUV中央处理器获得了实时更新的所述潜航器的坐标信息后，发出运动控制决策指令，本姿态控制系统驱动控制模块接收到所述运动控制决策指令后控制电磁阀控制指令输出，电磁阀开关执行动作实现左转、右转、俯冲、仰进、左平移、右平移、上平移、下平移、原地左右回转、上下回转、以及各种姿态动作的组合方式运动。
[0040]进一步地，所述S4步骤中所述本姿态控制系统驱动控制模块还接收本姿态控制系统水泵压力检测模块得到的水泵压力信号反馈信息，所述本姿态控制系统水泵压力检测模块在所述潜航器初始化后通过驱动进水系统的压力反馈信号检测所述水泵是否欠压，如果欠压则驱动进水系统工作，对喷嘴进行供水；如果不欠压则向所述本姿态控制系统驱动控制模块发送喷嘴可以正常开启并工作的水泵压力信号反馈信息。
[0041]进一步地，所述喷嘴对包括向上位于向上或向下的垂直向喷嘴，以及向左或向右的水平向喷嘴每个喷嘴对形成空间上的合力；
[0042]第一喷嘴对形成第一合力，第二喷嘴对形成第二合力，第三喷嘴对形成的第三合力，第四喷嘴对形成第四合力；
[0043]第二合力与第四合力共同形成的合力与第一合力与第三合力形成的合力形成反扭矩。
[0044]进一步地，所述第一喷嘴对包括前部右喷嘴和前部上喷嘴，所述第二喷嘴对包括前部做喷嘴和前部下喷嘴，所述第三喷嘴对包括后部左喷嘴和后部下喷嘴，所述第四喷嘴对包括后部右喷嘴和后部上喷嘴。
[0045]进一步地，所述S4步骤中左转时，本姿态控制系统驱动控制模块控制前部右喷嘴和后部左喷嘴开启，其余喷嘴关闭；
[0046]右转时，本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种水下自主式潜航器全自由度姿态控制方法，其特征在于，所述潜航器包括8个喷嘴，所述8个喷嘴为分别位于入水口的左右两侧的两对喷嘴对以及位于系统后部的两对喷嘴对；入水口的左右两侧的两个喷嘴对为第一喷嘴对和第三喷嘴对，所述位于系统后部的两个喷嘴对：第二喷嘴对和第四喷嘴对；所述四个喷嘴对形成在空间上的四个方向的力以及在地球固定坐标系{E}下三个方向上的扭矩，四个方向的力组成合力U1，三个方向上的扭矩分别为U2、U3和U4；所述方法包括以下步骤：S1：所述姿态传感器采集所述潜航器于地球固定坐标系{E}下的坐标ξ，ξ＝[x，y，z]
T
且将所采集到的的坐标ξ传输至AUV中央处理器；S2：所述AUV中央处理器将地球固定坐标系{E}下的坐标ξ通过转换矩阵R
T
转换为机身固定框架坐标系{Q}下的坐标η，η＝[φ，θ，ψ]
T
且转换计算公式如下：{Q}
T
＝R
T
×
{E}
T
；S3：所述AUV中央处理器构建四个喷嘴对的动力学模型：S3：所述AUV中央处理器构建四个喷嘴对的动力学模型：其中，Ω
d
是四个喷嘴对产生的总动力速度，J
r
是四个喷嘴对产生的动力惯性，所述I是四个喷嘴对的惯性矩；，E
z
为所述地球固定坐标系{E}中z轴的向量矩阵，定义为[0，0，1]
T
；S4：所述AUV中央处理器，通过构建转换矩阵R
T
的直接余弦矩阵并利用所述S3步骤的四个喷嘴对的动力学模型，进而实时更新所述姿态传感器所采集到的所述潜航器的坐标信息，并发出运动控制决策指令，本姿态控制系统驱动控制模块接收到所述运动控制决策指令后控制电磁阀控制指令输出，电磁阀开关执行动作实现左转、右转、俯冲、仰进、左平移、右平移、上平移、下平移、原地左右回转、上下回转、以及其他各种姿态动作的组合方式运动；S5：当所述潜航器状态达到调整目标后，则所述AUV中央处理器发送停止命令，各方向喷嘴开关关闭，系统停止动力输出。2.根据权利要求1所述的一种水下自主式潜航器全自由度姿态控制方法，其特征在于，所述转换矩阵R
T
的计算公式如下：3.根据权利要求1所述的水下自主式潜航器全自由度姿态控制方法，其特征在于，所述S3步骤中的惯性矩I为一个3
×
3的对角矩阵：其中I
xx
为所述地球固定坐标系{E}下的x轴的惯性矩元素，I
yy
为所述地球固定坐标系{E}下的y轴的惯性矩元素，I
zz
为所述地球固定坐标系{E}下的z轴的惯性矩元素。4.根据权利要求3所述的水下自主式潜航器全自由度姿态控制方法，其特征在于，根据
所述S3步骤中的动力学模型，四个喷嘴对的平动动力学模型和转动动力学模型：平动动力学模型为：平动动力学模型为：平动动力学模型为：转动动力学模型为：转动动力学模型为：转动动力学模型为：其中，τ为与四个喷嘴对形成的科里奥利扭矩总和τ
c
、陀螺力矩τ
g
以及支撑四个喷嘴对的体框架扭矩τ
a
...

【专利技术属性】
技术研发人员：吕冰冰，陈路，梁尔冰，杨睿，陈新，
申请(专利权)人：湖南国天电子科技有限公司，
类型：发明
国别省市：