Control method based on prismatic-rotary joint composite inverse kinematics model of robot arm

Abstract

This invention relates to a control method for a robotic arm based on a composite translational-rotational joint inverse kinematics model, comprising the following steps: selecting rotational joints to form a rotational subchain and selecting translational joints to form a translational subchain according to the type of each joint in the cascaded arm; constructing inverse kinematics models for the translational and rotational subchains respectively, based on motion constraints and different parallel relationships; and using the joint values ​​obtained from the inverse kinematics models as target values ​​to drive the movement of each joint of the robotic arm. This invention effectively solves the pose coupling problem caused by the rotational joint offset of the PR arm. Decomposing the PR arm into two subchains and solving them separately based on the parallel relationships of the axes of each subchain reduces the difficulty of deriving the inverse kinematics of the PR arm. It effectively solves the problem of balancing efficiency and accuracy in solving redundant parameters. This results in more continuous and smooth changes in trajectory points, thus playing an important role in the stable operation of the robotic arm.

Description

Technical Field

[0001] This invention belongs to the field of robotics and automation, and specifically relates to a robot control method based on a general robotic arm inverse kinematics model. Background Technology

[0002] Compared to traditional fully rotary joint robotic arms, serial arms composed of prismatic and revolute joints (PR arms) offer superior flexibility and a larger workspace. PR arms are highly applicable in aerospace, military, manufacturing, and medical services.

[0003] Inverse kinematics of robotic arms is fundamental to trajectory planning and control. However, current methods for analyzing the pose coupling problem caused by rotary joint offsets can only be performed on specific configurations, and these analyses are dependent on the configuration and the experience of the scientists. These factors make it difficult to obtain the inverse kinematics of robotic arms, thus hindering real-time control.

[0004] Redundant parameters in a robotic arm can alter its shape to achieve sub-tasks such as obstacle avoidance and joint-limit avoidance without affecting the primary task (end-effector pose). However, solving for redundant parameters in most robotic arms is a nonlinear problem, making it difficult to analytically derive the expressions for these parameters. Therefore, intelligent optimization algorithms are needed to find approximate solutions for the redundant parameters, enabling precise control of robotic arms with redundant joints. Generally, intelligent optimization algorithms can easily reduce the solution time by decreasing the number of particles and the maximum number of iterations; however, this can lead to incomplete convergence, resulting in robotic arm jitter. Summary of the Invention

[0005] To address the shortcomings of existing technologies, this invention provides an inverse kinematics control method for PR arm with rotary joint offsets and redundant degrees of freedom. Based on the pose coupling problem caused by the rotary joint offsets of the PR arm, the PR arm is split into two sub-chains and the inverse kinematics of each sub-chain is solved separately according to the parallel relationship of the rotation axes of each sub-chain, thereby realizing the derivation of the PR arm inverse kinematics, and then performing robot control based on the inverse kinematics.

[0006] The technical solution adopted by this invention to achieve the above objectives is: a control method based on the inverse kinematics model of a robotic arm with a composite translational and rotational joint, comprising the following steps:

[0007] Based on the type of each joint in the tandem arm, rotary joints are selected to form a rotary subchain, and translational joints are selected to form a translational subchain.

[0008] For translational and rotational subchains respectively, inverse kinematics models are constructed based on motion constraints and different parallel relationships;

[0009] The joint values ​​obtained from the inverse kinematics model are used as target values ​​to drive the movement of each joint of the robotic arm.

[0010] The specific parallel relationships are as follows:

[0011] For rotation subchains and translation subchains, there are two kinds of relationships between the joint directions: parallel and non-parallel. Based on the joint directions of the rotation subchain and translation subchain at the zero position, they are divided into three categories: at least two non-parallel relationships, only one non-parallel relationship, and all joint directions are parallel.

[0012] The existence of at least two non-parallel relationships means that at least two of the joint directions are not parallel.

[0013] The aforementioned non-parallel relationship refers to a situation where, among multiple joint directions, only two different joint directions exist.

[0014] The statement that all joint directions are parallel means that in at least one joint direction, all joint directions are parallel.

[0015] The joint direction refers to the direction of movement for translational joints and the direction of the axis of rotation for rotational joints.

[0016] For the rotating subchain, an inverse kinematics model is constructed based on motion constraints and different parallel relationships, where each joint direction is in the case that there are at least two non-parallel relationships:

[0017] For a rotating subchain with at least two non-parallel relationships, i.e. and R ω a , R ω b , R ω c Let a, b, and c represent the joint directions of the a-th, b-th, and c-th joints of the rotation subchain, respectively. This represents the current orientation of the i-th joint of the rotating subchain, where i = a, b, c, a < b < c; R The target orientation of the rotating subchain, R θ a , R θ b and R θ c Representing the a-th and a-th subchains respectively bThe joint values ​​of the c joints are: I3 is a 3rd order identity matrix; R1 is the attitude matrix composed of redundant joints from the base to joint a; R2 is the attitude matrix composed of redundant joints from joint a to joint b; R3 is the attitude matrix composed of redundant joints from joint b to joint c; and R4 is the attitude matrix composed of redundant joints from joint c to the actuator end effector.

[0018]

[0019] in, when At that time, determine

[0020]

[0021] in,

[0022] For rotating subchains, an inverse kinematics model is constructed based on motion constraints and different parallel relationships, where each joint direction is assumed to have only one non-parallel relationship:

[0023] When the rotating subchain encounters an issue during operation or R ω b ||R3 R ω c At this point, the spin subchain becomes singular and degenerates into a system with only one non-parallel relationship. The form;

