一种基于共形几何代数的机械臂运动规划的形式化分析方法及系统与流程

本发明涉及一种基于共形几何代数的机械臂运动规划的形式化分析方法及系统，属于计算机科学技术领域。

背景技术：

共形几何代数(CGA)作为一种先进的几何表示和计算系统，它为经典几何提供了简洁、直观和统一的齐性代数框架。CGA通过引入两个额外维度基准原点(e₀)和无穷远点(e_∞)，使得欧氏空间嵌入到共形空间中，并赋予其闵氏内积结构，不仅保留了齐次空间中外积的Grassmann结构、更使得内积具备了表征距离、角度等基本度量的明确几何意义。CGA不仅成功的解决了运用几何语言完成几何计算，基于CGA的新方法和新算法层出不穷,还在许多高科技领域中如工程学和计算机科学的几何问题的解决起了关键性的作用。在机器人学中，CGA与以往的代数不同，它的运算对象不是数字，而是几何体。而机器人研究对象是于基本几何体建立的系统所形成的几何关系。因此，共形几何代数在机器人研究中具有独特性。

目前利用CGA为数学工具进行建模和计算分析传统上使用基于计算机的数值计算分析和计算机代数系统(CASs)如Maple、CLUCalc、Gaalop等，然而这两种方法均不能完全保证结果的正确性和精确性，由于计算的迭代次数受限于计算机内存和浮点数限制，数值方法并不能完全保证结果的精确性，而CASs提供的符号方法虽然利用核心算法可以精确推导出符号表达式的解在一定程度上避免了数值计算解不精确的问题，但是对庞大的符号集进行运算的算法并没有经过验证，并且在边界条件的处理上存在短板，所得到的结果仍然可能存在问题。为避免这些问题在系统早期设计时就产生，对CGA理论进行形式化分析是一种理想的解决办法。近几十年来，形式化方法在很多领域中都取得了巨大进步，基础研究的进展加上技术进步的推动，使新方法和新工具不断出现并逐步完善成为一种成熟的高可靠验证技术。它的主要思想是根据数学理论来证明所设计的系统满足系统的规范或具有所期望的性质。与人为笔纸分析和上述传统方法相比，形式化方法可根据数学逻辑的严密性提高发现微小而关键的早期设计错误的机率。

技术实现要素：

有鉴于此，本发明的目的在于提供一种基于共形几何代数的机械臂运动规划的形式化分析方法及系统，主要是在高阶逻辑证明工具HOL-Light中建立CGA系统的形式化模型，并基于建立的共形几何逻辑模型对机器人的运动规划进行形式化建模与验证，以解决现有技术中计算复杂、结果不太精确的问题。

下面将本发明的技术方案详述如下：

本发明一方面提供了一种基于共形几何代数的机械臂运动规划的形式化分析方法，其包括：

基于共形几何代数理论对机器人的基本构件、运动规划约束构建对应的几何模型，再用高阶逻辑语言描述所建立的几何模型，形成机器人的基本几何逻辑模型系统；

确定机器人的具体结构参数和运动规划参数；

基于基本几何逻辑模型系统对具体机器人运动过程的形式化建模，得到具体机器人运动过程的几何关系逻辑模型；

用逻辑公式描述需要验证的机器人运动过程的约束或属性；

将具体机器人运动过程的所述几何关系逻辑模型和待验证的运动过程的约束或属性组成一个逻辑命题；

利用逻辑推理引擎证明所述逻辑命题是否成立，成立表明所述模型满足所述的约束或者具备所述的属性，不成立表明所述模型不满足所述的约束或者不具备所述的属性。

其中，所构建的几何模型包括：机器人关节抽象化成点模型、机器人关节末端可达范围抽象化为球模型、机器人关节所在辅助面抽象为面模型、机器人关节点连接构成的线模型、将机器人基本构件的约束关系抽象化为几何体求交模型、机器人基本构件旋转运动抽象化为几何体纯旋转模型、机器人基本构件平移运动抽象化为几何体纯平移模型、机器人关节末端到达期望位置抽象化为几何体刚体运动模型、机器人基本构件度量关系抽象化为几何体间距离模型、机器人基本构件夹角抽象化为几何体夹角模型。

