机器人碰撞检测方法形式化

源自：软件学报 作者：陈善言 关永 施智平 王国辉

摘要

为应对更为复杂的任务需求, 现代机器人产业发展愈发迅猛. 出于协调工作的灵活性、柔顺性以及智能性等多项考虑因素, 多臂/多机器人充分发挥了机器人的强大作用, 成为现代机器人产业的重要研究热点. 在机器人双臂协调运行当中, 机械臂之间以及机械臂与外部障碍物之间容易发生碰撞, 可能会造成财产损失甚至人员伤亡. 对机器人碰撞检测方法进行形式化验证, 以球体和胶囊体形式化模型为基础, 构建基本几何体单元之间最短距离和机器人碰撞的高阶逻辑模型, 证明其相关属性及碰撞条件, 建立机器人碰撞检测方法基础定理库, 为多机系统碰撞检测算法可靠性与稳定性的验证提供技术支撑和验证框架.

关键词

机器人 碰撞检测 形式化方法 定理证明 HOL-Light

多臂/多机器人作为现代机器人产业发展的必然产物, 在协调工作领域具有更优的灵活性、柔顺性以及智能性等特点, 能应对更为复杂的任务需求, 充分发挥机器人强大的作用. 在机器人协调运行当中, 机器人之间以及机器人与外部障碍物之间容易发生碰撞, 可能会造成财产损失甚至人员伤亡. 1978年9月, 日本广岛一间工厂的切割机器人将一名值班工人当作钢板切割造成惨案[1]. 2015年7月, 一名22岁的技术工在大众汽车包纳塔尔工厂中发生了一台机器人意外伤害致死事件[2]. 据腾讯新闻报道, 2021年7月, 位于伦敦东南部的英国电商Ocado仓库发生机器人碰撞事故, 导致了火灾的发生. 由此可见, 碰撞检测是多臂/多机器人应用的关键问题.

关于机器人碰撞检测问题的研究, 通常用抽象、形式化的数学语言建立模型, 再分析论证其性质, 最后在计算机系统中运行. 基于以上分析, 在设计实现一个机器人碰撞检测算法时, 如何保障它的正确性和可靠性?传统分析验证方法主要基于测试与仿真方法, 由于测试用例与仿真用例受限, 测试与仿真方法均无法完全覆盖所有可能的验证路径[3]. 因此, 这些传统的非完备性验证手段已经无法满足安全攸关系统的机器人协调工作设计对正确性和安全性的要求.

近年来, 针对多臂/多机碰撞检测算法的研究很多, 如基于二维图像空间的算法[4]、基于空间剖分法的算法[5]、GJK算法[6]、层次包围盒技术[7]等. 但是由于缺乏严格的理论论证, 导致诸多的算法研究仍停留在传统的分析验证方法上. 与传统方法不同, 形式化方法使用严格的数学逻辑分析证明系统的正确性, 对所验证的属性或性质而言是精确而又完备的[8]. 当在交互式定理证明器中进行这样的证明时, 最高置信水平可以通过系统的可靠运行获得. 这是因为, 证明者所密切关注的证明过程只接受一组遵循数学逻辑的最小基本推理步骤.

对于多臂碰撞检测问题来说, 其在数学本质上皆可简化为双臂碰撞检测问题. 因此, 本文针对机器人双臂碰撞检测问题的核心, 即碰撞检测方法, 在高阶逻辑定理证明器HOL-Light[9]上, 以胶囊体和球体几何体单元建立机器人简化形式化模型. 通过形式化技术, 构建机器人几何体单元之间的最短距离和机器人碰撞模型, 形式化验证其相关属性以及机器人碰撞条件, 建立机器人碰撞检测方法定理证明库, 为实现多机系统的碰撞检测算法的可靠性与稳定性验证提供技术支撑和方法参考, 其验证框架如图 1所示.

图 1 机器人碰撞检测验证框架

为了增加本文工作的通用性, 我们采用任意N维向量的集合表示机器人的几何姿态, 并用集合迭代的方式形式化定义机器人, 从而使得本文工作可适用于任意自由度的任意多个姿态参数的机器人碰撞检测问题. 此外, 本文用可重载方式形式化定义机器人的基本几何体表示, 增加除球体和胶囊体以外的(如长方体)各类不同机器人基本几何体的形式化代码的可扩展性.

本文第1节介绍机器人碰撞检测算法的相关工作. 第2节介绍HOL-Light定理证明器和机器人碰撞检测方法的基本流程. 第3节介绍基本几何体模型与性质的形式化, 包括几何体模型高阶逻辑表达、最短距离和碰撞条件的形式化. 第4节介绍机器人碰撞检测方法的形式化, 包括机器人几何模型构建、碰撞模型高阶逻辑表达及形式化验证. 第5节总结全文.

1 相关工作

传统碰撞检测算法

碰撞检测算法作为机器人运动控制规划领域研究的关键, 可广泛应用于协调工作、虚拟手术、自动驾驶等领域. 目前常见的碰撞检测算法可根据其检测目标的空间维度划分为图像空间和几何空间两类.

基于图像空间的碰撞检测算法的关键在于利用三维物体的二维投影图像的碰撞检测, 例如Francois等人[10]提出的处理三角网格特征碰撞检测方法、Qu等人[11]提出的基于图像处理的人车碰撞检测方法. 这类方法虽然能够缩短计算时间, 但会受限于图像的分辨率, 可能导致不准确碰撞检测结果的产生. 而且这类方法并不适用于检测物体表面存在凹陷的情况;

相较于图像空间, 基于几何空间的碰撞检测算法的应用范围更广. 近年来, 常见的基于几何空间的碰撞检测算法可大致分为空间剖分法、GJK算法、层次包围盒技术和其他融合智能优化算法的群算法[12]. 以赵亮等人[13]提出的一种基于网格包络的碰撞检测算法为例, 这类空间剖分法可在一定程度上处理多个物体的碰撞对, 但在处理过大物体碰撞对规模或复杂物体时具有局限性. 相较于空间剖分法, 以凸体为基础模型的GJK算法在处理两个凸体的碰撞检测问题上更具优势. 而层次包围盒技术在机器人碰撞检测问题研究上更为方便, 且计算更为简单. 由于机器人构件的可拆分性, 近年来, 许多学者采用层次包围盒技术分析机器人碰撞检测问题. 2020年, Huang等人[14]提出一种基于关键点和关键线段模型的双臂上肢康复机器人自碰撞检测新算法. 随后, 梁孟德[15]提出了以球体和胶囊体表示的机器人关节和连杆的空间机器臂碰撞检测方法. 但是上述传统碰撞检测算法的验证皆停留在数值仿真阶段, 缺乏形式化验证.

形式化方法在机器人验证领域的应用

机器人系统作为当前最热的研究方向, 其应用领域大多要求系统的高安全性和可靠性. 以应用于医疗的多臂机器人协调工作为例, 若该机器人系统发生故障或失效, 则可能危及人身、财产和环境安全, 带来灾难性的后果. 在美国2013版机器人白皮书《A Roadmap for U.S. Robotics: From Internet to Robotics, 2013 Edition》中有明确要求: 人机交互机器人需用形式化方法进行验证. 近年来, 形式化验证已被用于帮助开发复杂的机器人系统[16].

面向机器人验证, 定理证明的应用较少. 在机器人运动学领域, 日本学者Affeldt等人[17]在定理证明器中实现了串联机器人运动学模型的构建与验证; 首都师范大学陈琦等人[18]完成了平面并联机构正向运动学形式化模型的构建与验证. 在机器人动力学领域, 巴基斯坦伊斯兰堡国立科技大学Rashid等人[19]对细胞注射机器人动力学分析进行了形式化验证. 上述工作增强了人们将定理证明技术引入到机器人系统工程应用的信心.

