CN104199459B - 一种基于手机蓝牙技术的水下机器人控制系统 - Google Patents

Abstract

本发明公开一种基于手机蓝牙技术的水下机器人控制系统。该系统包括设于水下机器人载体内的处理系统、封装在浮标内的蓝牙装置、手机上的水下机器人控制端；其中水下机器人控制端通过蓝牙装置与水下机器人通讯，浮标与水下机器人通过零浮力缆连接进行数据传输，处理系统主控板通过深度传感器获取到的水下机器人实际深度以及姿态控制板获取到的实际姿态来控制螺旋桨的转速及转向，最终实现水下悬停、定深等功能。本系统能够很好地克服现有的水下机器人在自治模式下实时性的不足和有缆模式中灵活性上的限制、满足系统的实时通讯需要，并为便携地获取水下机器人信息、保证水下机器人灵活运动提供了很好的硬件支持。

Description

一种基于手机蓝牙技术的水下机器人控制系统

技术领域

本发明涉及水下机器人领域，具体来说是一种基于手机蓝牙技术的水下机器人控制系统。

背景技术

目前，水下机器人大多采用自治或者有缆控制模式。自治模式，多用在无缆机器人上，机器人按照事先设定或者根据不定时接收到的上位机指令进行水下作业，这样就不能实时地获取水下信息和控制机器人；而有缆模式，就是机器人通过与水面母船相连的脐带缆获得动力，并与母船上的上位机进行数据传输和人机交互，它可以实现对机器人的实时控制，但其活动受到脐带缆的制约，特别是复杂水下环境下容易造成缠绕事故，灵活性不够。

Android是一种以Linux内核为基础的开源操作系统，主要用于智能手机、平板电脑等移动设备。Android开发，以JAVA为主要编程语言，API接口丰富，开发较为便捷灵活。Android手机，基于Android操作系统，具有强大的运算能力和友好的用户界面，支持语言通讯、GPS定位、拍照、蓝牙、WIFI、GPRS等功能，满足各种复杂数据处理、实时数据更新、数据收集保存检索等需求，在工业生产作业中作为新的手持数据终端，它起着不可替代的作用。

发明内容

本发明针对现有水下机器人在自治模式下实时性的不足和有缆模式中灵活性上的限制,提供一种基于手机蓝牙技术的水下机器人控制系统，该系统不仅能便携实时地控制水下机器人和获取水下机器人的信息，而且能保证其灵活性。

本发明是通过以下技术方案实现的：

本发明涉及一种基于手机蓝牙技术的水下机器人控制系统，包括：设于水下机器人载体内的处理系统、封装在浮标内的蓝牙装置、手机上的水下机器人控制端；其中手机上的水下机器人控制端通过封装在浮标内的蓝牙装置与水下机器人通讯，浮标与水下机器人通过零浮力缆连接进行数据传输，水下机器人与零浮力缆用水密接插件连接，实现干插拔；

所述的设于水下机器人载体内的处理系统由主控板、姿态控制板、深度传感器、电源模块以及推进模块等组成；主控板，作为系统的控制中心，由嵌入式微处理器及其外围电路组成，包括：嵌入式微处理器、串行通信UART、模数转换A/D、输入输出IO口、电源电路等，其中：串行通信UART分别与浮标内的蓝牙模块、姿态控制板相连，模数转换A/D与深度传感器相连，输入输出IO口与伺服驱动器相连，电源电路为主控板提供工作电压，并获取电源的电量信息；姿态控制板将嵌入式微处理器、三轴陀螺仪、三轴加速度计和三轴磁力计集为一体，用来获取水下机器人载体内温度和姿态信息(横摇、纵倾和航向等)；深度传感器用来获取水下机器人所处的深度；电源模块包括直流电源、电量检测电路、换压电路、充电电路、系统开关装置等，为主控板、姿态控制板、深度传感器、推进模块以及蓝牙装置供电；推进模块包括伺服电机、伺服驱动器和螺旋桨等，分为六个独立舱室，对称分布，实现水下机器人上升、下潜、横滚、纵倾和转轴五个自由度的运动，其中伺服驱动器内设有编码器电路，用来反馈螺旋桨的转速和转向；所述的蓝牙装置可采用水晶滴胶将其封装在可由有机玻璃管制作的浮标内，蓝牙与处理系统主控板采用串口通信方式，其中：手机上的水下机器人控制端通过蓝牙装置向处理系统主控板传送指令，采用串口中断方式；处理系统主控板通过蓝牙装置向手机上的水下机器人控制端发送数据，采用定时器中断方式。

