CN114610017A - 一种足式机器人避免自碰撞的方法、相关装置及存储介质 - Google Patents

Abstract

本申请公开了一种足式机器人避免自碰撞的方法、相关装置及存储介质，用于减少机器人在受到侧向扰动时两条腿之间碰撞的发生，提高机器人在行进过程中的稳定性。本申请方法包括：更新机器人第一腿的足端与第二腿之间的距离；判断所述距离是否小于或等于第一距离；若是，则通过所述第一腿的动力模组向所述第一腿施加斥力，以使得所述第一腿的足端与所述第二腿的距离大于第二距离。

Description

一种足式机器人避免自碰撞的方法、相关装置及存储介质

技术领域

本申请涉及机器人控制技术领域，尤其涉及一种足式机器人避免自碰撞的方法、相关装置及存储介质。

背景技术

随着计算机性能的提升，传感器科技的发展，机器人技术得到了持续的提高，人类生活中逐步出现了各种各样的机器人帮助人类完成特定工作。腿足类机器人相较于传统的轮式机器人和履带式机器人，其优势在于良好的越障性能，其多自由度的腿足结构可以应对一些较为复杂的地形。

足式机器人可以看作是由多条多连杆机械臂构成的浮动基座的传统机械臂，现有技术中，足式机器人的落足点规划大多采用捕获点(Capture Point)的一步稳定的启发式规划法。这种落足点规划法充分考虑了落足点对于机身平稳的贡献。但是在一些特殊的恶劣工况下，比如受到剧烈的侧向冲击扰动，可能出现机器人摆动腿与支撑腿之间相互碰撞的问题，将可能直接导致机器人失稳。

发明内容

本申请提供了一种足式机器人避免自碰撞的方法、相关装置及存储介质，用于减少机器人在受到侧向扰动时两条腿之间碰撞的发生，提高机器人在行进过程中的稳定性。

本申请第一方面提供了一种足式机器人避免自碰撞的方法，包括：

更新机器人第一腿的足端与第二腿之间的距离；

判断所述距离是否小于或等于第一距离；

若是，则通过所述第一腿的动力模组向所述第一腿施加斥力，以使得所述第一腿的足端与所述第二腿的距离大于第二距离。

可选的，所述通过所述第一腿的动力模组向所述第一腿施加斥力，以使得所述第一腿的足端与所述第二腿的距离大于第二距离，包括：

通过所述第一腿的动力模组向所述第一腿施加斥力；

计算并判断所述第一腿的足端的当前预判轨迹上的点到所述第二腿的距离是否小于或等于第二距离；

若是，则继续通过所述第一腿的动力模组向所述第一腿施加斥力，直至计算出所述第一腿的足端的预判轨迹上的点到所述第二腿的距离大于第二距离，以使得所述第一腿的足端与所述第二腿的距离大于第二距离。

可选的，所述通过所述第一腿的动力模组向所述第一腿施加斥力，包括：

根据所述第一腿的足端的当前预判轨迹计算所述第一腿的各关节在施加斥力的控制周期对应的预判关节角度；

根据所述第一腿的各关节在施加斥力的控制周期所对应的预判关节角度，计算所述第一腿的各关节在施加斥力的控制周期所对应的目标关节角度，且所述目标关节角度与所述预判关节角度的差值小于第一预设值；

在所述施加斥力的控制周期，控制所述第一腿的各关节对应的动力模组，向所述第一腿施加斥力，将所述第一腿的各关节调整到目标关节角度。

可选的，所述第一腿的足端的预判位置和目标位置的关系如下式所示：

p1＝p0+n||d1-d||；

其中，n表示所述第一腿的足端速度矢量与所述第二腿足端位置和髋关节连线所构成的向量的叉积，p0表示所述预判位置，所述预判位置与所述预判关节角度关联，p1表示与所述目标位置，所述目标位置与所述目标关节角度关联，d1表示所述第一距离，d表示所述第一腿的足端与所述第二腿之间的距离。

可选的，所述更新机器人第一腿的足端与第二腿之间的距离包括：

根据机器人第一腿的足端确定第一目标点；

根据过所述第一腿的足端且与所述机器人机身平面平行的目标平面与第二腿足端的交点确定第二目标点；

根据所述第一目标点和所述第二目标点之间的距离更新所述第一腿与所述第二腿之间的距离。

可选的，在所述通过所述第一腿的动力模组向所述第一腿施加斥力之前，所述方法还包括：

判断所述第一腿的足端当前预判轨迹上的点到所述第二腿的距离是否小于或等于第二距离；

若是，则通过所述第一腿的动力模组向所述第一腿施加斥力。

本申请第二方面提供了一种足式机器人避免自碰撞的装置，包括：

更新单元，用于更新机器人第一腿的足端与第二腿之间的距离；

第一判断单元，用于判断所述距离是否小于或等于第一距离；

控制单元，用于当所述第一判断单元的判断结果为是时，通过所述第一腿的动力模组向所述第一腿施加斥力，以使得所述第一腿的足端与所述第二腿的距离大于第二距离。

可选的，所述控制单元包括：

控制模块，用于通过所述第一腿的动力模组向所述第一腿施加斥力；

判断模块，用于计算并判断所述第一腿的足端的当前预判轨迹上的点到所述第二腿的距离是否小于或等于第二距离；

所述控制模块还用于：

