CN104772773B

CN104772773B - A kind of Mechanical transmission test Formal Analysis Method

Info

Publication number: CN104772773B
Application number: CN201510233901.XA
Authority: CN
Inventors: 施智平; 关永; 吴爱轩; 李黎明; 李晓娟; 王瑞; 邵振洲; 张倩颖; 吴敏华
Original assignee: Capital Normal University
Current assignee: Capital Normal University
Priority date: 2015-05-08
Filing date: 2015-05-08
Publication date: 2016-08-17
Anticipated expiration: 2035-05-08
Also published as: CN104772773A

Abstract

The invention relates to a formalized analysis method for the kinematics of a mechanical arm, comprising the following steps: Step 1, determining the structural parameters of the mechanical arm mechanism; Step 2, using a high-level logic language to establish a kinematic logic model of the mechanical arm mechanism; Logical formulas describe the kinematic constraints or attributes that need to be verified; Step 4, combine the kinematics logic model of the mechanical arm mechanism and the kinematic constraints or attributes that need to be verified to form a logical proposition; Step 5, use the logical reasoning engine to prove whether the logical proposition If it is established, it means that the model satisfies the constraint or has the attribute; otherwise, if it does not hold, it indicates that the model does not satisfy the constraint or does not have the attribute. The formal verification in the present invention uses mathematical methods to prove the correctness of the system according to the formal specification or property of the system, which is accurate and complete for the verified property.

Description

A Formal Analysis Method of Manipulator Kinematics

技术领域technical field

本发明涉及机械臂技术领域，尤其涉及机械臂运动学形式化分析方法。The invention relates to the technical field of manipulators, in particular to a formalized analysis method for the kinematics of a manipulator.

背景技术Background technique

近年来机械臂技术进入高速发展期，带有机械臂的机器人在工业、科学考察、军事、抢险救灾、医疗和家用等各个领域广泛应用。但是机器人执行任务时失败甚至出现安全事故也屡见不鲜。人们意识到机器人的验证技术与设计技术之间存在显著差距。机器人设计中的一个微小漏洞可能造成巨大的经济损失，甚至危及人身安全，因此设计者必须花大量的时间精力对设计进行分析和验证，以便能在产品成型前找到机器人设计的错误。In recent years, robotic arm technology has entered a period of rapid development. Robots with mechanical arms are widely used in various fields such as industry, scientific investigation, military, emergency rescue, medical treatment and household use. However, it is not uncommon for robots to fail or even have safety accidents when performing tasks. It is recognized that there is a significant gap between the verification technology of the robot and the design technology. A small loophole in the robot design may cause huge economic losses and even endanger personal safety. Therefore, designers must spend a lot of time and energy analyzing and verifying the design in order to find errors in the robot design before the product is formed.

目前，机械臂设计正确性的常用验证方法有：(1)基于纸笔运算的人工分析；(2)计算机模拟仿真；(3)计算机代数系统。基于纸笔运算的人工分析方法是在设计者经验的基础上，通过建立合适的数学模型对机械臂的关键性质进行验证。但考虑到机器人设计的复杂性，这种方法往往因计算过于繁琐而难以实现且容易出错。计算机模拟仿真方法是机械臂的设计与验证使用最为广泛的方法，该方法在合适的仿真软件(如MATLAB)上建立相应的仿真模型来对机械臂设计模型进行验证。这种方法虽然高效但受到仿真平台的运算速度和内存的限制，同时也无法给出完全精确的仿真结果。随着Maple、Mathematica等软件的发展，基于计算机代数系统对机械臂属性的验证变得流行。基于计算机代数系统的方法与计算模拟仿真方法相比精度更高，但其在边界条件的处理上存在短板，同时计算机代数系统内核中的符号计算算法也尚未得到验证，可能因其自身存在漏洞而影响其验证结果的精确性。At present, the commonly used verification methods for the correctness of manipulator design are: (1) manual analysis based on paper and pen calculation; (2) computer simulation; (3) computer algebra system. The manual analysis method based on paper and pen calculation is to verify the key properties of the manipulator by establishing a suitable mathematical model based on the experience of the designer. But given the complexity of robot design, this approach is often too computationally cumbersome to implement and error-prone. The computer simulation method is the most widely used method for the design and verification of the manipulator. In this method, a corresponding simulation model is established on a suitable simulation software (such as MATLAB) to verify the design model of the manipulator. Although this method is efficient, it is limited by the computing speed and memory of the simulation platform, and it cannot give completely accurate simulation results. With the development of software such as Maple and Mathematica, the verification of the properties of manipulators based on computer algebra systems has become popular. The method based on the computer algebra system is more accurate than the computational simulation method, but it has shortcomings in the processing of boundary conditions. At the same time, the symbolic calculation algorithm in the core of the computer algebra system has not been verified, which may be due to its own loopholes. And affect the accuracy of its verification results.

发明内容Contents of the invention

有鉴于此，本发明的目的在于提供一种机械臂运动学形式化分析方法，以解决现有技术中计算复杂、结果不太精确的问题。In view of this, the object of the present invention is to provide a formalized analysis method for the kinematics of the manipulator to solve the problems of complicated calculation and inaccurate results in the prior art.

本发明提供的机械臂运动学形式化分析方法，包括如下步骤：The formalized analysis method of manipulator kinematics provided by the present invention comprises the following steps:

步骤一，确定机械臂机构的结构参数；Step 1, determining the structural parameters of the mechanical arm mechanism;

步骤二，用高阶逻辑语言建立机械臂机构运动学逻辑模型；Step 2, using high-level logic language to establish a logic model of the kinematics of the mechanical arm mechanism;

步骤三，用逻辑公式描述需要验证的运动学约束或属性；Step 3, use logical formulas to describe the kinematic constraints or attributes that need to be verified;

步骤四，将机械臂机构运动学逻辑模型和需要验证的运动学约束或属性组成一个逻辑命题；Step 4: Compose a logical proposition by combining the kinematics logic model of the manipulator mechanism and the kinematics constraints or attributes that need to be verified;

步骤五，利用逻辑推理引擎证明所述逻辑命题是否成立，成立表明所述模型满足所述的约束或者具备所述的属性，否则，不成立表明所述模型不满足所述的约束或者不具备所述的属性；所述步骤二中，机械臂机构运动学逻辑模型是基于旋量理论或D-H参数法的高阶逻辑形式化表达。Step 5: Use the logical reasoning engine to prove whether the logical proposition is true. If it is true, it means that the model satisfies the constraints or has the attributes. Otherwise, if it is not true, it means that the model does not satisfy the constraints or does not have the attributes. attribute; in the second step, the kinematics logic model of the manipulator mechanism is a formalized expression of high-order logic based on the screw theory or the D-H parameter method.

优选地，所述机械臂为n自由度旋转机械臂，其参数为：α_i表示各旋转副转动轴之间的扭角，a_i表示相邻两旋转副之间的偏距量，即连杆长度，d_i表示两相邻旋转副的旋转轴之间公法线垂足之间的距离，θ_i表示旋转副旋转的角度，i＝1,2,3…n；Preferably, the mechanical arm is an n-degree-of-freedom rotating mechanical arm, and its parameters are: α _i represents the twist angle between the rotation axes of each rotary pair, and a _i represents the offset distance between two adjacent rotary pairs, that is, Rod length, d _i represents the distance between the vertical feet of the common normal between the rotation axes of two adjacent revolving pairs, θ _i represents the angle of rotation of the revolving pair, i=1,2,3...n;

