Heterogeneous robot motion planning method

Abstract

The invention discloses a motion planning method for a heterogeneous robot, and relates to the technical field of robots. The method comprises the following steps: step 1, subspace definition and differential homeomorphic mapping establishment; step 2, dynamic situation field construction; step 3, geometric motion generation; and 4, controlling system energy. According to the method, universal adaptation of robot systems with different configurations can be realized, the algorithm reuse rate is greatly improved, the development cost is remarkably reduced, efficient obstacle avoidance can be realized in a dynamic environment, and the method has excellent real-time performance and wide applicability and robustness.

Description

Technical Field

[0001] The present invention relates to the field of robotics technology, and in particular to a heterogeneous robot motion planning method. Background Art

[0002] In recent years, with the rapid development of robotics, heterogeneous robotic systems (such as robotic arms with varying degrees of freedom, dual-arm collaborative robots, and mobile robotic arms) have been widely used in fields such as industrial automation, medical surgery, and service robotics. However, due to the diverse configuration spaces of heterogeneous robots, traditional motion planning methods are often difficult to directly apply to robotic systems of varying configurations. This is particularly true when it comes to dynamic obstacle avoidance and real-time reactive control.

[0003] Currently, mainstream robotic motion planning methods primarily optimize trajectories in configuration space or task space. For example, sampling-based planning algorithms (such as RRT* and PRM) and optimization methods (such as QP optimization and nonlinear programming) perform well in static environments but struggle to meet real-time requirements in dynamic environments. Furthermore, these methods often rely on specific robot configurations, making them difficult to generalize to robotic systems with varying degrees of freedom.

[0004] In recent years, the application of Riemannian geometry to robotic motion planning has gained increasing attention. By constructing Riemannian subspaces mapped to the configuration space, it is possible to geometrically unify the motion representations of different robots. For example, studies have used Riemannian metrics to optimize the motion trajectory of robotic arms or employed potential field methods for obstacle avoidance. However, existing methods are mostly limited to robots of specific configurations and lack a general, reactive motion planning framework that is scalable to heterogeneous robotic systems. Furthermore, traditional potential field methods are prone to falling into local minima and lack adaptability in complex dynamic environments.

[0005] Currently, existing technologies lack a universal motion planning method that can uniformly handle robots of different configurations and achieve real-time dynamic obstacle avoidance. Therefore, there is an urgent need to develop a robot motion planning method that can adapt to the configurational characteristics of robots with different degrees of freedom and achieve efficient and robust reactive obstacle avoidance through geometric optimization. Summary of the Invention

[0006] In view of the above-mentioned defects of the prior art, the technical problem to be solved by the present invention is how to improve the adaptability of the existing robot motion planning method to heterogeneous robots and realize dynamic obstacle avoidance and real-time reactive control.

[0007] To achieve the above object, the present invention provides a heterogeneous robot motion planning method, comprising the following steps:

[0008] Step 1: Subspace definition and differential homeomorphism mapping establishment;

[0009] Step 2: Dynamic potential field construction;

[0010] Step 3: Geometric motion generation;

[0011] Step 4: System energy control.

[0012] Furthermore, step one is specifically as follows:

[0013] According to the specific task requirements, define a set of N Riemann subspaces with clear physical meanings, and design the corresponding Riemann metric M for each subspace motion. i , construct a smooth mapping function for each defined subspace:

[0014] φ i :Q→S i ,

[0015] s i =φ i (q),

[0016]

[0017] Where N is the number of Riemann subspaces, M i is the Riemann metric, that is, the weight, Q is the configuration space, S i is a Riemannian manifold, q is a generalized variable in the configuration space, s i is the generalized variable of the i-th Riemann subspace, is the generalized velocity corresponding to the generalized variable, is the configuration velocity, J is the corresponding Jacobian matrix between the two spaces;

[0018] Map the configuration space Q to the corresponding Riemannian manifold S i .

[0019] Furthermore, the Riemann subspace includes a core task space describing the end effector's posture, a motion constraint space reflecting the joint coordination relationship, and an obstacle avoidance sensitive area for processing environmental interaction;

[0020] The core task space describing the end effector posture is the end operation subspace, the motion constraint space reflecting the joint coordination relationship is the joint restriction subspace and the joint coordination subspace, and the obstacle avoidance sensitive area for processing the environment interaction is the obstacle avoidance sensitive subspace;

[0021] The mapping needs to maintain the integrity of key motion features for reversibility to satisfy subsequent pullback operations while retaining adaptability to respond to real-time state changes.

[0022] Furthermore, the step 2 is specifically as follows:

[0023] Construct potential field functions in each subspace:

[0024] ψ i (s i ,t),

[0025] Among them, s i is the generalized variable of the i-th Riemann subspace, t is the time variable relative to the initial time;

[0026] The potential field function includes target attraction terms, obstacle repulsion terms and special constraint terms. The parameters of the potential field function are dynamically adjusted according to the real-time environment so that the planning results can adapt to the latest working conditions.

[0027] Furthermore, the subspace construction potential field functions are the terminal operation subspace potential field function, the joint restriction subspace potential field function, the joint coordination subspace potential field function, and the obstacle avoidance sensitive subspace potential field function.

[0028] Furthermore, the step three is specifically as follows:

[0029] Based on the gradient of the potential field function with respect to the subspace variable:

[0030]

[0031] Compute the optimal motion for each subspace:

[0032]

[0033] Among them, s i is the generalized variable of the ith Riemann subspace, t is the time, is the generalized velocity, h i is the generalized variable s i The corresponding acceleration, i.e. the corresponding control quantity, k is a hyperparameter used to adjust the intensity of the motion trend;

[0034] Define configuration space mapping and optimization for subsequent motion synthesis in multiple Riemann subspaces.

[0035] Furthermore, the configuration space mapping and optimization are specifically as follows:

[0036] Through the pullback operation, each subspace movement and the corresponding weight M i Pull back the configuration space:

[0037]

[0038] Among them, h i q is the motion formed after the motion control law of the i-th subspace is mapped to the configuration space, J i is the Jacobian matrix between the i-th subspace and the configuration space, is the derivative of the Jacobian matrix with respect to time, M i q is the weight matrix after mapping, is the configuration speed, the letter T in the upper right corner of the formula represents the transpose of the matrix, represents the generalized inverse matrix;

[0039] Solve a constrained least squares problem:

[0040]

[0041] in, is the motion control law in the final synthesized configuration space, N is the number of Riemann subspaces;

[0042] The final joint motion instruction is obtained, and the analytical solution of the minimization problem is:

[0043]

[0044] in, It is the motion formed in the configuration space after synthesizing all subspaces;

[0045] The optimization balances the contribution of each subspace for optimal overall performance.

[0046] Furthermore, the pullback operation is to convert the corresponding motion in the Riemann subspace and the Riemann metric M i Mapped into the configuration space.

[0047] Furthermore, the step 4 is specifically as follows:

[0048] The original continuous control law is discretized and becomes:

[0049]

[0050] Where k is the discrete time, is the motion control law in the final synthesized configuration space, is the spatial motion of the synthesized configuration, T represents the total control time,

[0051] And add energy control term to the control law:

[0052]

[0053] in,

[0054]

[0055] Where β is the energy control coefficient, dt represents the discrete time interval of the control law, is the configuration velocity at the kth moment, is the configuration velocity at the k+1th moment under the original control law. The letter T in the upper right corner of the formula represents the transpose of the matrix.

[0056] Furthermore, the above-mentioned heterogeneous robot motion planning method is applied to the motion control of special equipment, including heterogeneous robot collaborative operation systems in smart factories, complex environment safe navigation of service robots, space robotic arms, and underwater robots. The heterogeneous robots are robotic arms with different degrees of freedom, dual-arm collaborative robots, and mobile robotic arms.

[0057] Compared with the existing technology, the present invention has the following advantages: it adopts a multi-Riemann subspace mapping framework, can achieve universal adaptation to robot systems of different configurations, greatly improves the algorithm reuse rate, significantly reduces development costs, and can also achieve efficient obstacle avoidance in dynamic environments. It has excellent real-time performance, wide applicability and robustness.

[0058] The concept, specific structure and technical effects of the present invention will be further described below in conjunction with the accompanying drawings to fully understand the purpose, characteristics and effects of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] Figure 1 The figure is a schematic diagram of the robot motion planning process according to a preferred embodiment of the present invention. DETAILED DESCRIPTION

[0060] The following describes preferred embodiments of the present invention with reference to the accompanying drawings to make its technical content clearer and easier to understand. The present invention can be embodied in many different forms of embodiments, and the scope of protection of the present invention is not limited to the embodiments mentioned herein.

[0061] In the drawings, components with identical structures are denoted by the same reference numerals, and components with similar structures or functions are denoted by similar reference numerals. The size and thickness of each component shown in the drawings are arbitrary and are not limited by the present invention. For clarity, the thickness of components in some places in the drawings is appropriately exaggerated.

[0062] like Figure 1 As shown in FIG, the present invention discloses a discrete motion planning method for heterogeneous robots. It constructs multiple Riemann subspaces mapped to the configuration space, uses potential field gradients to generate geometric motion, and then maps it back to the configuration space through a pullback operation. Finally, it combines a weighted optimization method to achieve real-time motion planning. It includes the following steps:

[0063] Step 1: Subspace definition and differential homeomorphism mapping establishment

[0064] First, according to the specific task requirements, a set of N Riemann subspaces with clear physical meanings are defined, including the core task space that describes the end effector posture, the motion constraint space that reflects the joint coordination relationship, and the obstacle avoidance sensitive area that handles the interaction with the environment. At the same time, a corresponding Riemann metric M is designed for each subspace motion. i (i.e. weights), construct a smooth mapping function for each defined subspace:

[0065] φ i :Q→S i ,

[0066] s i =φ i (q),

[0067]

[0068] Among them, Q is the configuration space, S i is a Riemannian manifold, q is a generalized variable in the configuration space, s i is the generalized variable of the i-th Riemann subspace, is the corresponding generalized velocity, is the configuration velocity, and J is the corresponding Jacobian matrix between the two spaces.

[0069] Then, map the configuration space Q to the corresponding Riemann manifold S i These mappings need to maintain the integrity of key motion features and ensure reversibility to meet the needs of subsequent pullback operations (the pullback operation is to convert the corresponding motion in the Riemann subspace to the Riemann metric M i into configuration space) while retaining adaptability to respond to real-time state changes.

[0070] Step 2: Dynamic potential field construction

[0071] Construct potential field functions in each subspace:

[0072] ψ i (s i ,t),

[0073] Where t is a time variable relative to the initial time, and includes components such as target attraction, obstacle repulsion, and special constraints. Potential field parameters are dynamically adjusted based on the real-time environment, ensuring that the planning results always adapt to the latest working conditions.

[0074] Step 3: Geometric motion generation

[0075] Based on the gradient of the time-varying potential field with respect to the subspace variables:

[0076]

[0077] Compute the optimal motion for each subspace:

[0078]

[0079] Among them, h i is the generalized variable s i The corresponding acceleration is also the corresponding control quantity, and k is a hyperparameter used to adjust the trend intensity of the movement.

[0080] This step fully exploits the properties of Riemannian geometry to ensure that the generated motion fully complies with the geometric constraints of the subspace.

[0081] In order to synthesize motion in multiple Riemann subspaces, it is necessary to further define the configuration space mapping and optimization, as follows:

[0082] First, the Riemann subspace motion and the corresponding weight M are converted through the pullback operation. i Pull back the configuration space:

[0083]

[0084] Among them, h i q is the motion formed after the motion control law of the i-th subspace is mapped to the configuration space, J i is the Jacobian matrix between the i-th subspace and the configuration space, is the derivative of the Jacobian matrix with respect to time, M i q is the weight matrix after mapping. The letter T in the upper right corner of the formula represents the transpose of the matrix. represents the generalized inverse matrix.

[0085] Then, solve the constrained least squares problem:

[0086]

[0087] in, is the motion control law in the final synthesized configuration space.

[0088] Then, the final joint motion instructions are obtained. The minimization problem has an analytical solution:

[0089]

[0090] in, It is the motion formed in the configuration space after synthesizing all subspaces.

[0091] The optimization process balances the contributions of each subspace to ensure optimal overall performance.

[0092] Step 4: System Energy Control

[0093] During the planning process, the target position in the configuration space is located at the global minimum of the system's energy. Therefore, to ensure that the system ultimately converges to the desired target position within the configuration space, the system's energy needs to be controlled to ensure that energy is conserved during motion in free space. As the system approaches the target position, its energy begins to decay, ensuring convergence to the minimum.

[0094] To this end, it is necessary to first discretize the original continuous control law into:

[0095]

[0096] Where k is the discrete time and T represents the total duration of control.

[0097] And add energy control term to the control law:

[0098]

[0099] in,

[0100]

[0101] Where β is the energy control coefficient, dt represents the discrete time interval of the control law, is the configuration velocity at the kth moment, is the configuration speed at the k+1th moment under the original control law.

[0102] Based on the above method steps, the specific implementation process of this method is explained below using a 7-DOF collaborative robot arm as an example.

[0103] like Figure 1 As shown in the figure, firstly, the mapping relationship from the robot configuration space to the Riemann subspace is established, and then the optimization motion is generated through the potential field gradient.

[0104] Step 1: Riemann subspace configuration stage

[0105] According to the characteristics of the 7-DOF robotic arm, three Riemann subspaces with clear physical meanings are configured.

[0106] 1. Terminal operation subspace

[0107] Use displacement vectors and quaternions to represent the pose of the end effector and define the mapping:

[0108]

[0109] in, is the forward kinematic mapping function from the joint angle of the manipulator to the end pose, x *is the desired pose of the end effector. The Riemann metric M1 of this subspace is dynamically adjusted according to the end pose so that it is close to the target pose x * Automatically enhance the pose attraction weight.

[0110] 2. Joint restricted subspace

[0111] Establish the relative error space between the joint states and the dynamic limits of the manipulator:

[0112] s2=φ2(q)=[qq min ,q max -q],

[0113] Among them, q min and q max is the physical limit of the joint angle, and its metric matrix M2 is the inverse of the relative error, which adjusts the behavior of the robot when it approaches the joint limit.

[0114] 3. Obstacle Avoidance Sensitive Subspace

[0115] The obstacle avoidance characteristics are characterized by the shortest Euclidean distance between the seven links of the manipulator and the obstacle in the task space:

[0116]

[0117] in, is the mapping function from the manipulator joint angles to the task space pose of each link, and Γ(x) is the query function for the closest distance from a point x in the task space to an obstacle. The diagonal elements of its metric matrix are inversely proportional to the distance from the corresponding point to the obstacle, enabling automatic weighted processing of obstacle information.

[0118] 4. Joint Coordination Subspace

[0119] Condition number through the robot:

[0120] s4=φ4(q)=cond(J(q)),

[0121] Where cond(J) represents the condition number of the Jacobian matrix J. The coordination of the joints is described, and on this basis, the metric matrix is ​​designed so that the motion of each joint naturally conforms to the dynamic characteristics of the robot arm.

[0122] Step 2: Dynamic potential field construction stage

[0123] Construct potential field functions with clear physical meanings in each subspace.

[0124] 1. Terminal operation subspace

[0125] Design target attractive potential field:

[0126]

[0127] Among them, ω1 is the weight parameter that adjusts the gradient change trend of the potential field near the extreme point.

[0128] This gives the geometric motion:

[0129]

[0130] The Riemann metric M1 is adjusted according to the current position:

[0131] M1=0.5(ω2-ω3)tanh(-ω4(‖s1‖-ω5)),

[0132] Among them, ω2, ω3, ω4, and ω5 are used to adjust the changing relationship between the potential field weight and the current posture.

[0133] When approaching the target position, the weight of the position component is automatically increased to ensure stability.

[0134] 2. Joint restricted subspace

[0135] Design joint extremum repulsive potential field:

[0136]

[0137] Among them, ω6 is a hyperparameter for adjusting the size of the repulsive potential field, 1 represents the indicator function, and the value is 1 when the condition in the brackets is true, and the value is 0 when the condition is false.

[0138] At the same time, the corresponding metric is:

[0139]

[0140] 3. Obstacle Avoidance Sensitive Subspace

[0141] The repulsive potential field and metric are similar to those in the joint restricted subspace, but modified in terms of geometric motion generation:

[0142]

[0143] This design can generate a huge repulsive force when approaching an obstacle, ensuring the impenetrability between the robot and the obstacle.

[0144] 4. Joint Coordination Subspace

[0145] Design attractive potential field:

[0146]

[0147] Among them, ω7 is a hyperparameter that adjusts the size of the attractive potential field.

[0148] At the same time, the corresponding Riemann metric is:

[0149]

[0150] By monitoring the Jacobian matrix condition number, singular configuration regions can be avoided in advance during the trajectory planning stage.

[0151] Step 3: Motion Generation and Optimization

[0152] First, the potential field gradient is calculated in parallel in each subspace:

[0153]

[0154] The design and implementation of the Riemann metric is based on the autonomous adjustment of the current joint position and joint velocity. When a subspace undergoes drastic changes (such as the sudden appearance of an obstacle), its weight in the subsequent motion synthesis stage will automatically increase.

[0155] Through the pullback operation, the subspace motion and metric M i Map back to the configuration space, synthesize the planning results and perform energy control.

[0156] The present invention can achieve universal motion planning for robots with different configurations, such as 6-DOF, 7-DOF, and dual-arm robots, significantly reducing the cost of algorithm transplantation and system development; improve the real-time performance and reliability of dynamic obstacle avoidance, enabling robots to quickly generate smooth and safe motion trajectories in complex environments; solve the problem of motion instability caused by singular configurations in traditional planning, and ensure the robot's motion performance within the entire workspace; break through the bottleneck of high-dimensional space calculation, and enable complex robot systems (such as dual-arm robots with 12+ DOF) to achieve real-time motion planning. In addition, the present invention has excellent real-time performance, with a dynamic obstacle avoidance response time of less than 10ms, far exceeding the requirements of the ISO / TS15066 standard, fully meeting the stringent requirements of modern industry for high-speed and high-precision operations.

[0157] The present invention has unique advantages in multiple high-value application scenarios, and is particularly suitable for heterogeneous robot collaborative operation systems in smart factories, safe navigation of service robots in complex environments, and motion control of special equipment such as space manipulators and underwater robots. These fields have an urgent need for reliable and efficient motion planning technology.

[0158] The preferred embodiments of the present invention have been described in detail above. It should be understood that numerous modifications and variations based on the concepts of the present invention can be made by one of ordinary skill in the art without inventive effort. Therefore, any technical solution that can be derived by one of ordinary skill in the art through logical analysis, reasoning, or limited experimentation based on the concepts of the present invention and the prior art should be within the scope of protection defined by the claims.

Claims

1. A heterogeneous robot motion planning method, characterized in that: The steps include: Step 1: Subspace definition and differential homeomorphism mapping establishment; Step 2: Dynamic potential field construction; Step 3: Geometric motion generation; Step 4: System energy control.

2. The heterogeneous robot motion planning method according to claim 1, characterized in that: Step 1 is as follows: According to the specific task requirements, define a set of N Riemann subspaces with clear physical meanings, and design the corresponding Riemann metric M for each subspace motion. i , construct a smooth mapping function for each defined subspace: f i :Q→S i , s i =φ i (q), Where N is the number of Riemann subspaces, M i is the Riemann metric, that is, the weight, Q is the configuration space, S i is a Riemannian manifold, q is a generalized variable in the configuration space, s i is the generalized variable of the i-th Riemann subspace, is the generalized velocity corresponding to the generalized variable, is the configuration velocity, J is the corresponding Jacobian matrix between the two spaces; Map the configuration space Q to the corresponding Riemannian manifold S i .

3. The heterogeneous robot motion planning method according to claim 2, characterized in that: The Riemann subspace includes a core task space that describes the end effector's posture, a motion constraint space that reflects the coordination relationship of joints, and an obstacle avoidance sensitive area that handles environmental interactions; The core task space describing the end effector posture is the end operation subspace, the motion constraint space reflecting the joint coordination relationship is the joint restriction subspace and the joint coordination subspace, and the obstacle avoidance sensitive area for processing the environment interaction is the obstacle avoidance sensitive subspace; The mapping needs to maintain the integrity of key motion features for reversibility to satisfy subsequent pullback operations while retaining adaptability to respond to real-time state changes.

4. The heterogeneous robot motion planning method according to claim 1, wherein: The step 2 is specifically as follows: Construct potential field functions in each subspace: ψ i (s i ,t), Among them, s i is the generalized variable of the i-th Riemann subspace, t is the time variable relative to the initial time; The potential field function includes target attraction terms, obstacle repulsion terms and special constraint terms. The parameters of the potential field function are dynamically adjusted according to the real-time environment so that the planning results can adapt to the latest working conditions.

5. The heterogeneous robot motion planning method according to claim 4, characterized in that: The subspace construction potential field functions are the terminal operation subspace potential field function, the joint restriction subspace potential field function, the joint coordination subspace potential field function, and the obstacle avoidance sensitive subspace potential field function.

6. The heterogeneous robot motion planning method according to claim 1, wherein: The step three is specifically as follows: Based on the gradient of the potential field function with respect to the subspace variable: Compute the optimal motion for each subspace: Among them, s i is the generalized variable of the ith Riemann subspace, t is the time, is the generalized velocity, h i is the generalized variable s i The corresponding acceleration, i.e. the corresponding control quantity, k is a hyperparameter used to adjust the intensity of the motion trend; Define configuration space mapping and optimization for subsequent motion synthesis in multiple Riemann subspaces.

7. The heterogeneous robot motion planning method according to claim 6, characterized in that: The definition of configuration space mapping and optimization is specifically as follows: Through the pullback operation, each subspace movement and the corresponding weight M i Pull back the configuration space: Among them, h i q is the motion formed after the motion control law of the i-th subspace is mapped to the configuration space, J i is the Jacobian matrix between the i-th subspace and the configuration space, is the derivative of the Jacobian matrix with respect to time, M i q is the weight matrix after mapping, is the configuration speed, the letter T in the upper right corner of the formula represents the transpose of the matrix, represents the generalized inverse matrix; Solve a constrained least squares problem: in, is the motion control law in the final synthesized configuration space, N is the number of Riemann subspaces; The final joint motion instruction is obtained, and the analytical solution of the minimization problem is: in, It is the motion formed in the configuration space after synthesizing all subspaces; The optimization balances the contribution of each subspace for optimal overall performance.

8. The heterogeneous robot motion planning method according to claim 7, characterized in that: The pullback operation is to convert the corresponding motion in the Riemann subspace and the Riemann metric M i Mapped into the configuration space.

9. The heterogeneous robot motion planning method according to claim 1, wherein: The step 4 is specifically as follows: The original continuous control law is discretized and becomes: Where k is the discrete time, is the motion control law in the final synthesized configuration space, is the spatial motion of the synthesized configuration, T represents the total control time, And add energy control term to the control law: in, Where β is the energy control coefficient, dt represents the discrete time interval of the control law, is the configuration velocity at the kth moment, is the configuration velocity at the k+1th moment under the original control law. The letter T in the upper right corner of the formula represents the transpose of the matrix.

10. The heterogeneous robot motion planning method according to any one of claims 1 to 9, characterized in that: Applied to motion control of special equipment, including heterogeneous robot collaborative operation systems in smart factories, safe navigation of service robots in complex environments, space manipulators, and underwater robots. The heterogeneous robots are manipulators with different degrees of freedom, dual-arm collaborative robots, and mobile manipulators.

Images