CN113306685B - 一种水下仿生球形/半球形机器人的位姿感知方法 - Google Patents

Abstract

本发明提出一种水下仿生球形/半球形机器人的位姿感知方法，该机器人包括一球形/半球形外壳、一压力传感器阵列，压力传感器阵列沿外壳周向布置，该方法包括：步骤1，确定压力传感器阵列中的迎水面压力传感器，读取迎水面压力传感器的数据，计算各个迎水面压力传感器感测的压力；步骤2，将各个迎水面压力传感器感测的压力、水流对机器人迎水面的压力、以及读数最大的压力传感器的感受面垂直方向与水流方向的夹角为参数，求解机器人的运动速度和运动方向；步骤3，根据机器人在世界坐标系O_E下的运动速度，以及机器人的姿态信息，计算机器人的位置。本发明解决了小型化水下机器人无法实现运动速度和运动方向感知的问题，且实现成本低。

Description

一种水下仿生球形/半球形机器人的位姿感知方法

技术领域

本发明属于水下机器人感知领域，具体涉及一种水下仿生球形/半球形机器人的位姿感知方法。

背景技术

水下环境复杂限制人类对海洋的探索，特别是深海区域，缺乏光线，高压，缺氧，人类难以在这样的极端环境下作业。因此，人们开始越来越多的把探索海洋的重任交给水下机器人去完成。当前常用的水下机器人为大型水下潜器，如AUV、ROV或UUV等，可搭载空间大，携带设备多，如DVL、光纤陀螺、声呐等，具备较强的自身姿态感知能力。依据此感知信息作为控制系统反馈量，可实现水下机器人任务操作。但大型水下潜器体积大，采用螺旋桨推进，产生噪音，隐蔽性差；并且对狭窄空间内任务，难以完成。

小型仿生机器人采用仿生推进机制，如喷水、波动鳍等，噪音小，推进效率高，机体体积小，隐蔽性好，并且适合狭窄空间内任务。但是小型机器人体积小，传感器搭载空间有限。而常用DVL、声呐、光纤陀螺和水声定位设备等，需要较强的换能器，导致其尺寸较大，无法安装在小型机器人上。低成本IMU传感器尺寸小，可以用于小型水下机器人位姿感知。但是对于水下运动速度及移动方向目前没有较好的处理方法。因此需要为小型水下机器人设计一种位姿感知方法。

发明内容

本发明的目的是提出一种水下仿生球形/半球形机器人的位姿感知方法，能够利用较低的成本和较小的体积对机器人的水下运动实现运动速度和运动方向的感知。

有鉴于此，本发明提供了一种水下仿生球形/半球形机器人的位姿感知方法，所述机器人包括一球形/半球形外壳、一压力传感器阵列，所述压力传感器阵列沿所述外壳周向布置，所述方法包括：

步骤1，确定所述压力传感器阵列中的迎水面压力传感器，读取所述迎水面压力传感器的数据，计算各个所述迎水面压力传感器感测的压力；

步骤2，将各个所述迎水面压力传感器感测的压力、水流对机器人迎水面的压力、以及读数最大的压力传感器的感受面垂直方向与水流方向的夹角为参数，求解所述机器人的运动速度和运动方向；

步骤3，根据所述机器人在世界坐标系O_E下的运动速度，以及所述机器人的姿态信息，计算机器人的位置。

优选地，所述压力传感器阵列沿所述外壳周向等角度间隔分布。

优选地，所述压力传感器阵列至少由4个压力传感器组成，且至少在所述机器人的正前方、正后方、正左方和正右方分别设置所述压力传感器。

优选地，所述压力传感器阵列由12个压力传感器组成。

优选地，所述压力传感器的感受面垂直方向平行于所述机器人的赤道面。

优选地，在所述步骤1中，所述“确定所述压力传感器阵列中的迎水面压力传感器”包括：

读取所述压力传感器阵列中各个压力传感器的数据，选择数值较大的前N/2个压力传感器作为迎水面压力传感器，其中N为压力传感器个数。

优选地，在所述步骤2中，求解所述机器人的运动速度和运动方向具体包括：

以各个所述迎水面压力传感器感测的压力、水流对机器人迎水面的压力F_W、以及读数最大的压力传感器的感受面垂直方向与水流方向的夹角α为参数，求得多组F_W和α的解；

根据多组F_W解的平均值

求解所述机器人的运动速度

其中，

ρ为水下环境密度，S为压力传感器感受面横截面积；

根据多组α解的平均值

求解所述机器人的运动方向

其中，β为读数最大的压力传感器的感受面垂直方向与机器人正前方的夹角。

