CN107756400A - A kind of 6R Robotic inverse kinematics geometry solving methods based on spinor theory - Google Patents

A kind of 6R Robotic inverse kinematics geometry solving methods based on spinor theory Download PDF

Info

Publication number
CN107756400A
CN107756400A CN201710953599.4A CN201710953599A CN107756400A CN 107756400 A CN107756400 A CN 107756400A CN 201710953599 A CN201710953599 A CN 201710953599A CN 107756400 A CN107756400 A CN 107756400A
Authority
CN
China
Prior art keywords
msub
mrow
mtd
mtr
msup
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201710953599.4A
Other languages
Chinese (zh)
Other versions
CN107756400B (en
Inventor
刘志峰
许静静
赵永胜
蔡力钢
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing University of Technology
Original Assignee
Beijing University of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing University of Technology filed Critical Beijing University of Technology
Priority to CN201710953599.4A priority Critical patent/CN107756400B/en
Publication of CN107756400A publication Critical patent/CN107756400A/en
Application granted granted Critical
Publication of CN107756400B publication Critical patent/CN107756400B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/1605—Simulation of manipulator lay-out, design, modelling of manipulator
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • B25J19/007—Means or methods for designing or fabricating manipulators
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • Mechanical Engineering (AREA)
  • Theoretical Computer Science (AREA)
  • Robotics (AREA)
  • Computational Mathematics (AREA)
  • Databases & Information Systems (AREA)
  • Algebra (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Operations Research (AREA)
  • Automation & Control Theory (AREA)
  • External Artificial Organs (AREA)
  • Manipulator (AREA)
  • Numerical Control (AREA)

Abstract

The invention discloses a kind of 6R Robotic inverse kinematics geometry solving methods based on spinor theory, belong to the inverse solution technique study field of robot kinematics.Basis coordinates system and tool coordinates system are established, 6R robot kinematics' parameters are determined by basis coordinates system and tool coordinates system, and establish positive kinematics model.The inverse solution Kinematic Decomposition in first three joint of 6R robots is described and establishes hexa-atomic quadratic equation group.Based on spinor positive kinematics model solution initial position qs1Corresponding target location qe1.This method is combined geometric description with spinor theory, and geometric meaning definitely, is solved for Algebraic Equation set by simplifying inverse arithmetic, effectively improves computational efficiency, can be that robot motion controls a kind of new inverse solution processing method of offer in real time.

Description

一种基于旋量理论的6R机器人逆运动学几何求解方法A geometric solution method for inverse kinematics of 6R robot based on screw theory

技术领域technical field

本发明属于机器人运动学逆解方法研究领域，尤其涉及一种基于旋量理论的6R机器人逆运动学几何求解方法。The invention belongs to the research field of robot kinematics inverse solution method, in particular to a 6R robot inverse kinematics geometric solution method based on screw theory.

背景技术Background technique

运动学分析是实现运动控制的基础，主要建立关节变量与末端位姿的映射模型。其中逆解问题，即已知末端位姿求解关节变量的问题，是机器人领域研究热点之一，其求解效率直接影响着机器人运动控制的实时性能。目前机器人运动学逆解建模主要基于D-H法和旋量理论，研究者们对比了两种方法并发现后者的应用具有以下优点：可避免建立局部坐标系，简化计算模型并克服了局部参数产生的奇异性；其几何意义明确，可方便的确定产生多解的条件和个数。目前基于旋量描述的Paden-Kahan子问题被广泛的机器人逆向求解问题中，但该方法不适用于任意构型的六自由度机器人，因此多数学者基于三种基本子问题进行了延伸并提出了一些新的子问题模型，如Tan对子问题二进行了改进建立了针对“绕两个不相交轴的旋转运动”子问题的数学模型；Chen描述了一种“绕三个轴线旋转的运动，其中两个轴线平行且与第三轴线异面”的子问题，针对其逆解进行了详细的旋量描述。基于这些子问题模型在六自由度串联机器人逆解问题中得到了广泛应用，研究表明当6R机器人满足Piper准则时其逆运动学问题具有封闭解。Sariyildi等基于三个子问题实现了6R机器人的逆运动学求解；吕世增等通过在旋量法中引入吴方法减少了对子问题的依赖性，并将6R机器人的逆解问题转化为六元八次方程组的求解问题。为了进一步简化6R机器人逆解模型，本方法通过几何描述方法将6R机器人的逆解问题转化为一个六元二次方程组和一个三元二次方程组的求解，从而使其几何意义更加清晰，求解过程更加简单。Kinematics analysis is the basis for realizing motion control, and mainly establishes a mapping model between joint variables and terminal poses. Among them, the inverse solution problem, that is, the problem of solving joint variables with known terminal poses, is one of the research hotspots in the field of robotics, and its solution efficiency directly affects the real-time performance of robot motion control. At present, the inverse modeling of robot kinematics is mainly based on the D-H method and the screw theory. The researchers compared the two methods and found that the application of the latter has the following advantages: it can avoid the establishment of a local coordinate system, simplify the calculation model and overcome the problems caused by local parameters. Singularity; its geometric meaning is clear, and it is convenient to determine the conditions and numbers of multiple solutions. At present, the Paden-Kahan subproblem based on the screwer description is widely used in the reverse solution of robots, but this method is not suitable for any configuration of six-degree-of-freedom robots. Therefore, most scholars have extended based on three basic subproblems and proposed Some new sub-problem models, such as Tan improved sub-problem 2 and established a mathematical model for the sub-problem of "rotational motion around two disjoint axes"; Chen described a "rotational motion around three axes, For the subproblem where two axes are parallel and different from the third axis", a detailed screwor description is given for its inverse solution. Based on these sub-problem models, the inverse kinematics problem of the 6R robot has a closed solution when it satisfies the Piper criterion. Sariyildi et al. realized the inverse kinematics solution of the 6R robot based on three sub-problems; Lu Shizeng et al. reduced the dependence on the sub-problem by introducing the Wu method in the screw method, and transformed the inverse solution problem of the 6R robot into a six-element eight-degree Problem solving for systems of equations. In order to further simplify the inverse solution model of the 6R robot, this method converts the inverse solution problem of the 6R robot into a solution of a quadratic equation in six variables and a quadratic equation in three variables through the geometric description method, so that its geometric meaning is clearer. The solution process is simpler.

发明内容Contents of the invention

本发明的目的旨在提供一种基于旋量理论的6R机器人逆运动学几何求解方法。该方法的主要特点是通过结合几何描述方法和旋量理论，针对6R机器人运动学逆解问题提出一种几何意义清晰、求解过程简单的逆运动学模型。The purpose of the present invention is to provide a method for solving the inverse kinematics geometry of a 6R robot based on screw theory. The main feature of this method is to propose an inverse kinematics model with clear geometric meaning and simple solution process for the inverse solution problem of 6R robot kinematics by combining geometric description method and screw theory.

为实现上述目的，本发明采用的技术手段为一种基于旋量理论的6R机器人逆运动学几何求解方法，该方法的实现过程如下：In order to achieve the above object, the technical means adopted in the present invention is a 6R robot inverse kinematics geometric solution method based on the screw theory, and the realization process of the method is as follows:

S1、建立基坐标系与工具坐标系，通过基坐标系与工具坐标系确定6R机器人运动学参数，并建立正运动学模型。S1. Establish a base coordinate system and a tool coordinate system, determine the kinematic parameters of the 6R robot through the base coordinate system and the tool coordinate system, and establish a forward kinematics model.

S2、如图1，将6R机器人的前三个关节的逆解运动分解描述：由点qe到点c2＝[x2 y2z2]绕关节1的旋转运动；点c2到点c1＝[x1 y1 z1]绕关节2的旋转运动；点c1到点qs绕关节3的旋转运动。其中qs，qe分别为机器人末端的初始和目标位置，根据图1所示的几何关系描述建立以x1,y1,z1,x2,y2和z2为变量的六元二次方程组。S2. As shown in Figure 1, decompose and describe the inverse motion of the first three joints of the 6R robot: from point q e to point c 2 =[x 2 y 2 z 2 ] rotation around joint 1; from point c 2 to point c 1 =[x 1 y 1 z 1 ] rotation around joint 2; point c 1 to point q s rotation around joint 3. Among them, q s and q e are the initial and target positions of the robot end respectively. According to the description of the geometric relationship shown in Fig. 1 , a six - element two secondary equations.

S3、采用MATLAB中“SOLVE”函数求解以上方程组，得到c1和c2的坐标矢量。S3. Using the "SOLVE" function in MATLAB to solve the above equations to obtain the coordinate vectors of c1 and c2 .

S4、自此，前三个关节的逆解运动始末位置已知，基于Paden-Kahan子问题1分别建立6R机器人的前三个关节角变量θ1，θ2和θ3的显式求解模型，并进行求解。S4. Since then, the starting and ending positions of the inverse motion of the first three joints are known, and the explicit solution models of the first three joint angle variables θ 1 , θ 2 and θ 3 of the 6R robot are respectively established based on Paden-Kahan sub-problem 1, and solve it.

S5、在关节6的轴线上取一点作为后三个关节逆解运动中的初始位置qs1，并基于旋量正运动学模型求解初始位置qs1对应的目标位置qe1。S5. Take a point on the axis of joint 6 as the initial position q s1 in the inverse solution motion of the last three joints, and solve the target position q e1 corresponding to the initial position q s1 based on the screw forward kinematics model.

S6、如图2，将关节4和5的逆解运动分解描述：由点qe1到点c3＝[x3y3z3]的旋转运动；由点c3到点qs1的旋转运动。根据图2示，根据几何关系建立以x3,y3和z3为变量的三元二次方程组，并采用“SOLVE”函数求解，得到c3的坐标矢量。S6, as shown in Figure 2, decompose and describe the inverse motion of joints 4 and 5: the rotational motion from point q e1 to point c 3 =[x 3 y 3 z 3 ]; the rotational motion from point c 3 to point q s1 . As shown in Figure 2, a ternary quadratic equation system with x 3 , y 3 and z 3 as variables is established according to the geometric relationship, and the "SOLVE" function is used to solve it to obtain the coordinate vector of c 3 .

S7、自此，关节4和关节5的逆解运动始末位置已知，基于Paden-Kahan子问题1分别建立关节4和关节5的关节角变量θ4和θ5显式求解模型，并进行求解。S7. Since then, the starting and ending positions of joint 4 and joint 5 are known, and the joint angle variables θ 4 and θ 5 of joint 4 and joint 5 are respectively established based on Paden-Kahan sub-problem 1 to explicitly solve the model and solve .

S8、取不在关节6轴线上的任意一点qs2，基于旋量正运动学模型求解qs2对应的目标位置qe2，则关节6的逆解运动始末位置已知。同理基于子问题1求解θ6。S8. Take any point q s2 that is not on the axis of the joint 6, and solve the target position q e2 corresponding to q s2 based on the screw forward kinematics model, then the starting and ending positions of the inverse solution motion of the joint 6 are known. Similarly, solve θ 6 based on sub-problem 1.

本发明的特点在于基于几何描述方法将6R机器人的运动学逆解问题转化为一个六元二次方程组和一个三元二次方程组的求解，简化了逆解模型，且几何意义更加明确，为6R机器人的实时运动控制提供一定的方法支撑。该方法将几何描述与旋量理论结合，几何意义更加明确，通过简化逆解算法为代数方程组求解，有效提升了计算效率，能够为机器人运动实时控制提供一种新的逆解处理方法。The feature of the present invention is that based on the geometric description method, the kinematic inverse solution problem of the 6R robot is transformed into a solution of a six-element quadratic equation group and a three-element quadratic equation group, which simplifies the inverse solution model, and the geometric meaning is clearer. Provide some method support for the real-time motion control of 6R robot. This method combines the geometric description with the screw theory, and the geometric meaning is clearer. By simplifying the inverse solution algorithm to solve the algebraic equations, the calculation efficiency is effectively improved, and it can provide a new inverse solution processing method for the real-time control of robot motion.

附图说明Description of drawings

图1关节1、2和3的逆解运动几何描述；The inverse kinematics description of joints 1, 2 and 3 in Fig. 1;

图2关节4和5的逆解运动几何描述；The inverse kinematics description of joints 4 and 5 in Fig. 2;

图3某6R机器人参数坐标系。Figure 3 The parameter coordinate system of a 6R robot.

具体实施方式Detailed ways

以下结合附图1-3对本发明进行详细说明。The present invention will be described in detail below in conjunction with accompanying drawings 1-3.

S1确定6R机器人运动学参数并建立正运动学模型S1 determines the kinematics parameters of the 6R robot and establishes a positive kinematics model

如图3所示，已知该6R机器人的初始状态各关节所在位置矢量及旋转矢量如下：As shown in Figure 3, the position vectors and rotation vectors of each joint in the initial state of the 6R robot are known as follows:

其中ri，1≤i≤6表示i关节在基坐标系的位置矢量，ωi表示i关节的旋转矢量。Where r i , 1≤i≤6, represents the position vector of joint i in the base coordinate system, and ω i represents the rotation vector of joint i.

基于旋量理论，该6R机器人正运动学模型表示为，Based on the screw theory, the forward kinematics model of the 6R robot is expressed as,

其中gst(θ),gst(0)分别表示机器人末端的初始位姿与目标位姿，表示i关节旋转运动的指数积形式，where g st (θ), g st (0) represent the initial pose and target pose of the robot end, respectively, Indicates the exponential product form of the rotation motion of the i joint,

式中θi为第i个关节角位移；是i关节旋转矢量ωi的另一种表示形式，由ωi＝[ω1 ω2 ω3]定义为则νi是i关节运动的旋转线速度，νi＝-ωi×ri。where θ i is the angular displacement of the ith joint; is another representation of i joint rotation vector ω i , defined by ω i =[ω 1 ω 2 ω 3 ] as but ν i is the rotation linear velocity of joint i, ν i =-ω i ×r i .

给定目标位姿，Given the target pose,

S2针对前三个关节建立六元二次方程组S2 establishes a six-element quadratic equation system for the first three joints

基于旋量理论前三个关节的旋转运动描述为，Based on the screw theory, the rotational motion of the first three joints is described as,

式中q′s和q′e分别表示6R机器人末端在该旋转运动的初始位置与目标位置。由S1可知，In the formula, q' s and q' e represent the initial position and target position of the end of the 6R robot in the rotational movement, respectively. It can be known from S1 that

q′s＝[xs ys zs 1]＝[0 744 940 1]q' s = [x s y s z s 1] = [0 744 940 1]

q′e＝[xe ye ze 1]＝[-936.6611 631.7859 570.0752 1]q′ e =[x e y e z e 1]=[-936.6611 631.7859 570.0752 1]

设运动经过点c1和c2,根据图1所示几何描述建立以下关系式组，Assuming that the movement passes through points c 1 and c 2 , the following relational formulas are established according to the geometric description shown in Fig. 1,

其中q1，q2和q3分别为关节1、关节2和关节3旋转轴线上的任意一点，为简化模型取q1＝[0 0 0]，q2＝[0 150 250]和q3＝[0 150 800]，那么式(7)表示为如下方程组，Among them, q 1 , q 2 and q 3 are any points on the rotation axes of joint 1, joint 2 and joint 3 respectively, and q 1 =[0 0 0], q 2 =[0 150 250] and q 3 are taken for the simplified model =[0 150 800], then formula (7) is expressed as the following equations,

采用MATLAB中“SOLVE”函数求解方程组(8)，得到x1,y1,z1,x2,y2和z2的解。Using the "SOLVE" function in MATLAB to solve equations (8), the solutions of x 1 , y 1 , z 1 , x 2 , y 2 and z 2 are obtained.

S3计算关节角位移θ1，θ2和θ3 S3 calculates joint angular displacement θ 1 , θ 2 and θ 3

得到过程点坐标c1＝[x1 y1 z1]和c2＝[x2 y2 z2]后，将绕关节1、关节2和关节3的旋转运动分别描述如下，After obtaining the process point coordinates c 1 =[x 1 y 1 z 1 ] and c 2 =[x 2 y 2 z 2 ], the rotational motions around joint 1, joint 2 and joint 3 are described as follows,

基于Paden-Kahan子问题1，得到关节角位移的显示表达式如下，Based on Paden-Kahan sub-problem 1, the display expression of joint angular displacement is obtained as follows,

步骤(4)针对关节4和关节5建立三元二次方程组Step (4) Establish a ternary quadratic equation system for joints 4 and 5

由式(3)知，From formula (3), we know that

其中在关节6的轴线上取一点qs1＝[xs1 ys1 zs1]＝[0 744 0]，其绕关节4与关节5的旋转运动描述如下，in Take a point q s1 =[x s1 y s1 z s1 ]=[0 744 0] on the axis of joint 6, and its rotational motion around joint 4 and joint 5 is described as follows,

其中q′e1＝g1q′s1＝[xe1 ye1 ze1 1]。where q′ e1 =g 1 q′ s1 =[x e1 y e1 z e1 1].

设运动经过点坐标为c3＝[x3 y3 z3]，根据图2所示几何描述建立以下关系式组，Assuming that the coordinates of the moving points are c 3 =[x 3 y 3 z 3 ], the following relational formulas are established according to the geometric description shown in Figure 2,

其中q4＝[0 744 940]。则式(14)表示为如下方程，where q 4 =[0 744 940]. Then formula (14) is expressed as the following equation,

采用MATLAB中“SOLVE”函数求解方程组(15)，得到x3,y3和z3的解。Using the "SOLVE" function in MATLAB to solve equations (15), the solutions of x 3 , y 3 and z 3 are obtained.

步骤(5)计算关节角位移θ4和θ5 Step (5) Calculate the joint angular displacement θ 4 and θ 5

得到过程点坐标c3＝[x3 y3 z3]后，将绕关节4和关节5的旋转运动分别描述如下，After obtaining the coordinates of the process point c 3 =[x 3 y 3 z 3 ], the rotational motions around joint 4 and joint 5 are described as follows,

基于Paden-Kahan子问题1，得到关节角位移θ4和θ5的显示表达式如下，Based on Paden-Kahan sub-problem 1, the explicit expressions of joint angular displacements θ 4 and θ 5 are obtained as follows,

步骤(6)计算关节角位移θ6 Step (6) Calculate joint angular displacement θ 6

取不在关节6轴线上一点qs2＝[0 750 940]，其绕关节6的旋转运动描述为，Take a point q s2 =[0 750 940] not on the axis of joint 6, and its rotational motion around joint 6 is described as,

其中同理基于Paden-Kahan子问题1，得到关节角位移θ6的显示表达式如下，in Similarly, based on Paden-Kahan sub-problem 1, the display expression of the joint angular displacement θ 6 is obtained as follows,

通过以上求解得到八组运动学逆解如表1所示。Eight sets of kinematic inverse solutions are obtained through the above solutions, as shown in Table 1.

表1八组运动学逆解Table 1 Eight groups of kinematic inverse solutions

Claims (2)

  1. A kind of 1. 6R Robotic inverse kinematics geometry solving methods based on spinor theory, it is characterised in that：The realization of this method Process is as follows：
    S1, basis coordinates system and tool coordinates system are established, determine that 6R robot kinematics join by basis coordinates system and tool coordinates system Number, and establish positive kinematics model；
    S2, the inverse solution Kinematic Decomposition description by first three joint of 6R robots：By point qeTo point c2=[x2 y2 z2] around joint 1 Rotary motion；Point c2To point c1=[x1 y1 z1] rotary motion around joint 2；Point c1To point qsRotary motion around joint 3； Wherein qs, qeThe respectively initial and target location of robot end, established according to geometrical relationship description with x1,y1,z1,x2,y2 And z2For the hexa-atomic quadratic equation group of variable；
    S3, using in MATLAB " SOLVE " function solve above equation group, obtain c1And c2Coordinate vector；
    S4, since then, the inverse solution in first three joint move whole story position, it is known that establishing 6R respectively based on Paden-Kahan subproblems 1 First three joint angle variable θ of robot1, θ2And θ3Explicit solution model, and solved；
    S5, some initial position q as after in the inverse solution motion in three joints is taken on the axis in joint 6s1, and based on spinor just Kinematics model solves initial position qs1Corresponding target location qe1；
    S6, the inverse solution Kinematic Decomposition description by joint 4 and 5：By point qe1To point c3=[x3 y3 z3] rotary motion；By point c3Arrive Point qs1Rotary motion；Established according to geometrical relationship with x3,y3And z3For the ternary quadratic equation group of variable, and use " SOLVE " Function solves, and obtains c3Coordinate vector；
    S7, since then, the inverse solution in joint 4 and joint 5 move whole story position, it is known that being established respectively based on Paden-Kahan subproblems 1 Joint 4 and the joint angle variable θ in joint 54And θ5Explicit solution model, and solved；
    S8, take any point q not on the axis of joint 6s2, based on spinor positive kinematics model solution qs2Corresponding target location qe2, then known to the inverse solution motion whole story position in joint 6；θ is similarly solved based on subproblem 16。
  2. 2. a kind of 6R Robotic inverse kinematics geometry solving methods based on spinor theory according to claim 1, it is special Sign is：S1 determines 6R robot kinematics parameter and establishes positive kinematics model
    Each joint position vector of original state and rotating vector of the known 6R robots are as follows：
    <mrow> <mtable> <mtr> <mtd> <mtable> <mtr> <mtd> <mrow> <msub> <mi>r</mi> <mn>1</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> <mtd> <mrow> <msub> <mi>r</mi> <mn>2</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>150</mn> </mtd> <mtd> <mn>250</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> <mtd> <mrow> <msub> <mi>r</mi> <mn>3</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>150</mn> </mtd> <mtd> <mn>800</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> </mtable> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>r</mi> <mn>4</mn> </msub> <mo>=</mo> <msub> <mi>r</mi> <mn>5</mn> </msub> <mo>=</mo> <msub> <mi>r</mi> <mn>6</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>744</mn> </mtd> <mtd> <mn>940</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow>
    <mrow> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;omega;</mi> <mn>1</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;omega;</mi> <mn>2</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;omega;</mi> <mn>3</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;omega;</mi> <mn>4</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;omega;</mi> <mn>5</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> <mtd> <mrow> <msub> <mi>&amp;omega;</mi> <mn>6</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </mtd> </mtr> </mtable> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow>
    Wherein ri, 1≤i≤6 represent i joints in the position vector of basis coordinates system, ωiRepresent the rotating vector in i joints；
    Based on spinor theory, the 6R robots positive kinematics model is expressed as,
    <mrow> <msub> <mi>g</mi> <mrow> <mi>s</mi> <mi>t</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>&amp;theta;</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>1</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>2</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>2</mn> </msub> <mo>)</mo> </mrow> <mo>...</mo> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>6</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>6</mn> </msub> <mo>)</mo> </mrow> <msub> <mi>g</mi> <mrow> <mi>s</mi> <mi>t</mi> </mrow> </msub> <mrow> <mo>(</mo> <mn>0</mn> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>3</mn> <mo>)</mo> </mrow> </mrow>
    Wherein gst(θ),gst(0) the initial pose and object pose of robot end is represented respectively,Represent the rotation of i joints The dynamic exponent product form of transhipment,
    <mrow> <mi>exp</mi> <mrow> <mo>(</mo> <mrow> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mi>i</mi> </msub> <msub> <mi>&amp;theta;</mi> <mi>i</mi> </msub> </mrow> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>exp</mi> <mrow> <mo>(</mo> <mrow> <msub> <mover> <mi>&amp;omega;</mi> <mo>^</mo> </mover> <mi>i</mi> </msub> <msub> <mi>&amp;theta;</mi> <mi>i</mi> </msub> </mrow> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mrow> <mo>(</mo> <mrow> <mi>I</mi> <mo>-</mo> <mi>exp</mi> <mrow> <mo>(</mo> <mrow> <msub> <mover> <mi>&amp;omega;</mi> <mo>^</mo> </mover> <mi>i</mi> </msub> <msub> <mi>&amp;theta;</mi> <mi>i</mi> </msub> </mrow> <mo>)</mo> </mrow> </mrow> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <mrow> <msub> <mi>&amp;omega;</mi> <mi>i</mi> </msub> <mo>&amp;times;</mo> <msub> <mi>v</mi> <mi>i</mi> </msub> </mrow> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>&amp;theta;</mi> <mi>i</mi> </msub> <msub> <mi>&amp;omega;</mi> <mi>i</mi> </msub> <msup> <msub> <mi>&amp;omega;</mi> <mi>i</mi> </msub> <mi>T</mi> </msup> <msub> <mi>v</mi> <mi>i</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>4</mn> <mo>)</mo> </mrow> </mrow>
    θ in formulaiFor i-th of joint angular displacement；It is i joints rotating vector ωiAnother representation, by ωi=[ω1 ω2 ω3] be defined asThenνiIt is that i is closed Save the linear velocity of motion, νi=-ωi×ri；
    Given object pose,
    <mrow> <msub> <mi>g</mi> <mrow> <mi>s</mi> <mi>t</mi> </mrow> </msub> <mrow> <mo>(</mo> <mi>&amp;theta;</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0.5234</mn> </mtd> <mtd> <mn>0.4033</mn> </mtd> <mtd> <mrow> <mo>-</mo> <mn>0.7506</mn> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mn>936.6611</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0.5979</mn> </mtd> <mtd> <mn>0.4537</mn> </mtd> <mtd> <mn>0.6608</mn> </mtd> <mtd> <mn>631.7859</mn> </mtd> </mtr> <mtr> <mtd> <mn>0.6071</mn> </mtd> <mtd> <mrow> <mo>-</mo> <mn>0.7946</mn> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <mn>0.0036</mn> </mrow> </mtd> <mtd> <mn>570.0752</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>5</mn> <mo>)</mo> </mrow> </mrow>
    S2 establishes hexa-atomic quadratic equation group for first three joint
    Rotary motion based on first three joint of spinor theory is described as,
    <mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>1</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>2</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>2</mn> </msub> <mo>)</mo> </mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>3</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>3</mn> </msub> <mo>)</mo> </mrow> <msubsup> <mi>q</mi> <mi>s</mi> <mo>&amp;prime;</mo> </msubsup> <mo>=</mo> <msubsup> <mi>q</mi> <mi>e</mi> <mo>&amp;prime;</mo> </msubsup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>6</mn> <mo>)</mo> </mrow> </mrow>
    Q ' in formulasWith q 'eInitial position and target location of the 6R robot ends in the rotary motion are represented respectively；From S1,
    q′s=[xs ys zs1]=[0 744 940 1]
    q′e=[xe ye ze1]=[- 936.6611 631.7859 570.0752 1]
    If move across point c1And c2, relationship below group is established according to geometric description,
    <mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mo>|</mo> <mo>|</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>-</mo> <msub> <mi>q</mi> <mi>e</mi> </msub> <mo>|</mo> <mo>|</mo> <mo>=</mo> <mo>|</mo> <mo>|</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <mo>-</mo> <msub> <mi>c</mi> <mn>2</mn> </msub> <mo>|</mo> <mo>|</mo> <mo>,</mo> </mrow> </mtd> <mtd> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mi>e</mi> </msub> <mo>-</mo> <msub> <mi>c</mi> <mn>2</mn> </msub> <mo>)</mo> <msup> <msub> <mi>&amp;omega;</mi> <mn>1</mn> </msub> <mi>T</mi> </msup> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>|</mo> <mo>|</mo> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>-</mo> <msub> <mi>c</mi> <mn>2</mn> </msub> <mo>|</mo> <mo>|</mo> <mo>=</mo> <mo>|</mo> <mo>|</mo> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>-</mo> <msub> <mi>c</mi> <mn>1</mn> </msub> <mo>|</mo> <mo>|</mo> <mo>,</mo> </mrow> </mtd> <mtd> <mrow> <mo>(</mo> <msub> <mi>c</mi> <mn>2</mn> </msub> <mo>-</mo> <msub> <mi>c</mi> <mn>1</mn> </msub> <mo>)</mo> <msup> <msub> <mi>&amp;omega;</mi> <mn>2</mn> </msub> <mi>T</mi> </msup> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>|</mo> <mo>|</mo> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>c</mi> <mn>1</mn> </msub> <mo>|</mo> <mo>|</mo> <mo>=</mo> <mo>|</mo> <mo>|</mo> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>q</mi> <mi>s</mi> </msub> <mo>|</mo> <mo>|</mo> <mo>,</mo> </mrow> </mtd> <mtd> <mrow> <mo>(</mo> <msub> <mi>c</mi> <mn>1</mn> </msub> <mo>-</mo> <msub> <mi>q</mi> <mi>s</mi> </msub> <mo>)</mo> <msup> <msub> <mi>&amp;omega;</mi> <mn>3</mn> </msub> <mi>T</mi> </msup> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>7</mn> <mo>)</mo> </mrow> </mrow>
    Wherein q1, q2And q3Any point respectively on joint 1, joint 2 and the rotation axis of joint 3, q is taken for simplified model1= [0 0 0], q2=[0 150 250] and q3=[0 150 800], then formula (7) is expressed as equation group,
    <mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msup> <msub> <mi>x</mi> <mn>2</mn> </msub> <mn>2</mn> </msup> <mo>+</mo> <msup> <msub> <mi>y</mi> <mn>2</mn> </msub> <mn>2</mn> </msup> <mo>=</mo> <msup> <msub> <mi>x</mi> <mi>e</mi> </msub> <mn>2</mn> </msup> <mo>+</mo> <msup> <msub> <mi>y</mi> <mi>e</mi> </msub> <mn>2</mn> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msup> <msub> <mi>y</mi> <mn>1</mn> </msub> <mn>2</mn> </msup> <mo>-</mo> <mn>300</mn> <msub> <mi>y</mi> <mn>1</mn> </msub> <mo>+</mo> <msup> <msub> <mi>z</mi> <mn>1</mn> </msub> <mn>2</mn> </msup> <mo>-</mo> <mn>500</mn> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>=</mo> <msup> <msub> <mi>y</mi> <mn>2</mn> </msub> <mn>3</mn> </msup> <mo>-</mo> <mn>300</mn> <msub> <mi>y</mi> <mn>2</mn> </msub> <mo>+</mo> <msup> <msub> <mi>z</mi> <mn>2</mn> </msub> <mn>2</mn> </msup> <mo>-</mo> <mn>500</mn> <msub> <mi>z</mi> <mn>2</mn> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msup> <msub> <mi>y</mi> <mn>1</mn> </msub> <mn>2</mn> </msup> <mo>-</mo> <mn>300</mn> <msub> <mi>y</mi> <mn>1</mn> </msub> <mo>+</mo> <msup> <msub> <mi>z</mi> <mn>1</mn> </msub> <mn>2</mn> </msup> <mo>-</mo> <mn>1600</mn> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>=</mo> <msup> <msub> <mi>y</mi> <mi>s</mi> </msub> <mn>2</mn> </msup> <mo>-</mo> <mn>300</mn> <msub> <mi>y</mi> <mi>s</mi> </msub> <mo>+</mo> <msup> <msub> <mi>z</mi> <mi>s</mi> </msub> <mn>2</mn> </msup> <mo>-</mo> <mn>1600</mn> <msub> <mi>z</mi> <mi>s</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>z</mi> <mn>2</mn> </msub> <mo>=</mo> <msub> <mi>z</mi> <mi>e</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>x</mi> <mn>1</mn> </msub> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>x</mi> <mn>2</mn> </msub> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>8</mn> <mo>)</mo> </mrow> </mrow>
    Equation group (8) is solved using " SOLVE " function in MATLAB, obtains x1,y1,z1,x2,y2And z2Solution；
    S3 calculates joint angular displacement1, θ2And θ3
    Obtain process point coordinates c1=[x1 y1 z1] and c2=[x2 y2 z2] after, by around the rotation in joint 1, joint 2 and joint 3 Motion is described as follows respectively,
    <mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>1</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>1</mn> </msub> <mo>)</mo> </mrow> <msubsup> <mi>c</mi> <mn>2</mn> <mo>&amp;prime;</mo> </msubsup> <mo>=</mo> <msubsup> <mi>q</mi> <mi>e</mi> <mo>&amp;prime;</mo> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>2</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>2</mn> </msub> <mo>)</mo> </mrow> <msubsup> <mi>c</mi> <mn>1</mn> <mo>&amp;prime;</mo> </msubsup> <mo>=</mo> <msubsup> <mi>c</mi> <mn>2</mn> <mo>&amp;prime;</mo> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>3</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>3</mn> </msub> <mo>)</mo> </mrow> <msubsup> <mi>q</mi> <mi>s</mi> <mo>&amp;prime;</mo> </msubsup> <mo>=</mo> <msubsup> <mi>c</mi> <mn>1</mn> <mo>&amp;prime;</mo> </msubsup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>9</mn> <mo>)</mo> </mrow> </mrow>
    Based on Paden-Kahan subproblems 1, it is as follows to obtain the display expression formula of joint angular displacement,
    <mrow> <msub> <mi>&amp;theta;</mi> <mn>1</mn> </msub> <mo>=</mo> <mi>a</mi> <mi> </mi> <mi>t</mi> <mi>a</mi> <mi>n</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <msub> <mi>x</mi> <mi>e</mi> </msub> <msub> <mi>y</mi> <mn>2</mn> </msub> <mo>-</mo> <msub> <mi>y</mi> <mi>e</mi> </msub> <msub> <mi>x</mi> <mn>2</mn> </msub> </mrow> <mrow> <msub> <mi>x</mi> <mn>2</mn> </msub> <msub> <mi>x</mi> <mi>e</mi> </msub> <mo>+</mo> <msub> <mi>y</mi> <mn>2</mn> </msub> <msub> <mi>y</mi> <mi>e</mi> </msub> </mrow> </mfrac> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>10</mn> <mo>)</mo> </mrow> </mrow>
    <mrow> <msub> <mi>&amp;theta;</mi> <mn>2</mn> </msub> <mo>=</mo> <mi>a</mi> <mi> </mi> <mi>t</mi> <mi>a</mi> <mi>n</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mn>1</mn> </msub> <mo>-</mo> <mn>150</mn> <mo>)</mo> <mo>(</mo> <msub> <mi>z</mi> <mn>2</mn> </msub> <mo>-</mo> <mn>250</mn> <mo>)</mo> <mo>-</mo> <mo>(</mo> <msub> <mi>y</mi> <mn>2</mn> </msub> <mo>-</mo> <mn>150</mn> <mo>)</mo> <mo>(</mo> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>-</mo> <mn>250</mn> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mn>1</mn> </msub> <mo>-</mo> <mn>150</mn> <mo>)</mo> <mo>(</mo> <msub> <mi>y</mi> <mn>2</mn> </msub> <mo>-</mo> <mn>150</mn> <mo>)</mo> <mo>+</mo> <mo>(</mo> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>-</mo> <mn>250</mn> <mo>)</mo> <mo>(</mo> <msub> <mi>z</mi> <mn>2</mn> </msub> <mo>-</mo> <mn>250</mn> <mo>)</mo> </mrow> </mfrac> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>11</mn> <mo>)</mo> </mrow> </mrow>
    <mrow> <msub> <mi>&amp;theta;</mi> <mn>3</mn> </msub> <mo>=</mo> <mi>a</mi> <mi> </mi> <mi>t</mi> <mi>a</mi> <mi>n</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mi>s</mi> </msub> <mo>-</mo> <mn>150</mn> <mo>)</mo> <mo>(</mo> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>-</mo> <mn>800</mn> <mo>)</mo> <mo>-</mo> <mo>(</mo> <msub> <mi>y</mi> <mn>1</mn> </msub> <mo>-</mo> <mn>150</mn> <mo>)</mo> <mo>(</mo> <msub> <mi>z</mi> <mi>s</mi> </msub> <mo>-</mo> <mn>800</mn> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mi>s</mi> </msub> <mo>-</mo> <mn>150</mn> <mo>)</mo> <mo>(</mo> <msub> <mi>y</mi> <mn>1</mn> </msub> <mo>-</mo> <mn>150</mn> <mo>)</mo> <mo>+</mo> <mo>(</mo> <msub> <mi>z</mi> <mi>s</mi> </msub> <mo>-</mo> <mn>800</mn> <mo>)</mo> <mo>(</mo> <msub> <mi>z</mi> <mn>1</mn> </msub> <mo>-</mo> <mn>800</mn> <mo>)</mo> </mrow> </mfrac> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>12</mn> <mo>)</mo> </mrow> </mrow>
    Step (4) establishes ternary quadratic equation group for joint 4 and joint 5
    Known by formula (3),
    <mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>4</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>4</mn> </msub> <mo>)</mo> </mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>5</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>5</mn> </msub> <mo>)</mo> </mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>6</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>6</mn> </msub> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>g</mi> <mn>1</mn> </msub> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>12</mn> <mo>)</mo> </mrow> </mrow>
    WhereinA point q is taken on the axis in joint 6s1 =[xs1 ys1 zs1]=[0 744 0], its rotary motion around joint 4 and joint 5 is described as follows,
    <mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>4</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>4</mn> </msub> <mo>)</mo> </mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>5</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>5</mn> </msub> <mo>)</mo> </mrow> <msubsup> <mi>q</mi> <mrow> <mi>s</mi> <mn>1</mn> </mrow> <mo>&amp;prime;</mo> </msubsup> <mo>=</mo> <msubsup> <mi>q</mi> <mrow> <mi>e</mi> <mn>1</mn> </mrow> <mo>&amp;prime;</mo> </msubsup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>13</mn> <mo>)</mo> </mrow> </mrow>
    Wherein
    If it is c to move across point coordinates3=[x3 y3 z3], relationship below group is established according to geometric description,
    <mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mo>|</mo> <mo>|</mo> <msub> <mi>c</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>q</mi> <mn>4</mn> </msub> <mo>|</mo> <mo>|</mo> <mo>=</mo> <mo>|</mo> <mo>|</mo> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>q</mi> <mn>4</mn> </msub> <mo>|</mo> <mo>|</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>(</mo> <msub> <mi>c</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>q</mi> <mrow> <mi>e</mi> <mn>1</mn> </mrow> </msub> <mo>)</mo> <msup> <msub> <mi>&amp;omega;</mi> <mn>4</mn> </msub> <mi>T</mi> </msup> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>(</mo> <msub> <mi>c</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>q</mi> <mrow> <mi>s</mi> <mn>1</mn> </mrow> </msub> <mo>)</mo> <msup> <msub> <mi>&amp;omega;</mi> <mn>5</mn> </msub> <mi>T</mi> </msup> <mo>=</mo> <mn>0</mn> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>14</mn> <mo>)</mo> </mrow> </mrow>
    Wherein q4=[0 744 940]；Then formula (14) is expressed as equation,
    <mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msup> <msub> <mi>x</mi> <mn>3</mn> </msub> <mn>2</mn> </msup> <mo>+</mo> <msup> <msub> <mi>z</mi> <mn>3</mn> </msub> <mn>2</mn> </msup> <mo>-</mo> <mn>1880</mn> <msub> <mi>z</mi> <mn>3</mn> </msub> <mo>=</mo> <msup> <msub> <mi>x</mi> <mrow> <mi>e</mi> <mn>1</mn> </mrow> </msub> <mn>2</mn> </msup> <mo>+</mo> <msup> <msub> <mi>z</mi> <mrow> <mi>e</mi> <mn>1</mn> </mrow> </msub> <mn>2</mn> </msup> <mo>-</mo> <mn>1880</mn> <msub> <mi>z</mi> <mrow> <mi>e</mi> <mn>1</mn> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>x</mi> <mn>3</mn> </msub> <mo>=</mo> <msub> <mi>x</mi> <mrow> <mi>s</mi> <mn>1</mn> </mrow> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>y</mi> <mn>3</mn> </msub> <mo>=</mo> <msub> <mi>y</mi> <mrow> <mi>e</mi> <mn>1</mn> </mrow> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>15</mn> <mo>)</mo> </mrow> </mrow>
    Equation group (15) is solved using " SOLVE " function in MATLAB, obtains x3,y3And z3Solution；
    Step (5) calculates joint angular displacement4And θ5
    Obtain process point coordinates c3=[x3 y3 z3] after, it will be described as follows respectively around the rotary motion in joint 4 and joint 5,
    <mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>4</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>4</mn> </msub> <mo>)</mo> </mrow> <msubsup> <mi>c</mi> <mn>3</mn> <mo>&amp;prime;</mo> </msubsup> <mo>=</mo> <msubsup> <mi>q</mi> <mrow> <mi>e</mi> <mn>1</mn> </mrow> <mo>&amp;prime;</mo> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>5</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>5</mn> </msub> <mo>)</mo> </mrow> <msubsup> <mi>q</mi> <mrow> <mi>s</mi> <mn>1</mn> </mrow> <mo>&amp;prime;</mo> </msubsup> <mo>=</mo> <msubsup> <mi>c</mi> <mn>3</mn> <mo>&amp;prime;</mo> </msubsup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>16</mn> <mo>)</mo> </mrow> </mrow>
    Based on Paden-Kahan subproblems 1, joint angular displacement is obtained4And θ5Display expression formula it is as follows,
    <mrow> <msub> <mi>&amp;theta;</mi> <mn>4</mn> </msub> <mo>=</mo> <mi>a</mi> <mi> </mi> <mi>t</mi> <mi>a</mi> <mi>n</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <mo>(</mo> <msub> <mi>z</mi> <mn>3</mn> </msub> <mo>-</mo> <mn>940</mn> <mo>)</mo> <msub> <mi>x</mi> <mrow> <mi>e</mi> <mn>1</mn> </mrow> </msub> <mo>-</mo> <msub> <mi>x</mi> <mn>3</mn> </msub> <mo>(</mo> <msub> <mi>z</mi> <mrow> <mi>e</mi> <mn>1</mn> </mrow> </msub> <mo>-</mo> <mn>940</mn> <mo>)</mo> </mrow> <mrow> <msub> <mi>x</mi> <mn>3</mn> </msub> <msub> <mi>x</mi> <mrow> <mi>e</mi> <mn>1</mn> </mrow> </msub> <mo>+</mo> <mrow> <mo>(</mo> <msub> <mi>z</mi> <mn>3</mn> </msub> <mo>-</mo> <mn>940</mn> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <msub> <mi>z</mi> <mrow> <mi>e</mi> <mn>1</mn> </mrow> </msub> <mo>-</mo> <mn>940</mn> <mo>)</mo> </mrow> </mrow> </mfrac> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>17</mn> <mo>)</mo> </mrow> </mrow>
    <mrow> <msub> <mi>&amp;theta;</mi> <mn>5</mn> </msub> <mo>=</mo> <mi>a</mi> <mi> </mi> <mi>t</mi> <mi>a</mi> <mi>n</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mrow> <mi>s</mi> <mn>1</mn> </mrow> </msub> <mo>-</mo> <mn>744</mn> <mo>)</mo> <mo>(</mo> <msub> <mi>z</mi> <mn>3</mn> </msub> <mo>-</mo> <mn>940</mn> <mo>)</mo> <mo>-</mo> <mo>(</mo> <msub> <mi>z</mi> <mrow> <mi>s</mi> <mn>1</mn> </mrow> </msub> <mo>-</mo> <mn>940</mn> <mo>)</mo> <mo>(</mo> <msub> <mi>y</mi> <mn>3</mn> </msub> <mo>-</mo> <mn>744</mn> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mrow> <mi>s</mi> <mn>1</mn> </mrow> </msub> <mo>-</mo> <mn>744</mn> <mo>)</mo> <mo>(</mo> <msub> <mi>y</mi> <mn>3</mn> </msub> <mo>-</mo> <mn>744</mn> <mo>)</mo> <mo>+</mo> <mo>(</mo> <msub> <mi>z</mi> <mn>3</mn> </msub> <mo>-</mo> <mn>940</mn> <mo>)</mo> <mo>(</mo> <msub> <mi>z</mi> <mrow> <mi>s</mi> <mn>1</mn> </mrow> </msub> <mo>-</mo> <mn>940</mn> <mo>)</mo> </mrow> </mfrac> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>18</mn> <mo>)</mo> </mrow> </mrow>
    Step (6) calculates joint angular displacement6
    Take a not point q on the axis of joint 6s2=[0 750 940], its rotary motion around joint 6 are described as,
    <mrow> <mi>exp</mi> <mrow> <mo>(</mo> <msub> <mover> <mi>&amp;xi;</mi> <mo>^</mo> </mover> <mn>6</mn> </msub> <msub> <mi>&amp;theta;</mi> <mn>6</mn> </msub> <mo>)</mo> </mrow> <msubsup> <mi>q</mi> <mrow> <mi>s</mi> <mn>2</mn> </mrow> <mo>&amp;prime;</mo> </msubsup> <mo>=</mo> <msubsup> <mi>q</mi> <mrow> <mi>e</mi> <mn>2</mn> </mrow> <mo>&amp;prime;</mo> </msubsup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>19</mn> <mo>)</mo> </mrow> </mrow>
    WhereinSimilarly it is based on Paden-Kahan Subproblem 1, obtain joint angular displacement6Display expression formula it is as follows,
    <mrow> <msub> <mi>&amp;theta;</mi> <mn>5</mn> </msub> <mo>=</mo> <mi>a</mi> <mi> </mi> <mi>t</mi> <mi>a</mi> <mi>n</mi> <mrow> <mo>(</mo> <mfrac> <mrow> <msub> <mi>x</mi> <mrow> <mi>s</mi> <mn>2</mn> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mrow> <mi>e</mi> <mn>2</mn> </mrow> </msub> <mo>-</mo> <mn>744</mn> <mo>)</mo> </mrow> <mo>-</mo> <msub> <mi>x</mi> <mrow> <mi>e</mi> <mn>2</mn> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mrow> <mi>s</mi> <mn>2</mn> </mrow> </msub> <mo>-</mo> <mn>744</mn> <mo>)</mo> </mrow> </mrow> <mrow> <msub> <mi>x</mi> <mrow> <mi>s</mi> <mn>2</mn> </mrow> </msub> <msub> <mi>x</mi> <mrow> <mi>e</mi> <mn>2</mn> </mrow> </msub> <mo>+</mo> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mrow> <mi>s</mi> <mn>2</mn> </mrow> </msub> <mo>-</mo> <mn>744</mn> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <msub> <mi>y</mi> <mrow> <mi>e</mi> <mn>2</mn> </mrow> </msub> <mo>-</mo> <mn>744</mn> <mo>)</mo> </mrow> </mrow> </mfrac> <mo>)</mo> </mrow> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>20</mn> <mo>)</mo> </mrow> </mrow>
    Solved more than and obtain eight groups of Inverse Kinematics Solutions.
CN201710953599.4A 2017-10-13 2017-10-13 A geometric solution method for inverse kinematics of 6R robot based on screw theory Active CN107756400B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710953599.4A CN107756400B (en) 2017-10-13 2017-10-13 A geometric solution method for inverse kinematics of 6R robot based on screw theory

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710953599.4A CN107756400B (en) 2017-10-13 2017-10-13 A geometric solution method for inverse kinematics of 6R robot based on screw theory

Publications (2)

Publication Number Publication Date
CN107756400A true CN107756400A (en) 2018-03-06
CN107756400B CN107756400B (en) 2020-12-04

Family

ID=61269419

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710953599.4A Active CN107756400B (en) 2017-10-13 2017-10-13 A geometric solution method for inverse kinematics of 6R robot based on screw theory

Country Status (1)

Country Link
CN (1) CN107756400B (en)

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108406770A (en) * 2018-03-09 2018-08-17 首都师范大学 The joint rotation angle of serial manipulator determines method and device
CN109015641A (en) * 2018-08-16 2018-12-18 居鹤华 The inverse solution modeling of general 6R mechanical arm based on axis invariant and calculation method
CN109291047A (en) * 2018-08-16 2019-02-01 居鹤华 Based on axis invariant and the inverse solution modeling method of DH parameter 1R/2R/3R
CN109483529A (en) * 2018-10-12 2019-03-19 华南智能机器人创新研究院 A kind of mechanical arm method of servo-controlling, system and device based on screw theory
CN109968358A (en) * 2019-03-28 2019-07-05 北京工业大学 A full-joint obstacle avoidance trajectory optimization method for redundant robots considering motion stability
CN110385716A (en) * 2018-04-18 2019-10-29 B和R工业自动化有限公司 For controlling the method and kinematic mechanisms of kinematic mechanisms
CN111452041A (en) * 2020-03-17 2020-07-28 湖南工业大学 Inverse kinematics solving method for non-spherical wrist 6R robot
CN111496783A (en) * 2020-03-26 2020-08-07 天津大学 A solution method of inverse kinematics for 6R industrial robot
CN113400310A (en) * 2021-06-28 2021-09-17 首都师范大学 Formal verification method for dexterous hand inverse kinematics of three-finger robot and electronic equipment
CN114290331A (en) * 2021-12-14 2022-04-08 中国科学院深圳先进技术研究院 Robot motion control method, robot and computer-readable storage device
CN115741672A (en) * 2022-10-21 2023-03-07 杭州邦杰星医疗科技有限公司 DH derivation method based on rigid body transformation

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102243620A (en) * 2011-06-02 2011-11-16 安凯 Rapid solving method for inverse kinematics problem of six-joint mechanical arm
CN102637158A (en) * 2012-04-28 2012-08-15 谷菲 Inverse kinematics solution method for six-degree-of-freedom serial robot
CN106647282A (en) * 2017-01-19 2017-05-10 北京工业大学 Six-freedom-degree robot track planning method giving consideration to tail end motion error
CN106845037A (en) * 2017-03-21 2017-06-13 山东科技大学 A kind of inverse kinematics general method for solving of five degree of freedom serial manipulator
CN107203653A (en) * 2017-04-12 2017-09-26 山东科技大学 A kind of inverse kinematics general method for solving of six degree of freedom serial manipulator

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102243620A (en) * 2011-06-02 2011-11-16 安凯 Rapid solving method for inverse kinematics problem of six-joint mechanical arm
CN102637158A (en) * 2012-04-28 2012-08-15 谷菲 Inverse kinematics solution method for six-degree-of-freedom serial robot
CN106647282A (en) * 2017-01-19 2017-05-10 北京工业大学 Six-freedom-degree robot track planning method giving consideration to tail end motion error
CN106845037A (en) * 2017-03-21 2017-06-13 山东科技大学 A kind of inverse kinematics general method for solving of five degree of freedom serial manipulator
CN107203653A (en) * 2017-04-12 2017-09-26 山东科技大学 A kind of inverse kinematics general method for solving of six degree of freedom serial manipulator

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
洪磊: "ABB1410工业机器人的旋量运动学逆解方法", 《机械设计与制造》 *

Cited By (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108406770B (en) * 2018-03-09 2020-12-18 首都师范大学 Method and device for determining joint rotation angle of 6-DOF serial robot
CN108406770A (en) * 2018-03-09 2018-08-17 首都师范大学 The joint rotation angle of serial manipulator determines method and device
CN110385716A (en) * 2018-04-18 2019-10-29 B和R工业自动化有限公司 For controlling the method and kinematic mechanisms of kinematic mechanisms
CN110385716B (en) * 2018-04-18 2024-03-12 B和R工业自动化有限公司 Method for controlling a kinematic mechanism and kinematic mechanism
CN109015641A (en) * 2018-08-16 2018-12-18 居鹤华 The inverse solution modeling of general 6R mechanical arm based on axis invariant and calculation method
CN109291047A (en) * 2018-08-16 2019-02-01 居鹤华 Based on axis invariant and the inverse solution modeling method of DH parameter 1R/2R/3R
CN109483529A (en) * 2018-10-12 2019-03-19 华南智能机器人创新研究院 A kind of mechanical arm method of servo-controlling, system and device based on screw theory
CN109483529B (en) * 2018-10-12 2021-02-26 华南智能机器人创新研究院 Mechanical arm servo control method, system and device based on spiral theory
CN109968358A (en) * 2019-03-28 2019-07-05 北京工业大学 A full-joint obstacle avoidance trajectory optimization method for redundant robots considering motion stability
CN109968358B (en) * 2019-03-28 2021-04-09 北京工业大学 A full-joint obstacle avoidance trajectory optimization method for redundant robots considering motion stability
CN111452041A (en) * 2020-03-17 2020-07-28 湖南工业大学 Inverse kinematics solving method for non-spherical wrist 6R robot
CN111496783A (en) * 2020-03-26 2020-08-07 天津大学 A solution method of inverse kinematics for 6R industrial robot
CN113400310A (en) * 2021-06-28 2021-09-17 首都师范大学 Formal verification method for dexterous hand inverse kinematics of three-finger robot and electronic equipment
CN113400310B (en) * 2021-06-28 2022-07-05 首都师范大学 Formal verification method for dexterous hand inverse kinematics of three-finger robot and electronic equipment
CN114290331A (en) * 2021-12-14 2022-04-08 中国科学院深圳先进技术研究院 Robot motion control method, robot and computer-readable storage device
CN115741672A (en) * 2022-10-21 2023-03-07 杭州邦杰星医疗科技有限公司 DH derivation method based on rigid body transformation
CN115741672B (en) * 2022-10-21 2024-04-19 杭州邦杰星医疗科技有限公司 A DH derivation method based on rigid body transformation

Also Published As

Publication number Publication date
CN107756400B (en) 2020-12-04

Similar Documents

Publication Publication Date Title
CN107756400B (en) A geometric solution method for inverse kinematics of 6R robot based on screw theory
Gong et al. Analytical inverse kinematics and self-motion application for 7-DOF redundant manipulator
CN107589934B (en) Solving method for inverse kinematics analytic solution of joint type mechanical arm
CN106647282B (en) Six-degree-of-freedom robot trajectory planning method considering tail end motion error
CN102785248B (en) Motion control method of decoupling type 6-DOF (six degrees of freedom) industrial robot
CN107584474B (en) A Coordinated Motion Method of Spherical Joint Dual-arm Robot Based on Geometric Projection
CN108621158A (en) A kind of time optimal trajectory planning control method and device about mechanical arm
CN102609002B (en) Position reversal solution control method of six-freedom cascade mechanical arm
CN103390101B (en) The general method for solving of inverse kinematics of cascade robot
CN106844951B (en) A method and system for solving the inverse kinematics of a hyper-redundant robot based on piecewise geometry
CN107957684A (en) A kind of robot three-dimensional based on pseudo-velocity vector field is without touching method for planning track
CN103481288A (en) 5-joint robot end-of-arm tool pose controlling method
CN107203653A (en) A kind of inverse kinematics general method for solving of six degree of freedom serial manipulator
CN106845037A (en) A kind of inverse kinematics general method for solving of five degree of freedom serial manipulator
KR102030141B1 (en) Method and system for controlling elbow of robot
CN105234930B (en) Control method of motion of redundant mechanical arm based on configuration plane
Gharahsofloo et al. An efficient algorithm for workspace generation of delta robot
CN111283682B (en) A Geometric Projection Solution for Forward Kinematics of a 4-UPU Parallel Robot
CN104999463B (en) A kind of redundant mechanical arm motion control method based on configuration plane
CN109434838B (en) Coordinated motion planning method and system for endoscopic operation of line-driven continuous robot
CN108326854A (en) A kind of inverse kinematics method of multi-joint mechanical arm spatial function track movement
CN103529856A (en) 5-joint robot end tool position and posture control method
Liqiu et al. Trajectory planning and simulation of industrial robot based on MATLAB and RobotStudio
Zhang et al. Kuka youBot arm shortest path planning based on geodesics
CN111531532A (en) A Climbing Velocity Modeling Method of Robot Based on Screw Theory

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