[0024] R ω a , R ω b , R ω c Let a, b, and c represent the joint directions of the a-th, b-th, and c-th joints of the rotation subchain, respectively, where a < b; R is the target pose of the rotation subchain. R θ a , R θ b and R θ c I1 represents the joint values ​​of the a, b, and c joints of the rotating subchain, respectively; I3 is a 3rd order identity matrix; R1 is the attitude matrix composed of redundant joints from the base to joint a; R2 is the attitude matrix composed of redundant joints from joint a to joint b; R3 is the attitude matrix composed of redundant joints from joint b to joint c; and R4 is the attitude matrix composed of redundant joints from joint c to the actuator end effector.

[0025]

[0026] in, ub =R2R3R T R1 R ω a .

[0027] For rotating subchains, an inverse kinematics model is constructed based on motion constraints and different parallel relationships, where the joint directions are all parallel:

[0028] When the rotating subchain is in operation R ω a ||R2 R ω b At this time, the rotating subchain becomes singular and degenerates into a form where all joints are parallel;

[0029] R ω a , R ω b , R ω c These represent the joint directions of the a-th, b-th, and c-th joints of the rotation subchain, respectively. This represents the current pose of the i-th joint of the rotation subchain, where i = a, b, c, a < b < c; R is the target pose of the rotation subchain. R θ a , R θ b and R θ c I1 represents the joint values ​​of the a, b, and c joints of the rotating subchain, respectively; I3 is a 3rd order identity matrix; R1 is the attitude matrix composed of redundant joints from the base to joint a; R2 is the attitude matrix composed of redundant joints from joint a to joint b; R3 is the attitude matrix composed of redundant joints from joint b to joint c; and R4 is the attitude matrix composed of redundant joints from joint c to the actuator end effector.

[0030]

[0031] in, for The sine value, tr represents the trace of the matrix.

[0032] For the translational subchain, an inverse kinematics model is constructed based on motion constraints and different parallel relationships, where each joint direction is in the case that at least two non-parallel relationships exist:

[0033] For a translation subchain that has at least two non-coplanar and non-parallel relationships, P υ a , P υ b , P υc These represent the joint directions of the a-th, b-th, and c-th joints of the translation subchain, respectively, where a < b < c. P θ a , P θ b and P θ c Let P represent the joint values ​​of the a-th, b-th, and c-th joints of the translation subchain, respectively. offset The offset generated by the rotating subchain to satisfy the attitude constraints. R ω0 , R θ0 Let and represent any 3rd-order unit vector and 0, respectively, satisfying .

[0034] P is the target position of the tandem arm, P1 is the position matrix formed by redundant joints from the base to joint a, P2 is the position matrix formed by redundant joints from joint a to joint b, P3 is the position matrix formed by redundant joints from joint b to joint c, and P4 is the position matrix formed by redundant joints from joint c to the actuator end.

[0035] Let P′ = PP offset -P1-P2-P3-P4,V=[ P υ a P υ b P υ c ],θ=[ P θ a P θ b P θ c ] T ,

[0036] V 3×3 θ 3×1 =P3′ ×1

[0037] because P υ a , P υ b and P υ c Since they are linearly independent, rank(V) = rank(V, P′) = 3, thus the above formula has a unique solution θ = V. -1 P′.

[0038] For the translational subchain, an inverse kinematics model is constructed based on motion constraints and different parallel relationships, where each joint direction is assumed to have only one non-parallel relationship:

[0039] During the translational subchain motion, P υ a , P υ b , P υ c When linearly dependent, the translated subchains become singular and degenerate into a system with only one non-parallel relationship. The form;

[0040] P υ a , P υ b , P υ c These represent the joint directions of the a-th, b-th, and c-th joints of the translation subchain, respectively, where a < b. P θ a , P θ b and P θ c Let P represent the joint values ​​of the a-th, b-th, and c-th joints of the translation subchain, respectively. offset The offset generated by the rotating subchain to satisfy the attitude constraints. R ω0、 R Let θ0 represent any 3rd-order unit vector and 0, respectively, satisfying the condition...

[0041] P is the target position of the tandem arm, P1 is the position matrix formed by redundant joints from the base to joint a, P2 is the position matrix formed by redundant joints from joint a to joint b, P3 is the position matrix formed by redundant joints from joint b to joint c, and P4 is the position matrix formed by redundant joints from joint c to the actuator end.

[0042] Let P′ = PP offset -P1-P2-P3, V=[ P υ a P υ b ], θ=[ P θ a P θ b ] T ,

[0043] V 3×2 θ 2×1 =P3′ ×1

[0044] because When rank(V) = 2, there is a unique solution θ = V. + P′; where rank(V)=2, V +The left inverse of V + =(V T V) -1 V T .

[0045] For the translational subchain, an inverse kinematics model is constructed based on motion constraints and different parallel relationships, where each joint direction is assumed to be parallel to all joint directions:

[0046] When the translation subchain is running P υ a || P υ b At this time, the translation subchain becomes singular and degenerates into a form where all joints are parallel;

[0047] P υ a , P υ b , P υ c Let a, b, and c represent the joint directions of the a-th, b-th, and c-th joints of the translation subchain, respectively. P θ a , P θ b and P θ c Let P represent the joint values ​​of the a-th, b-th, and c-th joints of the translation subchain, respectively. offset The offset generated by the rotating subchain to satisfy the attitude constraints. R ω0、 R θ0 represents any 3rd-order unit vector and 0, respectively, satisfying

[0048] P is the target position of the tandem arm, P1 is the position matrix formed by redundant joints from the base to joint a, P2 is the position matrix formed by redundant joints from joint a to joint b, P3 is the position matrix formed by redundant joints from joint b to joint c, and P4 is the position matrix formed by redundant joints from joint c to the actuator end.

[0049] Let P′ = PP offset -P1-P2,

[0050] P υ a P θ a =P′