所述的水下机器人控制端，基于Android2.3或更高版本的手机，主界面由显示区和日历、天气、地图、拍照、记录、设置、电池状态以及蓝牙状态等图标组成，其中：显示区实时显示水下机器人自身姿态、所处深度、载体内温度以及手机所处方位；日历、天气、地图等图标，用于保存水下机器人的操作日期、当前天气和所处位置；拍照图标，用于调用手机系统中自带的拍照功能；记录图标，用于保存水下机器人的自身姿态、所处深度等信息；设置图标，用于设定水下机器人的运动状态，实现遥控功能；电池状态图标，显示水下机器人中的电源状态以及电量；蓝牙状态图标，显示蓝牙的连接状况和信号强度。

本发明涉及一种基于上述系统的控制方法：一方面，手机上的水下机器人控制端通过水面蓝牙向水下机器人发送目标深度、目标姿态等控制指令，处理系统主控板响应串口中断，接收目标深度、目标姿态等信息，并根据深度传感器获取到的水下机器人实际深度以及姿态控制板获取到的实际姿态来控制螺旋桨的转速及转向，最终实现水下悬停、定深等功能；另一方面，处理系统主控板通过水面蓝牙将水下机器人的自身姿态、所处深度、载体内温度以及电源状态等数据发送到手机上控制端，实时显示。

所述的实际深度获取方法是：处理系统主控板用12位单通道模数转换ADC连续采集深度传感器中的电压信号，直接内存存取控制器DMA传输ADC转换值，并将其保存在ADC寄存器中，主控板定时将ADC寄存器中的ADC转换值取出并作进一步处理，获取水下机器人所处的深度值。

所述的实际姿态获取方法是：主控板处理器通过串口获取姿态控制板处理器解算出的姿态信息，其中姿态控制板上的嵌入式微处理器通过I2C总线采集三轴陀螺仪、三轴加速度计、三轴磁力计寄存器中相对应的三轴角速度、三轴加速度、三轴磁场强度等九轴数据，并将获取到的九轴数据进行姿态解算，得出水下机器人的航向、俯仰和横摇等姿态信息。

控制水下机器人使其自身姿态和所处深度在目标设定值内，就可保证机器人运动的可控性和稳定性。

本发明的有益效果是：

本发明提供的一种基于手机蓝牙技术的水下机器人控制系统，能够很好地克服现有的水下机器人在自治模式下实时性的不足和有缆模式中灵活性上的限制、满足系统的实时通讯需要，并为便携地获取水下机器人信息、保证水下机器人灵活运动提供了很好的硬件支持。该系统使用安装有水下机器人控制端的手机代替传统的上位机，通过水面上封装在浮标内的蓝牙装置与水下机器人进行通讯。浮标与水下机器人用零浮力缆连接，有缆的存在，它可以弥补自治模式下实时性的不足，而用仅仅当作传送数据的零浮力缆替换能够拖曳的脐带缆，可以减少缆的制约，增强水下机器人的运动灵活性。采用手机蓝牙通讯实现上位机和浮标二者之间的无线数据传输，避免了繁琐的线缆拖曳，将手机作为专门的上位机，简易便捷，同时该系统采用嵌入式微处理器，并进行层次化设计，姿态控制板处理器专门负责大量的姿态信号的分析处理和通讯，大为减轻了主控板处理器的工作负荷，提高了系统的可靠性。

附图说明

图1是本发明的系统连接示意图；

图2是本发明的结构示意图；

图3是本发明手机上的水下机器人控制端主界面示意图；

图4是本发明手机上的水下机器人控制端的主程序流程图；

图5是本发明手机上的水下机器人控制端触摸设置图标的系统响应程序流程图；

图6是本发明手机上的水下机器人控制端触摸日历天气地图拍照记录等图标的手机响应程序流程图；

图1中1.手机，2.浮标，3.水面，4.零浮力缆，5.水下机器人；