优选地，在所述步骤3之前，还包括：将所述机器人在机器人坐标系O_B下的运动速度_bv_t＝[v_t cosα_t,v_t sinα_t]^T，转换成所述机器人在世界坐标系O_E下的运动速度_ev_t＝R×_bv_t，其中

ψ为机器人的偏航角。

优选地，所述机器人还包括一深度传感器，在所述步骤3中，所述机器人的位置P_t+1＝[x_t+1,y_t+1,z_t+1]表示为：

其中，P₀＝[x₀,y₀,z₀]表示世界坐标系O_E下所述机器人的初始位置；Δt表示采样的时间间隔；

表示深度传感器压力/压强读数；m和n为参数，通过所述深度传感器压强/压力读数和深度标定实验，经线性拟合获得。

优选地，所述方法还包括：步骤4，根据所述机器人的位置判断所述机器人是否到达目标位置，若到达目标位置，则结束位姿感知；否则，返回步骤1。

通过以上技术方案，本发明能够取得以下技术效果：

1.压力传感器沿机器人球形/半球形外壳周向等间距布置，具有高度对称性，可以感知来自各个方向的压力，提升了小型水下机器人的感知能力。

2.压力传感器的体积小、价格低，通过本发明提出的方法能够利用较低的成本和较小的体积对机器人的水下运动实现运动速度和运动方向的感知。

附图说明

图1是本发明实施例提供的一种水下仿生球形/半球形机器人的结构示意图；

图2是本发明实施例提供的压力传感器阵列周向分布示意图；

图3是本发明实施例提供的压力传感器及其封装示意图；

图4是本发明实施例提供的水下仿生球形/半球形机器人的位姿感知方法示意图；

图5是本发明实施例提供的机器人水下运动压力情况示意图；

图6是本发明实施例提供的正压力区压力传感器受力分析示意图；

图7是本发明实施例提供的机器人坐标系与世界坐标系示意图；

图8是本发明实施例提供的机器人位置估计示意图；

图9是本发明实施例提供的一种水下仿生球形/半球形机器人的组成单元示意图。

图中，100-水下仿生球形/半球形机器人、10-半球形外壳、11-顶部外壳、12-密封外壳、13-双目摄像头、14-压力传感器、15-防水外壳、16-压力传感器感受面、20-腿部机构、30-底盘、301-处理器、302-存储器。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明的水下仿生球形/半球形机器人100包括球形外壳或半球形外壳10，具有轴向高度对称性。如图1所示，半球形外壳10位于机器人100的上半部，压力传感器阵列12沿半球形外壳10周向分布。优选的，压力传感器阵列12至少由4个压力传感器14(P1，P2，P3，P4......PN，N≥4)组成，且压力传感器14等间隔地沿机器人外壳10周向分布。在一优选的实施例中，压力传感器阵列12至少在机器人100的正前方、正后方、正左方和正右方分别设置压力传感器14。优选地，各个压力传感器的感受面垂直方向平行于机器人100的赤道面。如图2所示，压力传感器阵列12由12个压力传感器14组成，包括压力传感器P1、P2......P12，相邻的压力传感器14周向间隔30°。机器人100还包括一双目摄像头13朝向机器人正前方，机器人100包括腿部机构20，腿部机构20共有四条腿，可用于机器人100的水下爬行运动。优选地，腿部机构20的一端连接于机器人的底盘30上，另一端设置喷水推进器，可用于机器人的水下三维运动。

如图1所示，机器人100的半球形外壳10与底盘30连接构成密封舱，压力传感器14安装在机器人100的密封舱外壁上。如图3所示，压力传感器14采用防水外壳15包裹，并灌胶防水。压力传感器采用STM32f103VET6微处理器通过SPI通信方式读取，为节省微处理器引脚资源，采用4-16译码器实现四个引脚控制12个压力传感器14轮询读取，微处理器通过串口将数据发送到嵌入式电脑，发送频率为20Hz。

机器人100的密封舱外壁具有容置压力传感器14及其防水外壳15的孔，压力传感器的防水外壳15呈台阶状，尺寸较小端嵌入密封舱壁，尺寸较大端抵接在密封舱外壁上，压力传感器抵接于防水外壳内，并使用防水胶填充防水外壳15与密封舱外壁、防水外壳15与压力传感器14的间隙，使之固定连接。压力传感器的感受面16露出，用于感测水下压力。在一优选的实施例中，半球形外壳10可包括顶部外壳11和密封外壳12两部分。顶部外壳11固定在密封外壳12之上，密封外壳12的尺寸及其与底盘30形成的密封舱容积取决于机器人100的质量，以满足机器人100可在无推进力的情况下沉在水底。

实施例1

