[发明专利]一种基于手机蓝牙技术的水下机器人控制系统

摘要

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

主权项

一种基于手机蓝牙技术的水下机器人控制系统，其特征在于包括设于水下机器人载体内的处理系统、封装在浮标内的蓝牙装置、手机上的水下机器人控制端；其中手机上的水下机器人控制端通过封装在浮标内的蓝牙装置与水下机器人通讯，浮标与水下机器人通过零浮力缆连接进行数据传输，水下机器人与零浮力缆用水密接插件连接，实现干插拔；设于水下机器人载体内的处理系统主控板通过深度传感器获取到的水下机器人实际深度以及姿态控制板获取到的实际姿态来控制螺旋桨的转速及转向，最终实现水下悬停、定深；所述的水下机器人实际深度值获取方法是：设于水下机器人载体内的处理系统主控板用12位单通道模数转换ADC连续采集深度传感器中的电压信号，直接内存存取控制器DMA传输ADC转换值，并将其保存在ADC寄存器中，主控板定时将ADC寄存器中的ADC转换值取出并作进一步处理，获取水下机器人所处深度值的公式如下：Drob=hV2-V1×(Vref×Vadc4096-V1)]]>其中：Vadc为ADC转换值，Vref为处理器电压参考值，传感器深度量程0‑h所对应的线性直流电压为V1‑V2，Drob为深度值；所述的水下机器人姿态信息获取方法是：设于水下机器人载体内的处理系统主控板处理器通过串口获取姿态控制板处理器解算出的姿态信息，其中：姿态控制板上的嵌入式微处理器通过I2C总线采集三轴陀螺仪、三轴加速度计、三轴磁力计寄存器中相对应的三轴角速度、三轴加速度、三轴磁场强度九轴数据，并将获取到的九轴数据进行姿态解算，得出水下机器人的航向、俯仰和横摇姿态信息，可通过以下算法得到：系统复位姿态初始化，通过三轴加速度计采集到的三轴加速度数据[ax ay az]T解算出初始欧拉角再用初始欧拉角初始化四元数[q0 q1 q2 q3]T；初始化后，用经过滤波、归一化处理和融合三轴加速度计、三轴磁力计数据后的角速度[wx wy wz]T来更新四元数，通过四元数解算得到当前欧拉角，如此不断采样、处理，更新当前的姿态信息；其中定义ψ、θ分别为水下机器人在中心点沿着Z、Y、X轴旋转的角度，对应为航向、俯仰、横摇姿态信息；所述的角速度融合加速度计、磁力计的方法是：利用四元数将地球的重力加速度旋至水下机器人载体坐标系上，将其经过坐标转换后的加速度参考值[gbx gby gbz]T与加速度计测量到的三轴加速度数据[ax ay az]T做外积，所得到的值作为加速度的误差参考值[eax eay eaz]T；由于地磁场的三轴磁场强度在不同位置大小不同，故需实时测算出其准确值，再如同加速度计，利用四元数将地磁场三轴磁场强度的准确值旋至水下机器人载体坐标系上，将其经过坐标转换后的磁场强度参考值[mbx mby mbz]T与磁力计测量到的三轴磁场强度数据[mx my mz]T做外积，所得到的值作为磁场强度的误差参考值[emx emy emz]T；最后使用PI算法融合加速度误差参考值和磁场强度误差参考值至角速度上，采用融合后的角速度值更新四元数；其中，上述数值均做过滤波和归一化处理；所述的地磁场三轴磁场强度准确值的测算方法是：利用四元数将磁力计测量到的三轴磁场强度数据[mx my mz]T旋至地球坐标系中，将其经过坐标转换后的值[mdx mdy mdz]T作分解，得到地磁场三轴磁场强度的准确值[mcx mcy mcz]T，分解公式如下：mcx=mdx2+mdy2,mcy=0,mcz=mdz.]]>