Redundant robot continuous control method based on null space projection

Abstract

The present application relates to the technical field of robot motion control, and particularly relates to a redundant robot continuous control method based on zero space projection, comprising the following steps: S1, constructing a shaping zero space projection operator: based on Mansard continuous inverse definition and task constraint conditions of the redundant robot, an augmented Jacobian operator under an activated task is established; S2, solving a continuous solution of a kinematics equation: inverse containing all activated tasks is constructed, and joint speed is preliminarily calculated by using a closed loop control law; S3, optimizing the solution of joint speed: in the case that conflicts occur between activated tasks, a damping coefficient is introduced, and a new closed loop control law is written; S4, realizing task priority control: a general solution of joint speed is formalized through the shaping zero space, so that the priority of the activated task is controllable. The present application is helpful to improve the working performance and reliability of the robot in a complex dynamic environment.

Description

Technical Field

[0001] This invention relates to the field of robot motion control technology, and in particular to a redundant robot continuous control method based on zero-space projection. Background Technology

[0002] In the field of robot motion control, how to control the robot's motion process to adapt to the dynamic environment is an important challenge. When performing tasks, robots not only need to achieve trajectory tracking, but also need to ensure that various constraints in the environment are met. Traditional joint space control methods directly control the joint trajectory by pre-planning the trajectory of the robot's end effector. However, it is very difficult to realize environmental constraints in joint space, and typical joint space control frameworks still have many limitations.

[0003] Existing technologies for solving robot motion control problems typically employ pseudo-inverse methods to solve kinematic equations in order to achieve trajectory tracking and meet environmental constraints. However, these methods have significant drawbacks when facing multi-task control and dynamic environments. First, pseudo-inverse methods are prone to discontinuities when task activation changes, affecting the smoothness and stability of control. Second, existing methods struggle to effectively handle conflicts between activated tasks, leading to reduced control accuracy and response speed. Furthermore, existing methods suffer from insufficient numerical stability under singular conditions, limiting the robot's adaptability.

[0004] To address the aforementioned issues, this invention proposes a continuous inverse method based on continuous shaping zero-space projection and task control, aiming to improve the control performance of redundant robots in dynamic environments. This invention provides an efficient, stable, and highly adaptable robot control method, significantly enhancing the robot's performance and reliability in complex dynamic environments. Summary of the Invention

[0005] To achieve the above objectives, this invention provides a method for continuous control of redundant robots based on zero-space projection.

[0006] A continuous control method for redundant robots based on null-spatial projection includes the following steps:

[0007] S1, Constructing the Integer Null Space Projection Operator: Based on Mansard's continuous inverse definition and the task constraints of redundant robots, establish the augmented Jacobi operator under the activation task;

[0008] S2, Find the continuous solution of the kinematic equations: Construct the inverse of all activated tasks and use the closed-loop control law to initially calculate the joint velocities;

[0009] S3, solving of the optimized joint velocity: in the case of mutual conflict between the activated tasks, a damping coefficient is introduced, and a new closed-loop control law is written;

[0010] S4, implementation of the task priority control: the general solution of the joint velocity is formalized through the shaping null space formula, so that the priority of the activated task is controllable.

[0011] Further, the construction of the shaping null space projection operator in S1 comprises:

[0012] S11, definition of the task constraint condition: the task constraint condition of the redundant robot is defined, and the constraint condition comprises an equality constraint and an inequality constraint;

[0013] The calculation formula of the equality constraint is:

[0014] ;

[0015] Wherein, is the vector of the joint position, is the vector representing the equality constraint, → is the equality task function;

[0016] The calculation formula of the inequality constraint is:

[0017] ;

[0018] Wherein, → is the constraint function, is the dimension of all inequality tasks, is the threshold value of the inequality task;

[0019] S12, establishment of the augmented Jacobian operator: based on the task constraint condition, the augmented Jacobian operator under the activated task is established, which is represented as:

[0020] ;

[0021] ;

[0022] Wherein, is the augmented Jacobian matrix of all tasks, is the augmented Jacobian matrix of the first to the task.

[0023] Further, the calculation formula of the augmented Jacobian operator under the activated task is:

[0024] ;