图3中6.日历图标，7.天气图标，8.地图图标，9.拍照图标，10.记录图标，11.设置图标，12.电池状态图标，13.蓝牙状态图标，14.手机所处方位，15.底层，16.水下机器人所处深度与载体内温度，17.深度标尺，18.水下机器人姿态信息，19.显示区。

具体实施方式

下面对本发明的实施例作详细说明，本实施例在以本发明技术方案为前提下进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。

实施例1

如图1所示，本实施例一种基于手机蓝牙技术的水下机器人控制系统包括：设于水下机器人5载体内的处理系统、封装在浮标2内的蓝牙装置、手机1上的水下机器人控制端；其中手机1上的水下机器人控制端通过封装在浮标2内的蓝牙装置与位于水面3下方的水下机器人5通讯，水下机器人5与浮标2用零浮力缆4连接，水下机器人5与零浮力缆4用水密接插件相连，实现干插拔。浮标2与手机1的距离不大于30米，在实际操作中可查看图3中手机1上的水下机器人控制端主界面上的蓝牙状态图标13，根据蓝牙连接状况和信号强度调整手机1和浮标2的距离，使水下机器人5在可控范围内。

实施例2

如图2所示，设于水下机器人5载体内的处理系统包括：主控板、姿态控制板、深度传感器、电源模块、推进模块等，其中：主控板处理器分别通过串口和模数转换A/D获取姿态控制板处理器解算出的姿态信息和深度信号。

所述的主控板，作为系统的控制中心，由嵌入式微处理器STM32F103VCT6及其外围电路组成，包括：嵌入式微处理器STM32F103VCT6、串行通信UART、模数转换A/D、输入输出IO口、电源电路等，其中：串行通信UART分别与浮标2内的蓝牙模块、姿态控制板相连，模数转换A/D与深度传感器相连，输入输出IO口与伺服驱动器相连，电源电路为主控板提供工作电压，并获取电源的电量信息；

所述的姿态控制板，包括：嵌入式微处理器STM32F103VCT6、三轴陀螺仪ITG3205、三轴加速度计ADXL345、三轴磁力计HMC5883L及其外围电路等，其中：嵌入式微处理器STM32F103VCT6通过I2C总线采集三轴陀螺仪ITG3205、三轴加速度计ADXL345、三轴磁力计HMC5883L寄存器中相对应的三轴角速度、三轴加速度、三轴磁场强度等九轴信息的二进制补码数据，并完成对九轴数据的处理，用来获取水下机器人5载体内姿态信息；另外，三轴陀螺仪中的温度传感器以数字量输出载体内的温度。

所述的深度传感器，包括压敏元件和转换元件以及转换电路等，将水压转换成主控板可测范围内的直流电压，并使水下机器人5所处深度和输出电压呈线性关系，用来获取水下机器人5所处的深度。

所述的电源模块，为系统的各个模块提供合适的电源，包括：直流电源、换压电路、电量检测电路、充电电路和系统开关装置等，其中：电源采用DC12V，换压电路实现12V-5V和12-3.3V的电压转换，电量检测电路用来获取电源的电量，充电电路为系统所需电源充电提供保护，系统开关使用磁控方式实现对电源的非接触式控制。

所述的推进模块，包括：伺服电机、伺服驱动器和螺旋桨等，分为六个独立舱室，对称分布，实现水下机器人上升、下潜、横滚、纵倾和转轴等五个自由度的运动，其中：伺服驱动器内设有编码器电路，用来反馈螺旋桨的转速和转向。

所述的蓝牙模块，可用水晶滴胶将其封装在可由有机玻璃管制作的浮标内，蓝牙与主控板用零浮力缆连接，采用串口通信方式，工作电压3.3V，串口波特率默认为9600位每秒，通讯距离不大于30米。

所述的手机1上的水下机器人控制端，基于Android2.3或更高版本的手机，主界面如图3所示，由显示区19和日历、天气、地图、拍照、记录、设置、电池状态以及蓝牙状态等图标组成。其中：显示区19实时显示水下机器人5的姿态信息18、所处深度与载体内温度16以及手机所处方位14，深度标尺17可自动调节，底层15为动态显示的背景；日历6、天气7、地图8等图标，保存水下机器人5的操作日期、当前天气和所处位置；拍照图标9，调用手机系统中自带的拍照功能；记录图标10，保存水下机器人5的自身姿态和所处深度等信息；设置图标11，设定水下机器人5的运动状态，实现遥控功能；电池状态图标12，显示水下机器人5中的电源状态以及电量；蓝牙状态图标13，显示蓝牙的连接状况和信号强度。

