Mechanical arm obstacle avoidance path planning based on improved bidirectional RRT algorithm

Abstract

The invention relates to the technical field of robots, and discloses a mechanical arm obstacle avoidance path planning based on an improved bidirectional RRT algorithm, which comprises the following steps: defining a mechanical arm and a working space: determining a kinetic model and kinematics constraint of the mechanical arm and obstacle distribution in the working space, by reasonably setting a starting point, a target point and algorithm parameters, it can be ensured that the algorithm has high exploration efficiency and convergence speed in the initial stage, a heuristic search strategy, such as a heuristic function in an A * algorithm, is introduced on a node expansion strategy, nodes can be guided to expand in a more desirable direction, and therefore the path planning quality is improved, and the path planning efficiency is improved. The position and the motion state of an obstacle are detected in real time, the node expansion direction is dynamically adjusted according to detected information, the mechanical arm can be effectively prevented from colliding with the obstacle, and the function is achieved generally through sensor data such as a laser radar and a camera.

Description

Technical Field

[0001] The present invention relates to the field of robotics technology, and in particular to a path planning for obstacle avoidance of a robotic arm based on an improved bidirectional RRT algorithm. Background Art

[0002] With the rapid development of robotics technology, robotic arms have been widely used in industrial automation, medical surgery, service robots, etc. However, when performing tasks, robotic arms need to face complex and changeable obstacle environments. How to quickly and accurately plan a collision-free path has become a key factor restricting the efficiency and safety of robotic arm applications.

[0003] Although the traditional bidirectional RRT algorithm can solve the path planning problem of the robot to a certain extent, there is still room for improvement in path quality and search efficiency. Therefore, an improved bidirectional RRT algorithm is urgently needed to improve the efficiency and quality of the robot's obstacle avoidance path planning, which has important practical significance and application value. Summary of the invention

[0004] The object of the present invention is to provide a path planning for obstacle avoidance of a robotic arm based on an improved bidirectional RRT algorithm to solve the problems raised in the above background technology.

[0005] In order to solve the above technical problems, the present invention provides the following technical solutions: a path planning for obstacle avoidance of a manipulator based on an improved bidirectional RRT algorithm, comprising the following steps:

[0006] Define the robot and workspace: determine the robot's dynamic model, kinematic constraints, and obstacle distribution in the workspace;

[0007] Bidirectional RRT algorithm initialization: Initialize two RRT trees from the starting point and the target point respectively, and set the algorithm parameters;

[0008] Heuristic node expansion: In the first tree, a heuristic search strategy is used to select an expansion node to generate a new node; in the second tree, the new node is used as a virtual target point for expansion to generate a second new node;

[0009] Dynamic obstacle detection and avoidance: During the node expansion process of the heuristic node expansion step, obstacles are detected in real time, and the node expansion direction is dynamically adjusted according to the obstacle position to avoid collision with obstacles;

[0010] Optimal parent node selection and path connection: In the process of bidirectional expansion, the optimal parent node selection strategy is introduced to select the parent node with the smallest path cost for connection to form a preliminary path;

[0011] Path smoothing: Smoothing the generated preliminary path using a suitable smoothing algorithm, such as cubic Hermite curves or cubic B-spline curves, to meet the actual kinematic constraints and fluency requirements;

[0012] Path verification and optimization: Verify whether the path meets the obstacle avoidance requirements, dynamic model and kinematic constraints. If not, optimize the path;

[0013] Output the final path: If the path meets all requirements, output the final path.

[0014] Preferably, in the step of defining the robotic arm and the workspace, the physical characteristics and motion laws of the robotic arm are first determined according to the actual application scenario and working environment of the robotic arm, and then the positions and shapes of all static and dynamic obstacles are identified in the workspace.

[0015] Preferably, in the bidirectional RRT algorithm initialization step, two RRT trees are initialized from the starting point and the target point respectively, one tree is expanded from the starting point, and the other tree is expanded from the target point, and parameters such as step size, maximum number of iterations, collision detection accuracy, etc. are set.

[0016] Preferably, in the heuristic node expansion step, the first tree adopts a heuristic search strategy (such as the heuristic function in the A* algorithm) to select an expansion node and generate a new node, and the second tree uses the newly generated node in the first tree as a virtual target point, expands in the second tree, and generates a second new node.

[0017] Preferably, in the dynamic obstacle detection and avoidance step, the position and motion state of the obstacle are detected in real time by using sensor data during the node expansion process of the heuristic node expansion step, and the direction of node expansion is dynamically adjusted according to the detected obstacle information to avoid collision with the obstacle.

[0018] Preferably, in the optimal parent node selection and path connection step, by introducing the optimal parent node selection strategy in the node expansion process of the heuristic node expansion step, the parent node with the smallest path cost is selected for connection according to the path cost (such as path length, energy consumption, etc.) to form a preliminary path, and then the nearest nodes in the two trees are connected to form a preliminary path from the starting point to the target point. At the same time, the RRT* optimization mechanism can be introduced to further improve the optimality of the path by iteratively updating the node positions in the path.

[0019] Preferably, in the path smoothing step, the preliminary path generated in the optimal parent node selection and path connection step is smoothed, and a cubic Hermite curve, a cubic B-spline curve or other algorithms are used to ensure that the smoothed path meets the kinematic limitations of the robot arm and ensure the feasibility and smoothness of the path.

[0020] Preferably, in the path verification and optimization step, it is verified whether the path meets the obstacle avoidance requirements, dynamic model and kinematic constraints. If the path does not meet the requirements, the heuristic node expansion step is returned to replan the path until a path that meets the requirements is found.

[0021] Preferably, in the path verification and optimization step, it is verified whether the path meets the obstacle avoidance requirements, dynamic model and kinematic constraints. If the path does not meet the requirements, the heuristic node expansion step is returned to replan the path until a path that meets the requirements is found.

[0022] Compared with the prior art, the beneficial effects achieved by the present invention are:

[0023] First, in the algorithm initialization step, the present invention can ensure that the algorithm has high exploration efficiency and convergence speed in the initial stage by reasonably setting the starting point, target point and algorithm parameters. In terms of node expansion strategy, the introduction of heuristic search strategy, such as the heuristic function in the A* algorithm, can guide the node to expand in a more promising direction, thereby improving the quality of path planning. By real-time detection of the position and motion state of obstacles and dynamically adjusting the direction of node expansion according to the detected information, the collision between the robot arm and the obstacle can be effectively avoided. The realization of this function usually requires the help of sensor data, such as lidar, camera, etc.

[0024] Second, in the optimal parent node selection and path connection steps of the present invention, by introducing the optimal parent node selection strategy, the parent node with the smallest path cost (such as path length, energy consumption, etc.) can be selected for connection to form a preliminary path. In addition, the RRT* optimization mechanism can be introduced to iteratively update the node positions in the path to further improve the optimality of the path. By adopting a suitable smoothing algorithm, such as a cubic Hermite curve, a cubic B-spline curve, etc., the generated preliminary path can be smoothed to ensure that the smoothed path meets the kinematic limitations of the robot arm and ensure the feasibility and fluency of the path.

[0025] Third, in the path verification and optimization step of the present invention, the generated path needs to be strictly verified to ensure that it meets the obstacle avoidance requirements, dynamic model and kinematic constraints. If the path does not meet the requirements, it is necessary to return to the previous steps to replan the path until a path that meets the requirements is found. BRIEF DESCRIPTION OF THE DRAWINGS

[0026] Figure 1 It is the overall flow chart of the present invention;

[0027] Figure 2 A flow chart of the steps of defining the robot arm and the workspace for the present invention;

[0028] Figure 3 It is a flow chart of the initialization steps of the bidirectional RRT algorithm of the present invention;

[0029] Figure 4 It is a flowchart of the heuristic node expansion steps of the present invention;

[0030] Figure 5 This is a flow chart of the dynamic obstacle detection and obstacle avoidance steps of the present invention;

[0031] Figure 6 A flow chart of the optimal parent node selection and path connection steps of the present invention;

[0032] Figure 7 A flow chart of the optimal parent node selection and path connection steps of the present invention;

[0033] Figure 8 This is a flow chart of the path verification and optimization steps of the present invention;

[0034] Fig. 9 Output the final path step flow chart for the present invention. DETAILED DESCRIPTION

[0035] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0036] The present invention provides the following technical solutions:

[0037] See also Figure 1 , Figure 2 , Figure 3 , Figure 4 , Figure 5 , Figure 6 , Figure 7 , Figure 8 and Fig. 9 , a path planning for obstacle avoidance of a robotic arm based on an improved bidirectional RRT algorithm, comprising the following steps:

[0038] 1. Define the manipulator and workspace: determine the dynamic model of the manipulator, kinematic constraints, and obstacle distribution in the workspace;

[0039] In the step of defining the robot and workspace, first determine the physical characteristics and motion laws of the robot according to the actual application scenario and working environment of the robot, and then identify the positions and shapes of all static and dynamic obstacles in the workspace.

[0040] Through the above technical solution, first, according to the actual application scenarios and working environments of the robot arm, such as industrial production lines, medical operations, home services, etc., a three-dimensional model of the workspace is established using three-dimensional modeling software or point cloud data to facilitate path planning, determine the dynamic model and kinematic constraints of the robot arm, and the distribution of obstacles in the workspace. The dynamic model of the robot arm includes physical properties and motion laws, wherein the physical properties include the mass, inertia matrix, friction coefficient, etc. of the robot arm, the motion laws include the motion equations of each joint of the robot arm, including the relationship between joint angle, velocity and acceleration, and control inputs such as torque or current of the motor, the kinematic constraints include joint limits, speed limits and workspace limits of the robot arm, the obstacle distribution in the workspace includes static obstacles and dynamic obstacles, the workspace is represented by a three-dimensional space coordinate system, and the position and shape of static and dynamic obstacles are described by point clouds, polygons or other geometric shapes. This step is the basis of path planning, which provides the algorithm with necessary input information and constraints, ensuring that the planned path not only conforms to the physical properties of the robot arm, but also can be safely and effectively executed in a given environment.

[0041] 2. Bidirectional RRT algorithm initialization: Initialize two RRT trees from the starting point and the target point respectively, and set the algorithm parameters;

[0042] In the initialization step of the bidirectional RRT algorithm, two RRT trees are initialized from the starting point and the target point respectively, one tree is expanded from the starting point and the other is expanded from the target point, and parameters such as step size, maximum number of iterations, and collision detection accuracy are set.

[0043] Through the above technical solution, the initialization phase sets the basic framework and parameters for the bidirectional RRT algorithm, including the algorithm's operating environment, the definition of the search space, the representation of nodes, etc., ensuring that the algorithm can run correctly and efficiently in subsequent steps. In the bidirectional RRT algorithm, two random expansion trees need to be constructed. The two RRT trees represent the search process from the starting point to the target point respectively. Each tree consists of nodes and edges. The nodes represent the possible positions that the robot arm may reach, and the edges represent the possible paths for the robot arm to move from one position to another. The initialization phase sets the starting point and the target point, and creates two empty trees with them as the root nodes, which provides a basis for subsequent node expansion and path connection. A series of algorithm parameters also need to be set in the initialization phase, such as step size, maximum number of iterations, and collision detection accuracy. Etc. These parameters have an important impact on the performance and results of the algorithm. By setting these parameters reasonably, the algorithm's search speed and path quality can be balanced to ensure that the algorithm finds a path that meets the requirements within a given time. The initialization stage also needs to prepare the data and environment required for the algorithm to run, including the location, shape and attribute information of obstacles, as well as the physical characteristics and kinematic constraints of the robot arm. These data and environmental information provide the necessary support for the algorithm to perform collision detection, path planning and obstacle avoidance strategy implementation in subsequent steps. After the initialization stage, the bidirectional RRT algorithm already has the basic conditions and parameter settings for operation. At this time, the algorithm can begin to enter the search stage, and gradually construct a feasible path from the starting point to the target point through steps such as random sampling, node expansion, collision detection and path connection.

[0044] 3. Heuristic node expansion: In the first tree, a heuristic search strategy is used to select an expansion node to generate a new node; in the second tree, the new node is used as a virtual target point for expansion to generate a second new node;

[0045] In the heuristic node expansion step, the first tree uses a heuristic search strategy (such as the heuristic function in the A* algorithm) to select the expanded node and generate a new node. The second tree uses the newly generated node in the first tree as the virtual target point, expands in the second tree, and generates a second new node.

[0046] Through the above technical scheme, the heuristic search strategy can improve the search efficiency. By setting the virtual target point, the two-way expansion of the two trees can be realized. The heuristic search strategy includes heuristic functions based on the target point distance, heuristic functions based on the obstacle density, etc. The heuristic function (such as the heuristic function in the A* algorithm) is used to select the expansion node. Common heuristic functions include Euclidean distance, Manhattan distance, etc. A nearest node is selected according to the heuristic function, a new node is generated and added to the tree, and the newly generated node in the first tree is used as the virtual target point. A point is randomly selected in the workspace as a candidate node, and the node closest to the candidate node is found in the tree. A certain step length is extended from the nearest node to the virtual target point to generate a new node. It is detected whether the new node collides with the obstacle. If it does not collide, it is added to the tree. The heuristic node expansion guides the search process by providing heuristic information in the search algorithm, thereby reducing the blindness of the search, improving efficiency, optimizing the search path and adapting to the dynamic environment.

[0047] 4. Dynamic obstacle detection and avoidance: During the node expansion process of the heuristic node expansion step, obstacles are detected in real time, and the node expansion direction is dynamically adjusted according to the obstacle position to avoid collision with obstacles;

[0048] In the dynamic obstacle detection and avoidance step, the position and motion state of obstacles are detected in real time by using sensor data during the node expansion process of the heuristic node expansion step. According to the detected obstacle information, the direction of node expansion is dynamically adjusted to avoid collision with obstacles. When a new obstacle is detected, the path is locally replanned to ensure the safety of the path.

[0049] Through the above technical solution, dynamic obstacle detection uses lidar, cameras, ultrasonic sensors, etc. to detect the position and motion status of obstacles in real time. According to the detected obstacle information, the direction of node expansion is dynamically adjusted to avoid collision with obstacles. When a new obstacle is detected, the path is locally replanned to ensure the safety of the path. According to the shape and motion status of the obstacle, appropriate obstacle avoidance strategies are selected, such as detour, waiting, etc.

[0050] 5. Optimal parent node selection and path connection: In the process of bidirectional expansion, the optimal parent node selection strategy is introduced to select the parent node with the smallest path cost for connection to form a preliminary path;

[0051] In the optimal parent node selection and path connection steps, by introducing the optimal parent node selection strategy in the node expansion process of the heuristic node expansion step, the parent node with the smallest path cost is selected for connection according to the path cost (such as path length, energy consumption, etc.) to form a preliminary path, and then the nearest nodes in the two trees are connected to form a preliminary path from the starting point to the target point. At the same time, the RRT* optimization mechanism can be introduced to further improve the optimality of the path by iteratively updating the node positions in the path.

[0052] Through the above technical solution, the path cost of each node is calculated. Common cost functions include path length, energy consumption, time, etc. The appropriate cost function is selected according to the specific application requirements, the path cost of each node is calculated, and the parent node with the smallest path cost is selected for connection to form a preliminary path. The nearest nodes in the two trees are connected to form a preliminary path from the starting point to the target point. The node positions in the path are iteratively updated to further improve the optimality of the path. In each iteration, the nodes in the path are re-evaluated, and better nodes are selected for connection to gradually optimize the path.

[0053] 6. Path smoothing: Smoothing the generated preliminary path using a suitable smoothing algorithm, such as a cubic Hermite curve or a cubic B-spline curve, to meet the actual kinematic constraints and fluency requirements;

[0054] In the path smoothing step, the preliminary path generated in the optimal parent node selection and path connection step is smoothed, and cubic Hermite curves, cubic B-spline curves or other algorithms are used to ensure that the smoothed path meets the kinematic limitations of the robot arm and ensure the feasibility and smoothness of the path.

[0055] Through the above technical solution, the cubic Hermite curve, cubic B-spline curve and other algorithms are used to make the path smoother, while ensuring that the smoothed path meets the kinematic limitations of the robot arm, ensuring the feasibility and smoothness of the path, and ensuring that the path smoothly transitions between nodes without mutations. The cubic Hermite curve uses the Hermite interpolation formula to calculate the middle points on the path to make the path more continuous and smooth. The cubic B-spline curve uses the B-spline curve formula to calculate the middle points on the path to make the path smoother. Other algorithms include spline interpolation, Bezier curves, etc. After smoothing, the path is further optimized to improve the performance of the path.

[0056] 7. Path verification and optimization: Verify whether the path meets the obstacle avoidance requirements, dynamics model and kinematic constraints. If not, optimize the path;

[0057] In the path verification and optimization step, it is verified whether the path meets the obstacle avoidance requirements, dynamic model and kinematic constraints. If the path does not meet the requirements, it returns to the heuristic node expansion step to replan the path until a path that meets the requirements is found.

[0058] Through the above technical solution, an accurate collision detection algorithm is used to ensure that the path does not collide with obstacles and that the path conforms to the dynamic model and kinematic constraints of the robot arm. The dynamic model includes torque, velocity, acceleration, etc., and the kinematic constraints include joint angles, velocity, acceleration, etc. When the path does not meet the requirements, the path is replanned until a path that meets the requirements is found. At the same time, the feasibility of the path is verified in a simulation environment to ensure that there will be no problems with the path during actual execution.

[0059] 8. Output the final path: If the path meets all requirements, output the final path.

[0060] Through the above technical solutions, it is ensured that the path meets all requirements, including obstacle avoidance, dynamics and kinematic constraints, and the final path is output to the control system of the robot arm for execution by the robot arm. At the same time, the path is visualized in the simulation environment for easy debugging and verification.

[0061] While embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and alterations may be made to the embodiments without departing from the principles and spirit thereof, and that the scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A path planning method for robot arm obstacle avoidance based on improved bidirectional RRT algorithm, characterized in that: The following steps are involved: Define the robot and workspace: determine the robot's dynamic model, kinematic constraints, and obstacle distribution in the workspace; Bidirectional RRT algorithm initialization: Initialize two RRT trees from the starting point and the target point respectively, and set the algorithm parameters; Heuristic node expansion: In the first tree, a heuristic search strategy is used to select an expansion node to generate a new node; in the second tree, the new node is used as a virtual target point for expansion to generate a second new node; Dynamic obstacle detection and avoidance: During the node expansion process of the heuristic node expansion step, obstacles are detected in real time, and the node expansion direction is dynamically adjusted according to the obstacle position to avoid collision with obstacles; Optimal parent node selection and path connection: In the process of bidirectional expansion, the optimal parent node selection strategy is introduced to select the parent node with the smallest path cost for connection to form a preliminary path; Path smoothing: Smoothing the generated preliminary path using a suitable smoothing algorithm, such as cubic Hermite curves or cubic B-spline curves, to meet the actual kinematic constraints and fluency requirements; Path verification and optimization: Verify whether the path meets the obstacle avoidance requirements, dynamics model and kinematic constraints. If not, optimize the path; Output the final path: If the path meets all requirements, output the final path.

2. The path planning of a robot arm obstacle avoidance based on an improved bidirectional RRT algorithm according to claim 1, characterized in that: In the step of defining the robot arm and the workspace, firstly, the physical characteristics and motion laws of the robot arm are determined according to the actual application scenario and working environment of the robot arm, and then the positions and shapes of all static and dynamic obstacles are identified in the workspace.

3. The path planning of a robot arm obstacle avoidance based on an improved bidirectional RRT algorithm according to claim 1, characterized in that: In the bidirectional RRT algorithm initialization step, two RRT trees are initialized from the starting point and the target point respectively, one tree is expanded from the starting point and the other tree is expanded from the target point, and parameters such as step size, maximum number of iterations, collision detection accuracy, etc. are set.

4. The path planning of a robot arm obstacle avoidance based on an improved bidirectional RRT algorithm according to claim 1, characterized in that: In the heuristic node expansion step, the first tree adopts a heuristic search strategy (such as the heuristic function in the A* algorithm) to select an expansion node and generate a new node. The second tree uses the newly generated node in the first tree as a virtual target point, expands in the second tree, and generates a second new node.

5. The path planning for obstacle avoidance of a manipulator based on an improved bidirectional RRT algorithm according to claim 1, characterized in that: In the dynamic obstacle detection and avoidance step, the position and motion state of obstacles are detected in real time by using sensor data during the node expansion process of the heuristic node expansion step, and the direction of node expansion is dynamically adjusted according to the detected obstacle information to avoid collision with obstacles.

6. The path planning for obstacle avoidance of a manipulator based on an improved bidirectional RRT algorithm according to claim 1, characterized in that: In the optimal parent node selection and path connection step, by introducing the optimal parent node selection strategy in the node expansion process of the heuristic node expansion step, the parent node with the smallest path cost is selected for connection according to the path cost (such as path length, energy consumption, etc.) to form a preliminary path, and then the nearest nodes in the two trees are connected to form a preliminary path from the starting point to the target point. At the same time, the RRT* optimization mechanism can be introduced to further improve the optimality of the path by iteratively updating the node positions in the path.

7. The path planning of a robot arm obstacle avoidance based on an improved bidirectional RRT algorithm according to claim 1, characterized in that: In the path smoothing step, the preliminary path generated in the optimal parent node selection and path connection step is smoothed, and a cubic Hermite curve, a cubic B-spline curve or other algorithms are used to ensure that the smoothed path meets the kinematic limitations of the robot arm and ensure the feasibility and smoothness of the path.

8. The path planning of a robot arm obstacle avoidance based on an improved bidirectional RRT algorithm according to claim 1, characterized in that: In the path verification and optimization step, it is verified whether the path meets the obstacle avoidance requirements, dynamic model and kinematic constraints. If the path does not meet the requirements, the heuristic node expansion step is returned to replan the path until a path that meets the requirements is found.

9. The path planning of a robot arm obstacle avoidance based on an improved bidirectional RRT algorithm according to claim 1, characterized in that: In the step of outputting the final path, if the path meets all requirements, the final path is output for execution by the robot arm.

Images