根据上述参数依次确定各旋转关节的旋量坐标为：According to the above parameters, the screw coordinates of each rotary joint are determined in sequence as:

其中，表示旋量的原部，表示旋量的偶部，i＝1,2,3，旋量通过如下的方式确定：in, represents the primordial part of the spinor, Represents the even part of the screw, i=1,2,3, the screw is determined by the following method:

$\begin{matrix} {\overset{&OverBar; &OverBar;}{z z}}_{11} = = {((00,, 00,, 11))}^{T T} \\ {\overset{&OverBar; &OverBar;}{z z}}_{22} = = {e e}^{{θ θ}_{11} {\overset{^^}{z z}}_{11}} {z z}_{11} \\ {\overset{&OverBar; &OverBar;}{z z}}_{33} = = {e e}^{{θ θ}_{22} {\overset{^^}{z z}}_{22}} {e e}^{{θ θ}_{11} {\overset{^^}{z z}}_{11}} {z z}_{11} \\ ... ... \\ {\overset{&OverBar; &OverBar;}{z z}}_{n no} = = {e e}^{{θ θ}_{n no} {\overset{^^}{z z}}_{n no}} ... ... {e e}^{{θ θ}_{22} {\overset{^^}{z z}}_{22}} {e e}^{{θ θ}_{11} {\overset{^^}{z z}}_{11}} {z z}_{11} \end{matrix}$

$\begin{matrix} {\overset{&OverBar; &OverBar;}{p p}}_{11} = = {p p}_{11} = = {((00,, 00,, 00))}^{T T} \\ {\overset{&OverBar; &OverBar;}{p p}}_{22} = = {e e}^{{θ θ}_{11} {\overset{^^}{z z}}_{11}} {p p}_{22} \\ {\overset{&OverBar; &OverBar;}{p p}}_{33} = = {e e}^{{θ θ}_{22} {\overset{^^}{z z}}_{22}} {e e}^{{θ θ}_{11} {\overset{^^}{z z}}_{11}} {p p}_{33} \\ ... ... \\ {\overset{&OverBar; &OverBar;}{p p}}_{n no} = = {e e}^{{θ θ}_{n no} {\overset{^^}{z z}}_{n no}} ... ... {e e}^{{θ θ}_{22} {\overset{^^}{z z}}_{22}} {e e}^{{θ θ}_{11} {\overset{^^}{z z}}_{11}} {p p}_{33} \end{matrix}$

其中，z_i,p_i分别表示机械臂第i个旋转副在初始状态下的旋转轴和位置矢量。Among them, z _i , p _i represent the rotation axis and position vector of the i-th rotation joint of the manipulator in the initial state, respectively.

优选地，所述步骤二中构建的机构运动学逻辑模型为，Preferably, the mechanism kinematics logic model constructed in the second step is,

定义1.旋转副的旋量，Definition 1. The screw of the revolving joint,

val rotation_screw z p＝(z；p×z)val rotation_screw z p = (z; p × z)

其中，函数rotation_screw的输入变量z,p分别表示旋转副运动后的旋转轴和位置矢量，其返回值为旋转副的旋量，其数据类型为旋量；Among them, the input variables z and p of the function rotation_screw respectively represent the rotation axis and the position vector after the rotation pair moves, and the return value is the screw of the rotation pair, and its data type is a screw;

定义2.串联n旋转副机械臂：Definition 2. Serial n-rotation secondary mechanical arm:

valserial_ndof_MA₁A₂...A_n＝A₁+A₂+...+A_n valserial_ndof_MA ₁ A ₂ ... A _n = A ₁ + A ₂ + ... + A _n

其中，函数serial_ndof_M的输入变量为各旋转副的旋量，函数的返回值为机械臂末端执行器的等价旋量；Among them, the input variable of the function serial_ndof_M is the screw of each rotating pair, and the return value of the function is the equivalent screw of the end effector of the manipulator;

定义3.平面n自由度初始状态：Definition 3. Plane n degrees of freedom initial state:

val snm_initial_state＝(A₁,A₂,...,A_n)val snm_initial_state=(A ₁ ,A ₂ ,...,A _n )

其中，变量A₁,A₂,...,A_n为旋量，其原部分别表示各旋转副在初始状态下的旋转轴，其偶部分别表示各旋转副在初始状态下的位置矢量。Among them, the variables A ₁ , A ₂ ,...,A _n are screw quantities, and their original parts respectively represent the rotation axes of each revolving pair in the initial state, and their even parts respectively represent the position vectors of each revolving pair in the initial state .

优选地，所述机械臂为三自由度机械臂，所述步骤三中的运动学属性为末端执行器的旋转角度等于各旋转副的旋转角度之和；所述步骤四中的逻辑命题为：Preferably, the robotic arm is a three-degree-of-freedom robotic arm, and the kinematics attribute in step three is that the rotation angle of the end effector is equal to the sum of the rotation angles of each rotary pair; the logical proposition in step four is:

上述逻辑命题中，sin_x函数的输入变量为3维向量，函数的返回值为该向量与x轴的夹角的正弦函数值；cos_x函数的输入变量为3维向量，函数的返回值为该向量与x轴的夹角的余弦函数值；函数的功能是将对空间中的点或者向量绕固定轴r旋转角度θ，该函数返回值的数据类型为向量类型。In the above logical proposition, the input variable of the sin_x function is a 3-dimensional vector, and the return value of the function is the sine function value of the angle between the vector and the x-axis; the input variable of the cos_x function is a 3-dimensional vector, and the return value of the function is the vector The cosine function value of the angle with the x-axis; The function of the function is to rotate the point or vector in the space by an angle θ around the fixed axis r, and the data type of the return value of this function is a vector type.

优选地，所述机械臂为三自由度机械臂，所述步骤三中的运动学属性为末端执行器的可达位置位于半径为(a₁+a₂+a₃)圆形区域之内；所述步骤四中的逻辑命题为，Preferably, the robotic arm is a three-degree-of-freedom robotic arm, and the kinematics property in step 3 is that the accessible position of the end effector is located within a circular area with a radius of (a ₁ +a ₂ +a ₃ ); The logical proposition in the step 4 is,

其中，transf为定义的高阶逻辑函数，其功能为将4维向量类型的数据取前三位顺序排列构成新的3维向量。Among them, transf is a defined high-order logic function, and its function is to arrange the data of the 4-dimensional vector type in the order of the first three bits to form a new 3-dimensional vector.

优选地，所述机械臂为三自由度机械臂，所述步骤三中的运动学属性为末端执行器的可达区域分析；所述步骤四中的逻辑命题为，Preferably, the manipulator is a three-degree-of-freedom manipulator, and the kinematics attribute in step three is an analysis of the reachable area of the end effector; the logical proposition in step four is,

其中，在此逻辑命题中si_configuration(θ₁,θ₂,θ₃,Q)是判断机构是否奇异的补偿函数，如果命题的返回值为真，则机械臂的在该位置可达，反之则不可达。。Among them, si_configuration(θ ₁ ,θ ₂ ,θ ₃ ,Q) in this logic proposition is a compensation function for judging whether the mechanism is singular. If the return value of the proposition is true, the position of the manipulator is reachable, and vice versa. Da. .