如图4所示，实施例2通过以下方式实现具体控制：

步骤S1，打开手机1上的水下机器人控制端，手机1上的水下机器人控制端初始化；

步骤S2，搜索蓝牙设备，判断有否搜到浮标内的蓝牙设备，若有则执行步骤S3，否则执行步骤S2；

步骤S3，点击确认，输入浮标内蓝牙设备的匹配PIN值，判断是否配对成功，若是则执行步骤S4，否则执行步骤S3；

步骤S4，确认手机蓝牙与浮标内的蓝牙进行连接；

步骤S5，显示手机1上的水下机器人控制端主界面，显示区实时显示水下机器人自身姿态、所处深度、载体内温度以及手机所处方位等信息，判断是否有相关操作，若有则执行相应的响应动作，否则执行S5。

在执行相应的响应动作中，若有按退出键则退出手机控制端；若有触摸图标，则进行图标响应，其中：图标响应分为设置图标和非设置图标响应两种，其对应的程序流程图分别如图5和图6所示。

若有触摸设置图标，则进入遥控模式，对水下机器人的运动状态进行设置，可依据获取到的水下机器人信息对机器人作左转、右转、深潜、后退、前进、悬停、定深等指令控制，主控板收到指令的目标设定值，并根据传感器获取到深度和姿态等数据来控制螺旋桨的转向及转速，从而使水下机器人的运动状态符合目标设定值。

所述的深度获取方法是：主控板用12位单通道模数转换ADC连续采集深度传感器中的电压信号，直接内存存取控制器DMA传输ADC转换值，并将其保存在ADC寄存器中，主控板定时将ADC寄存器中的ADC转换值取出并作进一步处理，获取水下机器人所处深度值的公式如下：

其中：V_adc为ADC转换值，V_ref为处理器电压参考值，传感器深度量程0-h所对应的线性直流电压为V₁-V₂，D_rob为深度值。

所述的姿态获取方法是：主控板处理器通过串口获取姿态控制板处理器解算出的姿态信息，其中：姿态控制板上的嵌入式微处理器STM32F103VCT6通过I2C总线采集三轴陀螺仪ITG3205、三轴加速度计ADXL345、三轴磁力计HMC5883L寄存器中相对应的三轴角速度、三轴加速度、三轴磁场强度等九轴信号，并将获取到的九轴数据进行姿态解算，得出水下机器人的航向、俯仰和横摇等姿态信息。

所述的姿态解算方法是：系统复位姿态初始化，通过加速度计采集到的三轴加速度数据[a_x a_y a_z]^T解算出初始欧拉角再用初始欧拉角初始化四元数[q₀q₁q₂q₃]^T；初始化后，用经过滤波、归一化处理和融合加速度计、磁力计数据后的角速度[w_x w_y w_z]^T来更新四元数，通过四元数解算得到当前欧拉角，如此不断采样、处理，更新当前的姿态信息；其中：定义ψ、θ分别为水下机器人在中心点沿着Z、Y、X轴旋转的角度，对应为航向Yaw、俯仰Pitch、横摇Roll等姿态信息。

所述的角速度融合加速度计、磁力计的方法是：利用四元数将地球的重力加速度旋至水下机器人载体坐标系上上，将其经过坐标转换后的加速度参考值[g_bx g_by g_bz]^T与加速度计测量到的三轴加速度数据[a_x a_y a_z]^T做外积，所得到的值作为加速度的误差参考值[e_ax e_ay e_az]^T；由于地磁场的三轴磁场强度在不同位置大小不同，故需实时测算出其准确值，再如同加速度计，利用四元数将地磁场三轴磁场强度的准确值旋至水下机器人载体坐标系上，将其经过坐标转换后的磁场强度参考值[m_bx m_by m_bz]^T与磁力计测量到的三轴磁场强度数据[m_x m_y m_z]^T做外积，所得到的值作为磁场强度的误差参考值[e_mx e_my e_mz]^T；最后使用PI算法融合加速度误差参考值和磁场强度误差参考值至角速度上，采用融合后的角速度值更新四元数；其中，上述数值均做过滤波和归一化处理。

