CN113400310A - 三指机器人灵巧手逆运动学的形式化验证方法及电子设备 - Google Patents

三指机器人灵巧手逆运动学的形式化验证方法及电子设备 Download PDF

Info

Publication number
CN113400310A
CN113400310A CN202110719629.1A CN202110719629A CN113400310A CN 113400310 A CN113400310 A CN 113400310A CN 202110719629 A CN202110719629 A CN 202110719629A CN 113400310 A CN113400310 A CN 113400310A
Authority
CN
China
Prior art keywords
dot
matrix
pow
point
theorem
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202110719629.1A
Other languages
English (en)
Other versions
CN113400310B (zh
Inventor
王国辉
施智平
关永
陈善言
王畅
张倩颖
邵振洲
王瑞
李晓娟
李希萌
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Capital Normal University
Original Assignee
Capital Normal University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Capital Normal University filed Critical Capital Normal University
Priority to CN202110719629.1A priority Critical patent/CN113400310B/zh
Publication of CN113400310A publication Critical patent/CN113400310A/zh
Application granted granted Critical
Publication of CN113400310B publication Critical patent/CN113400310B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1679—Programme controls characterised by the tasks executed
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J15/00—Gripping heads and other end effectors
    • B25J15/08—Gripping heads and other end effectors having finger members
    • B25J15/10—Gripping heads and other end effectors having finger members with three or more finger members
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J17/00—Joints
    • B25J17/02—Wrist joints
    • B25J17/0258—Two-dimensional joints
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1661—Programme controls characterised by programming, planning systems for manipulators characterised by task planning, object-oriented languages

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)

Abstract

本公开实施例公开了一种三指机器人灵巧手逆运动学的形式化验证方法及电子设备，所述方法包括：形式化描述旋量理论的内容；将形式化描述的旋量理论的内容输入至定理证明器中进行证明，获得定理1、定理2和定理3；形式化描述Paden‑Kahan子问题1、子问题2和子问题3；将形式化描述的子问题1、子问题2和子问题3输入至所述定理证明器中进行证明，获得定理4、定理5和定理6；其中，所述定理4、定理5和定理6分别为子问题1、子问题2和子问题3的解；利用形式化描述的所述定理1、定理2、定理3、定理4、定理5、定理6、子问题1、子问题2和子问题3验证所述三指机器人灵巧手的运动学求解问题。

Description

