Spraying robot trajectory planning method and system based on depth camera scanning

Abstract

The invention belongs to the technical field of robots, and particularly relates to a spraying robot trajectory planning method and system based on depth camera scanning, and the method comprises the steps: scanning three-dimensional point cloud data of a wall surface at different positions in real time, converting the three-dimensional point cloud data of the wall surface scanned at different positions into a mechanical arm base coordinate system, and carrying out the splicing, a wall surface splicing point cloud is obtained; based on the normal vector and curvature information of the wall surface splicing point cloud, carrying out three-dimensional visual point cloud segmentation to obtain a segmentation point cloud; projecting the segmentation point cloud to a wall surface, and performing wall surface feature extraction to obtain wall surface edge information; boundary constraints in different directions are obtained on the basis of wall surface edge information, a spraying height sequence is obtained by combining spraying intervals, a spraying waypoint sequence under a wall surface coordinate system is obtained on the basis of the spraying height sequence, the spraying waypoint sequence is converted into a space spraying waypoint sequence under a mechanical arm base coordinate system, and a spraying track of the spraying robot is obtained.

Description

Technical Field

[0001] The present invention belongs to the field of robot technology, and in particular relates to a spraying robot trajectory planning method and system based on depth camera scanning. Background Art

[0002] At present, there are two main types of perception and trajectory planning methods for building spraying robots, including spatial voxelization processing and direct perception and planning using segmented point cloud data. The former is to use optimization to plan trajectories, which is to divide the perceived space into voxelized discrete areas, convert the area to be sprayed into a series of spraying waypoints through the spraying model, and then combine all the waypoints into a point set, in which an optimization algorithm is used to find an optimal path, but this method will lead to a decrease in spraying accuracy; the latter is to use the ox-ploughing method to plan the coverage trajectory from the perspective of process, which can reduce the large amount of calculations required for optimization and the accuracy loss caused by voxelization, but the building spraying robot Pictobot involved uses a depth camera to obtain the point cloud information of the wall, and then uses Boustrophedon for trajectory planning without using the prior features of the wall to extract the wall. The point cloud data obtained by scanning is segmented by a random sampling consistency method, and then the trajectory planning based on the wall data is difficult when facing two walls with the same normal vector and little difference in depth; the accuracy and practicality of the existing perception and planning algorithms of spraying robots are not high. Summary of the invention

[0003] The present invention provides a method and system for spraying robot trajectory planning based on depth camera scanning. By extracting and processing wall point cloud information and projecting it, the three-dimensional spraying trajectory planning problem is converted into a two-dimensional problem, so that the accuracy information of the point cloud is retained to the greatest extent, the accuracy of wall perception is improved, the spraying waypoints of the spraying robot are optimized, the calculation time is reduced, and full coverage of the spraying robot path planning is achieved.

[0004] A painting robot trajectory planning method based on depth camera scanning, comprising:

[0005] Scan the 3D point cloud data of the wall at different positions in real time, and convert the 3D point cloud data of the wall scanned at different positions into the coordinate system of the robot base for splicing to obtain the wall splicing point cloud;

[0006] Based on the normal vector and curvature information of the wall splicing point cloud, three-dimensional visual point cloud segmentation is performed to obtain the segmented point cloud;

[0007] Project the segmented point cloud onto the wall, extract wall features, and obtain wall edge information;

[0008] Based on the wall edge information, the boundary constraints in different directions are obtained, and the spraying height sequence is obtained in combination with the spraying spacing. Based on the spraying height sequence, the spraying waypoint sequence in the wall coordinate system is obtained, which is converted into a spatial spraying waypoint sequence in the robotic arm base coordinate system to obtain the spraying trajectory of the spray robot.

[0009] By extracting and processing the wall point cloud information and projecting it, the three-dimensional spraying trajectory planning problem is converted into a two-dimensional problem, so that the accuracy information of the point cloud is retained to the greatest extent, the accuracy of wall perception is improved, the spraying waypoints of the spray robot are optimized, the calculation time is reduced, and full coverage of the spray robot path planning is achieved.

[0010] Furthermore, the real-time scanning of the three-dimensional point cloud data of the wall at different positions and the point cloud splicing based on the posture sequence to obtain the wall splicing point cloud in the robot arm base coordinate system include:

[0011] Based on any scanning position on the wall, define the position and posture of the scanning position in the robot base coordinate system, and control the robot arm to move to the scanning position, and obtain multiple three-dimensional point cloud data of the wall in the camera coordinate system according to the scanning sequence of the position and posture;

[0012] The three-dimensional point cloud data of multiple walls in the camera coordinate system are converted into three-dimensional point cloud data in the robot base coordinate system, and the three-dimensional point cloud data of multiple walls are spliced ​​based on the pose sequence to obtain the wall splicing point cloud in the robot base coordinate system.

[0013] Furthermore, the three-dimensional visual point cloud segmentation is performed based on the normal vector and curvature information of the wall splicing point cloud to obtain the segmented point cloud, including:

[0014] Traverse all the wall splicing point clouds and calculate the normal vector and curvature information of each point;

[0015] Based on the normal vector and curvature information corresponding to each point, the region growing algorithm is used to segment the point cloud to obtain the segmented point cloud.

[0016] Furthermore, the point cloud segmentation is performed using a region growing algorithm based on the normal vector and curvature information corresponding to each point to obtain a segmented point cloud, including:

[0017] Select all points whose curvature is less than the preset curvature constant as seed points, and add the seed points to the seed set;

[0018] Based on each point in the seed set, construct the corresponding growth point set;

[0019] Based on each point in the growing point set, find all points adjacent to the point to construct an adjacent point set;

[0020] Based on each point in the adjacent point set, the current point is moved out of the adjacent point set, and the difference between the normal vectors of the current point and the adjacent points in the adjacent point set is calculated. When the difference between the normal vectors is less than the preset difference, the current point is added to the growth point set to update the point set.

[0021] When the merging conditions are met between two growing point sets, the point sets are merged;

[0022] Until all points in the spliced ​​point cloud are assigned, the segmented point cloud is obtained.

[0023] By segmenting the point cloud through the region growing algorithm, we can better distinguish the location of curvature changes, so that walls with the same normal vector and similar depth can be segmented.

[0024] Furthermore, the segmented point cloud is projected onto the wall surface, and wall feature extraction is performed to obtain wall edge information, including:

[0025] Based on any part of the segmented point cloud, define the coordinates of each segmented point cloud, and construct the data matrix of the segmented point cloud;

[0026] The principal component analysis method is used to solve the eigenvectors and eigenvalues ​​of the data matrix, and the eigenvector with the smallest eigenvalue is selected as the normal vector direction of the wall;

[0027] Based on the normal vector direction of the wall and combined with prior information, the representation of the X-axis and Y-axis in the wall coordinate system in the robot coordinate system are determined respectively;

[0028] The centroid of the wall point cloud is used as the origin of the wall coordinate system, and each point in the segmented point cloud is projected into the wall coordinate system using coordinate transformation to obtain the projection point in the wall coordinate system;

[0029] Based on the projection points, the Alpha-Shape algorithm is used to extract the boundary points of the wall and obtain the wall edge information.

[0030] By introducing the principal component analysis method, the accuracy loss caused by voxelization can be avoided, and the wall point cloud can be projected onto the plane, so that the spraying trajectory planning problem can be transformed into a full coverage path problem on the plane. The accuracy of the point cloud data can be retained to the greatest extent, and the Alpha-Shape algorithm is used to extract and smooth the outer edges of the wall to determine the constraints of the full coverage path planning.

[0031] Furthermore, the boundary constraints in different directions are obtained based on the wall edge information, the spraying height sequence is obtained in combination with the spraying spacing, and the spraying waypoint sequence in the wall coordinate system is obtained based on the spraying height sequence, which is converted into a spatial spraying waypoint sequence in the robot arm base coordinate system to obtain the spraying trajectory of the spraying robot, including:

[0032] Based on the wall edge information set, obtain the boundary constraints on the wall in the X-axis and Y-axis directions;

[0033] Based on the process spraying parameters, the spacing between spraying lines is obtained;

[0034] Combining the spacing between spraying rows and the wall boundary constraints, the spraying height sequence is obtained;

[0035] Based on the spraying line segments in the spraying height sequence, the intersection sequence formed by the spraying line segments and the wall boundary constraints is obtained in sequence;

[0036] Based on the intersection sequence and spraying conditions, the Boustrophedon algorithm is used to obtain the spraying waypoint sequence in the wall coordinate system;

[0037] The spraying waypoint sequence in the wall coordinate system is converted into a spatial spraying waypoint sequence in the robot arm base coordinate system to obtain the spraying trajectory of the spraying robot, which is used to achieve full coverage of the wall.

[0038] By converting the full coverage path on the plane into three-dimensional space, the trajectory planning of the spraying robot can be obtained.

[0039] Furthermore, the spraying conditions include:

[0040] When the spraying waypoint sequence number is an odd number, the spraying valve is opened for spraying;

[0041] When the spraying waypoint sequence number is an even number, the spraying valve is closed.

[0042] A system for a spraying robot trajectory planning method based on depth camera scanning, comprising:

[0043] The point cloud acquisition and processing module is used to scan the three-dimensional point cloud data of the wall at different positions in real time, and convert the three-dimensional point cloud data of the wall scanned at different positions into the coordinate system of the robot base for splicing to obtain the wall splicing point cloud;

[0044] The point cloud segmentation module is used to perform three-dimensional visual point cloud segmentation based on the normal vector and curvature information of the wall splicing point cloud to obtain the segmented point cloud;

[0045] A feature extraction module is used to extract wall features based on the segmented point cloud to obtain wall edge information;

[0046] The trajectory planning module is used to obtain boundary constraints in different directions based on the wall edge information, obtain the spraying height sequence in combination with the spraying spacing, and obtain the spraying waypoint sequence in the wall coordinate system based on the spraying height sequence, and convert it into a spatial spraying waypoint sequence in the robot arm base coordinate system to obtain the spraying trajectory of the spray robot.

[0047] A device for a spraying robot trajectory planning method based on depth camera scanning, comprising:

[0048] Spraying robot;

[0049] A lifting mechanism, which is mounted on the spraying robot;

[0050] A wall sensing device comprises a mechanical arm and a depth camera; the mechanical arm is mounted on the lifting mechanism; the depth camera is mounted at the end of the mechanical arm and is used to scan a large range of walls under the action of the lifting mechanism and the mechanical arm.

