一种自动躲避的仿海鬣蜥机器人的水下运动控制方法

1.本发明涉及一种机器人控制方法，具体是一种自动躲避的仿海鬣蜥机器人的水下运动控制方法。


背景技术：

2.在现代军事领域中，无人作战平台代表未来武器装备的发展方向，军用机器人同时具有非常好的发展前景,因此世界各国都不惜投入巨资研发新型无人作战平台。
3.海鬣蜥是一种两栖动物，是世界上唯一能适应海洋生活的鬣蜥。它依靠强壮的尾巴特点，摆动提供动力实现游泳，同时通过四肢爬行实现陆上的行走。因此，海鬣蜥具备良好的水陆两栖运动能力，可以适应水陆两栖复杂环境。这些特点可为研制更易控制、灵活度更高且能实现的探测并进行自动躲避仿生仿海鬣蜥机器人提供新思路，例如公布号为cn114683784a、名称为“一种仿生两栖海鬣蜥机器人”的中国发明专利，提供了一种采用两自由度爬行装置的设计、能够适应不同环境的地形的纯机械结构的机器人，但是该机器人还缺少一种控制方达在运动时自动躲避障碍物，因此有待进一步改进。


技术实现要素：

4.本发明的目的是克服上述背景技术中的不足，提供一种自动躲避的仿海鬣蜥机器人的水下运动控制方法，该方法应能控制四足仿海鬣蜥机器人基于视觉实现自动躲避障碍物。
5.本发明的技术方案是：
6.一种自动躲避的仿海鬣蜥机器人的水下运动控制方法，包括以下步骤：
7.1)机器人按照预定航向角向着目标位置前进，双目摄像机拍摄水下的图像；
8.2)上位机根据躲避算法处理图像并计算出机器人位置和障碍物位置，然后根据路径规划得到躲避航向角，再根据运动模型向单片机发出控制指令；
9.3)单片机根据尾部和四足的运行状态做出反馈，控制尾部和四足运动，精准控制机器人水下运动，机器人自动躲避障碍物并朝目标位置逼近；
10.所述仿海鬣蜥机器人包括头部、躯干、尾部和四足；所述头部设有双目摄像机，躯干设有上位机模块与单片机模块；上位机模块电连接双目摄像机与单片机模块，单片机模块电连接尾部和四足。
11.所述躲避算法包括：
12.1)水下障碍物图像预处理
13.将图像分割成多个区域，通过计算区域对比度获取每个区域的显著性值，如果识别出障碍物，在图像上进行标注，进入下一步，否则按照预定航向角前进；
14.2)水下障碍物图像动态识别
15.①
获取障碍物
16.确定图像中障碍物的基本几何元素，运用视觉计算，通过迭代的方法进行极小化，
即给定一个初始圆圈，经过迭代，向外扩展，任意方向在碰到图像障碍物的基本几何元素后停止扩展，而其他方向继续扩展，并最终收敛于目标轮廓，采用snakes算法，框选出障碍物以获取障碍物轮廓；
17.②
获取二维描述
18.将前后获取的不同图像中的障碍物轮廓进行对比，确定障碍物的运动状态，如果障碍物是运动的，进入下一步；
19.③
获取物体定整的三维描述
20.将前后获取的图像中的障碍物轮廓进行对比，判断障碍物的运动方向；
21.3)障碍物水下定位
22.将图像进行灰度化处理，运用rgb模型，计算出r、g、b的平均值作为灰度值：
23.i(x，y)＝(r(x，y)+g(x，y)+b(x，y))/3；
24.其中i(x，y)表示位于(x，y)处的像素，r(x，y)、g(x，y)、b(x，y)为该点像素的r分量、g分量、b分量值分别的灰度化；
25.读取图像上框选出的障碍物轮廓外的不同检测点的灰度值i，若连续n个邻域检测点大于阈值p，判定检测点为特征点，将特征点与海域图像数据库进行比对，如果图像匹配则得到障碍物位置和机器人位置。
26.所述路径规划包括根据机器人位置、障碍物位置、目标位置，建立虚拟势场，目标位置对机器人产生引力，障碍物对机器人产生斥力，根据所受合力的方向与大小决定躲避航向角，不断获取躲避航向角以确定到达目标位置的避障路径。
27.所述引力公式为：
[0028][0029][0030]
其中：f
att
为目标位置对机器人的引力，u
att
为引力势场，q为水下机器在水中位置，q
goal
为目标位置，ε为尺度因子，ρ(q,q
gosl
)为机器人位置与目标位置的欧几里德距离，d
goal
为防止引力势场过大的阈值，可根据仿海鬣蜥机器人周围环境状况修改。
[0031]
所述斥力公式为：
[0032][0033]
[0034][0035][0036]
其中：f
rep
为合斥力，f
rep 1
为防止碰撞排斥分力，f
rep 2
为防止陷入局部极小值的排斥分力,u
rep
为改进式的斥力势场，n
or
为与f
rep1
同向的单位向量,n
rg
为与f
rep2
同向的单位向量，n为正数，η为正比例系数，ρ(q,q
obs
)代表物体和障碍物之间的距离，ρ0代表障碍物对目标点的影响半径，p
obs
为障碍物位置。
[0037]
所述躲避航向角的计算公式如下：
[0038][0039][0040][0041]
其中：目标位置为(x
gaal
,y
goal
)，机器人位置为(x,y)，第i个障碍物位置为(x
obsi
,y
obsi
)，f
att
为目标位置对机器人的引力，f
rep1i
为i个障碍物位置对机器人的防止碰撞排斥分力，f
rep2i
为第i个障碍物位置对机器人的防止陷入局部极小值的排斥分力，θ为机器人到目标值的向量角，αi为第i个障碍物到机器人的向量角，为机器人避障航向角。
[0042]
所述运动模型包括：
[0043]
当机器人在水中上升或下潜时，控制尾部摆动，四足缩起；
[0044]
当机器人曲线运动时，控制尾部轻微摆动来调整方向；
[0045]
当机器人在海底直线爬行时，控制四足做对角步态运动，尾部拖行不摆动；
[0046]
当机器人在海底直线爬行遇到障碍物时，控制尾部摆动来调整方向，同时四足运动；
[0047]
当机器人在近海底游动时，未碰到障碍物时仅尾部摆动，四足缩起，遇到障碍物控制四足运动来蹬离障碍物；
[0048]
当遇到海浪时，控制四足和尾部共同运动以保持稳定。
[0049]
本发明的有益效果是：
[0050]
仿海鬣蜥机器人通过双目摄像头获取图像，上位机计算出障碍物位置与机器人位置，再得到躲避航向角，并向单片机发出控制指令，控制仿海鬣蜥机器人运动，朝着目标位置不断前进并自动避障，实现识别、定位、路径规划、避障及运动控制的全自动化。
附图说明
[0051]
图1是本发明的原理示意图。
[0052]
图2是本发明的仿海鬣蜥机器人的连接关系示意图。
[0053]
图3是本发明的获取障碍物的示意图。
[0054]
图4是本发明的仿海鬣蜥机器人的尾部示意图。
[0055]
图5是本发明的仿海鬣蜥机器人的尾部的内部结构示意图。
[0056]
图6是本发明的仿海鬣蜥机器人的尾部的摆动方式示意图。
[0057]
图7是本发明的仿海鬣蜥机器人的运动方式示意图。
[0058]
图8是本发明的仿海鬣蜥机器人的尾部的拉绳摆动示意图。
具体实施方式
[0059]
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用于解释本发明，并不用于限定本发明。
[0060]
一种自动躲避的仿海鬣蜥机器人的水下运动控制方法，包括以下步骤：
[0061]
1)机器人按照预定航向角向着目标位置前进，双目摄像机拍摄水下的图像；
[0062]
目标位置为预先设定，机器人往目标位置的前进方向为预定航向角，同时海域图像数据库中已储存有机器人运动区域的图像、特征点、位置信息；
[0063]
2)上位机根据躲避算法处理图像并计算出机器人位置和障碍物位置，然后根据路径规划得到躲避航向角，再根据运动模型向单片机发出控制指令；
[0064]
3)单片机根据尾部和四足的运行状态做出反馈，控制尾部和四足运动，精准控制机器人水下运动，机器人自动躲避障碍物并朝目标位置逼近。
[0065]
所述躲避算法包括：
[0066]
1)水下障碍物图像预处理(利用不同的色彩通道将障碍物与海洋蓝色背景明显地区分出来)
[0067]
将图像分割成多个区域，通过计算区域对比度(rc)获取每个区域的显著性值，即与蓝色通道的相似性，与海洋背景的区别；如果识别出障碍物，在图像上进行标注，进入下一步，否则按照预定航向角前进；
[0068]
获得图像某点的像素为i，记为ii(i＝1,2,
···
,n,n为图像中像素量)，计算图片所有像素点的显著性数值：
[0069][0070]
其中ij是预设蓝色区域像素值，dd(ii，ij)是图像某点与蓝色区域像素值的对比度,对比度越大，显著性越高，将显著性接近的点作为同一区域；
[0071]
对除蓝色区域外的区域增加其权重以增强对比度来计算区域显著性值：
[0072][0073]
[0074]
最后框选出显著性高的区域作为障碍物。
[0075]
其中：
[0076]
w(rn):为区域rn中的像素值，利用其对区域rn进行加权(划分区域越大权重越大)，
[0077]
dc(rn)：区域rn内的颜色加权值(非蓝色区域越大，值越大)，
[0078]
p(cn，k):为区域n中第k种色彩在所有cn中彩色的概率，
[0079]
d(cn，k)：为区域n中第k种色彩与蓝色区别加权值(蓝色为1，非蓝色为10)；
[0080]
2)水下障碍物图像动态识别(用于获取障碍物动态)
[0081]
①
获取障碍物
[0082]
确定图像中障碍物的基本几何元素(包括边缘点、直线段、曲线等)，运用视觉计算，通过迭代的方法进行极小化，即给定一个初始圆圈(图3所示)，经过迭代，向外扩展，任意方向在碰到图像障碍物的基本几何元素后停止扩展，而其他方向继续扩展，并最终收敛于目标轮廓，采用snakes算法，框选出障碍物以获取障碍物轮廓；
[0083]
②
获取二维描述
[0084]
将前后获取的图像中的障碍物轮廓进行对比，确定障碍物的运动状态(运动或静止)，如果障碍物是运动的，进入下一步；
[0085]
③
获取物体定整的三维描述
[0086]
将前后获取的图像中的障碍物轮廓进行对比，判断障碍物的运动方向(障碍物往某一方向运动，机器人需要往相反方向运动来躲避障碍物)；
[0087]
3)障碍物水下定位
[0088]
将图像进行灰度化处理，运用rgb模型，计算出r，g，b的平均值作为灰度值：
[0089]
i(x，y)＝(r(x，y)+g(x，y)+b(x，y))/3
[0090]
其中i(x，y)表示位于(x，y)处的像素，r(x，y)、g(x，y)、b(x，y)为该点像素的r分量、g分量、b分量值分别的灰度化；
[0091]
剔除图像中框选出的障碍物轮廓，读取图像上不同检测点的灰度值i，若连续n个邻域检测点大于阈值p(p为i的120％)，判定检测点为特征点，将特征点与海域图像数据库进行比对，如果图像匹配(匹配度高于80％)则得到障碍物位置和机器人位置；在没有障碍物时，可以通过该方式得到机器人位置；
[0092]
海域图像数据库中的图像对应不同经纬度，图像匹配后可以确定图像的经纬度，从而确定仿海鬣蜥机器人的位置，再由双目摄像机为原点建立坐标系，在计算出障碍物的经纬度，进而得到障碍物位置(3-3)。
[0093]
所述路径规划包括：
[0094]
根据机器人位置、障碍物位置、目标位置，建立虚拟势场，目标位置对机器人产生引力，障碍物对机器人产生斥力，机器人根据所受合力的方向与大小决定躲避航向角，不断获取航向角以确定到达目标位置的避障路径。
[0095]
具体地，当前机器人位置、障碍物位置通过图像对比得到，目标位置则为人工预先设置并传输给机器人。机器人位置、障碍物位置、目标位置都在预设的坐标系中完成。
[0096]
考虑到在水下环境中，机器人需要躲避多个障碍物才能到达目标位置，整个过程较为复杂，因此在水下环境中建立虚拟势场，让机器人靠近多个障碍物而又需要朝着目标点前进时，能准确避开障碍物，以最佳航向角向目标位置移动。
[0097]
具体地，以机器人所处的环境建立虚拟势场，虚拟势场构建的是一个虚拟势场，虚拟势场包括吸引势场与斥力势场。机器人处于目标的吸引势场，即目标对机器人有一个吸引力。以仿海鬣蜥机器人处于各障碍物的斥力势场内，即障碍物对机器人有一个斥力。在遇到障碍物时，在所建立的虚拟势场中，目标点对机器人产生引力，障碍物对机器人产生斥力，机器人根据所受合力的方向与大小决定躲避航向角，否则只受所给目标位置的引力而进行直线行驶。
[0098]
具体地，对于仿海鬣蜥机器人所处引力势场，由目标位置对仿海鬣蜥机器人的吸引产生，与当前机器人位置到目标位置的欧几里得距离相关，而对引力势场求导即可获得目标位置对仿海鬣蜥机器人产生的引力。在预设坐标系下，根据获得的机器人位置和开始输入的目标位置，可计算当前机器人到目标的欧几里得距离。机器人与目标的欧几里徳距离预设阈值，阈值大小为2，可根据仿海鬣蜥机器人周围环境状况修改。根据仿海鬣蜥机器人与目标的欧几里徳距离、欧几里徳距离预设阈值计算得到引力势场。对引力势场求导，计算得到引力。引力公式如下：
[0099][0100][0101]
其中：f
att
为目标位置对机器人的引力，u
att
为引力势场，q为水下机器在水中位置，q
goal
为目标位置，ε为尺度因子，ρ(q,q
gosl
)为机器人位置与目标位置的欧几里德距离，d
goal
为防止引力势场过大的阈值,可根据仿海鬣蜥机器人周围环境状况修改。
[0102]
具体地，对于机器人所处斥力势场，由各障碍物位置对机器人的排斥产生，与当前机器人位置到各障碍物位置的欧几里得距离相关。考虑到机器人在某个位置时，如果若干个障碍物的合斥力与目标点的引力大小相等、方向相反，则合力为0，这将导致机器人不再“受力”，故无法向前搜索避障路径。为了防止斥力大小陷入局部最小值，我们使用了改进式的斥力势场函数。对斥力势场求导即可获得各障碍物对仿海鬣蜥机器人产生的斥力，斥力包括防止碰撞排斥分力和防止陷入局部最小值排斥分力。在预设坐标系下，根据当前机器人位置和各个障碍物位置，可计算当前机器人到达障碍物的欧几里徳距离距离。根据当前机器人到达障碍物的欧几里徳距离距离、当前机器人到达目标的欧几里徳距离距离，可计算得到斥力势场。对斥场势力求导，可计算得到防止碰撞排斥分力和防止陷入局部最小值排斥分力。斥力计算公式如下：
[0103][0104]
[0105][0106][0107]
其中：f
rep
为合斥力，f
rep1
为防止碰撞排斥分力，f
rep 2
为防止陷入局部极小值的排斥分力,u
rep
为改进式的斥力势场，n
or
为与f
rep1
同向的单位向量,n
rg
为与f
rep2
同向的单位向量，n为正数，η为正比例系数，ρ(q,q
obs
)代表物体和障碍物之间的距离，ρ0代表障碍物对目标点的影响半径，p
obs
为障碍物位置。
[0108]
具体地，根据上述计算所得机器人受到的引力与斥力，根据矢量求和即可得到合力方向，从而获得机器人的躲避航向角，不断获取躲避航向角进而规划仿海鬣蜥机器人不断逼近目标值的避障路径。躲避航向角计算公式如下：
[0109][0110][0111][0112]
其中：目标位置为(x
gaal
,y
goal
)，机器人位置为(x,y)，第i个障碍物位置为(x
obsi
,y
obsi
)，f
att
为目标位置对机器人的引力，f
rep1i
为i个障碍物位置对机器人的防止碰撞排斥分力，f
rep2i
为第i个障碍物位置对机器人的防止陷入局部极小值的排斥分力，θ为机器人到目标值的向量角，αi为第i个障碍物到机器人的向量角,为机器人避障航向角。
[0113]
所述运动模型包括：
[0114]
如图7(a)、(b)所示，当机器人在水中上升或下潜时，控制尾部摆动，四足缩起。
[0115]
如图7(c)、(d)所示，当机器人在海底直线爬行时，控制四足做对角步态运动，尾部拖行不摆动。当机器人曲线运动时，控制尾部轻微摆动来调整方向。
[0116]
如图7(d)所示当机器人在海底直线爬行遇到障碍物时，控制尾部摆动来调整方向，同时四足运动；四足根据障碍高度向上抬起至相应高度后再向前移动，实现避障。
[0117]
如图7(a)、(b)、(c)所示当机器人在近海底游动时，未碰到障碍物时仅尾部摆动，四足缩起，遇到障碍物控制四足运动来蹬离障碍物。
[0118]
如图7(d)所示，当遇到海浪时，控制四足和尾部共同运动以保持稳定，以防止仿生机器人被海浪冲走；若海浪与仿生机器人原本运动方向一致，则控制仿生机器人尾部摆动转向直至与海浪方向相反；当海浪与仿生机器人原本运动方向相反时，控制四足以对角小跑步态向前划动，尾部根据海浪大小以一定的摆动幅度和频率运动，以提供抵御海浪的动
力。
[0119]
所述仿海鬣蜥机器人包括头部、躯干、尾部和四足。
[0120]
所述头部设有双目摄像机，躯干设有上位机模块与单片机模块；上位机模块通过串口或者网口电连接双目摄像机，上位机模块通过数据线电连接单片机模块，单片机模块电连接尾部和四足。
[0121]
仿海鬣蜥机器人水下运动过程中，在四足和尾部运动相互配合的作用下保持稳定，并通过控制尾部摆动角度、速度及四足的运动速度和步态以改变前进动力和方向，其在复杂水下环境中具有高灵活性和高可控性，能很好适应各种水下环境，降低了复杂水下环境对水下运动的影响。
[0122]
双目摄像机先得到水下环境图像，上位机处理图像后得到避障路径，再向单片机发生运动控制指令，单片机根据所述运动控制命令解释成相应时序信号直接控制机器人的尾部和四足执行相关动作，实现仿海鬣蜥机器人精准水下运动控制。
[0123]
需要根据仿生机器人所处状态调节尾部左右摆动的角度和频率。
[0124]
如图4所示，尾部包括若干个椭圆形的尾关节1、拉绳2、弹簧3、尾尖4和尾舵机5。尾部左右摆动来提供向前的动力。摆动频率越大，动力越大；摆动角度越大，动力越大。尾巴左右摆动的角度一样时，机器人做直线运动；尾巴向左摆动角度大于向右时，机器人向右方向运动；尾巴向右摆动角度大于向右时，机器人向左方向运动。其中，尾舵机如图5所示，摆动的角度和速度由尾舵机控制，也可以由尾舵机驱动的摆臂5.1来控制，摆臂上设有两对限位孔5.11，当绳子系在外侧的限位孔，尾巴摆动的速度越快并且摆动角度越大。根据实际需求对应调节尾舵机的转动角度和速度，实现尾部摆动运动控制。
[0125]
本发明中的尾部控制方法为，尾巴从根部整体向左或右摆动带来前进动力，摆动过程当中尾部每根关节之间也会产生相应的弯曲，从而使摆动幅度增大，增大前进动力水下游行尾部运动控制方法中主要采用左右摇摆的方式。尾部摆动可分为四个阶段，在此，机器人尾部摆动的角度随时间t的变化符合以下方程：
[0126][0127]
选择合适的方法简化运算得到更精确、更真实的数据，用尾部的摆动幅度来精确的描述随时间t的变化函数，其方程如下：
[0128][0129]
如图8所示，其中l为尾巴长度，r为两条拉绳子根部节点间距的一半，h为摆臂中心点到尾巴根部平面的垂直距离，ω为舵机旋转角速度。
[0130]
本发明中，机器人的四足运动控制与四足机器人基本相同，采用对角步态控制方法。
[0131]
附图中给出了本发明的较佳实施例。但是，本发明可以以许多不同的形式来实现，
并不限于本说明书所描述的实施例。相反地，提供这些实施例的目的是使对本发明的公开内容的理解更加透彻全面。