三指机器人灵巧手逆运动学的形式化验证方法及电子设备
技术领域
本公开涉及机器人技术领域，具体涉及一种三指机器人灵巧手逆运动学的形式化验证方法及电子设备。
背景技术
随着人工智能和计算机的快速发展，机器人被广泛应用于生产、生活各个领域。给人类带来便利的同时也带来显著的危险。因此，机器人安全性问题备受关注，而逆运动学问题是机器人安全性的决定因素。
传统的测试与模拟仿真是保证机器人安全运行的基本方法。随着机器人***越来越复杂，测试用例规模越来越大，对***进行完备测试几乎是不可能实现的；模拟仿真方法建立在如Maple、Mathematica等计算机软件平台上，这不仅受计算机硬件运算速度和内存的限制，同时也受计算机软件***中隐藏Bug的影响。因此，仅仅依靠这些传统的非完备性验证手段，已经无法满足安全攸关的机器人***对安全性的要求。
发明内容
本公开实施例提供一种三指机器人灵巧手逆运动学的形式化验证方法及电子设备。
第一方面，本公开实施例中提供了1、一种三指机器人灵巧手逆运动学的形式化验证方法，其特征在于，包括：
形式化描述旋量理论的内容；其中，形式化描述的旋量理论内容至少包括定义旋量的算子、齐次矩阵以及所述齐次矩阵演变的通用矩阵；
将形式化描述的旋量理论的内容输入至定理证明器中进行证明，获得定理1、定理2和定理3；其中，所述定理1为运动旋量的矩阵表达式，定理2为三指机器人灵巧手仅平移时运动旋量的指数表达式；定理3为三指机器人灵巧手即平移又旋转时的运动旋量的指数表达式；
形式化描述Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3；
将形式化描述的Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3输入至所述定理证明器中进行证明，获得定理4、定理5和定理6；其中，所述定理4、定理5和定理6分别为Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3的解；
利用形式化描述的所述定理1、定理2、定理3、定理4、定理5、定理6、Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3验证所述三指机器人灵巧手的运动学求解问题。
进一步地，形式化描述旋量理论的内容，包括：
将运动旋量的算子、齐次矩阵以及齐次矩阵演变的通用矩阵形式化描述为定义1、定义2和定义3；其中，
定义1：运动旋量的算子：
运动旋量的数学公式如下表示：
Figure BDA0003136479500000021
ξ＝(ω；υ)
式中，ξ是运动旋量
Figure BDA0003136479500000022
的坐标表示，υ是平移线速度，ω是旋转角速度，
Figure BDA0003136479500000023
是对应于ω的反对称矩阵；
其中，运动旋量的算子用于表示将坐标表示ξ变换成运动旋量
Figure BDA0003136479500000024
的映射关系，定理证明器中公式(1)的形式化描述如下：
val screw_2_matrix s＝lambda i j.if i<＝3/\j<＝3then(vec3_2_ssm(FSTs))$i$j else if i<＝3/\j＝4then(SND s)$i else&0；
定理证明器中，运动旋量用s表示，FST s表示旋转角速度ω，为运动旋量s的第一个分量；SND s表示平移线速度υ，为运动旋量s的第二个分量；函数screw_2_matrix的输入变量是运动旋量s，i，j表示矩阵中元素的标号，i表示行，j表示列，输出的是3+1维矩阵；vec3_2_ssm表示反对称矩阵转化函数；
其中，
Figure BDA0003136479500000025
是3×3矩阵，在i≤3，j≤3时，旋量s中的元素为
Figure BDA0003136479500000026
j＝4时为υ，其他元素为0；
定义2：齐次矩阵
为了三指机器人灵巧手的姿态，假设{S}和{T}分别为惯性坐标系和工具坐标系，其中{T}固连于三指机器人灵巧手上，则三指机器人灵巧手相对于惯性坐标系{S}的位姿映射集合可由工具坐标系{T}相对于惯性坐标系{S}的映射集合如下表示：
Figure BDA0003136479500000031
式中，SE(3)为特殊欧式群，即李群，R是3×3的旋转矩阵，P是位置矢量，SO(3)是特殊正交群；
其中，齐次矩阵T∈SE(3)，满足李群的性质，定理证明器中公式(2)的形式化描述如下：
val homo_trans x R＝lambda i j.
if i＝(dimindex(:N)+1)/\∧～(j＝dimindex(:N)+1)then&0
else if(i＝dimindex(:N)+1)/\(j＝dimindex(:N)+1)then&1
else if～(i＝dimindex(:N)+1)/\(j＝dimindex(:N)+1)then x$i
else R$i$j；
定理证明器中，homo_trans的输入是N维向量x和N维旋转矩阵R的变量，输出是N+1维的矩阵；dimindex在定理证明器中表示的是矩阵的维度；x$i表示向量x的第i个分量；R$i$j表示旋转矩阵R中第i行第j列的元素；符号&表示将自然数类型转化为实数类型；
齐次矩阵T为
Figure BDA0003136479500000032
即在i＝N+1时，齐次矩阵T中的元素为0，在i＝N+1、j＝N+1时，齐次矩阵T中的元素为1，i≠N+1、j＝N+1时，齐次矩阵T中的元素为x，其他元素为R；
定义3：齐次矩阵演变的通用矩阵在定理证明器中的形式化描述如下：
val homo_trans_tangent x R＝lambda i j.
if i＝(dimindex(:N)+1)then&0
else if～(i＝dimindex(:N)+1)/\(j＝dimindex(:N)+1)then x$i
else R$i$j
定理证明器中，homo_trans_tangent的输入变量是N维向量x和N维旋转矩阵R，输出是N+1维的矩阵；
通用矩阵为
Figure BDA0003136479500000041
即在i＝N+1时，通用矩阵中的元素为0，在i≠N+1、j＝N+1时，通用矩阵中的元素为x，其他元素为R。
进一步地，将形式化描述的旋量理论的内容输入至定理证明器中进行证明，获得定理1、定理2和定理3，包括：
通过在定理证明器中基于定义1和定义3证明运动旋量的数学形式和齐次矩阵演变的通用矩阵的数学形式相同得到定理1；
将三指机器人灵巧手仅平移时运动旋量的指数表达式在定理证明器中形式化描述如下：
Figure BDA0003136479500000042
a.FST s＝vec 0＝＝>matrix_exp(a％％screw_2_matrix s)＝homo_trans(a％(SND s))(mat 1)
其中，a是实数，表示旋转角度θ，取值范围为[0，2π)；函数matrix_exp表示矩阵域上的指数函数matrix_exp，函数matrix_exp的输入变量a％％screw_2_matrix s为3+1阶矩阵，函数matrix_exp的输出结果为该矩阵的指数映射，类型为3阶矩阵；mat是定理证明器中定义的高阶逻辑函数，该高阶逻辑函数的输入变量为自然数，返回类型为n阶实矩阵，mat的返回值是斜对角为该输入变量对应实数的矩阵，用mat 1可表示单位矩阵；符号＝＝>代表推出；符号“％”表示向量的标量乘符号；符号“％％”表示矩阵的标量乘符号；
基于定义3的形式化描述，在定理证明器中证明上述形式化描述的等式两边相等的关系的过程以获得定理2；
将三指机器人灵巧手即平移又旋转时运动旋量的指数表达式在定理证明器中形式化描述如下：
Figure BDA0003136479500000043
a.norm(FST s)＝&1＝＝>matrix_exp(a％％screw_2_matrix s)＝homo_trans((mat 1-matrix_exp(a％％vec3_2_ssm(FST s)))**((FST s)cross(SND s))+a％(vec3_vtrans(FST s)**(SND s)))(matrix_exp(a％％vec3_2_ssm(FST s)))
其中，vec3_vtrans表示矩阵的转置；norm是定理证明器中定义的一个高阶逻辑函数，该高阶逻辑函数的输入变量是N维向量，返回类型是实数，输出结果是N维向量的范数，即该向量的长度值；符号&表示将自然数类型转化为实数类型；符号“**”表示乘向量的矢量乘符号；
基于定义3的形式化描述，在定理证明器中证明上述形式化描述的等式两边相等的关系的过程以获得定理3。
进一步地，将形式化描述的Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3输入至所述定理证明器中进行证明，获得定理4、定理5和定理6，包括：
定理证明器中形式化描述Paden-Kahan子问题1，并证明Paden-Kahan子问题1的解的以获得定理4；其中，Paden-Kahan子问题1在定理证明器中如下形式化描述：
Figure BDA0003136479500000051
w r u u′v v′p q s a.
(--(pi/&2)<a∧a<pi/&2)/\s＝(w,r cross w)/\norm w＝&1/\u＝p-r∧v＝q-r/\u′＝u-(vec3_vtrans(FST s)**u)/\v′＝v-(vec3_vtrans(FST s)**v)/\matrix_exp(a％％screw_2_matrix s)**(homo_point(mk_point r))＝(homo_point(mk_point r))/\matrix_exp(a％％screw_2_matrix s)**(homo_point(mk_point p))＝(homo_point(mk_point q))/\～(u′＝vec 0)norm(u′)＝norm(v′)/\w dot u＝w dot v
Figure BDA0003136479500000052
a＝atn((w dot(u’cross v′))/(u′dot v′))
其中，w，r，u，u'，v，v'，p，q是向量，w代表旋转轴，u'，v'分别是向量u，v的投影；s是运动旋量；a是实数，表示旋转角度θ，范围为[-π/2，π/2]；r是旋转轴上的一点，满足位置不变原则；homo_point表示齐次点；screw_2_matrix表示旋量的算子；vec 0表示0向量；dot表示向量的点乘；cross表示向量的叉乘；
其中，在定理证明器中输入Paden-Kahan子问题1的形式化描述，并进行证明得到Paden-Kahan子问题1的解的过程包括：
引入子目标
Figure BDA0003136479500000053
并在定理证明器中证明该子目标成立；
将证明成立后的所述子目标加入假设列表，利用定理证明器证明Paden-Kahan子问题1的解以获得定理4。
进一步地，将形式化描述的Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3输入至所述定理证明器中进行证明，获得定理4、定理5和定理6，包括：
定理证明器中形式化描述Paden-Kahan子问题2，并证明Paden-Kahan子问题1的解以获得定理5；
其中，Paden-Kahan子问题2在定理证明器中如下形式化描述：
Figure BDA0003136479500000061
w1 w2 r u u'v v'p q c z a a1 a2 x1 x2 x3 s1 s2.
(--(pi/&2)<a/\a<pi/&2)/\
(--(pi/&2)<a1/\a1<pi/&2)/\
(--(pi/&2)<a2/\a2<pi/&2)/\
s1＝(w1,r cross(w1))/\s2＝(w2,r cross(w2))/\
norm w1＝&1/\norm w2＝&1/\u＝p-r/\v＝q-r/\z＝c-r/\
u'＝u-(vec3_vtrans(FST s2)**u)/\
v'＝v-(vec3_vtrans(FST s1)**v)/\
z＝x1％w1+x2％w2+x3％(w1 cross w2)/\(norm z)pow 2＝x1 pow 2+x2 pow 2+(&2*x1*x2)*(w1 dot w2)+(x3pow 2)*(norm(w1 cross w2))pow 2/\～((norm(w1 crossw2))pow 2＝&0)/\～((w1 dot w2)pow 2-&1＝&0)/\
matrix_exp(a1％％screw_2_matrix s1)**(homo_point(mk_point r))＝(homo_point(mk_point r))/\
matrix_exp((--a1)％％screw_2_matrix s1)**(homo_point(mk_point r))＝(homo_point(mk_point r))/\
matrix_exp(a2％％screw_2_matrix s2)**(homo_point(mk_point r))＝(homo_point(mk_point r))/\
matrix_exp(a2％％screw_2_matrix s2)**(homo_point(mk_point p))＝(homo_point(mk_point c))/\
matrix_exp((--a1)％％screw_2_matrix s1)**(homo_point(mk_point q))＝(homo_point(mk_point c))/\
matrix_exp(a1％％screw_2_matrix s1)**matrix_exp(a2％％screw_2_matrixs2)**(homo_point(mk_point p))＝(homo_point(mk_point q))/\
a＝a1+a2/\～(u'＝vec 0)/\～(v'＝vec 0)＝＝>((&0<＝x3)＝＝>(a＝--atn((w1 dot(v'cross((((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2+sqrt((norm u pow2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1 dotw2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1 dot w2)*(w2 dotu)-w1 dot v)/((w1 dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dotw2)pow 2-&1))*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1 cross w2))-vec3_vtrans w1**(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2+sqrt((norm u pow 2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dotw2)pow 2-&1))*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1 cross w2)))))/(v'dot((((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2+sqrt((norm u pow 2-(((w1 dotw2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1 dot w2)*(w1 dotv)-w2 dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1 dot w2)*(w2 dot u)-w1 dotv)/((w1 dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow2-&1))*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1 cross w2))-vec3_vtrans w1**(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2+sqrt((norm u pow 2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1 dot w2)*(w1 dot v)-w2dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))*(w1dot w2))/norm(w1 cross w2)pow 2)％(w1 cross w2)))))+atn((w2 dot(u'cross((((w1dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dotv)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2+sqrt((norm u pow 2-x1 pow 2-x2 pow 2-(&2*x1*x2)*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1 cross w2))-vec3_vtransw2**(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dotw2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2+sqrt((norm u pow 2-x1 pow2-x2 pow 2-(&2*x1*x2)*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1 crossw2)))))/(u'dot((((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2+sqrt((norm u pow2-x1 pow 2-x2 pow 2-(&2*x1*x2)*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1cross w2))-vec3_vtrans w2**(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2+sqrt((norm u pow 2-x1 pow 2-x2 pow 2-(&2*x1*x2)*(w1dot w2))/norm(w1 cross w2)pow2)％(w1 cross w2)))))))/\
((x3<&0)＝＝>(a＝--atn((w1 dot(v'cross((((w1 dot w2)*(w2 dot u)-w1dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dotw2)pow 2-&1)％w2-sqrt((norm u pow 2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1dot w2)pow 2-&1))pow 2-(((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)*((w1 dotw2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))*(w1 dot w2))/norm(w1 crossw2)pow 2)％(w1 cross w2))-vec3_vtrans w1**(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow2-&1)％w2-sqrt((norm u pow 2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))pow2-(&2*((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))*(w1 dot w2))/norm(w1 cross w2)pow2)％(w1 cross w2)))))/(v'dot((((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2-sqrt((norm u pow 2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1 dot w2)*(w1 dot v)-w2dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2dot u)/((w1 dot w2)pow 2-&1))*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1 crossw2))-vec3_vtrans w1**(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2-sqrt((normu pow 2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1 dot w2)*(w2dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1dot w2)pow 2-&1))*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1 cross w2)))))+atn((w2 dot(u'cross((((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2-sqrt((norm u pow2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1 dotw2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1 dot w2)*(w2 dotu)-w1 dot v)/((w1 dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dotw2)pow 2-&1))*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1 cross w2))-vec3_vtrans w2**(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2-sqrt((norm u pow 2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dotw2)pow 2-&1))*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1 cross w2)))))/(u'dot((((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2-sqrt((norm u pow 2-(((w1 dotw2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1 dot w2)*(w1 dotv)-w2 dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1 dot w2)*(w2 dot u)-w1 dotv)/((w1 dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow2-&1))*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1 cross w2))-vec3_vtrans w2**(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2-sqrt((norm u pow 2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1 dot w2)*(w1 dot v)-w2dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))*(w1dot w2))/norm(w1 cross w2)pow 2)％(w1 cross w2)))))))
其中，w1，w2，r，u，u'，v，v'，p，q，c，z是向量，w1和w2代表三指机器人灵巧手分别绕其旋转的第一旋转轴和第二旋转轴，u'，v'分别是向量u，v的投影；s1和s2分别是第一旋转轴的旋量和第二旋转轴的旋量；a、a1和a2是实数，表示旋转角度θ、θ1和θ2，范围为[-π/2，π/2]；x1，x2和x3是实数，是自定义的变量；r是旋转轴上的一点，满足位置不变原则；homo_point表示齐次点；screw_2_matrix表示旋量的算子；vec 0表示0向量。dot表示向量的点乘；cross表示向量的叉乘。
进一步地，将形式化描述的Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3输入至所述定理证明器中进行证明，获得定理4、定理5和定理6，包括：
定理证明器中形式化描述Paden-Kahan子问题3，并证明Paden-Kahan子问题3的解以获得定理6；
其中，Paden-Kahan子问题3在定理证明器中如下形式化描述：
Figure BDA0003136479500000101
w r u u′v v′p q s d a a0c d′.
((--(pi/&2)<a0/\a0<pi/&2)/\(--(pi/&2)<a/\a<pi/&2)/\&0<＝(abs c)/\(absc)<＝pi/\s＝(w,r cross w)/\norm w＝&1/\u＝p-r/\v＝q-r/\c＝a0-a/\u′＝u-(vec3_vtrans(FST s)**u)/\v′＝v-(vec3_vtrans(FST s)**v)/\
matrix_exp(a0％％screw_2_matrix s)**(homo_point(mk_point r))＝(homo_point(mk_point r))/\
matrix_exp(a0％％screw_2_matrix s)**(homo_point(mk_point p))＝(homo_point(mk_point q))/\～(u′＝vec 0)/\～(v′＝vec 0))/\
(d)pow 2＝(norm(v-matrix_exp(a％％vec3_2_ssm w)**u))pow 2/\
(d′)pow 2＝(d)pow 2-(abs(w dot(p-q)))pow 2＝＝>
(&0<＝c＝＝>a＝a0-acs(((norm(u′))pow 2+(norm(v′))pow 2-(d′pow 2))/
(&2*(norm(u′))*(norm(v′)))))/\
(c<&0＝＝>a＝a0+acs(((norm(u′))pow 2+(norm(v′))pow 2-(d′pow 2))/
(&2*(norm(u′))*(norm(v′)))))
其中，w，r，u，u'，v，v'，p，q是向量，w代表旋转轴，u'，v'分别是向量u，v的投影；r是转轴上的一点，满足位置不变原则；s是运动旋量；a和a0是实数，代表旋转的角度，范围为[-π/2，π/2]；d是实数，代表点p绕旋转轴旋转后到点q的距离；d'是d在平面上的投影；c是实数表示a0-a的差；homo_point表示齐次点；screw_2_matrix表示运动旋量的算子；vec 0表示0向量。dot表示向量的点乘；cross表示向量的叉乘。
进一步地，利用形式化描述的所述定理1、定理2、定理3、定理4、定理5、定理6、Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3验证所述三指机器人灵巧手的运动学求解问题，包括：
将所述三指机器人灵巧手食指的正向运动学在定理证明器中如下形式化描述：
gst_a a1 s1 a2 s2 a3 s3 a4 s4 x＝
matrix_exp(a1％％screw_2_matrix s1)**
matrix_exp(a2％％screw_2_matrix s2)**
matrix_exp(a3％％screw_2_matrix s3)**
matrix_exp(a4％％screw_2_matrix s4)**(gst_initial x)
其中，s1、s2、s3、s4、分别为所述三指机器人灵巧手食指的第一至第四关节的运动旋量
Figure BDA0003136479500000121
和
Figure BDA0003136479500000122
a1、a2、a3和a4分别代表所述三指机器人灵巧手食指的第一至第四关节的旋转角度θ1、θ2、θ3和θ4；matrix_exp代表以e为底的指数函数；screw_2_matrix表示旋量的算子；gst_initial x的输入变量是向量x，x表示初始位姿的三维坐标，输出是齐次矩阵，代表初始位姿gst(0)；gst_a表示三指机器人灵巧手食指经过运动之后的位姿；
将所述三指机器人灵巧手食指从第二关节到末端执行器的刚体变换的运动过程在定理证明器中如下形式化描述：
g_1 a2 s2 a3 s3 a4 s4＝matrix_exp(a2％％screw_2_matrix s2)**
matrix_exp(a3％％screw_2_matrix s3)**
matrix_exp(a4％％screw_2_matrix s4)
其中，s2、s3、s4、分别为所述三指机器人灵巧手食指的第二至第四关节的运动旋量
Figure BDA0003136479500000123
和
Figure BDA0003136479500000124
a2、a3和a4分别代表是所述三指机器人灵巧手食指运动中第二至第四关节的旋转角度θ2、θ3和θ4；matrix_exp代表以e为底的指数函数；screw_2_matrix表示旋量的算子；
在定理证明器中验证所述三指机器人灵巧手食指从第一个关节到末端执行器的刚体变换的运动过程的代数运算，该验证过程在定理证明器中如下形式化描述：
Figure BDA0003136479500000125
a1a2 a3 a4 s1 s2 s3 s4 x gd.
gst_a a1 s1 a2 s2 a3 s3 a4 s4 x＝gd＝＝>
matrix_exp(--(a1)％％screw_2_matrix s1)**gd**(matrix_inv(gst_initialx))＝(g_1a2 s2 a3 s3 a4 s4)
其中，s1、s2、s3、s4、分别为所述三指机器人灵巧手食指第一至第四关节的运动旋量
Figure BDA0003136479500000126
和
Figure BDA0003136479500000127
a1、a2、a3和a4分别代表是所述三指机器人灵巧手运动中食指的第一至第四关节的旋转角度θ1、θ2、θ3和θ4；matrix_exp代表以e为底的指数函数；screw_2_matrix表示运动旋量的算子；gd被定义为所述三指机器人灵巧手食指从第一个关节到末端执行器的刚体变换的运动过程；matrix_inv表示矩阵的逆；gst_initial x的输入变量是向量x，输出是齐次矩阵，代表所述三指机器人灵巧手食指的初始位姿gst(0)。
进一步地，利用形式化描述的所述定理1、定理2、定理3、定理4、定理5、定理6、Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3验证所述三指机器人灵巧手的运动学，还包括：
在定理证明器中验证所述三指机器人灵巧手食指从第二关节到末端执行器的刚体变换的运动过程的代数运算，该验证过程如下形式化描述：
Figure BDA0003136479500000131
θ2θ3θ4s2s3 s4.
matrix_exp(θ2％％screw_2_matrix s2)＝
(g_1a2 s2 a3 s3 a4 s4)**matrix_exp(--θ4％％screw_2_matrix s4)**
matrix_exp(--θ3％％screw_2_matrix s3)
其中，s2、s3、s4、分别为所述三指机器人灵巧手食指第二至第四关节的运动旋量
Figure BDA0003136479500000132
和
Figure BDA0003136479500000133
a2、a3和a4分别代表是所述三指机器人灵巧手运动中食指第二至第四关节的旋转角度θ2、θ3和θ4；matrix_exp代表以e为底的指数函数；screw_2_matrix表示旋量的算子；和/或，
在定理证明器中验证所述三指机器人灵巧手食指第一关节的旋转角度θ1，该验证过程如下形式化描述：
Figure BDA0003136479500000134
a1a2 a3 a4 h1 h2 h3 h4 px py.
px＝cos(a1)*(h1+h2*cos(a2)+h3*cos(a2+a3)+h4*cos(a2+a3+a4))/\
py＝sin(a1)*(h1+h2*cos(a2)+h3*cos(a2+a3)+h4*cos(a2+a3+a4))/\
a1＝atn((cos(a1)*(h1+h2*cos(a2)+h3*cos(a2+a3)+h4*cos(a2+a3+a4)))/
(sin(a1)*(h1+h2*cos(a2)+h3*cos(a2+a3)+h4*cos(a2+a3+a4))))＝＝>
a1＝atn(px/py)
其中，px、py是实数，分别代表所述三指机器人灵巧手食指第一关节位置的三维向量的x轴和y轴分量；a1、a2、a3和a4是实数，代表所述三指机器人灵巧手运动中食指第一至第四关节的旋转角度θ1、θ2、θ3和θ4；h1、h2、h3和h4是实数，分别代表所述三指机器人灵巧手食指第二至第四关节角之间的距离；和/或，
在定理证明器中验证所述三指机器人灵巧手食指第三关节的旋转角度θ3，该验证过程如下形式化描述：
Figure BDA0003136479500000141
p q s2s3 s4 a2 a3 a4.
(matrix_exp((a2)％％screw_2_matrix s2)**(homo_point(mk_point p))＝(homo_point(mk_point p))/\matrix_exp(a2％％screw_2_matrix s2)**(matrix_exp(a3％％screw_2_matrix s3)**(homo_point(mk_point q))-(homo_point(mk_point p)))＝matrix_exp(a3％％screw_2_matrix s3)**(homo_point(mk_point q))-(homo_point(mk_point p))/\
matrix_exp((a4)％％screw_2_matrix s4)**(homo_point(mk_point q))＝(homo_point(mk_point q)))＝＝>
norm(matrix_exp(a3％％screw_2_matrix s3)**
(homo_point(mk_point q))-(homo_point(mk_point p)))＝
norm(g_1a2 s2 a3 s3 a4 s4**(homo_point(mk_point q))–
(homo_point(mk_point p)))
其中，p、q为向量，分别代表所述三指机器人灵巧手食指第二关节轴上一点和第四关节轴上一点；a2、a3和a4是实数，代表所述三指机器人灵巧手食指第二至第四关节的旋转角度θ2、θ3和θ4；s2、s3、s4分别为所述三指机器人灵巧手食指第二至第四关节的运动旋量
Figure BDA0003136479500000142
和
Figure BDA0003136479500000143
homo_point表示齐次点；和/或，
在定理证明器中验证所述三指机器人灵巧手食指第二关节的旋转角度θ2，该验证过程如下形式化描述：
Figure BDA0003136479500000144
a2a3 a4 s2 s3 s4 p'g2.
g2＝(g_1a2 s2 a3 s3 a4 s4)**matrix_exp(--(a4)％％screw_2_matrix s4)**matrix_exp(--(a3)％％screw_2_matrix s3)＝＝>
matrix_exp(a2％％screw_2_matrix s2)**homo_point(mk_point p')＝g2**homo_point(mk_point p')
其中，p'为向量，代表所述三指机器人灵巧手食指第二关节轴外一点；a2、a3和a4是实数，代表所述三指机器人灵巧手食指第二至第四关节的旋转角度θ2、θ3和θ4；s2、s3、s4分别为所述三指机器人灵巧手食指第二至第四关节的运动旋量
Figure BDA0003136479500000151
和
Figure BDA0003136479500000152
homo_point表示齐次点；screw_2_matrix表示运动旋量的算子。
进一步地，利用形式化描述的所述定理1、定理2、定理3、定理4、定理5、定理6、Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3验证所述三指机器人灵巧手的运动学，包括：
在定理证明器中定义所述三指机器人灵巧手拇指的正向运动学，该定义的形式化描述如下：
ga a1 s1 a2 s2 a3 s3 a4 s4 a5 s5 x＝
matrix_exp(a1％％screw_2_matrix s1)**
matrix_exp(a2％％screw_2_matrix s2)**
matrix_exp(a3％％screw_2_matrix s3)**
matrix_exp(a4％％screw_2_matrix s4)**
matrix_exp(a5％％screw_2_matrix s5)**(gst_initial x)
其中，s1、s2、s3、s4、s5分别为所述三指机器人灵巧手拇指第一至第五关节的运动旋量
Figure BDA0003136479500000153
和
Figure BDA0003136479500000154
θ1、θ2、θ3、θ4和θ5是所述三指机器人灵巧手拇指运动中第一至第五关节的旋转角度；matrix_exp代表以e为底的指数函数；screw_2_matrix表示运动旋量的算子；gst_initial x的输入变量是向量x，输出是齐次矩阵，代表初始位姿gst(0)；
在定理证明器中验证所述三指机器人灵巧手拇指第五关节角度θ5，该验证过程在定理证明器中的形式化描述如下：
Figure BDA0003136479500000155
h2a1 a2 a3 a4 a5 s1 s2 s3 s4 s5 g_d pw q.
((matrix_exp((a1)％％screw_2_matrix s1)**
matrix_exp((a2)％％screw_2_matrix s2))**((homo_point(mk_point pw))-(homo_point(mk_point q)))＝(homo_point(mk_point pw))-(homo_point(mk_pointq))/\matrix_exp((a3)％％screw_2_matrix s3)**matrix_exp((a4)％％screw_2_matrixs4)**(homo_point(mk_point pw))＝(homo_point(mk_point pw))/\matrix_exp((a1)％％screw_2_matrix s1)**matrix_exp((a2)％％screw_2_matrix s2)**(homo_point(mk_point q))＝(homo_point(mk_point q))/\g_d＝ga a1 s1 a2 s2 a3 s3 a4 s4a5 s5 x)＝＝>norm((homo_point(mk_point pw))-(homo_point(mk_point q)))＝norm(g_d**(matrix_inv(gst_initial x))**matrix_exp(--a5％％screw_2_matrix s5)**(homo_point(mk_point pw))-(homo_point(mk_point q)))
其中，pw和q为向量，代表所述三指机器人灵巧手拇指第三关节轴线与第四关节轴线的交点和第一关节轴线与第二关节轴线的交点；h2为实数；s1、s2、s3、s4、s5分别为所述三指机器人灵巧手拇指第一至第五关节的运动旋量
Figure BDA0003136479500000161
和
Figure BDA0003136479500000162
θ1、θ2、θ3、θ4和θ5是所述三指机器人灵巧手运动中拇指第一至第五关节的旋转角度；matrix_exp代表以e为底的指数函数；screw_2_matrix表示运动旋量的算子；homo_point表示齐次点；g_d被定义为所述三指机器人灵巧手拇指从第一个关节到末端执行器的刚体变换的运动过程；gst_initial x的输入变量是向量x，输出是齐次矩阵，代表所述三指机器人灵巧手拇指的初始位姿gst(0)；和/或，
在定理证明器中验证所述三指机器人灵巧手拇指第一和第二关节角度θ1和θ2，该验证过程在定理证明器中的形式化描述如下：
Figure BDA0003136479500000163
s1s2 s3 s4 s5 a1 a2 a3 a4 a5 pw x g_d.
(g_d＝ga a1 s1 a2 s2 a3 s3 a4 s4 a5 s5 x/\
matrix_exp((a3)％％screw_2_matrix s3)**
matrix_exp((a4)％％screw_2_matrix s4)**
(homo_point(mk_point pw))＝(homo_point(mk_point pw))/\
ga_2＝g_d**(matrix_inv(gst_initial x))**
matrix_exp(--(a5)％％screw_2_matrix s5)**
(homo_point(mk_point pw)))＝＝>
matrix_exp((a1)％％screw_2_matrix s1)**
matrix_exp((a2)％％screw_2_matrix s2)**
(homo_point(mk_point pw))＝ga_2
其中，pw为向量，代表所述三指机器人灵巧手拇指第三关节轴线与第四关节轴线的交点；s1、s2、s3、s4、s5分别为所述三指机器人灵巧手拇指第一至第五关节的运动旋量
Figure BDA0003136479500000171
和
Figure BDA0003136479500000172
θ1、θ2、θ3、θ4和θ5是所述三指机器人灵巧手运动中拇指第一至第五关节的旋转角度；matrix_exp代表以e为底的指数函数；screw_2_matrix表示旋量的算子；homo_point表示齐次点；g_d被定义为所述三指机器人灵巧手拇指从第一个关节到末端执行器的刚体变换的运动过程；gst_initial x的输入变量是向量x，输出是齐次矩阵，代表初始位姿gst(0)；和/或，
在定理证明器中验证所述三指机器人灵巧手拇指第三和第四关节角度θ3和θ4，该验证过程在定理证明器中的形式化描述如下：
Figure BDA0003136479500000173
s1 s2 s3 s4 s5 a1 a2 a3 a4 a5 p q x g_d.
(g_d＝ga a1 s1 a2 s2 a3 s3 a4 s4 a5 s5 x/\
q＝matrix_exp(--(a2)％％screw_2_matrix s2)**
matrix_exp(--(a1)％％screw_2_matrix s1)**g_d**
(matrix_inv(gst_initial x))**matrix_exp(--(a5)％％screw_2_matrixs5)**(homo_point(mk_point p)))＝＝>
matrix_exp(a3％％screw_2_matrix s3)**
matrix_exp(a4)％％screw_2_matrix s4)**(homo_point(mk_point p))＝q
其中，p、q为向量，代表所述三指机器人灵巧手拇指第三关节轴线与第四关节轴线外一点和第一关节轴线与第二关节轴线的交点；s1、s2、s3、s4、s5分别为所述三指机器人灵巧手拇指第一至第五关节的运动旋量
Figure BDA0003136479500000174
和
Figure BDA0003136479500000175
θ1、θ2、θ3、θ4和θ5是所述三指机器人灵巧手运动中拇指第一至第五关节的旋转角度；matrix_exp代表以e为底的指数函数；screw_2_matrix表示运动旋量的算子；homo_point表示齐次点；g_d被定义为所述三指机器人灵巧手拇指从第一个关节到末端执行器的刚体变换的运动过程；gst_initial x的输入变量是向量x，输出是齐次矩阵，代表初始位姿gst(0)。
第二方面，本公开实施例提供了一种电子设备，包括存储器和处理器；其中，所述存储器用于存储一条或多条计算机指令，其中，所述一条或多条计算机指令被所述处理器执行以实现第一方面所述的方法。
第三方面，本公开实施例提供了一种计算机可读存储介质，用于存储企业账户的安全认证装置所用的计算机指令，其包含用于执行上述第一方面所述方法所涉及的计算机指令。
本公开实施例提供的技术方案可以包括以下有益效果：
本公开实施例通过在定理证明器中将三指机器人灵巧手的运动学进行建模，进而再定理证明器中验证相关的定义以及定理，从而利用经过证明的定义以及定理验证三指机器人灵巧手的运动学求解问题，能够实现对三指机器人灵巧手的完备性验证手段，并且能够满足机器人***对安全性的要求。
应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本公开。
附图说明
结合附图，通过以下非限制性实施方式的详细描述，本公开的其它特征、目的和优点将变得更加明显。在附图中：
图1示出根据本公开一实施方式的三指机器人灵巧手逆运动学的形式化验证方法的流程图；
图2示出根据本公开一实施方式中Paden-Kahan子问题1的示意图；
图3示出根据本公开一实施方式中Paden-Kahan子问题1的投影示意图；
图4示出根据本公开一实施方式中Paden-Kahan子问题2的示意图；
图5示出根据本公开一实施方式中Paden-Kahan子问题3的示意图；
图6示出根据本公开一实施方式中Paden-Kahan子问题3的投影示意图；
图7示出根据本公开一实施方式中三指机器人灵巧手的结构示意图；
图8示出根据本公开一实施方式中三指机器人灵巧手食指的运动学模型示意图；
图9示出根据本公开一实施方式中三指机器人灵巧手拇指的运动学模型示意图；
图10是适于用来实现根据本公开一实施方式的三指机器人灵巧手逆运动学的形式化验证方法的电子设备的结构示意图。
具体实施方式
下文中，将参考附图详细描述本公开的示例性实施方式，以使本领域技术人员可容易地实现它们。此外，为了清楚起见，在附图中省略了与描述示例性实施方式无关的部分。
在本公开中，应理解，诸如“包括”或“具有”等的术语旨在指示本说明书中所公开的特征、数字、步骤、行为、部件、部分或其组合的存在，并且不欲排除一个或多个其他特征、数字、步骤、行为、部件、部分或其组合存在或被添加的可能性。
另外还需要说明的是，在不冲突的情况下，本公开中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本公开。
图1示出根据本公开一实施方式的三指机器人灵巧手逆运动学的形式化验证方法的流程图。如图1所示，所述三指机器人灵巧手逆运动学的形式化验证方法包括以下步骤：
在步骤S101中，形式化描述旋量理论的内容；其中，形式化描述的旋量理论内容至少包括定义旋量的算子、齐次矩阵以及所述齐次矩阵演变的通用矩阵；
在步骤S102中，将形式化描述的旋量理论的内容输入至定理证明器中进行证明，获得定理1、定理2和定理3；其中，所述定理1为运动旋量的矩阵表达式，定理2为三指机器人灵巧手仅平移时运动旋量的指数表达式；定理3为三指机器人灵巧手即平移又旋转时的运动旋量的指数表达式；
在步骤S103中，形式化描述Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3；
在步骤S104中，将形式化描述的Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3输入至所述定理证明器中进行证明，获得定理4、定理5和定理6；其中，所述定理4、定理5和定理6分别为Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3的解；
在步骤S105中，利用形式化描述的所述定理1、定理2、定理3、定理4、定理5、定理6、Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3验证所述三指机器人灵巧手的运动学求解问题。
本实施例中，为了验证三指机器人灵巧手逆运动学，首先对于机器人旋量理论的内容进行形式化描述，具体包括定义旋量的算子、其次矩阵以及齐次矩阵演变的通用矩阵。
定义1：运动旋量的算子
运动旋量即李代数元素来描述螺旋运动的无穷小量，表示如下：
Figure BDA0003136479500000201
ξ＝(ω；υ)
式中，ξ是运动旋量
Figure BDA0003136479500000202
的坐标表示，υ是平移线速度，ω是旋转角速度，
Figure BDA0003136479500000203
是对应于ω的反对称矩阵。
其中，旋量的算子用于表示将坐标表示ξ变换成运动旋量
Figure BDA0003136479500000204
的映射关系，定理证明器中的形式化描述如下：
val screw_2_matrix s＝lambda i j.if i<＝3/\j<＝3then(vec3_2_ssm(FSTs))$i$j else if i<＝3/\j＝4then(SND s)$i else&0
定理证明器中，运动旋量用s表示，FST s表示旋转角速度ω，为旋量s的第一个分量；SND s表示平移线速度υ，为旋量s的第二个分量；函数screw_2_matrix的输入变量是运动旋量s，i，j表示矩阵中元素的标号，i表示行，j表示列，输出的是3+1维矩阵。vec3_2_ssm表示反对称矩阵转化函数。
上述定义1形式化描述了公式(1)，
Figure BDA0003136479500000205
是3×3矩阵，在i≤3，j≤3时，旋量s中的元素为
Figure BDA0003136479500000206
j＝4时为υ，其他元素为0。
定义2：齐次矩阵
为了描述空间刚体的姿态，假设{S}和{T}分别为惯性坐标系和工具坐标系，其中{T}固连于刚体上，则刚体相对于惯性坐标系{S}的位姿映射集合可由工具坐标系{T}相对于惯性坐标系{S}的映射集合来表示：
Figure BDA0003136479500000207
式中，SE(3)为特殊欧式群，即李群，R是3×3的旋转矩阵，P是位置矢量，SO(3)是特殊正交群。需要说明的是，本公开实施例中，刚体指三指机器人灵巧手、三指机器人灵巧手食指、三指机器人灵巧手拇指中的其中一个或多个。
其中，齐次矩阵T∈SE(3)，满足李群的性质，定理证明器中的形式化描述如下：
val homo_trans x R＝lambda i j.
if i＝(dimindex(:N)+1)/\∧～(j＝dimindex(:N)+1)then&0
else if(i＝dimindex(:N)+1)/\∧(j＝dimindex(:N)+1)then&1
else if～(i＝dimindex(:N)+1)/\(j＝dimindex(:N)+1)then x$i
else R$i$j
定理证明器中，homo_trans的输入是N维向量x和N维旋转矩阵R的变量，输出是N+1维的矩阵；dimindex在定理证明器HOL-Light中表示的是矩阵的维度；x$i表示向量x的第i个分量；R$i$j表示旋转矩阵R中第i行第j列的元素；符号&表示将自然数类型转化为实数类型。
上述定义2形式化描述了公式(2)，齐次矩阵T为
Figure BDA0003136479500000211
即在i＝N+1时，齐次矩阵T中的元素为0，在i＝N+1、j＝N+1时，齐次矩阵T中的元素为1，i≠N+1、j＝N+1时，齐次矩阵T中的元素为x，其他元素为R；
定义3：齐次矩阵演变的通用矩阵在定理证明器中的形式化描述如下：
val homo_trans_tangent x R＝lambda i j.
if i＝(dimindex(:N)+1)then&0
else if～(i＝dimindex(:N)+1)/\(j＝dimindex(:N)+1)then x$i
else R$i$j
定理证明器中，homo_trans_tangent的输入变量是N维向量x和N维旋转矩阵R，输出是N+1维的矩阵；
通用矩阵为
Figure BDA0003136479500000212
即在i＝N+1时，通用矩阵中的元素为0，在i≠N+1、j＝N+1时，通用矩阵中的元素为x，其他元素为R；与定义2不同的是，定义3中在i＝N+1、j＝N+1时，通用矩阵中的元素为0而非1。
在定理证明器中形式化描述旋量理论的内容后，在定理证明器中对旋量理论的内容进行证明。
定理1：运动旋量的矩阵表达式在定理证明器中的形式化描述如下：
Figure BDA0003136479500000221
screw_2_matrix s＝homo_trans_tangent(SND s)(vec3_2_ssm(FST s))
上述命题表示了运动旋量s可以转换成矩阵表达式。证明时，需要把定义1和定义2写进去，然后利用定理证明器库里已有的定理，比如：CART_EQ、LAMBDA_BETA等去证明等式两边是相等的关系。
基于旋量理论，刚体运动可通过运动旋量的指数形式表示为：
Figure BDA0003136479500000222
上式表明：刚体运动的指数形式可以有两种情况，一种是转轴ω为0，另一种是转轴ω不为0的情况，分别在定理证明器中对上述两种情况进行形式化描述，如下：
定理2：刚体仅平移时运动旋量的指数表达式
定理证明器中的形式化描述如下：
Figure BDA0003136479500000223
a.FST s＝vec 0＝＝>matrix_exp(a％％screw_2_matrix s)＝homo_trans(a％(SND s))(mat 1)
其中，a是实数，表示旋转角度θ，取值范围为[0，2π)；函数matrix_exp表示矩阵域上的指数函数，输入变量a％％screw_2_matrix s为3+1阶矩阵，matrix_exp的输出结果为该矩阵的指数映射，类型为3阶矩阵；mat是HOL Light中定义的高阶逻辑函数，函数的输入变量为自然数，返回类型为n阶实矩阵，mat的返回值是斜对角为该输入变量对应实数的矩阵，用mat 1可表示单位矩阵；符号＝＝>代表推出；符号“％”表示向量的标量乘符号；符号“％％”表示矩阵的标量乘符号。该命题成立的条件是旋量s的第一个分量FST s为0，相当于ω＝0，那么此时刚体运动没有进行旋转，仅进行平移。证明定理2时，需要把定义3事先写进去，然后利用定理证明器库里已有的定理证明定理2的等式两边是相等的关系。
定理3：刚体即平移又旋转时的运动旋量的指数表达式
定理证明器中的形式化描述如下：
Figure BDA0003136479500000231
a.norm(FST s)＝&1＝＝>matrix_exp(a％％screw_2_matrix s)＝homo_trans((mat 1-matrix_exp(a％％vec3_2_ssm(FST s)))**((FST s)cross(SND s))+a％(vec3_vtrans(FST s)**(SND s)))(matrix_exp(a％％vec3_2_ssm(FST s)))
其中，vec3_vtrans表示矩阵的转置；norm是HOL Light中定义的一个高阶逻辑函数，函数的输入变量是N维向量，返回类型是实数，输出结果是N维向量的范数，即该向量的长度值；符号&表示将自然数类型转化为实数类型；符号“**”表示乘向量的矢量乘符号。该命题成立的条件是旋量s的第一个分量FST s不为0，相当于ω≠0时，刚体的运动包含了平移和旋转两种运动。
Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3分别描述了机器人旋转角的求解过程，可以在定理证明器中形式化描述上述子问题。
图2示出根据本公开一实施方式中Paden-Kahan子问题1的示意图。如图2所示，Paden-Kahan子问题1描述的是一点p绕定轴ξ旋转一角度θ后至q点，求解角度θ的问题。
形式化描述Paden-Kahan子问题1，并输入定理证明器中证明得到Paden-Kahan子问题1的解作为定理4；
定理证明器中，形式化建模Paden-Kahan子问题1并得到Paden-Kahan子问题1的解的过程为定理4。
首先对Paden-Kahan子问题1进行说明，参见图2，Paden-Kahan子问题1的描述如下：
一点绕定轴ξ旋转一角度θ。设p、q为空间两点，点p绕轴ξ旋转至点q，求转过的角度θ。假定r是轴ξ上的一点，定义u＝(p-r)是r与p间的矢量，v＝(q-r)是r与q间的矢量。
图3示出根据本公开一实施方式中Paden-Kahan子问题1的投影示意图。如图3所示，u'和v'分别是u和v在垂直于旋转轴ξ平面上的投影，则
θ＝arctan(ωT(u′×v′)/u′Tv′)。
定理4：Paden-Kahan子问题1
定理证明器中的形式化描述如下：
Figure BDA0003136479500000241
w r u u′v v′p q s a.
(--(pi/&2)<a∧a<pi/&2)/\s＝(w,r cross w)/\norm w＝&1/\u＝p-r∧v＝q-r/\u′＝u-(vec3_vtrans(FST s)**u)/\v′＝v-(vec3_vtrans(FST s)**v)/\matrix_exp(a％％screw_2_matrix s)**(homo_point(mk_point r))＝(homo_point(mk_point r))/\matrix_exp(a％％screw_2_matrix s)**(homo_point(mk_point p))＝(homo_point(mk_point q))/\～(u′＝vec 0)norm(u′)＝norm(v′)/\w dot u＝w dot v
Figure BDA0003136479500000242
a＝atn((w dot(u’cross v′))/(u′dot v′))
其中，w，r，u，u'，v，v'，p，q是向量，w代表旋转轴，u'，v'分别是向量u，v的投影；s是运动旋量；a是实数，表示旋转角度θ，范围为[-π/2，π/2]；r是转轴上的一点，满足位置不变原则；homo_point表示齐次点；screw_2_matrix表示旋量的算子；vec 0表示0向量。dot表示向量的点乘；cross表示向量的叉乘。
上述命题表示了在r满足位置不变原则，以及空间一点q是由空间一点p绕轴旋转角度a得来时，可以通过u'、v'的叉乘和点乘公式来求得旋转角度。该命题成立的条件是u'不为0，如若u'为0，则存在无穷多个点，这时点p等于点r，且两点均在旋转轴上。
根据本公开的实施例，所述形式化描述Paden-Kahan子问题1，并输入定理证明器中证明得到Paden-Kahan子问题1的解作为定理4，包括：
引入子目标
Figure BDA0003136479500000243
并证明；
将所述子目标加入假设列表，利用定理证明器证明得到Paden-Kahan子问题1的解作为定理4。
上述定理4的证明过程是先引入必要的前提条件，然后通过逻辑推导出命题成立。其中，困难在于如何形式化表达出转轴w。在本公开方式中，通过引入子目标
Figure BDA0003136479500000244
证明定理4成立后加入假设列表里直接使用。通过公式中隐含的转轴w来证明定理4的成立，从而避免了直接描述转轴w。
图4示出根据本公开一实施方式中Paden-Kahan子问题2的示意图。如图4所示，Paden-Kahan子问题2描述的是将点p绕给定轴s2旋转角度a2，再绕轴s1旋转角度a1到点q重合，求解绕上述两个轴旋转的角度的问题。
定理5：Paden-Kahan子问题2
定理证明器中的形式化描述如下：
Figure BDA0003136479500000251
w1w2 r u u'v v'p q c z a a1 a2 x1 x2 x3 s1 s2.
(--(pi/&2)<a/\a<pi/&2)/\
(--(pi/&2)<a1/\a1<pi/&2)/\
(--(pi/&2)<a2/\a2<pi/&2)/\
s1＝(w1,r cross(w1))/\s2＝(w2,r cross(w2))/\
norm w1＝&1/\norm w2＝&1/\u＝p-r/\v＝q-r/\z＝c-r/\
u'＝u-(vec3_vtrans(FST s2)**u)/\
v'＝v-(vec3_vtrans(FST s1)**v)/\
z＝x1％w1+x2％w2+x3％(w1 cross w2)/\
(norm z)pow 2＝x1 pow 2+x2 pow 2+(&2*x1*x2)*(w1 dot w2)+(x3 pow 2)*(norm(w1 cross w2))pow 2/\
～((norm(w1 cross w2))pow 2＝&0)/\
～((w1 dot w2)pow 2-&1＝&0)/\
matrix_exp(a1％％screw_2_matrix s1)**(homo_point(mk_point r))＝(homo_point(mk_point r))/\
matrix_exp((--a1)％％screw_2_matrix s1)**(homo_point(mk_point r))＝(homo_point(mk_point r))/\
matrix_exp(a2％％screw_2_matrix s2)**(homo_point(mk_point r))＝(homo_point(mk_point r))/\
matrix_exp(a2％％screw_2_matrix s2)**(homo_point(mk_point p))＝(homo_point(mk_point c))/\
matrix_exp((--a1)％％screw_2_matrix s1)**(homo_point(mk_point q))＝(homo_point(mk_point c))/\
matrix_exp(a1％％screw_2_matrix s1)**matrix_exp(a2％％screw_2_matrixs2)**(homo_point(mk_point p))＝(homo_point(mk_point q))/\
a＝a1+a2/\～(u'＝vec 0)/\～(v'＝vec 0)＝＝>
((&0<＝x3)＝＝>(a＝--atn((w1 dot(v'cross
((((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2+sqrt((norm u pow 2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dotw2)pow 2-&1))*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1 cross w2))-vec3_vtrans w1**(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2+sqrt((norm u pow 2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dotw2)pow 2-&1))*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1 cross w2)))))/(v'dot((((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2+sqrt((norm u pow 2-(((w1 dotw2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1 dot w2)*(w1 dotv)-w2 dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1 dot w2)*(w2 dot u)-w1 dotv)/((w1 dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow2-&1))*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1 cross w2))-vec3_vtrans w1**(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2+sqrt((norm u pow 2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1 dot w2)*(w1 dot v)-w2dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))*(w1dot w2))/norm(w1 cross w2)pow 2)％(w1 cross w2)))))+atn((w2 dot(u'cross((((w1dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dotv)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2+sqrt((norm u pow 2-x1 pow 2-x2 pow 2-(&2*x1*x2)*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1 cross w2))-vec3_vtransw2**(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dotw2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2+sqrt((norm u pow 2-x1 pow2-x2 pow 2-(&2*x1*x2)*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1 crossw2)))))/(u'dot((((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2+sqrt((norm u pow2-x1 pow 2-x2 pow 2-(&2*x1*x2)*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1cross w2))-vec3_vtrans w2**(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2+sqrt((norm u pow 2-x1 pow 2-x2 pow 2-(&2*x1*x2)*(w1 dot w2))/norm(w1 cross w2)pow2)％(w1 cross w2)))))))/\
((x3<&0)＝＝>(a＝--atn((w1 dot(v'cross((((w1 dot w2)*(w2 dot u)-w1dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dotw2)pow 2-&1)％w2-sqrt((norm u pow 2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1dot w2)pow 2-&1))pow 2-(((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)*((w1 dotw2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))*(w1 dot w2))/norm(w1 crossw2)pow 2)％(w1 cross w2))-vec3_vtrans w1**(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow2-&1)％w2-sqrt((norm u pow 2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))pow2-(&2*((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))*(w1 dot w2))/norm(w1 cross w2)pow2)％(w1 cross w2)))))/(v'dot((((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2-sqrt((norm u pow 2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2dot u)/((w1 dot w2)pow 2-&1))*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1 crossw2))-vec3_vtrans w1**(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2-sqrt((normu pow 2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1 dot w2)*(w2dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1dot w2)pow 2-&1))*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1 cross w2)))))+atn((w2 dot(u'cross((((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2-sqrt((norm u pow2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1 dotw2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1 dot w2)*(w2 dotu)-w1 dot v)/((w1 dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dotw2)pow 2-&1))*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1 cross w2))-vec3_vtrans w2**(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2-sqrt((norm u pow 2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dotw2)pow 2-&1))*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1 cross w2)))))/(u'dot((((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2-sqrt((norm u pow 2-(((w1 dotw2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1 dot w2)*(w1 dotv)-w2 dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1 dot w2)*(w2 dot u)-w1 dotv)/((w1 dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow2-&1))*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1 cross w2))-vec3_vtrans w2**(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2-sqrt((norm u pow 2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1 dot w2)*(w1 dot v)-w2dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))*(w1dot w2))/norm(w1 cross w2)pow 2)％(w1 cross w2)))))))
其中，w1，w2，r，u，u'，v，v'，p，q，c，z是向量，w1和w2代表第一个旋转轴s1和第二个旋转轴s2的单位矢量，u'，v'分别是向量u，v的投影；s1和s2分别是第一个转轴的旋量和第二个转轴的旋量；a、a1和a2是实数，表示旋转角度θ、θ1和θ2，θ表示θ1和θ2之和，范围为[-π/2，π/2]；x1，x2和x3是实数，是自定义的变量；r是转轴上的一点，满足位置不变原则；homo_point表示齐次点；screw_2_matrix表示旋量的算子；vec 0表示0向量。dot表示向量的点乘；cross表示向量的叉乘。
在定理证明器中输入Paden-Kahan子问题2的形式化描述，并进行证明得到，表示的是刚体分别绕两个相交的转轴(s1和s2)旋转一定角度的运动。该问题实质上是将点p绕给定轴s2旋转角度a2，再绕轴s1旋转角度a1到点q重合。
图5示出根据本公开一实施方式中Paden-Kahan子问题3的示意图。如图5所示，Paden-Kahan子问题3描述的是点p绕旋转轴旋转一定的角度后与点q的距离始终为d，求解绕该旋转角度的问题。
形式化描述Paden-Kahan子问题3，并输入定理证明器中证明得到Paden-Kahan子问题3的解作为定理6；
首先对Paden-Kahan子问题3进行说明，参见图5，Paden-Kahan子问题3的描述如下：
一点绕定轴ξ旋转一角度θ。设p、q为空间两点，点p绕轴ξ旋转角度θ，然后移动δ至点q，使得点p与点q的距离始终为d，求转过的角度θ。假定r是轴ξ上的一点，定义u＝(p-r)是r与p间的矢量，v＝(q-r)是r与q间的矢量。
图6示出根据本公开一实施方式中Paden-Kahan子问题3的投影示意图。如图6所示，u'，v'，δ'分别是向量u，v，δ在垂直于旋转轴ξ平面上的投影，θ0是点p、点q以及旋转轴投影平面圆心之间的旋转角度，
Figure BDA0003136479500000301
是点p绕旋转轴ξ旋转角度θ之后点的投影。
定理6：Paden-Kahan子问题3
定理证明器中的形式化描述如下：
Figure BDA0003136479500000302
w r u u′v v′p q s d a a0c d′.
((--(pi/&2)<a0/\a0<pi/&2)/\(--(pi/&2)<a/\a<pi/&2)/\&0<＝(abs c)/\(absc)<＝pi/\s＝(w,r cross w)/\norm w＝&1/\u＝p-r/\v＝q-r/\c＝a0-a/\u′＝u-(vec3_vtrans(FST s)**u)/\v′＝v-(vec3_vtrans(FST s)**v)/\
matrix_exp(a0％％screw_2_matrix s)**(homo_point(mk_point r))＝(homo_point(mk_point r))/\
matrix_exp(a0％％screw_2_matrix s)**(homo_point(mk_point p))＝(homo_point(mk_point q))/\～(u′＝vec 0)/\～(v′＝vec 0))/\(d)pow 2＝(norm(v-matrix_exp(a％％vec3_2_ssm w)**u))pow 2/\(d′)pow 2＝(d)pow 2-(abs(w dot(p-q)))pow 2＝＝>(&0<＝c＝＝>a＝a0-acs(((norm(u′))pow 2+(norm(v′))pow 2-(d′pow2))/(&2*(norm(u′))*(norm(v′)))))/\(c<&0＝＝>a＝a0+acs(((norm(u′))pow 2+(norm(v′))pow2-(d′pow2))/(&2*(norm(u′))*(norm(v′)))))
其中，w，r，u，u'，v，v'，p，q是向量，w代表旋转轴，u'，v'分别是向量u，v的投影；r是转轴上的一点，满足位置不变原则；s是旋量；a和a0是实数，代表旋转的角度，范围为[-π/2，π/2]；d是实数，代表点p绕轴旋转后到点q的距离；d'是d在平面上的投影；c是实数表示a0-a的差；homo_point表示齐次点；screw_2_matrix表示旋量的算子；vec 0表示0向量。dot表示向量的点乘；cross表示向量的叉乘。
在步骤S105中，可以利用上述形式化描述并且在定理证明器中经过验证的定理验证三指机器人灵巧手的运动学。下面详细介绍：
图7示出根据本公开一实施方式中三指机器人灵巧手的结构示意图。如图7所示，三指机器人灵巧手包括手掌、食指、拇指和无名指。其中，拇指包括从手掌至末端的第一关节(位于手掌)、第二关节和第三关节。食指包括从手掌至末端的第一关节(位于手掌)、第二关节、第三关节和第四关节。本公开实施例中，三指机器人灵巧手的运动学可以包括但不限于食指逆运动学和拇指逆运动学。
下面分别针对食指逆运动学和拇指逆运动学分别介绍：
(一)Shadow灵巧手的食指逆运动学
图8示出根据本公开一实施方式中三指机器人灵巧手食指的运动学模型示意图。如图8所示，(x0，y0，z0)表示食指初始坐标系的x、y和z轴上数值，(x4，y4)表示食指末端执行器即第四关节对应的x轴和y轴上数值，θ1、θ2、θ3和θ4分别为食指的第一至第四关节在当前位姿下相对于初始位姿的旋转角度。a1、a2、a3和a4分别为所述三指机器人灵巧手食指各相邻关节之间连杆长度。
根据本公开的实施例，食指正向运动学的指数积公式，即
Figure BDA0003136479500000311
其中，gst(0)为初始位姿，gst(θ)为刚体在初始位姿下经过旋转或平移后的最终位姿。
Figure BDA0003136479500000312
和
Figure BDA0003136479500000313
分别为第一至第四关节的运动旋量；θ1、θ2、θ3和θ4分别为待求解的食指的第一至第四关节在当前位姿下相对于初始位姿的旋转角度。
定义4:食指的正向运动学
定理证明器中的形式化描述如下：
gst_a a1 s1 a2 s2 a3 s3 a4 s4 x＝
matrix_exp(a1％％screw_2_matrix s1)**
matrix_exp(a2％％screw_2_matrix s2)**
matrix_exp(a3％％screw_2_matrix s3)**
matrix_exp(a4％％screw_2_matrix s4)**(gst_initial x)
其中，s1、s2、s3、s4、分别为所述三指机器人灵巧手食指的第一至第四关节的运动旋量
Figure BDA0003136479500000321
和
Figure BDA0003136479500000322
a1、a2、a3和a4分别代表所述三指机器人灵巧手食指的第一至第四关节的旋转角度θ1、θ2、θ3和θ4；matrix_exp代表以e为底的指数函数；screw_2_matrix表示旋量的算子；gst_initial x的输入变量是向量x，x表示初始位姿的三维坐标，输出是齐次矩阵，代表初始位姿gst(0)；gst_a表示三指机器人灵巧手食指经过运动之后的位姿；
定义5：食指从第二关节到末端执行器的刚体变换的运动过程
定理证明器中的形式化描述如下：
g_1a2 s2 a3 s3 a4 s4＝matrix_exp(a2％％screw_2_matrix s2)**
matrix_exp(a3％％screw_2_matrix s3)**
matrix_exp(a4％％screw_2_matrix s4)
其中，s2、s3、s4、分别为第二至第四关节的运动旋量
Figure BDA0003136479500000323
和
Figure BDA0003136479500000324
a2、a3和a4分别代表是机器人运动中第二至第四关节的旋转角度θ2、θ3和θ4；matrix_exp代表以e为底的指数函数；screw_2_matrix表示旋量的算子。定义5描述了食指从第二个关节到末端执行器的刚体变换的运动过程。
经过上述定义4和定义5的形式化描述之后，可以对食指的运动过程进行验证，进而得到定理7-定理11。
定理7：食指从第一个关节到末端执行器的刚体变换的运动过程的代数运算
定理证明器中的形式化描述如下：
Figure BDA0003136479500000325
a1a2 a3 a4 s1 s2 s3 s4 x gd.
gst_a a1 s1 a2 s2 a3 s3 a4 s4 x＝gd＝＝>matrix_exp(--(a1)％％screw_2_matrix s1)**gd**(matrix_inv(gst_initial x))＝(g_1a2 s2 a3 s3 a4 s4)
其中，s1、s2、s3、s4、分别为食指第一至第四关节的运动旋量
Figure BDA0003136479500000331
和
Figure BDA0003136479500000332
a1、a2、a3和a4分别代表是机器人运动中食指的第一至第四关节的旋转角度θ1、θ2、θ3和θ4；matrix_exp代表以e为底的指数函数；screw_2_matrix表示旋量的算子；gd被定义为食指从第一个关节到末端执行器的刚体变换的运动过程；matrix_inv表示矩阵的逆；gst_initialx的输入变量是向量x，输出是齐次矩阵，代表食指的初始位姿gst(0)。
定理8：食指从第二关节到末端执行器的刚体变换的运动过程的代数运算
定理证明器中的形式化描述如下：
Figure BDA0003136479500000333
θ2θ3θ4s2s3 s4.
matrix_exp(θ2％％screw_2_matrix s2)＝(g_1a2 s2 a3 s3 a4 s4)**matrix_exp(--θ4％％screw_2_matrix s4)**matrix_exp(--θ3％％screw_2_matrix s3)
其中，s2、s3、s4、分别为食指第二至第四关节的运动旋量
Figure BDA0003136479500000334
和
Figure BDA0003136479500000335
a2、a3和a4分别代表是机器人运动中食指第二至第四关节的旋转角度θ2、θ3和θ4；matrix_exp代表以e为底的指数函数；screw_2_matrix表示旋量的算子。
根据食指正向运动学的公式，可得
Figure BDA0003136479500000336
其中，ci＝cosθi，si＝sinθi，i＝1，2，3，4，c23＝cos(θ2+θ3)，s23＝sin(θ2+θ3)，c234＝cos(θ2+θ3+θ4)，s234＝sin(θ2+θ3+θ4)。
定理9：验证食指第一关节的旋转角度θ1
定理证明器中的形式化描述如下：
Figure BDA0003136479500000337
a1a2 a3 a4 h1 h2 h3 h4 px py.
px＝cos(a1)*(h1+h2*cos(a2)+h3*cos(a2+a3)+h4*cos(a2+a3+a4))/\
py＝sin(a1)*(h1+h2*cos(a2)+h3*cos(a2+a3)+h4*cos(a2+a3+a4))/\
a1＝atn((cos(a1)*(h1+h2*cos(a2)+h3*cos(a2+a3)+h4*cos(a2+a3+a4)))/
(sin(a1)*(h1+h2*cos(a2)+h3*cos(a2+a3)+h4*cos(a2+a3+a4))))＝＝>
a1＝atn(px/py)
其中，px、py是实数，代表所述为三指机器人灵巧手食指第一关节位置的三维向量的x轴和y轴分量；a1、a2、a3和a4是实数，代表旋转的角度θ1、θ2、θ3和θ4；h1、h2、h3和h4是实数，分别代表食指各个关节角之间的距离。
定理10：验证食指第三关节角度θ3
定理证明器中的形式化描述如下：
Figure BDA0003136479500000341
p q s2s3 s4 a2 a3 a4.
(matrix_exp((a2)％％screw_2_matrix s2)**(homo_point(mk_point p))＝(homo_point(mk_point p))/\matrix_exp(a2％％screw_2_matrix s2)**(matrix_exp(a3％％screw_2_matrix s3)**(homo_point(mk_point q))-(homo_point(mk_point p)))＝matrix_exp(a3％％screw_2_matrix s3)**(homo_point(mk_point q))-(homo_point(mk_point p))/\matrix_exp((a4)％％screw_2_matrix s4)**(homo_point(mk_pointq))＝(homo_point(mk_point q)))＝＝>norm(matrix_exp(a3％％screw_2_matrix s3)**(homo_point(mk_point q))-(homo_point(mk_point p)))＝norm(g_1a2 s2 a3 s3 a4s4**(homo_point(mk_point q))–(homo_point(mk_point p)))
其中，p、q为向量，代表所述三指机器人灵巧手食指第二关节轴一点和第四关节轴上一点；a2、a3和a4是实数，代表食指第二至第四关键旋转的角度θ2、θ3和θ4；s2、s3、s4分别为第二至第四关节的运动旋量
Figure BDA0003136479500000342
和
Figure BDA0003136479500000343
homo_point表示齐次点。定理10验证了θ3满足Paden-Kahan子问题3的求解条件，因此可使用Paden-Kahan子问题3直接求解。
定理11：验证食指第二关节角度θ2
定理证明器中的形式化描述如下：
Figure BDA0003136479500000344
a2a3 a4 s2 s3 s4 p'g2.
g2＝(g_1a2 s2 a3 s3 a4 s4)**matrix_exp(--(a4)％％screw_2_matrix s4)**matrix_exp(--(a3)％％screw_2_matrix s3)＝＝>matrix_exp(a2％％screw_2_matrixs2)**homo_point(mk_point p')＝g2**homo_point(mk_point p')
其中，p'为向量，代表所述三指机器人灵巧手食指第二关节轴外一点；a2、a3和a4是实数，代表旋转的角度θ2、θ3和θ4；s2、s3、s4分别为第二至第四关节的运动旋量
Figure BDA0003136479500000351
和
Figure BDA0003136479500000352
homo_point表示齐次点；screw_2_matrix表示旋量的算子。定理11验证了θ2满足Paden-Kahan子问题1的求解条件，因此可使用Paden-Kahan子问题1直接求解。
(二)拇指逆运动学
图9示出根据本公开一实施方式中三指机器人灵巧手拇指的运动学模型示意图。如图9所示，(x0，y0，z0)表示拇指初始坐标系的x、y和z轴上数值，(x5，y5)表示拇指末端执行器即第五关节对应的x轴和y轴上数值，θ1、θ2、θ3、θ4和θ5分别为拇指的第一至第五关节在当前位姿下相对于初始位姿的旋转角度。a1、a2、a3和a4分别为所述三指机器人灵巧手拇指各个相邻关节间的连杆长度。
根据本公开的实施例，拇指正向运动学的指数积公式，即
Figure BDA0003136479500000353
其中，gst(0)为初始位姿，gst(θ)为刚体在初始位姿下经过旋转或平移后的最终位姿。
Figure BDA0003136479500000354
和
Figure BDA0003136479500000355
分别为第一至第五关节的运动旋量；θ1、θ2、θ3、θ4和θ5分别为待求解的食指的第一至第五关节在当前位姿下相对于初始位姿的旋转角度。
定义6:拇指的正向运动学
ga a1 s1 a2 s2 a3 s3 a4 s4 a5 s5 x＝
matrix_exp(a1％％screw_2_matrix s1)**
matrix_exp(a2％％screw_2_matrix s2)**
matrix_exp(a3％％screw_2_matrix s3)**
matrix_exp(a4％％screw_2_matrix s4)**
matrix_exp(a5％％screw_2_matrix s5)**(gst_initial x)
其中，s1、s2、s3、s4、s5分别为拇指第一至第五关节的运动旋量
Figure BDA0003136479500000361
和
Figure BDA0003136479500000362
θ1、θ2、θ3、θ4和θ5是机器人运动中各个关节的旋转角度；matrix_exp代表以e为底的指数函数；screw_2_matrix表示旋量的算子；gst_initial x的输入变量是向量x，输出是齐次矩阵，代表初始位姿gst(0)。
经过上述定义6的形式化描述之后，可以对拇指的运动过程在定理证明器中进行证明，获得如下定理12-定理14：
定理12：验证拇指第五关节角度θ5
定理证明器中的形式化描述如下：
Figure BDA0003136479500000363
h2a1 a2 a3 a4 a5 s1 s2 s3 s4 s5 g_d pw q.
((matrix_exp((a1)％％screw_2_matrix s1)**
matrix_exp((a2)％％screw_2_matrix s2))**((homo_point(mk_point pw))-(homo_point(mk_point q)))＝(homo_point(mk_point pw))-(homo_point(mk_pointq))/\matrix_exp((a3)％％screw_2_matrix s3)**matrix_exp((a4)％％screw_2_matrixs4)**(homo_point(mk_point pw))＝(homo_point(mk_point pw))/\matrix_exp((a1)％％screw_2_matrix s1)**matrix_exp((a2)％％screw_2_matrix s2)**(homo_point(mk_point q))＝(homo_point(mk_point q))/\g_d＝ga a1 s1 a2 s2 a3 s3 a4 s4a5 s5 x)＝＝>norm((homo_point(mk_point pw))-(homo_point(mk_point q)))＝norm(g_d**(matrix_inv(gst_initial x))**matrix_exp(--a5％％screw_2_matrix s5)**(homo_point(mk_point pw))-(homo_point(mk_point q)))
其中，pw和q为向量，代表所述三指机器人灵巧手拇指第三关节轴线与第四关节轴线的交点和第一关节轴线与第二关节轴线的交点；h2为实数；s1、s2、s3、s4、s5分别为第一至第五关节的运动旋量
Figure BDA0003136479500000364
和
Figure BDA0003136479500000365
θ1、θ2、θ3、θ4和θ5是机器人运动中拇指第一至第五关节的旋转角度；matrix_exp代表以e为底的指数函数；screw_2_matrix表示旋量的算子；homo_point表示齐次点；g_d被定义为拇指从第一个关节到末端执行器的刚体变换的运动过程；gst_initial x的输入变量是向量x，输出是齐次矩阵，代表拇指的初始位姿gst(0)。
定理13：验证拇指第一和第二关节角度θ1和θ2
定理证明器中的形式化描述如下：
Figure BDA0003136479500000371
s1s2 s3 s4 s5 a1 a2 a3 a4 a5 pw x g_d.
(g_d＝ga a1 s1 a2 s2 a3 s3 a4 s4 a5 s5 x/\
matrix_exp((a3)％％screw_2_matrix s3)**
matrix_exp((a4)％％screw_2_matrix s4)**
(homo_point(mk_point pw))＝(homo_point(mk_point pw))/\
ga_2＝g_d**(matrix_inv(gst_initial x))**
matrix_exp(--(a5)％％screw_2_matrix s5)**
(homo_point(mk_point pw)))＝＝>
matrix_exp((a1)％％screw_2_matrix s1)**
matrix_exp((a2)％％screw_2_matrix s2)**
(homo_point(mk_point pw))＝ga_2
其中，pw为向量，代表所述三指机器人灵巧手拇指第三关节轴线与第四关节轴线的交点；s1、s2、s3、s4、s5分别为第一至第五关节的运动旋量
Figure BDA0003136479500000372
和
Figure BDA0003136479500000373
θ1、θ2、θ3、θ4和θ5是机器人运动中各个关节的旋转角度；matrix_exp代表以e为底的指数函数；screw_2_matrix表示旋量的算子；homo_point表示齐次点；g_d被定义为所述三指机器人灵巧手拇指从第一个关节到末端执行器的刚体变换的运动过程；gst_initial x的输入变量是向量x，输出是齐次矩阵，代表初始位姿gst(0)。定理13验证了θ1和θ2满足Paden-Kahan子问题2的求解条件，因此可使用Paden-Kahan子问题2直接求解。
定理14：验证拇指第三和第四关节角度θ3和θ4
定理证明器中的形式化描述如下：
Figure BDA0003136479500000374
s1s2 s3 s4 s5 a1 a2 a3 a4 a5 p q x g_d.
(g_d＝ga a1 s1 a2 s2 a3 s3 a4 s4 a5 s5 x/\
q＝matrix_exp(--(a2)％％screw_2_matrix s2)**
matrix_exp(--(a1)％％screw_2_matrix s1)**g_d**
(matrix_inv(gst_initial x))**matrix_exp(--(a5)％％screw_2_matrixs5)**(homo_point(mk_point p)))＝＝>
matrix_exp(a3％％screw_2_matrix s3)**
matrix_exp(a4)％％screw_2_matrix s4)**(homo_point(mk_point p))＝q
其中，p、q为向量，代表所述三指机器人灵巧手拇指第三关节轴线与第四关节轴线外一点和第一关节轴线与第二关节轴线的交点；s1、s2、s3、s4、s5分别为第一至第五关节的运动旋量
Figure BDA0003136479500000381
和
Figure BDA0003136479500000382
θ1、θ2、θ3、θ4和θ5是机器人运动中拇指第一至第五关节的旋转角度；matrix_exp代表以e为底的指数函数；screw_2_matrix表示旋量的算子；homo_point表示齐次点；g_d被定义为拇指从第一个关节到末端执行器的刚体变换的运动过程；gst_initial x的输入变量是向量x，输出是齐次矩阵，代表初始位姿gst(0)。定理20验证了θ3和θ4满足Paden-Kahan子问题2的求解条件，因此可使用Paden-Kahan子问题2直接求解。
下述为本公开装置实施例，可以用于执行本公开方法实施例。
根据本公开一实施方式的三指机器人灵巧手逆运动学的形式化验证装置的结构框图，该装置可以通过软件、硬件或者两者的结合实现成为电子设备的部分或者全部。所述三指机器人灵巧手逆运动学的形式化验证装置包括：
第一形式化模块，被配置为形式化描述旋量理论的内容；其中，形式化描述的旋量理论内容至少包括定义旋量的算子、齐次矩阵以及所述齐次矩阵演变的通用矩阵；
第二形式化模块，被配置为将形式化描述的旋量理论的内容输入至定理证明器中进行证明，获得定理1、定理2和定理3；其中，所述定理1为运动旋量的矩阵表达式，定理2为三指机器人灵巧手仅平移时运动旋量的指数表达式；定理3为三指机器人灵巧手即平移又旋转时的运动旋量的指数表达式；
第三形式化模块，被配置为形式化描述Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3；
证明模块，被配置为将形式化描述的Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3输入至所述定理证明器中进行证明，获得定理4、定理5和定理6；其中，所述定理4、定理5和定理6分别为Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3的解；
验证模块，被配置为利用形式化描述的所述定理1、定理2、定理3、定理4、定理5、定理6、Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3验证所述三指机器人灵巧手的运动学求解问题。
图10是适于用来实现根据本公开实施方式的三指机器人灵巧手逆运动学的形式化验证方法的电子设备的结构示意图。
如图10所示，电子设备1000包括中央处理单元(CPU)1001，其可以根据存储在只读存储器(ROM)1002中的程序或者从存储部分1008加载到随机访问存储器(RAM)1003中的程序而执行本公开上述方法的实施方式中的各种处理。在RAM1003中，还存储有电子设备1000操作所需的各种程序和数据。CPU1001、ROM1002以及RAM1003通过总线1004彼此相连。输入/输出(I/O)接口1005也连接至总线1004。
以下部件连接至I/O接口1005：包括键盘、鼠标等的输入部分1006；包括诸如阴极射线管(CRT)、液晶显示器(LCD)等以及扬声器等的输出部分1007；包括硬盘等的存储部分1008；以及包括诸如LAN卡、调制解调器等的网络接口卡的通信部分1009。通信部分1009经由诸如因特网的网络执行通信处理。驱动器1010也根据需要连接至I/O接口1005。可拆卸介质1011，诸如磁盘、光盘、磁光盘、半导体存储器等等，根据需要安装在驱动器1010上，以便于从其上读出的计算机程序根据需要被安装入存储部分1008。
特别地，根据本公开的实施方式，上文参考本公开实施方式中的方法可以被实现为计算机软件程序。例如，本公开的实施方式包括一种计算机程序产品，其包括有形地包含在机器可读介质上的计算机程序，所述计算机程序包含用于执行本公开实施方式中方法的程序代码。在这样的实施方式中，该计算机程序可以通过通信部分1009从网络上被下载和安装，和/或从可拆卸介质1011被安装。
附图中的流程图和框图，图示了按照本公开各种实施方式的***、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，路程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的***来实现，或者可以用专用硬件与计算机指令的组合来实现。
描述于本公开实施方式中所涉及到的单元或模块可以通过软件的方式实现，也可以通过硬件的方式来实现。所描述的单元或模块也可以设置在处理器中，这些单元或模块的名称在某种情况下并不构成对该单元或模块本身的限定。
作为另一方面，本公开还提供了一种计算机可读存储介质，该计算机可读存储介质可以是上述实施方式中所述装置中所包含的计算机可读存储介质；也可以是单独存在，未装配入设备中的计算机可读存储介质。计算机可读存储介质存储有一个或者一个以上程序，所述程序被一个或者一个以上的处理器用来执行描述于本公开的方法。
以上描述仅为本公开的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解，本公开中所涉及的发明范围，并不限于上述技术特征的特定组合而成的技术方案，同时也应涵盖在不脱离所述发明构思的情况下，由上述技术特征或其等同特征进行任意组合而形成的其它技术方案。例如上述特征与本公开中公开的(但不限于)具有类似功能的技术特征进行互相替换而形成的技术方案。