[0051] When rank( P υ a When P′=1, the above formula has a unique solution. in P υ aThe ranks are full. for P υ a Left Reverse

[0052] After constructing the inverse kinematics model, if redundant parameters exist in the series arm, the redundant parameter values ​​are solved using CQPSO based on the subproblems of the series arm, including the following steps:

[0053] Step 3.1: Characterize the joint values ​​of redundant joints as particle positions, where the joint values ​​are joint angles or joint displacements; set the population particle count N and the maximum number of iterations t. max ;

[0054] Step 3.2: According to and Randomly initialize the particle positions;

[0055] Where, p max p represents the maximum value of the particle position. min p is the minimum position of the particle. gbest p is the best historical position for the population. range represents the maximum range of variation of redundant parameters between step sizes, and j represents the number of optimization iterations;

[0056] Step 3.3: Calculate the fitness function value for each particle to update the historical best position of the i-th particle. j p best,i (t), Update the historical best position of the population p gbest (t);

[0057] Step 3.4: Update particle positions

[0058] in,

[0059] Where, α max Let α be the initial value. min p is the final value of α. mbest u represents the average optimal position of the population. i r i , The number is a random floating-point number, where i represents the i-th particle;

[0060] Step 3.5: Update particle position p i After (t+1), complete one iteration, increment the iteration count by one, and return to step 3.3 until the iteration count t reaches the maximum iteration count t. max At this point, the optimal redundant joint value is obtained.

[0061] The control system based on the inverse kinematics model of a robotic arm with a combination of translational and rotary joints includes:

[0062] The sub-chain extraction module is used to select rotary joints to form rotary sub-chains and select translational joints to form translational sub-chains based on the type of each joint of the serial arm.

[0063] The inverse kinematics model building module is used to construct inverse kinematics models for translational subchains and rotational subchains respectively, based on motion constraints and different parallel relationships.

[0064] The robot control module is used to drive the movement of each joint of the robotic arm based on the joint values ​​obtained from the inverse kinematics model as target values.

[0065] The present invention has the following beneficial effects and advantages:

[0066] 1. This invention effectively solves the pose coupling problem caused by the rotational joint offset of the PR arm.

[0067] 2. Decomposing the PR arm into two sub-chains and solving them separately based on the parallel relationship of the axes of each sub-chain reduces the difficulty of deriving the inverse kinematics of the PR arm.

[0068] 3. It effectively solves the problem of balancing efficiency and accuracy in solving redundant parameters.

[0069] 4. This makes the changes in trajectory points more continuous and smooth, which plays an important role in the stable operation of the robotic arm. Attached Figure Description

[0070] Figure 1a Simplified diagram 1 for a rotating subchain with at least two non-parallel relationships;

[0071] Figure 1b Simplified diagram 2 for a rotating subchain with at least two non-parallel relationships;

[0072] Figure 2a Simplified diagram of a rotating subchain with only one type of non-parallel relationship (Figure 1);

[0073] Figure 2b Simplified diagram of a rotating subchain with only one type of non-parallel relationship (Figure 2);

[0074] Figure 3a Simplified diagram of a rotating subchain in which all joints are parallel;

[0075] Figure 3b Simplified diagram of a rotating subchain with all joints parallel (Figure 2);

[0076] Figure 4a Simplified diagram 1 of translation subchains with at least two non-parallel relationships;

[0077] Figure 4bSimplified diagram 2 shows translation subchains with at least two non-parallel relationships;

[0078] Figure 5a Simplified diagram of a translation subchain with only one type of non-parallel relationship (Figure 1);

[0079] Figure 5b Simplified diagram of translation subchains with only one type of non-parallel relationship (Figure 2);

[0080] Figure 6a Simplified diagram of a translational subchain in which all joints are parallel (Figure 1);

[0081] Figure 6b Simplified diagram of a translational subchain in which all joints are parallel (Figure 2);

[0082] Figure 7 An overall structural diagram of a PR arm is shown in the embodiment;

[0083] Figure 8 The diagram shows the reconstructed rotating subchain structure of the 7-DOF PR arm in the embodiment.

[0084] Figure 9 The diagram shows the reconstructed translational subchain structure of the 7-DOF PR arm in the embodiment.

[0085] Figure 10 The motion control block diagram of the 7-DOF PR arm in the embodiment. Detailed Implementation

[0086] The present invention will now be described in further detail with reference to the accompanying drawings and embodiments.

[0087] For PR arms with redundant degrees of freedom, existing intelligent optimization algorithms can easily reduce solution time by decreasing the number of particles and the maximum number of iterations. However, this can lead to incomplete convergence, causing the robotic arm to vibrate. By intervening in the position and offset direction of particles in the previous optimization, the current optimization result can be made closer to the previous result, thus making the changes in trajectory points more continuous and smooth.

[0088] This invention simplifies the kinematic model of the PR arm by splitting and reconstructing it according to joint type, obtaining rotational and translational subchains. This solves the pose coupling problem caused by rotational joint offsets and reduces the difficulty of PR arm inverse kinematics derivation by dividing the rotation axis relationships of the subchains, thus achieving precise robot control. Addressing the challenge of balancing efficiency and accuracy in optimizing redundant parameters for robotic arms with redundant degrees of freedom, this invention proposes a method using Continuous Quantum Particle Swarm Optimization (CQPSO) to optimize redundant parameters. In a continuous trajectory, redundant parameters are continuously changing. Therefore, when solving for redundant parameters using intelligent optimization algorithms, the global optimum of the j-th optimization is always close to the global optimum of the (j-1)-th optimization. This property is used to obtain CQPSO for optimizing continuously changing variables. By using CQPSO to optimize redundant parameters, subtasks such as the joint limit problem of translational joints can be solved.

[0089] Step 1: Based on the type of each joint in the tandem arm, divide the tandem arm into translational subchains and rotational subchains;

[0090] Step 2: Obtain the analytical solution expression based on the motion constraints of the subchains and different parallel relationships;

[0091] Step 3: If redundant parameters exist in the series arm, solve for the redundant parameter values ​​using CQPSO based on the subproblems of the series arm, as follows:

[0092] Step 3.1: Based on the specific subproblem of the trajectory points, set the number of particles N and the maximum number of iterations t in the optimization algorithm population. max ;

[0093] Step 3.2: Based on the upper and lower limits of particle position j p max , j p min To initialize the position of each particle;

[0094] Step 3.3: Calculate the fitness function value for each particle;

[0095] Step 3.4: Update the particle's position p i (t);

[0096] Step 3.5: Repeat steps 3.3 and 3.4 until the number of iterations t reaches the maximum number of iterations t. max Complete this optimization, determine the optimal value of the redundant parameters, update the fitness function value, and proceed to the next optimization (optimize the subproblem corresponding to the next trajectory point);

[0097] Step 4: Calculate the remaining joint values ​​based on the actual screw coordinates of the tandem arms, the redundant parameter values ​​(if they exist), and the analytical solution expression.

[0098] Step 5: Use the translation amount of the translation joints in the translation subchain, the rotation angle of the rotation joints in the rotation subchain, and the rotation angle and translation amount of the redundant joints as reference values ​​to control the movement of each joint in a closed loop.

[0099] In step 2, the attitude constraint equations are obtained according to the type of the subchain. and position constraint equations in

[0100] In step 2, the attitude constraint equations that require at least two non-parallel relationships in the rotating subchain are simplified to: Solve R θ a , R θ b and R θ c The attitude constraint equation with only one non-parallel relationship simplifies to: Solve R θ a and R θ b The attitude constraint equations, where all joints are parallel, are simplified to: It can be solved R θ a .

[0101] In step 2, the position constraint equations for at least two non-parallel relationships in the translation subchain are simplified to: P υ a P θ a + P υ b P θ b + P υ c P θ c =PP offset -P1-P2-P3-P4, solve for P θ a , P θ b , P θ c The position constraint equation with only one non-parallel relationship simplifies to: P υ a P θ a + P υ b P θb =PP offset -P1-P2-P3, solve for P θ a and P θ b The position constraint equation, where all joints are parallel, simplifies to: P υ a P θ a =PP offset -P1-P2 can be solved P θ a .

[0102] In step 3.4, according to p i (t), j p mbest (t), p gbest (t) and p best,i (t), update particle position p i (t+1).

[0103] In step 3, by intervening in the position and offset direction of the particles in each optimization, the optimization result of this time is made closer to the optimization result of the previous time.

[0104] In step 3.2, the upper limit of particle position in the j-th optimization. j p max Depend on The lower limit of the particle's position is determined. j p min Depend on Decide.

[0105] In step 3.4, during the j-th optimization, j p mbest The update formula for (t) is derived from Decision, among which Set the maximum value of β based on experience. max Set the minimum value β to 0.95. min It is 0.05.

[0106] The control method of the present invention based on the inverse kinematics model of a robotic arm with a combination of translational and rotational joints includes the following steps:

[0107] Step 1: By splitting and reconstructing the PR arm according to joint type, rotational and translational subchains are obtained. There are two types of relationships between the joint directions of each subchain: parallel and non-parallel. This affects the subsequent inverse kinematics solution; therefore, it is necessary to determine the non-parallel relationships between the joint axes of each subchain. Based on the joint directions at the zero position of the subchain, they are divided into three categories: at least two non-parallel relationships, only one non-parallel relationship, and all joint directions are parallel.

[0108] Step 2: Assume the revolved subchain has n joints, defined as joints 1 to n from the base to the end. The attitude constraint equation of the revolved subchain is (1):

[0109]

[0110] in, R represents the current pose of the i-th joint; R is the target pose of the rotating subchain.

[0111] Since the values ​​of redundant joints do not affect the accuracy of the inverse kinematics solution in this invention, the values ​​of redundant joints are treated as known quantities in the analytical solution derivation, denoted as a constant matrix R. i .

[0112] For at least two non-parallel relationships ( and Select the rotating subchain of ) R θ a , R θ b and R θ c As unknowns to be determined, we have a < b < c. Based on formula (1), we obtain the attitude constraint equation (2):

[0113]

[0114] Where R1 is the attitude matrix formed by redundant joints from the base to joint a, R2 is the attitude matrix formed by redundant joints from joint a to joint b, R3 is the attitude matrix formed by redundant joints from joint b to joint c, and R4 is the attitude matrix formed by redundant joints from joint c to the actuator end effector.

[0115] R ω a , R ω b , R ω c Let a, b, and c represent the joint directions of the a-th, b-th, and c-th joints of the rotation subchain, respectively. R θ a , R θ b and R θ c Let I represent the joint values ​​of the a-th, b-th, and c-th joints of the rotation subchain, respectively; I3 is a 3rd order identity matrix.

[0116] Simplifying formula (2) yields formula (3):

[0117]

[0118] After simplification, the original rotation subchain with n joints is transformed into a rotation subchain with 3 joints, and the target pose changes from R to... like Figure 1a , Figure 1b As shown.

[0119] like And if it satisfies formula (4), then the solution can be found. R θ a , R θ b and R θ c .

[0120]

[0121]

[0122] in when hour, R θ a and R θ c It is uniquely determined by (6).

[0123]

[0124] in

[0125] When the rotating subchain encounters an issue during operation or R ω b ||R3 R ω c At this point, the spin subchain becomes singular and degenerates into a system with only one non-parallel relationship. Choose the format R θ a and R θ b As unknowns to be determined, a < b, we obtain the attitude constraint equation (8):

[0126]

[0127] R1 is the attitude matrix formed by redundant joints from the base to joint a, R2 is the attitude matrix formed by redundant joints from joint a to joint b, and R3 is the attitude matrix formed by redundant joints from joint b to the actuator end effector.

[0128] Simplifying formula (8) yields formula (9):

[0129]

[0130] After simplification, the original rotation subchain with n joints is transformed into a rotation subchain with 2 joints, and the target pose changes from R to... like Figure 2a , Figure 2b As shown.

[0131] like And satisfy Then the solution can be found R θ a , R θ b .

[0132]

[0133] in u b =R2R3R T R1 R ω a .

[0134] When the rotating subchain is in operation, ω a ||R2 R ω b At that time, the rotating subchain becomes singular and degenerates into a form where all joints are parallel. R θ a As unknowns to be determined, the attitude constraint equation (11) is obtained:

[0135]

[0136] R1 is the attitude matrix formed by redundant joints from the base to joint a, and R2 is the attitude matrix formed by redundant joints from joint a to the actuator end effector.

[0137] This can be simplified to formula (12):

[0138]

[0139] After simplification, the original rotation subchain with n joints is transformed into a rotation subchain with only 1 joint, and the target pose changes from R to... like Figure 3a , Figure 3b As shown.

[0140] like Then the solution can be found R θ a .

[0141]

[0142] in

[0143] Assuming that the translation subchain contains a total of m joints, the position constraint equation of the translation subchain can be obtained as (9).

[0144]

[0145]

[0146] Where P is the target position of PR arm; P represents the direction of the i-th translation joint in the translation subchain. offset Define the offset generated by the rotating subchain to satisfy the attitude constraints. Let and 0 represent any 3rd order unit vector and 0 respectively, satisfying

[0147] For a translation subchain that has at least two non-coplanar and non-parallel relationships, P υ a , P υ b , P υ c Linearly independent, choose P θ a , P θ b and P θ c As unknowns to be determined, we have a < b < c. According to formula (14), we obtain the position constraint equation (16):

[0148] P υ a P θ a + P υ b P θ b + P υ c P θ c =PP offset -P1-P2-P3-P4 (16)

[0149] Where P1 is the position matrix formed by redundant joints from the base to joint a, P2 is the position matrix formed by redundant joints from joint a to joint b, P3 is the position matrix formed by redundant joints from joint b to joint c, and P4 is the position matrix formed by redundant joints from joint c to the actuator end. P υ a , P υ b , P υ c Let a, b, and c represent the joint directions of the a-th, b-th, and c-th joints of the translation subchain, respectively.P θ a , P θ b and P θ c They represent the a-th, b-th, and c-th subchains respectively. c Joint values ​​for each joint.

[0150] Using formula (16), the original translation subchain containing m joints is transformed into a translation subchain containing 3 joints, and the target position changes from P to PP. offset -P1-P2-P3-P4, such as Figure 4a , Figure 4b As shown.

[0151] Let P′ = PP offset -P1-P2-P3-P4,V=[ P υ a P υ b P υ c ],θ=[ P θ a P θ b P θ c ] T (16) can be written as:

[0152] V 3×3 θ 3×1 =P′ 3×1 (17)

[0153] because P υ a , P υ b and P υ c Since they are linearly independent, rank(V) = rank(V, P′) = 3, and we can obtain the unique solution θ = V of formula (17). -1 P′.

[0154] During the translational subchain motion, P υ a , P υ b , P υ c When linearly dependent, the translated subchains become singular and degenerate into a single non-parallel relationship. The format. Choose. P θ a and P θ b As unknowns to be determined, we have a < b, which gives us the position constraint equation (18):

[0155] P υ a P θ a + P υ b P θ b =PP offset -P1-P2-P3 (18)

[0156] Where P1 is the position matrix formed by redundant joints from the base to joint a, P2 is the position matrix formed by redundant joints from joint a to joint b, and P3 is the position matrix formed by redundant joints from joint b to the actuator end.

[0157] Using formula (18), the original translation subchain containing m joints is transformed into a translation subchain containing 2 joints, and the target position changes from P to PP. offset -P1-P2-P3, such as Figure 5a , Figure 5b As shown.

[0158] Let P′ = PP offset -P1-P2-P3, V=[ P υ a P υ b ], θ=[ P θ a P θ b ] T (19) can be written as:

[0159] V 3×2 θ 2×1 =P′ 3×1 (19)

[0160] because When rank(V) = 2, there is a unique solution θ = V. + P′. Since rank(V) = 2, therefore V + The left inverse of V + =(V T V) -1 V T .

[0161] When the translation subchain is running P υ a || P υ b At that time, the translation subchain becomes singular and degenerates into a form where all joints are parallel. P θ aAs unknowns to be determined, the position constraint equation (20) is obtained:

[0162] P υ a P θ a =PP offset -P1-P2 (20)

[0163] Where P1 is the position matrix formed by redundant joints between the base and joint a, and P2 is the position matrix formed by redundant joints between joint a and the actuator end.

[0164] Using formula (20), the original translation subchain containing m joints is transformed into a translation subchain containing only 1 joint, and the target position changes from P to PP. offset -P1-P2, such as Figure 6a , Figure 6b As shown.

[0165] Let P′ = PP offset -P1-P2, (20) can be written as:

[0166] P υ a P θ a =P′ (21)

[0167] When rank( P υ a When P′)=1, formula (21) has a unique solution. because P υ a The ranks are full. for P υ a Left Reverse

[0168] Step 3: If redundant parameters exist in the robotic arm, solve for the redundant parameters based on CQPSO, including the following steps:

[0169] Step 3.1: Online optimization of redundant parameters has high real-time requirements, typically requiring the time for one inverse kinematics calculation to be within 8ms. The optimization algorithm population particle number N and the maximum number of iterations t are set. max This ensures that the solution time meets the requirements of solving the inverse kinematics of the robotic arm as much as possible;

[0170] Step 3.2: According to and Where, p range Based on experience, the particle positions are randomly initialized within the specific optimization step size and optimization problem.

[0171] Step 3.3: For one or more specific sub-tasks in the redundant robotic arm's motion trajectory (such as moving away from obstacles, moving away from joint limits, minimizing energy, etc.), establish a suitable fitness function and calculate the fitness function value for each particle. The calculation of the fitness function value mostly requires the use of forward kinematics, so in this step, the redundant parameter value represented by each particle can be substituted back into the analytical solution obtained in steps 1 to 3 to easily calculate the fitness function value corresponding to each particle.

[0172] Step 3.4: Update particle positions

[0173] in, Where, α max Let α be the initial value. min This is the final value of α;

[0174] Step 3.5: Update particle position p i After (t+1), complete one iteration, increment the iteration count by one, and repeat until the iteration count t reaches the maximum iteration count t. max This optimization is complete;

[0175] Step 4: Calculate the remaining joint values ​​based on the actual screw coordinates of the tandem arms, the redundant parameter values ​​(if they exist), and the analytical solution expression.

[0176] Step 5: Based on the obtained joint values, use feedforward and feedback control algorithms to calculate the drive quantities of the robotic arm to achieve the main task and sub-tasks, such as... Figure 10 Where T represents the target pose matrix of the robotic arm, and q 目标 This represents the target joint value vector calculated by the robotic arm using inverse kinematics. q 真实 This represents the actual joint value vector of the robotic arm. q 误差 This represents the difference between the target joint value vector and the actual joint value vector of the robotic arm. q 驱动 This represents the drive joint value vector of the robotic arm.

[0177] The present invention will be further described below with reference to specific embodiments.

[0178] Combination Figure 7 The specific implementation plan is explained below: Taking a 7DOF PR arm used for angiography as an example.

[0179] To verify the effectiveness of the inverse kinematics method for the PR arm presented in this paper, an inverse kinematics simulation was performed on a 7-DOF PR arm. The configuration of this PR arm is PPRRRPR from top to bottom, and the joint numbers are numbered sequentially from 1 to 7. Its joints 3, 4, and 5 have rotational joint offsets.

[0180] Table 1 shows the motion spinor coordinate parameters of the 7-DOF PR arm in the embodiment.

[0181] Table 1. Kinematic spinor coordinate parameters of the 7-DOF PR arm

[0182]

[0183]

[0184] Figure 8 , 9 A schematic diagram of the two subchains after reconstructing a 7-DOF PR arm. Figure 10 The motion control block diagram for a 7-DOF PR arm.

[0185] The parameters and their meanings in this invention are shown in Table 2.

[0186] Table 2: Parameters and Meanings (Reference Table)

[0187]

[0188]

[0189] Pseudocode algorithm table for the j-th optimization of Algorithm 1 CQPSO;

[0190] like Figure 8 As shown, the method for establishing the 7-DOF PR arm rotation subchain is to extract the rotational joints 3, 4, 5 and 7 in the PR arm and connect them sequentially to form a serial robotic arm, with the tool center point coordinate system xyz as the reference coordinate system.

[0191] like Figure 9 As shown, the method for establishing the translation subchain of the 7-DOF PR arm is to extract the rotational joints 1, 2 and 6 in the PR arm and connect them sequentially to form a serial robotic arm, with the tool center point coordinate system xyz as the reference coordinate system.

[0192] Depend on Figure 8 It can be seen that the rotation subchain of the PR arm in this invention example has at least two non-parallel relationships. Regardless of which joint is chosen as the redundancy parameter, the remaining three joints all satisfy the condition that the rotation axis of the second joint is not parallel to the rotation axes of the other two joints. Therefore, θ4 and θ7 are chosen as redundancy parameters to derive the analytical solution of the rotation subchain of the PR arm in this invention example to verify the universality of the method of this invention.

[0193] The result of calculating the inverse kinematics of the rotating subchain using joint 7 as a redundant parameter is as follows:

[0194] Remember rij Let R be the element in the i-th row and j-th column. According to (5), we can solve for (22):

[0195]

[0196] when At that time, θ3 and θ5 are uniquely determined by (6):

[0197]

[0198] The result of calculating the inverse kinematics of the rotating subchain using joint 4 as a redundant parameter is as follows:

[0199] Remember r ij Let R be the element in the i-th row and j-th column. According to (5), we can solve for (24):

[0200]

[0201] when At that time, θ3 and θ7 are uniquely determined by (6):

[0202]

[0203] according to Figure 9 It can be seen that the translation subchain of PR arm belongs to the translation subchain with at least two non-parallel relationships, so according to (17), the analytical solution of the translation subchain is (26).

[0204]

[0205] To verify the correctness of the solution, we provided an arbitrary set of joint angles and obtained the position and orientation of the end effector using forward kinematics. Using the calculated position and orientation of the end effector as known conditions, we solved for the robot's joint angles using inverse kinematics. Comparing the given joint angles with the calculated joint angles, we found that they were identical, thus verifying the correctness of the algorithm.

[0206] In a practical example of this invention, θ4 is selected as a redundancy parameter, which includes the joint physical limit. θ1, θ2, and θ6 are nonlinear functions of θ4, and it is difficult to obtain an analytical expression for θ4 that satisfies the physical limits of the joints. Therefore, an intelligent optimization algorithm is needed to solve for the value of θ4, enabling the redundant robotic arm to perform null-space motion, thereby achieving the subtask of joint limit avoidance for the redundant robotic arm.

[0207] To keep the robotic arm as far away from joint limits as possible, a fitness function is established as follows: When the joint value θ i As (i = 1, 2, 6) gets closer to the joint limit, F increases, and θ increases. iAs (i = 1, 2, 6) moves further away from the joint limits, F decreases, where θ i,max θ represents the positive joint limit of the i-th joint in the actual example. i,min This represents the negative joint limit of the i-th joint in the actual example.

[0208] In the practical example of this invention, the target trajectory of the 7DOF robotic arm is a straight line trajectory.

[0209] Starting pose

[0210] Final pose

[0211] After linear interpolation of the path, 1000 target pose points are obtained. Since the trajectory points are continuously changing and the fitness function F is a continuous function of θ4, the global optimum of θ4 is continuously changing in the given straight trajectory, which is consistent with the applicable conditions of CQPSO.

[0212] Since the control cycle of the imaging robot is 8ms, and considering the time consumed in signal transmission and data processing, the single-step solution time for redundant parameters needs to be less than 7ms to meet the real-time control requirements of the imaging robot. Testing showed that when the number of particles is 5 and the maximum number of iterations is 10, the optimization time for a single trajectory point in CQPSO is exactly less than 7ms.

[0213] Simulation experiments show that CQPSO's average fitness function value is better than that of Particle Swarm Optimization (PSO) and PSO with quantum behavior, indicating that the redundant parameters it solves can allow the robotic arm in the embodiment to move further away from the joint limits. CQPSO's average solution time is much shorter than other intelligent optimization algorithms, indicating that CQPSO can achieve high efficiency while ensuring solution accuracy.

[0214] In summary, by decomposing and reconstructing the PR arm according to joint type, rotational and translational subchains are obtained, and analytical solutions for both are derived, simplifying the kinematic model of the PR arm. This method can solve analytical solutions for PR arms where the translational subchain has at least two non-coplanar, non-parallel relationships or where the rotational subchain is non-redundant.

[0215] The combination of zero-space motion of the robotic arm and CQPSO enables real-time joint limit avoidance, making the robotic arm in the actual example run smoothly and stay away from joint limits.

[0216] The above description is merely a specific example of the present invention and does not constitute any limitation on the present invention. Obviously, those skilled in the art, after understanding the content and principles of the present invention, may make various modifications and changes in form and details without departing from the principles and structure of the present invention. However, these modifications and changes based on the ideas of the present invention are still within the scope of protection of the claims of the present invention.

Claims

1. A control method based on the inverse kinematics model of a robotic arm with a composite translational-rotational joint, characterized in that, Includes the following steps: Based on the type of each joint in the tandem arm, rotary joints are selected to form a rotary subchain, and translational joints are selected to form a translational subchain. For translational and rotational subchains respectively, inverse kinematics models are constructed based on motion constraints and different parallel relationships. After constructing the inverse kinematics model, if there are redundant parameters in the serial arm, the values ​​of the redundant parameters are solved using continuous quantum particle swarm optimization based on the subproblems of the serial arm. The joint values ​​obtained from the inverse kinematics model are used as target values ​​to drive the movement of each joint of the robotic arm; For the rotating subchain, an inverse kinematics model is constructed based on motion constraints and different parallel relationships, where each joint direction is in the case that there are at least two non-parallel relationships: For a rotating subchain with at least two non-parallel relationships, i.e. and , These represent the first and second rotations of the subchain. , , The joint direction of each joint. Indicates the first rotation subchain The current posture of each joint = , ; The target orientation of the rotating subchain, , and Representing the first and second rotation subchains respectively , , Joint values ​​of each joint. It is a 3rd order identity matrix; From base to joint The attitude matrix formed by redundant joints between them. For joints To the joint The attitude matrix formed by redundant joints between them. For joints To the joint The attitude matrix formed by redundant joints between them. For joints The attitude matrix formed by redundant joints between the actuator end effector and the actuator end effector. ； in, , , ;when At that time, determine ； in, , , , .

2. The control method based on the inverse kinematics model of a parallel-rotary joint compound manipulator according to claim 1, wherein, The specific parallel relationships are as follows: For rotation subchains and translation subchains, there are two kinds of relationships between the joint directions: parallel and non-parallel. Based on the joint directions of the rotation subchain and translation subchain at the zero position, they are divided into three categories: at least two non-parallel relationships, only one non-parallel relationship, and all joint directions are parallel. The existence of at least two non-parallel relationships means that at least two of the joint directions are not parallel. The aforementioned non-parallel relationship refers to a situation where, among multiple joint directions, only two different joint directions exist. The statement that all joint directions are parallel means that in at least one joint direction, all joint directions are parallel. The joint direction refers to the direction of movement for translational joints and the direction of the axis of rotation for rotational joints.

3. The control method based on the inverse kinematics model of a parallel-rotary joint compound manipulator according to claim 1, wherein, For rotating subchains, an inverse kinematics model is constructed based on motion constraints and different parallel relationships, where each joint direction is assumed to have only one non-parallel relationship: When the rotating chain appears or during operation, the rotating chain becomes singular and degenerates into a form in which only one non-parallel relationship exists, i.e. ​ These represent the first and second rotations of the subchain. , , The joint direction of each joint. ; The target orientation of the rotating subchain, , and Representing the first and second rotation subchains, respectively. , , Joint values ​​for each joint; It is a 3rd order identity matrix; From base to joint The attitude matrix formed by redundant joints between them. For joints To the joint The attitude matrix formed by redundant joints between them. For joints To the joint The attitude matrix formed by redundant joints between them. For joints The attitude matrix formed by redundant joints between the actuator end effector and the actuator end effector. ； in, , .

4. The control method based on the inverse kinematics model of a parallel-rotary joint compound manipulator according to claim 1, wherein, For rotating subchains, an inverse kinematics model is constructed based on motion constraints and different parallel relationships, where the joint directions are all parallel: When the rotating chain is in operation The rotating chain degenerates into a form in which all joints are in parallel relationship. These represent the first and second rotations of the subchain. , , The joint direction of each joint. Indicates the first rotation subchain The current posture of each joint = , ; The target orientation of the rotating subchain, , and Representing the first and second rotation subchains respectively , , Joint values ​​for each joint; It is a 3rd order identity matrix; From base to joint The attitude matrix formed by redundant joints between them. For joints To the joint The attitude matrix formed by redundant joints between them. For joints To the joint The attitude matrix formed by redundant joints between them. For joints The attitude matrix formed by redundant joints between the actuator end effector and the actuator end effector. ； wherein is the sine of the matrix tr denotes the trace of the matrix , .

5. The control method based on the inverse kinematics model of a parallel-rotary joint compound manipulator according to claim 1, wherein, For the translational subchain, an inverse kinematics model is constructed based on motion constraints and different parallel relationships, where each joint direction is in the case that at least two non-parallel relationships exist: For a translation subchain that has at least two non-coplanar and non-parallel relationships, , , These represent the first and second subchains of the translation subchain, respectively. , , The joint direction of each joint. , , and They represent the translation subchains respectively. , , Joint values ​​of each joint. The offset generated by the rotating subchain to satisfy the attitude constraints. ; , Let and represent any 3rd-order unit vector and 0, respectively, satisfying . ; The target position of the tandem arm. From base to joint The position matrix formed by redundant joints between them For joints To the joint The position matrix formed by redundant joints between them For joints To the joint The position matrix formed by redundant joints between them For joints The position matrix formed by redundant joints between the actuator end; Recall , , , ； because , and Linearly independent The unique solution of the above formula is obtained. .

6. The control method for a robotic arm based on a composite translational-rotational joint inverse kinematics model according to claim 1, characterized in that, For the translational subchain, an inverse kinematics model is constructed based on motion constraints and different parallel relationships, where each joint direction is assumed to have only one non-parallel relationship: During the translational subchain motion, , , When linearly dependent, the translated subchains become singular and degenerate into a system with only one non-parallel relationship. The form; , , These represent the first and second subchains of the translation subchain, respectively. , , The joint direction of each joint. , , and They represent the translation subchains respectively. , , Joint values ​​of each joint. The offset generated by the rotating subchain to satisfy the attitude constraints. ; , Let and represent any 3rd-order unit vector and 0, respectively, satisfying . ; The target position of the tandem arm. From base to joint The position matrix formed by redundant joints between them For joints To the joint The position matrix formed by redundant joints between them For joints To the joint The position matrix formed by redundant joints between them For joints The position matrix formed by redundant joints between the actuator end; Recall , , , ； because , ,when When, it has a unique solution ;in, , for Left Reverse .

7. The control method for a robotic arm based on a composite translational-rotational joint inverse kinematics model according to claim 1, characterized in that, For the translational subchain, an inverse kinematics model is constructed based on motion constraints and different parallel relationships, where each joint direction is assumed to be parallel to all joint directions: When the translation subchain is running At this time, the translation subchain becomes singular and degenerates into a form where all joints are parallel; , , These represent the first and second subchains of the translation subchain, respectively. , , The joint direction of each joint. , and They represent the translation subchains respectively. , , Joint values ​​of each joint. The offset generated by the rotating subchain to satisfy the attitude constraints. ; , Let and 0 represent any 3rd order unit vector and 0 respectively, satisfying ; The target position of the tandem arm. From base to joint The position matrix formed by redundant joints between them For joints To the joint The position matrix formed by redundant joints between them For joints To the joint The position matrix formed by redundant joints between them For joints The position matrix formed by redundant joints between the actuator end; Recall , ； when When the above formula has a unique solution, ;in The ranks are full. for Left Reverse .

8. The control method for a robotic arm based on a composite translational-rotational joint inverse kinematics model according to claim 1, characterized in that, After constructing the inverse kinematics model, if redundant parameters exist in the tandem arm, the values ​​of the redundant parameters are solved using continuous quantum particle swarm optimization based on the subproblems of the tandem arm, including the following steps: Step 3.1: Characterize the joint values ​​of redundant joints as particle positions, where the joint values ​​are joint angles or joint displacements; set the population particle count. and maximum number of iterations ; Step 3.2: According to and randomly initialize the particle positions; in, The maximum value of the particle position. The minimum value of the particle position. This is the best historical position for the population. represents the maximum range of variation of redundant parameters between step sizes, and j represents the number of optimization iterations; Step 3.3: Calculate the fitness function value for each particle to update the historical best position of the i-th particle. Update the population's historical best position ; Step 3.4: Update particle positions , in, , , , ; in, for initial value, for The final value, This represents the average optimal position of the population. , , The number is a random floating-point number, where i represents the i-th particle; Step 3.5: Update particle positions After completing one iteration, increment the iteration count by one, return to step 3.3, and repeat until the iteration count reaches zero. Reaching the maximum number of iterations At this point, the optimal redundant joint value is obtained.

9. A control system based on a composite translational-rotational joint inverse kinematics model of a robotic arm, the system being used to implement the control method based on a composite translational-rotational joint inverse kinematics model of a robotic arm as described in any one of claims 1-8, characterized in that, include: The sub-chain extraction module is used to select rotary joints to form rotary sub-chains and select translational joints to form translational sub-chains based on the type of each joint of the serial arm. The inverse kinematics model building module is used to construct inverse kinematics models for translational subchains and rotational subchains respectively, based on motion constraints and different parallel relationships. The robot control module is used to drive the movement of each joint of the robotic arm based on the joint values ​​obtained from the inverse kinematics model as target values.

Images