Seven-degree-of-freedom mechanical arm joint angle solving method based on spinor and related device

Abstract

The invention provides a seven-degree-of-freedom mechanical arm joint angle solving method based on spinor and a related device, and relates to the field of robots. The electronic equipment obtains a forward kinematics equation of the mechanical arm based on spinor and the tail end pose of the mechanical arm. And the forward kinematics equation is converted into a Paden-Kahan sub-problem form to solve the tail end pose, and the rotation angle of each joint under the constraint of the preset configuration parameters is obtained. Thus, by introducing the spinor theory and the Paden-Kahan sub-problem solving technology, the complex inverse kinematics solving process is simplified, the solving efficiency and precision are improved, and accurate solving of each joint angle is ensured.

Description

Technical Field

[0001] The present application relates to the field of robotics, and in particular to a method for solving joint angles of a seven-degree-of-freedom robotic arm based on rotation and a related device. Background Art

[0002] In recent years, a robotic arm with seven degrees of freedom has become one of the research hotspots in robotics, and has shown broad application prospects in many fields such as human-machine collaboration, medical surgery, automotive painting, and aerospace. The kinematic model of the robotic arm describes the mapping relationship between the joint space and the task space, including forward kinematics solution and inverse kinematics solution. Compared with the forward kinematics solution, the inverse kinematics solution is more complicated to solve and is the core problem of kinematic modeling of the robotic arm. At present, the most commonly used robot kinematic modeling method is the DH (Denavit-Hartenberg) method.

[0003] However, there are certain limitations in the kinematic modeling of manipulators based on the DH method. First, the DH parameters are closely related to the specific configuration of the manipulator, making it difficult to construct a unified manipulator model; second, in the modeling process, it is necessary to establish a local coordinate system at each joint and analyze the relative motion relationship between adjacent coordinate systems. This process is not only complicated, but also not intuitive in terms of geometric meaning. In recent years, the application of screw theory in the kinematic modeling of manipulators, parallel robots, and CNC machine tools has become increasingly widespread. Compared with the DH method, the screw theory method only needs to establish a global coordinate system fixed to the base and a tool coordinate system fixed to the end flange. Through the topological information of each joint in the global coordinate system, the motion of the manipulator can be clearly described in the global coordinate system. This method not only effectively avoids the singularity problem that may be caused by the use of local coordinate systems, but also significantly simplifies the number of coordinate systems, making the kinematic modeling process more concise and clear. In the process of solving the inverse kinematics solution, the screw theory method can clarify the conditions for the generation of multiple solutions and the number of solutions, thereby simplifying the screening process.

[0004] In practice, it is found that although the screw theory has many advantages, the three basic sub-problems of the screw theory are only applicable to manipulators of specific configurations and cannot directly solve the analytical inverse kinematics solution of the seven-degree-of-freedom manipulator. Therefore, few studies have applied the screw theory to the analytical inverse kinematics solution of the seven-degree-of-freedom manipulator. Summary of the invention

[0005] In order to overcome at least one of the deficiencies in the prior art, the present application provides a method and a related device for solving the joint angle of a seven-degree-of-freedom manipulator based on rotation, specifically comprising:

[0006] In a first aspect, the present application provides a method for solving the joint angle of a seven-degree-of-freedom manipulator based on rotation, the method comprising:

[0007] Obtaining the forward kinematics equation of the robotic arm based on the screw and the end position of the robotic arm;

[0008] The forward kinematics equation is converted into a Paden-Kahan subproblem to solve the end position and obtain the rotation angle of each joint under the constraints of preset configuration parameters.

[0009] In a second aspect, the present application provides a seven-degree-of-freedom robot arm joint angle solving device based on rotation, the device comprising:

[0010] A data acquisition module, used to acquire the forward kinematics equation of the robotic arm based on the screw and the end position of the robotic arm;

[0011] The angle inverse solution module is used to convert the forward kinematics equation into a Paden-Kahan subproblem form to solve the end posture and obtain the rotation angle of each joint under the constraints of preset configuration parameters.

[0012] In a third aspect, the present application further provides a storage medium storing a computer program, which, when executed by a processor, implements the method for solving the joint angle of a seven-degree-of-freedom robotic arm based on rotation.

[0013] In a fourth aspect, the present application further provides an electronic device, comprising a processor and a memory, wherein the memory stores a computer program, and when the computer program is executed by the processor, the method for solving the joint angle of a seven-degree-of-freedom robotic arm based on rotation is implemented.

[0014] Compared with the prior art, this application has the following beneficial effects:

[0015] The present application provides a method and related device for solving the joint angle of a seven-degree-of-freedom manipulator based on a screw. Among them, the electronic device obtains the forward kinematics equation of the manipulator based on the screw and the end posture of the manipulator. The forward kinematics equation is converted into a Paden-Kahan subproblem form to solve the end posture, and the rotation angle of each joint under the constraints of the preset configuration parameters is obtained. In this way, by introducing the screw theory and the Paden-Kahan subproblem solving technology, the complex inverse kinematics solving process is simplified, the solving efficiency and accuracy are improved, and the accurate solution of each joint angle is ensured. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings required for use in the embodiments will be briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present application and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without paying creative work.