在机器人避碰方法验证领域, 德国研究中心的Täubig等人[20]使用定理证明的方法给出了安全函数算法, 对自动小车避障及路径规划算法进行验证, 并给出改进意见, 避免了车辆之间的相互碰撞, 确保了机器人的安全性. 卡内基梅隆大学的Mitsch等人[21]采用混成系统模型、定理证明建模和验证移动机器人的运动以及避障安全属性. 上述工作为定理证明方法在机器人碰撞问题的分析提供了基础思路.

综上所述, 基于定理证明的机器人碰撞检测算法验证的研究尚属空白. 针对该问题, 本文在定理证明器中建立了机器人碰撞检测方法基础定理库并对机器人碰撞检测方法进行了形式化建模与验证, 为实现多机系统碰撞检测算法的可靠性和稳定性验证提供了技术支撑和验证框架.

2.背景知识

HOL-Light定理证明器

定理证明系统HOL-Light[22]遵循LCF方法, 依赖于函数式编程语言ML, 支持高阶逻辑表达. 在HOL- Light系统中, 所有的证明最终都是在一小组原始推论的条件下完成的, 因此该系统拥有简洁的设计和极小可得逻辑内核. 虽然如此, 它却提供了强大的证明工具, 并已应用于一些重要的任务, 如数学的形式化和工业的形式化验证.

HOL-Light系统的证明是采用高阶逻辑表示目标, 基于策略和已有的定理库来证明目标. 根据目标形式, 可将证明方法分为直接处理原目标的正向证明方法和处理目标逆否命题的反向证明方法. 这两种方法皆是根据已知定理、公理、定义等处理目标, 不同之处在于目标形式是否为其等价逆否命题.

本文实现的机器人碰撞检测方法定理证明库是基于HOL-Light定理证明器中已有的集合库、实分析库和多元分析库等构建的. 值得注意的是, 在HOL-Light定理证明器已存在作为基本几何体球体的形式化定义及相关性质定理, 这为本文工作提供了良好的工具支撑.

机器人碰撞检测方法

机器人碰撞检测方法是通过由实际场景所得机器人姿态参数, 计算机器人各部分之间的最短距离, 从而判断是否碰撞的方法. 对于机器人碰撞检测而言, 其整体流程可分为两大部分: 第一, 基于基本几何体模型, 实现机器人本体高阶逻辑表达; 第二, 基于几何体最短距离模型及碰撞检测条件, 验证机器人碰撞模型.

因此, 在本文对机器人碰撞检测方法的形式化过程中, 依照这两个角度, 进一步划分形式化过程: 首先, 将机器人关节、连杆等几何体模型及其性质按照中心线和胶囊体划分描述为相应待验证的定理, 并对中心线和胶囊体的形式化表示及其基本性质进行验证; 其次, 针对各类几何体模型间最短距离问题, 在不同假设条件下, 将最短距离模型高阶逻辑建模及其相关性质证明分为球体与球体之间最短距离、球体与胶囊体之间最短距离、胶囊体与胶囊体之间最短距离这3类情况进行处理; 然后, 基于最短距离相关性质, 形式化推导并验证基本几何体碰撞条件; 最后, 在不同假设条件下, 构建机器人本体形式化模型, 分析机器人碰撞条件及检测判定模型, 实现机器人碰撞检测形式化验证.

基于几何体模型及性质

几何体模型高阶逻辑表达

对于机器人本体而言, 通常可将其关节、连杆、末端执行器等基本几何体单元简化为长方体、胶囊体、球体等形状, 进而通过利用计算基本几何体简化模型的碰撞检测方法, 达到减少计算量的目的. 因此, 本文采用球体和胶囊体来简化表示基本几何体形式化模型, 并对其相关性质进行形式化分析与验证.

在HOL Light中, 球体的形式化表示如定义1所示.

定义1(球体). ∀x e.cball(x, e)={y|dist(x, y)⇐e}.

其中, x表示球心, e表示该球体的半径, dist(x, y)表示空间中两点x和y的距离, 球体cball(x, e)表示所有与球心x距离小于等于半径e的点y的集合.

在数学上, 胶囊体可表示成在中心线上移动的球体的集合. 具体如图 2所示.

图 2 胶囊体

在图 2中, sp和ep分别表示胶囊体两端的端点, c1和c2分别表示胶囊体中心线两端的端点, u表示从端点sp到端点ep的向量, r表示胶囊体中心线上对应球体的半径, l表示中心线上的点, v表示胶囊体内的点.

因此, 由上述可知, 基于已有的球体定义, 我们给出中心线和胶囊体的相应形式化表示, 如定义2、定义3所示.

定义2(中心线).

∀c1 c2.p_center_line c1 c2={c1+s%(c2−c1)| & 0⇐s∧s⇐ & 1}.

定义3(胶囊体).

∀sp ep r.

capsule((sp, ep), r)=

{v|∃u c1 c2 l.u=ep−sp∧c1=(r*inv(norm u))%u+sp∧

c2=ep−(r*inv(norm u))%u∧ & 2*r < norm u∧

l IN p_center_line c1 c2∧norm(v−l)⇐r}

在上述定义中: ∧表示逻辑与; %表示向量的标量乘; & 表示将自然数变为实数的运算; *表示实数乘; inv表示倒数; norm表示向量范数; IN表示属于∈; s表示中心线上起点c1到任意一点l的距离与中心线起点c1到终点c2的距离的比值; c1+s%(c2−c1)表示中心线上任意一点l; p_center_line c1 c2表示距离比值s在0到1范围内的所有点c1+s%(c2−c1)的集合, 即以c1和c2为线段两端端点的中心线; capsule((sp, ep), r)表示以sp和ep为胶囊体两端端点、以r为半径的所有与中心线p_center_line c1 c2上任意一点l距离小于等于半径r的点v的集合.

根据胶囊体定义可知: 半径为正的条件是由实际物体的几何意义决定的, 胶囊体两端点的距离大于直径的条件是由胶囊体的几何形状决定的. 当胶囊体在半径非正或胶囊体两端点的距离小于等于直径时, 胶囊体为空集, 此时, 胶囊体表示无意义.

定理1(胶囊体为空).

∀sp ep r.r⇐ & 0∨norm(ep−sp)⇐ & 2*r⇒capsule((sp, ep), r)={⋅}.

其中, ∨表示逻辑或. 由于胶囊体是由中心线上移动球体内的所有点组成的集合, 所以胶囊体的相关性质都与球体和中心线相关. 因此, 这里我们只简单介绍部分中心线的相关性质.

通过分析中心线定义可知, 中心线的两端必在中心线上. 若中心线两端相同, 则该中心线等价于该端点. 因此, 可得如下定理.

定理2(线段起点). ∀c1 c2.c1 IN p_center_line c1 c2.

定理3(线段终点). ∀c1 c2.c2 IN p_center_line c1 c2.

定理4(中心线两端相同). ∀c.p_center_line c c={c}.

此外, 中心线上的点也满足以下两个性质.

若取中心线上任意两点, 则这两点之间的距离必定有界, 如定理5所示;

若取中心线上任意一点, 则由该点和线段两端点构成的向量必定互相平行, 且满足线性关系, 如定理6所示.

定理5(线段上任意两点距离有界).

∀x y c1 c2.x IN p_center_line c1 c2∧y IN p_center_line c1 c2⇒dist(x, y)⇐dist(c1, c2).

定理6(中心线上的向量平行).

∀x c1 c2.x IN p_center_line c1 c2⇒

c2−x=(norm(c2−x)*inv(norm(c2−c1)))%(c2−c1)∧

c1−x=(norm(c1−x)*inv(norm(c1−c2)))%(c1−c2)

其中, norm(c2−x)*inv(norm(c2−c1))表示中心线上一点x和端点c2之间的距离与中心线两端点c1和c2之间距离的比值, norm(c1−x)*inv(norm(c1−c2))表示中心线上一点x和端点c1之间的距离与中心线两端点c1和c2之间距离的比值. 这两个比值反映出了中心线上的点与线段端点构成向量的线性关系和平行关系.

了解更多详情可留言