其中，所述机器人关节抽象化成点模型如下表示：

e_∞＝e_-+e₊

其中，小写s＝s₁e₁+s₂e₂+s₃e₃表示欧氏三维空间中的点，e1-e3是欧式三维空间中的单位正交基，s1-s3为系数，大写S为该点从欧氏空间映射到共形空间中的点的表达式；e₀表示原点，e_∞表示无穷远点，e₊,e_-为共形几何空间中的第四个基矢量和第五个基矢量。

其中，所述机器人关节末端可达范围抽象化为球模型用于将机器人关节末端可达范围抽象化为以关节为球心以两关节间的连杆为半径的球，如下表示：

其中，P、r分别表示球心和半径，球心用欧氏空间中三维向量表示，e_∞表示无穷远点。

其中，所述机器人关节所在辅助面抽象为面模型用于将机器人关节所在辅助面抽象为面，如下表示：

π＝n+de_∞

其中，n、d分别表示辅助面法向量和辅助面到原点的距离，法向量用欧氏空间中三维向量表示。

其中，所述机器人关节点连接构成的线模型用于将两关节间的连杆抽象为线，如下表示：

L^*＝A∧B∧e_∞

其中，A和B分别表示两关节所代表的点。

其中，所述将机器人基本构件的约束关系抽象化为几何体求交模型如下表示：

o＝o₁∧o₂∧...∧o_n

其中，o_i表示代表第i个机器人基本构件的几何体，i＝1……n

其中，所述机器人基本构件旋转运动抽象化为几何体纯旋转模型如下表示：

其中，R为共形几何代数中的旋转算子，为R的几何反，L表示的旋转轴,φ是旋转角度，o表示旋转前几何体的表达形式，o_rotated表示旋转后几何体的表达形式。

其中，所述机器人基本构件平移运动抽象化为几何体纯平移模型如下表示：

其中，T为共形几何代数中的平移算子，为T的几何反，其中t＝t₁e₁+t₂e₂+t₃e₃是平移向量，表示平移的方向和长度,o表示平移前几何体的表达形式，o_translated表示平移后几何体的表达形式。

其中，所述机器人关节末端到达期望位置抽象化为几何体刚体运动模型如下表示：

其中，R,T分别是旋转算子和平移算子，为M的几何反，o_{rigid_body_motion}表示经过刚体运动后的几何体表达形式。

其中，所述机器人基本构件度量关系抽象化为几何体间距离模型如下表示：

其中，a、b表示三维欧氏空间中任意两点，A、B为共形空间中该两点的对应表达式，几何体间距包括点线距离、点球距离、球球距离。

其中，所述机器人基本构件夹角抽象化为几何体夹角模型用于计算机器人基本构件夹角，如下表示：

其中，o₁、o₂分别表示待求夹角的几何体，

其中，所述机器人运动过程的形式化建模包括机器人抓取物体运动的形式化建模，具体包括：

步骤31：计算物体的被抓取位置所在的目标圆；

步骤32：计算机械爪所在圆；

步骤33：计算机械爪抓取物体的平移算子，所述平移算子包括平移轴和平移长度；

步骤34：计算机械爪抓取物体的旋转算子，包括旋转轴和旋转角度；

步骤35：计算机械爪新的目标位置。

其中，步骤1中目标圆如下计算得到：

通过物体边缘的四个特征点x1、x2、x3、x4得到物体的位置方位，其中x1,x2,x3为物体底部边沿上任意三点，x4为物体顶部边沿上任意一点，由物体底部三点构成的参考圆通过圆的下述直接表达式得到：

则物体底部参考圆所在的面如下得到：

其中，I_c为共形几何代数的伪标量；e_∞表示无穷远点；