[0017] Figure 1 A schematic diagram of a flow chart of a method for solving joint angles of a seven-degree-of-freedom robotic arm based on rotation provided in an embodiment of the present application;

[0018] Figure 2 A schematic diagram of the structure of a seven-degree-of-freedom robotic arm of an SRS configuration provided in an embodiment of the present application;

[0019] Figure 3 A schematic diagram of the arm angle of a seven-DOF robotic arm of the SRS configuration provided in an embodiment of the present application;

[0020] Figure 4 A schematic diagram of the structure of a seven-degree-of-freedom robotic arm joint angle solving device based on rotation provided in an embodiment of the present application;

[0021] Figure 5 A schematic diagram of the structure of an electronic device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0022] In order to make the purpose, technical solution and advantages of the embodiments of the present application clearer, the technical solution in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are part of the embodiments of the present application, rather than all the embodiments. The components of the embodiments of the present application described and shown in the drawings here can be arranged and designed in various different configurations.

[0023] Therefore, the following detailed description of the embodiments of the present application provided in the accompanying drawings is not intended to limit the scope of the present application for which protection is sought, but merely represents selected embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in the field without creative work are within the scope of protection of the present application.

[0024] It should be noted that similar reference numerals and letters denote similar items in the following drawings, and therefore, once an item is defined in one drawing, further definition and explanation thereof is not required in subsequent drawings.

[0025] In the description of the present application, it should be noted that the terms "first", "second", "third", etc. are only used to distinguish the description and cannot be understood as indicating or implying relative importance. In addition, the terms "comprise", "include" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also includes other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, an element defined by the sentence "comprises a ..." does not exclude the presence of other identical elements in the process, method, article or device including the element.

[0026] Based on the above statement, as introduced in the background technology, although the screw theory has many advantages, the three basic sub-problems of the screw theory are only applicable to manipulators of specific configurations, and therefore it is impossible to directly solve the analytical inverse kinematics solution of a seven-degree-of-freedom manipulator.

[0027] Based on the discovery of the above technical problems, the inventors have proposed the following technical solutions to solve or improve the above problems through creative work. It should be noted that the defects in the solutions in the above prior art are the results obtained by the inventors after practice and careful research. Therefore, the discovery process of the above problems and the solutions proposed in the embodiments of the present application for the above problems below should all be the contributions made by the inventors to the present application in the process of invention and creation, and should not be understood as technical contents known to those skilled in the art.

[0028] In view of this, an embodiment of the present application (hereinafter referred to as the present embodiment) provides a method for solving the joint angle of a seven-degree-of-freedom manipulator based on rotation. Figure 1 As shown, the method includes:

[0029] S1, obtain the forward kinematics equation of the robot arm based on the screw and the end position of the robot arm.

[0030] S2, convert the forward kinematics equation into the form of Paden-Kahan subproblems to solve the end posture and obtain the rotation angle of each joint under the constraints of the preset configuration parameters.

[0031] In this way, by introducing the screw theory and Paden-Kahan subproblem solving technology, the complex inverse kinematics solution process is simplified, the solution efficiency and accuracy are improved, and the accurate solution of each joint angle is ensured.

[0032] It is worth noting that the electronic device implementing the method for solving the joint angle of the seven-degree-of-freedom manipulator based on the rotation quantity can be, but not limited to, a controller customized and developed for the manipulator (e.g., an embedded device), or a host computer that can communicate with the controller of the manipulator, and the host computer issues control instructions to the controller for control. The host computer can be, but not limited to, a mobile terminal, a tablet computer, a laptop computer, a desktop computer, etc.

[0033] To make the solution provided by this embodiment clearer, the following takes the host computer as an example to Figure 1 Each step of the method shown is described in detail. However, it should be understood that the operations of the flowchart may not be implemented in order, and steps without logical contextual relationship may be reversed or implemented simultaneously. In addition, those skilled in the art may add one or more other operations to the flowchart, or remove one or more operations from the flowchart under the guidance of the content of this application. Therefore, continue to refer to Figure 1 , the method comprising:

[0034] S1, obtain the forward kinematics equation of the robot arm based on the screw and the end position of the robot arm.

[0035] It should be understood that a screw is a mathematical tool used to describe the motion of a rigid body. The screw combines linear velocity and angular velocity, and can concisely represent the translational and rotational motion of a rigid body. Through screw theory, it is convenient to deal with the joint motion of a robot arm, the position control of an end effector, and the kinematic problems of a multi-rigid body system. Take a screw as an example. When the screw is tightened, the screw rotates and translates at the same time: rotation is rotation around its axis, and translation is forward motion along the axis. The screw is a tool that combines these two motions.

[0036] To make the subsequent embodiments easier to understand, the following description is made by taking a seven-degree-of-freedom robot arm of SRS configuration as an example. Figure 2 As shown in the figure, the global coordinate system is represented by {G}, the tool coordinate system is represented by {T}, ω i (i=1,2,…,7) is the unit vector in the positive direction of each joint axis of the robot arm, r i (i=1,2,…,7) are the coordinates of any point on the joint axis.

[0037] based on Figure 2 The seven-DOF manipulator of the SRS configuration shown in the figure has a modeling process based on the forward kinematic equation of the screw as follows:

[0038] The basic unit of kinematic modeling based on screw theory is the screw ξ and the matrix exponential expression of the screw motion The kinematic rotation of a revolute joint can be expressed as:

[0039]

[0040] Where v i =r i ×ω i ,ω i is the unit vector in the positive direction of the axis of joint i (hereinafter referred to as the i-th joint, the larger the i value is, the closer it is to the end of the robot arm, i≤7 is a positive integer), r i is any point on the axis of the i-th joint in the global coordinate system. Then the matrix exponential expression of the i-th joint rotation is:

[0041]

[0042] In the formula, θ i is the rotation angle of the i-th joint, I 3×3 is a 3×3 identity matrix. The operator “^” represents the conversion of ω i ∈R 3 Map to From this, we can get the matrix exponential expression of each joint rotation Then the forward kinematics equation of the seven-degree-of-freedom manipulator can be expressed as:

[0043]

[0044] In the formula, g st (0) is the initial pose of the tool coordinate system relative to the global coordinate system.

[0045] also, Figure 2 The seven-degree-of-freedom manipulator shown also has a unique self-motion feature. Figure 3 The simplified schematic diagram shown. When the end position is fixed, the link SE and link EW can rotate continuously around the straight line SW. Set the plane BSW as the reference plane. When θ 3 = 0, the robot plane BSE V W coincides with the reference plane, and the seven-degree-of-freedom robot arm degenerates into a six-degree-of-freedom virtual robot arm. Plane SEW and plane SE V The angle between them is defined as the arm angle ψ. In addition, in order to control the global manifold of the manipulator, the configuration parameter GC is introduced k To specify the branch of the inverse kinematics solution. The configuration parameters of the second joint are called the first configuration parameters, expressed as GC 2 The configuration parameters of the second virtual joint of the virtual robot arm are called virtual configuration parameters, expressed as GC 2v The configuration parameters of the fourth joint are called the second configuration parameters, expressed as GC 4 The configuration parameters of the sixth joint are called the third configuration parameters, expressed as GC 6 .

[0046] It should be understood that configuration parameters are parameters used to describe the geometric configuration or branch selection of the manipulator in a specific motion state. Its main role is to help determine the unique solution of the manipulator in the inverse kinematics solution, especially when there are multiple solutions. Due to the redundancy of the seven-degree-of-freedom manipulator, its inverse kinematics usually has infinite solutions. The role of the arm angle and configuration parameters is to narrow the range of solutions by specifying certain conditions, thereby selecting a specific solution that meets the task requirements.

[0047] Based on the description of the forward kinematics equation, virtual robot arm, and configuration parameters in the above embodiment, Figure 1 Step S2 in the description is as follows:

[0048] S2, convert the forward kinematics equation into the form of Paden-Kahan subproblems to solve the end posture and obtain the rotation angle of each joint under the constraints of the preset configuration parameters.

[0049] As for the Paden-Kahan subproblems, it should be understood that the Paden-Kahan subproblems are a method for solving complex inverse kinematics problems in the kinematics of a robotic arm, which are divided into three types: Subproblem 1 deals with motion on a single rotation or translation axis, Subproblem 2 deals with the angle between two rotation axes, and Subproblem 3 deals with more complex geometric relationships. In this embodiment, by applying these subproblems, the inverse kinematics problem of a seven-degree-of-freedom robotic arm can be effectively simplified and decomposed. That is, when solving the angle of the fourth joint, Subproblem 3 is used to deal with specific geometric relationships, and when solving the angles of the first joint, the second joint, and the third joint, Subproblems 1 and 2 are used to deal with different rotational and translational motions, thereby greatly simplifying the solution process and making the kinematic modeling of the robotic arm more efficient and accurate.

[0050] In this embodiment, the seven joints of the robot arm are also referred to as the i-th joints, and the larger the i value is, the closer to the end of the robot arm is, and i is a positive integer of ≤7. In addition, the arm surface of the robot arm is overlapped with the reference plane and degenerated into a six-degree-of-freedom virtual robot arm. The arm surface is a plane formed by the shoulder, elbow, and wrist of the robot arm, and the reference plane represents the plane formed by the base, shoulder, and wrist of the robot arm; the six joints of the virtual robot arm are respectively referred to as the n-th virtual joints, and the larger the n value is, the closer to the end of the robot arm is, and n is a positive integer of ≤7 and n≠3. Except for the third joint, the joints of the virtual robot arm correspond one-to-one with the joints of the seven-degree-of-freedom robot arm. For example, the first virtual joint of the virtual robot arm corresponds to the first joint of the seven-degree-of-freedom robot arm, and the second virtual joint of the virtual robot arm corresponds to the second joint of the seven-degree-of-freedom robot arm. Other corresponding relationships can be obtained by analogy, and this embodiment will not be repeated.

[0051] In addition, the above-mentioned preset configuration parameters include the first configuration parameters of the second joint, the second configuration parameters of the fourth joint, the third configuration parameters of the sixth joint, and the virtual configuration parameters of the second virtual joint.

[0052] Based on the above description of the robotic arm, virtual robotic arm and preset configuration parameters, step S2 may include:

[0053] S2-1, using the end posture, the forward kinematics equation is converted into a third-kind Paden-Kahan subproblem for solution, and the rotation angle of the fourth joint under the constraint of the second configuration parameter is obtained.

