Autonomous path planning method for robot main body and mechanical arm end effector

Abstract

The invention discloses an autonomous path planning method for a robot main body and a mechanical arm end effector, and the method comprises the following steps: obtaining point cloud data through a laser radar, preprocessing the point cloud data, analyzing a preprocessed point cloud data set, and determining that the point cloud data set is divided into a static obstacle and a dynamic obstacle according to clustering features; relevant distance, speed and repulsive force potential field functions are defined based on static and dynamic obstacles, a total potential field function is defined, the negative gradient of the total potential field function and the acceleration of the robot are calculated, and finally the speed and the position are updated through an integration method to obtain a path planning result. Therefore, the method has the beneficial effects of accurate environment perception and obstacle classification, real-time attitude updating and stability, efficient path planning and adaptability and the like.

Description

Technical Field

[0001] The present invention belongs to the field of intelligent manufacturing equipment industry, relates to the control of industrial robots, and particularly relates to an autonomous path planning method for a robot body and an end effector of a robotic arm. Background Art

[0002] In the intelligent manufacturing equipment industry, industrial robots, as a core component, play an important role in promoting the progress of the entire industry. With the advancement of technology and the improvement of automation, robots are increasingly widely used in various industries. Especially in high-risk environment operation tasks, such as live working in the power industry, the application of robot technology not only needs to consider the precise control of the robotic arm, but also needs to combine advanced sensor technology and intelligent control systems to achieve real-time perception of the high-voltage environment and safe operation. Live working refers to the work of repairing or maintaining high-voltage power equipment without power interruption, which poses extremely high requirements for the autonomy and safety of the robot and the precise control ability of the robotic arm. Especially for the automatic lead connection task at the 110 kV voltage level, the robotic arm, as the core part to perform specific operations, needs to complete complex path planning and motion control in a high-voltage environment. Since live working involves a high-voltage environment, any misoperation may lead to serious safety problems. Path planning technology is one of the keys to ensuring that the robotic arm can safely and efficiently complete live working in a complex environment.

[0003] Traditional path planning methods mainly include map-based methods and sensor-based methods. The former relies on a pre-built accurate environmental model to guide the robot to avoid known obstacles; the latter relies on real-time perception of environmental changes and allows the robot to dynamically adjust the route. However, both of these methods have significant limitations when applied to live working scenarios: map-based methods are difficult to cope with the constantly changing work site, while sensor-based methods may be inefficient or make incorrect decisions due to computational resource limitations or insufficient information processing speed when facing a complex and changeable high-voltage environment.

[0004] In addition, live working also involves special electrical isolation and protection requirements, which further increase the difficulty of path planning. For example, when performing an automatic lead connection task, the robot and the robotic arm must maintain a safe distance from the live body, while ensuring the correct connection of the lead to avoid short circuits or other electrical faults. Therefore, how to improve the path planning ability of the robot and its robotic arm in a complex environment, especially to achieve precise and efficient automatic lead connection in a high-voltage live environment, while ensuring the safety of personnel, has become an urgent technical problem to be solved. Summary of the Invention

[0005] The first object of the present invention aims to solve the technical problems in the above technology to at least a certain extent, and provides an autonomous path planning method for a robot body and an end effector of a robotic arm.

[0006] To achieve the above object, the present invention adopts the following technical solutions:

[0007] S1. Use a lidar to obtain a point cloud data set and perform preprocessing to obtain a preprocessed point cloud data set

[0008] S2. Perform clustering analysis on the preprocessed point cloud data set According to the clustering analysis result C = {C1, C2,..., C m}, divide the static obstacles O sa and dynamic obstacles O db ;

[0009] S3. Calculate the gravitational potential field and repulsive potential field of the robot body or the end effector of its robotic arm, where the repulsive potential field includes a static repulsive potential field related to the static obstacle and a dynamic repulsive potential field related to the dynamic obstacle

[0010] S4. Calculate the total potential field according to the gravitational potential field and the repulsive potential field Calculate the negative gradient of the total potential field Obtain the acceleration of the robot body or the end effector of its robotic arm Then, calculate the speed and position update of the robot body or the end effector of its robotic arm according to the acceleration to obtain the path planning result.

[0011] Further, in step S1, use a Gaussian filtering algorithm to preprocess the point cloud data set P = {P1, P2,..., P n} and obtain the processed point cloud data set where the three-dimensional Gaussian kernel function is σ is the standard deviation.

