CN113602462A - 水下机器人及其水中高可视度情况下姿态与运动控制方法 - Google Patents

水下机器人及其水中高可视度情况下姿态与运动控制方法 Download PDF

Info

Publication number
CN113602462A
CN113602462A CN202110769162.1A CN202110769162A CN113602462A CN 113602462 A CN113602462 A CN 113602462A CN 202110769162 A CN202110769162 A CN 202110769162A CN 113602462 A CN113602462 A CN 113602462A
Authority
CN
China
Prior art keywords
robot
machine body
underwater
attitude
auv
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.)
Granted
Application number
CN202110769162.1A
Other languages
English (en)
Other versions
CN113602462B (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 CN202110769162.1A priority Critical patent/CN113602462B/zh
Publication of CN113602462A publication Critical patent/CN113602462A/zh
Application granted granted Critical
Publication of CN113602462B publication Critical patent/CN113602462B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B63—SHIPS OR OTHER WATERBORNE VESSELS; RELATED EQUIPMENT
    • B63G—OFFENSIVE OR DEFENSIVE ARRANGEMENTS ON VESSELS; MINE-LAYING; MINE-SWEEPING; SUBMARINES; AIRCRAFT CARRIERS
    • B63G8/00—Underwater vessels, e.g. submarines; Equipment specially adapted therefor
    • B63G8/14—Control of attitude or depth
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B63—SHIPS OR OTHER WATERBORNE VESSELS; RELATED EQUIPMENT
    • B63C—LAUNCHING, HAULING-OUT, OR DRY-DOCKING OF VESSELS; LIFE-SAVING IN WATER; EQUIPMENT FOR DWELLING OR WORKING UNDER WATER; MEANS FOR SALVAGING OR SEARCHING FOR UNDERWATER OBJECTS
    • B63C11/00—Equipment for dwelling or working underwater; Means for searching for underwater objects
    • B63C11/52—Tools specially adapted for working underwater, not otherwise provided for
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B63—SHIPS OR OTHER WATERBORNE VESSELS; RELATED EQUIPMENT
    • B63G—OFFENSIVE OR DEFENSIVE ARRANGEMENTS ON VESSELS; MINE-LAYING; MINE-SWEEPING; SUBMARINES; AIRCRAFT CARRIERS
    • B63G8/00—Underwater vessels, e.g. submarines; Equipment specially adapted therefor
    • B63G8/001—Underwater vessels adapted for special purposes, e.g. unmanned underwater vessels; Equipment specially adapted therefor, e.g. docking stations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/10—Geometric CAD
    • G06F30/15—Vehicle, aircraft or watercraft design
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/10—Geometric CAD
    • G06F30/17—Mechanical parametric or variational design
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B63—SHIPS OR OTHER WATERBORNE VESSELS; RELATED EQUIPMENT
    • B63G—OFFENSIVE OR DEFENSIVE ARRANGEMENTS ON VESSELS; MINE-LAYING; MINE-SWEEPING; SUBMARINES; AIRCRAFT CARRIERS
    • B63G8/00—Underwater vessels, e.g. submarines; Equipment specially adapted therefor
    • B63G8/001—Underwater vessels adapted for special purposes, e.g. unmanned underwater vessels; Equipment specially adapted therefor, e.g. docking stations
    • B63G2008/002—Underwater vessels adapted for special purposes, e.g. unmanned underwater vessels; Equipment specially adapted therefor, e.g. docking stations unmanned
    • B63G2008/004—Underwater vessels adapted for special purposes, e.g. unmanned underwater vessels; Equipment specially adapted therefor, e.g. docking stations unmanned autonomously operating
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2119/00—Details relating to the type or aim of the analysis or the optimisation
    • G06F2119/14—Force analysis or force optimisation, e.g. static or dynamic forces

Abstract

本发明公开了一种AUV水下机器人，包括机器人本体和控制模块，控制模块包括单目测距摄像头、无线手柄、水陆信息交互功能的微型可移动地面站。本发明还公开了机器人水中姿态与运动控制方法，采用机体深度与姿态以及机体水平运动与姿态分离分析和控制的方法，根据机体的整体机械结构以及各运动姿态下的受力情况进行数学建模，以机体欧拉角的姿态信息、深度及平移的位置信息作为外环，以机体三轴加速度、角速度、磁场强度信息作为内环，根据机体数学模型建立双闭环PID控制系统并进行运动学仿真，设计AUV水下机器人自稳和惯性导航算法，该系统较好地实现了全方位AUV水下机器人的姿态稳定和运动控制，具有较高的实用价值。

Description

水下机器人及其水中高可视度情况下姿态与运动控制方法
技术领域
本发明属于水下机器人姿态与控制技术领域，尤其涉及一种水下机器人及其在水中环境高可视度情况下机器人的姿态与控制技术。
背景技术
占全球71％面积的海洋是生物资源、能源、水资源和金属资源的战略性开发基地，对人类的发展和社会的进步具有重要战略性意义[2]。随着水下勘测技术的发展以及水中机器人控制技术的完善，具有全方位水中作业能力的水下机器人已成为应用范围最广的海洋探测设备之一，其中应用最为广泛的水下机器人包括有缆遥控水下机器人ROV和自主水下机器人AUV。
对于水下机器人而言，准确的姿态估计以及快速的姿态校正是水下机器人稳定工作的必要条件。水下机器人在复杂的海况下进行水底探测与勘查时常常会受到水流和波浪的随机扰动等一系列干扰的影响，且通过惯性测量单元获取姿态角，其测量的数据噪声较大，稳定性和精度低。因此，一般采用多传感器信息融合的方法来获取水下机器人的姿态。目前多传感器融合算法包括：共轭梯度法、互补滤波、无迹卡尔曼滤波和EKF。但由于在计算量、实时性和收敛性方面存在的缺陷，上述算法在小型AUV水下无缆机器人的姿态估计和矫正控制中并不实用。
发明内容
发明目的：本发明的第一目的在于提供一种在水中具备自稳和导航功能的水下机器人；本发明的第二目的在于提供一种水中环境高可视度情况下单目摄像头获取水下机器人水平位置信息的方法；本发明的第三目的在于提供一种八推进器水下机器人数学模型和闭环PID控制系统实现姿态和位置的6自由度控制算法的AUV水下机器人的姿态控制方法。
技术方案：本发明的一种水下机器人，包括AUV机器人本体和控制模块，所述AUV机器人本体包括机械固定结构，所述机械固定结构为铝型材搭建的矩形框架；所述机械固定结构顶部固定连接有PVC浮力材料；四个相同的水下推进器通过推进器固定件固定于材机械固定结构顶部四个角上；另外四个相同的水下推进器通过推进器固定件分别固定于四个支撑板上，每个支撑板的两端分别固定于机械固定结构的两条底边上；AUV控制舱体固定于材机械固定结构上，AUV控制舱体内设有陀螺仪、深度计、加速度计和电池，机械固定结构的底部安装配重材料；
进一步，所述机械固定结构采用欧式铝型材搭建而成，并通过M5×3cm尺寸螺丝和欧标螺母实现铝型材之间的固定；所述支撑板通过M5×5cm尺寸螺丝和欧标螺母安装机械固定结构的两条底边上。
所述控制模块包括设于地面的单目测距摄像头、无线手柄、水陆信息交互功能的微型可移动地面站。
本发明的一种水下机器人在水中高可视度情况下的姿态控制方法，包括如下步骤：
步骤一、设置控制模块，确保与AUV水下机器人本体各项连接正确；
步骤二、根据刚体在水中运动的动力学和运动学模型推算AUV水下无缆机器人在机体坐标系下的动力学和运动学模型，并根据机体的机械结构和水中工作状态对其进行简化；
步骤三、根据AUV水下无缆机器人的推进器布局进行力学分析，采用分离控制的方法分别对机体的垂直方向和水平方向进行分析和设计；
步骤四、通过机体内置的陀螺仪、加速度计获取机体的实时角速度和加速度，其中加速度包括机体三轴加速度ax、ay、az，角速度包括翻滚角速度
Figure BDA0003152003420000021
俯仰角速度
Figure BDA0003152003420000022
和偏航角速度
Figure BDA0003152003420000023
并根据机体加速度和角速度分别求解机体相对姿态，将二者经过卡尔曼融合算法后的结果作为机体实际姿态；
步骤五、通过机体内置的深度计和可移动地面站连接的单目摄像头获取机体的实时位置信息，通过将单目摄像头的像素坐标系下的像素坐标与其对应的机体坐标系下的位置坐标进行数据拟合，并通过解算出的函数关系求解出AUV机体水面折射倒影位置，根据机体深度信息以及水的折射原理计算出机体实际水平位置，从而进行AUV机体的水平位置测距；
步骤六、通过可移动地面站所连接的无线手柄控制机体的运动，当接收到可移动地面站发送的停止信号时，机体以当前的姿态与位置信息作为水下机器人的期望姿态稳定参数包括翻滚角Φ、俯仰角θ、偏航角Ψ，和位置稳定参数包括水平位置X和Y、深度Z；
步骤七、水下机器人根据可移动地面站发送停止信号时机体的姿态稳定参数和位置稳定参数作为期望值，与通过惯性测量元件采集到的机体的惯性数据解算出的姿态角信息作比较，并根据计算出的偏差值分别引入姿态角度角速度双闭环PID算法和位置环单闭环PID算法进行调节，从而对机体的姿态与位置状态进行控制；
步骤八、根据步骤二建立的AUV水下无缆机器人的动力学和运动学模型以及步骤三建立的AUV水下无缆机器人的力学模型对姿态角度角速度双闭环PID算法和位置环单闭环PID输出项依次进行运动学动力学处理和力学处理，根据运动学动力学模型解算出控制机体姿态和运动的合力或合力矩，再根据力学模型解算出对应的水下推进器转速，从而通过对水下机器人推进器进行油门控制实现AUV水下无缆机器人的姿态和运动控制；
步骤九、判断机体当前的姿态和位置信息是否满足机体的期望值，即机体的姿态稳定参数和位置稳定参数，如不满足则执行步骤七，如满足则执行步骤十；
步骤十、AUV水下无缆机器人达到机体期望的姿态稳定参数和位置稳定参数，机体继续接收可移动地面站控制信号，如果接收到运动信号后机体按照控制要求进行运动，并在下次获取到停止信号时重新执行步骤七，如未接收到，则控制推进器保持机体当前姿态和位置不发生变化。
进一步地，步骤二中，刚体在水中运动的动力学模型如式(1)：
Figure BDA0003152003420000031
其中，
Figure BDA0003152003420000032
和CRB(v)v项分别为机体的刚体惯性力项和刚体科氏力项，其中MRB为刚体惯性矩阵，CRB(v)为科氏力矩阵，v为机体线速度，
Figure BDA0003152003420000033
为机体线性加速度；
Figure BDA0003152003420000034
CA(vr)vr、D(vr)vr项分别为液体附加惯性力项、附加科氏力项和阻尼项，其中MA为液体附加惯量矩阵、CA(vr)为液体附加科氏力矩阵、D(vr)为液体阻尼力矩阵，vr为液体线速度，
Figure BDA0003152003420000035
为液体线性加速度；g(η)为液体静力项，包括机体的重力项和液体对机体的浮力项；τwind和τwave分别为风力干扰项和水流干扰项；τcontrol为施加于机体的推力项；
由于AUV在较为平静的水下环境进行工作和调试，可忽略风力τwind和水流干扰τwave对系统的影响；同时，AUV水下运动时的运动速度和加速度较慢，因此忽略液体附加惯性力项、附加科氏力项和阻尼项对系统的干扰；当水的流速为零时，v＝vr。
简化后的AUV机体动力学模型为式(2)：
Figure BDA0003152003420000041
(1)首先考虑AUV无缆水下机器人的刚体惯性矩MRB如式(3)：
Figure BDA0003152003420000042
其中，m为机体的质量，xg、yg、zg为机体坐标系中心至机体重心的向量，Ix、Iy、Iz分别为机体的旋转惯性矩，Ixy、Ixz、Iyx、Iyz、Izx、Izy分别为机体旋转惯性积。设机体坐标系位于重心且质量分布平衡，则MRB简化为对角矩阵；
(2)其次考虑AUV水下无缆机器人的重力和浮力对机体动力学方程的干扰，其矩阵如式(4)：
Figure BDA0003152003420000043
其中，W为机体的重力，B为机体的浮力，xg、yg、zg为机体坐标系中心至机体重心的向量，Φ、θ分别为机体的翻滚角、俯仰角，设重心、浮心与机体坐标系重合，且机体可保持较好的水平姿态时，上式可简化为式(5)：
Figure BDA0003152003420000044
(3)考虑D(vr)vr项的线性阻尼作用并忽略阻尼项的非线性部分，且设v＝vr，则液体阻尼项为式(6)：
Figure BDA0003152003420000051
其中，u、v、ω、p、q、r分别为机体坐标系下X、Y、Z向三轴线速度和翻滚、俯仰、偏航角速度，Xu、Xv、Xω、Xp、Xq、Xr分别为X向运动对应的线性误差系数，以此类推。此时，D(vr)近似为对角阵；
(4)考虑机体的运动学模型和简化后的动力学模型，将其展开得式(7)：
Figure BDA0003152003420000052
其中，m为机体的质量，X、Y、Z、K、M、N分别为机体X向、Y向、Z向以及翻滚、俯仰、偏航6自由度的控制合力；u、v、ω、p、q、r分别为分别机体坐标系下X、Y、Z向三轴线速度和翻滚、俯仰、偏航角速度；
Figure BDA0003152003420000053
分别为机体6自由度加速度；xg、yg、zg为机体坐标系中心至机体重心的向量，Ix、Iy、Iz分别为机体的旋转惯性矩，Ixy、Ixz、Iyx、Iyz、Izx、Izy分别为机体旋转惯性积。设机体坐标系位于重心且质量分布平衡，则MRB简化为对角矩阵；
根据AUV机体建模后得数据可得机体旋转惯性矩和旋转惯性积几乎为0，且考虑到本次项目在较为平静的水下环境中进行工作和调试，将上式简化为式(8)：
Figure BDA0003152003420000061
忽略上式中的液体科氏力项并考虑液体阻尼项，可得AUV水下无缆机器人的简化2阶动力学模型如式(9)：
Figure BDA0003152003420000062
进一步地，步骤三中，所述垂直方向的设计包括：将其中两个对角线方向的推进器设为a、c号，另外两个对角线方向的推进器设为b、d号，其中a、c号推进器正转转向为顺时针，b、d号推进器正转转向为逆时针，正转对应Z+推力，以a、c号推进器的中心连接线为X轴方向，b、d号推进器的中心连接线为Y轴方向，以机体垂直向下方向为Z轴方向构建坐标系；垂直方向的a至d号推进器控制Z轴位移以及机体的俯仰和偏航角，设a号至d号推进器控制产生的推力分别为F1、F2、F3、F4，推进器的转速与推进器近似满足式(10)：
F＝K|ω|ω#(10)
为了简化系统数学模型，当转速不高时可近似取线性关系，推进器的转速与推力的关系可近似为式(11)：
F＝Kω#(11)
其中K为比例系数，单位为N*min/r，F为推进器的推力，单位为N，ω为电机的转速，单位为RPM，根据实际测量，推进器的比例系数K取0.0048N*min/r；
设以X轴为旋转轴旋转的角度为Φ，以Y轴为旋转轴旋转的角度为θ，垂直方向产生的机体推力如式(12)：
Figure BDA0003152003420000071
其中Uz是机体垂直z方向受到的合力，单位为N，UΦ和Uθ是绕X轴和绕Y轴的力矩，单位为N*m，R为a至d号推进器中心至AUV水下无缆机器人机体中心的距离，单位为m；
同时考虑不产生偏航力，设定约束条件如式(13)：
F1+F3-F2-F4＝0#(13)
将式(13)代入式(12)中，简化得式(14)：
Figure BDA0003152003420000072
将式(11)代入式(14)中，得式(15)：
Figure BDA0003152003420000073
根据机体本身所需的Uz、Uθ和UΦ，对式(15)中矩阵求逆可得出ω1、ω2、ω3的值，并根据式(11)和式(13)即可计算出ω4的值，从而得到垂直方向推力实现Φ、θ、Z的姿态和运动控制；
进一步地，步骤三中，所述水平方向的设计包括：将其中两个对角线方向的推进器设为e、g号，另外两个对角线方向的推进器设为f、h号，其中e、g号推进器正转转向相同，f、h号推进器正转转向相同，但e、g号推进器与f、h号推进器的正转转向相反；
以AUV机头前进方向为坐标系的X轴、机器前进方向的左方向为坐标系Y轴，以机体竖直向下方向为坐标系的Z轴，建立机体水平方向姿态的机体坐标系，水平方向的e至h号推进器控制X、Y轴位移以及机体的偏航角，设e、f、g、h号推进器的推力分别为F5、F6、F7、F8，设e至h号推进器正转的方向对应推进器所需推力F的正向推力，AUV水下无缆机器人水平方向姿态稳定需满足的条件如式(16)：
Figure BDA0003152003420000081
其中Ux和Uy分别表示X轴和Y轴方向的推力和，单位为N，Uψ表示偏航角的角力矩，单位为N*m，R为e至h号推进器中心至AUV水下无缆机器人机体中心的距离，单位为m，
将式(11)代入式(16)中，可得式(17)：
Figure BDA0003152003420000082
实现控制时，需根据机体本身所需的Ux、Uy和Uψ根据式(22)进行反解得出ω5、ω6、ω7、ω8，但由于有四个推进器的值需要求解，因此采用最小二范数解可得式(18)：
Figure BDA0003152003420000083
Figure BDA0003152003420000084
根据式(18)求出e至h推进器所需要的转速，即可实现AUV水下无缆机器人X、Y水平方向的移动和偏航角ψ的控制。
进一步地，步骤四中，机体的加速度和角速度通过下述公式(19)和(20)计算出机体的俯仰角、翻滚角和偏航角。其中机体加速度测量姿态的原理是利用静止时刻感受到重力加速度在机体坐标系下各轴分量计算姿态，机体角速度测量姿态的原理是通过对机体坐标系下各轴角速度积分计算姿态，最终将二者计算出的姿态信息经过卡尔曼滤波后的结果作为机体实际姿态角：
Figure BDA0003152003420000091
Figure BDA0003152003420000092
其中，Φacc、θacc为通过机体加速度解算出的翻滚角和俯仰角，ax、ay、az分别为机体坐标系下三轴加速度；Φgyro、θgyro和Ψgyro为通过机体角速度解算出的翻滚角、俯仰角和偏航角，
Figure BDA0003152003420000093
分别为机体坐标系下翻滚角速度、俯仰角速度、偏航角速度，Φgyro(n+1)、θgyro(n+1)、Ψgyro(n+1)和Φgyro(n)、θgyro(n)、Ψgyro(n)分别为n+1、n时刻的三轴姿态角，ΔΦgyro、Δθgyro、ΔΨgyro分别为n时刻至n+1时刻三轴姿态角的增量，Δt为n时刻至n+1时刻之间的时间间隔。
进一步地，步骤五中，机体的水平位置信息通过单目摄像头获取并计算求得，单目摄像头通过USB2.0接口与可移动地面站相连，其功能是识别AUV机体并对其进行定位，求解机体X轴方向和Y轴方向的位置信息。由于单目摄像头常为鱼眼摄像头，其获取的视频流画面存在畸变，因此需要对单目摄像头进行畸变矫正。其具体标定方法为每隔十厘米在测量尺上进行标记，并记录水面实际位置坐标与单目摄像头图像流的像素位置坐标，经过拟合后可得到一个近似的三次函数关系。
机体顶部大面积浮力材料为白色，采用图像处理对AUV水下机器人进行定位，将RGB视频流转换为灰度视频流，经过高斯滤波和三次3*3卷积核的腐蚀和膨胀消除图像噪声，并通过灰度二值化后提取白色区域面积。设定固定的白色区域面积值域，识别图中符合该值域的白色面积并对其进行最小外接椭圆拟合，计算出椭圆圆心并经过三次多项式曲线将摄像头坐标系下的像素坐标转换成机体的折射倒影中心位置坐标。
通过下述公式(21)计算出机体折射角：
Figure BDA0003152003420000101
其中，r为折射角，h为摄像头中心与水面的距离，x，y为摄像头获得的机体折射倒影中心位置坐标；
根据水中折射原理公式(22)计算机体折射倒影入射角：
Figure BDA0003152003420000102
其中，α为入射角，r为折射角；
根据机体深度和入射角，通过式(23)计算出机体的实际位置坐标：
Figure BDA0003152003420000103
其中，x′和y′为别为机体的实际位置坐标，x和y分别为摄像头获得的机体折射倒影中心位置坐标，d为摄像头中心与水面的距离，α为入射角。
有益效果：与现有技术相比，本发明具有如下显著优点：
(1)采用闭环控制系统，简化了手动操作难度，提高系统的实时性和控制性能；
(2)通过单目摄像头实现平移方向位置信息获取，解决水下机器人在特定环境下水平位置信息无法获取的问题；
(3)采用加速度、角速度和磁力强度作为系统内环，欧拉角、深度和平移位置信息作为外环，提高了系统的整体稳定性和快速性；
(4)通过MATLAB对整体系统进行运动学仿真，提供设计依据；
(5)涉及微计算机软硬件、图像识别、闭环运动控制、模拟仿真等众多相关知识，具有较高的实用价值。
附图说明
图1为本发明水下机器人的结构示意图；
图2为机体的垂直方向推进装置的布置结构及坐标系结构示意图；
图3为机体的水平方向推进装置的布置结构及坐标系结构示意图；
图4为本发明水下机器人的姿态与运动控制方法的Simulink仿真结构示意图；
图5为本发明水下机器人在水中高可视度情况下姿态与运动控制方法的流程示意图；
图6为本发明水下机器人的机体姿态(翻滚角Φ、俯仰角θ)与深度Z的控制方法流程示意图；
图7为本发明水下机器人的机体姿态(偏航角ψ)与水平位置X和Y的控制方法流程示意图。
具体实施方式
下面结合附图对本发明的技术方案作进一步说明。
如图1所示，本发明的水下机器人包括AUV机器人本体和控制模块，所述AUV机器人本体包括机械固定结构6，所述机械固定结构6为铝型材搭建的矩形框架；所述机械固定结构6顶部固定连接有PVC浮力材料1；四个相同的水下推进器2通过推进器固定件3固定于材机械固定结构6顶部四个角上；另外四个相同的水下推进器2通过推进器固定件3分别固定于四个支撑板5上，每个支撑板5的两端分别固定于机械固定结构6的两条底边上；AUV控制舱体4固定于材机械固定结构6上，AUV控制舱体4内设有陀螺仪、深度计、加速度计和电池，机械固定结构6的底部安装配重材料7；
所述控制模块包括设于地面的单目测距摄像头、无线手柄、水陆信息交互功能的微型可移动地面站。设于地面部分的集单目摄像头识别测距、无线XBOX手柄控制和水陆信息交互功能的微型可移动地面站，其中单目摄像头通过USB2.0接口与微型可移动地面站进行连接，可移动地面站读取视频流并进行图像处理，获取机体的水平移动的位置信息，并将手柄的控制信息、机体水平移动的X轴和Y轴的距离信息等通过433Mhz通信模块发送给设于水下部分的AUV水下无缆机器人本体。
AUV铝型材机械固定结构6采用2020欧式铝型材搭建而成，并通过M5×3cm尺寸螺丝和2020欧标螺母实现铝型材之间的固定；其次，高强度PVC浮力材料1通过M5×85cm尺寸螺丝和2020欧标螺母固定于铝型材机械固定结构6上，为AUV水下无缆机器人的机体提供稳定的浮力；四个相同的水下推进器2通过推进器固定件3固定于铝型材机械固定结构6上，为机体提供上升和下潜的推力并用于控制机体的俯仰和翻滚角，另外四个相同的水下推进器2通过推进器固定件3固定于支撑板5上，并通过M5×5cm尺寸螺丝和2020欧标螺母安装支撑板5，将水平方向的四个推进器固定于铝型材机械固定结构6上；最后，AUV控制舱体4固定于铝型材机械固定结构6上，并于机体正下方安装配重材料7。
基于STM32F407VGT6为主控制器的AUV水下无缆机器人本体设计，包括获取机体姿态信息的IMU、获取机体深度信息的深度计、与设于地面部分的微型地面站进行水陆信息交互的433Mhz通信模块。STM32通过DMA快速接收深度计、IMU以及微型地面站发送的数据通信协议并进行相关解算，根据机体的姿态和位置信息实现AUV无缆水下机器人的自稳和导航功能。机体整体机械机构采用八推进器结构，其中竖直方向的四个推进器控制俯仰角和翻滚角以及AUV的上升和下潜动作，水平方向的四个推进器控制AUV的偏航角以及AUV平移和旋转动作。机体通过3S电池供电，分别通过12V分压板和12V转5V降压板给机体推进器以及机体控制器供电。
如图5所示，本发明的一种水下机器人在水中高可视度情况下的姿态控制方法，包括如下步骤：
步骤一、设置控制模块，确保与AUV水下机器人本体各项连接正确；
步骤二、根据刚体在水中运动的动力学和运动学模型推算AUV水下无缆机器人在机体坐标系下的动力学和运动学模型，并根据机体的机械结构和水中工作状态对其进行简化；
步骤三、根据AUV水下无缆机器人的推进器布局进行力学分析，采用分离控制的方法分别对机体的垂直方向和水平方向进行分析和设计；
步骤四、通过机体内置的陀螺仪、加速度计获取机体的实时角速度和加速度，其中加速度包括机体三轴加速度ax、ay、az，角速度包括翻滚角速度
Figure BDA0003152003420000121
俯仰角速度
Figure BDA0003152003420000122
和偏航角速度
Figure BDA0003152003420000123
并根据机体加速度和角速度分别求解机体相对姿态，将二者经过卡尔曼融合算法后的结果作为机体实际姿态；
步骤五、通过机体内置的深度计和可移动地面站连接的单目摄像头获取机体的实时位置信息，通过将单目摄像头的像素坐标系下的像素坐标与其对应的机体坐标系下的位置坐标进行数据拟合，并通过解算出的函数关系求解出AUV机体水面折射倒影位置，根据机体深度信息以及水的折射原理计算出机体实际水平位置，从而进行AUV机体的水平位置测距；
步骤六、通过可移动地面站所连接的无线手柄控制机体的运动，当接收到可移动地面站发送的停止信号时，机体以当前的姿态与位置信息作为水下机器人的期望姿态稳定参数包括翻滚角Φ、俯仰角θ、偏航角Ψ，和位置稳定参数包括水平位置X和Y、深度Z；
步骤七、水下机器人根据可移动地面站发送停止信号时机体的姿态稳定参数和位置稳定参数作为期望值，与通过惯性测量元件采集到的机体的惯性数据解算出的姿态角信息作比较，并根据计算出的偏差值分别引入姿态角度角速度双闭环PID算法和位置环单闭环PID算法进行调节，从而对机体的姿态与位置状态进行控制；
步骤八、根据步骤二建立的AUV水下无缆机器人的动力学和运动学模型以及步骤三建立的AUV水下无缆机器人的力学模型对姿态角度角速度双闭环PID算法和位置环单闭环PID输出项依次进行运动学动力学处理和力学处理，根据运动学动力学模型解算出控制机体姿态和运动的合力或合力矩，再根据力学模型解算出对应的水下推进器转速，从而通过对水下机器人推进器进行油门控制实现AUV水下无缆机器人的姿态和运动控制；
步骤九、判断机体当前的姿态和位置信息是否满足机体的期望值，即机体的姿态稳定参数和位置稳定参数，如不满足则执行步骤七，如满足则执行步骤十；
步骤十、AUV水下无缆机器人达到机体期望的姿态稳定参数和位置稳定参数，机体继续接收可移动地面站控制信号，如果接收到运动信号后机体按照控制要求进行运动，并在下次获取到停止信号时重新执行步骤七，如未接收到，则控制推进器保持机体当前姿态和位置不发生变化。
进一步地，步骤二中，刚体在水中运动的动力学模型如式(1)：
Figure BDA0003152003420000131
其中，
Figure BDA0003152003420000132
和CRB(v)v项分别为机体的刚体惯性力项和刚体科氏力项，其中MRB为刚体惯性矩阵，CRB(v)为科氏力矩阵，v为机体线速度，
Figure BDA0003152003420000133
为机体线性加速度；
Figure BDA0003152003420000134
CA(vr)vr、D(vr)vr项分别为液体附加惯性力项、附加科氏力项和阻尼项，其中MA为液体附加惯量矩阵、CA(vr)为液体附加科氏力矩阵、D(vr)为液体阻尼力矩阵，vr为液体线速度，
Figure BDA0003152003420000135
为液体线性加速度；g(η)为液体静力项，包括机体的重力项和液体对机体的浮力项；τwind和τwave分别为风力干扰项和水流干扰项；τcontrol为施加于机体的推力项；
由于AUV在较为平静的水下环境进行工作和调试，可忽略风力τwind和水流干扰τwave对系统的影响；同时，AUV水下运动时的运动速度和加速度较慢，因此忽略液体附加惯性力项、附加科氏力项和阻尼项对系统的干扰；当水的流速为零时，v＝vr；
简化后的AUV机体动力学模型如式(2)：
Figure BDA0003152003420000141
设机体坐标系位于重心且质量分布平衡，机体地重心、浮心与机体坐标系重合且机体可保持较好的水平姿态，考虑D(vr)vr项的线性阻尼作用并忽略阻尼项的非线性部分且设v＝vr，根据AUV机体建模后得数据可得机体旋转惯性矩和旋转惯性积几乎为0，且考虑到本次项目在较为平静的水下环境中进行工作和调试，将式(2)展开并简化为式(3)：
Figure BDA0003152003420000142
其中，m为机体的质量，X、Y、Z、K、M、N分别为机体X向、Y向、Z向以及翻滚、俯仰、偏航6自由度的控制合力；u、v、ω、p、q、r分别为分别机体坐标系下X、Y、Z向三轴线速度和翻滚、俯仰、偏航角速度；
Figure BDA0003152003420000143
分别为机体6自由度加速度；Ix、Iy、Iz分别为机体的旋转惯性矩；
进一步地，如图2所示，步骤三中，所述垂直方向的设计包括：将其中两个对角线方向的推进器设为a、c号，另外两个对角线方向的推进器设为b、d号，其中a、c号推进器正转转向为顺时针，b、d号推进器正转转向为逆时针，正转对应Z+推力，以a、c号推进器的中心连接线为X轴方向，b、d号推进器的中心连接线为Y轴方向，以机体垂直向下方向为Z轴方向构建坐标系；垂直方向的a至d号推进器控制Z轴位移以及机体的俯仰和偏航角，设a号至d号推进器控制产生的推力分别为F1、F2、F3、F4，推进器的转速与推进器近似满足式(4)：
F＝K|ω|ω# (4)
为了简化系统数学模型，当转速不高时可近似取线性关系，推进器的转速与推力的关系可近似为式(5)：
F＝Kω# (5)
其中K为比例系数，单位为N*min/r，F为推进器的推力，单位为N，ω为电机的转速，单位为RPM，根据实际测量，推进器的比例系数K取0.0048N*min/r；
设以X轴为旋转轴旋转的角度为Φ，以Y轴为旋转轴旋转的角度为θ，垂直方向产生的机体推力如式(6)：
Figure BDA0003152003420000151
其中Uz是机体垂直Z方向受到的合力，单位为N，UΦ和Uθ是绕X轴和绕Y轴的力矩，单位为N*m，R为a至d号推进器中心至AUV水下无缆机器人机体中心的距离，单位为m；
同时考虑不产生偏航力，设定约束条件如式(7)：
F1+F3-F2-F4＝0# (7)
将式(7)代入式(6)中，简化得式(8)：
Figure BDA0003152003420000152
将式(5)代入式(8)中，得式(9)：
Figure BDA0003152003420000153
根据机体本身所需的Uz、Uθ和UΦ，对式(15)中矩阵求逆可得出ω1、ω2、ω3的值，并根据式(5)和式(7)即可计算出ω4的值，从而得到垂直方向推力实现Φ、θ、Z的姿态和运动控制；
进一步地，如图3所示，步骤三中，所述水平方向的设计包括：将其中两个对角线方向的推进器设为e、g号，另外两个对角线方向的推进器设为f、h号，其中e、g号推进器正转转向相同，f、h号推进器正转转向相同，但e、g号推进器与f、h号推进器的正转转向相反；
以AUV机头前进方向为坐标系的X轴、机器前进方向的左方向为坐标系Y轴，以机体竖直向下方向为坐标系的Z轴，建立机体水平方向姿态的机体坐标系，水平方向的e至h号推进器控制X、Y轴位移以及机体的偏航角，设e、f、g、h号推进器的推力分别为F5、F6、F7、F8，设e至h号推进器正转的方向对应推进器所需推力F的正向推力，AUV水下无缆机器人水平方向姿态稳定需满足的条件如式(10)：
Figure BDA0003152003420000161
其中Ux和Uy分别表示X轴和Y轴方向的推力和，单位为N，Uψ表示偏航角的角力矩，单位为N*m，R为e至h号推进器中心至AUV水下无缆机器人机体中心的距离，单位为m，
将式(5)代入式(10)中，可得式(11)：
Figure BDA0003152003420000162
实现控制时，需根据机体本身所需的Ux、Uy和Uψ通过对式(17)进行反解得出ω5、ω6、ω7、ω8，但由于有四个推进器的值需要求解，因此采用最小二范数解可得式(12)：
Figure BDA0003152003420000163
Figure BDA0003152003420000171
根据式(12)求出e至h推进器所需要的转速，即可实现AUV水下无缆机器人X、Y水平方向的移动和偏航角ψ的控制。
进一步地，步骤四中，机体的加速度和角速度通过下述公式(13)和(14)计算出机体的俯仰角、翻滚角和偏航角。其中机体加速度测量姿态的原理是利用静止时刻感受到重力加速度在机体坐标系下各轴分量计算姿态，机体角速度测量姿态的原理是通过对机体坐标系下各轴角速度积分计算姿态，最终将二者计算出的姿态信息经过卡尔曼滤波后的结果作为机体实际姿态角：
Figure BDA0003152003420000172
Figure BDA0003152003420000173
其中，Φacc、θacc为通过机体加速度解算出的翻滚角和俯仰角，ax、ay、az分别为机体坐标系下三轴加速度；Φgyro、θgyro和Ψgyro为通过机体角速度解算出的翻滚角、俯仰角和偏航角，
Figure BDA0003152003420000174
分别为机体坐标系下翻滚角速度、俯仰角速度、偏航角速度，Φgyro(n+1)、θgyro(n+1)、Ψgyro(n+1)和Φgyro(n)、θgyro(n)、Ψgyro(n)分别为n+1、n时刻的三轴姿态角，ΔΦgyro、Δθgyro、ΔΨgyro分别为n时刻至n+1时刻三轴姿态角的增量，Δt为n时刻至n+1时刻之间的时间间隔。
进一步地，步骤五中，机体的水平位置信息通过单目摄像头获取并计算求得，单目摄像头通过USB2.0接口与可移动地面站相连，其功能是识别AUV机体并对其进行定位，求解机体X轴方向和Y轴方向的位置信息。由于单目摄像头常为鱼眼摄像头，其获取的视频流画面存在畸变，因此需要对单目摄像头进行畸变矫正。其具体标定方法为每隔十厘米在测量尺上进行标记，并记录水面实际位置坐标与单目摄像头图像流的像素位置坐标，经过拟合后可得到一个近似的三次函数关系。
机体顶部大面积浮力材料为白色，采用图像处理对AUV水下机器人进行定位，将RGB视频流转换为灰度视频流，经过高斯滤波和三次3*3卷积核的腐蚀和膨胀消除图像噪声，并通过灰度二值化后提取白色区域面积。设定固定的白色区域面积值域，识别图中符合该值域的白色面积并对其进行最小外接椭圆拟合，计算出椭圆圆心并经过三次多项式曲线将摄像头坐标系下的像素坐标转换成机体的折射倒影中心位置坐标。
通过下述公式(15)计算出机体折射角：
Figure BDA0003152003420000181
其中，r为折射角，h为摄像头中心与水面的距离，x，y为摄像头获得的机体折射倒影中心位置坐标；
根据水中折射原理公式(16)计算机体折射倒影入射角：
Figure BDA0003152003420000182
其中，α为入射角，r为折射角；
根据机体深度和入射角，通过式(17)计算出机体的实际位置坐标：
Figure BDA0003152003420000183
其中，x′和y′为别为机体的实际位置坐标，x和y分别为摄像头获得的机体折射倒影中心位置坐标，d为摄像头中心与水面的距离，α为入射角。
本方法是基于Simulink的AUV水下无缆机器人的运动学仿真，采用机体深度与姿态以及机体水平运动与姿态分离分析和控制的方法，根据机体的整体机械结构以及各运动姿态下的受力情况进行系统性数学分析，并建立相关数学模型。
如图4所示，通过测量或者查阅文献得到相关模块所需要的参数大小以及误差因素等，根据机体的数学模型构建双闭环PID控制系统Simulink仿真模型，根据模型的性能指标要求计算PID参数并带入Simulink仿真模型里进行调试，并测试出现外界干扰时系统重新恢复稳定的性能指标，为AUV的姿态和运动控制提供理论基础。
通过闭环PID控制系统实现AUV水下无缆机器人机体姿态与运动控制算法。通过获取机体欧拉角的姿态信息、深度及平移的位置信息作为外环，获取机体三轴加速度、角速度、磁场强度信息作为内环，从而通过闭环PID控制系统完成AUV水下无缆机器人的姿态与运动控制，实现AUV水下的自稳和定位功能。

Claims (7)

1.一种水下机器人，包括AUV机器人本体和控制模块，其特征在于，所述AUV机器人本体包括机械固定结构(6)，所述机械固定结构(6)为铝型材搭建的矩形框架；所述机械固定结构(6)顶部固定连接有PVC浮力材料(1)；四个相同的水下推进器(2)通过推进器固定件(3)固定于材机械固定结构(6)顶部四个角上；另外四个相同的水下推进器(2)通过推进器固定件(3)分别固定于四个支撑板(5)上，每个支撑板(5)的两端分别固定于机械固定结构(6)的两条底边上；AUV控制舱体(4)固定于材机械固定结构(6)上，AUV控制舱体(4)内设有陀螺仪、深度计、加速度计和电池，机械固定结构(6)的底部安装配重材料(7)；
所述控制模块包括设于地面的单目测距摄像头、无线手柄、水陆信息交互功能的微型可移动地面站。
2.根据权利要求1所述的水下机器人，其特征在于，所述机械固定结构(6)采用欧式铝型材搭建而成，并通过M5×3cm尺寸螺丝和欧标螺母实现铝型材之间的固定；所述支撑板(5)通过M5×5cm尺寸螺丝和欧标螺母安装机械固定结构(6)的两条底边上。
3.一种权利要求1所述的水下机器人在水中高可视度情况下的姿态与运动控制方法，其特征在于，包括如下步骤：
步骤一、设置控制模块，确保与AUV水下机器人本体各项连接正确；
步骤二、根据刚体在水中运动的动力学和运动学模型推算AUV水下无缆机器人在机体坐标系下的动力学和运动学模型，并根据机体的机械结构和水中工作状态对其进行简化；
步骤三、根据AUV水下无缆机器人的推进器布局进行力学分析，采用分离控制的方法分别对机体的垂直方向和水平方向进行分析和设计；
步骤四、通过机体内置的陀螺仪、加速度计获取机体的实时角速度和加速度，其中加速度包括机体三轴加速度ax、ay、az，角速度包括翻滚角速度
Figure FDA0003152003410000011
俯仰角速度
Figure FDA0003152003410000012
和偏航角速度
Figure FDA0003152003410000013
并根据机体加速度和角速度分别求解机体相对姿态，将二者经过卡尔曼融合算法后的结果作为机体实际姿态；
步骤五、通过机体内置的深度计和可移动地面站连接的单目摄像头获取机体的实时位置信息，通过将单目摄像头的像素坐标系下的像素坐标与其对应的机体坐标系下的位置坐标进行数据拟合，并通过解算出的函数关系求解出AUV机体水面折射倒影位置，根据机体深度信息以及水的折射原理计算出机体实际水平位置，从而进行AUV机体的水平位置测距；
步骤六、通过可移动地面站所连接的无线手柄控制机体的运动，当接收到可移动地面站发送的停止信号时，机体以当前的姿态与位置信息作为水下机器人的期望姿态稳定参数包括翻滚角Φ、俯仰角θ、偏航角Ψ，和位置稳定参数包括水平位置X和Y、深度Z；
步骤七、水下机器人根据可移动地面站发送停止信号时机体的姿态稳定参数和位置稳定参数作为期望值，与通过惯性测量元件采集到的机体的惯性数据解算出的姿态角信息作比较，并根据计算出的偏差值分别引入姿态角度角速度双闭环PID算法和位置环单闭环PID算法进行调节，从而对机体的姿态与位置状态进行控制；
步骤八、根据步骤二建立的AUV水下无缆机器人的动力学和运动学模型以及步骤三建立的AUV水下无缆机器人的力学模型对姿态角度角速度双闭环PID算法和位置环单闭环PID输出项依次进行运动学动力学处理和力学处理，根据运动学动力学模型解算出控制机体姿态和运动的合力或合力矩，再根据力学模型解算出对应的水下推进器转速，从而通过对水下机器人推进器进行油门控制实现AUV水下无缆机器人的姿态和运动控制；
步骤九、判断机体当前的姿态和位置信息是否满足机体的期望值，即机体的姿态稳定参数和位置稳定参数，如不满足则执行步骤七，如满足则执行步骤十；
步骤十、AUV水下无缆机器人达到机体期望的姿态稳定参数和位置稳定参数，机体继续接收可移动地面站控制信号，如果接收到运动信号后机体按照控制要求进行运动，并在下次获取到停止信号时重新执行步骤七，如未接收到，则控制推进器保持机体当前姿态和位置不发生变化。
4.根据权利要求3所述的水下机器人在水中高可视度情况下的姿态与运动控制方法，其特征在于，步骤二中，刚体在水中运动的动力学模型如式(1)：
Figure FDA0003152003410000021
其中，
Figure FDA0003152003410000022
和CRB(v)v项分别为机体的刚体惯性力项和刚体科氏力项，其中MRB为刚体惯性矩阵，CRB(v)为科氏力矩阵，v为机体线速度，
Figure FDA0003152003410000023
为机体线性加速度；
Figure FDA0003152003410000024
CA(vr)vr、D(vr)vr项分别为液体附加惯性力项、附加科氏力项和阻尼项，其中MA为液体附加惯量矩阵、CA(vr)为液体附加科氏力矩阵、D(vr)为液体阻尼力矩阵，vr为液体线速度，
Figure FDA0003152003410000034
为液体线性加速度；g(η)为液体静力项，包括机体的重力项和液体对机体的浮力项；τwind和τwave分别为风力干扰项和水流干扰项；τcontrol为施加于机体的推力项；
由于AUV在较为平静的水下环境进行工作和调试，可忽略风力τwind和水流干扰τwave对系统的影响；同时，AUV水下运动时的运动速度和加速度较慢，因此忽略液体附加惯性力项、附加科氏力项和阻尼项对系统的干扰；当水的流速为零时，v＝vr；
简化后的AUV机体动力学模型如式(2)：
Figure FDA0003152003410000031
(1)首先考虑AUV无缆水下机器人的刚体惯性矩MRB如式(3)：
Figure FDA0003152003410000032
其中，m为机体的质量，xg、yg、zg为机体坐标系中心至机体重心的向量，Ix、Iy、Iz分别为机体的旋转惯性矩，Ixy、Ixz、Iyx、Iyz、Izx、Izy分别为机体旋转惯性积，设机体坐标系位于重心且质量分布平衡，则MRB简化为对角矩阵；
(2)其次考虑AUV水下无缆机器人的重力和浮力对机体动力学方程的干扰，其矩阵如式(4)：
Figure FDA0003152003410000033
其中，W为机体的重力，B为机体的浮力，xg、yg、zg为机体坐标系中心至机体重心的向量，Φ、θ分别为机体的翻滚角、俯仰角，设重心、浮心与机体坐标系重合，且机体可保持较好的水平姿态时，上式可简化为式(5)：
Figure FDA0003152003410000041
(3)考虑D(vr)vr项的线性阻尼作用并忽略阻尼项的非线性部分，且设v＝vr，则液体阻尼项为式(6)：
Figure FDA0003152003410000042
其中，u、v、ω、p、q、r分别为机体坐标系下X、Y、Z向三轴线速度和翻滚、俯仰、偏航角速度，Xu、Xv、Xω、Xp、Xq、Xr分别为X向运动对应的线性误差系数，以此类推，此时，D(vr)近似为对角阵；
(4)考虑机体的运动学模型和简化后的动力学模型，将其展开得式(7)：
Figure FDA0003152003410000043
其中，m为机体的质量，X、Y、Z、K、M、N分别为机体X向、Y向、Z向以及翻滚、俯仰、偏航6自由度的控制合力；u、v、ω、p、q、r分别为分别机体坐标系下X、Y、Z向三轴线速度和翻滚、俯仰、偏航角速度；
Figure FDA0003152003410000044
分别为机体6自由度加速度；xg、yg、zg为机体坐标系中心至机体重心的向量，Ix、Iy、Iz分别为机体的旋转惯性矩，Ixy、Ixz、Iyx、Iyz、Izx、Izy分别为机体旋转惯性积，设机体坐标系位于重心且质量分布平衡，则MRB简化为对角矩阵；
根据AUV机体建模后得数据可得机体旋转惯性矩和旋转惯性积几乎为0，且考虑到本次项目在较为平静的水下环境中进行工作和调试，将上式简化为式(8)：
Figure FDA0003152003410000051
忽略上式中的液体科氏力项并考虑液体阻尼项，可得AUV水下无缆机器人的简化2阶动力学模型如式(9)：
Figure FDA0003152003410000052
5.根据权利要求3所述的水下机器人在水中高可视度情况下的姿态控制方法，其特征在于，步骤三中，所述垂直方向的设计包括：将其中两个对角线方向的推进器设为a、c号，另外两个对角线方向的推进器设为b、d号，其中a、c号推进器正转转向为顺时针，b、d号推进器正转转向为逆时针，正转对应Z+推力，以a、c号推进器的中心连接线为X轴方向，b、d号推进器的中心连接线为Y轴方向，以机体垂直向下方向为Z轴方向构建坐标系；垂直方向的a至d号推进器控制Z轴位移以及机体的俯仰和偏航角，设a号至d号推进器控制产生的推力分别为F1、F2、F3、F4，推进器的转速与推进器近似满足式(10)：
F＝K|ω|ω#(10)
为了简化系统数学模型，当转速不高时可近似取线性关系，推进器的转速与推力的关系可近似为式(11)：
F＝Kω#(11)
其中K为比例系数，单位为N*min/r，F为推进器的推力，单位为N，ω为电机的转速，单位为RPM，根据实际测量，推进器的比例系数K取0.0048N*min/r；
设以X轴为旋转轴旋转的角度为Φ，以Y轴为旋转轴旋转的角度为θ，垂直方向产生的机体推力如式(12)：
Figure FDA0003152003410000061
其中Uz是机体垂直Z方向受到的合力，单位为N，UΦ和Uθ是绕X轴和绕Y轴的力矩，单位为N*m，R为a至d号推进器中心至AUV水下无缆机器人机体中心的距离，单位为m；
同时考虑不产生偏航力，设定约束条件如式(13)：
F1+F3-F2-F4＝0#(13)
将式(13)代入式(12)中，简化得式(14)：
Figure FDA0003152003410000062
将式(11)代入式(14)中，得式(15)：
Figure FDA0003152003410000063
根据机体本身所需的Uz、Uθ和UΦ，对式(15)中矩阵求逆可得出ω1、ω2、ω3的值，并根据式(11)和式(13)即可计算出ω4的值，从而得到垂直方向推力实现Φ、θ、Z的姿态和运动控制；
进一步地，步骤三中，所述水平方向的设计包括：将其中两个对角线方向的推进器设为e、g号，另外两个对角线方向的推进器设为f、h号，其中e、g号推进器正转转向相同，f、h号推进器正转转向相同，但e、g号推进器与f、h号推进器的正转转向相反；
以AUV机头前进方向为坐标系的X轴、机器前进方向的左方向为坐标系Y轴，以机体竖直向下方向为坐标系的Z轴，建立机体水平方向姿态的机体坐标系，水平方向的e至h号推进器控制X、Y轴位移以及机体的偏航角，设e、f、g、h号推进器的推力分别为F5、F6、F7、F8，设e至h号推进器正转的方向对应推进器所需推力F的正向推力，AUV水下无缆机器人水平方向姿态稳定需满足的条件如式(16)：
Figure FDA0003152003410000071
其中Ux和Uy分别表示X轴和Y轴方向的推力和，单位为N，Uψ表示偏航角的角力矩，单位为N*m，R为e至h号推进器中心至AUV水下无缆机器人机体中心的距离，单位为m，
将式(11)代入式(16)中，可得式(17)：
Figure FDA0003152003410000072
实现控制时，需根据机体本身所需的Ux、Uy和Uψ通过对式(17)进行反解得出ω5、ω6、ω7、ω8，但由于有四个推进器的值需要求解，因此采用最小二范数解可得式(18)：
Figure FDA0003152003410000073
Figure FDA0003152003410000081
根据式(18)求出e至h推进器所需要的转速，即可实现AUV水下无缆机器人X、Y水平方向的移动和偏航角ψ的控制。
6.根据权利要求3所述的水下机器人在水中高可视度情况下的姿态与运动控制方法，其特征在于，步骤四中，机体的加速度和角速度通过下述公式(19)和(20)计算出机体的俯仰角、翻滚角和偏航角，其中机体加速度测量姿态的原理是利用静止时刻感受到重力加速度在机体坐标系下各轴分量计算姿态，机体角速度测量姿态的原理是通过对机体坐标系下各轴角速度积分计算姿态，最终将二者计算出的姿态信息经过卡尔曼滤波后的结果作为机体实际姿态角：
Figure FDA0003152003410000082
Figure FDA0003152003410000083
其中，Φacc、θacc为通过机体加速度解算出的翻滚角和俯仰角，ax、ay、az分别为机体坐标系下三轴加速度；Φgyro、θgyro和Ψgyro为通过机体角速度解算出的翻滚角、俯仰角和偏航角，
Figure FDA0003152003410000084
分别为机体坐标系下翻滚角速度、俯仰角速度、偏航角速度，Φgyro(n+1)、θgyro(n+1)、Ψgyro(n+1)和Φgyro(n)、θgyro(n)、Ψgyro(n)分别为n+1、n时刻的三轴姿态角，ΔΦgyro、Δθgyro、ΔΨgyro分别为n时刻至n+1时刻三轴姿态角的增量，Δt为n时刻至n+1时刻之间的时间间隔。
7.根据权利要求3所述的水下机器人在水中高可视度情况下的姿态与运动控制方法，其特征在于，步骤五中，机体的水平位置信息通过单目摄像头获取并计算求得，单目摄像头通过USB2.0接口与可移动地面站相连，其功能是识别AUV机体并对其进行定位，求解机体X轴方向和Y轴方向的位置信息，由于单目摄像头常为鱼眼摄像头，其获取的视频流画面存在畸变，因此需要对单目摄像头进行畸变矫正，其具体标定方法为每隔十厘米在测量尺上进行标记，并记录水面实际位置坐标与单目摄像头图像流的像素位置坐标，经过拟合后可得到一个近似的三次函数关系，机体项部大面积浮力材料为白色，采用图像处理对AUV水下机器人进行定位，将RGB视频流转换为灰度视频流，经过高斯滤波和三次3*3卷积核的腐蚀和膨胀消除图像噪声，并通过灰度二值化后提取白色区域面积，设定固定的白色区域面积值域，识别图中符合该值域的白色面积并对其进行最小外接椭圆拟合，计算出椭圆圆心并经过三次多项式曲线将摄像头坐标系下的像素坐标转换成机体的折射倒影中心位置坐标，
通过下述公式(21)计算出机体折射角：
Figure FDA0003152003410000091
其中，r为折射角，h为摄像头中心与水面的距离，x，y为摄像头获得的机体折射倒影中心位置坐标；
根据水中折射原理公式(22)计算机体折射倒影入射角：
Figure FDA0003152003410000092
其中，α为入射角，r为折射角；
根据机体深度和入射角，通过式(23)计算出机体的实际位置坐标：
Figure FDA0003152003410000093
其中，x′和y′为别为机体的实际位置坐标，x和y分别为摄像头获得的机体折射倒影中心位置坐标，d为摄像头中心与水面的距离，α为入射角。
CN202110769162.1A 2021-10-08 2021-10-08 水下机器人及其水中高可视度情况下姿态与运动控制方法 Active CN113602462B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110769162.1A CN113602462B (zh) 2021-10-08 2021-10-08 水下机器人及其水中高可视度情况下姿态与运动控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110769162.1A CN113602462B (zh) 2021-10-08 2021-10-08 水下机器人及其水中高可视度情况下姿态与运动控制方法

Publications (2)

Publication Number Publication Date
CN113602462A true CN113602462A (zh) 2021-11-05
CN113602462B CN113602462B (zh) 2022-09-23

Family

ID=78304145

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110769162.1A Active CN113602462B (zh) 2021-10-08 2021-10-08 水下机器人及其水中高可视度情况下姿态与运动控制方法

Country Status (1)

Country Link
CN (1) CN113602462B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114354082A (zh) * 2022-03-18 2022-04-15 山东科技大学 一种基于仿鲟鱼吻须的海底管道智能循迹系统和循迹方法
CN114954862A (zh) * 2022-07-01 2022-08-30 扬州工业职业技术学院 一种带缆水下机器人运动控制方法
CN115230839A (zh) * 2022-09-19 2022-10-25 中国铁路设计集团有限公司 爬壁机器人推力控制方法
CN117184382A (zh) * 2023-11-07 2023-12-08 成都诸元天成智能装备有限公司 用于潜航器实时姿态的稳定系统

Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH06335876A (ja) * 1993-05-26 1994-12-06 Mitsubishi Heavy Ind Ltd 水中ロボット位置・姿勢制御装置
CN104155991A (zh) * 2014-08-25 2014-11-19 南京工程学院 水下机器人位姿控制方法
CN105404303A (zh) * 2015-12-28 2016-03-16 河海大学常州校区 一种rov水下机器人的运动控制方法
CN105676867A (zh) * 2016-04-21 2016-06-15 南京工程学院 一种rov水下机器人悬浮姿态稳定控制方法
CN107499476A (zh) * 2017-08-21 2017-12-22 江苏科技大学 水下机器人控制系统及运动控制方法
CN107957727A (zh) * 2016-10-17 2018-04-24 江苏舾普泰克自动化科技有限公司 水下机器人控制系统及动力定位方法
CN108089589A (zh) * 2017-11-24 2018-05-29 江苏科技大学 一种水下机器人姿态控制方法
CN111913489A (zh) * 2019-05-10 2020-11-10 欧舶智能科技（上海）有限公司 水下机器人多推进器耦合控制方法及系统
CN112793741A (zh) * 2020-09-30 2021-05-14 北京机电工程研究所 一种基于rs485总线的八推进器水下机器人控制系统

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH06335876A (ja) * 1993-05-26 1994-12-06 Mitsubishi Heavy Ind Ltd 水中ロボット位置・姿勢制御装置
CN104155991A (zh) * 2014-08-25 2014-11-19 南京工程学院 水下机器人位姿控制方法
CN105404303A (zh) * 2015-12-28 2016-03-16 河海大学常州校区 一种rov水下机器人的运动控制方法
CN105676867A (zh) * 2016-04-21 2016-06-15 南京工程学院 一种rov水下机器人悬浮姿态稳定控制方法
CN107957727A (zh) * 2016-10-17 2018-04-24 江苏舾普泰克自动化科技有限公司 水下机器人控制系统及动力定位方法
CN107499476A (zh) * 2017-08-21 2017-12-22 江苏科技大学 水下机器人控制系统及运动控制方法
CN108089589A (zh) * 2017-11-24 2018-05-29 江苏科技大学 一种水下机器人姿态控制方法
CN111913489A (zh) * 2019-05-10 2020-11-10 欧舶智能科技（上海）有限公司 水下机器人多推进器耦合控制方法及系统
CN112793741A (zh) * 2020-09-30 2021-05-14 北京机电工程研究所 一种基于rs485总线的八推进器水下机器人控制系统

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114354082A (zh) * 2022-03-18 2022-04-15 山东科技大学 一种基于仿鲟鱼吻须的海底管道智能循迹系统和循迹方法
CN114354082B (zh) * 2022-03-18 2022-05-31 山东科技大学 一种基于仿鲟鱼吻须的海底管道智能循迹系统和循迹方法
CN114954862A (zh) * 2022-07-01 2022-08-30 扬州工业职业技术学院 一种带缆水下机器人运动控制方法
CN115230839A (zh) * 2022-09-19 2022-10-25 中国铁路设计集团有限公司 爬壁机器人推力控制方法
CN117184382A (zh) * 2023-11-07 2023-12-08 成都诸元天成智能装备有限公司 用于潜航器实时姿态的稳定系统
CN117184382B (zh) * 2023-11-07 2024-01-19 成都诸元天成智能装备有限公司 用于潜航器实时姿态的稳定系统

Also Published As

Publication number Publication date
CN113602462B (zh) 2022-09-23

Similar Documents

Publication Publication Date Title
CN113602462B (zh) 水下机器人及其水中高可视度情况下姿态与运动控制方法
CN105676867B (zh) 一种rov水下机器人悬浮姿态稳定控制方法
CN103942383B (zh) 一种深海作业型水下机器人的动力学和运动学估计方法
CN104648685B (zh) 基于智能手机的四旋翼飞行器指定路径航拍系统和方法
CN105466477A (zh) 一种面向卫星目标和恒星目标的天基观测模拟系统及方法
CN207991560U (zh) 一种一体化模块
CN110744541A (zh) 一种视觉引导的水下机械臂控制方法
CN103868648A (zh) 三轴气浮仿真实验平台的质心测量方法
CN113189875B (zh) 一种基于圆形特征的无人机在移动平台上鲁棒着陆方法
CN102087110A (zh) 微型水下运动体自主姿态检测装置及方法
Hai et al. Investigation on the mechanical design and manipulation hydrodynamics for a small sized, single body and streamlined I-AUV
CN106840143B (zh) 一种判别水下机器人姿态稳定的方法
CN114692520B (zh) 一种面向多场景的无人艇虚拟仿真测试平台及测试方法
Zavari et al. Early stage design of a spherical underwater robotic vehicle
Tao et al. Modeling and control of swing oscillation of underactuated indoor miniature autonomous blimps
Hu et al. Construction of a digital twin system for the blended-wing-body underwater glider
CN112197765B (zh) 一种实现水下机器人精细导航的方法
CN106094842B (zh) 一种基于t-s模型和pdc的uuv垂直面运动h∞控制方法
Gao et al. Mechatronic design and maneuverability analysis of a novel robotic shark
CN115755939A (zh) 一种四旋翼水下航行器前进运动状态估计方法
CN108227723A (zh) 一种水下机器人及其稳定性分析及结构优化的应用方法
CN112241170B (zh) 基于并联六自由度平台的无人艇自稳定系统
Chaturvedi et al. RETRACTED: Gimbal orientation study and assessment for the autonomous underwater vehicles
CN111532398A (zh) 一种关于无人船的模拟与观测算法
CN110286688A (zh) 一种带有机械臂的水下四旋翼无人机控制方法

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