Robot motion control method and device, robot control apparatus, and storage medium

Abstract

The application provides a robot motion control method and device, a robot control equipment and a storage medium, and relates to the technical field of robot control. The application calculates a task transmission matrix and expected target information of a plurality of sub-tasks of a to-be-executed task, a constraint transmission matrix and constraint boundary information of a plurality of constraint conditions, constructs a task priority matrix and a constraint priority matrix adapted to the priority distribution state of the plurality of sub-tasks based on the priority distribution state of each of the plurality of sub-tasks in the current control period, then performs priority level quadratic programming solving according to the task transmission matrix, the expected target information, the constraint transmission matrix, the constraint boundary information, the task priority matrix and the constraint priority matrix, and controls the target robot to move according to the programming solving result, so as to effectively ensure that the influence degree of each constraint condition and the task priority adjustment state of each task target are mutually adapted in the complex operation task execution process.

Description

Technical Field

[0001] This application relates to the field of robot control technology, and more specifically, to a robot motion control method and apparatus, a robot control device, and a storage medium. Background Technology

[0002] With the continuous development of science and technology, the application of robotics technology in various industries is becoming more and more widespread. Often, it is necessary to utilize the redundant joint space degrees of freedom of redundant robots to meet various physical constraints (e.g., maintaining balance, avoiding joint limits, avoiding reaching singular positions, etc.) while achieving various task objectives (e.g., maintaining good configuration, avoiding obstacle collisions, compliantly responding to external force drag, etc.) in order to effectively ensure that the corresponding redundant robot can achieve the desired whole-body movement effect.

[0003] Currently, in the process of redundant robots performing complex operation tasks, it is necessary to dynamically adjust the priority levels of multiple sub-tasks included in the complex operation task according to the actual situation, and control the redundant robot to adjust its own motion state in a timely manner according to the changing actual task priority level based on the HQP (Hierarchical Quadratic Programming) method of Recursive Hierarchical Projection (RHP), so that the redundant robot can directly apply more joint degrees of freedom to sub-tasks with higher priority levels.

[0004] However, it is worth noting that this robot control scheme for complex tasks focuses on the specific impact of the differences in task priorities among multiple sub-tasks under the same complex task on the robot's overall motion effect. It does not consider the dependence and relationship between the multiple constraints of the complex task and each sub-task. This causes the influence of certain constraints dependent on sub-tasks to be directly fixed during the process of task priority changes, resulting in the final robot control commands not conforming to the actual task situation, which seriously affects the robot's operational stability and task execution effect. Summary of the Invention

[0005] In view of this, the purpose of this application is to provide a robot motion control method and device, robot control equipment and storage medium, which can dynamically adjust the constraint priority of the constraints as the task priority of the task target is adjusted during the execution of complex operation tasks, so that the influence of each constraint of the complex operation task is adapted to the task priority adjustment of each task target, thereby ensuring the smooth operation of the robot and the task execution effect.

[0006] To achieve the above objectives, the technical solutions adopted in the embodiments of this application are as follows:

[0007] In a first aspect, this application provides a robot motion control method, the method comprising:

[0008] Obtain the robot end-effector reference trajectory information and robot end-effector state estimation information corresponding to the task to be executed in the current control cycle of the target robot;

[0009] Based on the robot end-effector reference trajectory information and the robot end-effector state estimation information, calculate the task transfer matrix and expected target information of each of the multiple subtasks of the task to be executed, as well as the constraint transfer matrix and constraint boundary information of each of the multiple constraints of the task to be executed.

[0010] Based on the priority distribution of each of the multiple subtasks in the current control cycle, construct the task priority matrix of the task to be executed, and the constraint priority matrix adapted to the priority distribution of the multiple subtasks for the multiple constraints.

[0011] Based on the task transfer matrix and expected target information of each of the multiple subtasks, the constraint transfer matrix and constraint boundary information of each of the multiple constraints, the task priority matrix and the constraint priority matrix, a priority-level quadratic programming solution is performed to obtain the corresponding programming solution results.

[0012] Based on the planning and solving results, matching joint control commands are generated for the target robot, and the target robot is controlled to move according to the joint control commands.

[0013] Secondly, this application provides a robot motion control device, the device comprising:

[0014] The motion information acquisition module is used to acquire the robot end-effector reference trajectory information and robot end-effector state estimation information of the target robot in the current control cycle corresponding to the task to be executed.

[0015] The task parameter calculation module is used to calculate the task transfer matrix and expected target information of each of the multiple subtasks of the task to be executed, as well as the constraint transfer matrix and constraint boundary information of each of the multiple constraints of the task to be executed, based on the robot end-effector reference trajectory information and the robot end-effector state estimation information.

[0016] The priority matrix construction module is used to construct the task priority matrix of the task to be executed, and the constraint priority matrix adapted to the priority distribution of the multiple subtasks in the current control cycle, based on the priority distribution of the multiple subtasks.

[0017] The hierarchical quadratic programming module is used to perform priority-level quadratic programming based on the task transfer matrix and expected target information of each of the multiple subtasks, the constraint transfer matrix and constraint boundary information of each of the multiple constraints, the task priority matrix and the constraint priority matrix, and to obtain the corresponding programming solution results.

[0018] The motion state control module is used to generate matching joint control commands for the target robot based on the planning solution results, and control the target robot to move according to the joint control commands.

[0019] Thirdly, this application provides a robot control device, including a processor and a memory, wherein the memory stores a computer program that can be executed by the processor, and the processor can execute the computer program to implement the robot motion control method described in any of the foregoing embodiments.

[0020] Fourthly, this application provides a storage medium storing a computer program thereon, which, when executed by a processor, implements the robot motion control method described in any of the foregoing embodiments.

[0021] In this case, the beneficial effects of the embodiments of this application may include the following:

[0022] Based on the current robot end-effector reference trajectory information and robot end-effector state estimation information corresponding to the task to be performed, this application calculates the task transfer matrix and expected target information of each of the multiple sub-tasks of the task to be performed, as well as the constraint transfer matrix and constraint boundary information of each of the multiple constraints. Based on the priority distribution of each of the multiple sub-tasks in the current control cycle, a corresponding task priority matrix and a constraint priority matrix adapted to the priority distribution of the multiple constraints are constructed. Then, based on all task transfer matrices, all expected target information, all constraint transfer matrices, all constraint boundary information, task priority matrices, and constraint priority matrices, a priority-level quadratic programming solution is performed. The target robot is controlled to move according to the corresponding programming solution results to achieve the task to be performed. Thus, during the execution of complex operation tasks, the constraint priority status of the constraints is dynamically adjusted along with the task priority adjustment status of the task target, so that the influence of each constraint of the complex operation task is adapted to the task priority adjustment status of each task target, ensuring the stability of robot operation and the task execution effect.

[0023] To make the above-mentioned objectives, features and advantages of this application more apparent and understandable, preferred embodiments are described below in detail with reference to the accompanying drawings. Attached Figure Description

[0024] To more clearly illustrate the technical solutions of the embodiments of this application, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of this application and should not be regarded as a limitation of the scope. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.

[0025] Figure 1 This is a schematic diagram of the composition of the robot control device provided in the embodiments of this application;

[0026] Figure 2 A flowchart illustrating the robot motion control method provided in an embodiment of this application;

[0027] Figure 3 for Figure 2 A flowchart illustrating the sub-steps included in step S240;

[0028] Figure 4 This is a schematic diagram of the composition of the robot motion control device provided in the embodiments of this application;

[0029] Figure 5 This is a schematic diagram illustrating the composition of the hierarchical quadratic programming module provided in an embodiment of this application.

[0030] Icons: 10-Robot control device; 11-Memory; 12-Processor; 13-Communication unit; 100-Robot motion control device; 110-Motion information acquisition module; 120-Task parameter calculation module; 130-Priority matrix construction module; 140-Hierarchical quadratic programming module; 150-Motion state control module; 141-Task-related matrix calculation submodule; 142-Constraint-related matrix calculation submodule; 143-Hierarchical projection matrix calculation submodule; 144-Hierarchical programming equation construction submodule; 145-Hierarchical programming equation solving submodule. Detailed Implementation

[0031] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of this application, and not all embodiments. The components of the embodiments of this application described and shown in the accompanying drawings can generally be arranged and designed in various different configurations.

[0032] Therefore, the following detailed description of the embodiments of this application provided in the accompanying drawings is not intended to limit the scope of the claimed application, but merely to illustrate selected embodiments of the application. All other embodiments obtained by those skilled in the art based on the embodiments of this application without inventive effort are within the scope of protection of this application.

[0033] It should be noted that similar labels and letters in the following figures indicate similar items. Therefore, once an item is defined in one figure, it does not need to be further defined and explained in subsequent figures.

[0034] In the description of this application, it should be understood that relational terms such as "first" and "second" are used merely to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element. Those skilled in the art will understand the specific meaning of the above terms in this application based on the specific circumstances.

[0035] In the description of this application, it should also be noted that, unless otherwise expressly specified and limited, the terms "set up," "install," "connect," and "link" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral connection; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; and they can refer to the internal connection of two components. Those skilled in the art can understand the specific meaning of the above terms in this application based on the specific circumstances.

[0036] The following detailed description of some embodiments of this application is provided in conjunction with the accompanying drawings. Unless otherwise specified, the following embodiments and features can be combined with each other.

[0037] Please refer to Figure 1 , Figure 1This is a schematic diagram of the robot control device 10 provided in this application embodiment. In this application embodiment, the robot control device 10 is communicatively connected to the target robot and is used to control the motion state of the target robot, enabling the target robot to perform complex operation tasks. The robot control device 10 can be a computer device independent of the target robot, or it can be a hardware module device integrated with the target robot. The computer device can be a personal computer, cloud server, laptop, tablet computer, etc.; the target robot can be a redundant robot with a serial / parallel configuration using position control, force control, or a hybrid force-position control, or it can be a redundant robotic arm, quadruped robot, or humanoid robot applied in industrial, service, and special industries.

[0038] In this embodiment, the robot control device 10 may include a memory 11, a processor 12, a communication unit 13, and a robot motion control device 100. The memory 11, processor 12, and communication unit 13 are electrically connected directly or indirectly to each other to achieve data transmission or interaction. For example, these components can be electrically connected via one or more communication buses or signal lines.

[0039] In this embodiment, the memory 11 may be, but is not limited to, random access memory (RAM), read-only memory (ROM), programmable read-only memory (PROM), erasable programmable read-only memory (EPROM), electrically erasable programmable read-only memory (EEPROM), etc. The memory 11 is used to store computer programs, and the processor 12 can execute the computer programs accordingly after receiving execution instructions.

[0040] In this embodiment, the processor 12 can be an integrated circuit chip with signal processing capabilities. The processor 12 can be a general-purpose processor, including at least one of a Central Processing Unit (CPU), Graphics Processing Unit (GPU), Network Processor (NP), Digital Signal Processor (DSP), Application-Specific Integrated Circuit (ASIC), Field-Programmable Gate Array (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, or discrete hardware components. The general-purpose processor can be a microprocessor or any conventional processor, capable of implementing or executing the methods, steps, and logic block diagrams disclosed in this embodiment.

[0041] In this embodiment, the communication unit 13 is used to establish a communication connection between the robot control device 10 and other electronic devices via a network, and to send and receive data via the network, wherein the network includes wired communication networks and wireless communication networks. For example, the robot control device 10 can obtain motion reference trajectory information of the target robot corresponding to the task to be performed from the motion planning device through the communication unit 13, wherein the motion reference trajectory information may include the reference position, reference velocity, and reference acceleration of the robot end effector of the target robot; the robot control device 10 can send joint control commands to the target robot through the communication unit 13 to drive the target robot to move according to the received joint control commands.

[0042] In this embodiment, the robot motion control device 100 may include at least one software function module that can be stored in the memory 11 in the form of software or firmware or embedded in the operating system of the robot control device 10. The processor 12 can be used to execute the executable modules stored in the memory 11, such as the software function modules and computer programs included in the robot motion control device 100. The robot control device 10 can dynamically adjust the constraint priority of the constraints during the process of controlling the target robot to perform complex operation tasks, so that the influence of each constraint of the complex operation task during the task execution process is adapted to the task priority adjustment status of each task target, thereby effectively improving the adaptability between robot control commands and actual task conditions, and ensuring the smooth operation of the target robot and the task execution effect.

[0043] Understandable Figure 1The block diagram shown is only a schematic diagram of one composition of the robot control device 10. The robot control device 10 may also include... Figure 1 The more or fewer components shown, or having the same Figure 1 The different configurations shown. Figure 1 The components shown can be implemented using hardware, software, or a combination thereof.

[0044] In this application, to ensure that the robot control device 10 can dynamically adjust the constraint priority of each constraint condition in accordance with the task priority adjustment status of each sub-task during the process of controlling the target robot to perform complex operation tasks, so that the influence of each constraint condition of the complex operation task during task execution is adapted to the task priority adjustment status of each sub-task, this application provides a robot motion control method to achieve the aforementioned objective. The robot motion control method provided in this application will be described in detail below.

[0045] Please refer to Figure 2 , Figure 2 This is a schematic flowchart of a robot motion control method provided in an embodiment of this application. In this embodiment, the robot motion control method is applied to the robot control device 10 described above, and the robot motion control method may include steps S210 to S250.

[0046] Step S210: Obtain the robot end-effector reference trajectory information and robot end-effector state estimation information corresponding to the task to be executed in the current control cycle.

[0047] In this embodiment, the motion process of the target robot can be divided into multiple control cycles, and the motion planning device can perform trajectory planning for the specific motion process of the target robot performing the task to be executed. Therefore, after determining the current control cycle of the target robot, the robot control device 10 obtains the robot end-effector reference trajectory information corresponding to the task to be executed in the current control cycle from the motion planning device.

[0048] The robot control device 10 can acquire real-time status feedback information of the target robot in the current control cycle (e.g., the actual joint position, actual joint velocity, and actual joint acceleration of each robot joint of the target robot, the actual body posture of the target robot, etc.) from the sensing devices of the target robot (e.g., motor angle encoder, inertial accelerometer, force sensor, etc.), and then process the acquired real-time status feedback information based on the robot's forward kinematics principle and robot forward dynamics model to estimate the robot end effector information of the target robot in the current control cycle (e.g., the estimated position and estimated velocity of the robot end effector in the task space, etc.).

[0049] Step S220: Based on the robot end-effector reference trajectory information and robot end-effector state estimation information, calculate the task transfer matrix and expected target information of each of the multiple subtasks of the task to be executed, as well as the constraint transfer matrix and constraint boundary information of each of the multiple constraints of the task to be executed.

[0050] In this embodiment, the task to be performed by the target robot can be divided into multiple equation subtasks, and the complete constraint rules affecting the execution effect of the task can be divided into multiple inequality constraints. All equation subtasks and all inequality constraints can be represented by mathematical expressions.

[0051] In this case, if the total number of subtasks into which the task to be executed is n t Then the j-th (j=1,……,n) t Each subtask can be represented by the following mathematical expression:

[0052] A j x = b j ;

[0053] in, The task transfer matrix used to represent the j-th subtask has a value related to the real-time status feedback information mentioned above. It can usually be represented by the Jacobian matrix related to the j-th subtask, which maps the actual speed of the robot joints of the target robot in the current control cycle to the actual speed of the robot end effector. The value of the target information used to represent the j-th sub-task is related to the robot end-effector reference trajectory information and the robot end-effector state estimation information mentioned above. m is used to represent the joint control variables that need to be solved. t,j The matrix dimension is used to represent the task transfer matrix of the j-th subtask, and n is used to represent the number of drivable joints of the target robot.

[0054] If the total number of constraints derived from the complete constraint rules corresponding to the task to be executed is n c Then the j-th (j=1,……,n) c The constraints can be represented by the following mathematical expression:

[0055] C j x≤d j ;

[0056] in, The constraint transfer matrix used to represent the j-th constraint condition has a value that is related to the real-time status feedback information mentioned above. The constraint boundary information used to represent the j-th constraint condition is related to the aforementioned robot end-effector reference trajectory information and the aforementioned robot end-effector state estimation information; m c,j The matrix dimension used to represent the constraint transfer matrix of the j-th constraint condition.

[0057] It is understandable that if the target robot is a force-controlled redundant robot, the corresponding joint control quantity x is usually represented by joint acceleration; if the target robot is a position-controlled redundant robot, the corresponding joint control quantity x is usually expressed by joint velocity.

[0058] Therefore, after obtaining the robot end reference trajectory information and robot end state estimation information corresponding to the current task to be executed by the target robot, the robot control device 10 can solve for the corresponding task transfer matrix and expected target information for each sub-task based on the mathematical expressions of the multiple sub-tasks and multiple constraints corresponding to the task to be executed, and solve for the corresponding constraint transfer matrix and constraint boundary information for each constraint.

[0059] Step S230: Based on the priority distribution of each of the multiple subtasks in the current control cycle, construct the task priority matrix of the task to be executed, and the constraint priority matrix adapted to the priority distribution of the multiple subtasks for multiple constraints.

[0060] In this embodiment, the priority distribution status of the multiple subtasks corresponding to the task to be executed often changes with the change of the control cycle during the specific execution process. Therefore, after the robot control device 10 determines the current control cycle of the target robot, it may look up the priority distribution status of each of the multiple subtasks corresponding to the task to be executed in the current control cycle in the subtask priority planning strategy based on the current control cycle, and then use the concept of the task priority matrix disclosed by the HQP method based on recursive hierarchical projection to characterize the priority distribution status of each of the multiple subtasks in the current control cycle, so as to obtain the task priority matrix of the task to be executed at the current control cycle. The subtask priority planning strategy is used to describe the set task priority status of multiple subtasks of the task to be executed at different control cycles, and the task priority matrix can effectively describe the task priority difference status and the task priority transformation relationship among the multiple subtasks of the task to be executed.

[0061] Wherein, the task priority matrix ψ t can be expressed by the following matrix formula:

[0062]

[0063] Wherein, n lt is used to represent the hierarchy number of the end priority hierarchy, and is also used to represent the total number of priority hierarchies; the degree-of-freedom occupancy rate α i,j corresponds to the j-th subtask in the i-th priority hierarchy, which is used to describe the occupancy degree of the remaining degrees of freedom of the drivable joint by the j-th subtask at the i-th priority hierarchy, and its value range is [0,1], wherein the larger the hierarchy number of the priority hierarchy, the lower the priority degree corresponding to that priority hierarchy.

[0064] For α i,j , its value rule is as follows: if the j-th subtask does not occupy the remaining degrees of freedom of the drivable joint of the i-th priority hierarchy, then α i,j =0; if the j-th subtask fully occupies the remaining degrees of freedom of the drivable joint of the i-th priority hierarchy, then α i,j =1; if the j-th subtask partially occupies the remaining degrees of freedom of the drivable joint of the i-th priority hierarchy, then 0<α i,j <1; if a certain subtask is executed at a priority hierarchy with a higher priority degree, the subtask also needs to be executed at a priority hierarchy with a lower priority degree, that is, for the j-th subtask, when k<i, there exists α k,j ≤α i,j , and at the same time, when the j-th subtask is stably executed at the i-th priority hierarchy, if k<i, then α k,j=0, and if k≥i, then α k,j =1; if the j-th subtask is raised from the i-th priority level with a low priority degree to the k-th priority level with a high priority degree (i.e., k<i), then α k,j ,...,α i-1,j will all gradually change from 0 to 1 in a plurality of consecutive control cycles after the current control cycle; if the j-th subtask is lowered from the i-th priority level with a high priority degree to the k-th priority level with a low priority degree (i.e., i<k), then α i,j ,...,α k-1,j will all gradually change from 1 to 0 in a plurality of consecutive control cycles after the current control cycle; if the j-th subtask is removed from the i-th priority level, then will all gradually change from 1 to 0 in a plurality of consecutive control cycles after the current control cycle; if the j-th subtask is inserted into the i-th priority level, then will all gradually change from 0 to 1 in a plurality of consecutive control cycles after the current control cycle.

[0065] After the robot control device 10 constructs a task priority matrix substantially matching the current control cycle for the plurality of subtasks of the task to be executed, the robot control device 10 will synchronously adjust the constraint priority status of the plurality of constraint conditions according to the dependency relationship between each of the plurality of constraint conditions of the task to be executed and each subtask, so that the influence degree of each constraint condition is mutually adapted to the task priority adjustment status of each subtask, and a constraint priority matrix similar in concept to the task priority matrix is used to effectively characterize the magnitude of the influence degree of each of the plurality of constraint conditions on the remaining degrees of freedom of the drivable joint at different priority levels.

[0066] Wherein, the constraint priority matrix ψ c can be expressed by the following matrix formula:

[0067]

[0068] Wherein, n lc is used to represent the level number of the end priority level, and is also used to represent the total number of priority levels, n lc =n lt ; the degree-of-freedom occupancy rate β i,j corresponds to the j-th constraint condition located in the i-th priority level, which is used to describe the magnitude of the influence degree of the j-th constraint condition on the remaining degrees of freedom of the drivable joint at the i-th priority level, and its value range is [0,1], wherein the larger the level number of the priority level, the lower the priority degree corresponding to that priority level.

[0069] For β i,jspeaking, β i,j has a value rule similar to that of α i,j , and the value rule of β i,j at this time is as follows: if the j-th constraint condition does not occupy the remaining degrees of freedom of the drivable joints at the i-th priority level to exert an influence, then β i,j =0; if the j-th constraint condition fully occupies the remaining degrees of freedom of the drivable joints at the i-th priority level to exert an influence, then β i,j =1; if the j-th constraint condition partially occupies the remaining degrees of freedom of the drivable joints at the i-th priority level to exert an influence, then 0<β i,j <1; if a certain constraint condition exerts an influence at a higher priority level, the constraint condition will also exert an influence at a lower priority level, that is, for the j-th constraint condition, when k<i, there exists β k,j ≤β i,j , and when the j-th constraint condition stably exerts an influence at the i-th priority level, if k<i then β k,j =0, and if k≥i then β k,j =1; if the j-th constraint condition is raised from the i-th priority level of lower priority degree to the k-th priority level of higher priority degree (i.e., k<i), then β k,j ,…,β i-1,j will all gradually change from 0 to 1 in multiple consecutive control cycles after the current control cycle; if the j-th constraint condition is lowered from the i-th priority level of higher priority degree to the k-th priority level of lower priority degree (i.e., i<k), then β i,j ,…,β k-1,j will all gradually change from 1 to 0 in multiple consecutive control cycles after the current control cycle; if the j-th constraint condition is removed from the i-th priority level, then will all gradually change from 1 to 0 in multiple consecutive control cycles after the current control cycle; if the j-th constraint condition is inserted into the i-th priority level, then will all gradually change from 0 to 1 in multiple consecutive control cycles after the current control cycle.

[0070] Step S240: performing priority level quadratic programming solving according to the respective task transfer matrices and expected target information of the multiple subtasks, the respective constraint transfer matrices and constraint boundary information of the multiple constraint conditions, the task priority matrix and the constraint priority matrix, to obtain a corresponding programming solving result.

[0071] In this embodiment, when the robot control device 10 obtains the task transfer matrix and expected target information of each of the multiple subtasks corresponding to the task to be executed in the current control cycle, the constraint transfer matrix and constraint boundary information of each of the multiple constraints in the current control cycle, and the task priority matrix and constraint priority matrix corresponding to the current control cycle, it can determine the correlation of optimization results for joint control quantities between two adjacent priority levels based on the principle that the joint control quantities of lower priority levels inherit the joint control quantities of higher priority levels. Then, using the correlation of optimization results, the aforementioned task transfer matrix, the aforementioned expected target information, the aforementioned constraint transfer matrix, the aforementioned constraint boundary information, the aforementioned task priority matrix, and the aforementioned constraint priority matrix, it constructs a hierarchical quadratic programming control equation based on the priority level projection approach for the task to be executed. Then, by solving the hierarchical quadratic programming control equation, it obtains the optimization results of the constraint priority adjustment status and task priority adjustment status of the end priority level. Finally, it uses the optimization results of the end priority level as the planning solution results of the constraint priority adjustment status and task priority adjustment status of the current control cycle.

[0072] Step S250: Generate matching joint control commands for the target robot based on the planning solution results, and control the target robot to move according to the joint control commands.

[0073] In this embodiment, after the robot control device 10 obtains a planning solution that meets the constraint priority adjustment status and task priority adjustment status, the robot control device 10 can perform robot forward kinematics transformation on the planning solution based on the robot type of the target robot to obtain joint control commands that conform to the robot type, and adjust the joint state of each driveable joint of the target robot according to the joint control commands, so that the target robot can execute the specific task content corresponding to the task to be executed in the current control cycle by moving according to the joint control commands.

[0074] If the target robot is a force-controlled redundant robot, the corresponding planning solution is usually characterized by joint acceleration. The planning solution can be processed by the robot dynamics model to obtain joint control commands for joint torque. If the target robot is a position-controlled redundant robot, the corresponding planning solution is usually characterized by joint velocity. The planning solution can be integrated to obtain joint control commands for joint position.

[0075] Therefore, by executing the above steps S210 to S250, this application can dynamically adjust the constraint priority of the constraints along with the task priority adjustment status of the task target during the process of controlling the target robot to perform complex operation tasks. This makes the influence of each constraint of the complex operation task during the task execution process compatible with the task priority adjustment status of each task target, thereby effectively improving the adaptability between robot control commands and actual task conditions, and ensuring the stability of robot operation and the task execution effect of the target robot.

[0076] Alternatively, please refer to Figure 3 , Figure 3 yes Figure 2 The flowchart of step S240 includes the sub-steps. In the embodiments of this application, step S240 may include sub-steps S241 to S245, so as to effectively ensure the dynamic consistency of task execution between adjacent priority levels in the process of solving the optimal joint control quantity that meets the constraint priority adjustment state and task priority adjustment state, and avoid the task execution effect of the lower priority level from interfering with the task execution effect of the higher priority level.

[0077] Sub-step S241: Based on the degree-of-freedom occupancy rate of each of the multiple subtasks at different priority levels of the task priority matrix, and based on the task transfer matrix and expected target information of each of the multiple subtasks, calculate the augmented task matrix, augmented expected target, and task degree-of-freedom configuration diagonal matrix for each priority level.

[0078] In this embodiment, the robot control device 10 can, for each priority level involved in the task priority matrix, utilize the distribution of the degree-of-freedom occupancy rate of each subtask within that priority level, extract the task transfer matrix of the subtasks with non-zero occupancy rate values ​​to construct the augmented task matrix of that priority level, extract the expected target information of the subtasks with non-zero occupancy rate values ​​to construct the augmented expected target of that priority level, and simultaneously extract the specific degree-of-freedom occupancy rate of the subtasks with non-zero occupancy rate values ​​to construct the task degree-of-freedom configuration diagonal matrix of that priority level.

[0079] In this process, the specific execution flow of sub-step S241 may include:

[0080] For each priority level, select all first occupancy rates that correspond to that priority level and have a non-zero occupancy rate value from the task priority matrix;

[0081] Sort all the selected first occupancy rates in descending order to obtain the corresponding first sort result;

[0082] Sort the subtask sequence numbers corresponding to each first occupancy rate according to the first arrangement result to obtain the corresponding target subtask arrangement order;

[0083] The task transfer matrix corresponding to each subtask sequence number is augmented according to the order of the target subtasks to obtain the augmented task matrix for that priority level.

[0084] According to the order of the target sub-tasks, the expected target information corresponding to each sub-task sequence number is subjected to information augmentation processing to obtain the augmented expected target of this priority level.

[0085] According to the order of the target subtasks, the first occupancy rate corresponding to each subtask sequence number is processed into a diagonal matrix to obtain the task degree of freedom configuration diagonal matrix of this priority level.

[0086] Taking the i-th priority level as an example, the order of the target subtasks at the i-th priority level can be represented by the target subtask sequence number vector s. t,i =[s ti,1 …s ti,ni ] T Express, where s ti,1 ,…,s ti,ni The subtask sequence numbers s and s are used to represent the subtasks with non-zero occupancy values ​​at the i-th priority level in the task priority matrix. ti,1 ,…,s ti,ni The degrees of freedom occupancy rates corresponding to each task priority level are arranged in descending order; the augmented task matrix for the i-th priority level can be expressed as a matrix. To express, among which Used to represent the subtask sequence number s ti,k The task transfer matrix of the subtasks; the augmented expected target of the i-th priority level can be expressed in matrix form. To express, among which Used to represent the subtask sequence number s ti,k The expected target information of the subtasks; the diagonal matrix of the task degree of freedom configuration at the i-th priority level can be expressed by the following matrix form:

[0087]

[0088] in, Used to represent the subtask sequence number s ti,k The degree-of-freedom occupancy rate of the subtask at the i-th priority level of the task priority matrix. Used to represent a matrix with dimension m ti,k The identity matrix, m ti,k Used to represent the task transfer matrix The number of rows in the matrix.

[0089] Sub-step S242: Based on the degree-of-freedom occupancy rates of each of the multiple constraints at different priority levels of the constraint priority matrix, and based on the constraint transfer matrix and constraint boundary information of each of the multiple constraints, calculate the augmented constraint matrix, augmented constraint boundary, and constraint degree-of-freedom configuration diagonal matrix for each priority level.

[0090] In this embodiment, the robot control device 10 can, for each priority level involved in the task priority matrix, utilize the distribution of the degree-of-freedom occupancy rate corresponding to each constraint condition within that priority level, extract the constraint transfer matrix of the constraint condition with a non-zero occupancy rate to construct the augmented constraint matrix of that priority level, extract the constraint boundary information of the sub-task with a non-zero occupancy rate to construct the augmented constraint boundary of that priority level, and simultaneously extract the specific degree-of-freedom occupancy rate of the sub-task with a non-zero occupancy rate to construct the constraint degree-of-freedom configuration diagonal matrix of that priority level.

[0091] In this process, the specific execution flow of sub-step S242 may include:

[0092] For each priority level, select all second occupancy rates that correspond to that priority level and have a non-zero occupancy rate value from the constraint priority matrix;

[0093] Sort all the selected second occupancy rates in descending order to obtain the corresponding second sorting results;

[0094] Sort the constraint sequence numbers corresponding to each second occupancy rate according to the second arrangement result to obtain the corresponding target constraint arrangement order;

[0095] According to the order of the target constraints, the constraint transfer matrix corresponding to each constraint sequence number is subjected to matrix augmentation to obtain the augmented constraint matrix of that priority level.

[0096] According to the order of the target constraints, the constraint boundary information corresponding to each constraint sequence number is subjected to information augmentation processing to obtain the augmented constraint boundary of this priority level.

[0097] According to the order of the target constraints, the second occupancy rate corresponding to each constraint sequence number is processed into a diagonal matrix to obtain the constraint degree of freedom configuration diagonal matrix of this priority level.

[0098] Taking the i-th priority level as an example, the order of the target constraints at the i-th priority level can be represented by the target constraint sequence number vector s. c,i =[s ci,1 …sci,ni ] T Express, where s ci,1 ,…,s ci,ni The constraint sequence numbers s are used to represent the constraint conditions whose occupancy values ​​at the constraint priority matrix are non-zero for the i-th priority level. ci,1 ,…,s ci,ni The degrees of freedom occupancy rates corresponding to each constraint priority level are arranged in descending order; the augmented constraint matrix of the i-th priority level can be expressed in matrix form. To express, among which Used to represent constraint condition sequence number s ci,k The constraint transfer matrix of the constraint conditions; the augmented constraint boundary of the i-th priority level can be expressed in matrix form. To express, among which Used to represent constraint condition sequence number s ci,k The constraint boundary information of the constraint conditions; the diagonal matrix of the constraint degrees of freedom configuration of the i-th priority level can be expressed by the following matrix form:

[0099]

[0100] in, Used to represent constraint condition sequence number s ci,k The degree-of-freedom occupancy rate of the constraint conditions at the i-th priority level of the constraint priority matrix. Used to represent a matrix with dimension m ci,k The identity matrix, m ci,k Used to represent the constraint transfer matrix The number of rows in the matrix.

[0101] Sub-step S243: For each priority level except the end priority level, calculate the target level projection matrix with dynamic consistency for that priority level based on the augmented task matrix and task degree of freedom configuration diagonal matrix corresponding to that priority level.

[0102] In this embodiment, the robot control device 10 can ensure that the target level projection matrix of each priority level, except for the end priority level, has dynamic consistency by introducing a task weight matrix with dynamic consistency during the construction of the hierarchical projection matrix.

[0103] Specifically, for each priority level other than the terminal priority level, the specific execution process of sub-step S243 may include:

[0104] Based on the priority distribution of the multiple subtasks in the current control cycle, a matching task weight matrix with dynamic consistency is constructed for the priority level.

[0105] Perform Cholesky decomposition on the task weight matrix to obtain the target lower triangular matrix corresponding to the task weight matrix;

[0106] Perform matrix multiplication on the augmented task matrix of the prior level and the target lower triangular matrix to obtain the task matrix to be eliminated;

[0107] Perform linear correlation row removal processing on the task matrix to be eliminated to obtain a row full-rank task matrix corresponding to the augmented task matrix;

[0108] The transpose of the full-rank task matrix is ​​subjected to orthogonal triangular decomposition to obtain the row space orthogonal basis matrix of the full-rank task matrix.

[0109] In the task degree-of-freedom configuration diagonal matrix at this priority level, extract the diagonal element matrix whose matrix dimension matches the rank value of the full-rank task matrix;

[0110] Calculate the target level projection matrix based on the target lower triangular matrix, row space orthogonal basis matrix, and diagonal element matrix corresponding to the priority level.

[0111] The task weight matrix with dynamic consistency for a single priority level can be represented by the set of inverse matrices of the robot inertia matrices corresponding to each subtask involved in that priority level at the real-time state feedback information. The calculation formula for the target level projection matrix of a single priority level is expressed as follows:

[0112]

[0113] Among them, Z i L is used to represent the target hierarchy projection matrix of the i-th priority level. i The lower triangular matrix representing the target corresponding to the task weight matrix at the i-th priority level is denoted by I, which represents the identity matrix. i This is used to represent the row space orthogonal basis matrix corresponding to the full-rank task matrix of the i-th priority level. The diagonal element matrix representing the i-th priority level can be used as follows: To express this, the full-rank row task matrix of the i-th priority level can be expressed as follows: To express oneself.

[0114] Sub-step S244: Based on the augmented task matrix, augmented expected target, augmented constraint matrix, augmented constraint boundary, and constrained degrees of freedom configuration diagonal matrix and target level projection matrix of each priority level, and based on the correlation between optimization results of two adjacent priority levels, construct the corresponding hierarchical quadratic programming control equation.

[0115] In this embodiment, the correlation between the optimization results of two adjacent priority levels is represented by the following formula:

[0116] Where i = 2, ..., n lt ;

[0117] in, This is used to represent the optimization result of the i-th priority level at the hierarchical quadratic programming control equation. Z is used to represent the optimization result of the (i-1)th priority level at the hierarchical quadratic programming control equation. i-1 This is used to represent the target hierarchy projection matrix with dynamic consistency for the (i-1)th priority level. The optimal increment to be superimposed in the dynamically consistent hierarchical projection space of the (i-1)th priority level is used to represent the optimization result of the i-th priority level. n is used to represent the actual optimization increment of the optimization result at the i-th priority level relative to the optimization result at the (i-1)-th priority level. lt The hierarchy number used to represent the final priority level, where for the first priority level, the optimization result of the first priority level can be expressed as: Z0 is the identity matrix.

[0118] The hierarchical quadratic programming control equations can be divided into n lt There are several quadratic programming sub-equations, where the optimization results of higher-priority quadratic programming sub-equations affect the optimization results of lower-priority quadratic programming sub-equations. The subtasks involved in higher-priority quadratic programming sub-equations will still be executed at the lower-priority levels, and the constraints involved in higher-priority quadratic programming sub-equations will exert influence at the lower-priority levels. Therefore, the hierarchical quadratic programming governing equations are applied at the i-th (i = 1, ..., n) level. lt The quadratic programming optimization sub-equations at each priority level are expressed as follows:

[0119]

[0120]

[0121]

[0122] Among them, u iThis represents the increment to be optimized in the dynamically consistent hierarchical projection space of the i-th priority level, representing the optimization result of the i-th priority level. Z is used to represent the optimization result of the quadratic programming sub-equation at the governing equation of the quadratic programming level for the (i-1)th priority level. i-1 This is used to represent the target hierarchy projection matrix with dynamic consistency for the (i-1)th priority level. Used to represent the augmented task matrix of the i-th priority level. Used to represent the modified augmented expected target of the i-th priority level. Used to represent the augmented expected target of the i-th priority level. A matrix of diagonal elements in the diagonal matrix representing the task degree-of-freedom configuration for the i-th priority level, matching the rank of the corresponding full-rank task matrix. The diagonal matrix used to represent the constraint degrees of freedom configuration for the i-th priority level. Used to represent the augmented constraint matrix of the i-th priority level. Used to represent the modified augmented constraint boundary of the i-th priority level Used to represent the augmented constraint boundary of the i-th priority level. The diagonal matrix used to represent the constraint degrees of freedom configuration for the (i-1)th priority level. Used to represent the augmented constraint matrix of the (i-1)th priority level. Used to represent the modified augmented constraint boundary of the (i-1)th priority level. Used to represent the augmented constraint boundary of the (i-1)th priority level. The diagonal matrix used to represent the constraint degrees of freedom configuration for the first priority level. Used to represent the augmented constraint matrix of the first priority level. Used to represent the modified augmented constraint boundary of the first priority level. The first priority level of augmented constraint boundary is represented by I, which represents the identity matrix, and K, which represents the constraint relaxation ratio coefficient with a value greater than zero. The constraint relaxation ratio coefficient is used to characterize the specific proportion when the corresponding constraint boundary is relaxed proportionally.

[0123] Sub-step S245: Solve the constructed hierarchical quadratic programming control equations and use the optimization results of the final priority level as the programming solution results.

[0124] Therefore, by executing the above sub-steps S241 to S245, this application can effectively ensure the dynamic consistency of task execution between adjacent priority levels in the process of solving the optimal joint control quantity that meets the constraint priority adjustment status and task priority adjustment status, and avoid the task execution effect of the lower priority level from interfering with the task execution effect of the higher priority level.

[0125] In this application, to ensure that the robot control device 10 can effectively execute the aforementioned robot motion control method, the aforementioned function is achieved by dividing the robot motion control device 100 stored in the robot control device 10 into functional modules. The specific composition of the robot motion control device 100 applied to the aforementioned robot control device 10 provided in this application will be described below.

[0126] Please refer to Figure 4 , Figure 4 This is a schematic diagram of the robot motion control device 100 provided in an embodiment of this application. In this embodiment, the robot motion control device 100 may include a motion information acquisition module 110, a task parameter calculation module 120, a priority matrix construction module 130, a hierarchical quadratic programming module 140, and a motion state control module 150.

[0127] The motion information acquisition module 110 is used to acquire the robot end reference trajectory information and robot end state estimation information corresponding to the task to be executed in the current control cycle of the target robot.

[0128] The task parameter calculation module 120 is used to calculate the task transfer matrix and expected target information of each of the multiple subtasks of the task to be executed, as well as the constraint transfer matrix and constraint boundary information of each of the multiple constraints of the task to be executed, based on the robot end-effector reference trajectory information and the robot end-effector state estimation information.

[0129] The priority matrix construction module 130 is used to construct a task priority matrix for the task to be executed, and a constraint priority matrix adapted to the priority distribution of the multiple subtasks, based on the priority distribution of the multiple subtasks in the current control cycle.

[0130] The hierarchical quadratic programming module 140 is used to perform priority-level quadratic programming based on the task transfer matrix and expected target information of each of the multiple subtasks, the constraint transfer matrix and constraint boundary information of each of the multiple constraints, the task priority matrix and the constraint priority matrix, and to obtain the corresponding programming solution results.

[0131] The motion state control module 150 is used to generate matching joint control commands for the target robot based on the planning solution results, and control the target robot to move according to the joint control commands.

[0132] Alternatively, please refer to Figure 5 , Figure 5 This is a schematic diagram of the hierarchical quadratic programming module 140 provided in an embodiment of this application. In this embodiment, the hierarchical quadratic programming module 140 may include a task-related matrix calculation submodule 141, a constraint-related matrix calculation submodule 142, a hierarchical projection matrix calculation submodule 143, a hierarchical programming equation construction submodule 144, and a hierarchical programming equation solving submodule 145.

[0133] The task-related matrix calculation submodule 141 is used to calculate the augmented task matrix, augmented expected target, and task freedom configuration diagonal matrix for each priority level based on the degree of freedom occupancy rate of each of the multiple subtasks at different priority levels of the task priority matrix, and based on the task transfer matrix and expected target information of each of the multiple subtasks.

[0134] The constraint correlation matrix calculation submodule 142 is used to calculate the augmented constraint matrix, augmented constraint boundary and constraint degree of freedom configuration diagonal matrix for each priority level based on the degree of freedom occupancy rate corresponding to each of the multiple constraints at different priority levels of the constraint priority matrix, and based on the constraint transfer matrix and constraint boundary information of each of the multiple constraints.

[0135] The hierarchical projection matrix calculation submodule 143 is used to calculate the target hierarchical projection matrix with dynamic consistency for each priority level except the end priority level, based on the augmented task matrix and task degree of freedom configuration diagonal matrix corresponding to the priority level.

[0136] The hierarchical programming equation construction submodule 144 is used to construct the corresponding hierarchical quadratic programming control equations based on the augmented task matrix, augmented expected target, augmented constraint matrix, augmented constraint boundary, and constraint degrees of freedom configuration diagonal matrix and target level projection matrix of each priority level, and based on the correlation between the optimization results of two adjacent priority levels.

[0137] The hierarchical programming equation solving submodule 145 is used to solve the constructed hierarchical quadratic programming control equations and use the optimization results of the terminal priority level as the programming solution results.

[0138] It should be noted that the robot motion control device 100 provided in this embodiment has the same basic principle and technical effects as the aforementioned robot motion control method. For the sake of brevity, any parts not mentioned in this embodiment can be referred to the above description of the robot motion control method.

[0139] In the embodiments provided in this application, it should be understood that the disclosed apparatus and methods can also be implemented in other ways. The apparatus embodiments described above are merely illustrative. For example, the flowcharts and block diagrams in the accompanying drawings illustrate the architecture, functionality, and operation of possible implementations of apparatus, methods, and computer program products according to embodiments of this application. In this regard, each block in a flowchart or block diagram may represent a module, segment, or portion of code, which contains one or more executable instructions for implementing a specified logical function. It should also be noted that in some alternative implementations, the functions marked in the blocks may occur in a different order than those marked in the drawings. For example, two consecutive blocks may actually be executed substantially in parallel, and they may sometimes be executed in reverse order, depending on the functions involved. It should also be noted that each block in a block diagram and / or flowchart, and combinations of blocks in block diagrams and / or flowcharts, can be implemented using a dedicated hardware-based system that performs the specified function or action, or using a combination of dedicated hardware and computer instructions.

[0140] Furthermore, the functional modules in the various embodiments of this application can be integrated together to form an independent part, or each module can exist independently, or two or more modules can be integrated to form an independent part. If the various functions provided in this application are implemented in the form of software functional modules and sold or used as independent products, they can be stored in a storage medium. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a readable storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this application. The aforementioned readable storage medium includes: USB flash drives, mobile hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, optical disks, and other media capable of storing program code.

[0141] In summary, in the robot motion control method and apparatus, robot control device, and storage medium provided in the embodiments of this application, based on the current robot end-effector reference trajectory information and robot end-effector state estimation information corresponding to the task to be executed, this application calculates the task transfer matrix and expected target information of each of the multiple sub-tasks of the task to be executed, as well as the constraint transfer matrix and constraint boundary information of each of the multiple constraints. Based on the priority distribution of each of the multiple sub-tasks in the current control cycle, a corresponding task priority matrix and a constraint priority matrix adapted to the priority distribution of the multiple constraints are constructed. Then, based on all task transfer matrices and all expected target information... The target robot is controlled to move according to the target information, all constraint transfer matrices, all constraint boundary information, task priority matrix, and constraint priority matrix. This allows for the dynamic adjustment of constraint priorities as the task priority of the target robot performs complex tasks. This ensures that the influence of each constraint on the complex task is matched with the task priority adjustment of each target, effectively improving the adaptability between robot control commands and actual task conditions, and ensuring the smooth operation of the target robot and the effectiveness of task execution.

[0142] The above descriptions are merely various embodiments of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A robot motion control method, characterized in that, The method includes: Obtain the robot end-effector reference trajectory information and robot end-effector state estimation information corresponding to the task to be executed in the current control cycle of the target robot; Based on the robot end-effector reference trajectory information and the robot end-effector state estimation information, calculate the task transfer matrix and expected target information of each of the multiple subtasks of the task to be executed, as well as the constraint transfer matrix and constraint boundary information of each of the multiple constraints of the task to be executed. Based on the priority distribution of each of the multiple subtasks in the current control cycle, construct the task priority matrix of the task to be executed, and the constraint priority matrix adapted to the priority distribution of the multiple subtasks for the multiple constraints. Based on the task transfer matrix and expected target information of each of the multiple subtasks, the constraint transfer matrix and constraint boundary information of each of the multiple constraints, the task priority matrix and the constraint priority matrix, a priority-level quadratic programming solution is performed to obtain the corresponding programming solution results. Based on the planning and solving results, matching joint control commands are generated for the target robot, and the target robot is controlled to move according to the joint control commands; The step of performing priority-level quadratic programming based on the task transfer matrix and expected target information of each of the multiple subtasks, the constraint transfer matrix and constraint boundary information of each of the multiple constraints, the task priority matrix, and the constraint priority matrix to obtain the corresponding programming solution results includes: Based on the degree-of-freedom occupancy rate of each of the multiple subtasks at different priority levels of the task priority matrix, and based on the task transfer matrix and expected target information of each of the multiple subtasks, the augmented task matrix, augmented expected target, and task degree-of-freedom configuration diagonal matrix of each priority level are calculated. Based on the degree-of-freedom occupancy rate corresponding to each of the multiple constraints at different priority levels of the constraint priority matrix, and based on the constraint transfer matrix and constraint boundary information of each of the multiple constraints, the augmented constraint matrix, augmented constraint boundary, and constraint degree-of-freedom configuration diagonal matrix of each priority level are calculated. For each priority level except the final priority level, the target level projection matrix with dynamic consistency is calculated based on the augmented task matrix and task degrees of freedom configuration diagonal matrix corresponding to the priority level. Based on the augmented task matrix, augmented expected target, augmented constraint matrix, augmented constraint boundary, constrained degrees of freedom configuration diagonal matrix and target level projection matrix of each priority level, the corresponding hierarchical quadratic programming control equation is constructed based on the correlation between the optimization results of two adjacent priority levels. The constructed hierarchical quadratic programming control equations are solved, and the optimization results of the final priority level are used as the programming solution results.

2. The method according to claim 1, characterized in that, The step of calculating the augmented task matrix, augmented expected target, and task degree-of-freedom configuration diagonal matrix for each priority level based on the degree-of-freedom occupancy rate of each of the multiple subtasks at different priority levels of the task priority matrix, and based on the task transfer matrix and expected target information of each of the multiple subtasks, includes: For each priority level, select all first occupancy rates that correspond to that priority level and have a non-zero occupancy rate value from the task priority matrix; Sort all the selected first occupancy rates in descending order to obtain the corresponding first sort result; Sort the subtask sequence numbers corresponding to each first occupancy rate according to the first arrangement result to obtain the corresponding target subtask arrangement order; The task transfer matrix corresponding to each subtask sequence number is augmented according to the order of the target subtasks to obtain the augmented task matrix for that priority level. According to the order of the target sub-tasks, the expected target information corresponding to each sub-task sequence number is subjected to information augmentation processing to obtain the augmented expected target of this priority level. According to the order of the target subtasks, the first occupancy rate corresponding to each subtask sequence number is processed into a diagonal matrix to obtain the task degree of freedom configuration diagonal matrix of this priority level.

3. The method according to claim 1, characterized in that, The step of calculating the augmented constraint matrix, augmented constraint boundary, and constraint degree-of-freedom configuration diagonal matrix for each priority level based on the degree-of-freedom occupancy rate corresponding to each of the multiple constraints at different priority levels of the constraint priority matrix, and based on the constraint transfer matrix and constraint boundary information of each of the multiple constraints, includes: For each priority level, select all second occupancy rates that correspond to that priority level and have a non-zero occupancy rate value from the constraint priority matrix; Sort all the selected second occupancy rates in descending order to obtain the corresponding second sorting results; Sort the constraint sequence numbers corresponding to each second occupancy rate according to the second arrangement result to obtain the corresponding target constraint arrangement order; According to the order of the target constraints, the constraint transfer matrix corresponding to each constraint sequence number is subjected to matrix augmentation to obtain the augmented constraint matrix of that priority level. According to the order of the target constraints, the constraint boundary information corresponding to each constraint sequence number is subjected to information augmentation processing to obtain the augmented constraint boundary of this priority level. According to the order of the target constraints, the second occupancy rate corresponding to each constraint sequence number is processed into a diagonal matrix to obtain the constraint degree of freedom configuration diagonal matrix of this priority level.

4. The method according to claim 1, characterized in that, For each priority level except the final priority level, the step of calculating the target level projection matrix with dynamic consistency for that priority level based on the augmented task matrix and task degrees of freedom configuration diagonal matrix corresponding to that priority level includes: Based on the priority distribution of the multiple subtasks in the current control cycle, a matching task weight matrix with dynamic consistency is constructed for the priority level. Perform Cholesky decomposition on the task weight matrix to obtain the target lower triangular matrix corresponding to the task weight matrix; Perform matrix multiplication on the augmented task matrix of the prior level and the target lower triangular matrix to obtain the task matrix to be eliminated; Perform linear correlation row removal processing on the task matrix to be eliminated to obtain a row full-rank task matrix corresponding to the augmented task matrix; The transpose of the full-rank task matrix is ​​subjected to orthogonal triangular decomposition to obtain the row space orthogonal basis matrix of the full-rank task matrix. In the task degree-of-freedom configuration diagonal matrix at this priority level, extract the diagonal element matrix whose matrix dimension matches the rank value of the full-rank task matrix; Calculate the target level projection matrix based on the target lower triangular matrix, row space orthogonal basis matrix, and diagonal element matrix corresponding to the priority level.

5. The method according to claim 4, characterized in that, The formula for calculating the target level projection matrix of a single priority level is expressed as follows: ； in, Used to indicate the first Target-level projection matrix for each priority level Used to indicate the first The target lower triangular matrix corresponding to each priority level and the task weight matrix. Used to represent the identity matrix. Used to indicate the first The row space orthogonal basis matrices corresponding to the full-rank task matrices at each priority level. Used to indicate the first A diagonal matrix of priorities.

6. The method according to any one of claims 1-5, characterized in that, The correlation between the optimization results of two adjacent priority levels is expressed by the following formula: ； in, Used to indicate the first The optimization results of each priority level at the hierarchical quadratic programming control equation. Used to indicate the first -1 priority level optimization results at the hierarchical quadratic programming control equation. Used to indicate the first -1 priority level target hierarchy projection matrix with dynamic consistency Used to indicate the first The optimization result of the priority level is in the first priority level. -1 priority level, the optimal increment to be superimposed within the hierarchical projection space with dynamic consistency. Used to indicate the first The optimization result of the first priority level is relative to the first priority level. -1 priority level's actual optimization increment. The hierarchy number used to indicate the priority level of the end level.

7. The method according to claim 6, characterized in that, The hierarchical quadratic programming control equation is in the... The quadratic programming optimization sub-equations at each priority level are expressed as follows: ； in, Used to indicate the first The optimization result of the priority level is in the first priority level. -1 priority level of incremental changes to be optimized within the hierarchical projection space with dynamic consistency. Used to indicate the first -1 The optimization results of the quadratic programming sub-equations at the hierarchical quadratic programming governing equations of the priority level. Used to indicate the first -1 priority level target hierarchy projection matrix with dynamic consistency Used to indicate the first An augmented task matrix with priority levels. Used to indicate the first The revised augmented expected target for each priority level, Used to indicate the first Augmented expected goals at each priority level Used to indicate the first The task degree-of-freedom configuration matrix for each priority level consists of a matrix of diagonal elements that match the rank of the corresponding full-rank task matrix. Used to indicate the first The diagonal matrix is ​​configured to represent the constraint degrees of freedom at each priority level. Used to indicate the first Augmented constraint matrices for each priority level, Used to indicate the first Modified augmented constraint boundaries at each priority level Used to indicate the first Augmented constraint boundaries for each priority level, Used to indicate the first -1 priority level constraint degrees of freedom configuration diagonal matrix, Used to indicate the first -1 priority level augmented constraint matrix, Used to indicate the first -1 priority level of modified augmented constraint boundary, Used to indicate the first -1 priority level augmented constraint boundary, The diagonal matrix used to represent the constraint degrees of freedom configuration for the first priority level. Used to represent the augmented constraint matrix of the first priority level. Used to represent the modified augmented constraint boundary of the first priority level. Used to represent the augmented constraint boundary of the first priority level. Used to represent the identity matrix. A factor used to represent a relaxation ratio for constraints with values ​​greater than zero.

8. A robot motion control device, characterized in that, The device includes: The motion information acquisition module is used to acquire the robot end-effector reference trajectory information and robot end-effector state estimation information of the target robot in the current control cycle corresponding to the task to be executed. The task parameter calculation module is used to calculate the task transfer matrix and expected target information of each of the multiple subtasks of the task to be executed, as well as the constraint transfer matrix and constraint boundary information of each of the multiple constraints of the task to be executed, based on the robot end-effector reference trajectory information and the robot end-effector state estimation information. The priority matrix construction module is used to construct the task priority matrix of the task to be executed, and the constraint priority matrix adapted to the priority distribution of the multiple subtasks in the current control cycle, based on the priority distribution of the multiple subtasks. The hierarchical quadratic programming module is used to perform priority-level quadratic programming based on the task transfer matrix and expected target information of each of the multiple subtasks, the constraint transfer matrix and constraint boundary information of each of the multiple constraints, the task priority matrix and the constraint priority matrix, and to obtain the corresponding programming solution results. The motion state control module is used to generate matching joint control commands for the target robot based on the planning solution results, and control the target robot to move according to the joint control commands; The hierarchical quadratic programming module includes: The task-related matrix calculation submodule is used to calculate the augmented task matrix, augmented expected target, and task degree-of-freedom configuration diagonal matrix for each priority level based on the degree-of-freedom occupancy rate of each of the multiple subtasks at different priority levels of the task priority matrix, and based on the task transfer matrix and expected target information of each of the multiple subtasks. The constraint correlation matrix calculation submodule is used to calculate the augmented constraint matrix, augmented constraint boundary, and constraint degree of freedom configuration diagonal matrix for each priority level based on the degree of freedom occupancy rate corresponding to each of the multiple constraints at different priority levels of the constraint priority matrix, and based on the constraint transfer matrix and constraint boundary information of each of the multiple constraints. The hierarchical projection matrix calculation submodule is used to calculate the target hierarchical projection matrix with dynamic consistency for each priority level except the final priority level, based on the augmented task matrix and task degree of freedom configuration diagonal matrix corresponding to the priority level. The hierarchical programming equation construction submodule is used to construct the corresponding hierarchical quadratic programming control equation based on the augmented task matrix, augmented expected target, augmented constraint matrix, augmented constraint boundary, and constraint degrees of freedom configuration of each priority level, and the target level projection matrix. The hierarchical programming equation solving submodule is used to solve the constructed hierarchical quadratic programming control equations and use the optimization results of the terminal priority level as the programming solution results.

9. A robot control device, characterized in that, It includes a processor and a memory, the memory storing a computer program that can be executed by the processor, the processor being able to execute the computer program to implement the robot motion control method according to any one of claims 1-7.

10. A storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the robot motion control method according to any one of claims 1-7.

Images