[0025] wherein, is the Jacobian matrix of the th task, , is the augmented Jacobian operator of the first active tasks,

[0026] is defined as: .

[0027] Further, the reshaped null space projection operator of the augmented Jacobian operator is obtained by recursive operations, since is a symmetric matrix, is reformulated as:

[0028] .

[0029] wherein, and are the th input singular vector and singular value, respectively.

[0030] Further, the solving of the kinematics equation in S2 comprises:

[0031] S21, constructing the inverse of all active tasks: constructing the inverse of all active tasks based on the augmented Jacobian operator, the calculation formula is:

[0032] .

[0033] wherein, is the number of tasks, is a diagonal activation matrix, whose elements are , , is the reshaped null space projection operator of all tasks;

[0034] S22, using closed-loop control rule: using closed-loop control rule to calculate joint velocity, the calculation formula is:

[0035] .

[0036] wherein, is the tracking error, is the position of the required trajectory, is the error feedback gain.

[0037] Further, the solving of the optimized joint velocity in S3 comprises:

[0038] S31, introducing damping coefficient: in the case of mutual conflict between activated tasks, a damping coefficient is introduced;

[0039] S32, writing new closed-loop control rule: a new closed-loop control rule is written to optimize the joint speed.

[0040] Further, the new closed-loop control rule is expressed as:

[0041] .

[0042] Further, the new closed-loop control rule is expressed as: The calculation formula after introducing the damping factor is:

[0043] ;

[0044] Wherein, the positive scalar is the damping factor of ( );

[0045] ;

[0046] ;

[0047] Wherein, is the minimum singular value of , is the length of the singular avoidance domain, is the maximum damping parameter.

[0048] Further, the calculation formula of the general solution of the joint speed in S4 is:

[0049] ;

[0050] Wherein, is the free vector.

[0051] Advantages of the present application:

[0052] The present application can achieve trajectory tracking tasks while ensuring that the robot can meet environmental constraints by introducing a reshaping null space projection operator, especially suitable for multi-task control scenarios, ensuring priority control of each task, so that the performance of fully active tasks is not affected by other tasks. In addition, by constructing the inverse matrix containing all activated tasks and using the closed-loop control rule, the optimized joint speed can be calculated, so that the robot has higher adaptability and stability in complex environments.

[0053] This invention addresses the conflict between activation tasks by introducing a damping coefficient, thereby enhancing the numerical stability of the control algorithm under singular conditions. It uses recursive computation to obtain the integer null space projection of the augmented Jacobi operator and optimizes the solution of joint velocities. This approach improves control accuracy and response speed while maintaining task priority. Overall, it provides an efficient, stable, and adaptable robot control method that helps improve the robot's performance and reliability in complex dynamic environments. Attached Figure Description

[0054] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only for this invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0055] Fig. 1 This is a schematic diagram of the control method flow according to an embodiment of the present invention;

[0056] Fig. 2 This is a schematic diagram illustrating the construction of the shaping null space projection operator according to an embodiment of the present invention. Detailed Implementation

[0057] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to specific embodiments.

[0058] It should be noted that, unless otherwise defined, the technical or scientific terms used in this invention should have the ordinary meaning understood by one of ordinary skill in the art to which this invention pertains. The terms "first," "second," and similar terms used in this invention do not indicate any order, quantity, or importance, but are merely used to distinguish different components. Terms such as "comprising" or "including" mean that the element or object preceding the word encompasses the elements or objects listed following the word and their equivalents, without excluding other elements or objects. Terms such as "connected" or "linked" are not limited to physical or mechanical connections, but can include electrical connections, whether direct or indirect. Terms such as "upper," "lower," "left," and "right" are used only to indicate relative positional relationships; when the absolute position of the described object changes, the relative positional relationship may also change accordingly.

[0059] like Figs. 1-2 As shown, the continuous control method for redundant robots based on null-space projection includes the following steps:

[0060] S1, Inverse Kinematics Control:

[0061] 1. Task-based control:

[0062] During motion, a redundant robot can be subject to both equality constraints and inequality constraints. Let be the vector of joint positions, be the vector representing equality constraints. Equality constraints can be regarded as tracking tasks in the following formula:

[0063] (1)

[0064] where → is the equality task function, i.e., if is the position vector of the end-effector, then equation (1) is the forward kinematics equation of the robot. Furthermore, is the dimension of all equality constraints. The equality task at the velocity level can be obtained by differentiating (2) as follows:

[0065] (2)

[0066] While inequality constraints, such as obstacle avoidance and joint limit constraints, can usually be formulated as:

[0067] (3)

[0068] where → is the constraint function, is the dimension of all inequality tasks, is the threshold of the inequality task. A potential field can be introduced to force g(q) to stay in the feasible region, thus the task of inequality constraints can be formulated at the velocity level as:

[0069] (4)

[0070] where is the velocity vector that forces away from its threshold.

[0071] A redundant robot can need to handle multiple tasks during motion, such as obstacle avoidance and visual servoing. Usually, there are tasks, including equality and inequality tasks, which can be combined as:

[0072] (5)

[0073] where denotes the general task, including equality and inequality tasks. denotes the task, is the The dimension of each target. Defined as the augmented Jacobian matrix for all tasks. Furthermore, the first to the... The augmented Jacobian matrix of each task is represented as follows: , and They can be represented in the following forms:

[0074] (6)

[0075] if If the robot is redundant, then this paper only studies serially redundant robots. Typically, based on the pseudo-inverse, the minimum norm solution of equation (5) can be formulated as follows:

[0076] (7)

[0077] in yes The false reversal. Therefore, It is the minimum norm solution of equation (5). For redundant robots, there exists The non-zero homogeneous solution. Combining the non-zero homogeneous solution and the minimum norm solution, the general solution of equation (5) can be written as:

[0078] (8)

[0079] in It is a free vector. yes The null space projection is given by the formula:

[0080] (9)

[0081] To reveal the internal relationships between the rows of the Jacobian matrix, the Singular Value Decomposition (SVD) method is used to reformulate the Jacobian matrix as follows:

[0082] (10)

[0083] in It outputs singular vectors. orthogonal matrix, It is the input singular vector A normal matrix, and has ,in Assuming The rank is Then it exists:

[0084] ;

[0085] ;

[0086] ;

[0087] where is the null space of the matrix (or the kernel of the matrix ), is the range of the matrix . According to the SVD method, can be re-expressed as follows:

[0088] (11)

[0089] which can project a vector onto the null space of the Jacobian matrix. According to equation (11), the singular values of are equal to 0 or 1, and is endowed with the idempotent property .

[0090] 2. Discontinuity under the activation task mechanism:

[0091] During the entire motion process, some constraints of the robot are not always valid. For example, the obstacle constraint is only valid when the link of the robot is close to the obstacle. When the link moves out of the obstacle avoidance area, the obstacle constraint task should be taken out of the set of valid tasks, and therefore, the corresponding task should be removed from . To achieve the activation task mechanism, a binary activation matrix is defined, which represents the activation of each task as:

[0092] (12)

[0093] where the activation factor of the th task is equal to 1 or 0. If , the corresponding task is active, and if , the corresponding task is inactive. Since there are tasks in total, for simplicity, is denoted as . Inactive tasks can be removed by multiplying (5) by , and the kinematics equation under is:

[0094] (13)

[0095] In addition, the pseudo-inverse can be used to solve (13) in the following form:

[0096] (14)

[0097] Equation (14) can successfully remove the effect of inactive tasks, but it introduces discontinuity in joint velocities. In the rank of is constant, the pseudo-inverse based solution (7) is continuous. However, due to the change in the rank of as the tasks are activated, equation (14) is discontinuous.

[0098] If the activation factors vary in a continuous manner, the corresponding activation matrix can be defined as:

[0099] (15)

[0100] where is the activation factor of the th task. If , the th task is active; if , the th task is fully active; and if , the task is inactive. For simplicity, the activation matrix of all tasks is denoted as . Thus, equation (5) activated by can be rewritten as:

[0101] (16)

[0102] Assume is full rank, and due to the redundancy of the robot, the rank of (or ) is not greater than n. and are the least-norm solutions of (16) and (13), respectively. Since equation (16) is equivalent to (13), according to the uniqueness property of the least-norm solution, for a redundant robot, and are equal. Thus, even if the activation factors exhibit continuous behavior, discontinuity still exists when a task is switched from active to inactive, or vice versa.