和传统方法不同，形式化验证根据系统形式规范或属性，使用数学方法证明系统的正确性，对所验证的性质而言是精确和完备的。而基于刚体运动学发展而来的旋量理论具有物理意义明确、表达形式简单、代数运算方便等优点，大大降低了机器人机构学研究的难度，本发明中，通过应用旋量理论，只需建立两个坐标系，只要分析出当前位形下的各个运动副旋量，便可得到其雅可比矩阵，由于运动副旋量的线性相关性与坐标系的选择无关，还可以通过恰当选择坐标系进一步简化其雅可比的解析形式。Different from traditional methods, formal verification uses mathematical methods to prove the correctness of the system according to the formal specification or properties of the system, which is accurate and complete for the verified properties. The screw theory developed based on rigid body kinematics has the advantages of clear physical meaning, simple expression form, and convenient algebraic operation, which greatly reduces the difficulty of robot mechanism research. In the present invention, by applying the screw theory, only need to establish For the two coordinate systems, as long as the motion sub-screws in the current configuration are analyzed, the Jacobian matrix can be obtained. Since the linear correlation of the motion sub-screws has nothing to do with the selection of the coordinate system, it can also be obtained by properly selecting the coordinate system Further simplify the analytical form of its Jacobian.

附图说明Description of drawings

通过以下参照附图对本发明实施例的描述，本发明的上述以及其他目的、特征和优点将更为清楚，在附图中：Through the following description of the embodiments of the present invention with reference to the accompanying drawings, the above-mentioned and other objects, features and advantages of the present invention will be more clear, in the accompanying drawings:

图1为自由度为3的3R串联机械臂结构示意图；Figure 1 is a schematic diagram of the structure of a 3R series manipulator with 3 degrees of freedom;

图2为3R机械臂极限位置形式化验证流程图；Figure 2 is a flow chart of the formal verification of the limit position of the 3R manipulator;

图3为自由度为6的6R机械臂结构示意图。Figure 3 is a schematic diagram of the structure of a 6R manipulator with 6 degrees of freedom.

具体实施方式detailed description

以下将参照附图更详细地描述本发明的各种实施例。在各个附图中，相同的元件采用相同或类似的附图标记来表示。为了清楚起见，附图中的各个部分没有按比例绘制。Various embodiments of the invention will be described in more detail below with reference to the accompanying drawings. In the various drawings, the same elements are denoted by the same or similar reference numerals. For the sake of clarity, various parts in the drawings have not been drawn to scale.

刚体运动描述的是保持物体上任意两点距离不变的运动，它是研究机器人运动学、动力学、控制的核心。相比于简化的质点运动模型而言，刚体运动能将物理的运动描述得更为精确，因而物体的刚体运动模型在工程实际中应用得更为广泛。连续的刚体运动数学上表示为刚体变换。Rigid body motion describes the motion that keeps the distance between any two points on the object constant. It is the core of the study of robot kinematics, dynamics, and control. Compared with the simplified particle motion model, the rigid body motion can describe the physical motion more accurately, so the rigid body motion model of objects is more widely used in engineering practice. Continuous rigid body motion is mathematically represented as a rigid body transformation.

对于由的子集O描述的给定刚体，其运动可以用一系列的连续变换来描述，也就是说将刚体上各点相对于某个固定坐标系的运动描述为时间的函数。这一系列的变换被称为刚体变换。这些映射的全体构成一个6维的李群，称为特殊欧几里德群(SE(3))，简称特殊欧氏群。由于李群同时具有微分流形和群的性质，即所有刚体变换的全体所构成的集合既满足群的4个基本特征，也构成一个可微分的流形，具有可积可微性。基于刚体运动学发展而来的旋量理论，具有物理意义明确、表达形式简单、代数运算方便等优点，大大降低了机器人机构学研究的难度，本申请中，采用旋量理论来对机械臂运动学进行形式化分析。for by A given rigid body described by a subset O whose motion can be described by a series of continuous transformations To describe, that is to say, describe the motion of each point on the rigid body relative to a fixed coordinate system as a function of time. This series of transformations is called a rigid body transformation. The whole of these mappings constitutes a 6-dimensional Lie group, called the special Euclidean group (SE(3)), referred to as the special Euclidean group. Because the Lie group has the properties of differential manifold and group at the same time, that is, the set of all rigid body transformations not only satisfies the four basic characteristics of the group, but also constitutes a differentiable manifold, which has the property of integrability and differentiability. The screw theory developed based on rigid body kinematics has the advantages of clear physical meaning, simple expression form, and convenient algebraic operation, which greatly reduces the difficulty of robot mechanism research. In this application, the screw theory is used to control the movement of the mechanical arm to conduct formal analysis.

典型的机械臂机构如图1、图3所示(图1中示出了自由度为3的3R串联机械臂，图3中示出了自由度为6的6R串联机械臂)，其所有的运动副都是转动副，机械臂自由度的数目等于运动副的数目。Typical manipulator mechanisms are shown in Figure 1 and Figure 3 (Figure 1 shows a 3R series manipulator with 3 degrees of freedom, and Figure 3 shows a 6R series manipulator with 6 degrees of freedom), all of which The kinematic pairs are revolving joints, and the number of degrees of freedom of the mechanical arm is equal to the number of kinematic pairs.

基于旋量理论，机械臂的末端执行器的位形由各关节的运动旋量的线性组合来表示，即：Based on the screw theory, the configuration of the end effector of the manipulator is represented by the linear combination of the motion screws of each joint, namely:

上式中，ω₁，ω₂，…ω_j分别表示各个关节的角速度参量，表示各关节的单位旋量，各个旋量具有如下的特征：In the above formula, ω ₁ , ω ₂ , ... ω _j respectively represent the angular velocity parameters of each joint, Represents the unit screw of each joint, and each screw has the following characteristics:

根据旋量理论，旋量由2个三维矢量组成，一般写成对偶数的形式，如：According to the screw theory, the screw consists of two three-dimensional vectors, which are generally written in the form of even numbers, such as:

式中s和s⁰分别表旋量的原部和偶部，∈为对偶算子，满足：∈≠0，∈²＝∈³＝...＝0。旋量也可以写成Plucker坐标的形式，如：(s，s⁰)。In the formula, s and s ⁰ represent the primal part and the dual part of the spin, respectively, and ∈ is the dual operator, which satisfies: ∈≠0, ∈ ² =∈ ³ =...=0. The screw can also be written in the form of Plucker coordinates, such as: (s, s ⁰ ).

通过这两个三维矢量可以同时表示向量的方向和位置，如刚体运动中的速度和角速度、力与力矩等。在分析复杂的刚体运动时，运用旋量理论可以把问题的描述和解决变得十分的简洁统一，且易于和其他方法(如向量法、矩阵法)进行转换。Through these two three-dimensional vectors, the direction and position of the vector can be expressed simultaneously, such as the velocity and angular velocity, force and moment in rigid body motion. When analyzing complex rigid body motion, the use of screw theory can make the description and solution of the problem very concise and unified, and it is easy to convert with other methods (such as vector method and matrix method).

对串联机器人机构进行形式化；Formalize the tandem robot mechanism;

本申请中，使用剑桥大学研发的高阶逻辑定理证明器HOL4(Higer-Order-Logic)作为形式化工具。HOL4是最成熟的高阶逻辑定理证明器之一，拥有庞大的研究团队和用户群，在软硬件验证领域已有很多成功的应用。In this application, the high-order logic theorem prover HOL4 (Higer-Order-Logic) developed by Cambridge University is used as a formalization tool. HOL4 is one of the most mature high-order logic theorem provers, with a large research team and user base, and has many successful applications in the field of hardware and software verification.

在HOL4中，机械臂机构被形式化表示如下：In HOL4, the mechanical arm mechanism is formalized as follows:

定义：串联机器人机构Definition: Tandem robotic mechanism

$\begin{matrix} | | - - &ForAll; &ForAll; n A serial n A serial__manipulator n A manipulator n A = = \\ sc sc__SUM SUM ((count n count n)) A A \end{matrix}$

在上述定义中，函数serial_manipulator的输入变量n,A分别表示机构关节的个数n,各关节旋量所组成的序列An。函数的返回值为机械臂机构的和旋量。In the above definition, the input variables n and A of the function serial_manipulator respectively represent the number n of joints of the mechanism and the sequence An formed by the screw quantities of each joint. The return value of the function is the sum screw of the mechanical arm mechanism.

然后，基于各关节的运动参数，即各关节的转动角速度和位置矢量，各关节的旋量也可以随之确定。根据上述定义，6自由度的6R机械臂机构可以形式化表示如下：Then, based on the motion parameters of each joint, that is, the rotational angular velocity and position vector of each joint, the screw quantity of each joint can also be determined accordingly. According to the above definition, the 6R mechanical arm mechanism with 6 degrees of freedom can be formalized as follows:

$\begin{matrix} s the s e e r r i i a a l l__m m a a n no i i p p u u l l a a t t o o r r (({ω ω}_{11};; {ω ω}_{11} \times \times {p p}_{11})) (({ω ω}_{22};; {ω ω}_{22} \times \times {p p}_{22})) \\ (({ω ω}_{33};; {ω ω}_{33} \times \times {p p}_{33})) (({ω ω}_{44};; {ω ω}_{44} \times \times {p p}_{44})) \\ (({ω ω}_{55};; {ω ω}_{55} \times \times {p p}_{55})) (({ω ω}_{66};; {ω ω}_{66} \times \times {p p}_{66})) \\ = = (({ω ω}_{11} + + {ω ω}_{22} + + {ω ω}_{33} + + {ω ω}_{44} + + {ω ω}_{55} + + {ω ω}_{66};; {ω ω}_{11} \times \times {p p}_{11} + + {ω ω}_{22} \times \times {p p}_{22} + + {ω ω}_{33} \times \times {p p}_{33} + + \\ {ω ω}_{44} \times \times {p p}_{44} + + {ω ω}_{55} \times \times {p p}_{55} + + {ω ω}_{66} \times \times {p p}_{66})) \end{matrix}$

式中，ω_i表示各关节的转动角速度，P_i表示各关节的位置矢量。In the formula, ω _i represents the rotational angular velocity of each joint, and P _i represents the position vector of each joint.

根据各关节的旋量可以确定机构的旋量雅克比矩阵。The screw Jacobian matrix of the mechanism can be determined according to the screw of each joint.

定义：旋量雅克比矩阵Definition: Screw Jacobian Matrix

空间6R串联机械臂机构的雅克比矩阵被定义如下：The Jacobian matrix of the space 6R series manipulator mechanism is defined as follows:

$\begin{matrix} &ForAll; &ForAll; a a,, b b,, c c,, d d,, e e,, f f . . s the s c c__j j a a c c o o b b i i a a n no a a b b c c d d e e f f \\ = = s the s c c r r e e w w__22__m m a a b b c c d d e e f f \end{matrix}$

在上述定义中，函数sc_jacobian的输入变量为各关节的旋量，返回值为6×6的雅克比矩阵。In the above definition, the input variable of the function sc_jacobian is the screw of each joint, and the return value is a 6×6 Jacobian matrix.

雅克比矩阵是表征机构是否处于奇异位形的重要参数。当机构处于奇异位形时，末端执行器失控，可能引起不可预料的后果，因此在设计机器人机构时，应尽可能避免使机器人机构处于奇异位形状态。The Jacobian matrix is an important parameter to characterize whether the mechanism is in a singular configuration. When the mechanism is in a singular configuration, the end effector is out of control, which may cause unpredictable consequences. Therefore, when designing the robot mechanism, it should be avoided as much as possible to make the robot mechanism in a singular configuration.

如果雅克比矩阵的行列值为0，则说明机构处于奇异位形状态，生成错误的报告，结束本次形式化分析。If the row and column values of the Jacobian matrix are 0, it means that the organization is in a singular configuration state, and a wrong report is generated, and this formal analysis ends.

刚体运动通常以一种连续的变换——刚体变换来描述，这种变换满足如下两个条件：Rigid body motion is usually described by a continuous transformation - rigid body transformation, which satisfies the following two conditions:

(1)保持刚体上任意两点间的距离不变，对于任意的点均有：(1) Keep the distance between any two points on the rigid body constant, for any point Both have:

||g(p)-g(q)||＝||p-q||||g(p)-g(q)||＝||p-q||

(2)保持刚体上任意两点矢量间的相对姿态不变，对于任意的矢量，均有：(2) Keep the relative attitude between any two point vectors on the rigid body unchanged. For any vector, there are:

g(v×w)＝g(v)×g(w)g(v×w)=g(v)×g(w)

刚体运动须同时描述刚体上任意一点的平动及刚体绕该点的转动。其中，刚体的平动可以直接用向量来描述，而刚体的转动只改变刚体的姿态，刚体的姿态可以用附着在刚体上的动坐标{B}(物体坐标系)相对于参考坐标系{A}的相对姿态来描述。具体用别表示物体坐标系相对于参考坐标系的坐标，写成矩阵形式The motion of a rigid body must simultaneously describe the translation of any point on the rigid body and the rotation of the rigid body around that point. Among them, the translation of the rigid body can be directly described by vectors, while the rotation of the rigid body only changes the attitude of the rigid body, and the attitude of the rigid body can be relative to the reference coordinate system {A } to describe the relative posture. specific use Do not represent the coordinates of the object coordinate system relative to the reference coordinate system, written in matrix form

R＝[x_AB y_AB z_AB]R＝[x _AB y _AB z _AB ]

R被称为旋转矩阵，且满足如下的关系式：R is called a rotation matrix and satisfies the following relationship:

R＝[x_AB _yAB z_AB]，且det(R)＝1R=[x _AB _yAB z _AB ], and det(R)=1

针对于一般刚体运动，刚体上各点的运动情况都可以从物体坐标系的运动以及该点相对于物体坐标系的运动来得到，即：For general rigid body motion, the motion of each point on the rigid body can be obtained from the motion of the object coordinate system and the motion of the point relative to the object coordinate system, namely:

p_A＝Rp_B+tp _A =Rp _B +t

式中p_A是点p在参考坐标系中的坐标表示，p_B是点p在物体坐标系中的坐标表示，R,t分别表示旋转矩阵和位置矢量。In the formula, p _A is the coordinate representation of point p in the reference coordinate system, p _B is the coordinate representation of point p in the object coordinate system, and R and t represent the rotation matrix and position vector, respectively.

将上式变换成射影几何中齐次变换的表达形式：Transform the above formula into the expression form of homogeneous transformation in projective geometry:

$(\begin{matrix} {p p}_{A A} \\ 11 \end{matrix}) = = (\begin{matrix} R R & {t t}_{A A B B} \\ 00 & 11 \end{matrix}) (\begin{matrix} {p p}_{B B} \\ 11 \end{matrix})$

上式中， In the above formula,

称为刚体运动的齐次变换矩阵，为4×4方阵。It is called the homogeneous transformation matrix of rigid body motion, which is a 4×4 square matrix.

在HOL4中，齐次矩阵被定义如下：In HOL4, a homogeneous matrix is defined as follows:

定义：齐次矩阵Definition: Homogeneous matrix

此定义的类型为matrix→vector→matrix。The type of this definition is matrix→vector→matrix.

对于一般刚体运动而言，Chalses定理指出，任意刚体运动都可以通过螺旋运动即通过绕某轴的旋转与沿该轴移动的复合运动来实现，也就是说，运动旋量的指数映射与齐次变换矩阵：For general rigid body motion, the Chalses theorem points out that any rigid body motion can be realized by spiral motion, that is, through the compound motion of rotation around a certain axis and movement along the axis, that is, the exponential mapping of the motion screw with a homogeneous transformation matrix:

是等价的。 are equivalent.

如果刚体以单位角速度绕轴线ω在作匀速运动，那么刚体上一点p的速度可以表示为：If the rigid body is moving at a constant speed around the axis ω at a unit angular velocity, then the speed of a point p on the rigid body can be expressed as:

$\overset{\cdot &Center Dot;}{p p} ((t t)) = = ω ω \times \times ((p p ((t t)) - - r r))$

其中r表示位置矢量，引入如下的4×4矩阵 Where r represents the position vector, the following 4×4 matrix is introduced

$\overset{^^}{ξ ξ} = = (\begin{matrix} \overset{^^}{ω ω} & v v \\ 00 & 00 \end{matrix})$

其中，是反对称矩阵，满足且v＝-r×ω。由此，刚体上一点的速度满足如下的齐次方程：in, is an antisymmetric matrix, satisfying And v=-r×ω. Thus, the velocity of a point on a rigid body satisfies the following homogeneous equation:

$(\begin{matrix} \overset{\cdot &Center Dot;}{p p} \\ 11 \end{matrix}) = = (\begin{matrix} \overset{^^}{ω ω} & - - r r \times \times ω ω \\ 00 & 11 \end{matrix}) (\begin{matrix} p p \\ 11 \end{matrix}) = = \overset{^^}{ξ ξ} \overset{&OverBar; &OverBar;}{p p}$

也可以写成： can also be written as:

此微分方程的解为： The solution to this differential equation is:

上式中被称为运动旋量。In the above formula Known as a motion screw.

实施例一Embodiment one

本实施例中，以平面3自由度机械臂为例对本申请中的机械臂运动学形式化分析方法进行具体说明。In this embodiment, the formalized analysis method of the kinematics of the manipulator in this application is specifically described by taking a plane 3-DOF manipulator as an example.

如图1所示的平面3自由度机械臂的D-H参数如表一所示：The D-H parameters of the planar 3-DOF manipulator shown in Figure 1 are shown in Table 1:

表一平面3自由度机械臂的D-H参数Table 1 D-H parameters of the planar 3-DOF manipulator

关节joint α_i α _i a_i a _i d_i d _i θ_i θ _i 11 00 a₁ a ₁ a₁ a ₁ θ₁ θ ₁ 22 00 a₂ a ₂ a₂ a ₂ θ₂ θ ₂ 33 00 a₃ a ₃ a₃ a ₃ θ₃ θ ₃

表一中，α_i表示各旋转副转动轴之间的扭角，a_i表示相邻两旋转副之间的偏距量(连杆长度)，d_i表示两相邻旋转副的旋转轴之间公法线垂足之间的距离，θ_i表示旋转副旋转的角度。In Table 1, α _i represents the torsion angle between the rotation axes of each rotary pair, a _i represents the offset distance (connecting rod length) between two adjacent rotary pairs, and d _i represents the distance between the rotation axes of two adjacent rotary pairs. The distance between the vertical feet of the public normal, _θi represents the angle of rotation of the revolving pair.

根据D-H参数依次确定各旋转关节的旋量坐标Determine the screw coordinates of each rotary joint in turn according to the D-H parameters

其中，表示旋量的原部，表示旋量的偶部，i＝1,2,3。旋量可以通过如下的方式确定。in, represents the primordial part of the spinor, Represents the even part of the spinor, i=1,2,3. The screw can be determined as follows.

在HOL4中构建平面3自由度机械臂的高阶逻辑形式模型。Constructing a high-order logical form model of a planar 3-DOF manipulator in HOL4.

定义1.旋转副的旋量。Definition 1. The screw of a revolving joint.

val rotation_screw z p＝(z；p×z)val rotation_screw z p = (z; p × z)

其中，函数ration_screw的输入变量z,p分别表示旋转副运动后的旋转轴和位置矢量，其返回值为旋转副的旋量，其数据类型为旋量(3维矢量对)。Among them, the input variables z and p of the function ration_screw respectively represent the rotation axis and the position vector after the rotary joint movement, and the return value is the screw of the rotary joint, and its data type is a screw (3-dimensional vector pair).

定义2.移动副的旋量Definition 2. The screw of the moving pair

val prismatic_screw p＝(0；p)val prismatic_screw p = (0; p)

其中，函数prismatic_screw的输入变量p表示移动副移动的方向，其返回值为移动副的旋量。Among them, the input variable p of the function prismatic_screw represents the moving direction of the moving pair, and its return value is the screw quantity of the moving pair.

定义3.平面3自由度机械臂：Definition 3. Plane 3-DOF manipulator:

val planar_3dof_M A B C＝A+B+Cval planar_3dof_M A B C = A+B+C

其中，函数planar_3dof_M的输入变量为各旋转副的旋量，函数的返回值为机械臂末端执行器的等价旋量。Among them, the input variable of the function planar_3dof_M is the screw of each rotary pair, and the return value of the function is the equivalent screw of the end effector of the manipulator.

定义4.平面3自由度初始状态。Definition 4. Planar 3 DOF initial state.

val p3m_initial_state＝(A,B,C)val p3m_initial_state=(A,B,C)

其中，变量A,B,C为旋量，其原部分别表示各旋转副在初始状态下的旋转轴(3维矢量)，其偶部分别表示各旋转副在初始状态下的位置矢量。Among them, the variables A, B, and C are screw quantities, and their original parts represent the rotation axes (3-dimensional vectors) of each revolving pair in the initial state, and their even parts represent the position vectors of each revolving pair in the initial state.

相关属性验证Related attribute verification

由于机械臂设计需要达到的相关约束条件，或者说相关属性是多方面的，本申请中以其中的几个属性作为示例，说明本申请中的形式化分析方法的具体分析过程。Due to the relevant constraints that the design of the manipulator needs to meet, or the relevant attributes are multifaceted, this application uses several of these attributes as examples to illustrate the specific analysis process of the formal analysis method in this application.

一、属性1：如图1所示的平面3自由度机械臂末端执行器的旋转角度等于各旋转副的旋转角度之和。1. Attribute 1: As shown in Figure 1, the rotation angle of the end effector of the three-degree-of-freedom manipulator in the plane is equal to the sum of the rotation angles of each rotation pair.

建立属性1的形式化命题如下：The formalized proposition that establishes attribute 1 is as follows:

上述逻辑命题中，r，a,b,c是平面3自由度机构初始状态函数的输入值，r＝(0,0,1)为机构3个旋转副在初始状态下的旋转轴，a,b,c分别表示3个旋转副在初始状态下的位置矢量，命题的前件形式化表征了机构初始化的过程。sin_x是HOL4中定义的一个高阶逻辑函数，函数的输入变量为3维向量，函数的返回值为该向量与x轴的夹角的正弦函数值。cos_x是HOL4中定义的一个高阶逻辑函数，函数的输入变量为3维向量，函数的返回值为该向量与x轴的夹角的余弦函数值。是HOL4中定义的一个高阶逻辑函数，其功能将对空间中的点或者向量绕固定轴r旋转角度θ，该函数返回值的数据类型为向量类型。In the above logical proposition, r, a, b, c are the input values of the initial state function of the planar 3-DOF mechanism, r=(0,0,1) are the rotation axes of the three revolving pairs of the mechanism in the initial state, a, b and c represent the position vectors of the three revolving joints in the initial state respectively, and the antecedent of the proposition formally characterizes the initialization process of the mechanism. sin_x is a high-order logic function defined in HOL4. The input variable of the function is a 3-dimensional vector, and the return value of the function is the sine function value of the angle between the vector and the x-axis. cos_x is a high-order logic function defined in HOL4. The input variable of the function is a 3-dimensional vector, and the return value of the function is the cosine function value of the angle between the vector and the x-axis. It is a high-order logic function defined in HOL4. Its function will rotate a point or vector in space by an angle θ around a fixed axis r. The data type of the return value of this function is a vector type.

将以上逻辑命题在HOL4的高阶逻辑引擎下进行推理，该命题的返回值为true，即属性1在定理证明器HOL4中验证通过，命题的正确性得到了验证。The above logical proposition is reasoned under the high-order logic engine of HOL4, and the return value of the proposition is true, that is, attribute 1 is verified in the theorem prover HOL4, and the correctness of the proposition is verified.

二、属性2：平面3自由度机械臂末端执行器可达域的分析与验证。2. Attribute 2: Analysis and verification of the reachable domain of the end effector of the planar 3-DOF manipulator.

通过如图2的流程可以对平面3自由度机械臂末端执行器的可达域进行分析与验证。Through the process shown in Figure 2, the reachable domain of the end effector of the planar 3-DOF manipulator can be analyzed and verified.

首先确定机构参数，然后构建逻辑模型，再确定机构极限位姿First determine the mechanism parameters, then build a logical model, and then determine the limit pose of the mechanism

判断目标位置是否超出极限位置，如果超出，则判断为不可达，如果没有超出，则进一步判断是否为机构奇异位形，如果是奇异位形，则判断为不可达，不是奇异位形，则判断为可达。Determine whether the target position exceeds the limit position. If it exceeds, it is judged as unreachable. If it does not exceed, it is further judged whether it is a singular configuration of the mechanism. If it is a singular configuration, it is judged as unreachable. If it is not a singular configuration, it is judged for reachable.

2.1根据机构D-H参数确定机构的齐次变换矩阵，并将其形式化。2.1 Determine the homogeneous transformation matrix of the mechanism according to the D-H parameters of the mechanism and formalize it.

机构的齐次变换矩阵可以通过逐级计算关节的D-H变换矩阵确定。The homogeneous transformation matrix of the mechanism can be determined by calculating the D-H transformation matrix of the joints step by step.

关节1的D-H变换矩阵为The D-H transformation matrix of joint 1 is

${A A}_{00}^{11} = = [\begin{matrix} {cθ cθ}_{11} & - - {sθ sθ}_{11} & 00 & {a a}_{11} {cθ cθ}_{11} \\ {sθ sθ}_{11} & {cθ cθ}_{11} & 00 & {a a}_{11} {sθ sθ}_{11} \\ 00 & 00 & 11 & 00 \\ 00 & 00 & 00 & 11 \end{matrix}] - - - - - - ((11))$

关节2的D-H变换矩阵为The D-H transformation matrix of joint 2 is

${A A}_{11}^{22} = = [\begin{matrix} {cθ cθ}_{22} & - - {sθ sθ}_{22} & 00 & {a a}_{22} {cθ cθ}_{22} \\ {sθ sθ}_{22} & {cθ cθ}_{22} & 00 & {a a}_{22} {sθ sθ}_{22} \\ 00 & 00 & 11 & 00 \\ 00 & 00 & 00 & 11 \end{matrix}] - - - - - - ((22))$

关节3的D-H变换矩阵为The D-H transformation matrix of joint 3 is

${A A}_{22}^{33} = = [\begin{matrix} {cθ cθ}_{33} & - - {sθ sθ}_{33} & 00 & {a a}_{33} {cθ cθ}_{33} \\ {sθ sθ}_{33} & {cθ cθ}_{33} & 00 & {a a}_{33} {sθ sθ}_{33} \\ 00 & 00 & 11 & 00 \\ 00 & 00 & 00 & 11 \end{matrix}] ((33))$

则整个机构的齐次变换矩阵为Then the homogeneous transformation matrix of the whole mechanism is

${A A}_{00}^{33} = = {A A}_{00}^{11} {A A}_{11}^{22} {A A}_{22}^{33} = = [\begin{matrix} {cθ cθ}_{123123} & - - {sθ sθ}_{123123} & 00 & {a a}_{11} {cθ cθ}_{11} + + {a a}_{22} {cθ cθ}_{1212} + + {a a}_{33} {cθ cθ}_{123123} \\ {sθ sθ}_{123123} & {cθ cθ}_{123123} & 00 & {a a}_{11} {sθ sθ}_{11} + + {a a}_{22} {sθ sθ}_{1212} + + {a a}_{33} {sθ sθ}_{123123} \\ 00 & 00 & 11 & 00 \\ 00 & 00 & 00 & 11 \end{matrix}] - - - - - - ((44))$

其中，cθ＝cosθ,sθ＝sinθ,θ₁₂＝θ₁+θ₂。Wherein, cθ=cosθ, sθ=sinθ, θ ₁₂ =θ ₁ +θ ₂ .

上述求齐次变换矩阵的过程可以在HOL4构造相应的逻辑命题如下：The above process of finding a homogeneous transformation matrix can construct the corresponding logical proposition in HOL4 as follows:

其中，依次是按照(1)(2)(3)(4)式构造的高阶逻辑函数，这些函数的输入变量为θ₁,θ₂,θ₃，函数的返回值为4×4的矩阵。该命题在HOL4中推理的返回值为true。in, In turn are the higher-order logic functions constructed according to (1)(2)(3)(4). The input variables of these functions are θ ₁ , θ ₂ , θ ₃ , and the return value of the function is a 4×4 matrix. The return value of this proposition inference in HOL4 is true.

确定机构末端执行器的极限位置Determining the Limit Positions of Mechanism End Effectors

末端执行器的极限位姿区域为半径为(a₁+a₂+a₃)圆形区域。因此末端执行器的可能可达位置不可能位于圆形区域以外。The limit pose region of the end effector is a circular region with a radius of (a ₁ +a ₂ +a ₃ ). Therefore the possible accessible positions of the end effector cannot lie outside the circular area.

此属性的可以形式化为如下的命题。This property can be formalized as the following proposition.