[0054] Among them, the expression of the rotation angle of the fourth joint is:

[0055]

[0056] In the formula, θ 4 represents the rotation angle of the fourth joint, θ 0 = atan2(ω 4 T (u 1 '×v 1 '),u 1 ' T v 1 '), δ' 2 =δ 2 -|ω 4 T (p 1 -q 1 )| 2 ,δ=||g 1 r 6 -r 2 ||, p 1 =r 6 ,q 1 =r 2 , g 1 =g st (θ i )*g st (0) -1 , G.C. 4 represents the second configuration parameter, ω 4 Represents the unit vector in the positive direction of the fourth joint, r 6 represents any point on the axis of the sixth joint, r 2 represents any point on the axis of the second joint, g st (0) represents the initial position of the end tool coordinate system relative to the global coordinate system, g st (θ i ) represents the terminal pose.

[0057] Next, the derivation process of the rotation angle of the fourth joint is explained:

[0058] It should be understood that during the self-motion of the seven-degree-of-freedom robot arm, the angle of the fourth joint (elbow joint) remains unchanged; therefore, this embodiment first solves the angle of the fourth joint. st (θ i ) and multiply both sides of the forward kinematics equation by g st (0) -1 , we can get:

[0059]

[0060] In the formula, g 1 =g st (θ i )*g st (0) -1 Then, according to the characteristics of the spinor, if point r is located on the axis of joint i, then Established. From this we can get:

[0061]

[0062] In the formula, r 2 is the intersection of the axes of the first, second and third joints, r 6 It is the intersection of the axes of the fifth joint, the sixth joint and the seventh joint. According to formula (5), we can get:

[0063]

[0064] By taking advantage of the fact that the distance between two points in rigid body motion remains unchanged, we can obtain the following by taking the magnitude of both sides of equation (6):

[0065]

[0066] Since, Equation (7) conforms to the form of Paden-Kahan subproblem 3, where p 1 =r 6 ,q 1 =r 2 ,δ=||g 1 r 6 -r 2 ||. According to Paden-Kahan subproblem 3, we can get:

[0067]

[0068] In the formula, δ' 2 =δ 2 -|ω 4 T (p1 -q 1 )| 2 ,θ 0 It can be obtained by the following formula:

[0069] θ 0 = atan2(ω 4 T (u 1 '×v 1 '),u 1 ' T v 1 ') (9)

[0070] S2-2, obtaining the rotation angle of the first virtual joint and the rotation angle of the second virtual joint under the constraints of the virtual configuration parameters.

[0071] Among them, as introduced in the above embodiment, the virtual robot arm represents the six-degree-of-freedom robot arm degenerated after the arm surface of the robot arm coincides with the reference plane, the arm surface is the plane formed by the shoulder, elbow and wrist of the robot arm, and the reference plane represents the plane formed by the base, shoulder and wrist of the robot arm. The six joints of the virtual robot arm are respectively called the nth virtual joints, and the larger the n value is, the closer to the end of the robot arm, and n≤7 and n≠3 are positive integers. Therefore, except for the third joint, the joints of the virtual robot arm correspond one-to-one to the joints of the seven-degree-of-freedom robot arm.

[0072] In this embodiment, the expression of the rotation angle of the first virtual joint is:

[0073]

[0074] The expression of the rotation angle of the second virtual joint is:

[0075]

[0076] In the formula, represents the rotation angle of the first virtual joint, represents the rotation angle of the second virtual joint, c 2 =z 2 +r 2 , z 2 =α 2 ω 1 +β 2 ω 2 +γ 2 (ω 1 ×ω 2 ),

[0077] u 2 =p 2 -r2 ,v 2 =q 2 -r 2 , q 2 =g 1 r 6 , G.C. 2v represents the virtual configuration parameter, ω 1 Represents the unit vector in the positive direction of the first joint, ω 2 Represents the unit vector in the positive direction of the second joint, r 2 represents a point on the axis of the second joint, r 6 represents a point on the axis of the sixth joint, Represents the rotation of the fourth virtual joint.

[0078] Next, the derivation process of the rotation angles of the first virtual joint and the second virtual joint is described:

[0079] In the virtual robot, according to the forward kinematics equation (4), we can obtain:

[0080]

[0081] Multiply both sides of formula (10) by r 6 , we can get:

[0082]

[0083] Formula (11) conforms to the form of Paden-Kahan subproblem 2, where q 2 =g 1 r 6 Then equation (11) can be transformed into two Paden-Kahan subproblems 1:

[0084]

[0085] In the formula, c 2 =z 2 +r 2 , z 2 It can be obtained by the following formula:

[0086] z 2 =α 2 ω 1 +β 2 ω 2 +γ 2 (ω 1 ×ω 2 )

[0087]

[0088] In the formula, u 2 =p 2 -r 2 ,v 2 =q 2 -r 2 At this point, we can apply Paden-Kahan subproblem 1 to solve equation (12), and we can get:

[0089]

[0090] S2-3, according to the rotation angle of the first virtual joint and the rotation angle of the second virtual joint, obtain the rotation angle of the first joint, the rotation angle of the second joint, and the rotation angle of the third joint under the constraint of the first configuration parameter.

[0091] Among them, the rotation angles of the first joint, the second joint, and the third joint are respectively:

[0092]

[0093] In the formula, θ 1 represents the rotation angle of the first joint, θ 2 represents the rotation angle of the second joint, θ 3 represents the rotation angle of the third joint, a sij ,b sij ,c sij The matrix A is s ,B s ,C s The (i,j)th element of

[0094] 0 sw is the unit vector of the line between the shoulder and the wrist, ψ represents the arm angle of the robot, and GC 2 represents the first configuration parameter, The matrix exponential expression representing the rotation of the first virtual joint The posture matrix, The matrix exponential expression representing the rotation of the second virtual joint The posture matrix, The pose matrix representing the shoulder and elbow connection in the virtual robotic arm.

[0095] Next, the derivation process of the rotation angle of the first joint, the rotation angle of the second joint, and the rotation angle of the third joint is explained:

[0096] According to the virtual joint and You can get the link SE in the virtual robot arm V The posture matrix:

[0097]

[0098] According to the Rodriguez rotation formula, the posture matrix of the link SE in the actual robotic arm can be obtained:

[0099]

[0100] In the formula, The value of can be obtained according to the following formula:

[0101]

[0102] In the formula, 0 sw is the unit vector of vector sw, [ 0 sw×] is 0 The skew-symmetric matrix of sw. According to the coordinates of point S and point W, we can get vector sw.

[0103] Substituting formula (18) into formula (17), we can obtain:

[0104]

[0105] In the formula, According to the forward kinematics equation shown in formula (3), we can obtain:

[0106]

[0107] Combining equations (19) and (20), we can obtain:

[0108]

[0109] In the formula, a sij ,b sij ,c sij The matrix A is s ,B s ,C s The (i,j)th element of .

[0110] S2-4, using the end posture, the rotation angle of the first joint, the rotation angle of the second joint, the rotation angle of the third joint and the rotation angle of the fourth joint, the forward kinematics equation is converted into the first and second Paden-Kahan subproblems for solution, and the rotation angle of the fifth joint and the rotation angle of the sixth joint under the constraint of the third configuration parameters are obtained.

[0111] Among them, the expression of the rotation angle of the fifth joint is:

[0112]

[0113] The expression of the rotation angle of the sixth joint is:

[0114]

[0115] In the formula, θ 5 represents the rotation angle of the fifth joint, θ 6 represents the rotation angle of the sixth joint, c 5 =z 5 +r 6 , z 5 =α 5 ω 5 +β 5 ω 6 +γ 5 (ω 5 ×ω 6 ),

[0116] p 5 =r 7 ,q 5 =g 2 r 7 , ω 5 Represents the unit vector in the positive direction of the fifth joint, ω 6 Represents the unit vector in the positive direction of the sixth joint, r 7 Indicates a point on the axis of the seventh joint but not on the axis of the sixth joint. GC 6 Represents the third configuration parameter.

[0117] Next, the derivation process of the rotation angle of the fifth joint and the rotation angle of the sixth joint is explained:

[0118] Multiply both sides of formula (4) by You can get:

[0119]

[0120] In the formula,

[0121] Take a point r on the axis of the seventh joint, but not on the axis of the sixth joint 7 , multiply both sides of formula (22) by r 7 , we can get:

[0122]

[0123] Formula (23) conforms to the form of Paden-Kahan subproblem 2, where p 5 =r 7 ,q 5 =g2 r 7 Then equation (23) can be transformed into two Paden-Kahan subproblems 1:

[0124]

[0125] In the formula, c 5 =z 5 +r 6 , z 5 It can be obtained by the following formula:

[0126] z 5 =α 5 ω 5 +β 5 ω 6 +γ 5 (ω 5 ×ω 6 )

[0127]

[0128] In the formula, u 5 =p 5 -r 6 ,v 5 =q 5 -r 6 Applying Paden-Kahan subproblem 1 to solve equation (23), we can obtain:

[0129]

[0130] S2-5, using the end position, the rotation angle of the first joint, the rotation angle of the second joint, the third joint, the rotation angle of the fourth joint, the rotation angle of the fifth joint and the rotation angle of the sixth joint, the forward kinematics equation is converted into a first-order Paden-Kahan subproblem for solution to obtain the rotation angle of the seventh joint.

[0131] Among them, the expression of the rotation angle of the seventh joint is:

[0132]

[0133] In the formula, θ 7 represents the rotation angle p of the seventh joint 7 =r 7 ′,q 7 =g 3 r 7 ′, r 7 ' represents a point not on the axis of the seventh joint, r 7 represents a point on the axis of the seventh joint, ω 7Represents the unit vector in the positive direction of the seventh joint.

[0134] Next, the derivation process of the rotation angle of the seventh joint is explained:

[0135] Multiply both sides of equation (4) by You can get:

[0136]

[0137] In the formula,

[0138] Take a point r that is not on the axis of joint 7 7 ′, multiply both sides of formula (28) by r 7 ′, we can get:

[0139]

[0140] Equation (29) conforms to the form of Paden-Kahan subproblem 1, where p 7 =r 7 ′,q 7 =g 3 r 7 ′, we can get:

[0141]