所述的地磁场三轴磁场强度准确值的测算方法是：利用四元数将磁力计测量到的三轴磁场强度数据[m_x m_y m_z]^T旋至地球坐标系中，将其经过坐标转换后的值[m_dx m_dy m_dz]^T作分解，得到地磁场三轴磁场强度的准确值[m_cx m_cy m_cz]^T，分解公式如下：

触摸设置图标的系统响应程序流程图，如图5所示，步骤如下：

步骤S1，手机控制端显示设置图标响应界面，判断是否有指令操作，若是则执行步骤S2，否则执行步骤S1；

步骤S2，手机控制端发送指令到浮标内的蓝牙装置；

步骤S3，主控板处理器收到串口中断信息，主控板处理器根据目标设定值与传感器测量值所得到的误差给出相应响应，电机带动螺旋桨运动，判断是否达到目标设定值，若是则执行步骤S4，否则执行S3；

步骤S4，主控板停止设置响应。

若有触摸日历天气地图拍照记录等非设置图标，则进入其相对应的功能操作，触摸日历图标保存当前日期，触摸天气图标保存当前天气状况，触摸地图图标保存手机的所处位置，触摸拍照图标调用手机系统的拍照功能，触摸记录图标则从此刻起保存水下机器人的自身姿态和所处深度等信息，再次触摸则停止记录。

触摸日历天气地图拍照记录等非设置图标的手机响应程序流程图，如图6所示，步骤如下：

步骤S1，手机控制端由主界面转入日历天气地图拍照记录等非设置图标相对应的响应界面；

步骤S2，日历天气地图拍照记录等功能相对应的操作，判断操作是否完成，若是则执行步骤S3，否则执行步骤S2；

步骤S3，返回手机控制端的主界面。

Claims (4)

1.一种基于手机蓝牙技术的水下机器人控制系统，其特征在于包括设于水下机器人载体内的处理系统、封装在浮标内的蓝牙装置、手机上的水下机器人控制端；其中手机上的水下机器人控制端通过封装在浮标内的蓝牙装置与水下机器人通讯，浮标与水下机器人通过零浮力缆连接进行数据传输，水下机器人与零浮力缆用水密接插件连接，实现干插拔；设于水下机器人载体内的处理系统主控板通过深度传感器获取到的水下机器人实际深度以及姿态控制板获取到的实际姿态来控制螺旋桨的转速及转向，最终实现水下悬停、定深；

所述的水下机器人实际深度值获取方法是：设于水下机器人载体内的处理系统主控板用12位单通道模数转换ADC连续采集深度传感器中的电压信号，直接内存存取控制器DMA传输ADC转换值，并将其保存在ADC寄存器中，主控板定时将ADC寄存器中的ADC转换值取出并作进一步处理，获取水下机器人所处深度值的公式如下：

$D_{rob}=\frac{h}{V_2-V_1}\×(V_{ref}\×\frac{V_{adc}}{4096}-V_1)$

其中：V_adc为ADC转换值，V_ref为处理器电压参考值，传感器深度量程0-h所对应的线性直流电压为V₁-V₂，D_rob为深度值；

所述的水下机器人姿态信息获取方法是：设于水下机器人载体内的处理系统主控板处理器通过串口获取姿态控制板处理器解算出的姿态信息，其中：姿态控制板上的嵌入式微处理器通过I2C总线采集三轴陀螺仪、三轴加速度计、三轴磁力计寄存器中相对应的三轴角速度、三轴加速度、三轴磁场强度九轴数据，并将获取到的九轴数据进行姿态解算，得出水下机器人的航向、俯仰和横摇姿态信息，可通过以下算法得到：

系统复位姿态初始化，通过三轴加速度计采集到的三轴加速度数据[a_x a_y a_z]^T解算出初始欧拉角再用初始欧拉角初始化四元数[q₀ q₁ q₂ q₃]^T；初始化后，用经过滤波、归一化处理和融合三轴加速度计、三轴磁力计数据后的角速度[w_x w_y w_z]^T来更新四元数，通过四元数解算得到当前欧拉角，如此不断采样、处理，更新当前的姿态信息；其中定义ψ、θ分别为水下机器人在中心点沿着Z、Y、X轴旋转的角度，对应为航向、俯仰、横摇姿态信息；

所述的角速度融合加速度计、磁力计的方法是：利用四元数将地球的重力加速度旋至水下机器人载体坐标系上，将其经过坐标转换后的加速度参考值[g_bx g_by g_bz]^T与加速度计测量到的三轴加速度数据[a_x a_y a_z]^T做外积，所得到的值作为加速度的误差参考值[e_ax e_aye_az]^T；由于地磁场的三轴磁场强度在不同位置大小不同，故需实时测算出其准确值，再如同加速度计，利用四元数将地磁场三轴磁场强度的准确值旋至水下机器人载体坐标系上，将其经过坐标转换后的磁场强度参考值[m_bx m_by m_bz]^T与磁力计测量到的三轴磁场强度数据[m_x m_y m_z]^T做外积，所得到的值作为磁场强度的误差参考值[e_mx e_my e_mz]^T；最后使用PI算法融合加速度误差参考值和磁场强度误差参考值至角速度上，采用融合后的角速度值更新四元数；其中，上述数值均做过滤波和归一化处理；

所述的地磁场三轴磁场强度准确值的测算方法是：利用四元数将磁力计测量到的三轴磁场强度数据[m_x m_y m_z]^T旋至地球坐标系中，将其经过坐标转换后的值[m_dx m_dy m_dz]^T作分解，得到地磁场三轴磁场强度的准确值[m_cx m_cy m_cz]^T，分解公式如下：

$m_{cx}=\sqrt{m_{dx}^2+m_{dy}^2},m_{cy}=0,m_{cz}=m_{dz}.$

2.如权利要求1所述的一种基于手机蓝牙技术的水下机器人控制系统，其特征在于所述的设于水下机器人载体内的处理系统由主控板、姿态控制板、深度传感器、电源模块以及推进模块组成；所述的主控板由嵌入式微处理器及其外围电路组成，包括嵌入式微处理器、串行通信UART、模数转换A/D、输入输出IO口、电源电路，其中串行通信UART分别与封装在浮标内的蓝牙模块、姿态控制板相连，模数转换A/D与深度传感器相连，输入输出IO口与伺服驱动器相连，电源电路为主控板提供工作电压，并获取电源的电量信息；所述的姿态控制板包括嵌入式微处理器、三轴陀螺仪、三轴加速度计和三轴磁力计，用来获取水下机器人载体内温度和姿态信息；所述的深度传感器用来获取水下机器人所处的深度；所述的推进模块包括伺服电机、伺服驱动器和螺旋桨，实现水下机器人上升、下潜、横滚、纵倾和转轴五个自由度运动；所述的电源模块为主控板、姿态控制板、深度传感器、推进模块以及封装在浮标内的蓝牙装置供电。

3.如权利要求1所述的一种基于手机蓝牙技术的水下机器人控制系统，其特征在于所述的手机上的水下机器人控制端主界面由显示区和日历、天气、地图、拍照、记录、设置、电池状态以及蓝牙状态图标组成；其中显示区实时显示水下机器人的自身姿态、所处深度、载体内温度以及手机所处方位，深度标尺可自动调节；日历、天气、地图图标，分别用于保存水下机器人的操作日期、当前天气和所处位置；拍照图标，调用手机系统中自带的拍照功能；记录图标，保存水下机器人的自身姿态和所处深度信息；设置图标，设定水下机器人的运动状态，实现遥控功能；电池状态图标，显示水下机器人中的电源状态以及电量；蓝牙状态图标，显示蓝牙的连接状况和信号强度。

4.如权利要求1所述的一种基于手机蓝牙技术的水下机器人控制系统，其特征在于所述的蓝牙装置封装在浮标内，蓝牙装置与水下机器人载体内的处理系统主控板用零浮力缆连接，采用串口通信方式；其中手机上的水下机器人控制端通过蓝牙装置向处理系统主控板传送指令，采用串口中断方式；处理系统主控板通过蓝牙装置向手机上的水下机器人控制端发送数据，采用定时器中断方式。