WO2025112151A1 - Robot obstacle avoidance control method and related device - Google Patents

Abstract

The present application relates to the technical field of robot control. Provided are a robot obstacle avoidance control method and a related device. In the present application, an RMP mapping tree is constructed on the basis of individual actual spatial positions of all obstacles currently present in an operating environment where a target robot is located, such that a root node task of the RMP mapping tree corresponds to a robot joint space, and corresponding leaf node tasks comprise a position motion task and a pose motion task of a robot tail end executing an expected operation, and comprise obstacle avoidance motion tasks of a plurality of robot key parts of the robot tail end respectively performing obstacle avoidance on the obstacles; and then, geometric dynamical systems involving speed information are respectively constructed for various motion tasks, and an expected joint acceleration is solved on the basis of an RMP push-forward operation and an RMP pull-back operation, so as to control the target robot to move, thereby enabling the robot to achieve expected operation execution effects while avoiding dynamic obstacles with high agility and real-time performance.

Description

A robot obstacle avoidance control method and related equipment

This application claims priority to the Chinese patent application filed with the China Patent Office on November 28, 2023, with application number 202311614707.7 and invention name “A Robot Obstacle Avoidance Control Method and Related Equipment”, the entire contents of which are incorporated by reference into this application.

Technical Field

The present application relates to the field of robot control technology, and in particular to a robot obstacle avoidance control method and related equipment.

Background Art

With the continuous development of science and technology, the application of robotics technology in various industries has become more and more extensive. Robots are often required to avoid various obstacles in the robot's operating environment during the execution of the desired operation, so as to avoid robot damage caused by obstacles colliding with the robot, and the expected operation cannot be performed normally. In the actual operation of the robot, the various obstacles in the robot's operating environment are not necessarily static obstacles. Usually, new obstacles will appear or the position of existing obstacles will change, which will lead to dynamic obstacles in the robot's operating environment. Therefore, how to ensure that the robot can avoid dynamic obstacles with high dexterity and high real-time performance during the execution of the expected operation is an important technical issue in the field of robot control technology today.

Technical issues

In view of this, the purpose of the present application is to provide a robot obstacle avoidance control method and apparatus, a robot control device and a readable storage medium, which can utilize the fast iterative solution characteristics of RMP (Riemannian Motion Policies) to ensure that the robot can achieve reactive obstacle avoidance effects for dynamic obstacles while performing desired tasks, thereby improving the real-time performance of robot obstacle avoidance, and by introducing a geometric dynamic system (GDS) involving speed information to perform local motion strategy calculations, thereby utilizing the speed continuity characteristics of the geometric dynamic system to improve the robot's obstacle avoidance dexterity, thereby ensuring that the robot can achieve the desired task execution effect while avoiding dynamic obstacles with high dexterity and high real-time performance.

Technical Solutions

In order to achieve the above purpose, the technical solution adopted in the embodiment of the present application is as follows:

In a first aspect, the present application provides a robot obstacle avoidance control method, the method comprising:

Obtain the actual spatial positions of all obstacles currently existing in the operating environment of the target robot;

According to the actual spatial positions of all obstacles obtained, a corresponding RMP motion strategy mapping tree is constructed for the target robot, wherein the root node task of the RMP motion strategy mapping tree corresponds to the robot joint space, and the leaf node tasks of the RMP motion strategy mapping tree include the position motion task and posture motion task of the robot end to perform the desired operation, and the obstacle avoidance motion task of multiple robot key parts at the robot end to avoid each obstacle respectively;

Constructing a geometric dynamic system involving velocity information for the position motion task, the posture motion task and each obstacle avoidance motion task respectively;

Acquire actual joint state information of the target robot in the current control cycle, and expected position information and expected posture information of the robot end of the target robot that match the expected task in the current control cycle;

Based on the RMP forward operation, the actual joint state information, the expected position information and the expected posture information are transferred from the root node of the RMP motion strategy mapping tree to the geometric dynamic system of each leaf node task to solve the expected inertia matrix and the expected force, and the local expected inertia matrix and the local expected force corresponding to each leaf node task are obtained;

Based on the RMP pullback operation, the local expected inertia matrix and the local expected force of each leaf node task are transferred to the root node to solve the joint acceleration, so as to obtain the expected joint acceleration of the target robot in the current control cycle;

The target robot is controlled to move according to the expected joint acceleration.

In a second aspect, the present application provides a robot obstacle avoidance control device, the device comprising:

The obstacle determination module is used to obtain the actual spatial positions of all obstacles currently existing in the operating environment of the target robot;

An RMP tree construction module is used to construct a corresponding RMP motion strategy mapping tree for the target robot according to the actual spatial positions of all obstacles obtained, wherein the root node task of the RMP motion strategy mapping tree corresponds to the robot joint space, and the leaf node tasks of the RMP motion strategy mapping tree include the position motion task and posture motion task of the robot end to perform the desired operation, and the obstacle avoidance motion task of multiple robot key parts at the robot end to avoid each obstacle respectively;

A dynamic system construction module, used to respectively construct a geometric dynamic system involving velocity information for the position motion task, the posture motion task and each of the obstacle avoidance motion tasks;

A motion parameter acquisition module, used to acquire actual joint state information of the target robot in the current control cycle, and expected position information and expected posture information of the robot end of the target robot that match the expected task in the current control cycle;

A local strategy solving module is used to transfer the actual joint state information, the expected position information and the expected posture information from the root node of the RMP motion strategy mapping tree to the geometric dynamic system of each leaf node task to solve the expected inertia matrix and the expected force based on the RMP forward operation, so as to obtain the local expected inertia matrix and the local expected force corresponding to each leaf node task;

A global strategy solving module is used to transfer the local expected inertia matrix and the local expected force of each leaf node task to the root node to solve the joint acceleration based on the RMP pullback operation, so as to obtain the expected joint acceleration of the target robot in the current control cycle;

The obstacle avoidance operation control module is used to control the target robot to move according to the expected joint acceleration.

In a third aspect, the present application provides a robot control device, comprising a processor and a memory, wherein the memory stores a computer program executable by the processor, and the processor can execute the computer program to implement the robot obstacle avoidance control method described in any one of the aforementioned embodiments.

In a fourth aspect, the present application provides a readable storage medium having a computer program stored thereon, and when the computer program is executed, the robot obstacle avoidance control method described in any one of the aforementioned embodiments is implemented.

Beneficial Effects

In this case, the beneficial effects of the embodiments of the present application may include the following:

The present application constructs an RMP motion strategy mapping tree according to the actual spatial positions of all obstacles currently existing in the operating environment where the target robot is located, so that the root node task of the RMP motion strategy mapping tree corresponds to the robot joint space, and the corresponding leaf node tasks include the position motion task and posture motion task of the robot terminal to perform the expected operation, as well as the obstacle avoidance motion task of multiple key parts of the robot terminal to avoid each obstacle respectively. Then, geometric dynamic systems involving speed information are constructed for the aforementioned multiple motion tasks respectively, and based on the RMP forward operation, the actual joint state information of the target robot in the current control cycle, and the expected position information and expected posture information of the robot terminal of the target robot that match the expected operation in the current control cycle are transferred from the root node of the RMP motion strategy mapping tree to the geometric dynamic systems of each leaf node task to solve the expected inertia matrix and the expected force. , the local expected inertia matrix and local expected force of each leaf node task are obtained, and then based on the RMP pullback operation, the local expected inertia matrix and local expected force of each leaf node task are transferred to the root node for joint acceleration solution, and the expected joint acceleration of the target robot in the current control cycle is obtained, and the target robot is controlled to move according to the expected joint acceleration, so as to utilize the fast iterative solution characteristics corresponding to the RMP push forward operation and the RMP pullback operation to ensure that the robot can achieve reactive obstacle avoidance effect for dynamic obstacles in the process of performing the expected operation, improve the real-time obstacle avoidance of the robot, and introduce a geometric dynamic system involving speed information in the implementation process of the RMP push forward operation to calculate the local motion strategy, so as to utilize the speed continuity characteristics of the geometric dynamic system to improve the robot's obstacle avoidance dexterity, so as to ensure that the robot can achieve the expected operation execution effect while avoiding dynamic obstacles with high dexterity and high real-time performance.

In order to make the above-mentioned objects, features and advantages of the present application more obvious and easy to understand, preferred embodiments are specifically cited below and described in detail with reference to the attached drawings.

BRIEF DESCRIPTION OF THE DRAWINGS

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

FIG1 is a schematic diagram of the composition of a robot control device provided in an embodiment of the present application;

FIG2 is a schematic diagram of a flow chart of a robot obstacle avoidance control method provided in an embodiment of the present application;

FIG3 is a schematic flow chart of the sub-steps included in step S220 in FIG2 ;

FIG4 is a schematic diagram of a tree structure of an RMP motion strategy mapping tree provided in an embodiment of the present application;

FIG5 is a schematic flow chart of the sub-steps included in step S260 in FIG2 ;

FIG6 is a schematic diagram of the composition of a robot obstacle avoidance control device provided in an embodiment of the present application.

Icons: 10 - robot control device; 11 - memory; 12 - processor; 13 - communication unit; 100 - robot obstacle avoidance control device; 110 - obstacle determination module; 120 - RMP tree construction module; 130 - dynamic system construction module; 140 - motion parameter acquisition module; 150 - local strategy solution module; 160 - global strategy solution module; 170 - obstacle avoidance operation control module.

Embodiments of the present invention

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

Therefore, the following detailed description of the embodiments of the present application provided in the accompanying drawings is not intended to limit the scope of the present application as claimed. All other embodiments obtained by ordinary technicians in this field without creative work based on the embodiments in this application are within the scope of protection of this application.

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

In the description of the present application, it is to be understood that the relational terms such as the term "first" and "second" are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply that there is any such actual relationship or order between these entities or operations. Moreover, the term "includes", "comprising" or any other variant thereof is intended to cover non-exclusive inclusion, so that the process, method, article or equipment including a series of elements includes not only those elements, but also includes other elements not explicitly listed, or also includes elements inherent to such process, method, article or equipment. In the absence of more restrictions, the elements defined by the sentence "comprising one..." do not exclude the existence of other identical elements in the process, method, article or equipment including the elements. For those of ordinary skill in the art, the specific meaning of the above terms in the present application can be understood by specific circumstances.

In the description of this application, it should also be noted that, unless otherwise clearly specified and limited, the terms "set", "install", "connect", and "connect" should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection, or it can be indirectly connected through an intermediate medium, or it can be the internal communication of two elements. For ordinary technicians in this field, the specific meanings of the above terms in this application can be understood according to specific circumstances.

The applicant has found through hard research that the existing methods of controlling robots to avoid obstacles can be roughly divided into the following four types:

(1) Sampling method: The sampling method determines and expands the collision-free area by randomly sampling in the robot's operating environment. When it is expanded to the target position, a collision-free path is automatically formed. The advantage of this method lies in its completeness, that is, when a collision-free path exists, the sampling method can definitely find it. Therefore, it is suitable for path search in a large range of complex environments. However, this method cannot guarantee that the generated path is the shortest or the speed is continuous. At the same time, its solution efficiency is also very random. It is actually suitable for obstacle avoidance effects against static obstacles.

(2) Optimization method: The optimization method is to establish simple interpolation path points between the starting point and the target point as references, and then use collision-free as the constraint of these interpolation path points to construct numerical optimization equations about the output of the robot joints for solution. The advantage of this method is that the optimization equation can be improved according to the task requirements, such as requiring the shortest path or the shortest time, and once the corresponding optimization equation is solved, an ideal collision-free trajectory can be obtained. However, the solution quality and efficiency of the optimization method are inversely correlated, so this method is usually used as an offline static obstacle avoidance planning method.

(3) Stochastic process method: The stochastic process method can be seen as a combination of sampling and optimization methods, except that the elements of sampling and optimization are Gaussian process parameters that describe continuous trajectories. The advantage of this method is that it has natural speed continuity and can accelerate the optimization process through probabilistic reasoning methods. However, even so, the stochastic process method can only perform rapid replanning after identifying new obstacles in the environment, and its solution rate is not sufficient to achieve real-time online planning in the face of dynamic obstacles.

(4) Potential energy method: The potential energy law is based on the idea of force field. It regards the moving target point as the source of gravity in the force field and the obstacle as the source of repulsion. Therefore, the robot can be guided to avoid obstacles while moving toward the moving target point based on the potential energy superimposed between the gravity source and the repulsion source. The advantage of this method is that the algorithm is simple to implement and the solution is fast. It can avoid dynamic obstacles to a certain extent. The reason why it is said to be to a certain extent is that the potential energy method only considers the position state information of the robot, which makes it easy for this method to get stuck in the local optimum due to insufficient dynamic dexterity, and it is actually impossible to avoid dynamic obstacles with high dexterity.

In this case, in order to solve the above problems, the embodiments of the present application provide a robot obstacle avoidance control method and apparatus, a robot control device and a readable storage medium, so as to utilize the fast iterative solution characteristics of RMP and the speed continuity characteristics of the geometric dynamic system involving speed information, so as to ensure that the robot can achieve a reactive obstacle avoidance effect for dynamic obstacles in the process of performing the desired operation, improve the real-time and dexterity of the robot's obstacle avoidance, and enable the robot to avoid dynamic obstacles with high dexterity and high real-time performance while achieving the desired operation execution effect.

In conjunction with the accompanying drawings, some embodiments of the present application are described in detail below. In the absence of conflict, the following embodiments and features in the embodiments can be combined with each other.

Please refer to Figure 1, which is a schematic diagram of the composition of the robot control device 10 provided in the embodiment of the present application. In the embodiment of the present application, the robot control device 10 is connected to the target robot in communication, and is used to control the motion state of the target robot, so that the target robot can quickly and flexibly avoid dynamic obstacles in the operating environment of the target robot while performing complex operation tasks. Among them, 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. Among them, the computer device can be a personal computer, a cloud server, a laptop computer, a tablet computer, etc.; the target robot can be a serial/parallel configuration redundant robot with position control, force control or force-position mixed control, or it can be a redundant robotic arm, a quadruped robot or a humanoid robot used in industries, services, special industries and other fields.

In the embodiment of the present application, the robot control device 10 may include a memory 11, a processor 12, a communication unit 13, and a robot obstacle avoidance control device 100. The memory 11, the processor 12, and the communication unit 13 are electrically connected to each other directly or indirectly to achieve data transmission or interaction. For example, the memory 11, the processor 12, and the communication unit 13 may be electrically connected to each other via one or more communication buses or signal lines.

In the embodiment of the present application, the memory 11 may be, but not limited to, a random access memory (RAM), a read-only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable read-only memory (EEPROM), etc. The memory 11 is used to store a computer program, and the processor 12 may execute the computer program accordingly after receiving an execution instruction.

In the embodiment of the present application, the processor 12 may be an integrated circuit chip with signal processing capability. The processor 12 may be a general-purpose processor, including a central processing unit (CPU), a graphics processing unit (GPU), a network processor (NP), a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field programmable gate array (FPGA), or at least one of other programmable logic devices, discrete gate or transistor logic devices, and discrete hardware components. The general-purpose processor may be a microprocessor or the processor may be any conventional processor, etc., which may implement or execute the disclosed methods, steps, and logic block diagrams in the embodiment of the present application.

In the embodiment of the present application, the communication unit 13 is used to establish a communication connection between the robot control device 10 and other electronic devices through a network, and to send and receive data through the network, wherein the network includes a wired communication network and a wireless communication network. For example, the robot control device 10 can obtain the motion trajectory information of the desired operation to be performed by the robot end of the target robot from the operation planning device through the communication unit 13, wherein the motion trajectory information can include the desired position information, desired posture information, etc. corresponding to different control cycles of the robot end of the target robot in the process of performing the desired operation; the robot control device 10 can send joint control instructions to the target robot through the communication unit 13 to drive the target robot to move according to the received joint control instructions.

In the embodiment of the present application, the robot obstacle avoidance 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 solidified 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 obstacle avoidance control device 100. The robot control device 10 can use the fast iterative solution characteristics of RMP and the speed continuity characteristics of the geometric dynamic system involving speed information through the robot obstacle avoidance control device 100 to ensure that the robot achieves a reactive obstacle avoidance effect for dynamic obstacles in the process of performing the desired operation, improve the real-time and dexterity of robot obstacle avoidance, and enable the robot to avoid dynamic obstacles with high dexterity and high real-time performance while achieving the desired operation execution effect.

It is understandable that the block diagram shown in FIG1 is only a schematic diagram of a composition of the robot control device 10, and the robot control device 10 may also include more or fewer components than those shown in FIG1, or have a configuration different from that shown in FIG1. Each component shown in FIG1 may be implemented by hardware, software, or a combination thereof.

In the present application, in order to ensure that the robot control device 10 can utilize the fast iterative solution characteristics of RMP and the speed continuity characteristics of the geometric dynamic system involving speed information, ensure that the robot achieves a reactive obstacle avoidance effect for dynamic obstacles in the process of performing the desired operation, improve the robot's obstacle avoidance real-time performance and the robot's obstacle avoidance dexterity, and enable the robot to avoid dynamic obstacles with high dexterity and high real-time performance while achieving the desired operation execution effect, the present application embodiment provides a robot obstacle avoidance control method to achieve the above-mentioned purpose. The robot obstacle avoidance control method provided by the present application is described in detail below.

Please refer to Figure 2, which is a flow chart of a robot obstacle avoidance control method provided in an embodiment of the present application. In an embodiment of the present application, the robot obstacle avoidance control method shown in Figure 2 is applied to the above-mentioned robot control device 10, and the robot obstacle avoidance control method may include steps S210 to S270.

Step S210, obtaining the actual spatial positions of all obstacles currently existing in the operating environment of the target robot.

Among them, at least one obstacle detection device can be installed in the operating environment where the target robot is located, so as to detect which obstacles currently exist in the operating environment and the actual spatial position of each obstacle currently existing in the operating environment at the current moment through the at least one obstacle detection device, and then the at least one obstacle detection device sends the detected actual spatial position of all obstacles in the operating environment to the robot control device 10, so that the robot control device 10 can intuitively determine the changes in the number of obstacles and the changes in the positions of obstacles in the operating environment where the target robot is located. Among them, the aforementioned obstacle detection device can be, but is not limited to, a laser radar, a camera, an ultrasonic radar, etc.; the obstacle detection device can be installed on the target robot, or can be set around the target robot, or can be placed in the operating environment after the robot control device 10 is placed. In the domestic case, the corresponding obstacle detection device is set on the robot control device 10. The specific number and/or specific setting method of the obstacle detection device can be set differently according to the robot operating environment monitoring requirements, and this application does not limit this.

Step S220: construct a corresponding RMP motion strategy mapping tree for the target robot according to the acquired actual spatial positions of all obstacles.

In this embodiment, the root node task of the RMP motion strategy mapping tree corresponds to the robot joint space, and the leaf node tasks of the RMP motion strategy mapping tree include the position motion task and posture motion task of the robot end to perform the desired operation, and the obstacle avoidance motion task of multiple robot key parts of the robot end to avoid obstacles respectively. Among them, the position motion task is used to characterize the motion task of the robot end of the target robot to perform the desired operation at the position dimension level, the posture motion task is used to characterize the motion task of the robot end of the target robot to perform the desired operation at the posture dimension level, and the obstacle avoidance motion task is used to characterize the motion task of the robot key parts of the target robot to avoid existing obstacles in the operating environment.

As for RMP (Riemannian motion strategy), its essence belongs to the motion strategy with geometric information described by the second-order differential equation in the Riemann manifold space. The mathematical canonical form of RMP can be expressed as The mathematical natural form of RMP can be expressed as in, Indicates that the spatial coordinates belong to In the m-dimensional Riemannian manifold space, a:P ^m ×P ^m →P ^m represents a second-order continuous motion strategy, M:P ^m ×P ^m →P ^m×m represents a differential mapping, and f = Ma. When this RMP is applied to robot dynamics, a can be regarded as the expected acceleration, M as the expected inertia matrix, and f as the expected force.

Therefore, when the robot control device 10 obtains the actual spatial positions of all obstacles actually existing in the operating environment, the robot motion task can be split into multiple motion strategies contained in different dimensional Riemann spaces such as the robot joint space and the robot task space according to the RMP architecture, so that the split motion strategy in the robot joint space can directly serve as the root node of the RMP motion strategy mapping tree, and the split motion strategy in the robot task space can directly serve as the leaf node of the RMP motion strategy mapping tree. At this time, the root node task of the RMP motion strategy mapping tree is actually used to characterize the global motion task (i.e., the global motion strategy) corresponding to the expected effect of the target robot realizing the robot motion task in the robot joint space, and the leaf node tasks of the RMP motion strategy mapping tree are used to characterize the branch sub-tasks of the target robot realizing the robot motion task in the robot task space. The local motion task (i.e., local motion strategy) of the service, thereby the robot control device 10 can be split by "driving the target robot to perform the desired task and ensuring that each key part of the target robot avoids various obstacles in the current operating environment" as the robot motion task, thereby obtaining an RMP motion strategy mapping tree including the root node task "global motion task in the robot joint space", the leaf node task "position motion task of the robot terminal to perform the desired task in the robot task space", the leaf node task "posture change task of the robot terminal to perform the desired task in the robot task space", the leaf node task "obstacle avoidance motion task of the robot terminal to avoid various obstacles in the robot task space" and the leaf node task "obstacle avoidance motion task of each key part of the robot except the robot terminal to avoid various obstacles in the robot task space".

Optionally, in one implementation of the present embodiment, the robot control device 10 may directly use the root node task "the position motion task of the robot end to perform the desired operation in the robot task space" as the parent node of each leaf node task to construct the RMP motion strategy mapping tree, so as to use the spatial transformation relationship between the robot joint space corresponding to the root node task and the robot task space corresponding to each leaf node task as the connecting edge between the root node and the corresponding leaf node.

Optionally, please refer to FIG3 , which is a flowchart of the sub-steps included in step S220 in FIG2 . In another implementation of this embodiment, in order to ensure that the various local motion strategies involved in the RMP motion strategy mapping tree constructed by this application can be relatively easily transplanted to another robot for use, a stem node that plays a connecting role can be introduced between the root node and each leaf node, and the stem node is only related to the robot kinematic model, without actively adding motion strategies, so as to disconnect the body association between the local motion strategy corresponding to the leaf node and the target robot, so that after a leaf node is established, it can be directly replaced by replacing the stem node task connected to it. That is, the local motion strategy corresponding to the leaf node can be easily transplanted to another robot for use. At this time, the step S220 may include sub-steps S221 to S225 to construct an RMP motion strategy mapping tree involving desired operations and obstacle avoidance motions with strong portability of motion strategies.

Sub-step S221 decomposes the global motion task of the target robot in the robot joint space to obtain the posture change task of the robot end of the target robot in the posture space, the position change task of the robot end in the position space, and the position change tasks of each key part of the target robot except the robot end in the corresponding position space.

In this embodiment, the Characterize the target robot in the robot joint space The global motion task is Used to characterize the state parameters (joint position, joint velocity) involved in the global motion task, The mathematical natural form of RMP used to characterize the global motion task can be used Characterize the robot end in posture space The posture change task under It is used to characterize the state parameters (posture information, posture change speed) involved in the posture change task. The mathematical natural form of RMP used to characterize the posture change task is: Can be used Characterize the robot end in position space The position change task is It is used to characterize the state parameters involved in the position change task (position information, position change speed), The RMP mathematical natural form used to characterize the position change task is: Can be used Characterize the corresponding position space of l-1 key parts of the robot except the end of the robot The position change task is It is used to characterize the state parameters (position information, position change speed) involved in the position change task of the key parts of the i-th robot. The mathematical natural form of RMP used to characterize the position change task of the key part of the i-th robot, where At this time, the aforementioned global motion task can be regarded as the common parent node of the aforementioned posture change task and all position change tasks.

Sub-step S222, decomposing the posture change task of the robot terminal in the posture space into the task space of the robot terminal, and obtaining the posture movement task of the robot terminal to perform the desired operation in the corresponding task space.

In this embodiment, the Characterize the robot end in the task space The posture motion task of the expected job is performed under It is used to characterize the state parameters (posture information, posture change speed) involved in the posture motion task. The mathematical natural form of RMP used to characterize the posture motion task is: At this time, the aforementioned posture change task can be regarded as the parent node of the posture movement task.

Sub-step S223, according to the actual spatial positions of all obstacles, decomposes the position change task of the robot terminal in the position space into the task space of the robot terminal, and obtains the position movement task of the robot terminal to perform the desired operation in the corresponding task space, and the obstacle avoidance movement task of the robot terminal to avoid each obstacle in the corresponding task space.

In this embodiment, the robot end can be placed in the position space The position change task is decomposed into position space Task spaces of the same dimension Next, we get the robot end in the task space The position motion task of the expected operation is performed under the condition that the position motion task can be used To characterize, It is used to characterize the state parameters (position information, position change speed) involved in the position motion task. The mathematical natural form of RMP used to characterize the position motion task is: At this time, the aforementioned position change task can be regarded as the parent node of the position movement task.

At the same time, the robot end can be placed in the position space The position change task under the robot is decomposed into a one-dimensional manifold task space corresponding to each obstacle, which describes the change of the relative distance between the robot end and the corresponding obstacle. Next, we get the robot end in the task space The robot terminal is in the task space corresponding to the jth obstacle. The obstacle avoidance task to avoid the obstacle can be achieved by To characterize, It is used to characterize the state parameters involved in the obstacle avoidance motion task (the relative distance between the robot end and the jth obstacle, the relative motion speed between the robot end and the jth obstacle). The RMP mathematical natural form is used to characterize the obstacle avoidance motion task, and n is used to represent the total number of obstacles in the operating environment. At this time, the aforementioned position change task can be regarded as the parent node of the obstacle avoidance motion task, and the relative distance and relative motion speed between the aforementioned robot end and the jth obstacle can be obtained based on the actual spatial position of the jth obstacle.

Sub-step S224, according to the actual spatial positions of all obstacles, each key part of the robot except the end of the robot is moved The position change task in the corresponding position space is decomposed into the corresponding task space, and the obstacle avoidance motion tasks of the key parts of the robot except the end of the robot to avoid various obstacles in the corresponding task space are obtained.

In this embodiment, for each key part of the robot except the end of the robot, the key part of the robot can be positioned in the corresponding position space. The position change task under the robot is decomposed into a one-dimensional manifold task space corresponding to each obstacle, which describes the change of the relative distance between the key parts of the robot and the corresponding obstacle. Next, we get the key part of the i-th (i＝2,…,l) robot in the task space The next obstacle avoidance task is to avoid the jth obstacle. At this time, the key part of the i-th robot is in the task space corresponding to the j-th obstacle. The obstacle avoidance task to avoid the obstacle can be achieved by To characterize, It is used to characterize the state parameters involved in the obstacle avoidance motion task (the relative distance between the key part of the i-th robot and the j-th obstacle, the relative motion speed between the key part of the i-th robot and the j-th obstacle), The RMP mathematical natural form used to characterize the obstacle avoidance motion task. At this time, the aforementioned position change task can be regarded as the parent node of the obstacle avoidance motion task. The relative distance and relative motion speed between the aforementioned i-th robot key part and the j-th obstacle can be obtained based on the actual spatial position of the j-th obstacle.

Sub-step S225, according to the spatial transformation relationship between the robot joint space, posture space, position space and task space, construct a tree structure for the global motion task, posture change task, all position change tasks, posture motion task, position motion task and all obstacle avoidance motion tasks to obtain the RMP motion strategy mapping tree.

The spatial transformation relationship among the robot joint space, posture space, position space and task space may include the robot joint space and posture space The spatial transformation relationship between the robot joint space With location space The spatial transformation relationship between them, the posture space With Task Space The spatial transformation relationship between the position space With Task Space The spatial transformation relationship between them, and the position space With Task Space The spatial transformation relationship between them.

At this time, taking the tree structure diagram of the RMP motion strategy mapping tree shown in Figure 4 as an example, the global motion task can be taken as the root node r, the posture change task can be taken as the stem node (i.e., the child node of the root node r) _t0 , the position change task of the robot terminal can be taken as the stem node _t1 , the position change tasks of each key part of the robot except the robot terminal can be taken as the stem nodes _ti |(i＝2,…,l), the posture motion task of the robot terminal can be taken as the _leaf node (i.e., the child node of the stem node _t0 ) _g0 , the position motion task of the robot terminal can be taken as the leaf node _gp of the stem node _t1 , and the obstacle avoidance motion task corresponding to the jth (j＝1,…,n) obstacle of the robot terminal can be taken as the leaf node of the stem node _t1. The jth (j＝1,…,n) obstacle corresponding to the key part of the i-th robot except the end of the robot The obstacle avoidance task is the leaf node of the stem node _ti corresponding to the key part of the ith robot.

The RMP motion strategy mapping tree includes the leaf node task “the posture change task of the robot end to avoid various obstacles in the robot task space”, the leaf node task “the obstacle avoidance motion task of each robot key part except the robot end to avoid various obstacles in the robot task space”.

Therefore, the present application can construct an RMP motion strategy mapping tree involving desired operations and obstacle avoidance motions with strong motion strategy portability by executing the above sub-steps S221 to S225.

Step S230 , constructing a geometric dynamic system involving velocity information for the position motion task, the posture motion task and each obstacle avoidance motion task respectively.

In this embodiment, the geometric dynamic system can be regarded as a virtual mechanical system defined on the manifold space, and the system inertia is determined by the configuration and speed of the mechanical body. A single geometric dynamic system can be represented by a mathematical tuple To express, It is used to represent the manifold space where the corresponding geometric dynamic system is located (for example, the task space Mission Space and task space At this time, the geometric dynamic system needs to meet the following conditions:

in, Used to represent the corresponding geometric dynamic system and system state parameters The corresponding geometric metric matrix, Used to represent the corresponding geometric dynamic system and system state parameters The corresponding damping matrix, Φ(x), is used to represent the potential energy equation of the corresponding geometric dynamic system, Used to represent the corresponding geometric dynamic system and system state parameters The corresponding first curvature term matrix is, Used to represent the corresponding geometric dynamic system and system state parameters The corresponding second curvature term matrix is, Used to indicate The submatrix of column i in , It is used to express the partial derivative of the state parameter x. Used to indicate the status parameters Find the partial derivative, It is used to indicate the gradient of the state parameter x, and m is used to indicate The total number of matrix columns, state parameters is the differential of the state parameter x, the state parameter is the state parameter The differential of .

In this case, the state parameters x and state parameters corresponding to the position motion task They are used to represent the position information and position change speed information of the robot end, and the state parameters x and y corresponding to the posture motion task. They are used to represent the posture information and posture change speed information of the robot end, and the state parameters x and state parameters corresponding to the obstacle avoidance motion task. They are respectively used to represent the relative distance information and relative motion speed information between the key parts of the robot including the end of the robot and the corresponding obstacles.

It can be understood that the geometric dynamic systems corresponding to the above position motion tasks, posture motion tasks and various obstacle avoidance motion tasks can be The design is based on the robot's movement requirements (for example, maintaining balance, maintaining a good configuration, smoothly avoiding obstacle collisions, smoothly reaching the desired position, smoothly reaching the desired posture, jumping to the desired position, jumping to the desired posture, abruptly stopping to avoid obstacle collisions, etc.). This application does not limit the specific design ideas of the geometric dynamic system.

Optionally, in one implementation of this embodiment, based on the robot motion requirement of smoothly reaching the desired posture, the geometric dynamic system corresponding to the posture motion task can be designed as in It is used to indicate that the manifold space where the posture motion task is located is the task space where the leaf node g _o is located. and The calculation formula is as follows:

in, Used to represent geometric dynamic systems The geometric metric matrix, I is used to represent the identity matrix with the same dimension as the robot joint space, and w ^o is used to represent the geometric dynamic system The measurement coefficient of Used to represent geometric dynamic systems The damping matrix is Used to represent geometric dynamic systems The potential energy equation is: It is used to represent the desired posture of the robot end, _x0 is used to represent the actual posture of the robot end, e is used to represent the vector difference between the actual posture of the robot end and the desired posture, ||e|| is used to represent the modulus of vector e, It is used to express the gradient of vector e. Used to indicate the upper limit of the measurement coefficient w ^o , It is used to indicate the lower limit of the measurement coefficient w ^o , r _p is used to indicate the proportional gain, r _d is used to indicate the velocity gain, σ is used to indicate the scale parameter, and α is used to indicate the smoothness parameter.

Optionally, in one implementation of this embodiment, based on the robot motion requirement of smoothly reaching the desired position, the geometric dynamic system corresponding to the position motion task can be designed as in It is used to indicate that the manifold space where the position motion task is located is the task space where the leaf node _gp is located. and The calculation formula is as follows:

in, Used to represent geometric dynamic systems The geometric metric matrix, I is used to represent the identity matrix with the same dimension as the robot joint space, and w ^p is used to represent the geometric dynamic system The measurement coefficient of Used to represent geometric dynamic systems The damping matrix is Used to represent geometric dynamic systems The potential energy equation, is used to represent the desired position of the robot end, _x1 is used to represent the actual position of the robot end, d is used to represent the vector difference between the actual position and the desired position of the robot end, ||d|| is used to represent the modulus of vector d, It is used to express the gradient of vector d. Used to indicate the upper limit of the measurement coefficient w ^p , It is used to indicate the lower limit of the metric coefficient w ^p , r _p is used to indicate the proportional gain, r _d is used to indicate the velocity gain, σ is used to indicate the scale parameter, and α is used to indicate the smoothness parameter.

Optionally, in one implementation of this embodiment, based on the robot motion requirement of smoothly avoiding obstacle collision, the geometric dynamic system corresponding to the obstacle avoidance motion task of the i-th (i=1,…,l)-th robot key part for the j-th (j=1,…,n)-th obstacle can be designed as: in It is used to indicate that the manifold space where the obstacle avoidance motion task is located is a leaf node The task space where you are located, and The calculation formula is as follows:

in, Used to represent geometric dynamic systems The geometric metric matrix of Used to represent geometric dynamic systems The damping matrix is Used to represent geometric dynamic systems The potential energy equation, It is used to indicate the relative distance between the key part of the i-th robot and the j-th obstacle. It is used to indicate the relative motion speed between the key part of the i-th robot and the j-th obstacle. Used to indicate the status parameters Find the gradient, _su is used to indicate the maximum safe distance, _sl is used to indicate the minimum safe distance, _rp is used to indicate the proportional gain, _rd is used to indicate the velocity gain, σ is used to indicate the scale parameter, and ε is a small positive number.

Step S240, obtaining actual joint state information of the target robot in the current control cycle, and expected position information and expected posture information of the robot end of the target robot that match the expected task in the current control cycle.

The actual joint state information includes the actual joint position and actual joint speed of the target robot, and the actual joint state information can be directly read from the target robot by the robot control device 10; the expected position information and the expected posture information can be read from the target robot by the robot control device 10, or can be obtained from the operation planning device by the robot control device 10. This application does not limit the specific method for the robot control device 10 to obtain the expected position information and the expected posture information.

Step S250, based on the RMP forward operation, transfers the actual joint state information, expected position information and expected posture information from the root node of the RMP motion strategy mapping tree to the geometric dynamic system of each leaf node task to solve the expected inertia matrix and the expected force, and obtains the local expected inertia matrix and local expected force corresponding to each leaf node task.

In this embodiment, for the RMP architecture, the RMP forward operation is essentially a forward transfer operation of the state information flow from the parent node to the child node. And the K child nodes under the parent node There is a spatial transformation relationship between the parent node and the i-th child node Then the aforementioned RMP push forward operation can be expressed as:

in, Used to represent the partial derivative of the state parameter x.

Therefore, the robot control device 10 can be based on the spatial transformation relationship between the root node and each leaf node revealed by the RMP motion strategy mapping tree (including the spatial transformation relationship between the root node and each stem node in Figure 4, and the spatial transformation relationship between each stem node and at least one leaf node corresponding to it), and transmit the above-mentioned actual joint state information, the above-mentioned expected position information and the above-mentioned expected posture information to the geometric dynamic system of each leaf node through the RMP forward operation, and then solve the expected inertia matrix and the expected force for the geometric dynamic system of each leaf node to obtain the local expected inertia matrix and local expected force required for each leaf node to realize its corresponding branch subtask under the action of the above-mentioned actual joint state information, the above-mentioned expected position information and the above-mentioned expected posture information.

Among them, the calculation formula of the local expected inertia matrix and the local expected force corresponding to each geometric dynamic system is expressed as follows:

in, Used to represent the corresponding geometric dynamic system and system state parameters The corresponding local expected inertia matrix, Used to represent the corresponding geometric dynamic system and system state parameters The corresponding local expected force.

Therefore, the present application can accurately calculate the local expected inertia matrix and the local expected force of each leaf node task included in the RMP motion strategy mapping tree through the above calculation formula.

Optionally, in one implementation of this embodiment, when the geometric dynamic system corresponding to the posture motion task is designed as above Then the local expected inertia matrix corresponding to the posture motion task is and the local desired force The actual calculation formula is as follows:

in, It is used to indicate the vector change speed between the actual posture and the expected posture of the robot end. Used to indicate the speed of vector change The rate value.

Optionally, in one implementation of this embodiment, when the geometric dynamic system corresponding to the position motion task is designed as above Then the local expected inertia matrix corresponding to the position motion task is and the local desired force The actual calculation formula is as follows:

in, It is used to indicate the vector change speed between the actual position and the expected position of the robot end. Used to indicate the speed of vector change The rate value.

Optionally, in one implementation of this embodiment, when the geometric dynamic system corresponding to the obstacle avoidance motion task of the i-th (i=1, ..., l)-th robot key part for the j-th (j=1, ..., n)-th obstacle is designed as above Then the local expected inertia matrix corresponding to the obstacle avoidance motion task is and the local desired force The actual calculation formula is as follows:

in, Used to indicate the status parameters Find the gradient.

Step S250, based on the RMP pullback operation, the local expected inertia matrix and the local expected force of each leaf node task are transferred to the root node to solve the joint acceleration, so as to obtain the expected joint acceleration of the target robot in the current control cycle.

In this embodiment, for the RMP architecture, the RMP pullback operation is actually a reverse transfer operation of the RMP information flow from the child node to the parent node. And the K child nodes under the parent node There is a spatial transformation relationship between the parent node and the i-th child node and Then the aforementioned RMP pullback operation can be expressed as:

in, Used to represent the differential of _Ji .

Therefore, the robot control device 10 can transfer the local expected inertia matrix and local expected force of each leaf node task to the root node through the RMP pullback operation based on the spatial transformation relationship between the root node and each leaf node revealed by the RMP motion strategy mapping tree (including the spatial transformation relationship between the root node and each stem node in Figure 4, and the spatial transformation relationship between each stem node and at least one leaf node corresponding to it), and obtain the global expected inertia matrix and global expected force of the root node that simultaneously realize the expected task effects of each leaf node task in the robot joint space, and then solve the joint acceleration for the root node based on the obtained global expected inertia matrix and global expected force, so as to obtain the expected joint acceleration that realizes the expected effect of the global motion strategy corresponding to the robot motion task.

At this time, please refer to Figure 5, which is a flow chart of the sub-steps included in step S260 in Figure 2. In the embodiment of the present application, step S260 may include sub-steps S261 and S262 to quickly integrate multiple local motion strategies involving dynamic obstacle avoidance functions and desired operation execution functions into a global motion strategy, ensuring that the strategy output of the global motion strategy can achieve the robot motion effect of all local motion strategies, and at the same time ensuring the real-time output of the global motion strategy through the RMP propagation algorithm (including RMP forward push operation and RMP pull-back operation), so as to ensure that the target robot can achieve a reactive obstacle avoidance effect for dynamic obstacles in the process of performing the desired operation.

Sub-step S261, according to the spatial transformation relationship between the root node and each leaf node in the RMP motion strategy mapping tree, based on the RMP pullback operation, the local expected inertia matrix and local expected force of each leaf node task are transferred to the root node to obtain the global expected inertia matrix and global expected force of the root node in the current control cycle.

Sub-step S262, calculating the expected joint acceleration of the target robot in the current control cycle according to the global expected inertia matrix and the global expected force.

In one implementation of this embodiment, after obtaining the global expected inertia matrix and the global expected force corresponding to the root node, the robot control device 10 can calculate the expected joint acceleration corresponding to the global motion strategy of the target robot in the current control cycle based on the conversion relationship between the mathematical specification form and the mathematical natural form of RMP using the following formula:
a＝M ⁺ f;

Wherein, M ⁺ =(M ^T M) ^-1 M ^T , a is used to represent the expected joint acceleration, M ⁺ is used to represent the pseudo-inverse matrix of the global expected inertia matrix M, and f is used to represent the global expected force.

In another implementation of this embodiment, in order to avoid the situation where the expected joint acceleration settlement result of the robot control device 10 is too large when the target robot moves to the vicinity of the singularity, affecting the safety and reliability of the global motion strategy solution process, the above sub-step S262 may include sub-steps a to c:

Sub-step a: constructing a target acceleration constraint condition for the joint acceleration of the target robot according to the joint limit motion position range and the joint acceleration control range of the target robot.

The target acceleration constraint condition is expressed by the following formula:

Wherein, q ^* is used to represent the actual joint position included in the actual joint state information, is used to represent the actual joint speed included in the actual joint state information, Δt is used to represent the cycle length of a single control cycle, _qu is used to represent the upper limit value of the joint position included in the joint limit motion position interval, _ql is used to represent the lower limit value of the joint position included in the joint limit motion position interval, It is used to indicate the upper limit value of the joint acceleration included in the joint acceleration control interval, It is used to indicate the lower limit value of the joint acceleration included in the joint acceleration control interval, Used to represent the joint acceleration of the target robot.

Sub-step b: constructing a corresponding acceleration error function for the joint acceleration of the target robot according to the global expected inertia matrix and the global expected force.

The acceleration error function is expressed as follows:

Among them, _Mr is used to represent the global expected inertia matrix, and f _r is used to represent the global expected force.

Sub-step c, with the purpose of minimizing the actual output size of the acceleration error function, solves the quadratic programming problem for the joint acceleration based on the target acceleration constraint to obtain the expected joint acceleration of the target robot in the current control cycle.

The quadratic programming problem for calculating the desired joint acceleration is expressed as follows:

Therefore, the present application can avoid the situation where the expected joint acceleration settlement result is too large when the target robot moves to the vicinity of the singular position by executing the above sub-steps a to c, thereby improving the safety and reliability of the global motion strategy solution process.

Step S270: Control the target robot to move according to the desired joint acceleration.

In this embodiment, after the robot control device 10 obtains the expected joint acceleration of the target robot in the current control cycle that matches the global motion strategy, it can calculate the expected joint state information of the target robot in the current control cycle (including the expected joint position and expected joint acceleration of the target robot in the current control cycle) based on the actual joint state information of the target robot in the current control cycle and the expected joint acceleration, and then control the actual joint state information of the target robot in the robot joint space according to the obtained expected joint state information, so as to ensure that the target robot can achieve a reactive obstacle avoidance effect for dynamic obstacles in the process of performing the expected operation, so as to ensure that the target robot can avoid dynamic obstacles with high dexterity and real-time performance while achieving the expected operation execution effect.

Therefore, the present application can execute the above-mentioned steps S220 to S270, and utilize the fast iterative solution characteristics corresponding to the RMP push operation and the RMP pullback operation to ensure that the target robot achieves a reactive obstacle avoidance effect for dynamic obstacles in the process of performing the desired operation, thereby improving the real-time obstacle avoidance of the robot, and introducing a geometric dynamic system involving speed information to perform local motion strategy calculations during the implementation of the RMP push operation, so as to utilize the speed continuity characteristics of the geometric dynamic system to improve the robot's obstacle avoidance dexterity, thereby ensuring that the target robot can achieve the desired operation execution effect while avoiding dynamic obstacles with high dexterity and high real-time performance.

In this application, in order to ensure that the robot control device 10 can effectively execute the above robot obstacle avoidance control method, this application adopts The above functions are realized by dividing the robot obstacle avoidance control device 100 stored in the robot control device 10 into functional modules. The specific components of the robot obstacle avoidance control device 100 provided by the present application and applied to the robot control device 10 are described below.

Please refer to FIG6 , which is a schematic diagram of the composition of the robot obstacle avoidance control device 100 provided in an embodiment of the present application. In the embodiment of the present application, the robot obstacle avoidance control device 100 may include an obstacle determination module 110, an RMP tree construction module 120, a dynamic system construction module 130, a motion parameter acquisition module 140, a local strategy solution module 150, a global strategy solution module 160 and an obstacle avoidance operation control module 170.

The obstacle determination module 110 is used to obtain the actual spatial positions of all obstacles currently existing in the operating environment of the target robot.

The RMP tree construction module 120 is used to construct a corresponding RMP motion strategy mapping tree for the target robot based on the actual spatial positions of all obstacles obtained, wherein the root node task of the RMP motion strategy mapping tree corresponds to the robot joint space, and the leaf node tasks of the RMP motion strategy mapping tree include the position motion task and posture motion task of the robot terminal to perform the desired operation, as well as the obstacle avoidance motion task of multiple robot key parts at the robot terminal to avoid each obstacle respectively.

The dynamic system construction module 130 is used to respectively construct a geometric dynamic system involving velocity information for a position motion task, a posture motion task and each obstacle avoidance motion task.

The motion parameter acquisition module 140 is used to obtain the actual joint state information of the target robot in the current control cycle, and the expected position information and expected posture information of the robot end of the target robot that match the expected task in the current control cycle.

The local strategy solving module 150 is used to transfer the actual joint state information, expected position information and expected posture information from the root node of the RMP motion strategy mapping tree to the geometric dynamic system of each leaf node task based on the RMP forward operation to solve the expected inertia matrix and the expected force, and obtain the local expected inertia matrix and local expected force corresponding to each leaf node task.

The global strategy solving module 160 is used to transfer the local expected inertia matrix and local expected force of each leaf node task to the root node to solve the joint acceleration based on the RMP pullback operation, so as to obtain the expected joint acceleration of the target robot in the current control cycle.

The obstacle avoidance control module 170 is used to control the target robot to move according to the desired joint acceleration.

It should be noted that the basic principle and technical effect of the robot obstacle avoidance control device 100 provided in the embodiment of the present application are the same as those of the aforementioned robot obstacle avoidance control method. For the sake of brief description, for the parts not mentioned in this embodiment, reference can be made to the description of the aforementioned robot obstacle avoidance control method.

In the embodiments provided in the present application, it should be understood that the disclosed devices and methods can also be implemented in other ways. The device embodiments described above are merely schematic, for example, the flowcharts and block diagrams in the accompanying drawings show the possible architecture, functions and operations of the devices, methods and computer program products according to the embodiments of the present application. In this regard, each box in the flowchart or block diagram can represent a module, a program segment or a part of a code, and a module, a program segment or a part of a code 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 box can also occur in a different order from the order marked in the accompanying drawings. For example, two consecutive boxes can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the block diagram and/or the flowchart, and the combination of boxes in the block diagram and/or the flowchart can be implemented with a dedicated hardware-based system that performs a specified function or action, or can be implemented with a combination of dedicated hardware and computer instructions.

In addition, the functional modules in each embodiment of the present application can be integrated together to form an independent part, or each module can exist separately, or two or more modules can be integrated to form an independent part. If the various functions provided by the present 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 the present application is essentially or the part that contributes to the prior art or the part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a readable storage medium, including several instructions for a computer device (which can be a personal computer, server, robot, or network device, etc.) to perform all or part of the steps of the method recorded in each embodiment of the present application. The aforementioned readable storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk and other media that can store program code.

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

Claims (10)

A robot obstacle avoidance control method, characterized in that the method comprises: Obtain the actual spatial positions of all obstacles currently existing in the operating environment of the target robot; According to the actual spatial positions of all obstacles obtained, a corresponding RMP motion strategy mapping tree is constructed for the target robot, wherein the root node task of the RMP motion strategy mapping tree corresponds to the robot joint space, and the leaf node tasks of the RMP motion strategy mapping tree include the position motion task and posture motion task of the robot end to perform the desired operation, and the obstacle avoidance motion task of multiple robot key parts at the robot end to avoid each obstacle respectively; Constructing a geometric dynamic system involving velocity information for the position motion task, the posture motion task and each obstacle avoidance motion task respectively; Acquire actual joint state information of the target robot in the current control cycle, and expected position information and expected posture information of the robot end of the target robot that match the expected task in the current control cycle; Based on the RMP forward operation, the actual joint state information, the expected position information and the expected posture information are transferred from the root node of the RMP motion strategy mapping tree to the geometric dynamic system of each leaf node task to solve the expected inertia matrix and the expected force, and the local expected inertia matrix and the local expected force corresponding to each leaf node task are obtained; Based on the RMP pullback operation, the local expected inertia matrix and the local expected force of each leaf node task are transferred to the root node to solve the joint acceleration, so as to obtain the expected joint acceleration of the target robot in the current control cycle; The target robot is controlled to move according to the expected joint acceleration. The method according to claim 1 is characterized in that the step of constructing a corresponding RMP motion strategy mapping tree for the target robot according to the actual spatial positions of all obstacles obtained comprises: Decomposing the global motion task of the target robot in the robot joint space to obtain the posture change task of the robot end of the target robot in the posture space, the position change task of the robot end in the position space, and the position change tasks of each robot key part of the target robot except the robot end in the corresponding position space; Decomposing the posture change task of the robot end in the posture space into the task space of the robot end, and obtaining the posture movement task of the robot end to perform the desired operation in the corresponding task space; According to the actual spatial positions of all obstacles, the position change task of the robot end in the position space is decomposed into the task space of the robot end, so as to obtain the position movement task of the robot end to perform the desired operation in the corresponding task space and the obstacle avoidance movement task of the robot end to avoid each obstacle in the corresponding task space; According to the actual spatial positions of all obstacles, the position change tasks of the key parts of the robot except the end of the robot in the corresponding position space are decomposed into the corresponding task space, so as to obtain the obstacle avoidance movement tasks of the key parts of the robot except the end of the robot to avoid each obstacle in the corresponding task space; According to the spatial transformation relationship between the robot joint space, posture space, position space and task space, a tree structure is constructed for the global motion task, the posture change task, all position change tasks, the posture motion task, the position motion task and all obstacle avoidance motion tasks to obtain the RMP motion strategy mapping tree. The method according to claim 1 is characterized in that the geometric dynamic systems of the position motion task, the posture motion task and each of the obstacle avoidance motion tasks all satisfy the following conditions:
in, Used to represent the corresponding geometric dynamic system and system state parameters The corresponding geometric metric matrix, Used to represent the corresponding geometric dynamic system and system state parameters The corresponding damping matrix, Φ(x), is used to represent the potential energy equation of the corresponding geometric dynamic system, Used to represent the corresponding geometric dynamic system and system state parameters The corresponding first curvature term matrix is, Used to represent the corresponding geometric dynamic system and system state parameters The corresponding second curvature term matrix is, Used to indicate The submatrix of the i-th column in , It is used to express the partial derivative of the state parameter x. Used to indicate the status parameters Find the partial derivative, It is used to indicate the gradient of the state parameter x, and m is used to indicate The total number of matrix columns, state parameters is the differential of the state parameter x, the state parameter is the state parameter The differential of In this process, the state parameters x and state parameters corresponding to the position motion task They are used to represent the position information and position change speed information of the robot end, and the state parameters x and state parameters corresponding to the posture motion task. They are used to represent the posture information and posture change speed information of the robot end, and the state parameters x and state parameters corresponding to the obstacle avoidance motion task. They are used to represent the relative distance information and relative motion speed information between the key parts of the robot and the corresponding obstacles respectively. The method according to claim 3 is characterized in that the calculation formula of the local expected inertia matrix and the local expected force corresponding to each geometric dynamic system is expressed by the following formula:
in, Used to represent the corresponding geometric dynamic system and system state parameters The corresponding local expected inertia matrix, Used to represent the corresponding geometric dynamic system and system state parameters The corresponding local expected force. The method according to any one of claims 1 to 4 is characterized in that the step of transferring the expected inertia matrix and expected force of each leaf node task to the root node to solve the joint acceleration based on the RMP pullback operation to obtain the expected joint acceleration of the target robot in the current control cycle includes: According to the spatial transformation relationship between the root node and each leaf node in the RMP motion strategy mapping tree, the local expected inertia matrix and the local expected force of each leaf node task are transferred to the root node based on the RMP pullback operation, and the root node is obtained at the current The global expected inertia matrix and global expected forces within the control period; The expected joint acceleration of the target robot in the current control cycle is calculated according to the global expected inertia matrix and the global expected force. The method according to claim 5, characterized in that the step of calculating the expected joint acceleration of the target robot in the current control cycle according to the global expected inertia matrix and the global expected force comprises: According to the joint limit motion position interval and the joint acceleration control interval of the target robot, constructing a target acceleration constraint condition for the joint acceleration of the target robot; According to the global expected inertia matrix and the global expected force, constructing a corresponding acceleration error function for the joint acceleration of the target robot; With the purpose of minimizing the actual output size of the acceleration error function, a quadratic programming problem is solved for the joint acceleration based on the target acceleration constraint to obtain the expected joint acceleration of the target robot in the current control cycle. The method according to claim 6, characterized in that the target acceleration constraint condition is expressed by the following formula:
Wherein, q ^* is used to represent the actual joint position included in the actual joint state information, is used to represent the actual joint speed included in the actual joint state information, Δt is used to represent the cycle length of a single control cycle, _qu is used to represent the upper limit value of the joint position included in the joint limit motion position interval, _ql is used to represent the lower limit value of the joint position included in the joint limit motion position interval, It is used to indicate the upper limit value of the joint acceleration included in the joint acceleration control interval, It is used to indicate the lower limit value of the joint acceleration included in the joint acceleration control interval, Used to represent the joint acceleration of the target robot; The acceleration error function is expressed as follows:
Wherein, _Mr is used to represent the global expected inertia matrix, and _fr is used to represent the global expected force; At this time, the quadratic programming problem expression for calculating the desired joint acceleration is expressed as follows:

A robot obstacle avoidance control device, characterized in that the device comprises: The obstacle determination module is used to obtain the actual spatial positions of all obstacles currently existing in the operating environment of the target robot; An RMP tree construction module is used to construct a corresponding RMP motion strategy mapping tree for the target robot according to the actual spatial positions of all obstacles obtained, wherein the root node task of the RMP motion strategy mapping tree corresponds to the robot joint space, and the leaf node tasks of the RMP motion strategy mapping tree include the position motion task and posture motion task of the robot end to perform the desired operation, and the obstacle avoidance motion task of multiple robot key parts at the robot end to avoid each obstacle respectively; A dynamic system construction module, used to respectively construct a geometric dynamic system involving velocity information for the position motion task, the posture motion task and each of the obstacle avoidance motion tasks; A motion parameter acquisition module, used to acquire actual joint state information of the target robot in the current control cycle, and expected position information and expected posture information of the robot end of the target robot that match the expected task in the current control cycle; A local strategy solving module is used to transfer the actual joint state information, the expected position information and the expected posture information from the root node of the RMP motion strategy mapping tree to the geometric dynamic system of each leaf node task to solve the expected inertia matrix and the expected force based on the RMP forward operation, so as to obtain the local expected inertia matrix and the local expected force corresponding to each leaf node task; A global strategy solving module is used to transfer the local expected inertia matrix and the local expected force of each leaf node task to the root node to solve the joint acceleration based on the RMP pullback operation, so as to obtain the expected joint acceleration of the target robot in the current control cycle; The obstacle avoidance operation control module is used to control the target robot to move according to the expected joint acceleration. A robot control device, characterized in that it includes a processor and a memory, the memory stores a computer program executable by the processor, and the processor can execute the computer program to implement the robot obstacle avoidance control method described in any one of claims 1-7. A readable storage medium having a computer program stored thereon, characterized in that when the computer program is executed, the robot obstacle avoidance control method described in any one of claims 1 to 7 is implemented.