所述目标圆Z_t为底部参考圆沿-π_b方向平移长度后的圆，则相应的平移算子为：

则物体被抓取的部位所形成的目标圆为：

其中，步骤32中所述机械抓所在圆如下计算：

已知机械爪所在圆的圆心P_h、半径ρ、机械爪上两点a,b可计算出机械爪所在圆的位置，首先构造机械爪所在球的位置，通过球的标准表达式得到：

其中，S_h为所述机械爪所在的球；

计算机械爪上两点a,b计算机械爪所在面：

计算机械爪所在球S_h和所在面相交得到机械爪所在圆Z_h：

Z_h＝S_h∧π_h。

其中，所述步骤33中机械爪抓取物体的平移算子包括平移轴和平移长度，具体如下计算：

首先计算目标圆Z_t的圆心：

P_t＝Z_te_∞Z_t

通过直接表达式计算平移轴

其中，P_h为机械爪所在圆的圆心；

计算平移长度d：

其中，步骤34中机械爪抓取物体的旋转算子包括旋转轴和旋转角度，具体如下计算：

计算目标圆Z_t和机械爪所在圆Z_h的两轴线和

获取两轴线确定的面为：

计算旋转轴

计算旋转角度：

其中，步骤35中机械爪新的目标位置如下计算：

通过步骤3和步骤4计算得到的平移轴平移长度d、旋转轴旋转角度θ，得到机械爪抓取该物体运动的旋转算子和平移算子：

其中，R和T分别为旋转算子和平移算子，

机械爪新的目标位置如下计算：

其中，Z'_h为机械爪新的目标位置，Z_h为机械爪所在圆，为R的几何反，为T的几何反。

一种基于共形几何代数的机械臂运动规划的形式化分析系统，其包括：

机器人基本几何逻辑模型建立模块，其基于共形几何代数理论对机器人的基本构件、运动规划约束构建对应的几何模型，再用高阶逻辑语言描述所建立的几何模型，形成机器人的基本几何逻辑模型系统；

参数确定模块，其确定机器人的具体结构参数和运动规划参数；

机器人运动过程几何关系逻辑模型建立模块，其基于基本几何逻辑模型系统对具体机器人运动过程的形式化建模，得到具体机器人运动过程的几何关系逻辑模型；

机器人运动过程验证模块，其利用逻辑公式描述需要验证的机器人运动过程的约束或属性；

逻辑命题构成模块，其将具体机器人运动过程的所述几何关系逻辑模型和待验证的运动过程的约束或属性组成一个逻辑命题；

证明模块，其利用逻辑推理引擎证明所述逻辑命题是否成立，成立表明所述模型满足所述的约束或者具备所述的属性，不成立表明所述模型不满足所述的约束或者不具备所述的属性。

本发明优点及功效在于：与传统方法不同，本发明中形式化验证根据系统形式规范或属性，使用数学方法证明系统的正确性，对所验证的性质而言是精确和完备的。而共形几何代数CGA由于可以对点、线、面、圆、球等几何元素以及这些几何元素的旋转和平移进行统一建模和处理，在处理机器人运动学和运动规划问题上具有很强的优势。在本发明中，通过应用共形几何代数理论，以几何对象为运算对象，提高解决问题的维数来简化机器人学计算中的耦合，降低计算复杂程度。

【附图说明】

图1为本发明中基于CGA的机器人抓取物体的形式化建模流程图。

图2为本发明中机器人抓取物体算法的验证流程图。

图3为本发明中目标圆Z_t的计算示意图。

图4为本发明中机械爪圆Z_h的计算示意图。

图5为本发明中旋转算子和平移算子的求解示意图。

【具体实施方式】

以下将参照附图更详细地描述本发明的各种实施例。在各个附图中，相同的元件采用相同或类似的附图标记来表示。为了清楚起见，附图中的各个部分没有按比例绘制。

目前发展的共形几何代数系统，理论上可以推广到空间的任意维数，主要应用是5维共形空间，由3维欧拉空间和2维闵氏空间构成，共形几何代数提供了表示几何体的Grassmann结构、表示几何变换的统一旋量作用、表示几何量的括号系统和不变量系统，在几何数据处理和几何计算方面表现出显著优势。在几何代数框架下，不同变换表达和运算形式具有统一性，经几何空间变换后，仍可保持原始几何对象的几何意义和几何特性。机器人研究对象是于基本几何体建立的系统所形成的几何关系，而与以往的代数不同，共形几何代数的运算对象不是数字，正是几何体，所以共形几何代数在机器人研究中具有独特性。共形几何代数还可将研究机器人的传统方法如李群李代数、吴方法、Clifford代数、螺旋矢量进行统一构造，运用到机构的分析和综合中，推动了机构学的发展，形成了具有普遍性的理论方法，降低了复杂的机构学建模和推导计算的复杂度，通过扩展维数将空间的各种几何元素表达为球体坐标，在复杂多自由度并联机构建模中可以得到比较简单的数学模型。

CGA框架下的几何体表达和构建可分别基于内积和外积进行.基于外积的几何形体表达主要反映不同层次几何形体间相互构建关系，而内积形式表达则可通过距离、角度等度量表征参数构建相应的参数方程.对于任意k阶片积A，其基于外积和基于内积构建的参数方程分别为X∧A＝0，X·A＝0.两者的表达可以通过内、外积间的对偶运算进行相互转换.表1给出基于内积和基于外积构建的几何体表达，两种表示方式被很多文献分别称作标准表达(standard representation)和直接表达(direct representation).

表1 CGA基本几何体表示

Table 1:Representation of the Conformal Geometric Entities

表1中x、n标记为粗体，表示欧氏三维空间中的向量：

x＝x₁e₁+x₂e₂+x₃e₃

另外，表1中为欧氏空间中的标准内积即点积(scalar product)，可以用多维向量库中的点积函数描述.直接表示法中主要通过外积“∧”连接几何体上的点{P_i}来构建出高维的几何体，例如一个球可以用球上的四个点来表达.在标准表示法中外积的含义不同，表示几何体间的相交，例如一个圆可以由两个球相交而成。

图1是本发明提出的基于共形几何代数CGA方法对机器人抓取物体的形式化建模与验证流程图，基于共形几何代数CGA方法对机器人操作的主要思想是CGA的几何体运动变换表达，但是本发明的变换几何对象是参考圆而不是参考点，机械爪和被抓取物体的几何特征都用圆来表示。本发明中，使用剑桥大学研发的高阶逻辑定理证明器HOL-Light作为形式化工具。HOL-Light是最流行的定理证明器之一，不仅拥有庞大的研究团队和用户群，而且包含丰富的数学定理库如实数分析库、超越函数库、积分微分库等和一系列高效的证明策略。

如图1所示，本发明提出了一种基于共形几何代数的机械臂运动规划的形式化分析方法，其包括：

基于共形几何代数理论对机器人的基本构件、运动规划约束构建对应的几何模型，再用高阶逻辑语言描述所建立的几何模型，形成机器人的基本几何逻辑模型系统；

确定机器人的具体结构参数和运动规划参数；

基于基本几何逻辑模型系统对具体机器人运动过程的形式化建模，得到具体机器人运动过程的几何关系逻辑模型；

用逻辑公式描述需要验证的机器人运动过程的约束或属性；

将具体机器人运动过程的所述几何关系逻辑模型和待验证的运动过程的约束或属性组成一个逻辑命题；

利用逻辑推理引擎证明所述逻辑命题是否成立，成立表明所述模型满足所述的约束或者具备所述的属性，不成立表明所述模型不满足所述的约束或者不具备所述的属性。

共形几何代数理论是机器人运动规划的形式化分析方法的理论基础，机器人运动规划的形式化分析方法可以针对具体机器人问题分析的普遍方法，逻辑推理引擎实现对共形几何代数形式化系统和机器人运动规划的形式化分析方法的逻辑表达与证明的推理演算。

所述基于基本几何逻辑模型系统对具体机器人运动规划的运动规划过程形式化建模具体包括对机器人抓取物体的运动规划过程形式化建模。

优选地，所述步骤二中的具体机器人为n自由度串联机械臂，末端包括一个T字形机械爪

优选地，所述步骤三中具体运动规划为机器臂抓取物体的运动规划过程

优选地，所述步骤四中待验证的运动规划属性为机械爪实现物体抓取成功并且抓牢。

所构建的几何模型包括：机器人关节抽象化成点模型、机器人关节末端可达范围抽象化为球模型、机器人关节所在辅助面抽象为面模型、机器人关节点连接构成的线模型、将机器人基本构件的约束关系抽象化为几何体求交模型、机器人基本构件旋转运动抽象化为几何体纯旋转模型、机器人基本构件平移运动抽象化为几何体纯平移模型、机器人关节末端到达期望位置抽象化为几何体刚体运动模型、机器人基本构件度量关系抽象化为几何体间距离模型、机器人基本构件夹角抽象化为几何体夹角模型。

下面对上述各个模型进行详细介绍。其中，几何体包括点、线、面、圆、球、点对等，用于表示机器人的基本构件。

机器人关节抽象化成点模型：

该模型中将机器人关节抽象化成点，上述数学表达式是共形几何代数中点的标准表达式。

上述模型表达式中小写s＝s₁e₁+s₂e₂+s₃e₃表示欧氏三维空间中的点，大写S为该点从欧氏空间映射到共形空间中的点的表达式.函数point_CGA的输入变量为欧氏空间中三维向量,其返回值为五维共形空间中的多重矢量，由于共形几何代数可由几何代数Cl_4,1构造而成，其数据类型定义为real^(4,1)multivector.其中s$n表示多维向量s的第n个元素，mbasis函数表示基本片积函数,dot函数表示欧氏空间中的点积，null_inf和null_zero函数分别表示零矢量e₀和e_∞：

e_∞＝e_-+e₊， (2)

其中e₀表示原点，e_∞表示无穷远点，利用e_∞,e₀取代e₊,e_-能够更紧凑的表示共形空间中的点.本发明将基矢量e₊,e_-定义为mbasis{4}和mbasis{5}，表示CGA空间中第四个基矢量和第五个基矢量.形式化定义如下：

|-null_zero＝(&1/&2)％(mbasis{5}-mbasis{4})/\null_inf＝(mbasis{5})+(mbasis{4})

该模型中point_CGA函数功能表示从三维欧氏空间到五维共形空间中点的映射关系。

机器人关节末端可达范围抽象化为球模型：

将机器人关节末端可达范围抽象化成以关节为球心连杆为半径的球，上述数学表达式是共形几何代数中球的标准表达式。函数sphere_CGA的输入变量p、r分别表示球心和半径，其中球心用欧氏空间中三维向量表示。

机器人关节所在辅助面抽象为面模型：π＝n+de_∞

将机器人关节所在辅助面抽象化为面，上述数学表达式是共形几何代数中面的标准表达式。函数plane_CGA的输入变量n、d分别表示法向量和到原点的距离，其中法向量用欧氏空间中三维向量表示。

机器人关节点连接构成的线模型:L^*＝A∧B∧e_∞

上述数学表达式是共形几何代数中任意两点构成的线的表达式，可用于机器人中关节点构成的线或者其他辅助线的表示，其中A、B分别表示共形几何空间中的两个点。所述模型用于将两关节间的连杆抽象为线。上述表达式为共形几何代数中线的直接表达式，函数line_direct_CGA的输入变量a、b分别表示线上的两点，数据类型为real^(4,1)multivector，其中函数outer表示几何代数中外积运算，外积运算可实现由低维几何体向高维几何体的构建，即扩维运算。

将机器人基本构件的约束关系抽象化为几何体求交模型：o＝o₁∧o₂∧…∧o_n

|-MEET o₁o₂…on＝o₁outer o₂outer…o_n

其中，o_i表示代表第i个机器人基本构件的几何体，i＝1……n；

所述模型利用外积实现代表机器人基本构件的几何体的求交运算。函数MEET的输入变量表示待求交的任意几何体，数据类型为real^(4,1)multivector。约束关系包括关节在连杆上(即点在线上)、连杆在关节所在辅助面上(线在面上)等。

机器人基本构件旋转运动抽象化为几何体纯旋转模型:

|-rotation_CGA t l＝cos(t/&2)％mbasis{}-l*(sin(t/&2)％mbasis{}

|-pure_rotationed_CGA x t l＝(rotation_CGA t l)*x*(reversion(rotation_CGA t l))

所述利用几何积实现几何体旋转变换，R为共形几何代数中的旋转算子，为R的几何反，L表示的旋转轴,φ是旋转角度.o表示旋转前几何体的表达形式，orotated表示旋转后几何体的表达形式.函数rotation_CGA的输入变量t、l分别表示旋转副的旋转角度与旋转轴，该函数实现了旋转算子的功能。函数pure_rotationed_CGA的输入变量x、t、l表示几何体、旋转角度和旋转轴，该函数的返回值为旋转后的几何体表示，其中函数reversion表示几何反。

机器人基本构件平移运动抽象化为几何体纯平移模型：

|-Translation_CGA t＝mbasis{}-(&1/&2)％(t*null_inf)

|-pure_translationed_CGA x t＝(Translation_CGA t)*x*(reversion(Translation_CGA t))

所述模型利用几何积实现几何体平移变换，T为共形几何代数中的平移算子，为T的几何反，其中t＝t₁e₁+t₂e₂+t₃e₃是平移向量，表示平移的方向和长度.o表示平移前几何体的表达形式，o_translated表示平移后几何体的表达形式.函数Translation_CGA的输入变量t是平移向量，表示平移的方向和长度，该函数实现了平移算子的功能。函数pure_translationed_CGA的输入变量x、t表示几何体、平移向量，该函数的返回值为平移后的几何体表示。

所述机器人关节末端到达期望位置抽象化为几何体刚体运动模型：

|-motor_CGA a l t＝rotation_CGA a l*Translation_CGA t

|-rigid_body_motion x a l t＝(motor_CGA a l t)*x*(reversion(motor_CGA a l t))

该模型利用几何积实现几何体刚体变换，其中R,T分别是旋转算子和平移算子,两者用几何积运算连接，为M的几何反，o_{rigid_body_motion}表示运动后的几何体表达形式，函数motor_CGA的输入变量x、a、l、t分别表示几何体、旋转角度、旋转轴、平移向量，该函数实现了马达算子的功能。函数rigid_body_motion的返回值为经过刚体运动后的几何体表示，该表达式表示几何体是先平移后旋转，但马达算子具有结合性，旋转算子和平移算子是可以交换的。

机器人基本构件度量关系抽象化为几何体间距离模型：

其中a、b表示三维欧氏空间中任意两点，A、B为共形空间中该两点的对应表达式，上述模型为共形几何代数中点点距离表达式，其中dist(a,b)表示两点距离，可用几何代数内积表达.其中点线距离、点球距离、球球距离在共形几何代数中也有具体的表达式，可用于机器人问题中具体计算时使用.

机器人基本构件夹角抽象化为几何体夹角模型：

该模型为共形几何代数中几何体夹角表达式，函数vector_angles_CGA的输入变量o₁o₂分别表示待求夹角的几何体，可以是线或面等，数据类型为real^(4,1)multivector，其中函数inner表示几何代数中左缩积运算，$${}表示取0阶子空间即标量的大小，函数mult_norm功能为多重矢量的求模运算。该模型可用于机器人基本构件之间夹角的计算。

优选地，所述方法中具体机器人为n自由度串联机械臂，末端包括一个T字形机械爪；

所述方法中具体运动规划为机器臂抓取物体的运动规划过程；

所述方法中待验证的运动规划属性为机械爪实现物体抓取成功并且抓牢。

图2示出了本发明中对机器人抓取物体的运动规划过程形式化建模的流程图。如图2所示，其包括：

步骤30：分别提取物体和机械爪的特征点；

步骤31：计算物体的被抓取位置所在的目标圆；

如图3和图4所示，通过物体边缘的四个特征点(x1,x2,x3,x4)得到物体的位置方位，其中x1,x2,x3为物体底部边沿上任意三点，x4为物体顶部边沿上任意一点。由物体底部三点构成的参考圆可通过圆的直接表达式得到：

则物体底部参考圆所在的面为：

其中,Ic为共形几何代数的伪标量，通过伪标量是可实现对偶运算，对偶运算实现几何体两种表示方法的相互转换。

由于方便设定物体被抓取的位置是中部，所以目标圆Z_t应该是底部参考圆沿-π_b方向平移长度后的圆，则相应的平移算子为：

则物体被抓取的部位所形成的目标圆为：

其中，上述计算过程在HOL-Light中进行形式化：

let dual_circle_zb x1 x2 x3＝(circle_direct_CGA(point_CGA x1)(point_CGA x2)(point_CGA x3))

let plane_b x1 x2 x3＝((dual_circle_zb x1 x2 x3)outer null_inf)*pseudo

let circle_zt x1 x2 x3 x4＝pure_translationed_CGA(--DUAL(dual_circle_zb x1 x2 x3))(--(&1/&2)％((plane_b x1 x2 x3)inner(point_CGA x4)))

其中，物体底部参考圆调用圆的直接表示函数circle_direct_CGA,x1 x2 x3 x4表示物体的四个特征点，HOL类型均是real^3，表示三维欧氏向量，通过函数point_CGA使特征点由三维欧氏空间嵌入到五维共形空间中。物体被抓取的部位所形成的目标圆通过纯平移函数pure_translationed_CGA实现。

步骤32：计算机械爪所在圆；

如图5所示，已知机械爪所在圆的圆心ph、半径ρ、机械爪上两点a,b可计算出机械爪所在圆的位置，首先构造机械爪所在球的位置，通过球的标准表达式得到：

然后通过圆心ph、机械爪上两点a,b计算机械爪所在面：

机械爪所在球Sh和所在面πh相交得到机械爪所在圆：

Z_h＝S_h∧π_h

上述计算过程在HOL-Light中进行如下形式化：

let sphere_Sh ph r＝sphere_CGA ph r

let dual_plane_pih ph a b＝plane_direct_CGA(point_CGA ph)(point_CGA a)(point_CGA b)

let circle_zh ph r a b＝(sphere_Sh ph r)outer(--DUAL(dual_plane_pih ph a b))

其中，辅助球S_h调用球的标准表示函数sphere_CGA实现,(ph:real^3)和(r:real)分别表示辅助球的圆心Ph和半径r,机械爪所在的辅助面调用面的直接表示函数plane_direct_CGA实现，(a:real^3)和(b:real^3)分别表示机械爪上两点。对偶算子用DUAL函数。

步骤33：计算机械爪抓取物体的平移算子，所述平移算子包括平移轴和平移长度；

如图5所示，易知平移长度应该为目标圆Zh和机械爪所在圆Zt的圆心距离，而平移轴是过两圆心的直线。首先计算目标圆Zh的圆心：

P_t＝Z_te_∞Z_t

由于平移轴由两圆心确定，可通过直接表达式计算平移轴：

相应地，平移长度为

上述计算过程在HOL-Light中进行形式化：

let center_point_pt x1 x2 x3 x4＝(circle_zt x1 x2 x3 x4)*null_inf*(circle_zt x1 x2 x3 x4)

let dual_translation_axis ph x1 x2 x3 x4＝line_direct_CGA(point_CGA ph)(center_point_pt x1 x2 x3 x4)

let distance ph x1 x2 x3 x4＝--sqrt(&2*((point_CGA ph)inner(center_point_pt x1 x2 x3 x4))$${})

步骤34：计算机械爪抓取物体的旋转算子，包括旋转轴和旋转角度；

如图5，机械爪抓取物体的旋转轴满足几点约束：

1.过机械爪所在圆的圆心ph

2.在由两圆的轴线确定的面上

所以，先计算目标圆Z_t和机械爪所在圆Z_h的两轴线：

由两轴线确定的面为：

则旋转轴计算为：

而旋转角度易知为两轴线的夹角，由夹角公式得：

上述计算过程在HOL-Light中进行形式化：

let dual_lh ph r a b＝(circle_zh ph r a b)outer null_inf

let dual_lt x1 x2 x3 x4＝(circle_zt x1 x2 x3 x4)outer null_inf

let dual_plane_th x1 x2 x3 x4 ph r a b＝(dual_lt x1 x2 x3 x4)outer((dual_lh ph r a b)*(null_zero outer null_inf))

let dual_rotation_axis x1 x2 x3 x4 ph r a b＝(point_CGA ph)outer(--DUAL(dual_plane_th x1 x2 x3 x4ph r a b))outer null_inf

let rotation_angle x1 x2 x3 x4 ph r a b＝vector_angles_CGA(dual_lh ph r a b)(dual_lt x1 x2 x3 x4)

其中，旋转角度θ的求解则使用CGA夹角公式来计算，由函数vector_angles_CGA实现.

步骤35：计算机械爪新的目标位置；

通过步骤33和步骤34计算得到的平移轴平移长度d、旋转轴旋转角度θ，可以通过机器人几何逻辑模型中定义6和定义7得到机械爪抓取该物体运动的旋转算子和平移算子：

其中，R和T分别为旋转算子和平移算子，

机械爪新的目标位置可以先旋转后平移计算得到，利用定义8中提供的刚体算子可计算为：

最终可以得到机械爪新的目标位置，在HOL-Light中进行形式化：

let circle_zh_new x1 x2 x3 x4 ph r a b＝pure_translationed_CGA

(pure_rotationed_CGA(circle_zh ph r a b)(rotation_angle x1 x2 x3 x4 ph r a b)(--DUAL(dual_rotation_axis x1 x2 x3 x4 ph r a b)))

((distance ph r a b x1 x2 x3 x4)％(--DUAL(dual_translation_axis ph r a b x1 x2 x3 x4)))

其中，由于机械爪新的目标位置是通过先旋转后平移得到，所以先将机械爪面所构成的圆z_h通过纯旋转函数pure_rotationed_CGA进行旋转运动，该函数的输入变量是被旋转的几何体z_h、旋转角度θ和旋转轴，即(circle_zh ph r a b)、(rotation_angle x1 x2 x3 x4 ph r a b)和(--DUAL(dual_rotation_axis x1 x2 x3 x4 ph r a b))，再将旋转后的圆通过纯平移函数pure_translationed_CGA完成平移运动，该函数输入变量是旋转的几何体平移向量dl_T。

上述代码主要内容涉及共形几何代数CGA几何体表示、几何体间距离特征和几何体运动变换的形式化。机械爪圆Z_h一步步靠近物体位置所在目标圆Z_t，最终机械爪成功抓取到物体并且抓牢必须满足一定的几何约束关系。上述算法中，该约束关系为机械爪面所在圆Z_h新的位置最终应该与物体所在目标圆Z_t的位置吻合，即在共形几何代数CGA中表达式应该满足相等的关系。可以在HOL-light中建立目标验证这层几何约束关系：

goal:

本发明中关于基于CGA对机器人的运动规划进行形式化建模与验证的方法，在HOL-Light中可以加载使用。其中最大难点是定理证明技术需要大量的人机交互，工作量较大，时间耗费较长，建模过程中强调使用者对共形几何代数CGA理论知识的熟识度，验证过程中要求严密的思维和一定的逻辑推理经验。

以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。