CN111913489A - 水下机器人多推进器耦合控制方法及系统 - Google Patents

水下机器人多推进器耦合控制方法及系统 Download PDF

Info

Publication number
CN111913489A
CN111913489A CN201910390207.7A CN201910390207A CN111913489A CN 111913489 A CN111913489 A CN 111913489A CN 201910390207 A CN201910390207 A CN 201910390207A CN 111913489 A CN111913489 A CN 111913489A
Authority
CN
China
Prior art keywords
propeller
underwater robot
coupling control
coordinate system
control method
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.)
Pending
Application number
CN201910390207.7A
Other languages
English (en)
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.)
OUBO INTELLIGENT TECHNOLOGY (SHANGHAI) Co.,Ltd.
Shanghai Electric Wind Power Group Co Ltd
Original Assignee
Oubo Intelligent Technology Shanghai Co ltd
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 Oubo Intelligent Technology Shanghai Co ltd filed Critical Oubo Intelligent Technology Shanghai Co ltd
Priority to CN201910390207.7A priority Critical patent/CN111913489A/zh
Publication of CN111913489A publication Critical patent/CN111913489A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
    • G05D1/0875—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted to water vehicles

Abstract

本发明公开了一种水下机器人多推进器耦合控制方法，包括以下步骤：检测推进器状态并获得正常工作的推进器信息；通过姿态传感器获得水下机器人的姿态数据；通过耦合控制模块结合姿态数据和正常工作的推进器信息计算出水下机器人自稳状态的控制输出；将控制输出发送给推进器执行；还公开了一种水下机器人多推进器耦合控制系统，可以使水下机器人在某一个或者多个推进器出现异常的情况下正常工作，大大的提高了水下机器人的可靠性和安全性。

Description

