一种用于双关节机器鱼的复合避障方法和装置

技术领域

本发明属于机器人避障技术领域，具体涉及一种用于双关节机器鱼的复合避障方法和装置。

背景技术

随着科技的进步和经济的发展，小型机器人的应用越来越广泛，例如搜救、清淤、水质监测等等，在这些应用环境中，机器人面临的环境都是未知且复杂的，因此如何安全高效地避开障碍物是机器人能够完成任务的前提。

水下机器鱼因其灵活便携的特点，在水下探测和水质检测领域具有广阔的应用前景。然而，水下环境是十分复杂且多变的，机器鱼要完成任务首先就要保证自身的安全，即高效合理地避障。

发明内容

本发明要解决的技术问题是：提供一种用于双关节机器鱼的复合避障方法和装置，用于使机器人在未知环境中避障。

本发明为解决上述技术问题所采取的技术方案为：一种用于双关节机器鱼的复合避障方法，包括以下步骤：

S0：搭建用于双关节机器鱼的复合避障装置，包括摆动单元、浮潜单元、控制单元、传感器网络；控制单元的信号接收端连接传感器网络的信号发送端，控制单元的信号发送端分别连接摆动单元和浮潜单元的信号接收端；

摆动单元包括连接杆、第一关节舵机、第二关节舵机和鱼尾，第一关节舵机、第二关节舵机和鱼尾通过连接杆依次连接；

浮潜单元包括依次连接的抽水水泵、水箱和排水水泵；抽水水泵通过抽水水泵进水口连接水源，抽水水泵通过抽水水泵出水口连接水箱的水箱进水口；水箱通过排气口放气调节水箱内外气压平衡；排水水泵通过排水水泵进水口连接水箱的水箱排水口，排水水泵通过排水水泵出水口排水；

控制单元包括MCU、PWM调节模块、速度系数模糊控制器、转向角度模糊控制器和上浮下潜控制器；

传感器网络包括正前方测距传感器F

S1：传感器网络采集双关节机器鱼与环境障碍物的距离信息并分别发送给速度系数模糊控制器、转向角度模糊控制器和上浮下潜控制器；

S2：根据双关节机器鱼的关节摆动角度模型在速度系数模糊控制器和转向角度模糊控制器中建立模糊规则库，并根据模糊规则库分别输出速度系数V和转向角度α；

S3：在上浮下潜控制器中建立上浮下潜规则库，并根据上浮下潜规则库输出上浮距离或下潜距离；

S4：MCU接收步骤S2得到的速度系数V和转向角度α、步骤S3得到的上浮距离或下潜距离，并通过PWM调节模块将控制信号分别发送给摆动单元和浮潜单元执行避障动作。

按上述方案，所述的步骤S2中，建立关节摆动角度模型的具体步骤为：

在双关节机器鱼的游动过程中，第二关节舵机摆动角度的幅值的正弦函数相位滞后于第一关节舵机摆动角度的幅值的正弦函数的相位180°；设第一关节舵机摆动角度为第一关节舵机与轴线的夹角α，第二关节舵机摆动角度为第二关节舵机与轴线的夹角β，第一关节舵机摆动振幅为A

进一步的，所述的步骤S2中，双关节机器鱼以正常速度直线游动时，第一关节舵机与第二关节舵机在满足摆动幅值条件的同时保持周期一致为0.35s的关节摆动角度的数学模型为：

由上式得到，摆动幅值决定游动速度；因此双关节机器鱼减速时，减小α和β的摆动幅值，并在一个摆动周期中通过PWM调节模块的控制信号增加延时以保证摆动周期不变；

设k的范围为[-45°，45°]，则双关节机器鱼转向时关节摆动角度的数学模型为：

进一步的，所述的步骤S2中，建立模糊规则库并输出速度系数V的具体步骤为：

S21：速度系数模糊控制器的输入信号为正前方测距传感器F

l＝min{F

所测距离的最小值l的模糊子集为{NT，N，M，F}，分别对应表示很近，近，中，远；

则根据速度系数模糊规则，模糊规则库一一对应输出速度系数V的模糊子集为{VSM，VS，VM，VN}，分别对应表示最慢速，慢速，中速，正常速度；

S22：速度系数模糊控制器根据所测距离的最小值l匹配速度系数模糊规则，得到输出的速度系数V；

S23：所测距离的最小值l为F时使用梯形隶属函数作为输入隶属函数，所测距离的最小值l为其他值时采用三角隶属函数作为输入隶属函数；输出隶属函数采用三角隶属函数；

S24：采用最大隶属度最大值法lom对模糊规则库输出的速度系数V进行去模糊化处理；在速度系数V的模糊集合的论域中有多个点的隶属度取得最大，将这些点中坐标绝对值最大的点作为最终输出的速度系数V的清晰化数值：

进一步的，所述的步骤S2中，建立模糊规则库并输出转向角度α的具体步骤为：

S25：转向角度模糊控制器的输入信号为左侧测距传感器L

ε＝L

若ε为正值则值越大表明障碍物离机器鱼右侧越近；若ε为负值则值越小表明障碍物离机器鱼左侧越近；

若左侧测距传感器L

ε的模糊子集为{LF，LM，LN，EQ，RN，RM，RF}，分别对应表示左远，左中，左近，相等，右近，右中，右远；

则根据转向角度模糊规则，模糊规则一一对应输出转向角度α的模糊子集为{NB，NM，NS，Z，PS，PM，PB}，分别对应表示负大，负中，负小，零，正小，正中，正大；

S26：转向角度模糊控制器根据计算的距离的差值ε匹配转向角度模糊规则，得到输出的转向角度α；

S27：转向角度模糊控制器的输入隶属函数和输出隶属函数都采用三角隶属函数；

S28：采用最大隶属度最大值法lom对模糊规则库输出的转向角度α进行去模糊化处理；在转向角度α的模糊集合的论域中有多个点的隶属度取得最大，将这些点中坐标绝对值最大的点作为最终输出的转向角度α的清晰化数值：

按上述方案，所述的步骤S3中，具体步骤为：

S31：根据机器鱼的高度、上侧测距传感器T

S32：上浮下潜控制器的输入信号为上侧测距传感器T

若上侧测距传感器T

若上侧测距传感器T

若上侧测距传感器T

S33：上浮下潜控制器输出二值离散变量形式的上浮距离或下潜距离。

按上述方案，所述的步骤S4中，具体步骤为：

S41：MCU接收步骤S2得到的速度系数V和转向角度α、步骤S3得到的上浮距离或下潜距离；

S42：MCU通过PWM调节模块将控制信号发送给摆动单元驱动第一关节舵机和第二关节舵机转向避障；

S43：MCU通过PWM调节模块将控制信号发送给浮潜单元驱动抽水水泵和排水水泵；采用改变重力法，通过改变控制信号占空比的大小控制抽水水泵抽水的速度和排水水泵排水的速度：使抽水水泵往水箱抽水，双关节机器鱼重量增加从而下潜；或使排水水泵将水箱中的水排出，双关节机器鱼重量降低从而上浮；

S44：上浮下潜控制器对比测距传感器返回的数值，判断双关节机器鱼的上浮距离或下潜距离是否符合要求，若是则MCU停止向摆动单元和浮潜单元输出控制命令；若否则执行步骤S41。

进一步的，所述的步骤S4中，

单独执行步骤S42转向避障时，降低速度使双关节机器鱼的转向半径更小；

单独执行步骤S43上浮下潜避障时，设速度系数为1使双关节机器鱼的速度增加或保持最快速度；

若同时出现上述两种情况，转向角度模糊控制器和上浮下潜控制器均有输出信息时，转向避障的优先级高于上浮下潜避障，MCU取速度系数模糊控制器的输出信息控制双关节机器鱼的速度。

一种用于双关节机器鱼的复合避障装置，包括摆动单元、浮潜单元、控制单元、传感器网络；控制单元的信号接收端连接传感器网络的信号发送端，控制单元的信号发送端分别连接摆动单元和浮潜单元的信号接收端；

摆动单元用于实现双关节机器鱼的游动和转向；摆动单元包括连接杆、第一关节舵机、第二关节舵机和鱼尾，第一关节舵机、第二关节舵机和鱼尾通过连接杆依次连接；

浮潜单元用于实现双关节机器鱼的上浮和下潜；浮潜单元包括依次连接的抽水水泵、水箱和排水水泵；抽水水泵通过抽水水泵进水口连接水源，抽水水泵通过抽水水泵出水口连接水箱的水箱进水口；水箱通过排气口放气调节水箱内外气压平衡；排水水泵通过排水水泵进水口连接水箱的水箱排水口，排水水泵通过排水水泵出水口排水；

控制单元用于接收传感器信号并分别向摆动单元和浮潜单元发送控制信号；控制单元包括MCU、PWM调节模块、速度系数模糊控制器、转向角度模糊控制器和上浮下潜控制器；

传感器网络用于测量距离并将采集到的数据发送给控制单元；传感器网络包括正前方测距传感器F

进一步的，第一关节舵机和第二关节舵机采用可0～180°旋转的舵机；传感器采用红外测距传感器。

本发明的有益效果为：

1.本发明的一种用于双关节机器鱼的复合避障方法和装置，根据机器鱼的机械结构，针对双关节机器鱼建立游动转向模型和上浮下潜模型，采用机器鱼的三维运动和机器鱼头部的5个红外测距传感器组成的网络信息为输入信号，对速度和转向角度采用模糊控制，对上浮下潜使用自定义控制规则实现避障，经过综合构成复合避障方法，实现了双关节机器鱼在未知环境中的三维避障功能。

2.本发明提出了将转向与速度的模糊控制和上浮下潜的规则控制相结合的复合避障方法，具有模糊控制鲁棒性、适应性好、规则控制简单直接等的优点。

3.本发明输入的来自红外测距传感器的信息，通过调节红外测距传感器的测量范围来适应机器鱼的尺寸，调节之后本避障方法仍然适用；本发明提出的双关节机器鱼的运动模型也适用于多关节机器鱼，便于推广应用。

4.本发明结合实际情况，给出了解决例如上浮下潜时的速度和转向避障时的速度的矛盾的办法，实现了对机器鱼的转角、速度、上浮下潜距离的精确控制，使机器鱼在三维空间内灵活运动。

附图说明

图1是本发明实施例的双关节机器鱼的鱼尾结构示意图。

图2是本发明实施例的双关节机器鱼的鱼尾摆动示意图。

图3是本发明实施例的双关节机器鱼的上浮下潜结构示意图。

图4是本发明实施例的双关节机器鱼的上浮下潜控制流程图。

图5是本发明实施例的红外测距传感器安装位置示意图。

图6是本发明实施例的速度系数和转向角度的模糊控制框图。

图7是本发明实施例的复合避障控制框图。

图中：1.连接杆；2.第一关节舵机；3.第二关节舵机；4.鱼尾；5.抽水水泵；6.水泵进水口；7.水泵出水口；8.水箱进水口；9.排气口；10.水箱排水口；11.排水水泵。

具体实施方式

下面结合附图和具体实施方式对本发明作进一步详细的说明。

机器鱼的避障机理与其机械结构及运动模型密切相关。参见图1至图3，本发明实施例的双关节机器鱼包括摆动单元、浮潜单元、控制单元；

摆动单元包括连接杆1、第一关节舵机2、第二关节舵机3、鱼尾4；

浮潜单元包括抽水水泵5、水泵进水口6、水泵出水口7、水箱进水口8、排气口9、水箱排水口10、排水水泵11、水箱；

控制单元包括MCU、PWM调节模块。

机器鱼的运动分为游动转向和上浮下潜两个方面。

1.游动转向

双关节机器鱼的游动转向依靠机器鱼的尾部结构完成，尾部结构由两个关节组成，具体结构如图1所示，驱动电机采用可0-180°旋转的舵机。

在游动过程中，两个关节摆动的幅值都为正弦函数，幅值不同，周期不同，第二关节摆动角度幅值的正弦函数相位滞后于第一关节正弦函数的相位180°。设第一关节摆动角度为第一关节与轴线的夹角α，第二关节摆动角度为第二关节与轴线的夹角β，尾部摆动示意图如图2所示，忽略外界环境的影响，对双关节机器鱼进行运动分析可以得到关节摆动角度的数学模型为

式中，A

根据对鳇科鱼类鲤鱼的实际观察，双关节机器鱼在无障碍物正常游动时，α的幅值为20°，β的幅值为40°，周期均为0.25s。但是，根据舵机具体参数计算得出其最大速度无法满足要求，为此选择延长舵机的转动周期，使两个舵机能够在实际参数条件下满足摆动幅值条件的同时保持周期一致，并考虑环境阻力，得出周期为0.35s；最后可得机器鱼以正常速度直线游动时各关节的实际数学模型为

这是正常速度时的运动方程，在减速时，则减小α和β的摆动幅值，并在一个摆动周期的PWM控制信号中增加相应的延时以保证周期基本不变。

转向时的运动方程与直线游动的运动方程类似，据观察，鳇科鱼类鲤鱼在实际转弯时，α的最大幅度约为45°，β的最大幅度约为60°，周期均为0.3s。综合实际转弯情况和机器鱼前游计算得出的参数，将周期设定为0.35s，得到转向的数学模型为，

式中，k根据转向情况固定为一定角度，其范围为[-45°，45°]。

2.上浮下潜

目前控制机器鱼上浮与下潜的方法主要分为改变重心法和改变重力法两类。本发明针对的机器鱼采用了改变重力法，使用水箱、排水水泵11和抽水水泵5构成整个上浮下潜结构如图3所示。采用PWM信号驱动的水泵，控制流程如图4所示，水泵由MCU产生的PWM信号控制，通过占空比的大小控制水泵抽水和排水的速度，抽水水泵5往水箱抽水，机器鱼重量增加从而下潜，反之排水水泵11将水箱中的水排出，机器鱼重量降低从而上浮。

3.模糊避障方法

3.1传感器网络

传感器采用红外测距传感器，由5个红外传感器组成传感器网络，分别位于机器鱼鱼头位置的正前方(F

3.2速度系数模糊控制器和转向角度模糊控制器

根据这5个红外测距传感器的位置和返回值，本发明建立了由速度系数模糊控制器和转向角度模糊控制器构成的复合模糊控制器，控制框图如图6所示。

1)速度系数模糊控制器

式(2)建立了机器鱼游动的运动方程，式中，速度是由幅值决定的，通过改变幅值可以间接改变速度，因此模糊规则库输出的是速度系数而不是速度。将5个红外测距传感器测得的最小值表示为

l＝min{F

其论域为[10，80]，模糊子集为{很近(NT)，近(N)，中(M)，远(F)}，输出速度系数V的模糊子集为{VSM(最慢速)，VS(慢速)，VM(中速)，VN(正常速度)}，速度系数V的论域为[0.2，1]。根据机器鱼的尺寸规定一个安全距离为60cm，实际的最大检测距离设定为80cm，当l大于60cm(对应模糊子集为：远(F))时，机器鱼按照式(2)对应的摆动数学模型所对应的速度运动(VN(正常速度))，其它速度以式(2)为基础做相应改变即可，速度系数模糊规则如表1所示。

输入隶属函数除了F使用梯形隶属函数外，其余都采用三角隶属函数，输出隶属函数都采用三角隶属函数。

表1速度系数模糊规则

2)转向角度模糊控制器

将左侧(L

ε＝L

根据左右两侧红外测距传感器的检测范围为[10，80]，再加上一定的裕度，可得其论域为[-80，80]。当ε为正值时，其值越大则表明障碍物离机器鱼右侧越近；当ε为负值时，其值越小则表明障碍物离机器鱼左侧越近。当出现左侧(L

ε的模糊子集为{左远(LF)，左中(LM)，左近(LN)，相等(EQ)，右近(RN)，右中(RM)，右远(RF)}，输出转向角度α的模糊子集为{NB(负大)，NM(负中)，NS(负小)，Z(零)，PS(正小)，PM(正中)，PB(正大)}，根据第2节的转向运动模式，转向角度α的论域为[-45°，45°]；转向角度模糊规则库如表2所示。

转向角度模糊控制器的输入和输出隶属函数都采用三角隶属函数。

表2转向角度模糊规则

3)反模糊化

上述模糊规则表得到的都是模糊量，把模糊集合转化成单个数值，即选定一个清晰的数值来代表这个模糊集合是模糊控制中必不可少的一部分，常见的反模糊化方法有面积重心法(centroid)、最大隶属度法(maximum)、最大隶属度最大值法(lom)等，速度系数和转向角度模糊控制器的反模糊化方法都采用最大隶属度最大值法(lom)，其原理是，当模糊集合的论域上，有多个点的隶属度都取得最大，这些点中坐标绝对值最大的点作为最后清晰的数值，过程如下

式中，V、α为最终输出的清晰化数值。

3.3上浮下潜控制

上浮下潜控制的输入信号来自上侧(T

考虑到机器鱼的高度以及上侧和下侧传感器倾斜的角度，设定一个安全距离为40cm，将上侧(T

表3上浮下潜控制规则

表中，L

在上浮下潜避障时，目的是远离障碍，因此需要让机器鱼的速度增加或者保持最快速度(速度系数为1)，而在转向避障是，是靠近障碍物，此时则需要降低速度让机器鱼有更小的转向半径，出现这种情况是，综合考虑应该减速，即转向避障的优先级高于上浮下潜避障，只有转向角度控制器输出为0时，此时若需要进行上浮下潜避障，则应该加速或者保持最快速度(速度系数为1)，又考虑到舵机的硬件参数，因此不采用加速而选择保持最快速度(速度系数为1)。

综合速度系数、转向角度模糊控制和上浮下潜的控制方法，可以得到最终的复合避障控制方法，如图7所示。

参见图1，本发明实施例的一种用于双关节机器鱼的复合避障方法，包括以下步骤：

步骤一：获取输入信息：对于构造的转向角度、速度系数以及上浮下潜3个控制器，分别从5个红外测距传感器获取输入信息，其中转向角度控制器所需要的输入信息为左侧(L

步骤二：将左侧(L

步骤三：速度处理，若转向角度控制器和上浮下潜控制器都有输入值，则机器鱼的速度按照速度系数控制器的输出执行，若转向角度控制器输出为0，此时若需要上浮下潜，则按照最快速度(速度系数为1)执行。

以上实施例仅用于说明本发明的设计思想和特点，其目的在于使本领域内的技术人员能够了解本发明的内容并据以实施，本发明的保护范围不限于上述实施例。所以，凡依据本发明所揭示的原理、设计思路所作的等同变化或修饰，均在本发明的保护范围之内。