参见图4，图4是本发明提供的一种水下仿生球形/半球形机器人100的位姿感知方法示意图。本发明的水下仿生球形/半球形机器人100的位姿感知方法，包括：

步骤1，确定压力传感器阵列12中的迎水面压力传感器，读取迎水面压力传感器的数据，计算各个迎水面压力传感器感测的压力；

步骤2，将各个迎水面压力传感器感测的压力、水流对机器人100迎水面的压力F_W、以及读数最大的压力传感器的感受面垂直方向与水流方向的夹角α为参数建立方程组，求解机器人100的运动速度和运动方向；

步骤3，根据机器人100在世界坐标系O_E下的运动速度，以及机器人100的姿态信息，计算机器人100的位置。

实施例2

如图5-6所示，本发明的水下仿生球形/半球形机器人100的压力传感器阵列12包括12个压力传感器，以机器人100向前运动为例，迎水面的压力传感器Pⁱ,…,Pⁱ⁺⁵受到水冲击的作用，压力增加形成正压力区；背水面的压力传感器压力值降低，形成负压力区。具体的，水流对机器人100的迎水面压力为F_W，各个压力传感器14的沿球形/半球形外壳10的切向和径向分力分别为F_L ⁱ,…,F_L ⁱ⁺⁵和F_P ⁱ,…,F_P ⁱ⁺⁵。然而，压力传感器14仅能感知径向压力。机器人100迎水面的各个压力传感器测量到的压强为p₀ ⁱ，…，p₀ ⁱ⁺⁵，则各个压力传感器测量到的水流压强为p^i,…,i+5＝p₀ ^i,…,i+5-p₀，其中，p₀为机器人100静止在一定水深H下所受的压强。故而，各个压力传感器测得的压力为F_p ^i,…,i+5＝p^i,…,i+5×S，其中，S为压力传感器感受面横截面积，为定值。

机器人100的位姿感知方法具体包括以下步骤。

步骤1，确定压力传感器阵列12中的迎水面压力传感器，读取迎水面压力传感器的数据，计算各个迎水面压力传感器感测的压力。具体的，由于机器人100的迎水面压强高于背水面压强，可以在读取压力传感器阵列12中各个压力传感器的压强值后，对数据进行卡尔曼滤波，将读取的N个压强值由大到小排序(N为压力传感器个数)，选取数值较大的前N/2个压力传感器作为迎水面压力传感器。在本实施例中，压力传感器阵列12包括12个传感器，故而，读取各个压力传感器数据后，选取数值较大的6个传感器数据，并计算对应的压力传感器测得的压力F_p ^i,…,i+5＝p^i,…,i+5×S。

步骤2，将各个迎水面压力传感器感测的压力、水流对机器人迎水面的压力F_W、以及读数最大的压力传感器的感受面垂直方向与水流方向的夹角α为参数建立方程组，求解机器人100的运动速度和运动方向，其中，α∈[-π/N π/N]。在本实施例中，α∈[-π/12 π/12]。在本实施例中，迎水面压力传感器受力分析如图6所示，求解机器人100运动速度的过程具体包括，建立如下关系：

两两联立求解F_W与α，共计15组解。然后通过对15组数据求取平均值，易得，

因此，时刻t时，机器人迎水面受到压强p_t为：

其中S为压力传感器感受面横截面积。

机器人迎水面受到压强p_t与机器人100运动速度的关系为：

其中ρ为水下环境密度。

可得，机器人100的运动速度为：

如图7所示，世界坐标系及机器人本体坐标系，分别记为O_E-X_EY_EZ_E和O_B-X_BY_BZ_B，符合右手定则。时刻t时，读数最大的压力传感器的感受面垂直方向与机器人100正前方的夹角为β，则机器人100在机器人坐标系O_B下的运动方向为

从而，可得某一时刻t时，在机器人坐标系O_B下，水下机器人100的运动速度v_t和运动方向α_t。

步骤3，根据机器人100在世界坐标系O_E下的运动速度，以及机器人100的姿态信息，计算机器人100的位置。具体的，机器人100姿态信息通过读取位于机器人100赤道面中心的姿态传感器得到。机器人姿态的翻滚角、俯仰角和偏航角分别为[φ,θ,ψ]^T。由于机器人100采用半球形设计，结构高度对称，浮心高，重心较低，机器人100具有很好的稳定性，在水中运动时，翻滚角和俯仰角的角度较小可以忽略。在步骤3之前，还包括坐标系转换，具体的，时刻t时，机器人100在机器人坐标系O_B下的运动速度_bv_t＝[v_t cosα_t,v_t sinα_t]^T，则机器人100在世界坐标系O_E下的运动速度

可通过式_ev_t＝R×_bv_t获得。其中，

ψ为机器人的偏航角。

从而，基于航位推算对机器人100的位置进行估计。如图8所示，图8为基于航位推算的机器人位置估计示意图。机器人100还包括一深度传感器，深度传感器的感受面平行于机器人100的赤道面设置，用于感知机器人100静止在一定水深H下所受的压强p₀。机器人100的位置P_t+1＝[x_t+1,y_t+1,z_t+1]表示为：

其中，P₀＝[x₀,y₀,z₀]表示世界坐标系O_E下机器人100的初始位置；Δt表示采样的时间间隔；

表示深度传感器压力/压强读数；m和n为参数，通过深度传感器压强/压力读数和深度标定实验，经线性拟合获得。

步骤4，根据机器人100的位置判断机器人100是否到达目标位置，若到达目标位置，则结束位姿感知，否则，返回步骤1。

实施例3

参见图9，一种水下仿生球形/半球形机器人100，包括处理器301、存储器302以及存储在所述存储器302中且被配置为由所述处理器301执行的计算机程序，所述处理器301执行所述计算机程序时实现如上述的位姿感知方法。

示例性的，所述计算机程序可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器302中，并由所述处理器301执行，以完成本发明。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序在所述机器人100中的执行过程。

需说明的是，以上所描述的实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。

以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也视为本发明的保护范围。

Claims (8)

1.一种水下仿生球形/半球形机器人的位姿感知方法，所述机器人包括一球形/半球形外壳、一压力传感器阵列，所述压力传感器阵列沿所述外壳周向布置，所述方法包括：

步骤1，确定所述压力传感器阵列中的迎水面压力传感器，读取所述迎水面压力传感器的数据，计算各个所述迎水面压力传感器感测的压力；

步骤2，将各个所述迎水面压力传感器感测的压力、水流对机器人迎水面的压力、以及读数最大的压力传感器的感受面垂直方向与水流方向的夹角为参数，求解所述机器人的运动速度和运动方向；

步骤3，根据所述机器人在世界坐标系O_E下的运动速度，以及所述机器人的姿态信息，计算机器人的位置；

在所述步骤1中，所述“确定所述压力传感器阵列中的迎水面压力传感器”包括：

读取所述压力传感器阵列中各个压力传感器的数据，选择数值较大的前N/2个压力传感器作为迎水面压力传感器，其中N为压力传感器个数；

在所述步骤2中，求解所述机器人的运动速度和运动方向具体包括：

以各个所述迎水面压力传感器感测的压力、水流对机器人迎水面的压力F_W、以及读数最大的压力传感器的感受面垂直方向与水流方向的夹角α为参数，求得多组F_W和α的解；

根据多组F_W解的平均值

求解所述机器人的运动速度

其中，

ρ为水下环境密度，S为压力传感器感受面横截面积；

根据多组α解的平均值ā，求解所述机器人的运动方向

其中，β为读数最大的压力传感器的感受面垂直方向与机器人正前方的夹角。

2.根据权利要求1所述的方法，其特征在于，所述压力传感器阵列沿所述外壳周向等角度间隔分布。

3.根据权利要求2所述的方法，其特征在于，所述压力传感器阵列至少由4个压力传感器组成，且至少在所述机器人的正前方、正后方、正左方和正右方分别设置所述压力传感器。

4.根据权利要求3所述的方法，其特征在于，所述压力传感器阵列由12个压力传感器组成。

5.根据权利要求1所述的方法，其特征在于，所述压力传感器的感受面垂直方向平行于所述机器人的赤道面。

6.根据权利要求1所述的方法，其特征在于，在所述步骤3之前，还包括：将所述机器人在机器人坐标系O_B下的运动速度_bv_t＝[v_tcosα_t,v_tsinα_t]^T，转换成所述机器人在世界坐标系O_E下的运动速度_ev_t＝R×_bv_t，其中

ψ为机器人的偏航角。

7.根据权利要求1所述的方法，其特征在于，所述机器人还包括一深度传感器，在所述步骤3中，所述机器人的位置P_t+1＝[x_t+1,y_t+1,z_t+1]表示为：

其中，P₀＝[x₀,y₀,z₀]表示世界坐标系O_E下所述机器人的初始位置；Δt表示采样的时间间隔；

表示深度传感器压力/压强读数；m和n为参数，通过所述深度传感器压强/压力读数和深度标定实验，经线性拟合获得。

8.根据权利要求1所述的方法，其特征在于，所述方法还包括：步骤4，根据所述机器人的位置判断所述机器人是否到达目标位置，若到达目标位置，则结束位姿感知；否则，返回步骤1。

Images