基于模型预测的机器人自碰撞检测方法、系统及机器人

技术领域

本发明涉及机器人控制技术领域，特别涉及一种基于模型预测的机器人自碰撞检测方法、系统及机器人。

背景技术

仿人机械臂或者机器人的自碰撞的发生会对安全造成很大威胁，可能损坏机械臂机器人，甚至伤到周围的人员。随着机械臂/机器人自由度的增加，其发生自碰撞的可能性也将增大，因此解决这个问题对于拥有多自由度的机械臂或者机器人是至关重要的，也是机械臂或者机器人安全控制和执行任务的基础。

当前，机器人自碰撞检测的实现主要有两种：（1）基于电流或力矩的检测方法；（2）基于几何模型的检测方法。首先，基于电流或力矩的检测方法，如CN112549024 B、CN116476043A等专利，是通过检测电流或力矩的激增来判断是否有碰撞发生，且易出现误判，当检测到碰撞时，碰撞已经发生，存在设备损坏的风险。其次，基于几何模型的检测方法，是通过某些包络模型将机器人各部件包络起来，通过检测各包络模型的最小距离来判断是否发生碰撞，当最小距离小于设定的阈值时，则判定发生碰撞，如CN 116394266 A、CN111546378 B、CN 111360824 B等均采用此类方法进行自碰撞检测。而上述基于几何模型的检测方法，基于阈值的判断逻辑存在一定缺陷：当阈值设置过小时，碰撞报警后机器人来不及减速；当阈值设置过大时，会导致机器人可达工作空间缩小，且当最小距离在阈值以内但此时没有碰撞发生时，会发生碰撞误判；此外，固定的阈值设置无法满足所有运动状态下的减速。此外，专利CN 103722565 B预测模型无法体现机器人的动力学特性，因此预测精度较低。

发明内容

本发明实施例的目的是提供一种基于模型预测的机器人自碰撞检测方法、系统及机器人，通过对机器人当前状态进行检测，基于动力学模型预测后续运动状态信息，摒弃了现有的阈值判断逻辑，克服了阈值设定过大或过小导致的问题，提高了机器人碰撞检测的精度，算法具有较好的实时性。

为解决上述技术问题，本发明实施例的第一方面提供了一种基于模型预测的机器人自碰撞检测方法，包括如下步骤：

获取机器人各关节的第一运动状态信息，所述第一运动状态信息包括：当前位置信息、当前速度信息、当前驱动力矩信息和当前加速度信息；

依据所述第一运动状态信息，预测所述机器人各关节在预设控制周期次数之后的第二运动状态信息；

依据所述第二运动状态信息，得到所述机器人各关节包络模型的空间位姿信息；

基于所述空间位姿信息，预判所述机器人各关节在所述预设控制周期次数之后是否发生碰撞，并依据碰撞判断结果对应调整所述机器人各关节的运动控制策略。

进一步地，所述获取机器人各关节的第一运动状态信息，包括：

获取所述机器人各关节的当前位置信息、当前速度信息和当前驱动力矩信息；

基于所述当前位置信息、所述当前速度信息和所述当前驱动力矩信息，计算所述机器人各关节的当前加速度信息。

进一步地，所述机器人各关节的加速度信息的计算公式为：

；

其中，

进一步地，所述预测所述机器人各关节在预设控制周期次数之后的第二运动状态信息，包括：

依据所述机器人各关节的第一运动状态信息中的当前加速度信息，计算所述机器人各关节在每次控制周期之后的速度信息；

通过所述预设控制周期次数次的迭代计算，得到所述机器人各关节在预设控制周期次数之后预测的速度信息和位置信息；

依据速度信息和位置信息，得到所述机器人各关节的第二运动状态信息。

进一步地，所述机器人各关节在每次移动之后的速度信息的计算公式为：

在所述机器人第k步时的加速度为：

；

则第k+1步时，所述机器人的速度信息

；

其中，

进一步地，所述依据所述第二运动状态信息，得到所述机器人各关节包络模型的空间位姿信息，包括：

依据所述第二运动状态信息，基于正向运动学计算所述机器人各关节相对于基坐标系的齐次变换矩阵。

进一步地，所述机器人各关节相对于基坐标系的齐次变换矩阵为：

；

；

其中，

相应地，本发明实施例的第二方面提供了一种基于模型预测的机器人自碰撞检测系统，包括：

运动状态预测模块和自碰撞检测模块；

所述运动状态预测模块包括：

运动状态获取单元，其用于实时获取机器人各关节的第一运动状态信息；

状态积分器单元，其用于依据所述第一运动状态信息，获取所述机器人各关节在预设控制周期次数之后的第二运动状态信息，依据所述第二运动状态信息得到所述机器人各关节包络模型的空间位姿信息；

所述自碰撞检测模块包括：

自碰撞检测单元，其用于基于所述空间位姿信息，判断所述机器人各关节在所述预设控制周期次数之后是否发生碰撞；

碰撞反应单元，其用于依据碰撞判断结果对应调整所述机器人各关节的运动控制策略。

相应地，本发明实施例的第三方面提供了一种电子设备，包括：至少一个处理器；以及与所述至少一个处理器连接的存储器；其中，所述存储器存储有可被所述一个处理器执行的指令，所述指令被所述一个处理器执行，以使所述至少一个处理器执行如上述任一所述基于模型预测的机器人自碰撞检测方法。

相应地，本发明实施例的第四方面提供了一种计算机可读存储介质，其上存储有计算机指令，该指令被处理器执行时实现上述任一所述基于模型预测的机器人自碰撞检测方法。

相应地，本发明实施例的第五方面提供了一种带有多关节机械臂的机器人，所述机器人至少包括一个机械臂，每个所述机械臂至少包括3个关节，所述机器人还包括：

上述基于模型预测的机器人自碰撞检测系统；和/或，

上述电子设备及计算机可读存储介质。

本发明实施例的上述技术方案具有如下有益的技术效果：

通过对机器人当前状态进行检测，基于动力学模型预测后续运动状态信息，摒弃了现有的阈值判断逻辑，克服了阈值设定过大或过小导致的问题，提高了机器人碰撞检测的精度，算法具有较好的实时性。

附图说明

图1是本发明实施例提供的基于模型预测的机器人自碰撞检测方法流程图；

图2是本发明实施例提供的运动状态预测模块框图；

图3是本发明实施例提供的自碰撞检测模块框图；

图4是本发明实施例提供的状态积分器单元计算原理图。

附图标记：

200、运动状态预测模块，201、运动状态获取单元，202、动力学计算单元，203、状态积分器单元，300、自碰撞检测模块，301、自碰撞检测单元，302、碰撞反应单元，303、轨迹重规划单元。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明了，下面结合具体实施方式并参照附图，对本发明进一步详细说明。应该理解，这些描述只是示例性的，而并非要限制本发明的范围。此外，在以下说明中，省略了对公知结构和技术的描述，以避免不必要地混淆本发明的概念。

请参照图1，本发明实施例的第一方面提供了一种基于模型预测的机器人自碰撞检测方法，包括如下步骤：

步骤S100，获取机器人各关节的第一运动状态信息，第一运动状态包括：当前位置信息、当前速度信息、当前驱动力矩信息和当前加速度信息。

步骤S200，依据第一运动状态信息，预测机器人各关节在预设控制周期次数之后的第二运动状态信息。

步骤S300，依据第二运动状态信息，得到机器人各关节包络模型的空间位姿信息。

步骤S400，基于空间位姿信息，预判机器人各关节在预设控制周期次数之后是否发生碰撞，并依据碰撞判断结果对应调整机器人各关节的运动控制策略。

上述技术方案通过对机器人当前状态进行检测，基于动力学模型预测后续运动状态信息，摒弃了现有的阈值判断逻辑，克服了阈值设定过大或过小导致的问题，提高了机器人碰撞检测的精度，算法具有较好的实时性。

具体的，步骤S100中的获取机器人各关节的第一运动状态信息，包括：

步骤S110，获取机器人各关节的当前位置信息、当前速度信息和当前驱动力矩信息。

步骤S120，基于当前位置信息、当前速度信息和当前驱动力矩信息，计算机器人各关节的当前加速度信息。

通常情况下，机器人的各类运动状态信息可以通过各种设置于机器人上的传感器来获取。常见的传感器包括：编码器、IMU、力传感器及设置于机器人和周边环境中的多个视觉传感器。其中，编码器（Encoder）：用于测量每个关节的位置和速度信息。编码器通常安装在关节的电机轴上，通过记录电机转动的脉冲数或位置来测量关节的位置和速度。IMU（Inertial Measurement Unit）：用于测量机器人的加速度和角速度。IMU由加速度计和陀螺仪组成，可以提供关节的加速度和角速度信息。力传感器（Force Sensor）：用于测量机器人在运动过程中所受到的力和力矩。力传感器通常安装在机器人的末端执行器上，可以测量末端执行器施加到物体上的力和力矩。视觉传感器（Vision Sensor）：例如摄像头或深度相机，可以用于获取机器人周围环境中物体的位置和姿态信息，从而间接获取机器人关节的位置信息。

进一步地，机器人各关节的当前加速度信息的计算公式为：

； （1）

其中，

由上述公式（1）可以得到，在机器人第k步时的加速度为：

；（2）

则第k+1步时，机器人的运动状态可以预测得到：

；（3）

其中，

具体的，步骤S200中的预测机器人各关节在预设控制周期次数之后的第二运动状态信息，包括：

步骤S210，依据机器人各关节的第一运动状态信息中的当前加速度信息，计算机器人各关节在每次移动之后的速度信息。

步骤S220，通过预设控制周期次数次的迭代计算，得到机器人各关节在预设控制周期次数之后预测的速度信息和位置信息。

步骤S230，依据速度信息和位置信息，得到机器人各关节的预测运动状态信息。

通过状态积分器单元所进行的迭代计算后，机器人在第k+n步的运动状态如式（4）所示：

；（4）

其中，n为机器人移动的预设控制周期次数值。

具体的，步骤S300中的依据第二运动状态信息，得到机器人各关节包络模型的空间位姿信息，包括：

步骤S310，依据第二运动状态信息，基于正向运动学计算机器人各关节相对于基坐标系的齐次变换矩阵。

进一步地，机器人各关节相对于基坐标系的齐次变换矩阵为：

；（5）

； （6）

其中，

具体的，以关节2部件为例，其正向运动学可被描述为：

； （7）

式中，

定义关节2部件上的点为：

； （8）

式中，

那么

； （9）

式中，

同理，关节6部件在对应位形下基坐标系中的描述为：

。 （10）

具体的，步骤S400中的判断机器人各关节在预设控制周期次数之后是否发生碰撞，包括：

步骤S400，基于图形碰撞检测算法，判断机器人各关节在预设控制周期次数之后是否发生碰撞。

具体的，图形碰撞检测算法是一种用于判断两个或多个图形是否发生碰撞的技术。具体的图形碰撞检测算法可以包括：1.AABB碰撞检测（Axis-Aligned Bounding Box）：该算法将图形包围在一个与坐标轴对齐的矩形框中，然后通过判断矩形框是否相交来快速判断碰撞。它适用于矩形、圆形等简单形状的碰撞检测。2.方向包围盒（Oriented BoundingBox，OBB）是一种用于包围和表示一个物体或模型的三维轴对齐矩形框。与轴对齐包围盒（Axis-Aligned Bounding Box，AABB）不同，OBB 可以沿任意方向旋转，并且与物体的长宽高相匹配。OBB 在许多3D计算机图形和碰撞检测领域都得到广泛应用。在图形计算中，OBB可用于包围复杂的三维模型，用于快速检测碰撞、遮挡关系计算和简化物体边界。OBB 的形状可以更好地适应物体的几何形状，提供了更精确的包围盒，在某些情况下比 AABB 更有效。算法上，可以使用线性代数的方法计算和更新 OBB 的属性，另外还有一些优化策略可以加速 OBB 的构建和更新过程。

在本发明技术方案中，上述AABB算法和OBB算法仅用来表示可选的选项，其它的图像碰撞检测算法只要可以实现上述机器人的碰撞检测，即落入本发明的保护范围。

相应地，请参照图2和图3，本发明实施例的第二方面提供了一种基于模型预测的机器人自碰撞检测系统，包括：运动状态预测模块200和自碰撞检测模块300。

运动状态预测模块200包括：

运动状态获取单元201，其用于实时获取机器人各关节的第一运动状态信息；

状态积分器单元203，其用于依据第一运动状态信息，获取机器人各关节在预设控制周期次数之后的第二运动状态信息，依据第二运动状态信息得到机器人各关节包络模型的空间位姿信息。状态积分器单元203的实现原理如图4所示。具体实现，如公式11和公式12所示：

；（11）

；（12）

式中，

可选的，运动状态预测模块200包括：

动力学计算单元202，其用于依据第一运动状态信息中的当前速度信息、当前位置信息、当前驱动力矩信息计算机器人各关节的当前加速度信息。

自碰撞检测模块300包括：

自碰撞检测单元301，其用于基于空间位姿信息，判断机器人各关节在预设控制周期次数之后是否发生碰撞；

碰撞反应单元302，其用于依据碰撞判断结果对应调整机器人各关节的运动控制策略。

进一步地，自碰撞检测模块300还包括：

轨迹重规划模块303，用于停止时根据当前路径重新规划速度，以实现机器人工作端仍处于给定的工作路径上。

相应地，本发明实施例的第三方面提供了一种电子设备，包括：至少一个处理器；以及与至少一个处理器连接的存储器；其中，存储器存储有可被一个处理器执行的指令，指令被一个处理器执行，以使至少一个处理器执行如上述任一基于模型预测的机器人自碰撞检测方法。

相应地，本发明实施例的第四方面提供了一种计算机可读存储介质，其上存储有计算机指令，该指令被处理器执行时实现上述任一基于模型预测的机器人自碰撞检测方法。

相应地，本发明实施例的第五方面提供了一种带有多关节机械臂的机器人，机器人至少包括一个机械臂，每个机械臂至少包括3个关节，机器人还包括：

上述基于模型预测的机器人自碰撞检测系统；和/或，

上述电子设备及计算机可读存储介质。

本发明实施例旨在保护一种基于模型预测的机器人自碰撞检测方法、系统及机器人，其中方法包括如下步骤：获取机器人各关节的第一运动状态信息，实时运动状态包括：当前位置信息、当前速度信息、当前驱动力矩信息和当前加速度信息；依据第一运动状态信息，预测机器人各关节在预设控制周期次数之后的第二运动状态信息；依据第二运动状态信息，得到机器人各关节包络模型的空间位姿信息；基于空间位姿信息，预判机器人各关节在预设控制周期次数之后是否发生碰撞，并依据碰撞判断结果对应调整机器人各关节的运动控制策略。上述技术方案具备如下效果：

通过对机器人当前状态进行检测，基于动力学模型预测后续运动状态信息，摒弃了现有的阈值判断逻辑，克服了阈值设定过大或过小导致的问题，提高了机器人碰撞检测的精度，算法具有较好的实时性。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质（包括但不限于磁盘存储器、CD-ROM、光学存储器等）上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备（系统）、和计算机程序产品的流程图和／或方框图来描述的。应理解可由计算机程序指令实现流程图和／或方框图中的每一流程和／或方框、以及流程图和／或方框图中的流程和／或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和／或方框图一个方框或多个方框中指定的功能的步骤。

最后应当说明的是：以上实施例仅用以说明本发明的技术方案而非对其限制，尽管参照上述实施例对本发明进行了详细的说明，所属领域的普通技术人员应当理解：依然可以对本发明的具体实施方式进行修改或者等同替换，而未脱离本发明精神和范围的任何修改或者等同替换，其均应涵盖在本发明的权利要求保护范围之内。