技术特征：
1.一种自动躲避的仿海鬣蜥机器人的水下运动控制方法，包括以下步骤：1)机器人按照预定航向角向着目标位置前进，双目摄像机拍摄水下的图像；2)上位机根据躲避算法处理图像并计算出机器人位置和障碍物位置，然后根据路径规划得到躲避航向角，再根据运动模型向单片机发出控制指令；3)单片机根据尾部和四足的运行状态做出反馈，控制尾部和四足运动，精准控制机器人水下运动，机器人自动躲避障碍物并朝目标位置逼近；所述仿海鬣蜥机器人包括头部、躯干、尾部和四足；所述头部设有双目摄像机，躯干设有上位机模块与单片机模块；上位机模块电连接双目摄像机与单片机模块，单片机模块电连接尾部和四足。2.根据权利要求1所述的一种自动躲避的仿海鬣蜥机器人的水下运动控制方法，其特征在于：所述躲避算法包括：1)水下障碍物图像预处理将图像分割成多个区域，通过计算区域对比度获取每个区域的显著性值，如果识别出障碍物，在图像上进行标注，进入下一步，否则按照预定航向角前进；2)水下障碍物图像动态识别
①
获取障碍物确定图像中障碍物的基本几何元素，运用视觉计算，通过迭代的方法进行极小化，即给定一个初始圆圈，经过迭代，向外扩展，任意方向在碰到图像障碍物的基本几何元素后停止扩展，而其他方向继续扩展，并最终收敛于目标轮廓，采用snakes算法，框选出障碍物以获取障碍物轮廓；
②
获取二维描述将前后获取的不同图像中的障碍物轮廓进行对比，确定障碍物的运动状态，如果障碍物是运动的，进入下一步；
③
获取物体定整的三维描述将前后获取的图像中的障碍物轮廓进行对比，判断障碍物的运动方向；3)障碍物水下定位将图像进行灰度化处理，运用rgb模型，计算出r、g、b的平均值作为灰度值：i(x，y)＝(r(x，y)+g(x，y)+b(x，y))/3；其中i(x，y)表示位于(x，y)处的像素，r(x，y)、g(x，y)、b(x，y)为该点像素的r分量、g分量、b分量值分别的灰度化；读取图像上框选出的障碍物轮廓外的不同检测点的灰度值i，若连续n个邻域检测点大于阈值p，判定检测点为特征点，将特征点与海域图像数据库进行比对，如果图像匹配则得到障碍物位置和机器人位置。3.根据权利要求2所述的一种自动躲避的仿海鬣蜥机器人的水下运动控制方法，其特征在于：所述路径规划包括根据机器人位置、障碍物位置、目标位置，建立虚拟势场，目标位置对机器人产生引力，障碍物对机器人产生斥力，根据所受合力的方向与大小决定躲避航向角，不断获取躲避航向角以确定到达目标位置的避障路径。4.根据权利要求3所述的一种自动躲避的仿海鬣蜥机器人的水下运动控制方法，其特征在于：所述引力公式为：
其中：f
att
为目标位置对机器人的引力，u
att
为引力势场，q为水下机器在水中位置，q
goal
为目标位置，ε为尺度因子，ρ(q,q
gosl
)为机器人位置与目标位置的欧几里德距离，d
goal
为防止引力势场过大的阈值，可根据仿海鬣蜥机器人周围环境状况修改。5.根据权利要求4所述的一种自动躲避的仿海鬣蜥机器人的水下运动控制方法，其特征在于：所述斥力公式为：征在于：所述斥力公式为：征在于：所述斥力公式为：征在于：所述斥力公式为：其中：f
rep
为合斥力，f
rep 1
为防止碰撞排斥分力，f
rep 2
为防止陷入局部极小值的排斥分力,u
rep
为改进式的斥力势场，n
or
为与f
rep1
同向的单位向量,n
rg
为与f
rep2
同向的单位向量，n为正数，η为正比例系数，ρ(q,q
obs
)代表物体和障碍物之间的距离，ρ0代表障碍物对目标点的影响半径，p
obs
为障碍物位置。6.根据权利要求5所述的一种自动躲避的仿海鬣蜥机器人的水下运动控制方法，其特征在于：所述躲避航向角的计算公式如下：征在于：所述躲避航向角的计算公式如下：
其中：目标位置为(x
gaal
,y
goal
)，机器人位置为(x,y)，第i个障碍物位置为(x
obsi
,y
obsi
)，f
att
为目标位置对机器人的引力，f
rep1i
为i个障碍物位置对机器人的防止碰撞排斥分力，f
rep2i
为第i个障碍物位置对机器人的防止陷入局部极小值的排斥分力，θ为机器人到目标值的向量角，α
i
为第i个障碍物到机器人的向量角，为机器人避障航向角。7.根据权利要求6所述的一种自动躲避的仿海鬣蜥机器人的水下运动控制方法，其特征在于：所述运动模型包括：当机器人在水中上升或下潜时，控制尾部摆动，四足缩起；当机器人曲线运动时，控制尾部轻微摆动来调整方向；当机器人在海底直线爬行时，控制四足做对角步态运动，尾部拖行不摆动；当机器人在海底直线爬行遇到障碍物时，控制尾部摆动来调整方向，同时四足运动；当机器人在近海底游动时，未碰到障碍物时仅尾部摆动，四足缩起，遇到障碍物控制四足运动来蹬离障碍物；当遇到海浪时，控制四足和尾部共同运动以保持稳定。

技术总结
本发明涉及一种机器人控制方法，目的是提供一种自动躲避的仿海鬣蜥机器人的水下运动控制方法，该方法应能控制四足仿海鬣蜥机器人基于视觉实现自动躲避障碍物。技术方案是一种自动躲避的仿海鬣蜥机器人的水下运动控制方法，包括以下步骤：1)机器人按照预定航向角向着目标位置前进，双目摄像机拍摄水下的图像；2)上位机根据躲避算法处理图像并计算出机器人位置和障碍物位置，然后根据路径规划得到躲避航向角，再根据运动模型向单片机发出控制指令；3)单片机根据尾部和四足的运行状态做出反馈，控制尾部和四足运动，精准控制机器人水下运动，机器人自动躲避障碍物并朝目标位置逼近。近。近。


技术研发人员：陈刚 袁玥 熊姝 胡睿萱 卢明 吴政睿
受保护的技术使用者：浙江理工大学
技术研发日：2023.06.07
技术公布日：2023/9/6