[0142] Therefore, in this embodiment, the rotation angle of the fourth joint is first solved by the third type of Paden-Kahan subproblem. Then, the angle of the virtual joint is solved by the concept of the virtual robot arm, and the rotation angles of the first, second and third joints are further derived. Finally, the Paden-Kahan subproblem is used again to solve the rotation angles of the fifth, sixth and seventh joints in combination with these angles. This phased solution method not only ensures the accuracy of the solution, but also improves the overall solution efficiency.

[0143] Compared with the existing DH method, this embodiment uses the screw theory to solve the inverse kinematics, which not only simplifies the modeling difficulty, but also makes the multi-solution conditions clear and the number of solutions clearly visible under the constraints of the preset configuration parameters. Therefore, the simplicity of modeling and the efficiency of solving are significantly improved, and the flexibility and reliability of the robot arm operation are also enhanced.

[0144] Based on the same inventive concept as the method for solving the joint angle of a seven-degree-of-freedom manipulator based on a rotation provided in this embodiment, this embodiment also provides a seven-degree-of-freedom manipulator joint angle solving device based on a rotation, and the device includes at least one software function module that can be stored in a memory or solidified in an electronic device in the form of software. The processor in the electronic device is used to execute the executable module stored in the memory. For example, the software function module and computer program included in the device.

[0145] Please refer to Figure 4 , functionally speaking, the device may include:

[0146] The data acquisition module 11 is used to obtain the forward kinematics equation of the robot arm based on the screw and the end position of the robot arm;

[0147] The angle inverse solution module 12 is used to convert the forward kinematics equation into a Paden-Kahan subproblem form to solve the end posture and obtain the rotation angle of each joint under the constraints of the preset configuration parameters.

[0148] In this embodiment, the data acquisition module is used to implement Figure 1 In step S1, the angle inverse solution module 12 is used to implement Figure 1 Therefore, for the detailed description of each of the above modules, please refer to the specific implementation of the corresponding step.

[0149] Optionally, the seven joints of the robot arm are respectively referred to as n-th joints, and a larger value of n indicates a closer position to the end of the robot arm. The angle inverse solution module 12 is further specifically used for:

[0150] The forward kinematics equation is converted into a third-kind Paden-Kahan subproblem using the end-position to obtain the rotation angle of the fourth joint under the constraints of the preset configuration parameters.

[0151] Obtain the rotation angle of the first virtual joint and the rotation angle of the second virtual joint of the virtual robotic arm, wherein the virtual robotic arm represents the six-degree-of-freedom robotic arm degenerated after the arm surface of the robotic arm coincides with the reference plane, the arm surface is the plane formed by the shoulder, elbow, and wrist of the robotic arm, and the reference plane represents the plane formed by the base, shoulder, and wrist of the robotic arm; the six joints of the virtual robotic arm are respectively referred to as the nth virtual joints, the larger the n value is, the closer to the end of the robotic arm, n≤7 and n≠3 is a positive integer, and except for the third joint, the joints of the virtual robotic arm correspond one-to-one to the joints of the seven-degree-of-freedom robotic arm;

[0152] According to the rotation angle of the first virtual joint and the rotation angle of the second virtual joint, the rotation angle of the first joint, the rotation angle of the second joint, and the rotation angle of the third joint under the constraints of the preset configuration parameters are obtained;

[0153] The forward kinematic equation is converted into the first and second types of Paden-Kahan sub-problems for solution by using the end pose, the rotation angle of the first joint, the rotation angle of the second joint, the rotation angles of the third joint and the fourth joint, so as to obtain the rotation angle of the fifth joint and the rotation angle of the sixth joint under the constraints of the preset configuration parameters;

[0154] The forward kinematic equation is converted into the first type of Paden-Kahan sub-problem for solution by using the end pose, the rotation angle of the first joint, the rotation angle of the second joint, the rotation angles of the third joint, the fourth joint, the rotation angle of the fifth joint and the rotation angle of the sixth joint, so as to obtain the rotation angle of the seventh joint under the constraints of the preset configuration parameters.

[0155] In addition, in each embodiment of the present application, the functional modules can be integrated together to form an independent part, or each module can exist alone, or two or more modules can be integrated to form an independent part.

[0156] It should also be understood that if the above embodiments are implemented in the form of software function modules and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application, in essence, or the part that contributes to the prior art, or a part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium and includes several instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in the embodiments of the present application.

[0157] Therefore, this embodiment also provides a storage medium, which is a computer-readable storage medium. The storage medium stores a computer program, and when the computer program is executed by a processor, it implements the method for solving the joint angles of a seven-degree-of-freedom robotic arm based on screws provided in this embodiment. Among them, the storage medium can be various media such as a USB flash drive, a mobile hard disk, a read-only memory (ROM, Read-Only Memory), a random access memory (RAM, Random Access Memory), a magnetic disk, or an optical disc that can store program codes.

[0158] This embodiment also provides an electronic device for implementing the method for solving the joint angles of a seven-degree-of-freedom robotic arm based on screws. The electronic device may include a processor 22 and a memory 21. And, the memory 21 stores a computer program, and the processor reads and executes the computer program corresponding to the above embodiments in the memory 21 to implement the method for solving the joint angles of a seven-degree-of-freedom robotic arm based on screws provided in this embodiment.

[0159] Continue to see Figure 5 The electronic device further includes a communication unit 23. The memory 21, the processor 22 and the communication unit 23 are electrically connected to each other directly or indirectly through a system bus 24 to achieve data transmission or interaction.

[0160] The memory 21 may be an information recording device based on any electronic, magnetic, optical or other physical principle, used to record execution instructions, data, etc. In some embodiments, the memory 21 may be, but is not limited to, a volatile memory, a non-volatile memory, a storage drive, etc.

[0161] In some embodiments, the volatile memory may be a random access memory (RAM); in some embodiments, the non-volatile memory may be a read-only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable read-only memory (EEPROM), a flash memory, etc.; in some embodiments, the storage drive may be a disk drive, a solid-state drive, any type of storage disk (such as a CD, a DVD, etc.), or a similar storage medium, or a combination thereof, etc.

[0162] The communication unit 23 is used to send and receive data through a network. In some embodiments, the network may include a wired network, a wireless network, a fiber optic network, a telecommunication network, an intranet, the Internet, a local area network (LAN), a wide area network (WAN), a wireless local area network (WLAN), a metropolitan area network (MAN), a wide area network (WAN), a public switched telephone network (PSTN), a Bluetooth network, a ZigBee network, or a near field communication (NFC) network, or any combination thereof. In some embodiments, the network may include one or more network access points. For example, the network may include a wired or wireless network access point, such as a base station and / or a network switching node, through which one or more components of the service request processing system may be connected to the network to exchange data and / or information.

[0163] The processor 22 may be an integrated circuit chip having a signal processing capability, and the processor may include one or more processing cores (e.g., a single-core processor or a multi-core processor). By way of example only, the processor may include a central processing unit (CPU), an application specific integrated circuit (ASIC), an application specific instruction set processor (ASIP), a graphics processing unit (GPU), a physical processing unit (PPU), a digital signal processor (DSP), a field programmable gate array (FPGA), a programmable logic device (PLD), a controller, a microcontroller unit, a reduced instruction set computer (RISC), or a microprocessor, or any combination thereof.

[0164] Understandably, Figure 5The structure shown is for illustration only. The electronic device may also have Figure 5 More or fewer components than shown, or with Figure 5 Different configurations are shown. Figure 5 The components shown may be implemented in hardware, software or a combination thereof.

[0165] It should be understood that the apparatus and method disclosed in the above-mentioned embodiments can also be implemented in other ways. The apparatus embodiments described above are merely schematic. For example, the flowcharts and block diagrams in the accompanying drawings show the possible architecture, functions and operations of the apparatus, methods and computer program products according to the multiple embodiments of the present application. In this regard, each box in the flowchart or block diagram can represent a module, a program segment or a part of a code, and the module, a program segment or a part of a code contains one or more executable instructions for implementing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in a different order from the order marked in the accompanying drawings. For example, two consecutive boxes can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram and / or the flowchart, and the combination of boxes in the block diagram and / or the flowchart can be implemented with a dedicated hardware-based system that performs a specified function or action, or can be implemented with a combination of dedicated hardware and computer instructions.

[0166] The above are only various implementations of the present application, but the protection scope of the present application is not limited thereto. Any person skilled in the art who is familiar with the present technical field can easily think of changes or substitutions within the technical scope disclosed in the present application, which should be included in the protection scope of the present application. Therefore, the protection scope of the present application should be based on the protection scope of the claims.

Claims

1. A method for solving the joint angle of a seven-degree-of-freedom manipulator based on rotation, characterized in that: The method comprises: Obtaining the forward kinematics equation of the robotic arm based on the screw and the end position of the robotic arm; The forward kinematics equation is converted into a Paden-Kahan subproblem to solve the end position and obtain the rotation angle of each joint under the constraints of preset configuration parameters.

2. The method for solving the joint angle of a seven-degree-of-freedom manipulator based on rotation according to claim 1, characterized in that: The seven joints of the robotic arm are respectively referred to as the i-th joints, where a larger i value indicates a joint closer to the end of the robotic arm, and i is a positive integer ≤ 7; The arm surface of the robotic arm is overlapped with the reference plane and degenerates into a virtual robotic arm with six degrees of freedom, wherein the arm surface is a plane formed by the shoulder, elbow and wrist of the robotic arm, and the reference plane represents a plane formed by the base, shoulder and wrist of the robotic arm; the six joints of the virtual robotic arm are respectively referred to as the nth virtual joints, and the larger the n value is, the closer to the end of the robotic arm is, and n≤7 and n≠3 is a positive integer. Except for the third joint, the joints of the virtual robotic arm correspond one-to-one to the joints of the seven-degree-of-freedom robotic arm; The preset configuration parameters include a first configuration parameter of the second joint, a second configuration parameter of the fourth joint, a third configuration parameter of the sixth joint, and a virtual configuration parameter of the second virtual joint; The forward kinematics equation is converted into a Paden-Kahan subproblem to solve the end position and posture, and the rotation angle of each joint under the preset configuration parameter constraints is obtained, including: The forward kinematics equation is converted into a third-kind Paden-Kahan subproblem by using the end position and solved to obtain a rotation angle of the fourth joint under the constraint of the second configuration parameter; Obtaining a rotation angle of a first virtual joint and a rotation angle of a second virtual joint under the constraints of the virtual configuration parameters; According to the rotation angle of the first virtual joint and the rotation angle of the second virtual joint, the rotation angle of the first joint, the rotation angle of the second joint, and the rotation angle of the third joint under the constraint of the first configuration parameter are obtained; The forward kinematics equation is converted into first and second Paden-Kahan subproblems for solving by using the end position, the rotation angle of the first joint, the rotation angle of the second joint, the third joint, and the fourth joint, so as to obtain the rotation angle of the fifth joint and the rotation angle of the sixth joint under the constraint of the third configuration parameter; The forward kinematics equation is converted into a first-class Paden-Kahan subproblem for solution using the end posture, the rotation angle of the first joint, the rotation angle of the second joint, the third joint, the rotation angle of the fourth joint, the rotation angle of the fifth joint and the rotation angle of the sixth joint to obtain the rotation angle of the seventh joint.