水下机器人多推进器耦合控制方法及系统
技术领域
本发明涉及水下机器人控制技术领域，尤其涉及一种搭载不少于六个推进器的水下机器人多推进器耦合控制方法及系统。
背景技术
水下机器人无论是在军事，还是在人类生产生活中都有广泛应用，而对其良好的操作方法和运动控制是水下机器人顺利完成任务的重要前提和保障。然而，由于水下机器人在水中运动过程中，复杂多变的水域环境很可能会使水下机器人的某一个推进器处于非正常工作状态。目前水下机器人的多自由度姿态控制多采用多推进器解耦控制的方式来实现，如果某一个推进器出现异常工作，水下机器人姿态控制将会很差，甚至出现失控的情况。传统水下机器人对推进器的异常处理基本只有停机处理。
发明内容
鉴于目前存在的上述不足，本发明提供一种水下机器人多推进器耦合控制方法及系统，可以使水下机器人在某一个或者多个推进器出现异常的情况下正常工作，大大的提高了水下机器人的可靠性和安全性。
为达到上述目的，本发明的实施例采用如下技术方案：
一种水下机器人多推进器耦合控制方法，所述水下机器人多推进器耦合控制方法包括以下步骤：
检测推进器状态并获得正常工作的推进器信息；
通过姿态传感器获得水下机器人的姿态数据；
通过耦合控制模块结合姿态数据和正常工作的推进器信息计算出水下机器人自稳状态的控制输出；
将控制输出发送给推进器执行。
依照本发明的一个方面，所述姿态传感器包括电子罗盘、加速度计和陀螺仪，姿态传感器输出值为电子罗盘、加速度计、陀螺仪原始数据通过传感器融合算法融合后的值。
依照本发明的一个方面，所述通过姿态传感器获得水下机器人的姿态数据包括：姿态传感器三维角度输出值分别为航向角、俯仰角和横滚角；三维角速度输出值分别为航向角速度、俯仰角速度和横滚角速度；姿态传感器安装位置为X轴方向指向水下机器人的前进方向。
依照本发明的一个方面，所述耦合控制模块包括非线性PID控制算法、静止坐标系到运动坐标系的转换关系和推进器在坐标系的位置映射。
依照本发明的一个方面，所述非线性PID方向控制算法包括：利用相应的非线性化函数对PID控制器中的控制输入量进行非线性转换，构造出KP[e(t)]、KI[e(t)]、KD[e(t)]，得到非线性PID控制模型如式(1.1)所示：
Figure BDA0002056245300000021
其中：KP[e(t)]为非线性比例参数，KI[e(t)]为非线性积分参数，KD[e(t)]为非线性微分参数，e(t)为系统误差，u(t)为PID控制器的输出。
根据权利要求5所述的水下机器人多推进器耦合控制方法，其特征在于，所述非线性PID方向控制算法包括：
首先对式(1.1)进行离散化处理，用数字形式的差分方程代替连续系统的微分方程；
为了用数字形式的差分方程代替连续系统的微分方程，便于计算机实现，将微分项近似用求和及增量式表示：
Figure BDA0002056245300000022
将式(1.2)代入(1.1)，则可得到离散的PID表达式：
Figure BDA0002056245300000023
上式中，采样周期足够小才能保证系统具有一定精度；E(k)为第k次采样时的偏差值，E(k-1)为第(k-1)次采样时的偏差值，k＝0，1，2…；P(k)为第k次采样时的控制输出。
依照本发明的一个方面，所述静态坐标系到运动坐标系的转换关系包括：水下机器人姿态方向方面涉及两个坐标系，即运动坐标系相对于静止坐标系的取向；运动坐标系变换到静止坐标系为经过三个维度旋转得到的，从动坐标到静坐标的欧拉定律是绕Z-Y-X旋转的坐标变换；首先围绕Z轴旋转，然后围绕Y轴旋转，最后围绕X轴旋转，对应于偏航、俯仰、和横滚的旋转角度；速度矢量由线速度和角速度组成，由运动坐标系到静止坐标系的变换矩阵可以表示如下：
Figure BDA0002056245300000031
其中J1(vW)是线速度矢量的坐标变换关系，J2(wW)是角速度矢量的坐标变换关系；
线速度矢量的坐标变换关系如下：
Figure BDA0002056245300000032
角速度矢量的坐标变化关系如下：
Figure BDA0002056245300000033
六自由度下运动坐标系到静止坐标的坐标变换矩阵为：
Figure BDA0002056245300000034
其中C表示cos(.)，S表示sin(.)，T表示tan(.)，φ表示横滚角，θ表示俯仰角，ψ表示航向角。
依照本发明的一个方面，所述推进器在坐标系的位置映射关系包括：以六推进器水下机器人为例；六推进器可以在纵移、横移、升沉、横滚、俯仰和航向方向上提供推力/力矩，力和力矩矢量定义为：
τ＝LU
其中L是推进器位置映射矩阵：
Figure BDA0002056245300000041
U矩阵是推进器推力矩阵：
U＝[T1 T2 T3 T4 T5 T6]
射矩阵L是水下机器人电机相对于重心位置的位置关系，T1至T6分别表示水下机器人六个推进器。
依照本发明的一个方面，所述水下机器人多推进器耦合控制方法包括：如果某一个推进器工作异常，系统认为水下机器人为搭载五个推进器，推进器映射矩阵L将会发生相应的变化；当第N个推进器异常的工作情况下，推进器映射矩阵L将会变为删除第N行和第N列来形成新的推进器映射矩阵；这样在任何一个推进器工作异常的情况下，多推进器水下机器人都能保持正常工作。
依照本发明的一个方面，所述检测推进器状态并获得正常工作的推进器信息包括：电流检测和推进器转速检测；预测电流和推进器转速的关系如下：
I＝0.002604*V3-0.0102*V2+0.8016*V+0.05
其中I代表预测电流值，V代表电机当前转速值。
依照本发明的一个方面，所述水下机器人多推进器耦合控制方法包括以下步骤：将推进器的执行结果实时反馈给耦合控制模块。
一种水下机器人多推进器耦合控制系统，所述水下机器人多推进器耦合控制系统包括：硬件检测模块，姿态传感器模块，耦合控制模块，推进器系统；其中，所述硬件检测模块包括电流检测模块和推进器转速检测模块，所述姿态传感器模块包括括电子罗盘、加速度计和陀螺仪，所述耦合控制模块包括非线性PID控制器模块、推进器位置映射模块和动、静坐标系转换模块。
本发明实施的优点：本发明所述的水下机器人多推进器耦合控制方法包括以下步骤：检测推进器状态并获得正常工作的推进器信息；通过姿态传感器获得水下机器人的姿态数据；通过耦合控制模块结合姿态数据和正常工作的推进器信息计算出水下机器人自稳状态的控制输出；将控制输出发送给推进器执行；可以使水下机器人在某一个或者多个推进器出现异常的情况下正常工作，大大的提高了水下机器人的可靠性和安全性。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例一所述的一种水下机器人多推进器耦合控制方法示意图；
图2为本发明所述的推进器位置坐标系示意图；
图3为本发明所述的水下机器人多推进器耦合控制系统示意图；
图4为本发明实施例二所述的一种水下机器人多推进器耦合控制方法意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
实施例一
如图1和图2所示，一种水下机器人多推进器耦合控制方法，所述水下机器人多推进器耦合控制方法包括以下步骤：
步骤S1：检测推进器状态并获得正常工作的推进器信息；
硬件检测模块包括电流检测模块和推进器转速检测模块。电流检测模块采样周期为5ms，其输出值为近十个采样点的平均值，输出频率为20HZ。推进器转速检测模块的输出频率为20HZ。根据推进器的转速计算出当前转速下电流的预测值。预测电流和推进器转速的关系如下：
I＝0.002604*V3-0.0102*V2+0.8016*V+0.05
其中I代表预测电流值，V代表电机当前转速值。
预测值与电流检测模块输出误差在20％范围内表明推进器工作正常，预测值与电流检测模块连续50个周期的误差超过20％表明推进器工作异常。硬件检测模块对各个推进器实时的进行检测。
步骤S2：通过姿态传感器获得水下机器人的姿态数据；
所述姿态传感器包括电子罗盘、加速度计、陀螺仪。姿态传感器输出值为电子罗盘、加速度计、陀螺仪原始数据通过传感器融合算法融合后的值。姿态传感器三维角度输出值分别为：航向角、俯仰角、横滚角；三维角速度输出值分别为：航向角速度、俯仰角速度、横滚角速度。姿态传感器安装位置为X轴方向指向水下机器人的前进方向。
步骤S3：通过耦合控制模块结合姿态数据和正常工作的推进器信息计算出水下机器人自稳状态的控制输出；
在实际应用中，所述耦合控制模块包括非线性PID控制算法、静止坐标系到运动坐标系的转换关系和推进器在坐标系的位置映射。
在实际应用中，所述非线性PID方向控制算法包括：利用相应的非线性化函数对PID控制器中的控制输入量进行非线性转换，构造出KP[e(t)]、KI[e(t)]、KD[e(t)]，得到非线性PID控制模型如式(1.1)所示：
Figure BDA0002056245300000061
其中：KP[e(t)]为非线性比例参数，KI[e(t)]为非线性积分参数，KD[e(t)]为非线性微分参数，e(t)为系统误差，u(t)为PID控制器的输出。
根据权利要求5所述的水下机器人多推进器耦合控制方法，其特征在于，所述非线性PID方向控制算法包括：
首先对式(1.1)进行离散化处理，用数字形式的差分方程代替连续系统的微分方程；
为了用数字形式的差分方程代替连续系统的微分方程，便于计算机实现，将微分项近似用求和及增量式表示：
Figure BDA0002056245300000062
将式(1.2)代入(1.1)，则可得到离散的PID表达式：
Figure BDA0002056245300000063
上式中，采样周期足够小才能保证系统具有一定精度；E(k)为第k次采样时的偏差值，E(k-1)为第(k-1)次采样时的偏差值，k＝0，1，2…；P(k)为第k次采样时的控制输出。
在实际应用中，所述静态坐标系到运动坐标系的转换关系包括：水下机器人姿态方向方面涉及两个坐标系，即运动坐标系相对于静止坐标系的取向；运动坐标系变换到静止坐标系为经过三个维度旋转得到的，从动坐标到静坐标的欧拉定律是绕Z-Y-X旋转的坐标变换；首先围绕Z轴旋转，然后围绕Y轴旋转，最后围绕X轴旋转，对应于偏航、俯仰、和横滚的旋转角度；速度矢量由线速度和角速度组成，由运动坐标系到静止坐标系的变换矩阵可以表示如下：
Figure BDA0002056245300000071
其中J1(vW)是线速度矢量的坐标变换关系，J2(wW)是角速度矢量的坐标变换关系；
线速度矢量的坐标变换关系如下：
Figure BDA0002056245300000072
角速度矢量的坐标变化关系如下：
Figure BDA0002056245300000073
六自由度下运动坐标系到静止坐标的坐标变换矩阵为：
Figure BDA0002056245300000074
其中C表示cos(.)，S表示sin(.)，T表示tan(.)，φ表示横滚角，θ表示俯仰角，ψ表示航向角。
在实际应用中，所述推进器在坐标系的位置映射关系包括：以六推进器水下机器人为例；六推进器可以在纵移、横移、升沉、横滚、俯仰和航向方向上提供推力/力矩，力和力矩矢量定义为：
τ＝LU
其中L是推进器位置映射矩阵：
Figure BDA0002056245300000081
U矩阵是推进器推力矩阵：
U＝[T1 T2 T3 T4 T5 T6]
射矩阵L是水下机器人电机相对于重心位置的位置关系，T1至T6分别表示水下机器人六个推进器。
在实际应用中，所述水下机器人多推进器耦合控制方法包括：如果某一个推进器工作异常，系统认为水下机器人为搭载五个推进器，推进器映射矩阵L将会发生相应的变化；当第N个推进器异常的工作情况下，推进器映射矩阵L将会变为删除第N行和第N列来形成新的推进器映射矩阵；这样在任何一个推进器工作异常的情况下，多推进器水下机器人都能保持正常工作。
步骤S4：将控制输出发送给推进器执行。
实施例二
如图2和图4所示，一种水下机器人多推进器耦合控制方法，所述水下机器人多推进器耦合控制方法包括以下步骤：
步骤S1：检测推进器状态并获得正常工作的推进器信息；
硬件检测模块包括电流检测模块和推进器转速检测模块。电流检测模块采样周期为5ms，其输出值为近十个采样点的平均值，输出频率为20HZ。推进器转速检测模块的输出频率为20HZ。根据推进器的转速计算出当前转速下电流的预测值。预测电流和推进器转速的关系如下：
I＝0.002604*V3-0.0102*V2+0.8016*V+0.05
其中I代表预测电流值，V代表电机当前转速值。
预测值与电流检测模块输出误差在20％范围内表明推进器工作正常，预测值与电流检测模块连续50个周期的误差超过20％表明推进器工作异常。硬件检测模块对各个推进器实时的进行检测。
步骤S2：通过姿态传感器获得水下机器人的姿态数据；
所述姿态传感器包括电子罗盘、加速度计、陀螺仪。姿态传感器输出值为电子罗盘、加速度计、陀螺仪原始数据通过传感器融合算法融合后的值。姿态传感器三维角度输出值分别为：航向角、俯仰角、横滚角；三维角速度输出值分别为：航向角速度、俯仰角速度、横滚角速度。姿态传感器安装位置为X轴方向指向水下机器人的前进方向。
步骤S3：通过耦合控制模块结合姿态数据和正常工作的推进器信息计算出水下机器人自稳状态的控制输出；
在实际应用中，所述耦合控制模块包括非线性PID控制算法、静止坐标系到运动坐标系的转换关系和推进器在坐标系的位置映射。
在实际应用中，所述非线性PID方向控制算法包括：利用相应的非线性化函数对PID控制器中的控制输入量进行非线性转换，构造出KP[e(t)]、KI[e(t)]、KD[e(t)]，得到非线性PID控制模型如式(1.1)所示：
Figure BDA0002056245300000091
其中：KP[e(t)]为非线性比例参数，KI[e(t)]为非线性积分参数，KD[e(t)]为非线性微分参数，e(t)为系统误差，u(t)为PID控制器的输出。
根据权利要求5所述的水下机器人多推进器耦合控制方法，其特征在于，所述非线性PID方向控制算法包括：
首先对式(1.1)进行离散化处理，用数字形式的差分方程代替连续系统的微分方程；
为了用数字形式的差分方程代替连续系统的微分方程，便于计算机实现，将微分项近似用求和及增量式表示：
Figure BDA0002056245300000092
将式(1.2)代入(1.1)，则可得到离散的PID表达式：
Figure BDA0002056245300000093
上式中，采样周期足够小才能保证系统具有一定精度；E(k)为第k次采样时的偏差值，E(k-1)为第(k-1)次采样时的偏差值，k＝0，1，2…；P(k)为第k次采样时的控制输出。
在实际应用中，所述静态坐标系到运动坐标系的转换关系包括：水下机器人姿态方向方面涉及两个坐标系，即运动坐标系相对于静止坐标系的取向；运动坐标系变换到静止坐标系为经过三个维度旋转得到的，从动坐标到静坐标的欧拉定律是绕Z-Y-X旋转的坐标变换；首先围绕Z轴旋转，然后围绕Y轴旋转，最后围绕X轴旋转，对应于偏航、俯仰、和横滚的旋转角度；速度矢量由线速度和角速度组成，由运动坐标系到静止坐标系的变换矩阵可以表示如下：
Figure BDA0002056245300000101
其中J1(vW)是线速度矢量的坐标变换关系，J2(wW)是角速度矢量的坐标变换关系；
线速度矢量的坐标变换关系如下：
Figure BDA0002056245300000102
角速度矢量的坐标变化关系如下：
Figure BDA0002056245300000103
六自由度下运动坐标系到静止坐标的坐标变换矩阵为：
Figure BDA0002056245300000104
其中C表示cos(.)，S表示sin(.)，T表示tan(.)，φ表示横滚角，θ表示俯仰角，ψ表示航向角。
在实际应用中，所述推进器在坐标系的位置映射关系包括：以六推进器水下机器人为例；六推进器可以在纵移、横移、升沉、横滚、俯仰和航向方向上提供推力/力矩，力和力矩矢量定义为：
τ＝LU
其中L是推进器位置映射矩阵：
Figure BDA0002056245300000111
U矩阵是推进器推力矩阵：
U＝[T1 T2 T3 T4 T5 T6]
射矩阵L是水下机器人电机相对于重心位置的位置关系，T1至T6分别表示水下机器人六个推进器。
在实际应用中，所述水下机器人多推进器耦合控制方法包括：如果某一个推进器工作异常，系统认为水下机器人为搭载五个推进器，推进器映射矩阵L将会发生相应的变化；当第N个推进器异常的工作情况下，推进器映射矩阵L将会变为删除第N行和第N列来形成新的推进器映射矩阵；这样在任何一个推进器工作异常的情况下，多推进器水下机器人都能保持正常工作。
步骤S4：将控制输出发送给推进器执行。
步骤S5：将推进器的执行结果实时反馈给耦合控制模块。
实施例三
如图1、图2和图3所示，一种水下机器人多推进器耦合控制系统，所述水下机器人多推进器耦合控制系统包括：硬件检测模块，姿态传感器模块，耦合控制模块，推进器系统；其中，所述硬件检测模块包括电流检测模块和推进器转速检测模块，所述姿态传感器模块包括括电子罗盘、加速度计和陀螺仪，所述耦合控制模块包括非线性PID控制器模块、推进器位置映射模块和动、静坐标系转换模块。
具体包括如下实施方案：
首先，水下机器人通过硬件检测模块检测出正常工作的推进器；然后，结合姿态传感器数据和水下机器人耦合控制器，计算出水下机器人自稳状态的控制输出；最后，将控制输出发送给推进器，并将推进器的执行结果实时反馈给水下机器人耦合控制器。
硬件检测模块包括电流检测模块和推进器转速检测模块。电流检测模块采样周期为5ms，其输出值为近十个采样点的平均值，输出频率为20HZ。推进器转速检测模块的输出频率为20HZ。根据推进器的转速计算出当前转速下电流的预测值。预测电流和推进器转速的关系如下：
I＝0.002604*V3-0.0102*V2+0.8016*V+0.05
其中I代表预测电流值，V代表电机当前转速值。
预测值与电流检测模块输出误差在20％范围内表明推进器工作正常，预测值与电流检测模块连续50个周期的误差超过20％表明推进器工作异常。硬件检测模块对各个推进器实时的进行检测。
所述姿态传感器包括电子罗盘、加速度计、陀螺仪。姿态传感器输出值为电子罗盘、加速度计、陀螺仪原始数据通过传感器融合算法融合后的值。姿态传感器三维角度输出值分别为：航向角、俯仰角、横滚角；三维角速度输出值分别为：航向角速度、俯仰角速度、横滚角速度。姿态传感器安装位置为X轴方向指向水下机器人的前进方向。
所述水下机器人耦合控制器包括非线性PID控制算法、静止坐标系到运动坐标系的转换关系和推进器在坐标系的位置映射
所述非线性PID方向控制算法，为利用相应的非线性化函数对PID控制器中的控制输入量进行非线性转换，构造出KP[e(t)]、KI[e(t)]、KD[e(t)]，得到非线性PID控制模型如式(1.1)所示：
Figure BDA0002056245300000121
其中：KP[e(t)]为非线性比例参数，KI[e(t)]为非线性积分参数，KD[e(t)]为非线性微分参数，e(t)为系统误差，u(t)为PID控制器的输出。
由于计算机控制是一种采样控制，它只能根据采样时刻的偏差值来计算控制量。因此，在计算机控制系统中，必须首先对式(1.1)进行离散化处理，用数字形式的差分方程代替连续系统的微分方程。
为了用数字形式的差分方程代替连续系统的微分方程，便于计算机实现，将微分项近似用求和及增量式表示：
Figure BDA0002056245300000122
将式(1.2)代入(1.1)，则可得到离散的PID表达式：
Figure BDA0002056245300000123
上式中，采样周期足够小才能保证系统具有一定精度；E(k)为第k次采样时的偏差值，E(k-1)为第(k-1)次采样时的偏差值，k＝0，1，2…；P(k)为第k次采样时的控制输出。
所述静态坐标系到运动坐标系的转换关系，水下机器人姿态方向方面涉及两个坐标系，即运动坐标系相对于静止坐标系的取向。运动坐标系变换到静止坐标系可以理解为经过三个维度旋转得到的，从动坐标到静坐标的欧拉定律是绕Z-Y-X旋转的坐标变换。首先围绕Z轴旋转，然后围绕Y轴旋转，最后围绕X轴旋转，对应于偏航、俯仰、和横滚的旋转角度.
速度矢量由线速度和角速度组成，由运动坐标系到静止坐标系的变换矩阵可以表示如下：
Figure BDA0002056245300000131
其中J1(vW)是线速度矢量的坐标变换关系，J2(wW)是角速度矢量的坐标变换关系。
线速度矢量的坐标变换关系如下：
Figure BDA0002056245300000132
角速度矢量的坐标变化关系如下：
Figure BDA0002056245300000133
六自由度下运动坐标系到静止坐标的坐标变换矩阵为：
Figure BDA0002056245300000134
其中C表示cos(.)，S表示sin(.)，T表示tan(.)。φ表示横滚角，θ表示俯仰角，ψ表示航向角。
所述推进器在坐标系的位置映射关系，以六推进器水下机器人为例进行说明。六推进器可以在纵移、横移、升沉、横滚、俯仰和航向方向上提供推力/力矩。力和力矩矢量定义为：
τ＝LU
其中L是推进器位置映射矩阵
Figure BDA0002056245300000135
Figure DA00020562453050015
U矩阵是推进器推力矩阵：
U＝[T1 T2 T3 T4 T5 T6]
映射矩阵L是水下机器人电机相对于重心位置的位置关系。T1至T6分别表示水下机器人六个推进器系统。电机推力位置与重心位置的相对位置如图1所示
所述的多推进器耦合控制系统中，如果某一个推进器工作异常，系统认为水下机器人为搭载五个推进器，推进器映射矩阵L将会发生相应的变化。当第N个推进器异常的工作情况下，推进器映射矩阵L将会变为删除第N行和第N列来形成新的推进器映射矩阵。这样在任何一个推进器工作异常的情况下，多推进器水下机器人都能保持正常工作。
本发明实施的优点：本发明所述的水下机器人多推进器耦合控制方法包括以下步骤：检测推进器状态并获得正常工作的推进器信息；通过姿态传感器获得水下机器人的姿态数据；通过耦合控制模块结合姿态数据和正常工作的推进器信息计算出水下机器人自稳状态的控制输出；将控制输出发送给推进器执行；可以使水下机器人在某一个或者多个推进器出现异常的情况下正常工作，大大的提高了水下机器人的可靠性和安全性。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本领域技术的技术人员在本发明公开的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以所述权利要求的保护范围为准。

Claims (12)

1.一种水下机器人多推进器耦合控制方法，其特征在于，所述水下机器人多推进器耦合控制方法包括以下步骤：
检测推进器状态并获得正常工作的推进器信息；
通过姿态传感器获得水下机器人的姿态数据；
通过耦合控制模块结合姿态数据和正常工作的推进器信息计算出水下机器人自稳状态的控制输出；
将控制输出发送给推进器执行。
2.根据权利要求1所述的水下机器人多推进器耦合控制方法，其特征在于，所述姿态传感器包括电子罗盘、加速度计和陀螺仪，姿态传感器输出值为电子罗盘、加速度计、陀螺仪原始数据通过传感器融合算法融合后的值。
3.根据权利要求2所述的水下机器人多推进器耦合控制方法，其特征在于，所述通过姿态传感器获得水下机器人的姿态数据包括：姿态传感器三维角度输出值分别为航向角、俯仰角和横滚角；三维角速度输出值分别为航向角速度、俯仰角速度和横滚角速度；姿态传感器安装位置为X轴方向指向水下机器人的前进方向。
4.根据权利要求1所述的水下机器人多推进器耦合控制方法，其特征在于，所述耦合控制模块包括非线性PID控制算法、静止坐标系到运动坐标系的转换关系和推进器在坐标系的位置映射。
5.根据权利要求4所述的水下机器人多推进器耦合控制方法，其特征在于，所述非线性PID方向控制算法包括：利用相应的非线性化函数对PID控制器中的控制输入量进行非线性转换，构造出KP[e(t)]、KI[e(t)]、KD[e(t)]，得到非线性PID控制模型如式(1.1)所示：
Figure FDA0002056245290000011
其中：KP[e(t)]为非线性比例参数，KI[e(t)]为非线性积分参数，KD[e(t)]为非线性微分参数，e(t)为系统误差，u(t)为PID控制器的输出。
6.根据权利要求5所述的水下机器人多推进器耦合控制方法，其特征在于，所述非线性PID方向控制算法包括：
首先对式(1.1)进行离散化处理，用数字形式的差分方程代替连续系统的微分方程；
为了用数字形式的差分方程代替连续系统的微分方程，便于计算机实现，将微分项近似用求和及增量式表示：
Figure FDA0002056245290000021
将式(1.2)代入(1.1)，则可得到离散的PID表达式：
Figure FDA0002056245290000022
上式中，采样周期足够小才能保证系统具有一定精度；E(k)为第k次采样时的偏差值，E(k-1)为第(k-1)次采样时的偏差值，k＝0，1，2…；P(k)为第k次采样时的控制输出。
7.根据权利要求4所述的水下机器人多推进器耦合控制方法，其特征在于，所述静态坐标系到运动坐标系的转换关系包括：水下机器人姿态方向方面涉及两个坐标系，即运动坐标系相对于静止坐标系的取向；运动坐标系变换到静止坐标系为经过三个维度旋转得到的，从动坐标到静坐标的欧拉定律是绕Z-Y-X旋转的坐标变换；首先围绕Z轴旋转，然后围绕Y轴旋转，最后围绕X轴旋转，对应于偏航、俯仰、和横滚的旋转角度；速度矢量由线速度和角速度组成，由运动坐标系到静止坐标系的变换矩阵可以表示如下：
Figure FDA0002056245290000023
其中J1(vW)是线速度矢量的坐标变换关系，J2(wW)是角速度矢量的坐标变换关系；
线速度矢量的坐标变换关系如下：
Figure FDA0002056245290000024
角速度矢量的坐标变化关系如下：
Figure FDA0002056245290000025
六自由度下运动坐标系到静止坐标的坐标变换矩阵为：
Figure FDA0002056245290000031
其中C表示cos(.)，S表示sin(.)，T表示tan(.)，φ表示横滚角，θ表示俯仰角，ψ表示航向角。
8.根据权利要求4所述的水下机器人多推进器耦合控制方法，其特征在于，所述推进器在坐标系的位置映射关系包括：以六推进器水下机器人为例；六推进器可以在纵移、横移、升沉、横滚、俯仰和航向方向上提供推力/力矩，力和力矩矢量定义为：
τ＝LU
其中L是推进器位置映射矩阵：
Figure FDA0002056245290000032
U矩阵是推进器推力矩阵：
U＝[T1 T2 T3 T4 T5 T6]
射矩阵L是水下机器人电机相对于重心位置的位置关系，T1至T6分别表示水下机器人六个推进器。
9.根据权利要求8所述的水下机器人多推进器耦合控制方法，其特征在于，所述水下机器人多推进器耦合控制方法包括：如果某一个推进器工作异常，系统认为水下机器人为搭载五个推进器，推进器映射矩阵L将会发生相应的变化；当第N个推进器异常的工作情况下，推进器映射矩阵L将会变为删除第N行和第N列来形成新的推进器映射矩阵；这样在任何一个推进器工作异常的情况下，多推进器水下机器人都能保持正常工作。
10.根据权利要求1至9之一所述的水下机器人多推进器耦合控制方法，其特征在于，所述检测推进器状态并获得正常工作的推进器信息包括：电流检测和推进器转速检测；预测电流和推进器转速的关系如下：
I＝0.002604*V3-0.0102*V2+0.8016*V+0.05
其中I代表预测电流值，V代表电机当前转速值。
11.根据权利要求10所述的水下机器人多推进器耦合控制方法，其特征在于，所述水下机器人多推进器耦合控制方法包括以下步骤：将推进器的执行结果实时反馈给耦合控制模块。
12.一种水下机器人多推进器耦合控制系统，其特征在于，所述水下机器人多推进器耦合控制系统包括：硬件检测模块，姿态传感器模块，耦合控制模块，推进器系统；其中，所述硬件检测模块包括电流检测模块和推进器转速检测模块，所述姿态传感器模块包括括电子罗盘、加速度计和陀螺仪，所述耦合控制模块包括非线性PID控制器模块、推进器位置映射模块和动、静坐标系转换模块。
CN201910390207.7A 2019-05-10 2019-05-10 水下机器人多推进器耦合控制方法及系统 Pending CN111913489A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910390207.7A CN111913489A (zh) 2019-05-10 2019-05-10 水下机器人多推进器耦合控制方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910390207.7A CN111913489A (zh) 2019-05-10 2019-05-10 水下机器人多推进器耦合控制方法及系统

Publications (1)

Publication Number Publication Date
CN111913489A true CN111913489A (zh) 2020-11-10

Family

ID=73242936

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910390207.7A Pending CN111913489A (zh) 2019-05-10 2019-05-10 水下机器人多推进器耦合控制方法及系统

Country Status (1)

Country Link
CN (1) CN111913489A (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113247221A (zh) * 2021-07-07 2021-08-13 深之蓝海洋科技股份有限公司 水下机器人及其镜头去污方法
CN113602462A (zh) * 2021-10-08 2021-11-05 南京工程学院 水下机器人及其水中高可视度情况下姿态与运动控制方法
CN114217521A (zh) * 2021-11-30 2022-03-22 中国科学院沈阳自动化研究所 基于推进器矢量布局的水下机器人的全姿态运动控制方法
CN114779712A (zh) * 2022-06-21 2022-07-22 中国海洋大学 一种水下机器人控制系统
CN116451508A (zh) * 2023-06-14 2023-07-18 天津水动力科技有限公司 一种基于磁悬浮深海推进器的动力学建模方法

Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2010143238A (ja) * 2008-12-16 2010-07-01 Hitachi-Ge Nuclear Energy Ltd 水中移動体の制御装置
CN102485591A (zh) * 2010-12-04 2012-06-06 中国科学院沈阳自动化研究所 一种微型水下机器人矢量布置推进器推力分配方法及装置
CN106428479A (zh) * 2015-08-06 2017-02-22 欧舶智能科技（上海）有限公司 一种无人遥控水下机器人及其控制方法
CN106773741A (zh) * 2017-03-02 2017-05-31 华南理工大学 一种无人船动力定位系统及方法
CN107065898A (zh) * 2016-12-06 2017-08-18 北京臻迪科技股份有限公司 一种水下无人船航行控制方法及系统
CN107168073A (zh) * 2017-07-20 2017-09-15 江苏科技大学 基于自适应pid控制的水下机器人推进器控制装置及其控制方法
JP2017206154A (ja) * 2016-05-19 2017-11-24 三菱重工業株式会社 水中航走体制御装置、水中航走システム、水中航走体制御方法及びプログラム
CN107499476A (zh) * 2017-08-21 2017-12-22 江苏科技大学 水下机器人控制系统及运动控制方法
CN107526369A (zh) * 2017-10-17 2017-12-29 西北工业大学 多推进器的遥控式水下机器人轨迹跟踪控制方法
CN107697248A (zh) * 2017-09-26 2018-02-16 哈尔滨航士科技发展有限公司 一种深海作业型机器人推进器的误差及参数修正方法及方法
CN207216304U (zh) * 2017-07-20 2018-04-10 江苏科技大学 基于自适应pid控制的水下机器人推进器控制装置
CN207752374U (zh) * 2017-12-15 2018-08-21 中国船舶重工集团公司七五0试验场 水下机器人运动控制装置

Patent Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2010143238A (ja) * 2008-12-16 2010-07-01 Hitachi-Ge Nuclear Energy Ltd 水中移動体の制御装置
CN102485591A (zh) * 2010-12-04 2012-06-06 中国科学院沈阳自动化研究所 一种微型水下机器人矢量布置推进器推力分配方法及装置
CN106428479A (zh) * 2015-08-06 2017-02-22 欧舶智能科技（上海）有限公司 一种无人遥控水下机器人及其控制方法
JP2017206154A (ja) * 2016-05-19 2017-11-24 三菱重工業株式会社 水中航走体制御装置、水中航走システム、水中航走体制御方法及びプログラム
CN107065898A (zh) * 2016-12-06 2017-08-18 北京臻迪科技股份有限公司 一种水下无人船航行控制方法及系统
CN106773741A (zh) * 2017-03-02 2017-05-31 华南理工大学 一种无人船动力定位系统及方法
CN107168073A (zh) * 2017-07-20 2017-09-15 江苏科技大学 基于自适应pid控制的水下机器人推进器控制装置及其控制方法
CN207216304U (zh) * 2017-07-20 2018-04-10 江苏科技大学 基于自适应pid控制的水下机器人推进器控制装置
CN107499476A (zh) * 2017-08-21 2017-12-22 江苏科技大学 水下机器人控制系统及运动控制方法
CN107697248A (zh) * 2017-09-26 2018-02-16 哈尔滨航士科技发展有限公司 一种深海作业型机器人推进器的误差及参数修正方法及方法
CN107526369A (zh) * 2017-10-17 2017-12-29 西北工业大学 多推进器的遥控式水下机器人轨迹跟踪控制方法
CN207752374U (zh) * 2017-12-15 2018-08-21 中国船舶重工集团公司七五0试验场 水下机器人运动控制装置

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
王永鼎: "水下机器人动力学分析与控制系统研究", 制造业自动化, vol. 40, no. 3, pages 142 - 149 *

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113247221A (zh) * 2021-07-07 2021-08-13 深之蓝海洋科技股份有限公司 水下机器人及其镜头去污方法
CN113602462A (zh) * 2021-10-08 2021-11-05 南京工程学院 水下机器人及其水中高可视度情况下姿态与运动控制方法
CN114217521A (zh) * 2021-11-30 2022-03-22 中国科学院沈阳自动化研究所 基于推进器矢量布局的水下机器人的全姿态运动控制方法
CN114217521B (zh) * 2021-11-30 2023-11-10 中国科学院沈阳自动化研究所 基于推进器矢量布局的水下机器人的全姿态运动控制方法
CN114779712A (zh) * 2022-06-21 2022-07-22 中国海洋大学 一种水下机器人控制系统
CN116451508A (zh) * 2023-06-14 2023-07-18 天津水动力科技有限公司 一种基于磁悬浮深海推进器的动力学建模方法
CN116451508B (zh) * 2023-06-14 2023-08-18 天津水动力科技有限公司 一种基于磁悬浮深海推进器的动力学建模方法

Similar Documents

Publication Publication Date Title
CN111913489A (zh) 水下机器人多推进器耦合控制方法及系统
Zhang et al. Robust nonlinear model predictive control based visual servoing of quadrotor UAVs
CN108073175B (zh) 基于虚拟艇自适应规划的欠驱动无人艇编队智能控制方法
CN107957727B (zh) 水下机器人控制系统及动力定位方法
CN108319138A (zh) 一种欠驱动无人艇的滑模-反步双回路轨迹跟踪控制方法
CN107263466B (zh) 空间机器人基于二次规划问题的基座无扰控制方法
Gao et al. Model predictive visual servoing of fully-actuated underwater vehicles with a sliding mode disturbance observer
CN111618861A (zh) 一种基于四轴结构的双随动智能手臂控制方法
Hashimoto et al. Visual servoing with nonlinear observer
Londhe et al. Robust proportional derivative (PD)-like fuzzy control designs for diving and steering planes control of an autonomous underwater vehicle
CN115533915A (zh) 一种不确定环境下空中作业机器人主动接触检测控制方法
Shi et al. Distributed circumnavigation control of autonomous underwater vehicles based on local information
CN114274147B (zh) 目标跟踪控制方法及装置、机械臂控制设备和存储介质
CN105563482A (zh) 用于工业机器人的末端执行器的旋转运动规划方法
Wang et al. Bounded UDE-based control for a SLAM equipped quadrotor with input constraints
JP2006106910A (ja) ジンバル装置
CN110647161A (zh) 基于状态预测补偿的欠驱动uuv水平面轨迹跟踪控制方法
CN113238572B (zh) 基于预设性能控制的预设时间四旋翼无人机姿态跟踪方法
Hamel et al. Visual servoing of a class of under-actuated dynamic rigid-body systems
CN110286688B (zh) 一种带有机械臂的水下四旋翼无人机控制方法
CN109946970B (zh) 一种基于观测器的非完整机器人速度信息的获取方法
Ye et al. A staged finite-time control strategy for formation of underactuated unmanned surface vehicles
CN115922684A (zh) 用于六轴机器人的奇异点处理方法、装置、设备及介质
Meng et al. Trajectory‐tracking control of an unmanned surface vehicle based on characteristic modelling approach: Implementation and field testing
Papachristos et al. Haptic feedback-based reactive navigation for aerial robots subject to localization failure

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
TA01 Transfer of patent application right

Effective date of registration: 20201221

Address after: Room 1622, 1256 and 1258 Wanrong Road, Jing'an District, Shanghai, 200436

Applicant after: OUBO INTELLIGENT TECHNOLOGY (SHANGHAI) Co.,Ltd.

Applicant after: Shanghai Electric Wind Power Group Co.,Ltd.

Address before: Room 1622, 1256 and 1258 Wanrong Road, Jing'an District, Shanghai, 200436

Applicant before: OUBO INTELLIGENT TECHNOLOGY (SHANGHAI) Co.,Ltd.

TA01 Transfer of patent application right
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination