A multi-task hierarchical compensation control method for a redundant robot arm

Abstract

The application provides a kind of redundancy manipulator's multitask hierarchical compensation control method, belongs to robot control technical field, including the following steps: S1: obtaining joint variable, task variable, desired trajectory speed and priority sequence;S2: according to jacobian matrix, desired speed and tracking error generates joint speed;S3: recursive construction decoupling projection matrix, the task joint speed of each task is handled by hierarchical decoupling projection;S4: after each task joint speed of hierarchical decoupling projection processing is synthesized, obtains original joint speed;S5: when task priority switches, construct compensation, generate continuous reference speed;S6: output continuous reference speed as reference motion instruction.The application can reduce the mutual interference between multitask, and improve the discontinuity of reference joint speed when task priority switches.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, specifically a multi-task hierarchical compensation control method for redundant robotic arms, which can be used for motion control of redundant robotic arms under conditions of parallel execution of multiple tasks, hierarchical control of task priorities, and dynamic switching of task priorities. Background Technology

[0002] A redundant robotic arm is a robotic arm whose number of degrees of freedom exceeds the number of degrees of freedom required to complete a given main task. Due to its redundant degrees of freedom, a redundant robotic arm can perform tasks such as attitude maintenance, elbow position adjustment, and joint velocity constraints while performing end-effector trajectory tracking, thus having high application value in complex operation scenarios.

[0003] In existing multi-task control methods, one type typically generates joint velocities directly based on the tracking errors of each task and then directly superimposes the control signals corresponding to multiple tasks. This approach is simple to implement, but in dynamic tracking tasks, it easily overlooks the desired task velocity, leading to tracking lag. Furthermore, directly superimposing control signals from multiple tasks lacks strict priority constraints, potentially causing low-priority tasks to interfere with high-priority tasks.

[0004] To handle the priority relationships between tasks, existing hierarchical control methods typically employ task priority constraints and projection operations. However, when the total task dimension of multiple tasks approaches or exceeds the redundant degrees of freedom of the robotic arm, the corresponding projection matrix may degenerate into a zero matrix, and related pseudo-inverse operations are prone to numerical instability or computational failure. Furthermore, when task priorities need to be dynamically switched according to the environment or task state, the original joint velocity signals may undergo discontinuous changes due to abrupt changes in the projection structure, thereby causing sudden changes in the robotic arm's motion or unstable execution process.

[0005] Therefore, it is necessary to provide a redundant multi-task control method for robotic arms that can balance the expected task speed feedforward, task hierarchical decoupling, and the continuity of task priority switching. Summary of the Invention

[0006] In view of this, the present invention proposes a multi-task hierarchical compensation control method for a redundant robotic arm, which aims to solve the problems in the prior art where multi-task control is prone to low-priority tasks interfering with high-priority tasks, projection matrix degradation leading to computational failure, and task priority switching causing discontinuity in reference joint speed.

[0007] To achieve the above objectives, the present invention provides the following technical solution:

[0008] A multi-task hierarchical compensation control method for a redundant robotic arm includes the following steps:

[0009] S1: Obtain the joint variables of the redundant robotic arm, the task space variables of multiple tasks, the expected task trajectory, the expected task speed, and the task priority sequence.

[0010] S2: Determine the tracking error of each task based on the task space variables and the expected task trajectory, and generate the task joint velocity of each task based on the Jacobian matrix, expected task velocity and tracking error corresponding to each task.

[0011] S3: Recursively construct the priority decoupling projection matrix corresponding to each task according to the task priority sequence, and use the priority decoupling projection matrix to perform hierarchical decoupling projection processing on the task joint velocity of each task, so that the task joint velocity of low priority task is projected into the decoupling constraint space of high priority task.

[0012] S4: Synthesize the joint velocities of each task after hierarchical decoupling projection processing to obtain the original joint velocities.

[0013] S5: When the task priority sequence is switched, a switching compensation amount is constructed based on the difference between the original joint speeds before and after the switch, and a continuous reference joint speed is generated using the switching compensation amount.

[0014] S6: Output the continuous reference joint velocity as a reference motion command for the redundant robotic arm to perform multiple tasks.

[0015] Specifically, in step S1, the redundancy of the robotic arm is... A robotic arm with redundant degrees of freedom, whose joint variables are represented as follows: Multiple tasks are recorded as ,in, The total number of tasks; the task priority sequence switches according to the task execution stage. At any given task execution stage, the current task priority sequence is represented as... ,in, for An arrangement of symbols This indicates that the previous task has a higher priority than the next task; when a task execution phase switches, the current task priority sequence is switched to another preset task priority sequence; for the th There are 1 task, and the task space variable is represented as... ,in, For a nonlinear mapping from joint space to task space; the first The Jacobian matrix corresponding to each task is represented as follows: ;No. The tracking error for each task is expressed as: ,in, For the first The expected task trajectory for each task.