Claims (10)

1.一种三指机器人灵巧手逆运动学的形式化验证方法，其特征在于，包括：
形式化描述旋量理论的内容；其中，形式化描述的旋量理论内容至少包括定义旋量的算子、齐次矩阵以及所述齐次矩阵演变的通用矩阵；
将形式化描述的旋量理论的内容输入至定理证明器中进行证明，获得定理1、定理2和定理3；其中，所述定理1为运动旋量的矩阵表达式，定理2为三指机器人灵巧手仅平移时运动旋量的指数表达式；定理3为三指机器人灵巧手即平移又旋转时的运动旋量的指数表达式；
形式化描述Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3；
将形式化描述的Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3输入至所述定理证明器中进行证明，获得定理4、定理5和定理6；其中，所述定理4、定理5和定理6分别为Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3的解；
利用形式化描述的所述定理1、定理2、定理3、定理4、定理5、定理6、Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3验证所述三指机器人灵巧手的运动学求解问题。
2.根据权利要求1所述的方法，其特征在于，形式化描述旋量理论的内容，包括：
将运动旋量的算子、齐次矩阵以及齐次矩阵演变的通用矩阵形式化描述为定义1、定义2和定义3；其中，
定义1：运动旋量的算子：
运动旋量的数学公式如下表示：
Figure FDA0003136479490000011
ξ＝(ω；υ)
式中，ξ是运动旋量
Figure FDA0003136479490000012
的坐标表示，υ是平移线速度，ω是旋转角速度，
Figure FDA0003136479490000013
是对应于ω的反对称矩阵；
其中，运动旋量的算子用于表示将坐标表示ξ变换成运动旋量
Figure FDA0003136479490000014
的映射关系，定理证明器中公式(1)的形式化描述如下：
val screw_2_matrix s＝lambda i j.if i<＝3∧j<＝3then(vec3_2_ssm(FST s))$i$j else if i<＝3∧j＝4then(SND s)$i else&0；
定理证明器中，运动旋量用s表示，FST s表示旋转角速度ω，为运动旋量s的第一个分量；SND s表示平移线速度υ，为运动旋量s的第二个分量；函数screw_2_matrix的输入变量是运动旋量s，i，j表示矩阵中元素的标号，i表示行，j表示列，输出的是3+1维矩阵；vec3_2_ssm表示反对称矩阵转化函数；
其中，
Figure FDA0003136479490000021
是3×3矩阵，在i≤3，j≤3时，旋量s中的元素为
Figure FDA0003136479490000022
j＝4时为υ，其他元素为0；
定义2：齐次矩阵
为了三指机器人灵巧手的姿态，假设{S}和{T}分别为惯性坐标系和工具坐标系，其中{T}固连于三指机器人灵巧手上，则三指机器人灵巧手相对于惯性坐标系{S}的位姿映射集合可由工具坐标系{T}相对于惯性坐标系{S}的映射集合如下表示：
Figure FDA0003136479490000023
式中，SE(3)为特殊欧式群，即李群，R是3×3的旋转矩阵，P是位置矢量，SO(3)是特殊正交群；
其中，齐次矩阵T∈SE(3)，满足李群的性质，定理证明器中公式(2)的形式化描述如下：
val homo_trans x R＝lambda i j.
if i＝(dimindex(:N)+1)∧∧～(j＝dimindex(:N)+1)then&0 else if(i＝dimindex(:N)+1)∧(j＝dimindex(:N)+1)then&1 else if～(i＝dimindex(:N)+1)∧(j＝dimindex(:N)+1)then x$i else R$i$j；
定理证明器中，homo_trans的输入是N维向量x和N维旋转矩阵R的变量，输出是N+1维的矩阵；dimindex在定理证明器中表示的是矩阵的维度；x$i表示向量x的第i个分量；R$i$j表示旋转矩阵R中第i行第j列的元素；符号&表示将自然数类型转化为实数类型；
齐次矩阵T为
Figure FDA0003136479490000031
即在i＝N+1时，齐次矩阵T中的元素为0，在i＝N+1、j＝N+1时，齐次矩阵T中的元素为1，i≠N+1、j＝N+1时，齐次矩阵T中的元素为x，其他元素为R；
定义3：齐次矩阵演变的通用矩阵在定理证明器中的形式化描述如下：
val homo_trans_tangent x R＝lambda i j.
if i＝(dimindex(:N)+1)then&0 else if～(i＝dimindex(:N)+1)∧(j＝dimindex(:N)+1)then x$i else R$i$j
定理证明器中，homo_trans_tangent的输入变量是N维向量x和N维旋转矩阵R，输出是N+1维的矩阵；
通用矩阵为
Figure FDA0003136479490000032
即在i＝N+1时，通用矩阵中的元素为0，在i≠N+1、j＝N+1时，通用矩阵中的元素为x，其他元素为R。
3.根据权利要求1或2所述的方法，其特征在于，将形式化描述的旋量理论的内容输入至定理证明器中进行证明，获得定理1、定理2和定理3，包括：
通过在定理证明器中基于定义1和定义3证明运动旋量的数学形式和齐次矩阵演变的通用矩阵的数学形式相同得到定理1；
将三指机器人灵巧手仅平移时运动旋量的指数表达式在定理证明器中形式化描述如下：
Figure FDA0003136479490000033
a.FST s＝vec 0＝＝>matrix_exp(a％％screw_2_matrix s)＝homo_trans(a％(SND s))(mat 1)
其中，a是实数，表示旋转角度θ，取值范围为[0，2π)；函数matrix_exp表示矩阵域上的指数函数matrix_exp，函数matrix_exp的输入变量a％％screw_2_matrix s为3+1阶矩阵，函数matrix_exp的输出结果为该矩阵的指数映射，类型为3阶矩阵；mat是定理证明器中定义的高阶逻辑函数，该高阶逻辑函数的输入变量为自然数，返回类型为n阶实矩阵，mat的返回值是斜对角为该输入变量对应实数的矩阵，用mat 1可表示单位矩阵；符号＝＝>代表推出；符号“％”表示向量的标量乘符号；符号“％％”表示矩阵的标量乘符号；
基于定义3的形式化描述，在定理证明器中证明上述形式化描述的等式两边相等的关系的过程以获得定理2；
将三指机器人灵巧手即平移又旋转时运动旋量的指数表达式在定理证明器中形式化描述如下：
Figure FDA0003136479490000041
a.norm(FST s)＝&1＝＝>matrix_exp(a％％screw_2_matrix s)＝homo_trans((mat 1-matrix_exp(a％％vec3_2_ssm(FST s)))**((FST s)cross(SND s))+a％(vec3_vtrans(FST s)**(SND s)))(matrix_exp(a％％vec3_2_ssm(FST s)))
其中，vec3_vtrans表示矩阵的转置；norm是定理证明器中定义的一个高阶逻辑函数，该高阶逻辑函数的输入变量是N维向量，返回类型是实数，输出结果是N维向量的范数，即该向量的长度值；符号&表示将自然数类型转化为实数类型；符号“**”表示乘向量的矢量乘符号；
基于定义3的形式化描述，在定理证明器中证明上述形式化描述的等式两边相等的关系的过程以获得定理3。
4.根据权利要求1或2所述的方法，其特征在于，将形式化描述的Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3输入至所述定理证明器中进行证明，获得定理4、定理5和定理6，包括：
定理证明器中形式化描述Paden-Kahan子问题1，并证明Paden-Kahan子问题1的解的以获得定理4；其中，Paden-Kahan子问题1在定理证明器中如下形式化描述：
Figure FDA0003136479490000042
w r u u′v v′p q s a.
(--(pi/&2)<a∧a<pi/&2)∧s＝(w,r cross w)∧norm w＝&1∧u＝p-r∧v＝q-r∧u′＝u-(vec3_vtrans(FST s)**u)∧v′＝v-(vec3_vtrans(FST s)**v)∧matrix_exp(a％％screw_2_matrix s)**(homo_point(mk_point r))＝(homo_point(mk_point r))
∧matrix_exp(a％％screw_2_matrix s)**(homo_point(mk_point p))＝(homo_point(mk_point q))∧～(u′＝vec 0)
Figure FDA0003136479490000051
其中，w，r，u，u'，v，v'，p，q是向量，w代表旋转轴，u'，v'分别是向量u，v的投影；s是运动旋量；a是实数，表示旋转角度θ，范围为[-π/2，π/2]；r是旋转轴上的一点，满足位置不变原则；homo_point表示齐次点；screw_2_matrix表示旋量的算子；vec 0表示0向量；dot表示向量的点乘；cross表示向量的叉乘；
其中，在定理证明器中输入Paden-Kahan子问题1的形式化描述，并进行证明得到Paden-Kahan子问题1的解的过程包括：
引入子目标
Figure FDA0003136479490000052
并在定理证明器中证明该子目标成立；
将证明成立后的所述子目标加入假设列表，利用定理证明器证明Paden-Kahan子问题1的解以获得定理4。
5.根据权利要求1或2所述的方法，其特征在于，将形式化描述的Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3输入至所述定理证明器中进行证明，获得定理4、定理5和定理6，包括：
定理证明器中形式化描述Paden-Kahan子问题2，并证明Paden-Kahan子问题1的解以获得定理5；
其中，Paden-Kahan子问题2在定理证明器中如下形式化描述：
Figure FDA0003136479490000053
w1w2 r u u'v v'p q c z a a1 a2 x1 x2 x3 s1 s2.
(--(pi/&2)<a∧a<pi/&2)∧
(--(pi/&2)<a1∧a1<pi/&2)∧
(--(pi/&2)<a2∧a2<pi/&2)∧
s1＝(w1,r cross(w1))∧s2＝(w2,r cross(w2))∧
norm w1＝&1∧norm w2＝&1∧u＝p-r∧v＝q-r∧z＝c-r∧
u'＝u-(vec3_vtrans(FST s2)**u)∧
v'＝v-(vec3_vtrans(FST s1)**v)∧
z＝x1％w1+x2％w2+x3％(w1 cross w2)∧
(norm z)pow 2＝x1 pow 2+x2 pow 2+(&2*x1*x2)*(w1 dot w2)+(x3 pow 2)*(norm(w1 cross w2))pow 2∧
～((norm(w1 cross w2))pow 2＝&0)∧
～((w1 dot w2)pow 2-&1＝&0)∧
matrix_exp(a1％％screw_2_matrix s1)**(homo_point(mk_point r))＝(homo_point(mk_point r))∧
matrix_exp((--a1)％％screw_2_matrix s1)**(homo_point(mk_point r))＝(homo_point(mk_point r))∧
matrix_exp(a2％％screw_2_matrix s2)**(homo_point(mk_point r))＝(homo_point(mk_point r))∧
matrix_exp(a2％％screw_2_matrix s2)**(homo_point(mk_point p))＝(homo_point(mk_point c))∧
matrix_exp((--a1)％％screw_2_matrix s1)**(homo_point(mk_point q))＝(homo_point(mk_point c))∧
matrix_exp(a1％％screw_2_matrix s1)**matrix_exp(a2％％screw_2_matrixs2)**(homo_point(mk_point p))＝(homo_point(mk_point q))∧
a＝a1+a2∧～(u'＝vec 0)∧～(v'＝vec 0)＝＝>
((&0<＝x3)＝＝>(a＝--atn((w1 dot(v'cross((((w1 dot w2)*(w2 dot u)-w1 dotv)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2+sqrt((norm u pow 2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dotw2)pow 2-&1))pow 2-(((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)*((w1 dotw2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))*(w1 dot w2))/norm(w1 crossw2)pow 2)％(w1 cross w2))-vec3_vtrans w1**(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow2-&1)％w2+sqrt((norm u pow 2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))pow2-(&2*((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))*(w1 dot w2))/norm(w1 cross w2)pow2)％(w1 cross w2)))))/(v'dot((((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2+sqrt((norm u pow 2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2dot u)/((w1 dot w2)pow 2-&1))*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1 crossw2))-vec3_vtrans w1**(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2+sqrt((normu pow 2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1 dot w2)*(w2dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1dot w2)pow 2-&1))*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1 cross w2)))))+atn((w2 dot(u'cross((((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2+sqrt((norm u pow2-x1 pow 2-x2 pow 2-(&2*x1*x2)*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1cross w2))-vec3_vtrans w2**(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2+sqrt((norm u pow 2-x1 pow 2-x2 pow 2-(&2*x1*x2)*(w1 dot w2))/norm(w1 cross w2)pow2)％(w1 cross w2)))))/(u'dot((((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2+sqrt((norm u pow 2-x1 pow 2-x2 pow 2-(&2*x1*x2)*(w1 dot w2))/norm(w1 crossw2)pow 2)％(w1 cross w2))-vec3_vtrans w2**(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow2-&1)％w2+sqrt((norm u pow 2-x1 pow 2-x2 pow 2-(&2*x1*x2)*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1 cross w2)))))))∧
((x3<&0)＝＝>(a＝--atn((w1 dot(v'cross((((w1 dot w2)*(w2 dot u)-w1 dotv)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2-sqrt((norm u pow 2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dotw2)pow 2-&1))pow 2-(((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)*((w1 dotw2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))*(w1 dot w2))/norm(w1 crossw2)pow 2)％(w1 cross w2))-vec3_vtrans w1**(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow2-&1)％w2-sqrt((norm u pow 2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))pow2-(&2*((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))*(w1 dot w2))/norm(w1 cross w2)pow2)％(w1 cross w2)))))/(v'dot((((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2-sqrt((norm u pow 2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2dot u)/((w1 dot w2)pow 2-&1))*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1 crossw2))-vec3_vtrans w1**(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2-sqrt((normu pow 2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1 dot w2)*(w2dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1dot w2)pow 2-&1))*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1 cross w2)))))+atn((w2 dot(u'cross((((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2-sqrt((norm u pow2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1 dotw2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1 dot w2)*(w2 dotu)-w1 dot v)/((w1 dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dotw2)pow 2-&1))*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1 cross w2))-vec3_vtrans w2**(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2-sqrt((norm u pow 2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dotw2)pow 2-&1))*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1 cross w2)))))/(u'dot((((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2-sqrt((norm u pow 2-(((w1 dotw2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1 dot w2)*(w1 dotv)-w2 dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1 dot w2)*(w2 dot u)-w1 dotv)/((w1 dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow2-&1))*(w1 dot w2))/norm(w1 cross w2)pow 2)％(w1 cross w2))-vec3_vtrans w2**(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1)％w1+((w1 dot w2)*(w1dot v)-w2 dot u)/((w1 dot w2)pow 2-&1)％w2-sqrt((norm u pow 2-(((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1 dot w2)pow 2-&1))pow 2-(((w1 dot w2)*(w1 dot v)-w2dot u)/((w1 dot w2)pow 2-&1))pow 2-(&2*((w1 dot w2)*(w2 dot u)-w1 dot v)/((w1dot w2)pow 2-&1)*((w1 dot w2)*(w1 dot v)-w2 dot u)/((w1 dot w2)pow 2-&1))*(w1dot w2))/norm(w1 cross w2)pow 2)％(w1 cross w2)))))))
其中，w1，w2，r，u，u'，v，v'，p，q，c，z是向量，w1和w2代表三指机器人灵巧手分别绕其旋转的第一旋转轴和第二旋转轴，u'，v'分别是向量u，v的投影；s1和s2分别是第一旋转轴的旋量和第二旋转轴的旋量；a、a1和a2是实数，表示旋转角度θ、θ1和θ2，范围为[-π/2，π/2]；x1，x2和x3是实数，是自定义的变量；r是旋转轴上的一点，满足位置不变原则；homo_point表示齐次点；screw_2_matrix表示旋量的算子；vec 0表示0向量。dot表示向量的点乘；cross表示向量的叉乘。
6.根据权利要求1或2所述的方法，其特征在于，将形式化描述的Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3输入至所述定理证明器中进行证明，获得定理4、定理5和定理6，包括：
定理证明器中形式化描述Paden-Kahan子问题3，并证明Paden-Kahan子问题3的解以获得定理6；
其中，Paden-Kahan子问题3在定理证明器中如下形式化描述：
Figure FDA0003136479490000101
w r u u′v v′p q s d a a0 c d′.
((--(pi/&2)<a0∧a0<pi/&2)∧(--(pi/&2)<a∧a<pi/&2)∧
&0<＝(abs c)∧(abs c)<＝pi∧s＝(w,r cross w)∧norm w＝&1∧u＝p-r∧v＝q-r∧c＝a0-a∧u′＝u-(vec3_vtrans(FST s)**u)∧
v′＝v-(vec3_vtrans(FST s)**v)∧
matrix_exp(a0％％screw_2_matrix s)**(homo_point(mk_point r))＝(homo_point(mk_point r))∧
matrix_exp(a0％％screw_2_matrix s)**(homo_point(mk_point p))＝(homo_point(mk_point q))∧～(u′＝vec 0)∧～(v′＝vec 0))∧
(d)pow 2＝(norm(v-matrix_exp(a％％vec3_2_ssm w)**u))pow 2∧
(d′)pow 2＝(d)pow 2-(abs(w dot(p-q)))pow 2＝＝>
(&0<＝c＝＝>a＝a0-acs(((norm(u′))pow 2+(norm(v′))pow 2-(d′pow 2))/
(&2*(norm(u′))*(norm(v′)))))∧
(c<&0＝＝>a＝a0+acs(((norm(u′))pow 2+(norm(v′))pow 2-(d′pow 2))/
(&2*(norm(u′))*(norm(v′)))))
其中，w，r，u，u'，v，v'，p，q是向量，w代表旋转轴，u'，v'分别是向量u，v的投影；r是转轴上的一点，满足位置不变原则；s是运动旋量；a和a0是实数，代表旋转的角度，范围为[-π/2，π/2]；d是实数，代表点p绕旋转轴旋转后到点q的距离；d'是d在平面上的投影；c是实数表示a0-a的差；homo_point表示齐次点；screw_2_matrix表示运动旋量的算子；vec 0表示0向量。dot表示向量的点乘；cross表示向量的叉乘。
7.根据权利要求1或2所述的方法，其特征在于，利用形式化描述的所述定理1、定理2、定理3、定理4、定理5、定理6、Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3验证所述三指机器人灵巧手的运动学求解问题，包括：
将所述三指机器人灵巧手食指的正向运动学在定理证明器中如下形式化描述：
gst_a a1 s1 a2 s2 a3 s3 a4 s4 x＝matrix_exp(a1％％screw_2_matrix s1)**matrix_exp(a2％％screw_2_matrix s2)**matrix_exp(a3％％screw_2_matrix s3)**matrix_exp(a4％％screw_2_matrix s4)**(gst_initial x)
其中，s1、s2、s3、s4、分别为所述三指机器人灵巧手食指的第一至第四关节的运动旋量
Figure FDA0003136479490000111
和
Figure FDA0003136479490000112
a1、a2、a3和a4分别代表所述三指机器人灵巧手食指的第一至第四关节的旋转角度θ1、θ2、θ3和θ4；matrix_exp代表以e为底的指数函数；screw_2_matrix表示旋量的算子；gst_initial x的输入变量是向量x，x表示初始位姿的三维坐标，输出是齐次矩阵，代表初始位姿gst(0)；gst_a表示三指机器人灵巧手食指经过运动之后的位姿；
将所述三指机器人灵巧手食指从第二关节到末端执行器的刚体变换的运动过程在定理证明器中如下形式化描述：
g_1 a2 s2 a3 s3 a4 s4＝matrix_exp(a2％％screw_2_matrix s2)**matrix_exp(a3％％screw_2_matrix s3)**matrix_exp(a4％％screw_2_matrix s4)
其中，s2、s3、s4、分别为所述三指机器人灵巧手食指的第二至第四关节的运动旋量
Figure FDA0003136479490000113
和
Figure FDA0003136479490000114
a2、a3和a4分别代表是所述三指机器人灵巧手食指运动中第二至第四关节的旋转角度θ2、θ3和θ4；matrix_exp代表以e为底的指数函数；screw_2_matrix表示旋量的算子；
在定理证明器中验证所述三指机器人灵巧手食指从第一个关节到末端执行器的刚体变换的运动过程的代数运算，该验证过程在定理证明器中如下形式化描述：
Figure FDA0003136479490000121
a1 a2 a3 a4 s1 s2 s3 s4 x gd.
gst_a a1 s1 a2 s2 a3 s3 a4 s4 x＝gd＝＝>matrix_exp(--(a1)％％screw_2_matrix s1)**gd**(matrix_inv(gst_initial x))＝(g_1 a2 s2 a3 s3 a4 s4)
其中，s1、s2、s3、s4、分别为所述三指机器人灵巧手食指第一至第四关节的运动旋量
Figure FDA0003136479490000122
和
Figure FDA0003136479490000123
a1、a2、a3和a4分别代表是所述三指机器人灵巧手运动中食指的第一至第四关节的旋转角度θ1、θ2、θ3和θ4；matrix_exp代表以e为底的指数函数；screw_2_matrix表示运动旋量的算子；gd被定义为所述三指机器人灵巧手食指从第一个关节到末端执行器的刚体变换的运动过程；matrix_inv表示矩阵的逆；gst_initial x的输入变量是向量x，输出是齐次矩阵，代表所述三指机器人灵巧手食指的初始位姿gst(0)。
8.根据权利要求7所述的方法，其特征在于，利用形式化描述的所述定理1、定理2、定理3、定理4、定理5、定理6、Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3验证所述三指机器人灵巧手的运动学，还包括：
在定理证明器中验证所述三指机器人灵巧手食指从第二关节到末端执行器的刚体变换的运动过程的代数运算，该验证过程如下形式化描述：
Figure FDA0003136479490000124
θ2 θ3 θ4 s2 s3 s4.
matrix_exp(θ2％％screw_2_matrix s2)＝(g_1 a2 s2 a3 s3 a4 s4)**matrix_exp(--θ4％％screw_2_matrix s4)**matrix_exp(--θ3％％screw_2_matrix s3)
其中，s2、s3、s4、分别为所述三指机器人灵巧手食指第二至第四关节的运动旋量
Figure FDA0003136479490000125
和
Figure FDA0003136479490000126
a2、a3和a4分别代表是所述三指机器人灵巧手运动中食指第二至第四关节的旋转角度θ2、θ3和θ4；matrix_exp代表以e为底的指数函数；screw_2_matrix表示旋量的算子；和/或，
在定理证明器中验证所述三指机器人灵巧手食指第一关节的旋转角度θ1，该验证过程如下形式化描述：
Figure FDA0003136479490000131
a1 a2 a3 a4 h1 h2 h3 h4 px py.
px＝cos(a1)*(h1+h2*cos(a2)+h3*cos(a2+a3)+h4*cos(a2+a3+a4))∧
py＝sin(a1)*(h1+h2*cos(a2)+h3*cos(a2+a3)+h4*cos(a2+a3+a4))∧
a1＝atn((cos(a1)*(h1+h2*cos(a2)+h3*cos(a2+a3)+h4*cos(a2+a3+a4)))/
(sin(a1)*(h1+h2*cos(a2)+h3*cos(a2+a3)+h4*cos(a2+a3+a4))))＝＝>
a1＝atn(px/py)
其中，px、py是实数，分别代表所述三指机器人灵巧手食指第一关节位置的三维向量的x轴和y轴分量；a1、a2、a3和a4是实数，代表所述三指机器人灵巧手运动中食指第一至第四关节的旋转角度θ1、θ2、θ3和θ4；h1、h2、h3和h4是实数，分别代表所述三指机器人灵巧手食指第二至第四关节角之间的距离；和/或，
在定理证明器中验证所述三指机器人灵巧手食指第三关节的旋转角度θ3，该验证过程如下形式化描述：
Figure FDA0003136479490000132
p q s2 s3 s4 a2 a3 a4.
(matrix_exp((a2)％％screw_2_matrix s2)**(homo_point(mk_point p))＝(homo_point(mk_point p))∧matrix_exp(a2％％screw_2_matrix s2)**
(matrix_exp(a3％％screw_2_matrix s3)**(homo_point(mk_point q))-(homo_point(mk_point p)))＝matrix_exp(a3％％screw_2_matrix s3)**(homo_point(mk_point q))-(homo_point(mk_point p))∧
matrix_exp((a4)％％screw_2_matrix s4)**(homo_point(mk_point q))＝(homo_point(mk_point q)))＝＝>
norm(matrix_exp(a3％％screw_2_matrix s3)**(homo_point(mk_point q))-(homo_point(mk_point p)))＝norm(g_1a2 s2 a3 s3 a4 s4**(homo_point(mk_point q))–(homo_point(mk_point p)))
其中，p、q为向量，分别代表所述三指机器人灵巧手食指第二关节轴上一点和第四关节轴上一点；a2、a3和a4是实数，代表所述三指机器人灵巧手食指第二至第四关节的旋转角度θ2、θ3和θ4；s2、s3、s4分别为所述三指机器人灵巧手食指第二至第四关节的运动旋量
Figure FDA0003136479490000141
和
Figure FDA0003136479490000142
homo_point表示齐次点；和/或，
在定理证明器中验证所述三指机器人灵巧手食指第二关节的旋转角度θ2，该验证过程如下形式化描述：
Figure FDA0003136479490000143
a2 a3 a4 s2 s3 s4 p'g2.
g2＝(g_1 a2 s2 a3 s3 a4 s4)**matrix_exp(--(a4)％％screw_2_matrix s4)**matrix_exp(--(a3)％％screw_2_matrix s3)＝＝>matrix_exp(a2％％screw_2_matrixs2)**homo_point(mk_point p')＝g2**homo_point(mk_point p')
其中，p'为向量，代表所述三指机器人灵巧手食指第二关节轴外一点；a2、a3和a4是实数，代表所述三指机器人灵巧手食指第二至第四关节的旋转角度θ2、θ3和θ4；s2、s3、s4分别为所述三指机器人灵巧手食指第二至第四关节的运动旋量
Figure FDA0003136479490000144
和
Figure FDA0003136479490000145
homo_point表示齐次点；screw_2_matrix表示运动旋量的算子。
9.根据权利要求1或2所述的方法，其特征在于，利用形式化描述的所述定理1、定理2、定理3、定理4、定理5、定理6、Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3验证所述三指机器人灵巧手的运动学，包括：
在定理证明器中定义所述三指机器人灵巧手拇指的正向运动学，该定义的形式化描述如下：
ga a1 s1 a2 s2 a3 s3 a4 s4 a5 s5 x＝matrix_exp(a1％％screw_2_matrix s1)**matrix_exp(a2％％screw_2_matrix s2)**matrix_exp(a3％％screw_2_matrix s3)**matrix_exp(a4％％screw_2_matrix s4)**matrix_exp(a5％％screw_2_matrix s5)**(gst_initial x)
其中，s1、s2、s3、s4、s5分别为所述三指机器人灵巧手拇指第一至第五关节的运动旋量
Figure FDA0003136479490000151
和
Figure FDA0003136479490000152
θ1、θ2、θ3、θ4和θ5是所述三指机器人灵巧手拇指运动中第一至第五关节的旋转角度；matrix_exp代表以e为底的指数函数；screw_2_matrix表示运动旋量的算子；gst_initial x的输入变量是向量x，输出是齐次矩阵，代表初始位姿gst(0)；
在定理证明器中验证所述三指机器人灵巧手拇指第五关节角度θ5，该验证过程在定理证明器中的形式化描述如下：
Figure FDA0003136479490000155
h2 a1 a2 a3 a4 a5 s1 s2 s3 s4 s5 g_d pw q.
((matrix_exp((a1)％％screw_2_matrix s1)**matrix_exp((a2)％％screw_2_matrix s2))**((homo_point(mk_point pw))-(homo_point(mk_point q)))＝(homo_point(mk_point pw))-(homo_point(mk_point q))∧matrix_exp((a3)％％screw_2_matrix s3)**matrix_exp((a4)％％screw_2_matrix s4)**(homo_point(mk_point pw))＝(homo_point(mk_point pw))∧matrix_exp((a1)％％screw_2_matrix s1)**matrix_exp((a2)％％screw_2_matrix s2)**(homo_point(mk_point q))＝(homo_point(mk_point q))∧
g_d＝ga a1 s1 a2 s2 a3 s3 a4 s4 a5 s5 x)＝＝>norm((homo_point(mk_pointpw))-(homo_point(mk_point q)))＝norm(g_d**(matrix_inv(gst_initial x))**matrix_exp(--a5％％screw_2_matrix s5)**(homo_point(mk_point pw))-(homo_point(mk_point q)))
其中，pw和q为向量，代表所述三指机器人灵巧手拇指第三关节轴线与第四关节轴线的交点和第一关节轴线与第二关节轴线的交点；h2为实数；s1、s2、s3、s4、s5分别为所述三指机器人灵巧手拇指第一至第五关节的运动旋量
Figure FDA0003136479490000153
和
Figure FDA0003136479490000154
θ1、θ2、θ3、θ4和θ5是所述三指机器人灵巧手运动中拇指第一至第五关节的旋转角度；matrix_exp代表以e为底的指数函数；screw_2_matrix表示运动旋量的算子；homo_point表示齐次点；g_d被定义为所述三指机器人灵巧手拇指从第一个关节到末端执行器的刚体变换的运动过程；gst_initial x的输入变量是向量x，输出是齐次矩阵，代表所述三指机器人灵巧手拇指的初始位姿gst(0)；和/或，
在定理证明器中验证所述三指机器人灵巧手拇指第一和第二关节角度θ1和θ2，该验证过程在定理证明器中的形式化描述如下：
Figure FDA0003136479490000161
s1 s2 s3 s4 s5 a1 a2 a3 a4 a5 pw x g_d.
(g_d＝ga a1 s1 a2 s2 a3 s3 a4 s4 a5 s5 x∧
matrix_exp((a3)％％screw_2_matrix s3)**
matrix_exp((a4)％％screw_2_matrix s4)**(homo_point(mk_point pw))＝(homo_point(mk_point pw))∧
ga_2＝g_d**(matrix_inv(gst_initial x))**
matrix_exp(--(a5)％％screw_2_matrix s5)**(homo_point(mk_point pw)))＝＝>
matrix_exp((a1)％％screw_2_matrix s1)**
matrix_exp((a2)％％screw_2_matrix s2)**(homo_point(mk_point pw))＝ga_2
其中，pw为向量，代表所述三指机器人灵巧手拇指第三关节轴线与第四关节轴线的交点；s1、s2、s3、s4、s5分别为所述三指机器人灵巧手拇指第一至第五关节的运动旋量
Figure FDA0003136479490000162
和
Figure FDA0003136479490000163
θ1、θ2、θ3、θ4和θ5是所述三指机器人灵巧手运动中拇指第一至第五关节的旋转角度；matrix_exp代表以e为底的指数函数；screw_2_matrix表示旋量的算子；homo_point表示齐次点；g_d被定义为所述三指机器人灵巧手拇指从第一个关节到末端执行器的刚体变换的运动过程；gst_initial x的输入变量是向量x，输出是齐次矩阵，代表初始位姿gst(0)；和/或，
在定理证明器中验证所述三指机器人灵巧手拇指第三和第四关节角度θ3和θ4，该验证过程在定理证明器中的形式化描述如下：
Figure FDA0003136479490000164
s1 s2 s3 s4 s5 a1 a2 a3 a4 a5 p q x g_d.
(g_d＝ga a1 s1 a2 s2 a3 s3 a4 s4 a5 s5 x∧
q＝matrix_exp(--(a2)％％screw_2_matrix s2)**matrix_exp(--(a1)％％screw_2_matrix s1)**g_d**(matrix_inv(gst_initial x))**matrix_exp(--(a5)％％screw_2_matrix s5)**(homo_point(mk_point p)))＝＝>matrix_exp(a3％％screw_2_matrixs3)**matrix_exp(a4)％％screw_2_matrix s4)**(homo_point(mk_point p))＝q
其中，p、q为向量，代表所述三指机器人灵巧手拇指第三关节轴线与第四关节轴线外一点和第一关节轴线与第二关节轴线的交点；s1、s2、s3、s4、s5分别为所述三指机器人灵巧手拇指第一至第五关节的运动旋量
Figure FDA0003136479490000171
和
Figure FDA0003136479490000172
θ1、θ2、θ3、θ4和θ5是所述三指机器人灵巧手运动中拇指第一至第五关节的旋转角度；matrix_exp代表以e为底的指数函数；screw_2_matrix表示运动旋量的算子；homo_point表示齐次点；g_d被定义为所述三指机器人灵巧手拇指从第一个关节到末端执行器的刚体变换的运动过程；gst_initial x的输入变量是向量x，输出是齐次矩阵，代表初始位姿gst(0)。
10.一种电子设备，其特征在于，包括存储器和处理器；其中，
所述存储器用于存储一条或多条计算机指令，其中，所述一条或多条计算机指令被所述处理器执行以实现权利要求1-9任一项所述的方法。
CN202110719629.1A 2021-06-28 2021-06-28 三指机器人灵巧手逆运动学的形式化验证方法及电子设备 Active CN113400310B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110719629.1A CN113400310B (zh) 2021-06-28 2021-06-28 三指机器人灵巧手逆运动学的形式化验证方法及电子设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110719629.1A CN113400310B (zh) 2021-06-28 2021-06-28 三指机器人灵巧手逆运动学的形式化验证方法及电子设备

Publications (2)

Publication Number Publication Date
CN113400310A true CN113400310A (zh) 2021-09-17
CN113400310B CN113400310B (zh) 2022-07-05

Family

ID=77679762

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110719629.1A Active CN113400310B (zh) 2021-06-28 2021-06-28 三指机器人灵巧手逆运动学的形式化验证方法及电子设备

Country Status (1)

Country Link
CN (1) CN113400310B (zh)

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104772773A (zh) * 2015-05-08 2015-07-15 首都师范大学 一种机械臂运动学形式化分析方法
CN107756400A (zh) * 2017-10-13 2018-03-06 北京工业大学 一种基于旋量理论的6r机器人逆运动学几何求解方法
CN108199940A (zh) * 2017-12-26 2018-06-22 首都师范大学 一种机器人关节通信***模型的验证方法及***

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104772773A (zh) * 2015-05-08 2015-07-15 首都师范大学 一种机械臂运动学形式化分析方法
CN107756400A (zh) * 2017-10-13 2018-03-06 北京工业大学 一种基于旋量理论的6r机器人逆运动学几何求解方法
CN108199940A (zh) * 2017-12-26 2018-06-22 首都师范大学 一种机器人关节通信***模型的验证方法及***

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
JINGZHI ZHANG 等: "Formalizaion of functional variation in HOL Light", 《JOURNAL OF LOGICAL AND ALGEBRAIC METHODS IN PROGRAMMING》 *
杨秀梅 等: "串联机器人雅可比矩阵的高阶逻辑形式化", 《小型微型计算机***》 *
裴九芳 等: "基于旋量理论的三指机器人灵巧手逆运动学分析", 《中国机械工程》 *

Also Published As

Publication number Publication date
CN113400310B (zh) 2022-07-05

Similar Documents

Publication Publication Date Title
Corke et al. Robot arm kinematics
CN108972558B (zh) 一种基于轴不变量的多轴机器人动力学建模方法
CN108942942B (zh) 一种基于轴不变量的多轴机器人逆运动学建模与解算方法
CN108983705B (zh) 一种基于轴不变量的多轴机器人***正运动学建模与解算方法
Shabana Computational dynamics
Wittenburg Kinematics
CN104772773B (zh) 一种机械臂运动学形式化分析方法
Bajd et al. Introduction to robotics
CN109079784B (zh) 一种基于轴不变量的多轴机器人***建模与控制方法
Hauenstein et al. Synthesis of three-revolute spatial chains for body guidance
Bahuguna et al. Teaching and learning of robot kinematics using RoboAnalyzer software
CN104866722A (zh) 一种七轴工业机械臂的逆动学求解方法
Huang et al. A geometric algebra algorithm for the closed-form forward displacement analysis of 3-PPS parallel mechanisms
Chevallier et al. Multi-Body Kinematics and Dynamics with Lie Groups
Angeles et al. Kinematics of mechanical systems: fundamentals, analysis and synthesis
CN113400310B (zh) 三指机器人灵巧手逆运动学的形式化验证方法及电子设备
Wu et al. Formal kinematic analysis of a general 6R manipulator using the screw theory
Wu et al. Geometric interpretation of the general POE model for a serial-link robot via conversion into DH parameterization
Pikalov et al. Vector model for solving the inverse kinematics problem in the system of external adaptive control of robotic manipulators
Chen et al. Applications of an improved Dixon elimination method for the inverse kinematics of 6R manipulators
Wang et al. Formalization of the inverse kinematics of three-fingered dexterous hand
Parthasarathy et al. ADAMS-MATLAB co-simulation of a serial manipulator
Zhao et al. Model and simulation of the mitsubishi rv-m1 robot using matlab
Wang et al. Motion generation of a planar 3R serial chain based on conformal geometric algebra with applications to planar linkages
Schappler et al. A maple toolchain for rigid body dynamics of serial, hybrid and parallel robots

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant