A path planning method for uniform spraying robot

Abstract

The present invention discloses a path planning method for a uniform spraying robot, and relates to the field of industrial automation technology. The method includes: obtaining a three-dimensional point cloud model, preprocessing the point cloud model, plane detection, path planning, and dynamic adjustment. The present invention collects point cloud data through a depth camera, generates a three-dimensional model from the point cloud data; preprocesses the three-dimensional point cloud model to obtain a wall surface model; performs plane detection based on the wall to detect the plane where the wall is located; and performs path planning based on the plane information using a path planning algorithm based on curve generation. The present invention provides a path planning method for a uniform spraying robot, which improves spraying quality and efficiency, reduces paint waste, and protects worker safety.

Description

Technical Field

[0001] The present invention relates to the technical field of industrial automation, and in particular to a path planning method for a uniform spraying robot. Background Art

[0002] Spraying operations are very labor-intensive, cumbersome, and harmful to human health. Current spraying technologies mainly use programmed spraying and fixed-point spraying. Programmed spraying is divided into online programming and offline programming. Programmed spraying mainly uses offline programmed spraying. Offline programming is not suitable for simple spraying trajectories, and there are errors during programming, which have a certain impact on the spraying accuracy. Fixed-point spraying requires marking the spraying points one by one and then performing fixed-point spraying. This spraying method is time-consuming and labor-intensive, with low spraying efficiency and quality, uneven spraying, and a waste of paint. In addition, long-term contact with paint can easily cause physical injury or poisoning.

[0003] Therefore, it is an urgent problem for those skilled in the art to propose a path planning method for a uniform spraying robot to solve the difficulties existing in the prior art. Summary of the invention

[0004] In view of this, the present invention provides a path planning method for a uniform spraying robot, which can improve spraying quality and efficiency, reduce paint waste, and protect worker safety.

[0005] In order to achieve the above object, the present invention adopts the following technical solution:

[0006] A path planning method for a uniform spraying robot, characterized in that it comprises the following steps:

[0007] S1. Obtaining a three-dimensional point cloud model: using a depth camera to collect data, and converting the collected data into a three-dimensional point cloud model;

[0008] S2, point cloud model preprocessing: preprocess the obtained three-dimensional point cloud model to obtain the wall surface;

[0009] S3, plane detection: Based on the plane fitting algorithm of the point cloud, the plane where the wall is located is detected, and the flatness of the wall surface is determined according to the flatness of the wall surface to determine whether the flatness of the wall surface is within a threshold, the wall plane information is obtained, and the flat wall area is distinguished;

[0010] S4, path planning: Based on the wall plane information obtained by S3 detection, a path planning algorithm based on curve generation is constructed, and the path planning is adjusted according to the change of the flatness of the wall;

[0011] S5. Dynamic adjustment: During the spraying process, the flatness of the wall is monitored in real time, and a feedback control algorithm is used to dynamically adjust the path planning based on the real-time monitoring results.

[0012] In the above method, optionally, S1 obtains a three-dimensional point cloud model, uses a depth camera to collect data, obtains a depth image or a color image through an infrared camera, and converts the depth image or the color image into three-dimensional coordinate points according to the camera's focal length, principal point coordinates, distortion coefficient, and rotation matrix and translation vector that describe the camera's imaging model and posture to obtain a three-dimensional point cloud model P.

[0013] The above method is optional. S2, point cloud model preprocessing steps are as follows:

[0014] S201 denoising: selecting a local outlier factor algorithm based on neighborhood points to perform denoising on the three-dimensional point cloud model P;

[0015] S202 filtering processing: according to the application scenario and requirements, the denoised three-dimensional point cloud model P is subjected to moving average filtering; specifically, given a signal sequence x[n] containing N samples, where n is the index of the sample from 0 to N-1, the moving average filtering filters the denoised three-dimensional point cloud model P by sliding a window of a fixed length of M on the signal sequence and calculating the average value of the samples in the window;

[0016] S203 surface reconstruction: select the reconstruction method of surface fitting, find a corresponding surface patch for each point in the filtered three-dimensional point cloud model P in S202, so that the distance between the point and the surface patch is minimized, then splice all the surface patches into a complete surface model, perform surface reconstruction on the filtered three-dimensional point cloud model P_filtered, and obtain the reconstructed wall surface model surface_model;

[0017] S204 returns the processed three-dimensional point cloud model: returns the processed three-dimensional point cloud model processed_cloud, which contains data after denoising, filtering and surface reconstruction, for subsequent path planning and spraying operations.

[0018] The above method is optional, and the specific content of S3 is:

[0019] S301 uses the RANSAC plane fitting algorithm to detect plane D from the preprocessed 3D point cloud model P and expresses plane D as:

[0020] D: N·(PQ)=0;

[0021] Where N is the normal vector of plane D, and Q is any point on plane D;

[0022] S302: Substitute the points in the fitted plane D into the equation to confirm the fitting degree of the plane: Substitute N and Q into the plane equation D: N·(PQ)=0 to verify whether the points in the processed three-dimensional point cloud model satisfy the obtained plane equation, thereby confirming the fitting degree of the plane;

[0023] S303: taking the point set satisfying the plane equation as the point set in the detected plane D;

[0024] S304 outputs the detected plane D, which includes a normal vector N and a point Q on the plane.

[0025] In the above method, optionally, in step S4, path planning, the path generation method according to the detected plane information can be expressed as:

[0026] waypoints=generate_waypoints(plane_model,wall_smoothness),

[0027] The above method is optional, S4, the path planning step is as follows:

[0028] S401, dividing the wall surface to obtain different plane areas;

[0029] S402, performing curvature analysis on each plane region, calculating its flatness, and selecting a curve type with the largest flatness among various curve types, including but not limited to a polynomial curve and a double arc segment curve;

[0030] For each curve type, the least square method is used to solve the optimal curve parameters so that the error between the curve and the plane area is minimized; for each curve, the connection point and tangent direction with the adjacent curve are calculated to ensure the continuity and smoothness of the curve;

[0031] S403, determining the boundary conditions of each plane region according to the curve type, including but not limited to position, velocity, and acceleration;

[0032] S404. Use the undetermined coefficient method to solve the correlation coefficient of the curve; connect the curves of each plane area to form a complete path.

[0033] In the above method, optionally, in S4, in the path planning step, a set of path points is generated according to the plane model and the flatness of the wall to guide the movement of the spraying robot.

[0034] In the above method, optionally, in step S4, path planning, an index of wall flatness is introduced as a constraint condition, where flatness is expressed as an index to measure the flatness of a plane, and the position and shape of the plane are used as parameters.

[0035] Flatness calculation methods include:

[0036] Calculate curvature: For each point p in the processed 3D point cloud model processed_cloud, calculate its curvature value and add up all curvature values ​​to get the total curvature;

[0037] Analyze surface roughness: For each point p in the processed 3D point cloud model processed_cloud, consider the change in normal vector or local density of points in its neighborhood, and accumulate the roughness index according to the change;

[0038] Analyze the uniformity of point distribution: For each point p in the processed 3D point cloud model processed_cloud, calculate the distribution density or distance distribution of the surrounding points, and accumulate the uniformity index according to the density or distribution;

[0039] Comprehensive calculation of the flatness index: take a weighted average of the obtained sum of curvature, roughness index and uniformity index, assign different weights to different sub-indicators according to their importance, and then sum their products to obtain the flatness index.

[0040] In the above method, optionally, in step S5, dynamic adjustment, the position and direction of the path point are adjusted according to the path point and the flatness index.

[0041] The above method is optional, S5, the dynamic adjustment step is as follows:

[0042] S501 Calculate adjustment ratio: Calculate adjustment ratio according to given flatness index to determine the degree of path adjustment:

[0043] Adjustment ratio = flatness index / threshold,

[0044] The threshold value indicates the maximum allowable flatness value of the plane, which is 3mm or 4mm. If the value exceeds this value, the path needs to be adjusted. The adjustment ratio indicates the degree of path adjustment. The larger the ratio, the more adjustment is needed.

[0045] S502 Path adjustment: for each waypoint point in the original path, the position or direction of the point is adjusted while considering the adjustment ratio so that the path can better meet the flatness requirement, and the adjusted waypoint is added to the adjusted path adjusted_waypoints;

[0046] S503 obtains the adjusted path: outputs the adjusted path adjusted_waypoints as the final adjusted path for use in subsequent path execution.

[0047] It can be seen from the above technical solutions that, compared with the prior art, the present invention provides a path planning method and system for a uniform spraying robot, which has the following beneficial effects:

[0048] The present invention collects point cloud data through a depth camera and generates a three-dimensional model from the point cloud data. The three-dimensional point cloud model is preprocessed to obtain a wall surface model. Plane detection is performed according to the wall to detect the plane where the wall is located. Based on the plane information, a path planning algorithm based on curve generation is used to perform path planning. The present invention provides a path planning method for a uniform spraying robot, which improves spraying quality and efficiency, reduces paint waste, and protects worker safety. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on the provided drawings without paying creative work.

[0050] Figure 1 A flow chart of a path planning method for a uniform spraying robot provided by the present invention;

[0051] Figure 2 A flow chart of the point cloud model preprocessing steps provided by the present invention;

[0052] Figure 3 A flow chart of the plane detection steps provided by the present invention;

[0053] Figure 4 A flow chart of the path planning steps provided by the present invention;

[0054] Figure 5 A flow chart of the dynamic adjustment steps provided by the present invention. DETAILED DESCRIPTION

[0055] 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.

[0056] In this application, relational terms such as first and second, etc. are used only to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. The terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of more restrictions, the elements defined by the sentence "comprise one..." do not exclude the existence of other identical elements in the process, method, article or device including the elements.

[0057] Reference Figure 1 As shown, the present invention discloses a path planning method for a uniform spraying robot, comprising the following steps:

[0058] S1. Obtaining a three-dimensional point cloud model: using a depth camera to collect data, and converting the collected data into a three-dimensional point cloud model;

[0059] S2, point cloud model preprocessing: preprocess the obtained three-dimensional point cloud model to obtain the wall surface;

[0060] S3, plane detection: Based on the plane fitting algorithm of the point cloud, the plane where the wall is located is detected, and the flatness of the wall surface is determined according to the flatness of the wall surface to determine whether the flatness of the wall surface is within a threshold, the wall plane information is obtained, and the flat wall area is distinguished;

[0061] S4, path planning: Based on the wall plane information detected by S3, a path planning algorithm based on curve generation is constructed, and the path planning is adjusted according to the change of the flatness of the wall;

[0062] S5. Dynamic adjustment: During the spraying process, the flatness of the wall is monitored in real time, and a feedback control algorithm is used to dynamically adjust the path planning based on the real-time monitoring results.

[0063] Furthermore, S1 obtains a three-dimensional point cloud model, uses a depth camera to collect data, obtains a depth image or a color image through an infrared camera, and converts the depth image or the color image into three-dimensional coordinate points according to the focal length, principal point coordinates, distortion coefficient, and rotation matrix and translation vector describing the imaging model and posture of the camera to obtain a three-dimensional point cloud model P.

[0064] Furthermore, in S2, the point cloud model preprocessing step:

[0065] The obtained 3D point cloud model is preprocessed to obtain a 3D point cloud model processed_cloud;

[0066] filtered_cloud=preprocess(input_cloud),

[0067] Input: 3D point cloud model P,

[0068] Output: processed 3D point cloud model processed_cloud.

[0069] Further, refer to Figure 2 As shown in S2, the point cloud model preprocessing steps are as follows:

[0070] S201 denoising: selecting a local outlier factor algorithm based on neighborhood points to perform denoising on the three-dimensional point cloud model P;

[0071] S202 filtering processing: according to the application scenario and requirements, the denoised three-dimensional point cloud model P is subjected to moving average filtering; specifically, given a signal sequence x[n] containing N samples, where n is the index of the sample from 0 to N-1, the moving average filtering filters the denoised three-dimensional point cloud model P by sliding a window of a fixed length of M on the signal sequence and calculating the average value of the samples in the window;

[0072] S203 surface reconstruction: select the reconstruction method of surface fitting, find a corresponding surface patch for each point in the filtered three-dimensional point cloud model P in S202, so that the distance between the point and the surface patch is minimized, then splice all the surface patches into a complete surface model, perform surface reconstruction on the filtered three-dimensional point cloud model P_filtered, and obtain the reconstructed wall surface model surface_model;

[0073] S204 returns the processed three-dimensional point cloud model: returns the processed three-dimensional point cloud model processed_cloud, which contains data after denoising, filtering and surface reconstruction, for subsequent path planning and spraying operations.

[0074] Furthermore, S3, the plane detection steps are as follows:

[0075] S301 uses the RANSAC plane fitting algorithm to detect plane D from the preprocessed 3D point cloud model P and expresses plane D as:

[0076] D: N·(PQ)=0;

[0077] Where N is the normal vector of plane D, and Q is any point on plane D;

[0078] S302: Substitute the points in the fitted plane D into the equation to confirm the fitting degree of the plane: Substitute N and Q into the plane equation D: N·(PQ)=0 to verify whether the points in the processed three-dimensional point cloud model satisfy the obtained plane equation, thereby confirming the fitting degree of the plane;

[0079] S303: taking the point set satisfying the plane equation as the point set in the detected plane D;

[0080] S304 outputs the detected plane D, which includes a normal vector N and a point Q on the plane.

[0081] Furthermore, in step S4, path planning, the path generation method can be expressed as follows according to the detected plane information:

[0082] waypoints=generate_waypoints(plane_model,wall_smoothness),

[0083] Further, refer to Figure 4 As shown, S4, the path planning steps are as follows:

[0084] S401, dividing the wall surface to obtain different plane areas;

[0085] S402, performing curvature analysis on each plane region, calculating its flatness, and selecting a curve type with the highest flatness among various curve types, including but not limited to polynomial curves and double arc segment curves; for each curve type, using the least square method, solving the best curve parameters so that the error between the curve and the plane region is minimized; for each curve, calculating the connection point and tangent direction with the adjacent curve to ensure the continuity and smoothness of the curve;

[0086] S403, determining the boundary conditions of each plane region according to the curve type, including but not limited to position, velocity, and acceleration;

[0087] S404. Use the undetermined coefficient method to solve the correlation coefficient of the curve; connect the curves of each plane area to form a complete path.

[0088] Furthermore, in S4, the path planning step, a set of path points is generated according to the plane model and the wall flatness to guide the movement of the spray robot. The path point is a two-dimensional array, and each row represents the coordinates of a point. The plane model is a surface equation that describes the shape and position of the plane. The wall flatness is a numerical value that represents the curvature and degree of change of the plane.

[0089] Furthermore, optionally, in S4, the path planning step, an index of wall flatness is introduced as a constraint condition. Flatness is represented as an index to measure the flatness of a plane. The position and shape of the plane are used as parameters. The flatness calculation method includes:

[0090] Calculate curvature: For each point p in the processed 3D point cloud model processed_cloud, calculate its curvature value and add up all curvature values ​​to get the total curvature;

[0091] Analyze surface roughness: For each point p in the processed 3D point cloud model processed_cloud, consider the change in normal vector or local density of points in its neighborhood, and accumulate the roughness index according to the change;

[0092] Analyze the uniformity of point distribution: For each point p in the processed 3D point cloud model processed_cloud, calculate the distribution density or distance distribution of the surrounding points, and accumulate the uniformity index according to the density or distribution;

[0093] Comprehensive calculation of the flatness index: take a weighted average of the obtained sum of curvature, roughness index and uniformity index, assign different weights to different sub-indicators according to their importance, and then sum their products to obtain the flatness index.

[0094] Furthermore, in S5, the dynamic adjustment step, the path adjustment according to the flatness index can be expressed as:

[0095] adjusted_waypoints=adjust_waypoints(waypoints, smoothness_factor);

[0096] Call the adjust_waypoints() function to adjust the waypoints. According to the waypoints and flatness indicators, adjust the position and direction of the waypoints to make the path smoother and more reasonable.

[0097] Further, refer to Figure 5 As shown, S5, the dynamic adjustment steps are as follows:

[0098] S501 Calculate adjustment ratio: Calculate adjustment ratio according to given flatness index to determine the degree of path adjustment:

[0099] Adjustment ratio = flatness index / threshold,

[0100] The threshold value indicates the maximum allowable flatness value of the plane, which is 3mm or 4mm. If the value exceeds this value, the path needs to be adjusted. The adjustment ratio indicates the degree of path adjustment. The larger the ratio, the more adjustment is needed.

[0101] S502 Path adjustment: for each waypoint point in the original path, the position or direction of the point is adjusted while considering the adjustment ratio so that the path can better meet the flatness requirement, and the adjusted waypoint is added to the adjusted path adjusted_waypoints;

[0102] S503 obtains the adjusted path: outputs the adjusted path adjusted_waypoints as the final adjusted path for use in subsequent path execution.

[0103] Each embodiment in this specification is described in a progressive manner, and the same or similar parts between the embodiments can refer to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the system or system embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can refer to the partial description of the method embodiment. The system and system embodiments described above are merely schematic, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of this embodiment. Ordinary technicians in this field can understand and implement it without creative work.

[0104] The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A path planning method for a uniform spraying robot, It is characterized in that The following steps are involved: S1. Obtaining a three-dimensional point cloud model: using a depth camera to collect data, and converting the collected data into a three-dimensional point cloud model; S2, point cloud model preprocessing: preprocess the obtained three-dimensional point cloud model to obtain the wall surface; S3, plane detection: Based on the plane fitting algorithm of the point cloud, the plane where the wall is located is detected, and the flatness of the wall surface is determined according to the flatness of the wall surface to determine whether the flatness of the wall surface is within a threshold, the wall plane information is obtained, and the flat wall area is distinguished; S4, path planning: Based on the wall plane information detected by S3, a path planning algorithm based on curve generation is constructed, and the path planning is adjusted according to the change of the flatness of the wall; In the S4 path planning step, the wall flatness index is introduced as a constraint condition. Flatness is expressed as an index to measure the flatness of a plane. The position and shape of the plane are used as parameters. The flatness calculation method includes: Calculate curvature: For each point p in the processed 3D point cloud model processed_cloud, calculate its curvature value and add up all curvature values ​​to get the total curvature; Analyze surface roughness: For each point p in the processed 3D point cloud model processed_cloud, consider the change in normal vector or local density of points in its neighborhood, and accumulate the roughness index according to the change; Analyze the uniformity of point distribution: For each point p in the processed 3D point cloud model processed_cloud, calculate the distribution density or distance distribution of the surrounding points, and accumulate the uniformity index according to the density or distribution; Comprehensive calculation of flatness index: Take the weighted average of the obtained curvature sum, roughness index and uniformity index, assign different weights to different sub-indicators according to their importance, and then sum their products to obtain the flatness index; S5. Dynamic adjustment: During the spraying process, the flatness of the wall is monitored in real time, and a feedback control algorithm is used to dynamically adjust the path planning based on the real-time monitoring results.

2. A path planning method for a uniform spraying robot according to claim 1, Features: S1 obtains a three-dimensional point cloud model, uses a depth camera to collect data, obtains a depth image or a color image through an infrared camera, and converts the depth image or the color image into three-dimensional coordinate points according to the focal length, principal point coordinates, distortion coefficient, and rotation matrix and translation vector describing the imaging model and posture of the camera to obtain a three-dimensional point cloud model P.

3. A path planning method for a uniform spraying robot according to claim 2, It is characterized in that S2. The point cloud model preprocessing steps are as follows: S201 denoising: selecting a local outlier factor algorithm based on neighborhood points to perform denoising on the three-dimensional point cloud model P; S202 filtering processing: according to the application scenario and requirements, the denoised three-dimensional point cloud model P is subjected to moving average filtering; specifically, given a signal sequence x[n] containing N samples, where n is the index of the sample from 0 to N-1, the moving average filtering filters the denoised three-dimensional point cloud model P by sliding a window of a fixed length of M on the signal sequence and calculating the average value of the samples in the window; S203 Surface reconstruction: Select the reconstruction method of surface fitting, find a corresponding surface patch for each point in the filtered three-dimensional point cloud model P in S202, so that the distance between the point and the surface patch is minimized, then splice all the surface patches into a complete surface model, perform surface reconstruction on the filtered three-dimensional point cloud model P_filtered, and obtain the reconstructed wall surface model surface_model; S204 returns the processed three-dimensional point cloud model: returns the processed three-dimensional point cloud model processed_cloud, which contains data after denoising, filtering and surface reconstruction, for subsequent path planning and spraying operations.

4. A path planning method for a uniform spraying robot according to claim 3, It is characterized in that The specific contents of S3 are: S301 uses the RANSAC plane fitting algorithm to detect plane D from the preprocessed 3D point cloud model P, and expresses plane D as: D: N·(PQ)=0; Where N is the normal vector of plane D, and Q is any point on plane D; S302 Substituting the points in the fitted plane D into the equation to confirm the fitting degree of the plane: Substituting N and Q into the plane equation D: N·(PQ)=0, verifying whether the points in the processed three-dimensional point cloud model satisfy the obtained plane equation, and further confirming the fitting degree of the plane; S303 taking the point set satisfying the plane equation as the point set in the detected plane D; S304 outputs the detected plane D, which includes the normal vector N and a point Q on the plane.

5. A path planning method for a uniform spraying robot according to claim 1, It is characterized in that S4. The path planning steps are as follows: S401, dividing the wall surface to obtain different plane areas; S402, performing curvature analysis on each plane region, calculating its flatness, and selecting a curve type with the largest flatness among various curve types, including but not limited to a polynomial curve and a double arc segment curve; For each curve type, the least square method is used to solve the optimal curve parameters so that the error between the curve and the plane area is minimized; for each curve, the connection point and tangent direction with the adjacent curve are calculated to ensure the continuity and smoothness of the curve; S403, determining the boundary conditions of each plane region according to the curve type, including but not limited to position, velocity, and acceleration; S404. Use the undetermined coefficient method to solve the correlation coefficient of the curve; connect the curves of each plane area to form a complete path.

6. A path planning method for a uniform spraying robot according to claim 5, It is characterized in that In the path planning step S4, a set of path points is generated according to the plane model and the flatness of the wall to guide the movement of the spraying robot.

7. A path planning method for a uniform spraying robot according to claim 1, It is characterized in that S5, in the dynamic adjustment step, the position and direction of the path point are adjusted according to the path point and the flatness index.

8. A path planning method for a uniform spraying robot according to claim 1, It is characterized in that S5. The dynamic adjustment steps are as follows: S501 Calculate adjustment ratio: Calculate adjustment ratio according to given flatness index to determine the degree of path adjustment: Adjustment ratio = flatness index / threshold, The threshold value indicates the maximum allowable flatness value of the plane, which is 3mm or 4mm. If the value exceeds this value, the path needs to be adjusted. The adjustment ratio indicates the degree of path adjustment. The larger the ratio, the more adjustment is needed. S502 Path adjustment: for each waypoint point in the original path, the position or direction of the point is adjusted while considering the adjustment ratio so that the path can better meet the flatness requirement, and the adjusted waypoint is added to the adjusted path adjusted_waypoints; S503 Obtaining the adjusted path: outputting the adjusted path adjusted_waypoints as the final adjusted path for use in subsequent path execution.

Images