[0103] Since the pseudo-inverse based solution cannot guarantee continuity after a task evolves from inactive to fully active, which is a requirement for smooth motion of the robot, a continuous inverse has been proposed as follows:

[0104] Definition 1 (Mansard continuous inverse definition).

[0105] Let and be diagonal activation matrices whose diagonal elements belong to.Depend on Activation matrix continuous inverse It has the following two properties:

[0106] 1) If for Then we have:

[0107] ;

[0108] 2) Function It is continuous.

[0109] According to Mansard's definition of continuous inverses, the continuity of inverses is given high priority, and the equivalence with pseudo-inverses holds only when binary is activated.

[0110] S2, the shaping null space projection operator:

[0111] Multiply (9) by get Inspired by this equation, we can establish a continuous operator. Its working principle is the same as that defined in equation (9). Similarly, and can ensure The continuity of . Therefore, the projection operator is defined as:

[0112] (17)

[0113] in It is the first Jacobian matrix of each task .also, Defined as:

[0114] (18)

[0115] yes Before activation The augmented Jacobi operator for each task has an integer null space projection operator, and It can be done Obtained through recursive operations. Because... It is a symmetric matrix, and equation (19) is obtained from the sine decomposition. It can be rephrased as:

[0116] (19)

[0117] in and They are the first an input singular vector and singular value. Compared with the null space projection defined in equation (11), The singular values of are not limited to 0 and 1, which means that the components in certain directions will be stretched or shortened, thus no longer having the idempotency. It is named as the shaped null space projection, which is not necessarily idempotent.

[0118] It is noted that if , and are continuous, then will also have continuity. However, is very sensitive to the singularity of , when the rows of fall into the null space of , even if is full rank, may become rank deficient. Before studying the singularity of , the orthogonality between and the Jacobian matrix of each task is first discussed.

[0119] S3, Redundant Robot Control Based on Continuous Inverse:

[0120] 1. Construction of a new inverse:

[0121] With the shaped null space projection operator, a new inverse can be established according to the continuous inverse given in definition 1, as follows:

[0122] Definition 2 (Continuous inverse of ):

[0123] Suppose there are tasks, is the diagonal activation matrix defined in equation (15), whose elements are , , The continuous inverse of the activated is defined as:

[0124] (20)

[0125] where is the shaped null space projection operator containing all tasks defined in (17).

[0126] 2. Continuous control law:

[0127] Through the definition of the new inverse, a continuous solution to the kinematics equation (16) can be proposed:

[0128] (21)

[0129] Solution based on pseudo-inverse In comparison, activation matrix It does not appear in equation (21), and the function of the task activation mechanism is encapsulated in the integer null space operator. Therefore, to enhance the universality of the algorithm, the closed-loop control law is described in the following form:

[0130] (twenty two)

[0131] in It is tracking error. This is the location of the desired trajectory. It is the error feedback gain.

[0132] 3. Use of the shaping null space projection operator:

[0133] exist The convergence of a fully active task holds true under full rank conditions. However, conflicts may arise between tasks, i.e. This becomes rank-deficient, which raises two problems: first, the Jacobian matrix of the activity task may exhibit a linear correlation with other activity tasks, therefore... and After the calculation, It becomes a rank deficiency; secondly This transforms into a singular matrix, resulting in unrestricted joint velocities in equation (22). A damping factor can be used to balance accuracy and improve feasibility; a damping factor with... It can be rephrased as:

[0134] (twenty three)

[0135] Among them, positive scalar yes ( The damping factor of ) is defined as follows:

[0136] (twenty four)

[0137] in, yes The minimum singular value, It is the length of the singularity avoidance domain. It is the maximum damping parameter.

[0138] Similarly, the closed-loop control law with a damping factor can be written as:

[0139] (25)

[0140] in, is obtained by substituting equation (23) into equation (17), is a damping factor, which is defined in the same way as equation (24). The closed-loop control law (25) is obtained based on and can be obtained by performing recursions in each control cycle.

[0141] S4, task priority control:

[0142] The shaping null-space projection operator has the superior property of preserving orthogonality, which makes it possible to control the task priority of the redundant robot with the activated task mechanism. Regarding the classical redundancy, the general solution of (26) can be formulated as:

[0143] (26)

[0144] It should be understood by those of ordinary skill in the art that the above discussion of any of the embodiments is merely exemplary and is not intended to suggest that the scope of the present application is in any way limited to these examples; the embodiments or technical features among different embodiments can also be combined, and the steps can be implemented in any order, and there are many other changes to the different aspects of the present application as described above, which are not provided in detail for the sake of brevity. The above discussion of any of the embodiments is merely exemplary and is not intended to suggest that the scope of the present application is in any way limited to these examples; the embodiments or technical features among different embodiments can also be combined, and the steps can be implemented in any order, and there are many other changes to the different aspects of the present application as described above, which are not provided in detail for the sake of brevity.

[0145] The present application is intended to cover all such alternatives, modifications and variations as fall within the broad scope of the claims. Accordingly, any and all such modifications, variations or equivalents that fall within the spirit and scope of the application are intended to be included within the scope of the application.

Claims

1. A redundancy robot continuous control method based on null space projection, characterized by, The method comprises the following steps: S1, constructing a shaping null space projection operator: based on the Mansard continuous inverse definition and the task constraint condition of the redundant robot, an augmented Jacobian operator under the activated task is established; S2, solving the continuous solution of the kinematics equation: the inverse containing all activated tasks is constructed, and the joint velocity is calculated preliminarily by using the closed loop control law; S3, optimizing the solution of the joint velocity: in the case of mutual conflict between the activated tasks, a damping coefficient is introduced, and a new closed loop control law is written; S4, realizing the task priority control: the general solution of the joint velocity is formalized by the shaping null space, so that the priority of the activated task is controllable; The S1 comprises: S11, defining the task constraint condition: the task constraint condition of the redundant robot is defined, and the constraint condition comprises an equality constraint and an inequality constraint; The calculation formula of the equality constraint is: ； wherein, is a vector of joint positions, is a vector representing equality constraints, → is an equality task function; The calculation formula of the inequality constraint is: ； wherein, → is a constraint function, is the dimension of all inequality tasks, is a threshold value for inequality tasks; S12, establishing the augmented Jacobian operator: based on the task constraint condition, the augmented Jacobian operator under the activated task is established, and is expressed as: ； ； wherein, is the augmented Jacobian matrix of all tasks, is the augmented Jacobian matrix of the first to the task, is the Jacobian matrix of the task; The calculation formula of the augmented Jacobian operator under the activated task is: ； wherein is the augmented Jacobian operator of the active shaped null space projection operator of the active is the activation factor of the th task; is defined as: ; wherein is an activation matrix for all tasks ​ The shaping null space projection operator of the augmented jacobian operator By obtained by is a symmetric matrix, is reformulated as: ； wherein, and are the first input singular vectors and singular values, respectively, is a normal matrix of the input singular vectors; The S2 comprises: S21, constructing the inverse containing all activated tasks: based on the augmented Jacobian operator, the inverse containing all activated tasks is constructed, and the calculation formula is: ； wherein is the number of tasks, is a diagonal activation matrix, is a reshaping null space projection operator containing all tasks, is the pseudo-inverse of S22, using the closed loop control law: the joint velocity is calculated by using the closed loop control law, and the calculation formula is: ； wherein, is a tracking error, is a position of a desired trajectory, is an error feedback gain; The S3 comprises: S31, introducing the damping coefficient: in the case of mutual conflict between the activated tasks, the damping coefficient is introduced; S32, writing the new closed loop control law: the new closed loop control law is written, and the joint velocity is optimized and solved; The The calculation formula after introducing the damping factor is: ； wherein the positive scalar is ( ) a damping factor; ； ； wherein is the minimum singular value of is the length of the singular-avoiding region, is the maximum damping parameter; The new closed loop control law is expressed as: ； wherein is a damping factor.

2. The null-space projection-based redundant robot continuous control method according to claim 1, characterized in that, The calculation formula of the general solution of the joint velocity in the S4 is: ； wherein is a free vector.

Images