[0012] Further, in step S2, perform clustering analysis on the preprocessed point cloud data set including the following steps:

[0013] S2.1. For any point in the data set If at least MinPts points (including itself) are included within its neighborhood radius ∈, then mark as a core point, and MinPts is the minimum number of points parameter;

[0014] S2.2. If the point ​Located at the core point If the neighborhood radius ∈ is within the range of Subordinate to the core point Right now and Close enough to be considered part of the same cluster;

[0015] S2.3, if point Subordinate to the core point And the core point Subordinate to the core point Zeji Subordinate to the core point This transitive relationship ensures that the hierarchy of clusters is correctly identified;

[0016] S4. All data points that have a point chain and each point in the chain shares at least one core point with the next point form a cluster.

[0017] Furthermore, in step S2, cluster C is calculated j The features include calculating the centroid coordinates of the cluster (the average position of all points in the cluster, which can represent the center position of the cluster), volume and shape features (boundary shape, symmetry, complexity, etc.).

[0018] Furthermore, in step S4, a total potential field function is defined The effects of both gravitational and repulsive potential fields are comprehensively considered. This allows the live wire joining robot to calculate its potential field distribution based on real-time information about the environment, including the locations of static and dynamic obstacles. This enables the robot to autonomously plan a path that avoids obstacles while moving toward its target. This not only improves the robot's navigation capabilities in complex environments but also ensures the safety and efficiency of its path planning.

[0019] By calculating the total potential field function The negative gradient To determine the movement direction of the live wire robot. The negative gradient vector points to the direction of lowest energy in the potential field, which is exactly the direction the live wire robot should move, enabling effective path planning and obstacle avoidance. This method allows the live wire robot to flexibly avoid obstacles in complex environments while moving toward its target point, improving its autonomy and adaptability. By accurately calculating the negative gradient of the potential field and the live wire robot's acceleration, the live wire robot can react quickly and optimize its trajectory.

[0020] Furthermore, the present invention further comprises the following steps:

[0021] Updating the attitude in path planning:

[0022] Measure the acceleration and angular velocity of the robot body or the end effector of its robotic arm using an inertial unit (IMU), and update the quaternion of the attitude according to the quaternion kinematic equation to complete the attitude update of the robot body or the end effector of its robotic arm; where is the time derivative of q.

[0023] The second object of the present invention aims to provide a robot, including a robotic arm loaded with an end effector, a processor, a memory, and a computer program stored on the memory and executable on the processor. When the processor runs the computer program, the robot executes the above-mentioned autonomous path planning method.

[0024] Compared with the prior art, the present invention has the following beneficial effects:

[0025] (1) The present invention proposes a path planning method for an industrial robot in a dynamic and static complex environment, which can achieve accurate environmental perception and obstacle classification. By using a lidar to obtain point cloud data and preprocess it, through clustering analysis and feature judgment, static and dynamic obstacles can be accurately distinguished, providing accurate environmental information for path planning and effectively avoiding collisions;

[0026] (2) The present invention can achieve real-time attitude update and stability. By using an inertial unit to measure acceleration and angular velocity, and updating the attitude quaternion of the live working lead connecting robot according to the quaternion kinematic equation, it ensures that the live working lead connecting robot can always master its own attitude in real time, improving the motion stability and control accuracy;

[0027] (3) The present invention can achieve efficient path planning and adaptability. By defining a total potential field function based on the gravitational potential field and repulsive potential field functions, calculating the negative gradient to obtain the acceleration, and updating the velocity and position through integration, a reasonable path can be quickly planned, adapting to complex dynamic environments, improving the operation efficiency of the live working lead connecting robot, and meeting the application requirements of intelligent manufacturing equipment in complex environments.

[0028] In summary, by combining advanced sensing technologies and intelligent algorithms, the present invention can quickly establish an adaptive path planning scheme in unknown or semi-structured environments, while ensuring compliance with all safety specifications and technical requirements, thereby effectively improving the operation performance and reliability of the robot in high-voltage environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0029] The technical solutions and beneficial effects of the present invention will become obvious and easy to understand from the following content in conjunction with the drawings, where:

[0030] Figure 1Flow chart of the live working lead connection robot and the autonomous path planning method of its upper robotic arm according to the present invention;

[0031] Figure 2 Flow chart of the clustering analysis in step S2 of the present invention. Specific embodiments

