Outdoor live working robot and its laser point cloud modeling and positioning method

Abstract

The present invention provides an outdoor live working robot and a laser point cloud modeling and positioning method thereof. Before moving the mobile jib, the system automatically models the environment, and then based on the point cloud map, the telescopic robotic arm autonomously moves to a more ideal working position. Then, precise modeling using the laser point cloud enables the end operator to efficiently and reliably complete the operation task, thereby improving the automation level of the operation, enhancing the operation efficiency, and effectively reducing the dependence on professional personnel.

Description

Technical Field

[0001] The present invention relates to the technical field of local precise positioning of laser point clouds, and specifically to an outdoor live working robot and its laser point cloud modeling and positioning method. Background Technique

[0002] Laser point cloud modeling has advantages such as high precision and long ranging distance, and is currently widely used in many fields such as geographical terrain mapping, unmanned driving environment mapping, and robot perception and positioning. Currently, lidar has become a standard configuration for mobile robots, and many unmanned vehicles are even equipped with multiple lidars to achieve a non dead angle perception of the environment.

[0003] The outdoor live working robot that replaces manual operation transforms operations with high physical consumption, high skill requirements, and high danger into safe and simple operations. The outdoor high altitude live working robot consists of a boom truck, a telescopic robotic arm, and an end operator. The entire system has many modules and complex operations. Currently, it mainly relies on manual remote control of the telescopic robotic arm to dock the end operator near the target object, and then the end operator completes the final tasks such as wire dialing, wiring, and arrester replacement. In actual use, professional personnel with certain experience are required to complete the operation of moving the bucket, which greatly increases the difficulty of using the live working robot. Summary of the Invention

[0004] In order to solve the problems of the prior art, the present invention provides an outdoor live working robot and its laser point cloud modeling and positioning method. Before moving the boom, the system automatically models the environment, and then based on the point cloud map, the telescopic robotic arm autonomously moves to a more ideal working position. Then, precise laser point cloud modeling is used to enable the end operator to efficiently and reliably complete the operation tasks, thereby improving the degree of automation of the operation, improving the operation efficiency, and effectively reducing the dependence on professional personnel.

[0005] The present invention provides an outdoor live working robot, including a boom truck, an end operator is installed on the boom truck through a telescopic arm, a first lidar that moves through a guide rail is arranged on the boom truck, and a second lidar that moves through a guide rail is arranged on the end operator.

[0006] Further improvement, both the first lidar and the second lidar are multi line lidars. Among them, the first lidar adopts a solid state lidar, and the second lidar adopts a mechanical lidar.

[0007] The present invention also provides a laser point cloud modeling and positioning method for an outdoor live working robot, including the following steps:

[0008] 1) Perform global environment point cloud modeling through the first lidar that moves along the guide rail on the boom truck to obtain a global point cloud map;

[0009] 2) The telescopic arm conducts planning and motion control based on the global point cloud map, and finds the ideal position of the end effector through the global point cloud;

[0010] 3) Use the algorithm to plan the global path of the telescopic arm. During the movement of the telescopic arm towards the ideal position, the second lidar is matched and positioned in real time with the global point cloud map;

[0011] 4) After reaching the ideal position, the second lidar starts to perform precise modeling to obtain an accurate point cloud map.

[0012] For further improvement, the lidar modeling process in steps 1) and 4) is as follows: Assume there are k frames of lidar point cloud data, and the characteristic points extracted from the k frames of lidar data are spliced together to obtain a point cloud map.

[0013] For further improvement, the point cloud splicing is divided into two stages, including rough matching and fine matching. The map is represented by Pm; rough matching refers to the matching between adjacent two frames of lidar data to obtain the pose relationship between adjacent two frames

[0014] Fine matching is the matching between the current lidar data P(k + 1) and the point cloud map obtained by splicing the previous k frames of lidar data. The global pose of the kth frame of lidar and the point cloud map is obtained through precise matching It can be obtained through formula calculation The calculated at this time is an inaccurate global pose of the (k + 1)th frame of lidar in Pm. The coordinates of the point cloud of the current lidar frame in the global coordinate system are solved with the current rough pose, and then matched with Pm to obtain a more accurate global pose

[0015] For further improvement, during the fine matching, the coordinates between adjacent frames use the following formula to transform the coordinates of the (k + 1)th frame of lidar point cloud to the coordinate system of the kth frame of lidar:

[0016]

[0017] In the above formula, R represents the rotation matrix from the (k + 1)th frame coordinate system to the kth frame coordinate system, and t represents the translation vector from the (k + 1)th frame coordinate system to the kth frame coordinate system.

[0018] For further improvement, when performing fine matching, downsampling is carried out on the point cloud. The downsampling strategy is to retain only one laser point in a 5 - centimeter cubic grid; during fine matching, the strategy for matching the current frame with Pm is to find corresponding points in Pm through the kdtree algorithm, including corner point matching and surface point matching; in corner point matching, the corner feature points search for the 2 points closest to them in Pm and calculate the distance from the point to the line; in surface point matching, the surface feature points search for the 3 non - collinear points closest to them in Pm, calculate the distance from the point to the plane, and then solve through the cost function to obtain the pose of the current laser frame in Pm.

[0019] For further improvement, in the corner point matching, is the set of corner points of the (k + 1)-th frame laser projected into the k-th frame coordinate system; E(k) is the set of corner points of the k-th frame laser; select a point i from , select the point j closest to i in E(k), and select the point l closest to point j in the adjacent scan lines in E(k);

[0020] The three points selected at this time: The coordinates are respectively denoted as X(k,b), X(k,c); Calculate the shortest distance from point a to the line bc through the corner point a of the (k + 1)-th frame laser and the two closest laser points b and c found in the k-th frame laser. The formula is as follows:

[0021]

[0022] In the surface point matching, is the set of surface feature points of the (k + 1)-th frame laser point cloud transformed into the k-th frame coordinate system through the projection matrix (R,t), and H(k) is the set of surface feature points of the k-th frame laser point cloud; find a point i from , find the point l closest to point i from H(k), and find the closest point j in the same laser scan beam as point l, and find the closest point m to point j in the adjacent frame. Points j and m both belong to H(k); j, l, and m are non - collinear and can form a plane of three points;

[0023] Four points are selected at this time: The coordinates are respectively denoted as: X (k,j) , X (k,l) , X (k,m) . The closest distance from point i to the plane jlm is expressed by the formula as follows:

[0024]

[0025] For further improvement, the cost function is expressed as follows:

[0026]

[0027] In the above formula, p represents the number of corner points, and q represents the number of surface points;

[0028] Solve by the Gauss-Newton or Levenberg-Marquardt method. After obtaining the pose relationship (R, t) between frames, the point cloud is stitched to obtain the point cloud model of the environment.

[0029] The specific process of laser data extraction is as follows: Extract the k points with the largest curvature as corner points and the n points with the smallest curvature as surface points from the curvature of the neighborhood point cloud, and match the corner points and corner points of adjacent frame laser point clouds, and match the surface points and surface points.

[0030] The beneficial effects of the present invention are as follows: Install a lidar on the live working robot. Before moving the mobile boom, the system automatically completes the modeling of the environment, and then based on the point cloud map, the telescopic robotic arm autonomously moves to a more ideal working position. Then, use the laser point cloud for precise modeling, and the end effector can efficiently and reliably complete the operation task, thereby improving the automation level of the operation, improving the operation efficiency, and effectively reducing the dependence on professional personnel. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following will briefly introduce the drawings required in the embodiments. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, without creative efforts, other drawings can be obtained based on these drawings.

[0032] Figure 1 It is the overall structure diagram of the live working robot;

[0033] Figure 2 It is the work flow chart of live working at high altitude;

[0034] Figure 3 It is the laser point cloud modeling diagram under the actual environment;

[0035] Figure 4 It is the local laser point cloud modeling diagram under the actual environment. DETAILED DESCRIPTION OF THE INVENTION

[0036] The following will clearly and completely describe the technical solutions in the embodiments of the present invention with reference to the drawings in the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, rather than all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts belong to the scope of protection of the present invention.

[0037] I. Hardware System

[0038] This method provides a complete set of hardware systems, as Figure 1 shown, mainly including boom trucks, telescopic booms, end effectors, lidars, RTKs, integrated controllers and other components.

[0039] II. Software Modules

[0040] This method provides a complete set of software modules. The main module functions are described as follows:

[0041] 1. Global Lidar Point Cloud Modeling Module

[0042] 2. Local Lidar Point Cloud Modeling Module

[0043] 3. RTK-Assisted Positioning Module

[0044] 4. Telescopic Boom Planning and Control Module

[0045] 5. End Effector Planning and Control Module

[0046] III. Lidar Point Cloud Modeling

[0047] Two lidars are configured in the whole system, both of which are multi-line lidars. One lidar is installed on the guide rail of the engineering vehicle and denoted as Lidar 1; the other lidar is installed on the guide rail of the end effector box and denoted as Lidar 2. Lidar 1 uses a solid-state lidar. Because the scanning feature of the solid-state lidar is non-repetitive scanning, different points can be scanned at the same position, and all reflectable objects within the viewing range can be obtained from multiple frames of point clouds, which has a good effect on modeling the integrity of the environment. Lidar 2 uses a mechanical lidar, mainly taking advantage of its repetitive scanning feature, that is, the laser point scans to the same point each time. By using the sliding of the guide rail, the wire model can be scanned and modeled well.

[0048] Lidar 1 is used for global modeling, and Lidar 2 is used for local modeling.

[0049] Principle description of lidar point cloud modeling:

[0050] Map refers to: Assuming there are k frames of lidar point cloud data, then the characteristic points extracted from the k frames of lidar data are spliced together to obtain the point cloud map.

[0051] Point cloud splicing is divided into two stages, rough matching and fine matching.

[0052] Rough matching refers to the matching between adjacent two frames of lidar data; while fine matching is the matching between the current lidar data P(k + 1) and the point cloud map obtained by splicing the previous k frames of lidar data. The point cloud map is denoted as Pm.

[0053] Coarse matching can obtain the pose relationship between two adjacent frames Since the global pose of the k-th frame of laser has been obtained through precise matching with the point cloud map Then it can be calculated through the formula The calculated one at this time is an inaccurate global pose of the (k + 1)-th frame of laser in Pm. The coordinates of the point cloud of the current laser frame in the global coordinate system are obtained through the current rough pose solution, and then matched with Pm to obtain a more accurate global pose

[0054] During fine matching, the number of point clouds in Pm will increase due to stitching. Therefore, downsampling will be performed on the point clouds. The downsampling strategy is to only retain one laser point in a 5-cm cubic grid. During fine matching, the matching strategy between the current frame and Pm is to find corresponding points in Pm through kdtree. For corner feature points, find the two points closest to them in Pm and calculate the distance from the point to the line; for surface feature points, find the three non-collinear points closest to them in Pm and calculate the distance from the point to the surface. Then, the pose of the current laser frame in Pm is obtained through Costfunction

[0055] Both coarse matching and fine matching utilize the ICP matching principle of point clouds. Due to the large amount of data in multi-line laser point clouds, if all the point clouds of each frame of laser are matched using the ICP algorithm, it will consume a lot of CPU resources. Therefore, matching by extracting feature point clouds can improve the matching efficiency and also maintain good accuracy

[0056] Laser feature point cloud extraction: Extract the k points with the largest curvature as corner points and the n points with the smallest curvature as surface points through the curvature size of the neighborhood point cloud. Then, match the corner points of adjacent frames of laser point clouds with each other, and match the surface points with each other

[0057] The coordinates between adjacent frames can use the following formula to convert the coordinates of the (k + 1)-th frame of laser point cloud to the coordinate system of the k-th frame of laser. The coordinate transformation formula is as follows

[0058]

[0059] In the above formula, R represents the rotation matrix from the (k + 1)-th frame coordinate system to the k-th frame coordinate system, and t represents the translation vector from the (k + 1)-th frame coordinate system to the k-th frame coordinate system

[0060] In the corner point matching is the set of corner points of the (k + 1)-th frame of laser projected onto the k-th frame coordinate system; E(k) is the set of corner points of the k-th frame of laser; Select a point i from select the point j closest to i in E(k), and select the point l closest to point j in the adjacent scan lines in E(k);

[0061] At this time, the three selected points are: The coordinates are respectively denoted as X(k, b), X(k, c); Calculate the shortest distance from point a to line bc through the corner point a of the (k + 1)-frame laser and the two laser points b and c with the closest distance found in the k-frame laser. The formula is as follows:

[0062]

[0063] In the above surface point matching, is the set of surface feature points obtained by converting the (k + 1)-frame laser point cloud to the k-frame coordinate system through the projection matrix (R, t), and H(k) is the set of surface feature points of the k-frame laser point cloud; Find a point i from Find the point l closest to point i from H(k), and find the closest point j with the same laser scanning beam as point l. Find the point m closest to point j in adjacent frames. Points j and m both belong to H(k); j, l, and m are non-collinear and can form three points of a plane;

[0064] At this time, four points are selected: The coordinates are respectively denoted as: X (k,j) , X (k,l) , X (k,m) . The closest distance from point i to the plane jlm is expressed by the formula as follows:

[0065]

[0066] The so-called cost function is expressed as the following formula,

[0067]

[0068] In the above formula, p represents the number of corner points, and q represents the number of surface points.

[0069] In this way, the problem is transformed into a least squares problem and solved by the Gauss-Newton or Levenberg-Marquardt method.

[0070] After solving for the pose relationship (R, t) between frames, the point clouds can be stitched together to obtain the point cloud model of the environment.

[0071] Taking the Levenberg-Marquardt method as an example to illustrate how to solve (R, t) by the least squares method:

[0072] The detailed expression of the Cost function is as follows,

[0073]

[0074] Construct the Lagrangian function, where λ is the coefficient factor:

[0075]

[0076] In this case, after simplification and taking the derivative, we can obtain:

[0077]

[0078] After simplification, we get:

[0079] (JJ T + λD T D)ΔT = -Jf

[0080] And

[0081] Therefore, we can obtain the derivative as:

[0082] ΔT = -(JJ T + λD T D)-Jd

[0083] It can be seen that this derivative is related to the initial Jacobian matrix J and the coefficient matrix D. In actual use, the coefficient matrix D is usually represented by J T J, and thus the differential component ΔT is obtained.

[0084] Substituting into the gradient descent formula gives:

[0085]

[0086]

[0087] Continuously solve the above formula until convergence. The solution methods for rough matching and fine matching are the same.

[0088] The telescopic arm conducts path planning and motion control based on the global point cloud model. First, the ideal position of the end effector is found through the global point cloud (which can be selected by the operator), then the algorithm plans the global path of the telescopic arm, and then the telescopic arm moves to the ideal position. During the movement of the robotic arm, the second lidar can be matched and positioned with the global point cloud map in real time. After reaching the ideal position, the second lidar starts precise modeling, and the modeling principle is the same as described above. After the local point cloud map model is built, the end effector can plan a path to reach the ideal operation position to complete live working.

[0089] IV. Business problems solved

[0090] The workflow chart of live working at high altitude is as Figure 2As shown, a first lidar is used on an energized work robot to model the environment. Based on the point cloud model, the ideal position of the end effector is found. After the end effector reaches the ideal position, a second lidar is used to accurately model the wire, and finally the end effector is guided to complete the operation. The modeling of the first lidar can complete the global perception of the surrounding environment and quickly find the ideal operation position of the end effector, such as Figure 3 As shown, the telescopic arm can automatically plan a path based on the position information provided by the point cloud to reach the ideal operation position of the end effector; the second lidar completes local accurate modeling. A specific modeling is as follows Figure 4 As shown, the end effector is guided to complete the operation. The entire process reduces manual operation and improves operation efficiency.

[0091] Each embodiment in this specification is described in a progressive manner. For the same or similar parts among the embodiments, reference can be made to each other. The key point of each embodiment is to illustrate the differences from other embodiments. In particular, for the device embodiment, the above description is only the preferred embodiment of the present invention. Since it is basically similar to the method embodiment, the description is relatively simple. For the relevant parts, reference can be made to the partial description of the method embodiment. The above is only the specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. For any person skilled in the art in the technical field disclosed by the present invention, any change or replacement that can be easily thought of by those of ordinary skill in the technical field should be covered within the protection scope of the present invention without departing from the principle of the present invention. Therefore, the protection scope of the present invention should be subject to the protection scope of the claims.

Claims

1. A laser point cloud modeling and positioning method for an outdoor live working robot, characterized in that: An outdoor live working robot is adopted, which includes a boom truck. An end effector is installed on the boom truck through a telescopic arm. A first lidar that moves along a guide rail is arranged on the boom truck, and a second lidar that moves along a guide rail is arranged on the end effector. The method includes the following steps: 1) Perform global environmental point cloud modeling through the first lidar moving along the guide rail on the boom truck to obtain a global point cloud map; 2) The telescopic arm performs planning and motion control based on the global point cloud map, and finds the ideal position of the end effector through the global point cloud; 3) Use an algorithm to plan the global path of the telescopic arm. During the movement of the telescopic arm towards the ideal position, the second lidar is matched and positioned in real time with the global point cloud map; 4) After reaching the ideal position, the second lidar starts to perform precise modeling to obtain a precise point cloud map; The modeling processes of the first lidar and the second lidar in step 1) and step 4) are as follows: Assume there are k frames of lidar point cloud data. The characteristic points of the k frames of lidar data are extracted and spliced together to obtain a point cloud map. The splicing process is divided into two stages, including rough matching and fine matching. The map is represented by Pm; Coarse matching refers to the matching between the laser data of two adjacent frames to obtain the pose relationship between two adjacent frames , The fine matching is the matching of the current laser data P(k+1) with the point cloud map obtained by stitching the previous k frames of laser data. The global pose of the k-th frame of laser and the point cloud map is obtained through fine matching. , which can be obtained by formula calculation . At this time, the obtained is an inaccurate global pose of the (k+1)-th frame of laser in Pm. The coordinates of the current laser frame point cloud in the global coordinate system are calculated based on the current rough pose, and then matched with Pm to obtain a more accurate global pose. ; During fine matching, the coordinates between adjacent frames use the following formula to transform the laser point cloud coordinates of the (k+1)-th frame into the coordinate system of the k-th frame of laser: , where \(R\) in the above formula represents the rotation matrix from the coordinate system of the \((k + 1)\)-th frame to the coordinate system of the \(k\)-th frame, and \(t\) represents the translation vector from the coordinate system of the \((k + 1)\)-th frame to the coordinate system of the \(k\)-th frame.

2. The laser point cloud modeling and positioning method of the outdoor live working robot according to claim 1, wherein: During fine matching, downsampling is performed on the point cloud. The downsampling strategy is to only retain one lidar point in a 5-cm cubic grid. During fine matching, the strategy for the current frame to match with Pm is to find corresponding points in Pm through the kdtree algorithm, including corner point matching and surface point matching; In corner point matching, the corner feature points find the two points closest to them in Pm, and calculate the distance from the point to the line; In surface point matching, the surface feature points find the three non-collinear points closest to them in Pm, and calculate the distance from the point to the surface. Then, the pose of the current lidar frame in Pm is obtained by solving through the cost function.

3. The laser point cloud modeling and positioning method for the outdoor live working robot according to claim 2, characterized in that: In the corner point matching, is the set of corner points where the laser of the (k + 1)-th frame is projected onto the coordinate system of the k-th frame; E(k) is the set of corner points of the laser of the k-th frame; Select a point i from and select the point j closest to i in E(k), and select the point l closest to the adjacent scan line of point j in E(k); Three points selected at this time: { ; }, and their coordinates are respectively recorded as (k + 1, a), ; Calculate the shortest distance from point a to line bc through the corner point a of the (k + 1)-th frame laser and the two closest laser points b and c found in the k-th frame laser. The formula is as follows: , in the pastry matching, (k + 1) is the set of surface feature points obtained by transforming the (k + 1)-th frame of laser point cloud to the coordinate system of the k-th frame through the projection matrix (R, t), and H(k) is the set of surface feature points of the k-th frame of laser point cloud; from (k + 1), find a point i, find the point l closest to point i from H(k), and find the closest point j in the same laser scanning beam as point l, and find the closest point m to point j in adjacent frames. Points j and m both belong to H(k); j, l, and m are non-collinear and can form three points of a plane; At this time, 4 points are selected: , and their coordinates are respectively denoted as: (k+1,i) , X (k,j) , X (k,l) , X (k,m) , the shortest distance from point i to the plane jlm is expressed by the following formula: 。 4. The laser point cloud modeling and positioning method for an outdoor live working robot according to claim 3, characterized in that: The cost function is expressed as follows: , where p represents the number of corner points and q represents the number of face points in the above formula; Solve through the Gauss-Newton or Levenberg-Marquardt method. After obtaining the pose relationship (R, t) between frames, splice the point clouds to obtain the point cloud model of the environment.

5. The laser point cloud modeling and positioning method for the outdoor live working robot according to claim 1, wherein: The specific process of laser data extraction is as follows: Extract the k points with the largest curvature as corner points and the n points with the smallest curvature as surface points through the curvature size of the neighborhood point cloud. Match the corner points of adjacent frames of lidar point clouds with each other, and match the surface points with each other.

Images