3. The method for solving the joint angle of a seven-degree-of-freedom manipulator based on rotation according to claim 2, characterized in that: The expression of the rotation angle of the fourth joint is: Wherein, θ4 represents the rotation angle of the fourth joint, θ0=atan2(ω4 T (u1'×v1'),u1' T v1'), δ' 2 =δ 2 -|ω4 T (p1-q1)| 2 , δ=||g1r6-r2||, p1=r6, q1=r2, g1=g st (θ i )*g st (0) -1 , GC4 represents the second configuration parameter, ω4 represents the unit vector in the positive direction of the fourth joint, r6 represents any point on the axis of the sixth joint, r2 represents any point on the axis of the second joint, g st (0) represents the initial position of the end tool coordinate system relative to the global coordinate system, g st (θ i ) represents the terminal posture.

4. The method for solving the joint angle of a seven-degree-of-freedom manipulator based on rotation according to claim 3 is characterized in that: The expression of the rotation angle of the first virtual joint is: The expression of the rotation angle of the second virtual joint is: In the formula, represents the rotation angle of the first virtual joint, represents the rotation angle of the second virtual joint, c2=z2+r2, z2=α2ω1+β2ω2+γ2(ω1×ω2), u2=p2-r2,v2=q2-r2, q2=g1r6,GC 2v represents the virtual configuration parameter, ω1 represents the unit vector in the positive direction of the first joint, ω2 represents the unit vector in the positive direction of the second joint, r2 represents a point on the axis of the second joint, r6 represents a point on the axis of the sixth joint, Represents the rotation of the fourth virtual joint.

5. The method for solving the joint angle of a seven-degree-of-freedom manipulator based on rotation according to claim 4, characterized in that: The rotation angles of the first joint, the second joint, and the third joint are respectively: Where θ1 represents the rotation angle of the first joint, θ2 represents the rotation angle of the second joint, θ3 represents the rotation angle of the third joint, and a sij ,b sij ,c sij The matrix A is s ,B s ,C s The (i,j)th element of 0 sw is the unit vector of the line between the shoulder and the wrist, ψ represents the arm angle of the manipulator, GC2 represents the first configuration parameter, The matrix exponential expression representing the rotation of the first virtual joint The posture matrix, The matrix exponential expression representing the rotation of the second virtual joint The posture matrix, The pose matrix representing the shoulder and elbow connection in the virtual robotic arm.

6. The method for solving the joint angle of a seven-degree-of-freedom manipulator based on rotation according to claim 5, characterized in that: The expression of the rotation angle of the fifth joint is: The expression of the rotation angle of the sixth joint is: Wherein, θ5 represents the rotation angle of the fifth joint, θ6 represents the rotation angle of the sixth joint, c5=z5+r6, z5=α5ω5+β5ω6+γ5(ω5×ω6), p5=r7,q5=g2r7, ω5 represents a unit vector in the positive direction of the fifth joint, ω6 represents a unit vector in the positive direction of the sixth joint, r7 represents a point on the axis of the seventh joint but not on the axis of the sixth joint, and GC6 represents the third configuration parameter.

7. The method for solving the joint angle of a seven-degree-of-freedom manipulator based on rotation according to claim 6, characterized in that: The rotation angle of the seventh joint is: Wherein, θ7 represents the rotation angle of the seventh joint p7=r7′, q7=g3r7′, r7′ represents a point that is not on the axis of the seventh joint, r7 represents a point on the axis of the seventh joint, and ω7 represents a unit vector in the positive direction of the seventh joint.

8. A seven-degree-of-freedom robot arm joint angle solving device based on rotation, characterized in that: The device comprises: A data acquisition module, used to acquire the forward kinematics equation of the robotic arm based on the screw and the end position of the robotic arm; The angle inverse solution module is used to convert the forward kinematics equation into a Paden-Kahan subproblem form to solve the end posture and obtain the rotation angle of each joint under the constraints of preset configuration parameters.

9. A storage medium, characterized in that: The storage medium stores a computer program, and when the computer program is executed by a processor, the method for solving the joint angle of a seven-degree-of-freedom robotic arm based on rotation as described in any one of claims 1 to 7 is implemented.

10. An electronic device, characterized in that: The electronic device includes a processor and a memory, wherein the memory stores a computer program. When the computer program is executed by the processor, the method for solving the joint angle of a seven-degree-of-freedom robotic arm based on rotation as described in any one of claims 1 to 7 is implemented.

Images