[0016] In step S2, the first The task joint velocity for each task is determined jointly by the desired task velocity feedforward term and the tracking error feedback term, expressed as follows: ,in, Indicates the first Each task corresponds to the Moore-Penrose pseudoinverse of the Jacobian matrix. Indicates the first The expected task speed for each task Positive feedback gain This indicates the transpose operation.

[0017] In step S3, the priority decoupling projection matrix is ​​recursively constructed according to task priorities. For the highest priority task, it is set... For the first A non-highest priority task, When constructing the Jacobian matrix first, construct the priority Jacobian matrix. Then, based on the priority decoupling projection matrix corresponding to the previous priority task and the priority Jacobian matrix, the priority decoupling projection matrix corresponding to the current task is constructed. The specific calculation formula is as follows:

[0018]

[0019] in, for An identity matrix of order 1. It is a zero matrix. When When the matrix is ​​not zero, it can be based on... The pseudo-inverse is used to construct the current priority decoupling projection matrix; when When degenerating to a zero matrix, the transpose operation is used instead of the pseudo-inverse operation to avoid computational failure caused by performing pseudo-inverse calculation on the zero matrix.

[0020] In step S4, the original joint velocity is the sum of the task joint velocities of each task after processing with the corresponding priority decoupling projection matrix, expressed as follows: ,in, Indicates the number of tasks. Indicates the first The priority decoupling projection matrix corresponding to each task Indicates the first The speed of each task's joints.

[0021] In step S5, when the task priority sequence changes, when the task priority sequence changes at the switching time, The switching stimulus term, constructed from the velocity jump variable at the moment of task priority switching, is denoted as: ,in, Indicates the number of times task priority is switched. Indicates the first When the priority of a task changes Indicates that it is located at The Dirac function, Indicates the first The original joint velocity jump variables before and after the secondary task priority switch are determined, and an exponentially decaying switching compensation amount is constructed based on the velocity jump variables. The continuous reference joint speed is obtained by subtracting the switching compensation amount from the original joint speed; the continuous reference joint speed is expressed as... ,in, The attenuation parameter is positive.

[0022] The beneficial effects of this invention are as follows: by generating the task joint velocity together with the expected task velocity feedforward term and the tracking error feedback term, dynamic tracking lag can be reduced; by recursively constructing the priority decoupling projection matrix, interference of low-priority tasks on high-priority tasks can be reduced; by using substitution matrix operations in the case of projection matrix degradation, the risk of pseudo-inverse solution failure can be reduced; and by constructing a task priority switching compensation amount, the continuity of reference joint velocity during the task priority switching process can be improved. Attached Figure Description

[0023] To further illustrate the objectives and technical solutions of this invention, the following figures are provided for illustrative purposes:

[0024] Figure 1 A flowchart illustrating a multi-task hierarchical compensation control method for a redundant robotic arm provided in an embodiment of the present invention;

[0025] Figure 2 The joint velocity curve provided in the embodiment of the present invention is shown with time (unit: seconds) on the horizontal axis and joint velocity (unit: radians / second) on the vertical axis.

[0026] Figure 3 The terminal position error curve of Task 1 provided in this embodiment of the invention is shown on the horizontal axis as time (unit: seconds) and the vertical axis as the position error of Task 1 (unit: meters).

[0027] Figure 4 The attitude maintenance error curve for Task 2 provided in this embodiment of the invention is shown. The horizontal axis represents time (in seconds), and the vertical axis represents the attitude error of Task 2 (in radians).

[0028] Figure 5The elbow position error curve for Task 3 provided in this embodiment of the invention is shown on the horizontal axis as time (unit: seconds) and the vertical axis as the position error of Task 3 (unit: meters).

[0029] Figure 6 The figure shows the joint velocity task error curves for Task 4 and Task 5 provided in this embodiment of the invention. The horizontal axis represents time (unit: seconds), and the vertical axis represents the joint velocity task error of Task 4 and Task 5 (unit: radians / second). Detailed Implementation

[0030] To make the objectives and technical solutions of this invention clearer, the invention will be described in detail below with reference to the accompanying drawings and embodiments.