当所述判断模块的判断结果为是时，继续通过所述第一腿的动力模组向所述第一腿施加斥力，直到计算出所述第一腿的足端的预判轨迹上的点到所述第二腿的距离大于第二距离，以使得所述第一腿的足端与所述第二腿的距离大于第二距离。

可选的，所述控制模块具体用于：

根据所述第一腿的足端的当前预判轨迹计算所述第一腿的各关节在施加斥力的控制周期对应的预判关节角度；

根据所述第一腿的各关节在施加斥力的控制周期所对应的预判关节角度，计算所述第一腿的各关节在施加斥力的控制周期所对应的目标关节角度，且所述目标关节角度与所述预判关节角度的差值小于第一预设值；

在所述施加斥力的控制周期，控制所述第一腿的各关节对应的动力模组，向所述第一腿施加斥力，将所述第一腿的各关节调整到目标关节角度。

可选的，所述第一腿的足端的预判位置和目标位置的关系如下式所示：

p1＝p0+n||d1-d||；

其中，n表示所述第一腿的足端速度矢量与所述第二腿足端位置和髋关节连线所构成的向量的叉积，p表示所述预判位置，所述预判位置与所述预判关节角度关联，p1表示与所述目标位置，所述目标位置与所述目标关节角度关联，d1表示所述第一距离，d表示所述第一腿的足端与所述第二腿之间的距离。

可选的，所述更新单元包括：

第一确定模块，用于根据机器人第一腿的足端确定第一目标点；

第二确定模块，用于根据过所述第一腿的足端且与所述机器人机身平面平行的目标平面与第二腿足端的交点确定第二目标点；

更新模块，用于根据所述第一目标点和所述第二目标点之间的距离更新所述第一腿与所述第二腿之间的距离。

可选的，所述装置还包括：

第二判断单元，用于在通过所述第一腿的动力模组向所述第一腿施加斥力之前，判断所述第一腿的足端当前预判轨迹上的点到所述第二腿的距离是否小于或等于第二距离；

若是，则所述控制单元还用于：通过所述第一腿的动力模组向所述第一腿施加斥力。

本申请第三方面提供了一种机器人，所述机器人包括：

处理器、存储器、输入输出单元以及总线；

所述处理器与所述存储器、所述输入输出单元以及所述总线相连；

所述存储器保存有程序，所述处理器调用所述程序以执行第一方面以及第一方面中任一项可选的足式机器人避免自碰撞的方法。

本申请第四方面提供了一种计算机可读存储介质，所述计算机可读存储介质上保存有程序，所述程序在计算机上执行时执行第一方面以及第一方面中任一项可选的足式机器人避免自碰撞的方法。

从以上技术方案可以看出，本申请具有以下优点：

通过实时更新机器人第一腿(摆动腿)与第二腿(支撑腿)之间的距离，当检测到机器人第一腿与第二腿之间的距离小于或等于预设的第一距离时，通过第一腿的动力模组向第一腿施加斥力，以实现将第一腿向远离第二腿的方向排斥的目的，从而减少了机器人在受到侧向扰动时两条腿之间碰撞的发生，提高机器人在行进过程中的稳定性。

附图说明

为了更清楚地说明本申请中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本申请提供的足式机器人的硬件结构和机械结构的示意图；

图2为本申请提供的足式机器人避免自碰撞的方法一个实施例流程示意图；

图3为本申请提供的足式机器人避免自碰撞的方法中摆动腿和支撑腿的碰撞关系示意图；

图4为本申请提供的足式机器人避免自碰撞的方法中第一距离、第二距离及预判轨迹的示意图；

图5为本申请提供的足式机器人避免自碰撞的方法另一个实施例流程示意图；

图6为叉积的定义示意图；

图7为本申请提供的足式机器人避免自碰撞的装置中第一腿和第二腿的矢量示意图；

图8为本申请提供的足式机器人避免自碰撞的装置一个实施例结构示意图；

图9为本申请提供的机器人一个实施例结构示意图。

具体实施方式

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

需要说明的是，本申请所提供的一种足式机器人避免自碰撞的方法、相关装置及存储介质，以下将对本申请提供的机器人的硬件结构和机械结构进行说明。在后续的描述中，使用用于表示部件的诸如“模块”、“部件”或“单元”的后缀仅为了有利于本发明的说明，其本身没有特定的意义。因此，“模块”、“部件”或“单元”可以混合地使用。

请参阅图1，图1为本发明其中一个实施方式的足式机器人100的硬件结构示意图。在图1所示的实施方式中，足式机器人100包括机械单元101、通讯单元102、传感单元103、接口单元104、存储单元105、控制模块110、电源111。足式机器人100的各种部件可以以任何方式连接，包括有线或无线连接等。本领域技术人员可以理解，图1中示出的足式机器人100的具体结构并不构成对足式机器人100的限定，足式机器人100可以包括比图示更多或更少的部件，某些部件也并不属于足式机器人100的必须构成，完全可以根据需要在不改变发明的本质的范围内而省略，或者组合某些部件。

下面结合图1对足式机器人100的各个部件进行具体的介绍：