[0051] The beneficial effects of the present invention are:

[0052] The present invention uses a depth camera to accurately extract wall point cloud data information, and uses a regional growing algorithm to segment the point cloud, which can better distinguish the location of curvature change, so that walls with the same normal vector and similar depth can be segmented; by introducing the principal component analysis method and the normal vector constraint method, the wall point cloud is projected to a plane, so that the three-dimensional spraying trajectory planning problem is converted into a two-dimensional problem, so as to avoid the accuracy loss caused by voxelization, and thus retain the accuracy of the point cloud data to the greatest extent; by using the Alpha-Shape algorithm to extract and smooth the outer edge of the wall, the constraints of the full coverage path planning can be determined; and then by re-converting the full coverage path on the projection plane into three-dimensional space, the trajectory planning of the spraying robot based on the depth camera scanning is obtained, the accuracy of wall perception is improved, the spraying waypoints of the spraying robot are optimized, the calculation time is reduced, and the full coverage spraying robot path planning is realized. BRIEF DESCRIPTION OF THE DRAWINGS

[0053] Figure 1 is a flow chart of the present invention;

[0054] Figure 2 is a schematic diagram of the region growing algorithm in the present invention;

[0055] Figure 3 This is a schematic diagram of the wall splicing point cloud;

[0056] Figure 4 Schematic diagram of segmented point cloud;

[0057] Figure 5 This is a schematic diagram of the spraying trajectory of the spraying robot;

[0058] Figure 6 It is a schematic diagram of the structure of the system in the present invention. DETAILED DESCRIPTION

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

[0060] It should be noted that various aspects of the embodiments within the scope of the appended claims are described below. It should be apparent that the aspects described herein can be embodied in a wide variety of forms, and any specific structure and / or function described herein is merely illustrative. Based on the present disclosure, it should be understood by those skilled in the art that an aspect described herein can be implemented independently of any other aspect, and two or more of these aspects can be combined in various ways. For example, any number of aspects described herein can be used to implement the device and / or practice the method. In addition, other structures and / or functionalities other than one or more of the aspects described herein can be used to implement this device and / or practice this method.

[0061] In addition, in the following description, specific details are provided to facilitate a thorough understanding of the examples, and a person of ordinary skill in the art can understand the specific meanings of the above terms in this application under specific circumstances.

[0062] Example 1

[0063] Figure 1 The method is a spraying robot trajectory planning method based on depth camera scanning. By extracting and processing the wall point cloud information and projecting it, the three-dimensional spraying trajectory planning problem is converted into a two-dimensional problem, so that the accuracy of the point cloud information is retained to the greatest extent, the accuracy of wall perception is improved, the spraying waypoints of the spraying robot are optimized, the calculation time is reduced, and the full coverage of the spraying robot path planning is realized. The specific steps include the following:

[0064] S1: Scan the three-dimensional point cloud data of the wall at different positions in real time, and convert the three-dimensional point cloud data of the wall scanned at different positions into the coordinate system of the robot base for splicing to obtain the wall splicing point cloud;

[0065] S11: Based on any scanning position on the wall Define the position of the scan position in the robot base coordinate system The robot arm is controlled to move to the scanning position, and multiple three-dimensional point cloud data of the wall in the camera coordinate system are obtained according to the posture sequence scanning;

[0066] Among them, the wall scanning position is defined as Control the robot arm to scan the scanning position and obtain

[0067] The point cloud expression in the camera coordinate system is:

[0068]

[0069] In the formula, represents the i-th point cloud in the camera coordinate system; p j Indicates the position coordinates of the jth point cloud, x j Indicates the position coordinate of the jth point cloud on the X axis, y j Indicates the position coordinate of the jth point cloud on the Y axis, z j Represents the jth point cloud

[0070] Position coordinate on the Z axis; N i Represents the number of points in the i-th point cloud;

[0071] S12: converting the three-dimensional point cloud data of multiple walls in the camera coordinate system into three-dimensional point cloud data in the robot base coordinate system, and splicing the three-dimensional point cloud data of multiple walls based on the pose sequence to obtain a spliced ​​point cloud of the wall in the robot base coordinate system;

[0072] Due to the position between the end position of the robot arm and the position between the hand and the eye It is known that the 3D point cloud of the wall is converted into the base of the robot arm

[0073] The expression of the three-dimensional point cloud in the coordinate system is:

[0074]

[0075] In the formula, Represents the i-th point cloud coordinates in the robot arm coordinate system; Indicates the end position of the robot arm; Represents the hand-eye pose in the wall coordinate system;

[0076] After converting multiple point clouds scanned at different positions into the robot arm coordinate system, the point clouds are stitched together to obtain the wall stitching point cloud, which is expressed as:

[0077]

[0078] Where P nase Represents the wall splicing point cloud in the robot arm coordinate system; M represents the number of scanned point clouds;

[0079] S2: Based on the normal vector and curvature information of the wall splicing point cloud, perform 3D visual point cloud segmentation to obtain the segmented point cloud;

[0080] S21: traverse all wall splicing point clouds and calculate the normal vector and curvature information of each point;

[0081] The point cloud information P of the normal vector and curvature of each point in the spliced ​​point cloud is expressed as:

[0082] P={(p t ,n t ,κ t |p t ∈P base};

[0083] Where P represents the wall splicing point cloud, its normal vector and curvature point cloud information set; p t represents the tth point cloud; n t Represents point p t The normal vector corresponding to t Represents point p t The corresponding curvature at

[0084] S22: Based on the normal vector and curvature information corresponding to each point, a region growing algorithm is used to segment the point cloud to obtain a segmented point cloud;

[0085] S221: Select all the curvatures that are less than the preset curvature range and within the preset curvature constant, i.e., κ t <δ κ The point is taken as the seed point and added to the seed set Seed;

[0086] S222: Based on each point s in the seed set Seed t , construct the corresponding growth point set Cluster t ={s t};

[0087] S223: Cluster based on growth point set t Each point s in t , find all the points s t Adjacent points construct neighboring point set Neighbor t ;

[0088] S224: Based on the neighboring point set Neighbor t Each point s in v , the current point s v Remove the neighboring point set Neighbor t , and calculate the current point s v and the neighboring point set Neighbor t The difference in normal vectors When the condition is met, the current point s v Add to the growth point set Cluster t Update the point set in

[0089] S2241: When the selected point sv and the neighboring point set Neighbor t The difference between the normal vectors of adjacent points in Less than the preset difference δ n ,Right now When , we will select point s v Add to the growth point set Cluster t middle;

[0090] S2242: When the selected point s v and the neighboring point set Neighbor t The difference between the normal vectors of adjacent points in Not less than the preset difference δ n ,Right now When , we will select point s v Do not add the growth point set Cluster t middle;

[0091] S225: When two growth point sets meet the merging condition, the point sets are merged;

[0092] Specifically, when there is a selected point s v , which satisfies s v ∈Cluster m 、s v ∈Cluster n , and m≠n, the growth point set Cluster m and growth point set Cluster n Merge and merge the adjacent point set Neighbor m Neighbor n ;

[0093] S226: until all points in the spliced ​​point cloud are allocated, a segmented point cloud is obtained;

[0094] Specifically, when every point in the spliced ​​point cloud P is added to the growing point set, the algorithm operation is terminated; when there are points that are not added to the growing point set, the adjacent point set is reconstructed and recalculated.

[0095] Figure 2 The flowchart of the region growing algorithm is shown. By segmenting the point cloud through the region growing algorithm, the curvature change position can be better distinguished, so that the wall surfaces with the same normal vector and similar depth can be segmented.

[0096] S3: Project the segmented point cloud onto the wall, extract wall features, and obtain wall edge information;

[0097] S31: based on the arbitrary partial segmentation point cloud, defining the coordinates of each segmentation point cloud, and constructing a data matrix of the partial segmentation point cloud;

[0098] Clustering point clouds based on arbitrary parts t , let the coordinates of any point be p t =[x t ,y t ,z t ] T , then the data matrix U of all point clouds in this part is expressed as:

[0099]

[0100] Where U represents the data matrix of all point clouds in the selected part; Indicates the number of point clouds in the selected part;

[0101] S32: using the principal component analysis method to solve the eigenvectors and eigenvalues ​​of the data matrix, and selecting the eigenvector with the smallest eigenvalue as the normal vector direction of the wall;

[0102] Construct eigenvectors based on principal component analysis to solve the expression:

[0103] UU T W = -λW;

[0104] Where U T represents the transpose of the data matrix; W represents the eigenvector, that is, W = [w1, w2, w3]; λ represents the eigenvalue; the eigenvalues ​​corresponding to the eigenvector are λ1, λ2, and λ3, and λ1>λ2>λ3, that is, the eigenvector w3 is the normal vector direction of the wall.

[0105] Cluster based on segmented point cloud t For any point in the in, To segment the point cloud Cluster t The principal component direction with the smallest eigenvalue.

[0106] S33: Based on the normal vector direction of the wall and in combination with prior information, respectively determine the representation of the X-axis and the Y-axis in the wall coordinate system in the robot arm coordinate system;

[0107] The X-axis transformation in the wall coordinate system is expressed in the robot coordinate system as follows:

[0108]

[0109] In the formula, Indicates the X-axis in the robot coordinate system; Indicates the vertical direction, that is

[0110] The Y-axis transformation in the wall coordinate system is expressed in the robot coordinate system as follows:

[0111]

[0112] In the formula, Indicates the Y-axis in the robot coordinate system;

[0113] S34: taking the centroid of the wall point cloud as the origin of the wall coordinate system, and projecting each point in the segmented point cloud into the wall coordinate system by using coordinate transformation to obtain a projection point in the wall coordinate system;

[0114] Define the point cloud of the projection point as P t ′, its calculation expression is:

[0115]

[0116] Where P t ′ represents the point cloud after the projection of the t-th segmentation point cloud; L t Represents the normal vector corresponding to the t-th segmentation point cloud, that is, L t =[n x ,n y ,n z ]; Project() represents the function of converting three-dimensional coordinates to two-dimensional coordinates, that is, Project([x1,x2,x3] T )=[x1,x2] T ;

[0117] S35: Based on the projection points, the Alpha-Shape algorithm is used to extract the boundary points of the wall to obtain the wall edge information. The Alpha-Shape algorithm is used to extract the boundary points of the wall to obtain the wall edge information set Info, that is:

[0118] Info = {{vertex t ,edge t}|t∈[1,N cluster ]};

[0119] Where Info represents the wall edge information set; vertex t Represents the vertex of the polygon edge; edge t The edge representing the edge of the polygon; N cluster Indicates the number of segmented point cloud areas;

[0120] By introducing the principal component analysis method, the accuracy loss caused by voxelization can be avoided, and the wall point cloud can be projected onto a plane, so that the spraying trajectory planning problem can be transformed into a full coverage path problem on the plane, and the accuracy of the point cloud data can be retained to the greatest extent. The Alpha-Shape algorithm is used to extract and smooth the outer edge of the wall, and the constraints of the full coverage path planning can be determined;

[0121] S4: Based on the wall edge information, boundary constraints in different directions are obtained, and the spraying height sequence is obtained in combination with the spraying spacing. Based on the spraying height sequence, the spraying waypoint sequence in the wall coordinate system is obtained, which is converted into a spatial spraying waypoint sequence in the robot arm base coordinate system to obtain the spraying trajectory of the spraying robot;

[0122] S41: Based on the wall edge information set, obtain boundary constraints on the wall in the X-axis and Y-axis directions;

[0123] Based on the wall edge information edge t , the boundary constraint on the wall in the X-axis direction is obtained as:

[0124]

[0125] In the formula, Indicates the upper bound constraint in the X-axis direction; Indicates the lower bound constraint in the X-axis direction; Indicates the position of the cth point cloud in the segmented point cloud in the X-axis direction; (x c ,y c ) represents the coordinates of the cth point cloud;

[0126] The boundary constraint on the wall in the Y-axis direction is:

[0127]

[0128] In the formula, Indicates the upper bound constraint in the Y-axis direction; Indicates the lower bound constraint in the Y-axis direction; The position of the cth point cloud in the segmented point cloud in the Y-axis direction;

[0129] S42: based on the process spraying parameters, obtaining the spraying line spacing Δy;

[0130] S43: combining the spraying row spacing and the wall boundary constraint to obtain the spraying height sequence;

[0131] Among them, the expression of the spraying height sequence is:

[0132]

[0133] In the formula, Represents the spraying height sequence; y k represents the kth spraying height; y0 represents the initial spraying height; y N Indicates the last spraying height;

[0134] S44: based on the spraying line segments in the spraying height sequence, sequentially obtaining a sequence of intersection points formed by the spraying line segments and the wall boundary constraints;

[0135] Based on any spraying height y k , get the corresponding spray line segment Obtain the spray line segment and wall edge information edge t The intersection sequence

[0136] Intersection point sequence corresponding to the initial spraying height Its expression is:

[0137]

[0138] In the formula, Indicates the initial spraying height y0 and the wall edge information edge t The number of intersections, and the intersections are arranged in order from small to large.

[0139] S45: Based on the intersection sequence and spraying conditions, the Boustrophedon algorithm is used to obtain the spraying waypoint sequence in the wall coordinate system;

[0140] Based on the intersection sequence corresponding to other spray line segments When the sequence points are arranged from small to large on the X-axis, the point order is arranged from large to small on the X-axis; conversely, when the sequence points are arranged from large to small on the X-axis, the point order is arranged from small to large on the X-axis; then the spray point sequence obtained from all spray heights is combined in sequence to obtain the spray waypoint sequence Spray t .

[0141] Among them, when the spraying waypoint sequence number is an odd number, the spraying valve is opened for spraying; when the spraying waypoint sequence number is an even number, the spraying valve is closed.

[0142] S46: converting the spraying waypoint sequence in the wall coordinate system into a spatial spraying waypoint sequence in the robot arm base coordinate system to obtain a spraying trajectory of the spraying robot, which is used to achieve full wall coverage spraying.

[0143] Example 2

[0144] Based on the same design concept, the present invention provides a spraying robot trajectory planning method based on depth camera scanning, including:

[0145] T1: Real-time scanning of the 3D point cloud data of the wall at different positions, and conversion of the 3D point cloud data of the wall scanned at different positions to the robot base coordinate system for splicing, to obtain the wall splicing point cloud P base ;

[0146] T11: Based on any scanning position on the wall Define the position of the scan position in the robot base coordinate system And control the robot arm to move to the scanning position According to the pose sequence scanning, multiple wall 3D point cloud data in the camera coordinate system are obtained

[0147] T12: Multiple wall 3D point cloud data in the camera coordinate system Converted into 3D point cloud data in the robot base coordinate system Based on the pose sequence, multiple wall 3D point cloud data are spliced ​​to obtain the wall splicing point cloud P in the robot arm base coordinate system. base ;

[0148] T2: Based on the normal vector and curvature information of the wall splicing point cloud, perform 3D visual point cloud segmentation to obtain the segmented point cloud;

[0149] T21: Traverse all wall stitching point clouds P base , calculate the point cloud information P of each point’s normal vector and curvature information;

[0150] T22: Based on the point cloud information P of the normal vector and curvature corresponding to each point, the region growing algorithm is used to segment the point cloud to obtain the segmented point cloud Cluster t ;

[0151] T221: Select all curvatures less than the preset curvature constant, i.e., κ t <δ κ The point is taken as the seed point and added to the seed set Seed;

[0152] T222: Yes Construct the corresponding growth point set Cluster t ={s t};

[0153] T223: Yes Find all clusters with growth points t The points adjacent to the point construct the neighboring point set Neighbor t ;

[0154] T224: Yes Set the current point s v Remove the neighboring point set Neighbor t, and calculate the current point s v and The difference between the normal vectors of adjacent points in When the condition is met, the current point s v Add to the growth point set Cluster t Update the point set in

[0155] T2241: When the point s is selected v and the neighboring point set Neighbor t The difference between the normal vectors of adjacent points in Less than the preset difference δ n ,Right now When , we will select point s v Add to the growth point set Cluster t middle;

[0156] T2242: When the point s is selected v and the neighboring point set Neighbor t The difference between the normal vectors of adjacent points in Not less than the preset difference δ n ,Right now When , we will select point s v Do not add the growth point set Cluster t middle;

[0157] T225: When two growth point sets meet the merging conditions, the point sets are merged;

[0158] T2251: When Satisfaction v ∈Cluster m And s v ∈Cluster n , when m≠n, the growth point set Cluster m and growth point set Cluster n Merge and merge the adjacent point set Neighbor m Neighbor n ;

[0159] T226: until all points in the spliced ​​point cloud are assigned, the segmented point cloud is obtained;

[0160] T2261: Based on a point in any spliced ​​point cloud, if there exists a point sequence t such that the point belongs to any growing point set Cluster t , then the algorithm ends;

[0161] T2262: Based on a point in any spliced ​​point cloud, when there is no point sequence t that makes the point belong to any growing point set Cluster t, then return to execute T223;

[0162] T3: Project the segmented point cloud onto the wall and extract wall features to obtain wall edge information;

[0163] T31: Clustering point clouds based on arbitrary parts t , define the coordinates p of each segmentation point cloud t =[x t ,y t ,z t ] T , and construct the data matrix of this part of the segmented point cloud

[0164] T32: Use the principal component analysis method to solve the eigenvector W and eigenvalue λ of the data matrix U, and select the eigenvector w3 with the smallest eigenvalue as the normal vector direction of the wall

[0165] T33: Normal vector direction based on the wall Combined with the prior information, the representation of the X-axis and Y-axis in the wall coordinate system in the robot coordinate system is determined respectively;

[0166] T34: Take the centroid of the wall point cloud as the origin of the wall coordinate system, and project each point in the segmented point cloud into the wall coordinate system using coordinate transformation to obtain the projection point P′ in the wall coordinate system. t ;

[0167] T35: Based on the projection point P′ t , use the Alpha-Shape algorithm to extract the boundary points of the wall and obtain the wall edge information Info = {{vertex t ,edge t}|t∈[1,N cluster ]};

[0168] T4: Based on the wall edge information Info, boundary constraints in different directions are obtained, and the spraying height sequence is obtained in combination with the spraying spacing. Based on the spraying height sequence, the spraying waypoint sequence in the wall coordinate system is obtained, which is converted into a spatial spraying waypoint sequence in the robot arm base coordinate system to obtain the spraying trajectory of the spraying robot;

[0169] T41: Based on the wall edge information set edge t , get the boundary constraint of the X axis on the wall And the boundary constraints in the Y-axis direction

[0170] T42: Based on the process spraying parameters, the spacing between spraying lines Δy is obtained;

[0171] T43: Combine the spraying row spacing Δy and the wall boundary constraint to obtain the spraying height sequence

[0172] T44: Based on the spraying line segments in the spraying height sequence, obtain the intersection sequence formed by the spraying line segments and the wall boundary constraints in sequence

[0173] T45: Based on the intersection sequence and spraying conditions, the Boustrophedon algorithm is used to obtain the spraying waypoint sequence in the wall coordinate system. t ;

[0174] T451: When the spraying waypoint sequence number is an odd number, open the spraying valve to spray;

[0175] T452: When the spraying waypoint sequence number is an even number, the spraying valve is closed;

[0176] T46: Convert the spraying waypoint sequence in the wall coordinate system into the spatial spraying waypoint sequence in the robot base coordinate system to obtain the spraying trajectory of the spraying robot, which is used to achieve full coverage of the wall.

[0177] Example 3

[0178] Compared with Embodiments 1 and 2, in this embodiment, when working in a building spraying environment, a point cloud scan is performed on a complete wall surface including two windows according to a scanning sequence, and the scanning sequence is expressed as:

[0179]

[0180] In the formula, Scan represents the scanning sequence; represents the position and posture of the i-th scanning point in the robot base coordinate system, and T represents the homogeneous transformation matrix, that is, R represents the rotation matrix, f represents the translation vector; N scan Indicates the number of scan points.

[0181] Based on the position of the scanning point at the end of the robotic arm, the robotic arm is moved to the specified point to scan the point cloud, and the wall splicing point cloud P in the robotic arm base coordinate system is obtained by point cloud splicing. base , the schematic diagram of the spliced ​​point cloud is as follows Figure 3 As shown;

[0182] By preprocessing and segmenting the spliced ​​point cloud information, multiple segmented point cloud regions are obtained, namely:

[0183] C={Cluster t |t∈[1,N cluster ]};

[0184] In the formula, Clustert Represents the point cloud area; N cluster Indicates the number of wall areas after segmentation;

[0185] The wall information is extracted for each segmented area to obtain the wall edge information set, namely:

[0186] D={Wall r |r∈[1,n wall ]};

[0187] Where D represents the wall edge information set; Wall r Represents the edge point information set of the wall plane; N wall Indicates the number of walls segmented.

[0188] Since the present embodiment only includes one wall surface to be sprayed, a point cloud region is obtained by segmentation. Figure 4 Shown is a schematic diagram of the segmented point cloud.

[0189] Based on the spraying trajectory planning of the Boustrophedon algorithm, the spraying waypoint sequence is obtained as follows:

[0190] Spray = {WP e |e∈[1,N WayPoints ]};

[0191] Where Spray represents the spraying waypoint sequence; WP e Indicates the e-th spraying waypoint; N WayPoints Indicates the number of spraying waypoints;

[0192] Based on the obtained multiple groups of spraying waypoint sequences, the robotic arm is controlled to move according to the spraying waypoint sequences, and the spraying valve is controlled to switch on and off spraying according to the spraying conditions to obtain the spraying trajectory planning of the spraying robot. Figure 5 Shown is a schematic diagram of the spraying trajectory of the spraying robot.

[0193] Example 4

[0194] like Figure 6 As shown, in this embodiment, a system for a painting robot trajectory planning method based on depth camera scanning is provided, including a point cloud acquisition and processing module, a point cloud segmentation module, a feature extraction module, and a trajectory planning module.

[0195] Specifically, the point cloud acquisition and processing module is used to scan the three-dimensional point cloud data of the wall at different positions in real time, and convert the three-dimensional point cloud data of the wall scanned at different positions into the coordinate system of the robot base for splicing to obtain the wall splicing point cloud;

[0196] Specifically, the point cloud segmentation module is used to perform three-dimensional visual point cloud segmentation based on the normal vector and curvature information of the wall splicing point cloud to obtain the segmented point cloud;

[0197] Specifically, the feature extraction module is used to extract wall features based on the segmented point cloud to obtain wall edge information;

[0198] Specifically, the trajectory planning module is used to obtain boundary constraints in different directions based on the wall edge information, obtain the spraying height sequence in combination with the spraying spacing, and obtain the spraying waypoint sequence in the wall coordinate system based on the spraying height sequence, and convert it into a spatial spraying waypoint sequence in the robotic arm base coordinate system to obtain the spraying trajectory of the spray robot.

[0199] Example 5

[0200] In this embodiment, a device for a painting robot trajectory planning method based on depth camera scanning is provided, including a painting robot, a lifting mechanism and a wall sensing device.

[0201] Specifically, a lifting mechanism, which is mounted on the spraying robot;

[0202] Specifically, the wall sensing device includes a robotic arm and a depth camera; the robotic arm is installed on a lifting mechanism; the depth camera is installed at the end of the robotic arm, and is used to scan a large range of walls under the action of the lifting mechanism and the robotic arm; there is a constant posture transformation between the camera coordinate system where the depth camera is located and the robotic arm coordinate system where the end of the robotic arm is located.

[0203] The lifting mechanism and robotic arm can drive the depth camera to scan a large range of walls and plan the trajectory for the robotic arm.

[0204] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "examples", "specific examples", or "some examples" means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner.

[0205] Although the embodiments of the present invention have been shown and described above, it is to be understood that the above embodiments are exemplary and are not to be construed as limitations of the present invention. A person skilled in the art may change, modify, replace and vary the above embodiments within the scope of the present invention.

Claims

1. A painting robot trajectory planning method based on depth camera scanning, characterized in that: include: Scan the 3D point cloud data of the wall at different positions in real time, and convert the 3D point cloud data of the wall scanned at different positions into the coordinate system of the robot base for splicing to obtain the wall splicing point cloud; Based on the normal vector and curvature information of the wall splicing point cloud, three-dimensional visual point cloud segmentation is performed to obtain the segmented point cloud; Project the segmented point cloud onto the wall, extract wall features, and obtain wall edge information; Based on the wall edge information, the boundary constraints in different directions are obtained, and the spraying height sequence is obtained in combination with the spraying spacing. Based on the spraying height sequence, the spraying waypoint sequence in the wall coordinate system is obtained, which is converted into a spatial spraying waypoint sequence in the robotic arm base coordinate system to obtain the spraying trajectory of the spray robot.

2. A painting robot trajectory planning method based on depth camera scanning according to claim 1, characterized in that: The real-time scanning of the three-dimensional point cloud data of the wall at different positions and the point cloud splicing based on the posture sequence to obtain the wall splicing point cloud in the robot arm base coordinate system include: Based on any scanning position on the wall, define the position and posture of the scanning position in the robot base coordinate system, and control the robot arm to move to the scanning position, and obtain multiple three-dimensional point cloud data of the wall in the camera coordinate system according to the scanning sequence of the position and posture; The three-dimensional point cloud data of multiple walls in the camera coordinate system are converted into three-dimensional point cloud data in the robot base coordinate system, and the three-dimensional point cloud data of multiple walls are spliced ​​based on the pose sequence to obtain the wall splicing point cloud in the robot base coordinate system.

3. The trajectory planning method of a painting robot based on depth camera scanning according to claim 1 is characterized in that: The method of performing three-dimensional visual point cloud segmentation based on the normal vector and curvature information of the wall splicing point cloud to obtain the segmented point cloud includes: Traverse all the wall splicing point clouds and calculate the normal vector and curvature information of each point; Based on the normal vector and curvature information corresponding to each point, the region growing algorithm is used to segment the point cloud to obtain the segmented point cloud.

4. A painting robot trajectory planning method based on depth camera scanning according to claim 3, characterized in that: The point cloud segmentation is performed based on the normal vector and curvature information corresponding to each point using a region growing algorithm to obtain a segmented point cloud, including: Select all points whose curvature is less than the preset curvature constant as seed points, and add the seed points to the seed set; Based on each point in the seed set, construct the corresponding growth point set; Based on each point in the growing point set, find all points adjacent to the point to construct an adjacent point set; Based on each point in the adjacent point set, the current point is moved out of the adjacent point set, and the difference between the normal vectors of the current point and the adjacent points in the adjacent point set is calculated. When the difference between the normal vectors is less than the preset difference, the current point is added to the growth point set to update the point set. When the merging conditions are met between two growing point sets, the point sets are merged; Until all points in the spliced ​​point cloud are assigned, the segmented point cloud is obtained.

5. The trajectory planning method of a painting robot based on depth camera scanning according to claim 1, characterized in that: The segmented point cloud is projected onto the wall surface, and wall feature extraction is performed to obtain wall edge information, including: Based on any part of the segmented point cloud, define the coordinates of each segmented point cloud, and construct a data matrix of the segmented point cloud; The principal component analysis method is used to solve the eigenvectors and eigenvalues ​​of the data matrix, and the eigenvector with the smallest eigenvalue is selected as the normal vector direction of the wall; Based on the normal vector direction of the wall and combined with prior information, the representation of the X-axis and Y-axis in the wall coordinate system in the robot coordinate system are determined respectively; The centroid of the wall point cloud is used as the origin of the wall coordinate system, and each point in the segmented point cloud is projected into the wall coordinate system using coordinate transformation to obtain the projection point in the wall coordinate system; Based on the projection points, the Alpha-Shape algorithm is used to extract the boundary points of the wall and obtain the wall edge information.

6. The method for trajectory planning of a painting robot based on depth camera scanning according to claim 1, characterized in that: The boundary constraints in different directions are obtained based on the wall edge information, the spraying height sequence is obtained in combination with the spraying spacing, and the spraying waypoint sequence in the wall coordinate system is obtained based on the spraying height sequence, which is converted into a spatial spraying waypoint sequence in the robot arm base coordinate system to obtain the spraying trajectory of the spraying robot, including: Based on the wall edge information set, obtain the boundary constraints on the wall in the X-axis and Y-axis directions; Based on the process spraying parameters, the spacing between spraying lines is obtained; Combining the spacing between spraying rows and the wall boundary constraints, the spraying height sequence is obtained; Based on the spraying line segments in the spraying height sequence, the intersection sequence formed by the spraying line segments and the wall boundary constraints is obtained in sequence; Based on the intersection sequence and spraying conditions, the Boustrophedon algorithm is used to obtain the spraying waypoint sequence in the wall coordinate system; The spraying waypoint sequence in the wall coordinate system is converted into a spatial spraying waypoint sequence in the robot arm base coordinate system to obtain the spraying trajectory of the spraying robot, which is used to achieve full coverage of the wall.

7. A painting robot trajectory planning method based on depth camera scanning according to claim 6, characterized in that: The spraying conditions include: When the spraying waypoint sequence number is an odd number, the spraying valve is opened for spraying; When the spraying waypoint sequence number is an even number, the spraying valve is closed.

8. A system for the trajectory planning method of a painting robot based on depth camera scanning as claimed in claim 1, characterized in that: include: The point cloud acquisition and processing module is used to scan the three-dimensional point cloud data of the wall at different positions in real time, and convert the three-dimensional point cloud data of the wall scanned at different positions into the coordinate system of the robot base for splicing to obtain the wall splicing point cloud; The point cloud segmentation module is used to perform three-dimensional visual point cloud segmentation based on the normal vector and curvature information of the wall splicing point cloud to obtain the segmented point cloud; A feature extraction module is used to extract wall features based on the segmented point cloud to obtain wall edge information; The trajectory planning module is used to obtain boundary constraints in different directions based on the wall edge information, obtain the spraying height sequence in combination with the spraying spacing, and obtain the spraying waypoint sequence in the wall coordinate system based on the spraying height sequence, and convert it into a spatial spraying waypoint sequence in the robot arm base coordinate system to obtain the spraying trajectory of the spray robot.

9. A device for the trajectory planning method of a painting robot based on depth camera scanning as claimed in claim 1, characterized in that: include: Spraying robot; A lifting mechanism, which is mounted on the spraying robot; Wall sensing equipment, which includes a robotic arm and a depth camera; The mechanical arm is installed on the lifting mechanism; the depth camera is installed at the end of the mechanical arm, and is used to scan a large range of walls under the action of the lifting mechanism and the mechanical arm.

Images