基于蛇形机器人蜿蜒步态的目标定位方法、系统及介质

本发明涉及机器人运动控制技术领域，尤其涉及一种基于蛇形机器人蜿蜒步态的目标定位方法、系统及介质。

背景技术：

蛇形机器人是一种仿生类机器人的一种，其拥有多自由度、高冗余度的特点。蜿蜒运动作为其二维步态下的一种基础步态，具备稳定性高，控制简单的特点。因此基于蛇形机器人蜿蜒步态的目标定位以及导航等领域一直是研究的热门方向。

正交链接的蛇形机器人蜿蜒运动的原理在于利用偏航关节摆动时连杆与地面之间的摩擦力，产生前进方向的推力，实现蜿蜒步态。而传统的蜿蜒步态中，机器人整体的摆动幅度与关节控制函数设定的幅值相关，且所有关节的摆动幅值均相同。因此运动过程中，搭载在机器人头部的单孔深度相机的摆动会十分剧烈，获取的图像多不可使用。且由于视野范围随头部关节摆动幅度增加而增加，因此在目标定位与跟踪过程中，传统蜿蜒步态有着丢失目标的可能性。因此，有效图像信息的获取问题和视野范围太大的问题使得蛇形机器人蜿蜒运动下的目标定位任务难以完成。

技术实现要素：

为至少一定程度上解决现有技术中存在的技术问题之一，本发明的目的在于提供一种基于蛇形机器人蜿蜒步态的目标定位方法、系统及介质。

本发明所采用的技术方案是：

一种基于蛇形机器人蜿蜒步态的目标定位方法，包括以下步骤：

采用改进的蜿蜒步态控制函数控制蛇形机器人运动，使所述蛇形机器人的头部关节在运动过程中固定；

采用安装在所述头部关节的相机采集图片信息，根据所述图片信息计算图像中目标质心的像素坐标；

采用相机模型计算所述目标质心的像素坐标与实际坐标之间的变换等式，根据变换等式获取目标坐标与相机坐标之间的变换关系，根据所述变换关系实现目标定位。

进一步，所述改进的蜿蜒步态控制函数为原始蜿蜒步态控制函数与sigmoid函数结合获得；

所述蛇形机器人为正交链接蛇形机器，包括n个俯仰关节与n个偏航关节，所述n为大于2的整数。

进一步，所述改进的蜿蜒步态控制函数包括偏航关节控制函数和俯仰关节控制函数；

所述偏航关节控制函数为：

所述俯仰关节控制函数为：

其中，i为关节编号，k为控制曲线上升阶段的速率，n为期望限幅的关节数目，a为曲线峰值高度，c表示函数曲线上升和下降的速率。

进一步，所述根据所述图片信息计算图像中目标质心的像素坐标，包括：

在所述蛇形机器人蜿蜒运动过程中，获取头部关节的摆动角度θ1与图像信息，对所述摆动角度θ1＝0对应的图像信息进行保存；

采用canny边缘检测算法对保存的图像信息进行处理，获取目标质心的像素坐标。

进一步，所述采用canny边缘检测算法对保存的图像信息进行处理，获取目标质心的像素坐标，包括：

对所述图像信息进行平滑处理；

获取平滑处理后的所述图像信息上的每个像素点的梯度强度和方向；

应用非极大值抑制方法，降低边缘检测算子的重复响应造成的影响；

利用双阈值算法检测和连接目标边缘，完成基于canny算法的边缘检测，获得目标的边缘图像；

计算所述目标质心的像素坐标(xc,yc)及距离信息dc。

进一步，所述对所述图像信息进行平滑处理，包括：

采用高斯滤波器对所述图像信息中的噪声进行抑制，以使图像平滑；

所述高斯滤波器的核的生成公式为：

其中，1≤i,j≤(2k+1)。

进一步，所述相机模型的工作原理为：

根据物理成像的原理，设点p的实际世界坐标为[x,y,z]^t，成像点p′的坐标为[x′,y′,z′]^t，并设物理成像平面到小孔的距离为f；

假设相机的像素平面得到了p′的像素坐标puv＝[u,v]^t，根据相机参数其与成像点坐标p′＝[x′,y′]^t有着一个缩放与平移关系，假设像素坐标在u轴上缩放了α倍，在v轴上缩放了β倍；同时，原点平移了[cx,cy]^t；p的实际坐标[x,y,z]^t与成像点坐标[x′,y′]^t与像素点坐标[u,v]^t之间的关系为：

把该式转成齐次矩阵形式，可得：

设点p的世界坐标为pw，并扩展其像素坐标为puv＝[u,v,1]^t，相机与世界之间的旋转矩阵为r，平移向量为t那么由以下关系：

zpuv＝k(rpw+t)＝ktpw

根据目标物体质心像素坐标puv以及对应的深度值dc即可计算出该图像点对应的实体在世界坐标系下的位置pw，实现了蛇形机器人蜿蜒运动下的目标定位。

本发明所采用的另一技术方案是：

一种基于蛇形机器人蜿蜒步态的目标定位系统，包括：

运动控制模块，用于采用改进的蜿蜒步态控制函数控制蛇形机器人运动，使所述蛇形机器人的头部关节在运动过程中固定；

图像处理模块，用于采用安装在所述头部关节的相机采集图片信息，根据所述图片信息计算图像中目标质心的像素坐标；

目标定位模块，用于采用相机模型计算所述目标质心的像素坐标与实际坐标之间的变换等式，根据变换等式获取目标坐标与相机坐标之间的变换关系，根据所述变换关系实现目标定位。

本发明所采用的另一技术方案是：

一种基于蛇形机器人蜿蜒步态的目标定位系统，包括：

至少一个处理器；

至少一个存储器，用于存储至少一个程序；

当所述至少一个程序被所述至少一个处理器执行，使得所述至少一个处理器实现上所述方法。

本发明所采用的另一技术方案是：

一种存储介质，其中存储有处理器可执行的指令，所述处理器可执行的指令在由处理器执行时用于执行如上所述方法。

本发明的有益效果是：本发明通过控制蛇形机器人的头部关节固定，有效减少由蜿蜒运动本身带来的相机抖动，适用于蛇形机器人稳定获取有效图像信息及对目标进行定位。

附图说明

为了更清楚地说明本发明实施例或者现有技术中的技术方案，下面对本发明实施例或者现有技术中的相关技术方案附图作以下介绍，应当理解的是，下面介绍中的附图仅仅为了方便清晰表述本发明的技术方案中的部分实施例，对于本领域的技术人员而言，在无需付出创造性劳动的前提下，还可以根据这些附图获取到其他附图。

图1是本发明实施例中蛇形机器人的模型图；

图2是本发明实施例中蛇形机器人传统蜿蜒步态仿真图；

图3是本发明实施例中蛇形机器人头部关节固定下的蜿蜒步态仿真图；

图4是本发明实施例中目标边缘检测结果图；

图5是本发明实施例中一种基于蛇形机器人蜿蜒步态的目标定位方法的步骤流程图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。对于以下实施例中的步骤编号，其仅为了便于阐述说明而设置，对步骤之间的顺序不做任何限定，实施例中的各步骤的执行顺序均可根据本领域技术人员的理解来进行适应性调整。

在本发明的描述中，需要理解的是，涉及到方位描述，例如上、下、前、后、左、右等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

在本发明的描述中，若干的含义是一个或者多个，多个的含义是两个以上，大于、小于、超过等理解为不包括本数，以上、以下、以内等理解为包括本数。如果有描述到第一、第二只是用于区分技术特征为目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量或者隐含指明所指示的技术特征的先后关系。

本发明的描述中，除非另有明确的限定，设置、安装、连接等词语应做广义理解，所属技术领域技术人员可以结合技术方案的具体内容合理确定上述词语在本发明中的具体含义。

如图5所示，本实施例提供一种基于蛇形机器人蜿蜒步态的目标定位方法，包括如下步骤：

步骤s1：采用改进的蜿蜒步态控制函数控制蛇形机器人运动，使蛇形机器人的头部关节在运动过程中固定。需要注意的是，使蛇形机器人的头部关节“固定”是指从第一个水平关节开始，控制一个或多个水平关节尽量不摆动，与前进的方向保持一致，而对于后续的水平关节则与传统蜿蜒步态保持一致，而并非控制头部关节不移动。

结合传统蜿蜒步态控制函数与sigmoid函数，提出头部固定下的蜿蜒步态控制函数。并利用机器人的具体参数信息，如关节个数、关节转动范围等，来确定相关的控制变量。最后使用改进后的步态函数实现头部固定下的蜿蜒运动，具体如下：

该蛇形机器人为具有2n+1个连杆的正交链接蛇形机器人，其理论上应具备n个俯仰关节与n个偏航关节。在本实施例中，如图1所示，该模型即为基于ros/gazebo平台搭建的正交链接的蛇形机器人，其由俯仰关机和偏航关节依次交替组成。图1中的机器人模型具备16个关节，且关节编号i为奇数的关节为俯仰关节，为偶数的则为偏航关节。图2所示则为蛇形机器人传统蜿蜒步态仿真图，可见机器人整体的摆动幅度一致，从而导致运动过程中的相机抖动十分剧烈，因此有必要提出改善后的蜿蜒步态控制函数。如下所示：

偏航关节控制函数为：

俯仰关节控制函数为：

对于偏航关节，参数a、ω、ν、ε均与原始蜿蜒步态控制函数的参数一致，而k、n则为新加入参数，其中参数k控制曲线上升阶段的速率，n为期望限幅的关节数目。对于俯仰关节，上述控制函数可将机器人头部抬离地面，用于解决由于头部摆动减少而带来整机前进阻力增加的问题。其中参数a为曲线峰值高度，应控制在关节转动角度范围之内。同时由于俯仰关节编号为离散值且为偶数，为了使机器人摄像头保持水平，参数b要避免在曲线顶点处取值，故b值取奇数即可。参数c表示函数曲线上升和下降的速率，为了尽量减少该步态控制函数对机器人整体稳定性的影响，c值应尽量取小但又不至于使处取值点外其他点为0，此处可选择c为常数1。

针对图1的仿真模型，可通过matlab平台进行参数选择，最终俯仰关节选取参数k＝2,n＝2,a＝1.2,ω＝3,v＝0.5,ε＝0时可获得理想的函数曲线。对于偏航关节，考虑本模型中舵机的转动范围为[-0.785,+0.785]，因此选取a＝0.5,b＝1。由此则确定了本机器人蜿蜒步态控制函数中的所有控制变量。在该函数控制下模型运动姿态如图3所示，可见其头部运动幅度基本为0，同时并不影响后续整体的蜿蜒姿态，可更为方便地获取有效地图像信息，为目标定位与定向运动打下了基础。

步骤s2：采用安装在头部关节的相机(如单孔深度相机)采集图片信息，根据图片信息计算图像中目标质心的像素坐标。

记录头部首个偏航关节θ1＝0时对应的图像信息(如rgb-d图片信息)，然后结合canny边缘检测算法，计算目标的质心像素坐标，具体步骤如下：

s21、首先使用高斯滤波其对图像进行平滑处理，对噪声进行抑制，以减少边缘检测器上明显的噪声影响。使用的高斯滤波器核的生成方程式如下：

上式中，1≤i,j≤(2k+1)。其中像素点坐标为(i,j)，且本次任务中图像大小为230*180。

s22、对平滑后的图像求取每个像素点的梯度强度和方向。先由边缘检测的算子返回水平方向和垂直方向的一阶导数值gx和gy，然后确定像素点的梯度g和方向θ，计算公式如下：

s23、应用非极大值抑制方法，降低边缘检测算子的重复响应造成的影响。先将当前像素的梯度强度与沿正负梯度方向上的两个像素进行比较。如果当前像素的梯度强度与另外两个像素相比最大，则该像素点保留为边缘点，否则该像素点将被抑制。

s24、利用双阈值算法检测和连接目标边缘，从而降低因为抑制噪声过度使得部分真实的边缘信息丢失而得到不完整的边缘检测结果造成的影响。

通过以上步骤即可以得到目标的边缘图像，并利用下述公式得到目标质心的像素坐标(xc,yc)，同时利用深度信息也可知距离信息dc：

最终获得目标的边缘检测结果图像以及计算的质心如图4所示。

步骤s3：采用相机模型计算目标质心的像素坐标与实际坐标之间的变换等式，根据变换等式获取目标坐标与相机坐标之间的变换关系，根据变换关系实现目标定位。

建立单孔深度相机的模型，计算得出质心与机器人之间的坐标变化关系。对本模型中的单孔深度相机进行几何建模，可以直接计算其固定的内部参数，如下所示：

则像素坐标与实际坐标之间的关系如下：

至此，设点p的世界坐标为pw，并扩展其像素坐标为puv＝[u,v,1]^t，相机与世界之间的旋转矩阵为r，平移向量为t那么由以下关系：

zpuv＝k(rpw+t)＝ktpw

公式右侧的tpw表示把一个世界坐标系下的齐次坐标变换到相机坐标系下。此时再根据目标物体质心像素坐标puv以及对应的深度值dc即可计算出该图像点对应的实体在世界坐标系下的位置pw，即实现了蛇形机器人蜿蜒运动下的目标定位。

本实施例还提供一种基于蛇形机器人蜿蜒步态的目标定位系统，包括：

运动控制模块，用于采用改进的蜿蜒步态控制函数控制蛇形机器人运动，使所述蛇形机器人的头部关节在运动过程中固定；

图像处理模块，用于采用安装在所述头部关节的相机采集图片信息，根据所述图片信息计算图像中目标质心的像素坐标；

目标定位模块，用于采用相机模型计算所述目标质心的像素坐标与实际坐标之间的变换等式，根据变换等式获取目标坐标与相机坐标之间的变换关系，根据所述变换关系实现目标定位。

综上所述，本实施例所述的头部关节固定下蜿蜒步态控制算法能有效减少由蜿蜒运动本身带来的相机抖动，适用于正交链接的蛇形机器人稳定获取有效图像信息的场景，而后续的目标定位计算方法适用于不同内参的单孔深度相机。同时所述设计方法易于用代码实现。

本实施例的一种基于蛇形机器人蜿蜒步态的目标定位系统，可执行本发明方法实施例所提供的一种基于蛇形机器人蜿蜒步态的目标定位方法，可执行方法实施例的任意组合实施步骤，具备该方法相应的功能和有益效果。

本实施例还提供一种基于蛇形机器人蜿蜒步态的目标定位系统，包括：

至少一个处理器；

至少一个存储器，用于存储至少一个程序；

当所述至少一个程序被所述至少一个处理器执行，使得所述至少一个处理器实现上所述方法。

本实施例的一种基于蛇形机器人蜿蜒步态的目标定位系统，可执行本发明方法实施例所提供的一种基于蛇形机器人蜿蜒步态的目标定位方法，可执行方法实施例的任意组合实施步骤，具备该方法相应的功能和有益效果。

本申请实施例还公开了一种计算机程序产品或计算机程序，该计算机程序产品或计算机程序包括计算机指令，该计算机指令存储在计算机可读存介质中。计算机设备的处理器可以从计算机可读存储介质读取该计算机指令，处理器执行该计算机指令，使得该计算机设备执行图5所示的方法。

本实施例还提供了一种存储介质，存储有可执行本发明方法实施例所提供的一种基于蛇形机器人蜿蜒步态的目标定位方法的指令或程序，当运行该指令或程序时，可执行方法实施例的任意组合实施步骤，具备该方法相应的功能和有益效果。

在一些可选择的实施例中，在方框图中提到的功能/操作可以不按照操作示图提到的顺序发生。例如，取决于所涉及的功能/操作，连续示出的两个方框实际上可以被大体上同时地执行或所述方框有时能以相反顺序被执行。此外，在本发明的流程图中所呈现和描述的实施例以示例的方式被提供，目的在于提供对技术更全面的理解。所公开的方法不限于本文所呈现的操作和逻辑流程。可选择的实施例是可预期的，其中各种操作的顺序被改变以及其中被描述为较大操作的一部分的子操作被独立地执行。

此外，虽然在功能性模块的背景下描述了本发明，但应当理解的是，除非另有相反说明，所述的功能和/或特征中的一个或多个可以被集成在单个物理装置和/或软件模块中，或者一个或多个功能和/或特征可以在单独的物理装置或软件模块中被实现。还可以理解的是，有关每个模块的实际实现的详细讨论对于理解本发明是不必要的。更确切地说，考虑到在本文中公开的装置中各种功能模块的属性、功能和内部关系的情况下，在工程师的常规技术内将会了解该模块的实际实现。因此，本领域技术人员运用普通技术就能够在无需过度试验的情况下实现在权利要求书中所阐明的本发明。还可以理解的是，所公开的特定概念仅仅是说明性的，并不意在限制本发明的范围，本发明的范围由所附权利要求书及其等同方案的全部范围来决定。

所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(rom，read-onlymemory)、随机存取存储器(ram，randomaccessmemory)、磁碟或者光盘等各种可以存储程序代码的介质。

在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。就本说明书而言，“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。

计算机可读介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(ram)，只读存储器(rom)，可擦除可编辑只读存储器(eprom或闪速存储器)，光纤装置，以及便携式光盘只读存储器(cdrom)。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。

应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(pga)，现场可编程门阵列(fpga)等。

在本说明书的上述描述中，参考术语“一个实施方式/实施例”、“另一实施方式/实施例”或“某些实施方式/实施例”等的描述意指结合实施方式或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施方式或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施方式或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施方式或示例中以合适的方式结合。

尽管已经示出和描述了本发明的实施方式，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施方式进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。

以上是对本发明的较佳实施进行了具体说明，但本发明并不限于上述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可做作出种种的等同变形或替换，这些等同的变形或替换均包含在本申请权利要求所限定的范围内。