CN113400310A - 三指机器人灵巧手逆运动学的形式化验证方法及电子设备 - Google Patents
三指机器人灵巧手逆运动学的形式化验证方法及电子设备 Download PDFInfo
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 64
- 238000012795 verification Methods 0.000 title claims abstract description 35
- 239000011159 matrix material Substances 0.000 claims description 399
- 239000013598 vector Substances 0.000 claims description 155
- 230000033001 locomotion Effects 0.000 claims description 150
- 210000003811 finger Anatomy 0.000 claims description 88
- 210000003813 thumb Anatomy 0.000 claims description 64
- 230000006870 function Effects 0.000 claims description 56
- 230000008569 process Effects 0.000 claims description 32
- 230000009466 transformation Effects 0.000 claims description 25
- 239000012636 effector Substances 0.000 claims description 24
- 238000013519 translation Methods 0.000 claims description 15
- 238000013507 mapping Methods 0.000 claims description 9
- 102100040428 Chitobiosyldiphosphodolichol beta-mannosyltransferase Human genes 0.000 claims description 6
- 230000008676 import Effects 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 24
- 238000004590 computer program Methods 0.000 description 6
- 238000004891 communication Methods 0.000 description 4
- 238000012360 testing method Methods 0.000 description 3
- 230000006399 behavior Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 241000208140 Acer Species 0.000 description 1
- 241000282414 Homo sapiens Species 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 210000005224 forefinger Anatomy 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
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:运动旋量的算子:
运动旋量的数学公式如下表示:
ξ=(ω;υ)
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表示反对称矩阵转化函数;
定义2:齐次矩阵
为了三指机器人灵巧手的姿态,假设{S}和{T}分别为惯性坐标系和工具坐标系,其中{T}固连于三指机器人灵巧手上,则三指机器人灵巧手相对于惯性坐标系{S}的位姿映射集合可由工具坐标系{T}相对于惯性坐标系{S}的映射集合如下表示:
式中,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列的元素;符号&表示将自然数类型转化为实数类型;
定义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维的矩阵;
进一步地,将形式化描述的旋量理论的内容输入至定理证明器中进行证明,获得定理1、定理2和定理3,包括:
通过在定理证明器中基于定义1和定义3证明运动旋量的数学形式和齐次矩阵演变的通用矩阵的数学形式相同得到定理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;
将三指机器人灵巧手即平移又旋转时运动旋量的指数表达式在定理证明器中形式化描述如下:
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在定理证明器中如下形式化描述:
(--(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 va=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的解的过程包括:
将证明成立后的所述子目标加入假设列表,利用定理证明器证明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在定理证明器中如下形式化描述:
(--(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在定理证明器中如下形式化描述:
((--(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、分别为所述三指机器人灵巧手食指的第一至第四关节的运动旋量和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、分别为所述三指机器人灵巧手食指的第二至第四关节的运动旋量和a2、a3和a4分别代表是所述三指机器人灵巧手食指运动中第二至第四关节的旋转角度θ2、θ3和θ4;matrix_exp代表以e为底的指数函数;screw_2_matrix表示旋量的算子;
在定理证明器中验证所述三指机器人灵巧手食指从第一个关节到末端执行器的刚体变换的运动过程的代数运算,该验证过程在定理证明器中如下形式化描述:
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、分别为所述三指机器人灵巧手食指第一至第四关节的运动旋量和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验证所述三指机器人灵巧手的运动学,还包括:
在定理证明器中验证所述三指机器人灵巧手食指从第二关节到末端执行器的刚体变换的运动过程的代数运算,该验证过程如下形式化描述:
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、分别为所述三指机器人灵巧手食指第二至第四关节的运动旋量和a2、a3和a4分别代表是所述三指机器人灵巧手运动中食指第二至第四关节的旋转角度θ2、θ3和θ4;matrix_exp代表以e为底的指数函数;screw_2_matrix表示旋量的算子;和/或,
在定理证明器中验证所述三指机器人灵巧手食指第一关节的旋转角度θ1,该验证过程如下形式化描述:
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,该验证过程如下形式化描述:
(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分别为所述三指机器人灵巧手食指第二至第四关节的运动旋量和homo_point表示齐次点;和/或,
在定理证明器中验证所述三指机器人灵巧手食指第二关节的旋转角度θ2,该验证过程如下形式化描述:
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分别为所述三指机器人灵巧手食指第二至第四关节的运动旋量和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分别为所述三指机器人灵巧手拇指第一至第五关节的运动旋量和θ1、θ2、θ3、θ4和θ5是所述三指机器人灵巧手拇指运动中第一至第五关节的旋转角度;matrix_exp代表以e为底的指数函数;screw_2_matrix表示运动旋量的算子;gst_initial x的输入变量是向量x,输出是齐次矩阵,代表初始位姿gst(0);
在定理证明器中验证所述三指机器人灵巧手拇指第五关节角度θ5,该验证过程在定理证明器中的形式化描述如下:
((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分别为所述三指机器人灵巧手拇指第一至第五关节的运动旋量和θ1、θ2、θ3、θ4和θ5是所述三指机器人灵巧手运动中拇指第一至第五关节的旋转角度;matrix_exp代表以e为底的指数函数;screw_2_matrix表示运动旋量的算子;homo_point表示齐次点;g_d被定义为所述三指机器人灵巧手拇指从第一个关节到末端执行器的刚体变换的运动过程;gst_initial x的输入变量是向量x,输出是齐次矩阵,代表所述三指机器人灵巧手拇指的初始位姿gst(0);和/或,
(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分别为所述三指机器人灵巧手拇指第一至第五关节的运动旋量和θ1、θ2、θ3、θ4和θ5是所述三指机器人灵巧手运动中拇指第一至第五关节的旋转角度;matrix_exp代表以e为底的指数函数;screw_2_matrix表示旋量的算子;homo_point表示齐次点;g_d被定义为所述三指机器人灵巧手拇指从第一个关节到末端执行器的刚体变换的运动过程;gst_initial x的输入变量是向量x,输出是齐次矩阵,代表初始位姿gst(0);和/或,
在定理证明器中验证所述三指机器人灵巧手拇指第三和第四关节角度θ3和θ4,该验证过程在定理证明器中的形式化描述如下:
(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分别为所述三指机器人灵巧手拇指第一至第五关节的运动旋量和θ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:运动旋量的算子
运动旋量即李代数元素来描述螺旋运动的无穷小量,表示如下:
ξ=(ω;υ)
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表示反对称矩阵转化函数。
定义2:齐次矩阵
为了描述空间刚体的姿态,假设{S}和{T}分别为惯性坐标系和工具坐标系,其中{T}固连于刚体上,则刚体相对于惯性坐标系{S}的位姿映射集合可由工具坐标系{T}相对于惯性坐标系{S}的映射集合来表示:
式中,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为即在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维的矩阵;
在定理证明器中形式化描述旋量理论的内容后,在定理证明器中对旋量理论的内容进行证明。
定理1:运动旋量的矩阵表达式在定理证明器中的形式化描述如下:
上述命题表示了运动旋量s可以转换成矩阵表达式。证明时,需要把定义1和定义2写进去,然后利用定理证明器库里已有的定理,比如:CART_EQ、LAMBDA_BETA等去证明等式两边是相等的关系。
基于旋量理论,刚体运动可通过运动旋量的指数形式表示为:
上式表明:刚体运动的指数形式可以有两种情况,一种是转轴ω为0,另一种是转轴ω不为0的情况,分别在定理证明器中对上述两种情况进行形式化描述,如下:
定理2:刚体仅平移时运动旋量的指数表达式
定理证明器中的形式化描述如下:
其中,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:刚体即平移又旋转时的运动旋量的指数表达式
定理证明器中的形式化描述如下:
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
定理证明器中的形式化描述如下:
(--(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 va=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,包括:
将所述子目标加入假设列表,利用定理证明器证明得到Paden-Kahan子问题1的解作为定理4。
上述定理4的证明过程是先引入必要的前提条件,然后通过逻辑推导出命题成立。其中,困难在于如何形式化表达出转轴w。在本公开方式中,通过引入子目标证明定理4成立后加入假设列表里直接使用。通过公式中隐含的转轴w来证明定理4的成立,从而避免了直接描述转轴w。
图4示出根据本公开一实施方式中Paden-Kahan子问题2的示意图。如图4所示,Paden-Kahan子问题2描述的是将点p绕给定轴s2旋转角度a2,再绕轴s1旋转角度a1到点q重合,求解绕上述两个轴旋转的角度的问题。
定理5:Paden-Kahan子问题2
定理证明器中的形式化描述如下:
(--(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以及旋转轴投影平面圆心之间的旋转角度,是点p绕旋转轴ξ旋转角度θ之后点的投影。
定理6:Paden-Kahan子问题3
定理证明器中的形式化描述如下:
((--(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分别为所述三指机器人灵巧手食指各相邻关节之间连杆长度。
根据本公开的实施例,食指正向运动学的指数积公式,即
其中,gst(0)为初始位姿,gst(θ)为刚体在初始位姿下经过旋转或平移后的最终位姿。和分别为第一至第四关节的运动旋量;θ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、分别为所述三指机器人灵巧手食指的第一至第四关节的运动旋量和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、分别为第二至第四关节的运动旋量和a2、a3和a4分别代表是机器人运动中第二至第四关节的旋转角度θ2、θ3和θ4;matrix_exp代表以e为底的指数函数;screw_2_matrix表示旋量的算子。定义5描述了食指从第二个关节到末端执行器的刚体变换的运动过程。
经过上述定义4和定义5的形式化描述之后,可以对食指的运动过程进行验证,进而得到定理7-定理11。
定理7:食指从第一个关节到末端执行器的刚体变换的运动过程的代数运算
定理证明器中的形式化描述如下:
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、分别为食指第一至第四关节的运动旋量和a1、a2、a3和a4分别代表是机器人运动中食指的第一至第四关节的旋转角度θ1、θ2、θ3和θ4;matrix_exp代表以e为底的指数函数;screw_2_matrix表示旋量的算子;gd被定义为食指从第一个关节到末端执行器的刚体变换的运动过程;matrix_inv表示矩阵的逆;gst_initialx的输入变量是向量x,输出是齐次矩阵,代表食指的初始位姿gst(0)。
定理8:食指从第二关节到末端执行器的刚体变换的运动过程的代数运算
定理证明器中的形式化描述如下:
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、分别为食指第二至第四关节的运动旋量和a2、a3和a4分别代表是机器人运动中食指第二至第四关节的旋转角度θ2、θ3和θ4;matrix_exp代表以e为底的指数函数;screw_2_matrix表示旋量的算子。
根据食指正向运动学的公式,可得
其中,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
定理证明器中的形式化描述如下:
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
定理证明器中的形式化描述如下:
(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分别为第二至第四关节的运动旋量和homo_point表示齐次点。定理10验证了θ3满足Paden-Kahan子问题3的求解条件,因此可使用Paden-Kahan子问题3直接求解。
定理11:验证食指第二关节角度θ2
定理证明器中的形式化描述如下:
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分别为第二至第四关节的运动旋量和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分别为所述三指机器人灵巧手拇指各个相邻关节间的连杆长度。
根据本公开的实施例,拇指正向运动学的指数积公式,即
其中,gst(0)为初始位姿,gst(θ)为刚体在初始位姿下经过旋转或平移后的最终位姿。和分别为第一至第五关节的运动旋量;θ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分别为拇指第一至第五关节的运动旋量和θ1、θ2、θ3、θ4和θ5是机器人运动中各个关节的旋转角度;matrix_exp代表以e为底的指数函数;screw_2_matrix表示旋量的算子;gst_initial x的输入变量是向量x,输出是齐次矩阵,代表初始位姿gst(0)。
经过上述定义6的形式化描述之后,可以对拇指的运动过程在定理证明器中进行证明,获得如下定理12-定理14:
定理12:验证拇指第五关节角度θ5
定理证明器中的形式化描述如下:
((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分别为第一至第五关节的运动旋量和θ1、θ2、θ3、θ4和θ5是机器人运动中拇指第一至第五关节的旋转角度;matrix_exp代表以e为底的指数函数;screw_2_matrix表示旋量的算子;homo_point表示齐次点;g_d被定义为拇指从第一个关节到末端执行器的刚体变换的运动过程;gst_initial x的输入变量是向量x,输出是齐次矩阵,代表拇指的初始位姿gst(0)。
定理13:验证拇指第一和第二关节角度θ1和θ2
定理证明器中的形式化描述如下:
(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分别为第一至第五关节的运动旋量和θ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
定理证明器中的形式化描述如下:
(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分别为第一至第五关节的运动旋量和θ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:运动旋量的算子:
运动旋量的数学公式如下表示:
ξ=(ω;υ)
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表示反对称矩阵转化函数;
定义2:齐次矩阵
为了三指机器人灵巧手的姿态,假设{S}和{T}分别为惯性坐标系和工具坐标系,其中{T}固连于三指机器人灵巧手上,则三指机器人灵巧手相对于惯性坐标系{S}的位姿映射集合可由工具坐标系{T}相对于惯性坐标系{S}的映射集合如下表示:
式中,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列的元素;符号&表示将自然数类型转化为实数类型;
定义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维的矩阵;
3.根据权利要求1或2所述的方法,其特征在于,将形式化描述的旋量理论的内容输入至定理证明器中进行证明,获得定理1、定理2和定理3,包括:
通过在定理证明器中基于定义1和定义3证明运动旋量的数学形式和齐次矩阵演变的通用矩阵的数学形式相同得到定理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;
将三指机器人灵巧手即平移又旋转时运动旋量的指数表达式在定理证明器中形式化描述如下:
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在定理证明器中如下形式化描述:
(--(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)
其中,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的解的过程包括:
将证明成立后的所述子目标加入假设列表,利用定理证明器证明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在定理证明器中如下形式化描述:
(--(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在定理证明器中如下形式化描述:
((--(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、分别为所述三指机器人灵巧手食指的第一至第四关节的运动旋量和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、分别为所述三指机器人灵巧手食指的第二至第四关节的运动旋量和a2、a3和a4分别代表是所述三指机器人灵巧手食指运动中第二至第四关节的旋转角度θ2、θ3和θ4;matrix_exp代表以e为底的指数函数;screw_2_matrix表示旋量的算子;
在定理证明器中验证所述三指机器人灵巧手食指从第一个关节到末端执行器的刚体变换的运动过程的代数运算,该验证过程在定理证明器中如下形式化描述:
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)
8.根据权利要求7所述的方法,其特征在于,利用形式化描述的所述定理1、定理2、定理3、定理4、定理5、定理6、Paden-Kahan子问题1、Paden-Kahan子问题2和Paden-Kahan子问题3验证所述三指机器人灵巧手的运动学,还包括:
在定理证明器中验证所述三指机器人灵巧手食指从第二关节到末端执行器的刚体变换的运动过程的代数运算,该验证过程如下形式化描述:
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、分别为所述三指机器人灵巧手食指第二至第四关节的运动旋量和a2、a3和a4分别代表是所述三指机器人灵巧手运动中食指第二至第四关节的旋转角度θ2、θ3和θ4;matrix_exp代表以e为底的指数函数;screw_2_matrix表示旋量的算子;和/或,
在定理证明器中验证所述三指机器人灵巧手食指第一关节的旋转角度θ1,该验证过程如下形式化描述:
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,该验证过程如下形式化描述:
(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分别为所述三指机器人灵巧手食指第二至第四关节的运动旋量和homo_point表示齐次点;和/或,
在定理证明器中验证所述三指机器人灵巧手食指第二关节的旋转角度θ2,该验证过程如下形式化描述:
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')
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分别为所述三指机器人灵巧手拇指第一至第五关节的运动旋量和θ1、θ2、θ3、θ4和θ5是所述三指机器人灵巧手拇指运动中第一至第五关节的旋转角度;matrix_exp代表以e为底的指数函数;screw_2_matrix表示运动旋量的算子;gst_initial x的输入变量是向量x,输出是齐次矩阵,代表初始位姿gst(0);
在定理证明器中验证所述三指机器人灵巧手拇指第五关节角度θ5,该验证过程在定理证明器中的形式化描述如下:
((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分别为所述三指机器人灵巧手拇指第一至第五关节的运动旋量和θ1、θ2、θ3、θ4和θ5是所述三指机器人灵巧手运动中拇指第一至第五关节的旋转角度;matrix_exp代表以e为底的指数函数;screw_2_matrix表示运动旋量的算子;homo_point表示齐次点;g_d被定义为所述三指机器人灵巧手拇指从第一个关节到末端执行器的刚体变换的运动过程;gst_initial x的输入变量是向量x,输出是齐次矩阵,代表所述三指机器人灵巧手拇指的初始位姿gst(0);和/或,
在定理证明器中验证所述三指机器人灵巧手拇指第一和第二关节角度θ1和θ2,该验证过程在定理证明器中的形式化描述如下:
(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分别为所述三指机器人灵巧手拇指第一至第五关节的运动旋量和θ1、θ2、θ3、θ4和θ5是所述三指机器人灵巧手运动中拇指第一至第五关节的旋转角度;matrix_exp代表以e为底的指数函数;screw_2_matrix表示旋量的算子;homo_point表示齐次点;g_d被定义为所述三指机器人灵巧手拇指从第一个关节到末端执行器的刚体变换的运动过程;gst_initial x的输入变量是向量x,输出是齐次矩阵,代表初始位姿gst(0);和/或,
在定理证明器中验证所述三指机器人灵巧手拇指第三和第四关节角度θ3和θ4,该验证过程在定理证明器中的形式化描述如下:
(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
10.一种电子设备,其特征在于,包括存储器和处理器;其中,
所述存储器用于存储一条或多条计算机指令,其中,所述一条或多条计算机指令被所述处理器执行以实现权利要求1-9任一项所述的方法。
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)
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 | 首都师范大学 | 一种机器人关节通信***模型的验证方法及*** |
-
2021
- 2021-06-28 CN CN202110719629.1A patent/CN113400310B/zh active Active
Patent Citations (3)
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)
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 |