机械单元101为足式机器人100的硬件。如图1所示，机械单元101可包括驱动板1011、电机1012、机械结构1013，在其他具体的实施方式中，机械结构1013还可包括可伸展的机械臂、可转动的头部结构、可摇动的尾巴结构、载物结构、鞍座结构、摄像头结构等。需要说明的是，机械单元101的各个部件模块可以为一个也可以为多个，可根据具体情况设置，比如对于双足机器人来说，其腿部可为2个，每个腿部可配置3个电机1012，对应的电机1012为6个，而对于四足机器人来说，其腿部可为4个，每个腿部可配置3个电机1012，对应的电机1012为12个。

通讯单元102可用于信号的接收和发送，还可以通过与网络和其他设备通信，比如，接收遥控器或其他足式机器人100发送的按照特定步态以特定速度值向特定方向移动的指令信息后，传输给控制模块110处理。通讯单元102包括如WiFi模块、4G模块、5G模块、蓝牙模块、红外模块等。

传感单元103用于获取足式机器人100周围环境的信息数据以及监控足式机器人100内部各部件的参数数据，并发送给控制模块110。传感单元103包括多种传感器，如获取周围环境信息的传感器：激光雷达(用于远程物体检测、距离确定和/或速度值确定)、毫米波雷达(用于短程物体检测、距离确定和/或速度值确定)、摄像头、红外摄像头、全球导航卫星系统(GNSS，Global Navigation Satellite System)等。如监控足式机器人100内部各部件的传感器：惯性测量单元(IMU，Inertial Measurement Unit)(用于测量速度值、加速度值和角速度值的值)，足底传感器(用于监测足底着力点位置、足底姿态、触地力大小和方向)、温度传感器(用于检测部件温度)。至于足式机器人100还可配置的载荷传感器、触摸传感器、电机角度传感器、扭矩传感器等其他传感器，在此不再赘述。

接口单元104可以用于接收来自外部装置的输入(例如，数据信息、电力等)并且将接收到的输入传输到足式机器人100内的一个或多个部件，或者可以用于向外部装置输出(例如，数据信息、电力等)。接口单元104可包括电源端口、数据端口(如USB端口)、存储卡端口、用于连接具有识别模块的装置的端口、音频输入/输出(I/O)端口、视频I/O端口等。

存储单元105用于存储软件程序以及各种数据。存储单元105可主要包括程序存储区和数据存储区，其中，程序存储区可存储操作系统程序、运动控制程序、应用程序(比如文本编辑器)等；数据存储区可存储足式机器人100在使用中所生成的数据(比如传感单元103获取的各种传感数据，日志文件数据)等。此外，存储单元105可以包括高速随机存取存储器，还可以包括非易失性存储器，例如磁盘存储器、闪存器、或其他易失性固态存储器。

显示单元106用于显示由用户输入的信息或提供给用户的信息。显示单元106可包括显示面板1061，可以采用液晶显示器(Liquid Crystal Display，LCD)、有机发光二极管(Organic Light-Emitting Diode,OLED)等形式来配置显示面板1061。

输入单元107可用于接收输入的数字或字符信息。具体地，输入单元107可包括触控面板1071以及其他输入设备1072。触控面板1071，也称为触摸屏，可收集用户的触摸操作(比如用户使用手掌、手指或适合的附件在触控面板1071上或在触控面板1071附近的操作)，并根据预先设定的程式驱动相应的连接装置。触控面板1071可包括触摸检测装置1073和触摸控制器1074两个部分。其中，触摸检测装置1073检测用户的触摸方位，并检测触摸操作带来的信号，将信号传送给触摸控制器1074；触摸控制器1074从触摸检测装置1073上接收触摸信息，并将它转换成触点坐标，再送给控制模块110，并能接收控制模块110发来的命令并加以执行。除了触控面板1071，输入单元107还可以包括其他输入设备1072。具体地，其他输入设备1072可以包括但不限于遥控操作手柄等中的一种或多种，具体此处不做限定。

进一步的，触控面板1071可覆盖显示面板1061，当触控面板1071检测到在其上或附近的触摸操作后，传送给控制模块110以确定触摸事件的类型，随后控制模块110根据触摸事件的类型在显示面板1061上提供相应的视觉输出。虽然在图1中，触控面板1071与显示面板1061是作为两个独立的部件来分别实现输入和输出功能，但是在某些实施例中，可以将触控面板1071与显示面板1061集成而实现输入和输出功能，具体此处不做限定。

控制模块110是足式机器人100的控制中心，利用各种接口和线路连接整个足式机器人100的各个部件，通过运行或执行存储在存储单元105内的软件程序，以及调用存储在存储单元105内的数据，从而对足式机器人100进行整体控制。

电源111用于给各个部件供电，电源111可包括电池和电源控制板，电源控制板用于控制电池充电、放电、以及功耗管理等功能。在图1所示的实施方式中，电源111电连接控制模块110，在其它的实施方式中，电源111还可以分别与传感单元103(比如摄像头、雷达、音箱等)、电机1012电性连接。需要说明的是，各个部件可以各自连接到不同的电源111，或者由相同的电源111供电。

在上述实施方式的基础上，具体地，在一些实施方式中，可以通过终端设备来与足式机器人100进行通信连接，在终端设备与足式机器人100进行通信时，可以通过终端设备来向足式机器人100发送指令信息，足式机器人100可通过通讯单元102来接收指令信息，并可在接收到指令信息的情况下，将指令信息传输至控制模块110，使得控制模块110可根据指令信息来处理得到目标速度值。终端设备包括但不限于：具备图像拍摄功能的手机、平板电脑、服务器、个人计算机、可穿戴智能设备、其它电器设备。

指令信息可以根据预设条件来确定。在一个实施方式中，足式机器人100可以包括传感单元103，传感单元103可根据足式机器人100所在的当前环境可生成指令信息。控制模块110可根据指令信息来判断足式机器人100的当前速度值是否满足对应的预设条件。若满足，则会保持足式机器人100的当前速度值和当前步态移动；若不满足，则会根据对应的预设条件来确定目标速度值和相应的目标步态，从而可控制足式机器人100以目标速度值和相应的目标步态移动。环境传感器可以包括温度传感器、气压传感器、视觉传感器、声音传感器。指令信息可以包括温度信息、气压信息、图像信息、声音信息。环境传感器与控制模块110之间的通信方式可以为有线通信，也可以为无线通信。无线通信的方式包括但不限于：无线网络、移动通信网络(3G、4G、5G等)、蓝牙、红外。

以上对本申请提供的足式机器人的硬件结构和机械结构进行说明，下面对本申请提供的足式机器人避免自碰撞的方法进行说明。

本申请提供了一种足式机器人避免自碰撞的方法、相关装置及存储介质，用于减少机器人在受到侧向扰动时两条腿之间碰撞的发生，提高机器人在行进过程中的稳定性。

请参阅图2，图2为本申请提供的足式机器人避免自碰撞的方法的一个实施例，该方法包括：

201、更新机器人第一腿的足端与第二腿之间的距离；

足式机器人具有多自由度的腿足结构，使其可以应对一些较为复杂的地形，足式机器人有两足、三足、四足、六足、八足等形式,足式机器人的行走机构是空间连杆机构，每条腿部分别有3个关节，相当于人类的髋关节、膝关节、腕关节，机器人的腿和身体的连接处的三个动力模组协调起来就能够实现一条腿在空间中多个自由度内的运动，多条腿协同工作就能够实现行走。

在足式机器人的行走过程中,行走机构始终满足静力学的静平衡条件,支持在地面上的腿即支撑腿，处于悬空状态的腿即摆动腿。但是在一些特殊的恶劣工况下，比如受到剧烈的侧向冲击扰动，如图3所示，以四足机器人的一对腿足为例，受到侧向冲击扰动后，进入悬空状态的摆动腿朝支撑腿的方向撞去，也就是以图3中的摆动轨迹运动，此时就可能出现机器人摆动腿与支撑腿之间相互碰撞的问题，这将直接导致机器人失稳，甚至摔倒。

为了避免足式机器人腿部自碰撞的问题，机器人的控制单元需要在移动的过程中实时更新机器人第一腿的足端与第二腿之间的距离。需要说明的是，本申请中的第一腿是指处于摆动状态的腿，第二腿是指处于支撑状态的腿。而更新的频率可以是每隔一个控制周期更新一次，也可以是每隔多个控制周期更新一次，具体此处不做限定。其中控制周期是指机器人的运动控制周期，机器人的控制周期是一个非常短的时间段，一般为0.001秒-0.003秒之间，优选0.002秒。

在机器人坐标系下，机器人腿部的各个点相对机器人质心的距离对于控制单元来说都是可获取的，即机器人腿部的各个点的坐标在以机器人质心为原点定义的机器人坐标系中都是可以确定的。进一步的，在更新第一腿的足端和第二腿之间的距离d时，可以是以第一腿的足端的中心点作为该距离d的一个端点，也可以是第一腿足端表面的任何一个点作为该距离d的一个端点；而该距离d的另一个端点可以是第二腿上的任何一点，比如以第二腿距第一腿足端最近的一个点确定该距离d的另一个端点，还可以将过第一腿足端的切面或过第一腿足端的移动方向的切面与第二腿的交点来确定该距离d的另一个端点。而距离d端点的选取可以是在机器人的骨骼上，还可以是在机器人骨骼表面所设置的皮肤上，具体在此均不作限定。

202、判断距离是否小于或等于第一距离，若是，则执行步骤203；

机器人的控制单元根据获取到的第一腿的足端到第二腿的距离d，随即判断该距离d是否小于或等于第一距离d1，该第一距离d1是人为定义的“安全距离”，当机器人第一腿的足端到第二腿的距离d小于或等于该第一距离d1时，就有可能会发生腿部的自碰撞，此时可以进行一定的干预以防止自碰撞的发生。

当控制单元确定第一腿的足端到第二腿之间的距离d小于或等于第一距离d1时，此时第一腿的足端到第二腿的距离较近，可能会有腿部自碰撞的情况发生，此时则执行步骤203以启动避障算法，控制第一腿的足端到第二腿的距离d，避免自碰撞的发生。

203、通过第一腿的动力模组向第一腿施加斥力，以使得第一腿的足端与第二腿的距离大于第二距离。

在控制单元确定第一腿的足端到第二腿的距离d小于第一距离d1时，则启动避障算法，控制第一腿的足端远离第二腿，具体可以是通过第一腿上的动力模组向第一腿施加斥力，使得第一腿的足端与第二腿的距离d大于第二距离d2，以此保证两者之间不会发生碰撞。

该第二距离d2是人为定义的“危险距离”，该第二距离可以是5cm、1cm，还可以是无穷小，具体此处不做限定。如图4所示，该第二距离d2小于第一距离d1，是保证机器人不会发生腿部自碰撞的最小距离。第一腿的足端到第二腿的距离d小于或等于第一距离d1时，将有可能发生自碰撞，但由于惯性存在，第一腿可能还会继续向靠近第二腿的方向运动，而当距离d小于或等于第二距离d2时，自碰撞就将会发生或自碰撞的风险将大大增加，因此在两者距离小于或等于第一距离d1时就需要进行干预，保证距离d大于第二距离d2，以此来避免自碰撞的发生。

第一距离d1和第二距离d2的数值需要根据不同机器人第一腿和第二腿之间的距离来作具体设定，如果两条腿之间相隔越远，该第一距离和第二距离的数值就相应越大，如果两条腿之间相隔较近，该第一距离和第二距离的数值就相应越小。

在一些具体的实施方式中，该斥力为虚拟斥力，具体可以通过设置人工势场，在人工势场中将第二腿设置为障碍物，使得第一腿在运动过程中受到第二腿的排斥作用，进而避免碰撞的发生。

在本实施例中，通过实时更新机器人第一腿与第二腿之间的距离，当检测到机器人第一腿与第二腿之间的距离小于或等于预设的第一距离时，通过第一腿的动力模组向第一腿施加斥力，以实现将第一腿向远离第二腿的方向排斥的目的，从而减少了机器人在受到侧向扰动时两条腿之间碰撞的发生，提高机器人在行进过程中的稳定性。

请参阅图5，图5为本申请提供的足式机器人避免自碰撞的方法的另一个实施例，该方法包括：

501、更新机器人第一腿的足端与第二腿之间的距离；

在本实施例中步骤501与前述实施例步骤201类似，此处不再赘述。

在本实施例中，关于机器人第一腿的足端与第二腿之间的距离d的确定，优选采用将机器人第一腿的足端作为第一目标点，将过第一腿足端且与机器人机身平面平行的目标平面与第二腿的交点作为第二目标点，由第一目标点和第二目标点之间的距离来描述机器人第一腿的足端与第二腿之间的距离d。

进一步的，机器人的控制单元可以获取到当前机器人每条腿部的关节角度，例如获取到第一腿关节空间配置为q＝[θ₁ θ₂ θ₃]。其中θ₁、θ₂、θ₃分别表示第一腿各关节的关节角度，三个关节角度共同决定第一腿足端在笛卡尔坐标系下的坐标(x，y，z)。因此控制单元可以通过计算当前机器人第一腿各关节的关节角度，在机器人质心为原点定义的机器人坐标系中确定第一腿足端(第一目标点)的坐标A(x1，y1，z1)，再确定过第一腿足端且与机身平面平行的目标平面与第二腿的交点(第二目标点)的坐标B(x2，y2，z2)，确定A点到B点的距离，从而确定第一腿足端到第二腿的距离d。

502、判断距离是否小于或等于第一距离，若是，则执行步骤503；

在本实施例中，步骤502与前述实施例步骤202类似，此处不再赘述。

需要说明的是，如果第一腿的足端到第二腿的距离大于第一距离，控制单元控制第一腿按照预先规划的轨迹摆动，继续执行步骤501即可。

503、通过第一腿的动力模组向第一腿施加斥力；

在控制单元确定第一腿的足端到第二腿的距离d小于第一距离d1时，则启动避障算法，控制第一腿的足端远离第二腿，具体可以是通过第一腿上的动力模组向第一腿施加斥力，使第一腿的足端向远离第二腿的方向运动，以此保证两者之间不会发生碰撞。

下面对本实施例中通过第一腿的动力模组向第一腿施加斥力的过程进行详细描述：

1)根据第一腿的足端的当前预判轨迹计算第一腿的各关节在施加斥力的控制周期的对应的预判关节角度；

当第一腿的足端到第二腿的距离小于或等于第一距离时，控制单元获取第一腿的足端的预判轨迹，该预判轨迹是指接下来一段时间内机器人第一腿的足端的轨迹，也就是受到侧向冲击或其它干扰条件后的轨迹，或者说是启动了避障算法后的轨迹。控制单元根据该预判轨迹计算施加斥力的控制周期内的第一腿的足端位置，可以反推计算出第一腿的各个关节在施加斥力的控制周期内的关节角度，即本申请中的预判关节角度。

2)根据第一腿的各关节在施加斥力的控制周期所对应的预判关节角度，计算第一腿的各关节在施加斥力的控制周期所对应的目标关节角度，且目标关节角度与预判关节角度的差值小于第一预设值；

控制单元根据计算得到的施加斥力的控制周期所对应的第一腿各关节的预判关节角度，来计算施加斥力的控制周期所对应的目标关节角度，实际上控制单元根据下一个控制周期的预判关节角度求解下一个控制周期的目标关节角度，再根据求解得到的目标关节角度进行控制。

为了使第一腿的足端避开第二腿，不发生自碰撞，需要将第一腿的足端向远离第二腿的方向排斥，因此避撞算法转换为求解第一腿的足端的坐标A(x1，y1，z1)与同平面的第二腿的坐标B(x2，y2，z2)的距离d始终大于第二距离且第一腿的轨迹突变最小，即可以描述为：

Min||q-q_d|| (1)

subject to d||AB||>d₂ (2)

其中q_d表示预判轨迹中接下来一个控制周期t_k+1对应的预判关节角度，q为对q_d进行调整后的t_k+1时刻新的目标关节角度，(1)式表示调整后的目标关节角度q(即待求解的目标关节位置)与同一时刻的预判关节角度要实现偏差最小，以防止关节位置突变造成冲击。(2)式表示调整后的目标关节角度q_d对应的足端点位置A与第二腿上的点B距离d要大于第二距离d₂，这样就在关节改变最小的情况下，保证支撑腿与摆动腿不发生碰撞。

显然的是(1)式的最小值为0，即关节角度不做任何调整，但这样无法满足机器人第一腿足端不发生自碰撞的要求。因此需要以(2)式作为(1)式求解的约束条件，即由q_d描述的第一腿的目标关节角度构成的足端坐标位置A与当前第二腿坐标B的距离始终大于第二距离d₂。

在一些具体的实施方式中，在计算目标关节角度时可以通过限制计算出的目标关节角度与对应的预判关节角度的差值小于第一预设值，将关节位置的变化限制在一个较小的范围内，从而避免关节位置突变带来的影响。

3)在施加斥力的控制周期，控制第一腿的各关节对应的动力模组，向第一腿施加斥力，将第一腿的各关节调整到目标关节角度。

当第一腿足端与第二腿的距离d小于第一距离d1时，第一腿受到虚拟斥力，具体是通过向第一腿施加斥力将第一腿的各关节调整到计算得到的目标关节角度，在这个斥力的作用下推动第一腿朝远离碰撞的方向(下式中的n方向)运动。

p1＝p0+n||d1-d|| (3)

其中n表示第一腿足端速度矢量与第二腿足端与髋关节连线构成的向量的叉积，叉积的定义如图6所示，其中OC就表示OA矢量和OB矢量的叉积，OC必垂直与OA矢量和OB矢量形成的平面。在计算时选择第二腿足端与髋关节，即带动第二腿大腿运动的关节的连线作为向量更能对第二腿当前的状态进行拟合，更接近真实的工况。p0表示预判轨迹中下一时刻的第一腿的预判位置，即预判关节角度所决定的足端位置，p1表示在p0基础上修改的足端的目标位置，即目标关节角度所决定的足端位置,也就是(2)式中的A。d1表示第一距离，d表示第一腿的足端到第二腿的距离。

如图7所示，当第一腿摆动到OA位置，具有沿轨迹切线方向的速度V时，并且第一腿的足端与第二腿距离小于第一距离时，避障算法启动，使得下个时刻的第一腿的关节位置为q(目标关节位置)，从而使得足端坐标由p0修改成p1，这样能够避免直接将摆动腿按照其速度反方向推动所导致摆动腿无法摆动的问题。

504、计算并判断第一腿的足端的当前预判轨迹上的点到第二腿的距离是否小于或等于第二距离，若是，则执行步骤505；

在本实施例中，在控制单元通过第一腿的动力模组向第一腿施加斥力的过程中，还需要持续计算第一腿的足端的预判轨迹上的点到第二腿的距离，然后判断该距离是否小于或等于第二距离。如果第一腿的足端预判轨迹上的点到第二腿的距离小于或等于第二距离，如图4所示的预判轨迹a，则说明如果按照当前预判轨迹运动第一腿的足端将撞上第二腿，此时则需要执行步骤504，继续对第一腿施加斥力。如果第一腿的足端当前预判轨迹上的点到第二腿的距离大于第二距离，如图4所示的预判轨迹b，则说明按照当前预判轨迹运动第一腿的足端不会撞上第二腿，此时则可以停止对第一腿施加斥力，返回执行步骤501即可。

需要说明的是，该预判轨迹是指第一腿的足端在接下来一段时间内的轨迹，该预判轨迹会随着斥力的施加不断变化，控制单元需要计算并判断的是当前时刻或控制周期的预判轨迹上的点到第二腿的距离。而该第二距离d2是人为定义的“危险距离”，该第二距离可以是5cm、1cm，还可以是无穷小，具体此处不做限定。如图4所示，该第二距离d2小于第一距离d1，是保证机器人不会发生腿部自碰撞的最小距离。第一腿的足端到第二腿的距离d小于或等于第一距离d1时，将有可能发生自碰撞，但由于惯性存在，第一腿可能还会继续向靠近第二腿的方向运动，而当距离d小于或等于第二距离d2时，自碰撞就将会发生或自碰撞的风险将大大增加，因此在两者距离小于或等于第一距离d1时就需要进行干预，保证距离d大于第二距离d2，以此来避免自碰撞的发生。

505、继续通过第一腿的动力模组向第一腿施加斥力，直到计算出第一腿的足端的预判轨迹上的点到第二腿的距离大于第二距离，以使得第一腿的足端与第二腿的距离大于第二距离。

如果第一腿的足端的预判轨迹上的点到第二腿的距离小于或等于第二距离，如图4所示的预判轨迹a，则说明碰撞可能发生，此时需要继续对第一腿施加斥力，直至第一腿的足端预判轨迹上的点到第二腿的距离大于第二距离，如图4所示的预判轨迹b，保证第一腿的足端在接下来一段时间内与第二腿的距离大于第二距离，由此使得第一腿的足端不会撞到第二腿。

进一步的，在本实施例步骤503中向第一腿施加斥力之前，还可以结合判断第一腿的足端当前预判轨迹上的点到第二腿的距离是否小于或等于第二距离，如果同时满足第一腿的足端到第二腿的距离小于或等于第一距离且第一腿的足端当前预判轨迹上的点到第二腿的距离小于或等于第二距离时，才执行步骤503及后续步骤以启动避障算法来对第一腿施加斥力，上述条件有任意一个不满足时则不启动，由此使避障算法在必要的时刻才启动，在一些受侧向扰动较小的情况下不进行干预，保证机器人的平稳运行。

在本实施例中，通过实时更新机器人第一腿(摆动腿)与第二腿(支撑腿)之间的距离，当检测到机器人第一腿与第二腿之间的距离小于或等于预设的第一距离时，通过第一腿的动力模组向第一腿施加斥力，以实现将第一腿向远离第二腿的方向排斥的目的，同时在控制第一腿的足端避开第二腿时考虑第一腿足端的预判轨迹，保证预判轨迹中的点与第二腿的距离大于第二距离，在关节改变最小的情况下，即第一腿的轨迹突变最小的情况下，保证第一腿与第二腿不发生碰撞。由此减少了机器人在受到侧向扰动时两条腿之间碰撞的发生，提高机器人在行进过程中的稳定性。

请参阅图8，图8为本申请提供的足式机器人避免自碰撞的装置一个实施例，该装置包括：

更新单元801，用于更新机器人第一腿的足端与第二腿之间的距离；

第一判断单元802，用于判断距离是否小于或等于第一距离；

控制单元803，用于当第一判断单元802的判断结果为是时，通过第一腿的动力模组向第一腿施加斥力，以使得第一腿的足端与第二腿的距离大于第二距离。

可选的，控制单元803包括：

控制模块8031，用于通过第一腿的动力模组向第一腿施加斥力；

判断模块8032，用于计算并判断第一腿的足端的当前预判轨迹上的点到第二腿的距离是否小于或等于第二距离；

控制模块8031还用于：

当判断模块8032的判断结果为是时，继续通过第一腿的动力模组向第一腿施加斥力，直到计算出第一腿的足端的预判轨迹上的点到第二腿的距离大于第二距离，以使得第一腿的足端与第二腿的距离大于第二距离。

可选的，控制模块8031具体用于：

根据第一腿的足端的当前预判轨迹计算第一腿的各关节在施加斥力的控制周期对应的预判关节角度；

根据第一腿的各关节在施加斥力的控制周期所对应的预判关节角度，计算第一腿的各关节在施加斥力的控制周期所对应的目标关节角度，且目标关节角度与预判关节角度的差值小于第一预设值；

在施加斥力的控制周期，控制第一腿的各关节对应的动力模组，向第一腿施加斥力，将第一腿的各关节调整到目标关节角度。

可选的，第一腿的足端的预判位置和目标位置的关系如下式所示：

p1＝p0+n||d1-d||；

其中，n表示第一腿的足端速度矢量与第二腿足端位置和髋关节连线所构成的向量的叉积，p0表示预判位置，预判位置与预判关节角度关联，p1表示与目标位置，目标位置与目标关节角度关联，d1表示第一距离，d表示第一腿的足端与第二腿之间的距离。

可选的，更新单元801包括：

第一确定模块8011，用于根据机器人第一腿的足端确定第一目标点；

第二确定模块8012，用于根据过第一腿的足端且与机器人机身平面平行的目标平面与第二腿足端的交点确定第二目标点；

更新模块8013，用于根据第一目标点和第二目标点之间的距离更新第一腿与第二腿之间的距离。

可选的，装置还包括：

第二判断单元804，用于用于在通过第一腿的动力模组向第一腿施加斥力之前，判断第一腿的足端当前预判轨迹上的点到第二腿的距离是否小于或等于第二距离；

若是，则控制单元8031还用于：通过第一腿的动力模组向第一腿施加斥力。

本实施例装置中，各单元的功能与前述图2或图5所示方法实施例中的步骤对应，此处不再赘述。

本申请还提供了一种机器人，请参阅图9，图9为本申请提供的机器人的一个实施例，该机器人包括：

处理器901、存储器902、输入输出单元903、总线904；

处理器901与存储器902、输入输出单元903以及总线904相连；

存储器902保存有程序，处理器901调用程序以执行如上任一足式机器人避免自碰撞的方法。

本申请还涉及一种计算机可读存储介质，计算机可读存储介质上保存有程序，其特征在于，当程序在计算机上运行时，使得计算机执行如上任一足式机器人避免自碰撞的方法。

所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统，装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

在本申请所提供的几个实施例中，应该理解到，所揭露的系统，装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

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

Claims (13)

1.一种足式机器人避免自碰撞的方法，其特征在于，所述方法包括：

更新机器人第一腿的足端与第二腿之间的距离；

判断所述距离是否小于或等于第一距离；

若是，则通过所述第一腿的动力模组向所述第一腿施加斥力，以使得所述第一腿的足端与所述第二腿的距离大于第二距离。

2.根据权利要求1所述的方法，其特征在于，所述通过所述第一腿的动力模组向所述第一腿施加斥力，以使得所述第一腿的足端与所述第二腿的距离大于第二距离，包括：

通过所述第一腿的动力模组向所述第一腿施加斥力；

计算并判断所述第一腿的足端的当前预判轨迹上的点到所述第二腿的距离是否小于或等于第二距离；

若是，则继续通过所述第一腿的动力模组向所述第一腿施加斥力，直至计算出所述第一腿的足端的预判轨迹上的点到所述第二腿的距离大于第二距离，以使得所述第一腿的足端与所述第二腿的距离大于第二距离。

3.根据权利要求1或2中任一项所述的方法，其特征在于，所述通过所述第一腿的动力模组向所述第一腿施加斥力，包括：

根据所述第一腿的足端的当前预判轨迹计算所述第一腿的各关节在施加斥力的控制周期对应的预判关节角度；

根据所述第一腿的各关节在施加斥力的控制周期所对应的预判关节角度，计算所述第一腿的各关节在施加斥力的控制周期所对应的目标关节角度，且所述目标关节角度与所述预判关节角度的差值小于第一预设值；

在所述施加斥力的控制周期，控制所述第一腿的各关节对应的动力模组，向所述第一腿施加斥力，将所述第一腿的各关节调整到目标关节角度。

4.根据权利要求3所述的方法，其特征在于，所述第一腿的足端的预判位置和目标位置的关系如下式所示：

p1＝p0+n||d1-d||；

其中，n表示所述第一腿的足端速度矢量与所述第二腿足端位置和髋关节连线所构成的向量的叉积，p0表示所述预判位置，所述预判位置与所述预判关节角度关联，p1表示与所述目标位置，所述目标位置与所述目标关节角度关联，d1表示所述第一距离，d表示所述第一腿的足端与所述第二腿之间的距离。

5.根据权利要求1所述的方法，其特征在于，所述更新机器人第一腿的足端与第二腿之间的距离包括：

根据机器人第一腿的足端确定第一目标点；

根据过所述第一腿的足端且与所述机器人机身平面平行的目标平面与第二腿足端的交点确定第二目标点；

根据所述第一目标点和所述第二目标点之间的距离更新所述第一腿与所述第二腿之间的距离。

6.根据权利要求1所述的方法，其特征在于，在所述通过所述第一腿的动力模组向所述第一腿施加斥力之前，所述方法还包括：

判断所述第一腿的足端当前预判轨迹上的点到所述第二腿的距离是否小于或等于第二距离；

若是，则通过所述第一腿的动力模组向所述第一腿施加斥力。

7.一种足式机器人避免自碰撞的装置，其特征在于，所述装置包括：

更新单元，用于更新机器人第一腿的足端与第二腿之间的距离；

第一判断单元，用于判断所述距离是否小于或等于第一距离；

控制单元，用于当所述第一判断单元的判断结果为是时，通过所述第一腿的动力模组向所述第一腿施加斥力，以使得所述第一腿的足端与所述第二腿的距离大于第二距离。

8.根据权利要求7所述的装置，其特征在于，所述控制单元包括：

控制模块，用于通过所述第一腿的动力模组向所述第一腿施加斥力；

判断模块，用于计算并判断所述第一腿的足端的当前预判轨迹上的点到所述第二腿的距离是否小于或等于第二距离；

所述控制模块还用于：

当所述判断模块的判断结果为是时，继续通过所述第一腿的动力模组向所述第一腿施加斥力，直到计算出所述第一腿的足端的预判轨迹上的点到所述第二腿的距离大于第二距离，以使得所述第一腿的足端与所述第二腿的距离大于第二距离。

9.根据权利要求6或7中任一项所述的装置，其特征在于，所述控制模块具体用于：

根据所述第一腿的足端的当前预判轨迹计算所述第一腿的各关节在施加斥力的控制周期对应的预判关节角度；

根据所述第一腿的各关节在施加斥力的控制周期所对应的预判关节角度，计算所述第一腿的各关节在施加斥力的控制周期所对应的目标关节角度，且所述目标关节角度与所述预判关节角度的差值小于第一预设值；

在所述施加斥力的控制周期，控制所述第一腿的各关节对应的动力模组，向所述第一腿施加斥力，将所述第一腿的各关节调整到目标关节角度。

10.根据权利要求9所述的装置，其特征在于，所述第一腿的足端的预判位置和目标位置的关系如下式所示：

p1＝p0+n||d1-d||；

其中，n表示所述第一腿的足端速度矢量与所述第二腿足端位置和髋关节连线所构成的向量的叉积，p0表示所述预判位置，所述预判位置与所述预判关节角度关联，p1表示与所述目标位置，所述目标位置与所述目标关节角度关联，d1表示所述第一距离，d表示所述第一腿的足端与所述第二腿之间的距离。

11.根据权利要求7所述的装置，其特征在于，所述更新单元包括：

第一确定模块，用于根据机器人第一腿的足端确定第一目标点；

第二确定模块，用于根据过所述第一腿的足端且与所述机器人机身平面平行的目标平面与第二腿足端的交点确定第二目标点；

更新模块，用于根据所述第一目标点和所述第二目标点之间的距离更新所述第一腿与所述第二腿之间的距离。

12.根据权利要求7所述的装置，其特征在于，所述装置还包括：

第二判断单元，用于在通过所述第一腿的动力模组向所述第一腿施加斥力之前，判断所述第一腿的足端当前预判轨迹上的点到所述第二腿的距离是否小于或等于第二距离；

若是，则所述控制单元还用于：通过所述第一腿的动力模组向所述第一腿施加斥力。

13.一种机器人，其特征在于，所述机器人包括：

处理器、存储器、输入输出单元以及总线；

所述处理器与所述存储器、所述输入输出单元以及所述总线相连；

所述存储器保存有程序，所述处理器调用所述程序以执行如权利要求1至6中任一项所述方法。

Images