在形式化推理过程中，goal3最终转换如下的命题,In the process of formal reasoning, goal3 finally converts the following propositions,

命题的返回值为true,该命题验证通过。The return value of the proposition is true, and the proposition is verified.

2.2可达性分析。2.2 Accessibility analysis.

基于上述步骤，并且结合图2的验证流程，可以构造末端执行器可达区域验证的高阶逻辑验证命题如下。Based on the above steps, and combined with the verification process in Figure 2, the high-order logic verification proposition for the verification of the reachable area of the end effector can be constructed as follows.

其中，在此逻辑命题中si_configuration(θ₁,θ₂,θ₃,Q)是一个判断机构是否奇异的补偿函数，对于不同的机构此补偿函数的形式也不同，有些奇异位形甚至无法用确定的数学表达式来表达，但在实际机器人控制过程中，奇异位形是需要尽可能避免的，并且是否处于奇异位形是不容易判断的。对于goal4而言，如果命题的返回值为真，则机械臂的在该位置可达，反之则不可达。Among them, si_configuration(θ ₁ ,θ ₂ ,θ ₃ ,Q) in this logical proposition is a compensation function for judging whether the mechanism is singular. The form of the compensation function is different for different mechanisms, and some singular configurations cannot even be determined by However, in the actual robot control process, the singular configuration needs to be avoided as much as possible, and it is not easy to judge whether it is in the singular configuration. For goal4, if the return value of the proposition is true, the position of the manipulator is reachable, otherwise it is not reachable.

实施例二Embodiment two

本实施例中，以一种6R机器人的奇异位形进行判定In this embodiment, a singular configuration of a 6R robot is used for judgment

图3示出了一种6R机器人处于奇异位形的状态图，其中标黑的旋转副处于奇异位形，该机构具有如下的特殊参数：Fig. 3 shows a state diagram of a 6R robot in a singular configuration, where the black-marked rotary joint is in a singular configuration, and this mechanism has the following special parameters:

关节joint z_i z _i θ_i _θi d_i d _i 11 (0,0,1)^T (0,0,1) ^T 任意角度any angle 11 22 (0,1,0)^T (0,1,0) ^T 00 11 33 (0,0,1)^T (0,0,1) ^T 任意角度any angle 11 44 任取Take any 任意角度any angle 11 55 任取Take any 任意角度any angle 11 66 任取Take any 任意角度any angle 11

表2 6R机器人的一种奇异位形参数Table 2 A singular configuration parameter of the 6R robot

表2中，z_i分别表示机构各关节的旋转轴，θ_i表示机构各关节的旋转角度，In Table 2, z _i represent the rotation axis of each joint of the mechanism, θ _i represents the rotation angle of each joint of the mechanism,

d_i表示连杆长度。d _i represents the length of the connecting rod.

其中，表示旋量的原部，表示旋量的偶部，i＝1,2,3,4,5,6，。旋量可以通过如下的方式确定。in, represents the primordial part of the spinor, Represents the even part of the screw, i=1,2,3,4,5,6,. The screw can be determined as follows.

确定该机构的速度雅克比具有如下的形式：Determining the velocity Jacobian of the body has the following form:

$(\begin{matrix} {((00,, 00,, 11))}^{T T} & {e e}^{{θ θ}_{11} {\overset{^^}{z z}}_{11}} {z z}_{11} & {((00,, 00,, k k))}^{T T} & {e e}^{{θ θ}_{33} {\overset{^^}{z z}}_{33}} {e e}^{{θ θ}_{22} {\overset{^^}{z z}}_{22}} {e e}^{{θ θ}_{11} {\overset{^^}{z z}}_{11}} {z z}_{11} & {e e}^{{θ θ}_{44} {\overset{^^}{z z}}_{44}} {e e}^{{θ θ}_{33} {\overset{^^}{z z}}_{33}} {e e}^{{θ θ}_{22} {\overset{^^}{z z}}_{22}} {e e}^{{θ θ}_{11} {\overset{^^}{z z}}_{11}} {z z}_{11} & {e e}^{{θ θ}_{55} {\overset{^^}{z z}}_{55}} {e e}^{{θ θ}_{44} {\overset{^^}{z z}}_{44}} {e e}^{{θ θ}_{33} {\overset{^^}{z z}}_{33}} {e e}^{{θ θ}_{22} {\overset{^^}{z z}}_{22}} {e e}^{{θ θ}_{11} {\overset{^^}{z z}}_{11}} {z z}_{11} \\ {((00,, 00,, 11))}^{T T} & {e e}^{{θ θ}_{11} {\overset{^^}{z z}}_{11}} {p p}_{22} \times \times {e e}^{{θ θ}_{11} {\overset{^^}{z z}}_{11}} {z z}_{11} & {((00,, 00,, 00))}^{T T} & \begin{matrix} {e e}^{{θ θ}_{33} {\overset{^^}{z z}}_{33}} {e e}^{{θ θ}_{22} {\overset{^^}{z z}}_{22}} {e e}^{{θ θ}_{11} {\overset{^^}{z z}}_{11}} {p p}_{44} \times \times \\ {e e}^{{θ θ}_{33} {\overset{^^}{z z}}_{33}} {e e}^{{θ θ}_{22} {\overset{^^}{z z}}_{22}} {e e}^{{θ θ}_{11} {\overset{^^}{z z}}_{11}} {z z}_{11} \end{matrix} & \begin{matrix} {e e}^{{θ θ}_{44} {\overset{^^}{z z}}_{44}} {e e}^{{θ θ}_{33} {\overset{^^}{z z}}_{33}} {e e}^{{θ θ}_{22} {\overset{^^}{z z}}_{22}} {e e}^{{θ θ}_{11} {\overset{^^}{z z}}_{11}} {p p}_{55} \times \times \\ {e e}^{{θ θ}_{44} {\overset{^^}{z z}}_{44}} {e e}^{{θ θ}_{33} {\overset{^^}{z z}}_{33}} {e e}^{{θ θ}_{22} {\overset{^^}{z z}}_{22}} {e e}^{{θ θ}_{11} {\overset{^^}{z z}}_{11}} {z z}_{11} \end{matrix} & \begin{matrix} {e e}^{{θ θ}_{55} {\overset{^^}{z z}}_{55}} {e e}^{{θ θ}_{44} {\overset{^^}{z z}}_{44}} {e e}^{{θ θ}_{33} {\overset{^^}{z z}}_{33}} {e e}^{{θ θ}_{22} {\overset{^^}{z z}}_{22}} {e e}^{{θ θ}_{11} {\overset{^^}{z z}}_{11}} {p p}_{66} \times \times \\ {e e}^{{θ θ}_{55} {\overset{^^}{z z}}_{55}} {e e}^{{θ θ}_{44} {\overset{^^}{z z}}_{44}} {e e}^{{θ θ}_{33} {\overset{^^}{z z}}_{33}} {e e}^{{θ θ}_{22} {\overset{^^}{z z}}_{22}} {e e}^{{θ θ}_{11} {\overset{^^}{z z}}_{11}} {z z}_{11} \end{matrix} \end{matrix})$

在HOL4中构造形如上式的6×6的矩阵函数，调用HOL4库有关行列式的函数，构造如下的命题：Construct a 6×6 matrix function of the above formula in HOL4, call the determinant-related functions of the HOL4 library, and construct the following proposition:

其中，Jacobian函数的输入值为各关节的旋量，返回值为6×6的矩阵。6R机器人初始状态具有如下的定义：Among them, the input value of the Jacobian function is the screw of each joint, and the return value is a 6×6 matrix. The initial state of the 6R robot has the following definition:

定义5 6R机器人初始状态Definition 5. Initial state of 6R robot

val 6r_initial_state＝(A,B,C,D,E,F)val 6r_initial_state=(A,B,C,D,E,F)

其中，变量A,B,C,D,E,F为旋量，其原部分别表示各旋转副在初始状态下的旋转轴(3维矢量)，其偶部分别表示各旋转副在初始状态下的位置矢量。Among them, the variables A, B, C, D, E, and F are screw quantities, and their original parts respectively represent the rotation axes (3-dimensional vectors) of each revolving pair in the initial state, and their even parts respectively represent the rotation axes of each revolving pair in the initial state The position vector below.

显然，机构的速度雅克比的行列式值为零，goal5的返回值为false,即该机构处于奇异位形。Obviously, the determinant value of the velocity Jacobian of the mechanism is zero, and the return value of goal5 is false, that is, the mechanism is in a singular configuration.

最后应说明的是：显然，上述实施例仅仅是为清楚地说明本发明所作的举例，而并非对实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。而由此所引申出的显而易见的变化或变动仍处于本发明的保护范围之中。Finally, it should be noted that obviously, the above-mentioned embodiments are only examples for clearly illustrating the present invention, rather than limiting the implementation. For those of ordinary skill in the art, other changes or changes in different forms can be made on the basis of the above description. It is not necessary and impossible to exhaustively list all the implementation manners here. However, the obvious changes or variations derived therefrom are still within the protection scope of the present invention.

Claims

1. A mechanical arm kinematics formalization analysis method is characterized by comprising the following steps:

determining structural parameters of a mechanical arm mechanism;

step two, establishing a mechanical arm mechanism kinematics logic model by using a high-order logic language;

step three, describing the kinematic constraint or attribute to be verified by using a logic formula;

step four, a mechanical arm mechanism kinematics logic model and kinematics constraint or attribute to be verified form a logic proposition;

step five, a logic reasoning engine is used for proving whether the logic proposition is established, the establishment indicates that the model meets the constraint or has the attribute, otherwise, the establishment does not indicate that the model does not meet the constraint or does not have the attribute;

in the second step, the mechanical arm mechanism kinematics logic model is a high-order logic formal expression based on a momentum theory or a D-H parameter method.

2. The method for the kinematic formalized analysis of a robotic arm according to claim 1, wherein the robotic arm is an n-DOF rotating arm with parameters of α_iRepresenting the torsion angle between the axes of rotation of the revolute pairs, a_iRepresenting the offset between two adjacent pairs of rotation, i.e. the length of the connecting rod, d_iRepresents the distance between the common normal vertical feet between the rotating shafts of two adjacent rotating pairs, theta_iRepresents the angle of rotation of the revolute pair, i is 1,2,3 … n;

and sequentially determining the rotation amount coordinates of each rotary joint according to the parameters as follows:

wherein,the original part of the rotation amount is shown,an even part indicating a momentum, i ═ 1,2,3, and the momentum is determined as follows:

\begin{matrix} {\overset{&OverBar;}{z}}_{1} = {(0, 0, 1)}^{T} \\ {\overset{&OverBar;}{z}}_{2} = e^{θ_{1} {\hat{z}}_{1}} z_{1} \\ {\overset{&OverBar;}{z}}_{3} = e^{θ_{2} {\hat{z}}_{2}} e^{θ_{1} {\hat{z}}_{1}} z_{1} \\ ... \\ {\overset{&OverBar;}{z}}_{n} = e^{θ_{n} {\hat{z}}_{n}} ... e^{θ_{2} {\hat{z}}_{2}} e^{θ_{1} {\hat{z}}_{1}} z_{1} \end{matrix}

\begin{matrix} {\overset{&OverBar;}{p}}_{1} = p_{1} = {(0, 0, 0)}^{T} \\ {\overset{&OverBar;}{p}}_{2} = e^{θ_{1} {\hat{z}}_{1}} p_{2} \\ {\overset{&OverBar;}{p}}_{3} = e^{θ_{2} {\hat{z}}_{2}} e^{θ_{1} {\hat{z}}_{1}} p_{3} \\ ... \\ {\overset{&OverBar;}{p}}_{n} = e^{θ_{n} {\hat{z}}_{n}} ... e^{θ_{2} {\hat{z}}_{2}} e^{θ_{1} {\hat{z}}_{1}} p_{3} \end{matrix}

wherein z is_i,p_iRespectively represents the rotation axis and the position vector of the ith rotation pair of the mechanical arm in the initial state.

3. The method for analyzing the kinematic formality of the mechanical arm according to claim 2, wherein the kinematic logic model of the mechanism constructed in the second step is,

definition 1. the amount of rotation of the revolute pair,

val rotation_screw z p＝(z；p×z)

wherein, the input variable z, p of the function rotation _ scan respectively represents the rotating shaft and the position vector after the rotating pair moves, the return value is the rotation of the rotating pair, and the data type is the rotation;

definition 2. series connection n rotating pair mechanical arm:

valserial_ndof_M A₁A₂...A_n＝A₁+A₂+...+A_n

wherein, the input variable of the function serial _ ndof _ M is the rotation quantity of each rotating pair, and the return value of the function is the equivalent rotation quantity of the mechanical arm end effector;

definition 3. initial state of planar n degrees of freedom:

val snm_initial_state＝(A₁,A₂,...,A_n)

wherein, the variable A₁,A₂,...,A_nThe original part of the rotation amount represents the rotation axis of each rotation pair in the initial state, and the even part represents the position vector of each rotation pair in the initial state.

4. The method for analyzing the kinematic formality of a mechanical arm according to claim 3, wherein the mechanical arm is a three-degree-of-freedom mechanical arm, and the kinematic property in the third step is that the rotation angle of the end effector is equal to the sum of the rotation angles of the rotation pairs; the logic problem in the fourth step is as follows:

in the logic proposition, an input variable of a sin _ x function is a 3-dimensional vector, and a return value of the function is a sine function value of an included angle between the vector and an x axis; the input variable of the cos _ x function is a 3-dimensional vector, and the return value of the function is a cosine function value of an included angle between the vector and the x axis;the function of the function is to rotate a point or vector in space by an angle θ about a fixed axis r, the data type of the return value of the function being a vector type.

5. The method of claim 3, wherein the robotic arm is a three-degree-of-freedom robotic arm, and the kinematic property in step three is that the reachable position of the end effector is located at a radius (a)₁+a₂+a₃) Within the circular region; the logical theme in step four is,

wherein, the transf is a defined high-order logic function, and the function of the defined high-order logic function is to arrange the data of the 4-dimensional vector type into a new 3-dimensional vector by taking the first three bits to be arranged in sequence.

6. The method for kinematic formalization analysis of a mechanical arm according to claim 3, wherein the mechanical arm is a three-degree-of-freedom mechanical arm, and the kinematic property in step three is the reachable area analysis of the end effector; the logical theme in step four is,

wherein si _ configuration (θ) in the logical proposition₁,θ₂,θ₃And Q) is a compensation function for judging whether the mechanism is singular or not, if the return value of the proposition is true, the mechanical arm can be reached at the position, and if not, the mechanical arm cannot be reached.