[0031] Example: In this example, the controlled object is the seven-degree-of-freedom redundant robotic arm Franka EmikaPanda. The initial joint variables are set to Radius. Control the total duration. seconds, sampling period is seconds, corresponding to a control frequency of Hertz. Multiple tasks including end effector position tracking. End effector attitude maintenance task Elbow position tracking task First joint velocity tracking task and the seventh joint velocity tracking task .in, The task dimension is , The task dimension is , The task dimension is , and Each task dimension is 1, and the total task dimension is 1. Greater than the degrees of freedom of the robotic arm .

[0032] In this embodiment, the task priority sequence switches according to the task execution stage: At seconds, the task priority sequence is set to ;exist At seconds, the task priority sequence switches to ;exist At the specified time, the task priority sequence further switches to... With the above settings, the hierarchical compensation control effect between the end effector position tracking task, the end effector attitude holding task, the elbow position tracking task, and the joint velocity tracking task can be verified at different stages.

[0033] In this embodiment, the feedback gain in the task joint velocity calculation can be taken as... The exponential decay parameter in task priority switching compensation is taken as follows: .

[0034] Step S1: Obtain the joint variables of the redundant robotic arm Task space variables for multiple tasks Expected task trajectory Expected task speed And the task priority sequence. The task space variables of a task can be represented as: ,in, This represents a nonlinear mapping from joint space to task space. The Jacobian matrix corresponding to each task is: The task space speed is satisfied. .

[0035] Step S2: Based on task space variables and expected task trajectory Determine tracking error To reduce tracking lag caused by relying solely on error feedback, this embodiment introduces a desired task velocity feedforward term into the task joint velocity. The task joint velocity of a task can be expressed as: .

[0036] Step S3: Recursively construct the priority decoupling projection matrix based on the task priority sequence. For the highest priority task, set... For the first One task, in At that time, the priority decoupling projection matrix is ​​determined according to the priority of the previous priority task. And priority Jacobian matrix structure .when When the matrix is ​​not zero, based on The pseudo-inverse is used to construct the current priority decoupling projection matrix; when When degenerating into a zero matrix, the transpose operation is used instead of the pseudo-inverse operation. This allows the execution of low-priority tasks to be suppressed when high-priority tasks occupy available degrees of freedom, reducing interference from low-priority tasks to high-priority tasks.

[0037] Step S4: Synthesize the joint velocities of each task after hierarchical decoupling projection processing to obtain the original joint velocities. The original joint velocities can be expressed as... .

[0038] Step S5: When the task priority sequence changes, the original joint velocities may abruptly change due to the change in the priority decoupling projection structure. Therefore, this embodiment constructs a velocity jump variable based on the difference between the original joint velocities before and after the switch, and constructs an exponentially decaying switching compensation amount based on the velocity jump variable. The continuous reference joint velocity can be expressed as... .

[0039] Step S6: Output the continuous reference joint speed as the reference motion command for the redundant robot arm to perform multiple tasks. This reference motion command can be provided to subsequent speed control, trajectory tracking control, or the underlying joint servo control unit, enabling the redundant robot arm to execute the corresponding task according to the set task priority.

[0040] This embodiment utilizes MATLAB software for simulation experiments to verify the correctness and superiority of the method of the present invention. Specific experimental results are as follows: Figure 2 , Figure 3 , Figure 4 , Figure 5 , Figure 6 As shown.

[0041] Furthermore, combined with Figure 2 It can be seen that the speed of each joint changes over time, and corresponding adjustments occur near the time of task priority switching. By compensating for the abrupt changes in the original joint speed through switching compensation, no divergence occurs in the speed of each joint, which reflects the improvement effect of this embodiment on the continuity of the reference joint speed under dynamic task priority switching conditions.

[0042] Combination Figure 3 It can be seen that the position error at the end of Task 1 is within and The task execution phase is kept to a very small extent; During the non-execution phase, the error curve remains smooth and bounded. This result demonstrates that by jointly generating the task joint velocity using the expected task velocity feedforward term and the tracking error feedback term, it is possible to track the end-position task and reduce the dynamic lag that may occur when relying solely on error feedback.

[0043] Combination Figure 4 It can be seen that the attitude maintenance error of Task 2 decreases rapidly in the initial stage. The execution phase maintains minimal error, which remains within a small range during subsequent task switching. This result demonstrates that after task-level decoupling projection processing, the attitude maintenance task can be executed according to the set priority, reducing interference from other tasks.

[0044] Combination Figure 5 It can be seen that the elbow position error in Task 3 occurred during the task execution phase. Keep it within a very small range; and During the non-execution phase, the error curve remains smooth and bounded. This result demonstrates that this embodiment can incorporate the elbow position task into a multi-task hierarchical framework and, through priority-decoupled projection processing, enable it to be executed collaboratively with the end-effector position task, attitude maintenance task, and joint velocity task.

[0045] Combination Figure 6 It can be seen that, During the execution phase, the joint velocity task error includes the first joint velocity error and the seventh joint velocity error. Both undergo transient changes near the task switching moment and remain bounded during subsequent control. This result indicates that the low-dimensional joint velocity task can be included as a component task in a multi-task set and executed in parallel with the spatial task through hierarchical decoupling projection processing.

[0046] Finally, it should be noted that the above preferred embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail through the above preferred embodiments, those skilled in the art should understand that various changes can be made to it in form and detail without departing from the scope defined by the claims of the present invention.

Claims

1. A multi-task hierarchical compensation control method for a redundant robotic arm, characterized in that, The method includes the following steps: S1: Obtain the joint variables of the redundant robotic arm, the task space variables of multiple tasks, the expected task trajectory, the expected task speed, and the task priority sequence; S2: Determine the tracking error of each task based on the task space variables and the expected task trajectory, and generate the task joint velocity of each task based on the Jacobian matrix, expected task velocity and tracking error corresponding to each task. S3: Recursively construct the priority decoupling projection matrix corresponding to each task according to the task priority sequence, and use the priority decoupling projection matrix to perform hierarchical decoupling projection processing on the task joint velocity of each task, so that the task joint velocity of low priority task is projected into the decoupling constraint space of high priority task. S4: Synthesize the joint velocities of each task after hierarchical decoupling projection processing to obtain the original joint velocities; S5: When the task priority sequence changes, a switching compensation amount is constructed based on the difference between the original joint speeds before and after the change, and a reference joint speed is generated using the switching compensation amount. S6: Output the reference joint velocity as a reference motion command for the redundant robotic arm to perform multiple tasks.

2. The multi-task hierarchical compensation control method for a redundant robotic arm according to claim 1, characterized in that, In step S1, the redundancy of the robotic arm is... A robotic arm with redundant degrees of freedom, whose joint variables are represented as follows: The multiple tasks are denoted as ,in, The total number of tasks; the task priority sequence switches according to the task execution stage, and in any task execution stage, the current task priority sequence is represented as follows: ,in, for An arrangement of symbols This indicates that the previous task has a higher priority than the next task; when a task execution phase switches, the current task priority sequence is switched to another preset task priority sequence; for the th There are 1 task, and the task space variable is represented as... ,in, This represents a nonlinear mapping from joint space to task space. Indicates the first The task space dimension of the task; the first The Jacobian matrix corresponding to each task is represented as follows: ,in, ;No. The tracking error for each task is expressed as: ,in, For the first The expected task trajectory for the first task; The task joint velocity for each task is determined jointly by the desired task velocity feedforward term and the tracking error feedback term, expressed as follows: ,in, Indicates the first Each task corresponds to the Moore-Penrose pseudoinverse of the Jacobian matrix. Indicates the first The expected task speed for each task Positive feedback gain This indicates the transpose operation.

3. The multi-task hierarchical compensation control method for a redundant robotic arm according to claim 1, characterized in that, In step S3, for the highest priority task, the corresponding priority decoupling projection matrix is: For the first A non-highest priority task, When constructing the Jacobian matrix first, construct the priority Jacobian matrix. Then, based on the priority of the previous priority task, decouple the projection matrix. And priority Jacobian matrix Construct the priority decoupling projection matrix corresponding to the current task. ; Based on the numerical state of the decoupled projection matrix corresponding to the priority of the previous priority task, a piecewise strategy is used to construct it: when At that time, pseudo-inverse operation is used to construct ;when In this case, the transpose operation is used instead of the pseudo-inverse operation to construct... .

4. The multi-task hierarchical compensation control method for a redundant robotic arm according to claim 1, characterized in that, In step S4, the original joint velocity is the sum of the task joint velocities of each task after processing with the corresponding priority decoupling projection matrix, expressed as follows: ,in, Indicates the number of tasks. Indicates the first The priority decoupling projection matrix corresponding to each task.

5. The multi-task hierarchical compensation control method for a redundant robotic arm according to claim 1, characterized in that, In step S5, when the task priority sequence changes at the switching time... The switching stimulus term, constructed from the velocity jump variable at the moment of task priority switching, is denoted as: ,in, Indicates the number of times task priority is switched. Indicates the first When the priority of a task changes express The Dirac function at time t, Indicates the first The original joint velocity jump variables before and after the secondary task priority switch are determined, and a switching compensation amount in the form of exponential decay is constructed based on the velocity jump variables. The continuous reference joint velocity is obtained by subtracting the switching compensation amount from the original joint velocity, expressed as: ,in The attenuation parameter is positive.

Images