[0032] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments.

[0033] Next, the live working lead connection robot based on environmental perception and the autonomous path planning method of its upper robotic arm disclosed by the present invention will be described with reference to the accompanying drawings.

[0034] As Figure 1 shown, the live working lead connection robot based on environmental perception and the autonomous path planning method of its upper robotic arm include the following steps:

[0035] S1. Use a lidar to obtain point cloud data. The point P in the point cloud data i =(x i , y i , z i ), and the point cloud data set P = {P1, P2,..., P n}}. Use a filtering algorithm to preprocess the point cloud data set P = {P1, P2,..., P n} and obtain the processed point cloud data set

[0036] It should be noted that the lidar is installed on the top of the main body of the live working lead connection robot and scans the surrounding environment at a certain frequency, such as 10 Hz, to obtain point cloud data. Each point P in the point cloud data i =(x i , y i , z i ) represents a position coordinate in space. Assuming that in one scan, n = 1000 points are obtained, forming a point cloud data set P = {P1, P2,..., p 1000};

[0037] It should also be noted that in step S1, the Gaussian filtering algorithm is used to preprocess the point cloud data set p = {P1, P2,..., P n} and obtain the processed point cloud data set wherein, the three-dimensional Gaussian kernel function is σ is the standard deviation;

[0038] Assume σ = 0.1, and remove the noise points through filtering to obtain the processed point cloud data set Suppose 50 noise points are removed;

[0039] S2. Cluster the preprocessed point cloud dataset to obtain the clustering result C = {C1, C2, …, C m}, calculate the features of each cluster C j , set the static obstacle judgment threshold T s and the dynamic obstacle judgment threshold T d . If the features of cluster C j satisfy the static obstacle judgment threshold T s , then add it to the static obstacle set O s = {O s1 , O s2 , …, O sk}. If the features of cluster C j satisfy the dynamic obstacle judgment threshold T d , then add it to the dynamic obstacle set O d = {O d1 , O d2 , …, O dl};

[0040] It should be noted that in step S2, clustering analysis of the preprocessed point cloud dataset includes the following steps:

[0041] S2.1. For any point in the dataset If there are at least MinPts points (including itself) in its radius ∈ neighborhood, then mark as a core point, where MinPts is the minimum number of points parameter;

[0042] S2.2. If point is located in the radius ∈ neighborhood of core point , then mark as belonging to core point , that is is close enough to to be considered part of the same cluster;

[0043] S2.3. If point belongs to core point and core point belongs to core point , then mark as belonging to core point . This transitive relationship ensures that the hierarchical structure of the clustering is correctly identified;

[0044] S2.4. Form a cluster by all data points that have a point chain where each point in the chain shares at least one core point with the next point.

[0045] The above process is as Figure 2 shown.

[0046] Perform clustering analysis on the preprocessed point cloud data set, setting ∈ = 0.3 and MinPts = 5;

[0047] For example, there are 8 points in the neighborhood of point , meeting the conditions, so is a core point. Assume is within the neighborhood of the core point , then belongs to

[0048] By such rules, form a cluster by all data points that are density-reachable from each other. Finally, obtain the clustering result C = {C1, C2, …, C 10}, assuming there are 10 clusters;

[0049] It should also be noted that in step S2, calculating the features of cluster C j includes calculating the centroid coordinates of the cluster (the average position of all points in the cluster, which can represent the central position of the cluster), volume, and shape features (boundary shape, symmetry, complexity, etc.);

[0050] Set the static obstacle judgment threshold T s . For example, a cluster with a relatively low centroid height, a large volume, and a relatively regular shape may meet the static obstacle judgment threshold, and add it to the static obstacle set O s = {O s1 , O s2 , …, O sk}, assuming k = 5, that is, 5 clusters are judged as static obstacles. Set the dynamic obstacle judgment threshold T d . If a cluster with certain moving speed characteristics meets the dynamic obstacle judgment threshold, add it to the dynamic obstacle set O d = {O d1 , O d2 , …, O dl}, assuming l = 3, that is, 3 clusters are judged as dynamic obstacles;

[0051] S3. Use an inertial unit to measure the acceleration and angular velocity of the live working lead connection robot and, according to the quaternion kinematic equation Update the quaternion of the attitude of the live working lead connection robot, where q = (q0, q1, q2, q3) represents the quaternion of the attitude of the live working lead connection robot;

[0052] It should be noted that in step S3, the accuracy of the acceleration of the inertial unit is not greater than ±0.05 m / s 2 , and the accuracy of the angular velocity is not greater than ±0.1 ° / s;

[0053] Specifically, for example, the measured acceleration a = (0.1 m / s 2 , 0.05 m / s 2 , 0.02 m / s 2 ), and the angular velocity ω = (0.5 ° / s, 0.3 ° / s, 0.2 ° / s);

[0054] Assume the initial quaternion q = (1, 0, 0, 0), and the updated quaternion is obtained through calculation;

[0055] S4. Define the current position of the live working lead connection robot Target position and the vector of the live working lead connection robot and define the gravitational potential field function where k att is the gravitational coefficient, is the transpose of d T , that is

[0056] Specifically, define the current position of the live working lead connection robot Target position The vector of the live working lead connection robot Define the gravitational coefficient k att = 1;

[0057] S5. Based on the static obstacle O sa ∈ O s , define the distance from the live working lead connection robot to the static obstacle O sa and the repulsive potential field function where k reps is the static obstacle repulsive coefficient, and d Os is the static obstacle influence range threshold;

[0058] It should be noted that in step S5, the static obstacle repulsive coefficient k reps ∈ [10, 50], and the static obstacle influence range threshold d Os ​∈[0.5 m, 2 m];

[0059] Specifically, assume that for a static obstacle O s1 , the distance from the live working lead connection robot to it Set the repulsive force coefficient k of the static obstacle reps = 30, and the influence range threshold d of the static obstacle Os = 1 m;

[0060] S6. Based on the dynamic obstacle O db ∈O d , define the distance from the live working lead connection robot to the dynamic obstacle O db Define the relative velocity of the live working lead connection robot to the dynamic obstacle O db Predicted time to collision Repulsive force potential field function Among them, is the velocity of the live working lead connection robot, is the velocity of the dynamic obstacle, d db is the current distance from the live working lead connection robot to the dynamic obstacle, k repd is the repulsive force coefficient of the dynamic obstacle, t Od is the influence time threshold of the dynamic obstacle;

[0061] It should be noted that in step S6, the repulsive force coefficient k of the dynamic obstacle repd ∈[20, 80], and the influence time threshold t of the dynamic obstacle Od ∈[1 s, 5 s];

[0062] Specifically, assume a dynamic obstacle O d1 , the current distance d from the live working lead connection robot to it db = 2 m, the velocity of the live working lead connection robot Velocity of the dynamic obstacle Then the relative velocity Predicted time to collision t co = 20 s, set the repulsive force coefficient k of the dynamic obstacle repd = 50, and the influence time threshold t of the dynamic obstacle Od = 3 s;

[0063] S7. Define the total potential field function:

[0064]

[0065] Among them, is the gravitational potential field function,​​ is the repulsive potential field function related to the static obstacle O sa and is the repulsive potential field function related to the dynamic obstacle O db , where k and l are the numbers of static and dynamic obstacles respectively.

[0066] S8. Calculate and define the negative gradient of the total potential field function The negative gradient vector points to the direction of the lowest energy in the potential field, that is, the direction in which the main body of the live working lead connecting robot should move, and calculate the acceleration of the main body of the live working lead connecting robot where m is the mass of the live working lead connecting robot;

[0067] S9. Calculate the acceleration of the live working lead connecting robot according to

[0067] Calculate the velocity and position update of the live working lead connecting robot through the integration method, so as to obtain the path planning result of the live working lead connecting robot;

[0068] It should be noted that in step S9, the calculation formula of the integration method is:

[0069] where h is the integration step size, is the acceleration of the live working lead connecting robot at time T n and velocity , is the updated velocity, T n+1 is the updated time, and k1, k2, k3, k4 are intermediate variables used to update the velocity, which respectively represent the acceleration estimates at different time points. In this way, the live working lead connecting robot can autonomously plan a path to avoid obstacles and move towards the target point at the same time.

[0070] By continuously repeating this process, update the velocity and position of the live working lead connecting robot, so as to obtain the path planning result of the live working lead connecting robot.

[0071] ​​The autonomous path planning method for the robot body and the end effector of the robotic arm is widely used in intelligent manufacturing equipment, especially in scenarios that require high-precision operations, such as industrial robots, intelligent assembly equipment, etc. In one embodiment of the present invention, this method can also be used for an outdoor live working lead connection robot. The body of this type of live working lead connection robot is usually used for tasks such as power facility maintenance and repair, and these tasks require highly precise operations to avoid electrical accidents. Therefore, the control of the end effector of the robotic arm (hereinafter referred to as the end effector) on the live working lead connection robot body not only needs to consider the safety and efficiency of path planning, but also ensure the accuracy of operations.

[0072] Specifically, the lidar on the end effector of the robotic arm obtains the point cloud data of the surrounding environment, and its processing method is similar to the lidar data processing in the path planning of the live working lead connection robot body. First, use the Gaussian filtering algorithm for the obtained point cloud data set P = {P1, P2, …, P n}, and use the filtering algorithm to preprocess the point cloud data set P = {P1, P2, …, P n} and obtain the processed point cloud data set The IMU on the end effector of the robotic arm measures the angular velocity of the end effector of the robotic arm, providing data support for the attitude update of the end effector of the robotic arm.

[0073] Perform clustering analysis on the preprocessed point cloud data set to obtain the clustering result C = {C1, C2, …, C m}, and calculate the features of each cluster, including centroid coordinates, volume, shape features, etc.

[0074] According to the set static obstacle judgment threshold T s and the dynamic obstacle judgment threshold T d , identify the static and dynamic obstacles around the end effector of the robotic arm. For the end effector of the robotic arm, static obstacles may include fixed facilities such as utility poles and transformer housings, and dynamic obstacles may include other equipment or personnel moving near the operation area.

[0075] Based on environmental perception, determine the initial position of the end effector of the robotic arm to avoid collisions between the end effector of the robotic arm and surrounding obstacles in the initial stage of movement. At the same time, according to the target operation position (such as the position of the insulator to be repaired) and the current position of the end effector of the robotic arm, determine the movement direction and approximate path of the end effector of the robotic arm. Define the current position target position and vector Define the gravitational potential field function where, k attmis the gravitational coefficient of the end effector of the robotic arm;

[0076] For static obstacles, based on the identified set of static obstacles O s , define the distance from the end effector of the robotic arm to the static obstacle O sa distance repulsive potential field function where k repsm is the repulsive coefficient of the end effector of the robotic arm with respect to static obstacles, and d Os is the influence range threshold of static obstacles;

[0077] For dynamic obstacles, based on the identified set of dynamic obstacles O d , define the distance from the end effector of the robotic arm to the dynamic obstacle O db distance Define the relative velocity of the end effector of the robotic arm to the dynamic obstacle O db relative velocity predicted time to collision repulsive potential field function where is the velocity of the end effector of the robotic arm, is the velocity of the dynamic obstacle, and d mdb is the current distance from the end effector of the robotic arm to the dynamic obstacle, and k repdm is the repulsive coefficient of the end effector of the robotic arm with respect to dynamic obstacles, and t Od is the influence time threshold of dynamic obstacles;

[0078] Define the total potential field function,

[0079] Calculate the negative gradient of the total potential field function negative gradient

[0080] Calculate the velocity and position updates of the end effector of the robotic arm through an integration method. The calculation formula of the integration method is similar to that in the path planning of the main body of the live working lead connecting robot. For example:

[0081] By continuously updating the velocity and position of the end effector of the robotic arm, it can accurately move towards the target operation position while avoiding obstacles;

[0082] When the end effector of the robotic arm approaches the target operation position, according to the requirements of the operation task, control the tool actuator to perform corresponding operations. For example, if it is a task of tightening nuts, control the electric wrench to tighten according to a predetermined torque value. If it is a task of replacing insulators, control the insulator replacement fixture to accurately grasp and replace the insulators;

[0083] During the operation of the end effector of the robotic arm, according to the angular velocity measured by the IMU and the environmental information sensed by the lidar, adjust the attitude of the end effector of the robotic arm in real time to ensure the stability and accuracy of the end effector of the robotic arm during the operation.

[0084] In addition, in another embodiment of the present invention, the robot autonomous path planning method based on environmental perception can well adapt to the degrees of freedom of the end effector of the robotic arm of the outdoor live working robot;

[0085] Specifically, assume that the end effector of the robotic arm has n degrees of freedom, and its joint space can be expressed as z = [z1, z2, …, z n T , where z i represents the position of the i-th joint (for a revolute joint, it can be the joint angle, and for a prismatic joint, it can be the joint displacement);

[0086] The position and attitude of the end effector of the robotic arm in the Cartesian space can be expressed as where (x, y, z) are the position coordinates of the end effector in the three-dimensional space, is the Euler angle (or other attitude representation methods, such as quaternions, but here the Euler angle is taken as an example for easy understanding) describing the attitude of the end effector;

[0087] For the forward kinematics model, there is a function which describes the mapping relationship from the joint space to the Cartesian space. This function is usually non-linear and has different specific forms for different structures of the end effector of the robotic arm;

[0088] For the inverse kinematics model, given the target position and attitude of the end effector in the Cartesian space solve for the joint positions in the joint space such that Here, an iterative algorithm can be used to solve the inverse kinematics, such as an iterative algorithm based on the Jacobian matrix. The Jacobian matrix whose elements Through the iterative formula to gradually approximate the solution, where, is ​Pseudo-inverse;

[0089] In path planning based on the potential field method, it is necessary to consider the degree-of-freedom constraints of the end effector of the robotic arm. For example, the gravitational potential field function where is the current position of the end effector, is the target position, and its negative gradient represents the gravitational direction. After calculating the gravitational direction, it is necessary to transform it into the joint space through the Jacobian matrix to obtain the gravitational force in the joint space

[0090] For the repulsive potential field function (static and dynamic obstacles), it is also necessary to transform the repulsive direction into the joint space. Taking the static obstacle as an example, let the static obstacle be O sa , and the distance from the end effector of the robotic arm to the static obstacle Static obstacle repulsive potential field function Its negative gradient At is The repulsive force transformed into the joint space

[0091] Total potential field function where k is the number of static obstacles and l is the number of dynamic obstacles. Calculate its negative gradient Then transform it into the joint space to obtain the total joint force

[0092] Consider the dynamic model of the end effector of the robotic arm where is the inertia matrix, is the centrifugal and Coriolis force terms, is the gravity term, τ is the joint driving torque. In path planning, τ = τ m ;

[0093] According to the dynamic model, the motion of the end effector of the robotic arm can be controlled by controlling the joint driving torque, so that it moves along the planned path and adapts to the degree-of-freedom constraints. For example, the control law where and are the proportional and derivative gain matrices, and are the desired joint positions and velocities;

[0094] To further optimize the motion of the end effector of the robotic arm in path planning, consider degree-of-freedom optimization. For example, define an optimization objective function Among them, is the kinetic energy of the end effector of the robotic arm, is the inertia matrix of the end effector of the robotic arm, is the time derivative of q, that is, the joint velocity; is the potential energy of the end effector of the robotic arm; λ is the weight coefficient, which is used to balance the energy consumption and the performance of path planning; t0 is the start time of path planning, and t f is the end time of path planning.

[0095] By solving this optimization problem, a better joint trajectory can be obtained such that the end effector of the robotic arm can make better use of the degrees of freedom while meeting the requirements of path planning, improving the motion efficiency and stability.

[0096] In summary, according to the live working lead connection robot based on environmental perception and its autonomous path planning method for the upper robotic arm end effector disclosed in the present invention, on the one hand, it has accurate environmental perception and obstacle classification. By using a lidar to obtain point cloud data and preprocess it, through clustering analysis and feature judgment, static and dynamic obstacles can be accurately distinguished, providing accurate environmental information for path planning and effectively avoiding collisions. On the other hand, it has real-time attitude update and stability. By using an inertial unit to measure acceleration and angular velocity and updating the attitude quaternion of the live working lead connection robot according to the quaternion kinematic equation, it ensures that the live working lead connection robot can always master its own attitude in real time, improving the motion stability and control accuracy. Finally, it has efficient path planning and adaptability. By defining a total potential field function based on the gravitational potential field and repulsive potential field functions, calculating the negative gradient to obtain the acceleration, and updating the velocity and position through integration, a reasonable path can be quickly planned to adapt to a complex dynamic environment and improve the operation efficiency of the live working lead connection robot.

[0097] Although the embodiments of the present invention have been shown and described above, it can be understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those of ordinary skill in the art can make changes, modifications, substitutions, and variations to the above embodiments within the scope of the present invention.

Claims

1. An autonomous path planning method for a robot main body and an end effector of a robotic arm, characterized in that, It includes the following steps: S1. Obtain a point cloud data set using a lidar and perform preprocessing; S2. Perform clustering analysis on the preprocessed point cloud data set, and divide static obstacles and dynamic obstacles according to the characteristics of the clustering analysis results; S3. Calculate the gravitational potential field and repulsive potential field of the robot body or the end effector of its robotic arm, where the repulsive potential field includes a static repulsive potential field related to static obstacles and a dynamic repulsive potential field related to dynamic obstacles; S4. Calculate the total potential field based on the gravitational potential field and the repulsive potential field, calculate the negative gradient of the total potential field to obtain the acceleration of the robot body or the end effector of its robotic arm, and then calculate the speed and position update of the robot body or the end effector of its robotic arm according to the acceleration to obtain the path planning result.

2. The autonomous path planning method for the robot body and the end effector of the robotic arm according to claim 1, characterized in that, In step S1, the preprocessing is Gaussian filtering.

3. The autonomous path planning method for the robot body and the end effector of the robotic arm according to claim 1, wherein In step S2, the clustering analysis includes the following steps: S1. For any point in the point cloud dataset If its neighborhood radius ∈ contains at least including MinPts points, then mark as a core point; where MinPts is the minimum number of points parameter; S2. If the point is within the neighborhood radius ∈ of the core point , it is denoted as subordinate to the core point S3. If the point is subordinate to the core point and the core point is subordinate to the core point then record is subordinate to the core point S4. Form a complete cluster with all the points that can be gradually connected through the core point and its neighborhood.

4. The autonomous path planning method for the robot body and the end effector of the robotic arm according to claim 1, wherein In step S3, the gravitational potential field is obtained according to the position vector between the current position of the robot body or the end effector of its robotic arm and the target position, and a preset gravitational coefficient.

5. The autonomous path planning method for the robot body and the end effector of the robotic arm according to claim 1, characterized in that In step S3, the static repulsive potential field is obtained according to the distance between the robot body or the end effector of its robotic arm and the static obstacle, the influence range threshold of the static obstacle, and a preset static obstacle repulsive coefficient; The dynamic repulsive potential field is obtained according to the time for the robot body or the end effector of its robotic arm to move to the dynamic obstacle, the dynamic obstacle influence time threshold, and a preset dynamic obstacle repulsive coefficient.

6. The autonomous path planning method for the robot body and the end effector of the robotic arm according to claim 1, characterized in that, In step S4, the gravitational potential field, the static repulsive potential field of each static obstacle, and the dynamic repulsive potential field of each dynamic obstacle are added together to calculate the total potential field.

7. The autonomous path planning method for the robot body and the end effector of the robotic arm according to claim 1, wherein The attitude update of the robot body or its robotic arm is also included in the path planning. The attitude update includes the following steps: Measure the acceleration and angular velocity of the robot body or the end effector of its robotic arm using an inertial unit, and update the attitude quaternion according to the quaternion kinematic equation to complete the attitude update.

8. The autonomous path planning method for the robot body and the end effector of the robotic arm according to claim 1, characterized in that, When performing path planning for the robotic arm, the degree-of-freedom constraint of the robotic arm is considered, specifically: Suppose the robotic arm has n degrees of freedom, and its joint space is defined as z = [z1, z2, …, z n T , where z i represents the position of the i-th joint, i = 1, ..., n;​ Define the position and orientation of the end effector of the robotic arm in Cartesian space as Calculating the gravitational potential field The repulsive potential field associated with static obstacles and the repulsive potential field associated with dynamic obstacles Obtaining the total potential field Calculate the total potential field of the negative gradient and then transform it to the joint space z to obtain the total joint force According to the dynamic model of the robotic arm, control the movement of the robotic arm by controlling the joint driving torque τ so that it moves along the planned path and adapts to the degree-of-freedom constraint.

9. The autonomous path planning method for the robot body and the end effector of the robotic arm according to claim 8, characterized in that, The following objective function is used to optimize the robotic arm path planning: Among them, is the kinetic energy of the robotic arm, is the inertia matrix of the robotic arm, is the time derivative of q, that is, the joint velocity; is the potential energy of the robotic arm; λ is the weight coefficient; t0 is the start time of path planning, and t f is the end time of path planning.

10. A robot, characterized in that, It includes a robotic arm equipped with an end effector, a processor, a memory, and a computer program stored on the memory and executable on the processor. When the processor runs the computer program, the robot executes the autonomous path planning method according to any one of claims 1-9.

Images