本发明涉及水下机器人技术，旨在提供一种四旋翼球状水下机器人的控制方法。是在水下机器人运行过程中，由电机驱动模块通过导线分别连接电池、控制板和推进器的电机以驱动螺旋桨运行，在此过程中，基本控制模块依据各传感器获取的信息计算水下机器人的航向，并向电机驱动模块传送实现PID控制的信号。基于本发明所述水下机器人的控制方法，能够利用相应控制算法实现推进器推力分配，进而实现水下机器人不少于四个自由度的运动，具有定深悬停，直行，旋转，侧移能力。
1.一种四旋翼球状水下机器人的控制方法，其特征在于，是基于下述四旋翼球状水下机器人而实现的：该水下机器人包括中空的机身主体，以及由电机和螺旋桨组成的推进器；
机身主体由两个半球形的耐压壳体组成，耐压壳体之间通过螺栓紧固的法兰盘和密封垫圈实现密封连接；在法兰盘上设置多个水平突出部作为安装支架，在其中两个安装支架上分别装有摄像机和照明灯；摄像机和照明灯固定在独立的外部夹具上，外部夹具固定安装在安装支架上；在另外四个等距布置的悬臂状的安装支架上各安装一组推进器，推进器采用数控永磁直流无刷电机带动3叶螺旋桨；机身主体底部对称装有两个压力探头装置，两者之间夹角呈90度；耐压壳体的内部装有椭球形电池、圆形双层贴片器件的控制板、存储卡、湿插拔电池电源开关和集中数控电机驱动模块，电机驱动模块通过导线分别连接电池、控制板和推进器的电机以驱动螺旋桨；所述机身主体内部设内部安装支架，电池、控制板和电机驱动模块由内部安装支架实现限位或安装；其中，控制板上装有基本控制模块、无线通讯模块、磁罗盘、姿态传感器、深度传感器和电源控制模块；基本控制模块分别通过导线连接其它模块和传感器，以实现数据采集、控制、通讯和供电；
在水下机器人运行过程中，由电机驱动模块通过导线分别连接电池、控制板和推进器的电机以驱动螺旋桨运行，在此过程中，基本控制模块依据各传感器获取的信息计算水下机器人的航向，并向电机驱动模块传送实现PID控制的信号；
其中，在推进器产生推力保持不变的前提下，水下机器人的纵向运动扰动方程为：
Aij为水下机器人推进器结构参数，式中s是拉氏变换中的复变量，i＝1或2，j＝1、2、3、4或5，i表示水下推进器纵向平面的两个维度，j表示水下机器人推进器的结构特征维度；q(s)为水下机器人的纵倾角速率，Aij与q(s)均为水下机器人的状态量，能分别通过测速仪和水下机器人的磁罗盘测量和计算得到；δe(s)为水下机器人的航向角，α(s)为水下机器人的水动力角，二者是控制输入量；
通过上式进一步获得纵倾角θ速度的传递函数 以及冲角α传递函数
基本控制模块给出的控制输出量通过下述公式计算获得：
设固定坐标系和运动坐标系分别用E-XEYEZE和o-xyz来表示，运动坐标系向固定坐标系的平动速度转换矩阵T1和转动速度转换矩阵T2分别为：式中，ψ为航向角，θ为纵倾角， 为横倾角；纵向运动时，
故得出 纵倾角θ的传递函数 式中的s是拉氏变换中的复变量；
在纵倾角小于5度、冲角小于20度的情况下，可以得到深度ZE与航速V和纵倾角θ的关系：
在上式中，各符号的含义是：ZE表示深度， 表示对深度的微分，即单位时间下潜的深度，V代表水下机器人航速，在固定坐标系下是个矢量，u、w分别代表前向速度和垂向速度，α代表水下机器人的冲角，θ代表水下机器人的纵倾角；
在法兰盘上设有多个水密插接件，其一端通过导线连接内部设备，另一端通过水密导线连接外部设备；所述摄像机自带wifi通讯模块和存储模块；控制板上设磁力开关，磁力开关通过电源控制模块控制电池供电的通断；所述深度传感器包括两个不同量程范围的深度传感器；所述机身主体内部装有压载材料；所述耐压壳体是亚克力材质；推进器的电机通过水密导线和设于法兰盘上的水密插接件与安装在机身主体内部的电机驱动模块相连；所述水下机器人的耐压壳体径向尺寸不超过20cm，整体尺寸不超过30×30×30cm，在空气中的重量小于5kg，设计使用深度不超过30m；所述电池采用狮子7.4V 2S锂电池，电池容量
5200MAH；所述推进器为朗宇A2212系列KV980无刷直流电机，配备4.8×55mm三叶螺旋桨；所述无线通讯模块为NRF24L01无线模块；所述电机驱动模块采用天空SKY 30A四合一电调；所述基本控制模块采用STM32F103 ZET6单片机，且连接了8G的SD存储卡；所述深度传感器采用科赛沃引线式压力传感器，量程范围分别为0～10m和0～50m；所述磁罗盘为SEC315 40度倾角补偿式三维电子磁传感器；所述摄像机为Remax SD-02潜水相机，通过透明有机玻璃外壳进行防水；所述照明灯有两个，为MBHTV ZZ-AAZ0-D8DJ潜水手电。
一种四旋翼球状水下机器人的控制方法
技术领域
[0001] 本发明涉及水下机器人技术领域，尤其涉及一种无缆自主式水下机器人的控制方法。该水下机器人是一种小尺度、智能化、能够在水下自动航行的小型球状水下航行器。
背景技术
[0002] 机器人技术是一种新兴的智能制造技术，在国内外受到广泛应用。大疆无人机和云洲智能无人船的兴起就说明了这一点。自主水下机器人是一种能够在水面以下几米甚至上千米、上万米深度进行自主航行的机器人，具有自动航行、自主导航、自主执行水下任务的能力。与无人机、无人车和无人船相比，它所处的任务环境更加复杂，所使用的传感器和推进系统也有不同。具体表现在：1)高频无线电波在水下十几米以下深度无法使用，将会导致操作人员与水下机器人难以保持良好的通信联系；而采用水声通信机，不仅成本高，且通信速率和容量都无法与无线电波相比；2)电子元部件等任何非水密电子部件、机械部件、推进部件均须做水密、耐压保护，否则将发生渗水、漏水导致不能正常水下航行。
[0003] 目前，水下机器人用户基本上全部是国家、地方或者企业、高校，价格十分昂贵，体积和重量都很大，装载的传感器大多为科学仪器。本专利是一种专门面向水下信息采集的小微球状水下机器人，具有专门的摄像功能、灵活的运动能力以及很强的实用性。
发明内容
[0004] 本发明要解决的技术问题是，克服现有技术中的不足，提供一种四旋翼球状水下机器人的控制方法。
[0005] 为解决技术问题，本发明的解决方案是：
[0006] 提供一种四旋翼球状水下机器人的控制方法，是基于下述四旋翼球状水下机器人而实现的：该水下机器人包括中空的机身主体，以及由电机和螺旋桨组成的推进器；机身主体由两个半球形的耐压壳体组成，耐压壳体之间通过螺栓紧固的法兰盘和密封垫圈实现密封连接；在法兰盘上设置多个水平突出部作为安装支架，在其中两个安装支架上分别装有摄像机和照明灯；在另外四个等距布置的悬臂状的安装支架上各安装一组推进器，推进器采用数控永磁直流无刷电机带动3叶螺旋桨；机身主体底部对称装有两个压力探头装置，两者之间夹角呈90度；耐压壳体的内部装有椭球形电池、圆形双层贴片器件的控制板、存储卡、湿插拔电池电源开关和集中数控电机驱动模块，电机驱动模块通过导线分别连接电池、控制板和推进器的电机以驱动螺旋桨；所述机身主体内部设内部安装支架，电池、控制板和电机驱动模块由内部安装支架实现限位或安装；其中，控制板上装有基本控制模块、无线通讯模块、磁罗盘、姿态传感器和电源控制模块；基本控制模块分别通过导线连接其它模块或传感器，以实现数据采集、控制、通讯和供电；
[0007] 在水下机器人运行过程中，由电机驱动模块通过导线分别连接电池、控制板和推进器的电机以驱动螺旋桨运行，在此过程中，基本控制模块依据各传感器获取的信息计算水下机器人的航向，并向电机驱动模块传送实现PID控制的信号；
[0008] 其中，在推进器产生推力保持不变的前提下，水下机器人的纵向运动扰动方程为：
[0009]
[0010] 式中：
[0011] Aij为水下机器人推进器结构参数，式中s是拉氏变换中的复变量，i＝1或2，j＝1、2、3、4或5，i表示水下推进器纵向平面的两个维度，j表示水下机器人推进器的结构特征维度；q(s)为水下机器人的纵倾角速率，Aij与q(s)均为水下机器人的状态量，能分别通过测速仪和水下机器人的磁罗盘测量和计算得到；δe(s)为水下机器人的航向角，α(s)为水下机器人的水动力角，二者是控制输入量；
[0012] 通过上式进一步获得纵倾角θ速度的传递函数 以及冲角α传递函数[0013] 本发明中，在法兰盘上设有多个水密插接件，其一端通过导线连接内部设备，另一端通过水密导线连接外部设备。
[0014] 本发明中，所述摄像机自带wifi通讯模块和存储模块；控制板上设磁力开关，磁力开关通过电源控制模块控制电池供电的通断；所述深度传感器包括两个不同量程范围的深度传感器。
[0015] 本发明中，所述机身主体内部装有压载材料。
[0016] 本发明中，所述耐压壳体是亚克力材质；推进器的电机通过水密导线和设于法兰盘上的水密插接件与安装在机身主体内部的电机驱动模块相连。
[0017] 本发明中，所述水下机器人的耐压壳体径向尺寸不超过20cm，整体尺寸不超过30×30×30cm，在空气中的重量小于5kg，设计使用深度不超过30m。
[0018] 本发明中，所述摄像机和照明灯固定在独立的外部夹具上，外部夹具固定安装在安装支架上(可以调整照明角度和摄像角度)。
[0019] 本发明中，所述电池采用狮子7.4V 2S锂电池，电池容量5200MAH；所述推进器为朗宇A2212系列KV980无刷直流电机，配备4.8×55mm三叶螺旋桨；所述无线通讯模块为NRF24L01无线模块；所述电机驱动模块采用天空SKY 30A四合一电调；所述基本控制模块采用STM32F103ZET6单片机，且连接了8G的SD存储卡；所述深度传感器采用科赛沃引线式压力传感器，量程范围分别为0～10m和0～50m；所述磁罗盘为SEC315 40度倾角补偿式三维电子磁传感器；所述摄像机为Remax SD-02潜水相机，通过透明有机玻璃外壳进行防水；所述照明灯有两个，为MBHTV ZZ-AAZ0-D8DJ潜水手电。
[0020] 本发明中，基本控制模块给出的控制输出量通过下述公式计算获得：
[0021] 设固定坐标系和运动坐标系分别用E-XEYEZE和o-xyz来表示，运动坐标系向固定坐标系的平动速度转换矩阵T1和转动速度转换矩阵T2分别为：
[0022]
[0023]
[0024] 式中，ψ为航向角，θ为纵倾角， 为横倾角；纵向运动时，
[0025] 故得出 纵倾角θ的传递函数 式中的s是拉氏变换中的复变量；
[0026] 在纵倾角小于5度、冲角小于20度的情况下，可以得到深度ZE与航速V和纵倾角θ的关系：
[0027]
[0028] 在上式中，各符号的含义是：ZE表示深度， 表示对深度的微分，即单位时间下潜的深度，V代表水下机器人航速，在固定坐标系下是个矢量，u、w分别代表前向速度和垂向速度，α代表水下机器人的冲角，θ代表水下机器人的纵倾角。
[0029] 与现有技术相比，本发明具有以下有益效果及优点：
[0030] 1、本发明尺寸小，重量轻，操作简便，无需吊车、船等其它设备辅助，没有任何专业背景的个人即可操作。
[0031] 2、本发明价格低廉，有明确的用途和使用价值，便于推广。
[0032] 3、本发明易于加工、生产，工艺简单，便于快速投放市场。
[0033] 4、本发明能够完成水下摄像、水下照明、定深悬停等功能，具有很强的娱乐性、实用性和趣味性。
[0034] 5、本发明主体为球形，耐压性能好，样式新颖美观。
[0035] 6、本发明基于所述水下机器人的控制方法，能够利用相应控制算法实现推进器推力分配，进而实现水下机器人不少于四个自由度的运动，具有定深悬停，直行，旋转，侧移能力。
附图说明
[0036] 图1是本发明的组成示意图；
[0037] 图2是本发明的信息采集系统组成示意图；
[0038] 图3是本发明的外形结构及各单元安装位置示意图。
[0039] 图3中的附图标记：1耐压壳体、2控制板、3电池、4法兰盘、5推进器、6照明灯、7电机驱动模块、8摄像机、9深度传感器。
具体实施方式
[0040] 首先需要说明的是，本发明涉及机器人技术。在本发明的实现过程中，可能会涉及到多个软件功能模块的应用。申请人认为，如在仔细阅读申请文件、准确理解本发明的实现原理和发明目的以后，在结合现有公知技术的情况下，本领域技术人员完全可以运用其掌握的软件编程技能实现本发明。凡本发明申请文件提及的均属此范畴，申请人不一一列举。另外，本发明的实现依赖于多种电子元器件的应用，而这些电子元器件均为现有技术，且有成熟产品可市场购置获得，例如下面提到的基本控制模块、电机驱动模块、推进器、无线通讯模块、磁罗盘、电源控制模块、摄像机、照明灯、深度传感器，等等。
[0041] 本发明提供的四旋翼球状水下机器人，包括结构系统、运动控制系统和信息采集系统。
[0042] 结构系统：包括由两个透明亚克力材质的半球形耐压壳体1及法兰盘4、密封垫圈组成的机身主体，内置了内部安装支架、压载材料和椭球形电池3。圆形双层贴片器件的控制板2、存储卡、湿插拔电池电源开关和集中数控电机驱动模块7，电机驱动模块7通过导线分别连接电池3、控制板2和推进器的电机以驱动螺旋桨；推进器5一共四个，等间距安装在法兰盘4上突出设置的悬臂状安装支架上，各推进器5相互之间的连线构成正方形。推进器采用数控永磁直流无刷电机带动3叶螺旋桨。在法兰盘4上设有多个水密插接件，其一端通过导线连接内部设备，另一端通过水密导线连接外部设备。
[0043] 运动控制系统：圆形双层贴片器件的控制板2、电机驱动模块7与电池3均由内部安装支架实现限位或安装；其中，控制板2上装有基本控制模块、无线通讯模块、磁罗盘、姿态传感器和电源控制模块，基本控制模块分别通过导线连接其它模块或传感器，以实现数据采集、控制、通讯和供电。控制板2上设磁力开关，磁力开关通过电源控制模块控制电池3供电的通断；推进器5的电机与电机驱动模块相连。基本控制模块中内置无线数据下载和程序上传模块(该模块是软件功能模块)。
[0044] 信息采集系统包括磁罗盘、设于耐压壳体外部的摄像机8、照明灯6和两个不同量程范围的深度传感器9；磁罗盘、深度传感器9分别通过信号线连接至基本控制模块；摄像机8和照明灯6为独立模块，下水前需打开各自的开关，摄像机8自带wifi通讯模块和存储模块。摄像头8、照明灯6固定在独立的外部夹具上，外部夹具固定安装在法兰盘4的安装支架上(可以根据需要自由调整照明角度与摄像角度)。在机身主体底部对称装有两个压力探头装置，二者之间夹角呈90度。
[0045] 各设备的选型示例：电池3采用狮子7.4V 2S锂电池，电池容量5200MAH；推进器5为朗宇A2212系列KV980无刷直流电机，配备4.8*55mm三叶螺旋桨；基本控制模块采用STM32F103ZET6单片机，且连接了8G的SD存储卡；无线通讯模块为NRF24L01无线模块；深度传感器9采用科赛沃引线式压力传感器，量程范围分别为0～10m和0～50m；磁罗盘为SEC315 40度倾角补偿式三维电子磁传感器。电机驱动模块7采用天空SKY 30A四合一电调；摄像机8为Remax SD-02潜水相机，通过透明有机玻璃外壳进行防水；照明灯6有两个，为MBHTV ZZ-AAZ0-D8DJ潜水手电。水下机器人的耐压壳体径向尺寸不超过20cm，整体尺寸不超过30×30×30cm，在空气中的重量小于5kg，设计使用深度不超过30m。
[0046] 四旋翼球状水下机器人的控制方法包括：由电机驱动模块通过导线分别连接电池3、控制板和推进器5的电机以驱动螺旋桨运行，在此过程中，基本控制模块依据各传感器获取的信息计算水下机器人的航向，并向电机驱动模块7传送实现PID控制的信号；
[0047] 其中，在推进器产生推力保持不变的前提下，水下机器人的纵向运动扰动方程为：
[0048]
[0049] 式中：
[0050] Aij为水下机器人推进器结构参数，式中s是拉氏变换中的复变量，i＝1或2，j＝1、2、3、4或5，i表示水下推进器纵向平面的两个维度，j表示水下机器人推进器的结构特征维度；q(s)为水下机器人的纵倾角速率，Aij与q(s)均为水下机器人的状态量，能分别通过测速仪和水下机器人的磁罗盘测量和计算得到；δe(s)为水下机器人的航向角，α(s)为水下机器人的水动力角或冲角，二者是控制输入量；
[0051] 通过上式进一步获得纵倾角(θ)速度的传递函数 冲角(α)传递函数[0052] 基本控制模块依据各传感器获取的信息控制推进器的启停，进而实现对水下机器人的定深控制，其控制输出量通过下述公式计算获得：
[0053] 固定坐标系和运动坐标系分别用E-XEYEZE和o-xyz来表示，运动坐标系向固定坐标系的平动速度转换矩阵T1和转动速度转换矩阵T2分别为：
[0054]
[0055]
[0056] 式中，ψ为航向角，θ为纵倾角， 为横倾角；纵向运动时，
[0057] 故得出 纵倾角(θ)的传递函数 式中的s是拉氏变换中的复变量；
[0058] 在纵倾角小于5度、冲角小于20度(水下机器人冲角一般为3度到15度)的情况下，可以得到深度(ZE)与航速(V)和纵倾角(θ)的关系：
[0059]
[0060] 在上式中，各符号的含义是：ZE表示深度， 表示对深度的微分，即单位时间下潜的深度,V代表水下机器人的实际航速，在固定坐标系下是个矢量，u、w分别代表航速在纵向平面的两个分量，α代表水下机器人的冲角，θ代表水下机器人的纵倾角。
[0061] 本发明中的水下机器人的主要用户为水产养殖企业、水下工程企业、科研院所以及大中专高校。可用于检查海底管道的损伤情况，实现海底或水底石油管道无人探测、天然气管道等其它海底管道的自动识别与自动探伤。它是一种无人无缆水下航行器，其浮力比重力大。通过控制算法和推进器5推力分配设计水下机器人运动控制系统，实现水下机器人不少于四个自由度的运动，具有定深悬停，直行，旋转，侧移能力。
[0062] 水下机器人在深度传感器9和磁罗盘指引下，利用基本控制模块调节推进器5的推力和力矩，实现自主航行；电池3为机器人提供电子部件电源和推进器5的动力，支持机器人长时间水下续航；信息采集系统在基本控制模块控制下采集沿途及水底的图像信息，通过采用磁罗盘和深度传感器9来获取水下机器人航向、姿态和速度、深度信息。
[0063] 具体实施例子：
[0064] 在耐压外壳1的底部安装两个不同量程的深度传感器9，分别工作于0～10m的浅水和10～30m的深水，内部安装支架的最上层是控制板2及磁罗盘等，控制板2下方安装电池3，电池3下方安装电机驱动模块7。两个半球状耐压壳体1的法兰盘4之间设密封垫圈，由螺栓实现紧固。法兰盘4上的安装支架固定安装了四个由电机和螺旋桨组成的推进器5，装在法兰盘4上的外部支架上，还对称安放防水的照明灯6和摄像机8。水下机器人首先利用深度传感器9检测所处深度，根据设定深度下潜上浮，当到达指定深度后，进行姿态调整，利用姿态传感器反馈姿态信息，定深定向航行，利用摄像机8采集沿途水下环境。工作一段时间后，设备停止工作，自动上浮。
