Trolley butt joint positioning method and equipment

Abstract

The invention discloses a trolley butt joint positioning method and equipment, and belongs to the technical field of robot positioning. The method comprises the following steps: acquiring laser point cloud data when the trolley is relatively positioned; carrying out segmentation processing on the laser point cloud data, and separating out a plurality of laser segments; effective laser segments are screened out by calculating the lengths of the multiple laser segments; a first scoring function is used for the effective laser segments, and key laser segments are determined; a second scoring function is used for the key laser section, a key point is determined, and a middle point pose is output; matching the key point with a key coordinate of a pre-established template to obtain a relative pose of the central point pose and the central point of the trolley; according to the relative pose, constructing a nonlinear optimization problem, and solving to obtain an optimal solution of the relative pose; and the optimal solution of the relative pose is converted, so that the trolley is positioned and butt joint is realized. According to the method, the key points for matching are determined through segmentation, screening and two rounds of scoring, and the positioning precision and stability can be improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot positioning, and particularly relates to a method and device for docking and positioning a trolley. Background Art

[0002] When a robot docks with a trolley, in order to prevent the trolley from suddenly slipping during the high-speed movement of the robot, a shaft pin is generally used to fix it at the docking position. In order to accurately dock the shaft pin during docking, the Iterative Closest Point (ICP) algorithm is usually used in the prior art to match and position the leg features of the trolley; mainly by pre-recording key frames, matching the point cloud observed by the lidar in real time with the key frames, and then determining the relative position of the robot and the trolley according to the relative positioning of the two and the pose of the key frames, so as to achieve docking. However, its disadvantages are as follows: 1. The observation angle of the trolley by the robot during movement is different, resulting in incomplete observation of the leg features, large positioning errors, and possible docking failures; 2. During the process of recording key frames, objects with similar shapes will interfere with the matching, making it difficult to cope with complex or dynamic environments. Summary of the Invention

[0003] The purpose of the present invention is to overcome the deficiencies in the prior art, and provide a method and device for docking and positioning a trolley, which can improve the positioning accuracy and stability by determining the key points for matching through methods of segmentation, screening, and two rounds of scoring.

[0004] To achieve the above object, the present invention is implemented by the following technical solutions:

[0005] In a first aspect, the present invention provides a method for docking and positioning a trolley, the method comprising:

[0006] Obtaining lidar point cloud data during the relative positioning of the trolley;

[0007] Performing segmentation processing on the lidar point cloud data to separate multiple laser segments;

[0008] By calculating the lengths of the multiple laser segments, screening out effective laser segments;

[0009] Using a first scoring function for the effective laser segments to determine key laser segments;

[0010] Using a second scoring function for the key laser segments to determine key points and output the midpoint pose;

[0011] Matching the key points with the key coordinates of a pre-established template to obtain the relative pose of the midpoint pose and the center point of the trolley, wherein the template is a mathematical model established according to the pre-measured length and width of the trolley;

[0012] Construct a non - linear optimization problem according to the relative pose, and solve it to obtain the optimal solution of the relative pose;

[0013] Transform the optimal solution of the relative pose to locate the trolley and achieve docking.

[0014] In combination with the first aspect, further, the segmentation processing of the laser point cloud data to separate multiple laser segments includes:

[0015] Initialize a laser segment;

[0016] Arbitrarily select an unmarked point from the laser point cloud data, add it to the laser segment, and mark it;

[0017] Traverse the remaining unmarked points;

[0018] Calculate the distance between the remaining unmarked point and the nearest point in the laser segment;

[0019] In response to the distance being greater than or equal to a preset distance threshold, continue to traverse the next remaining unmarked point; in response to the distance being less than the distance threshold, add the remaining unmarked point to the laser segment and mark it;

[0020] In response to a new point being added to the laser segment, continue the next round of traversal and judgment; in response to no new point being added, separate the laser segment;

[0021] Judge whether there are unmarked points in the laser point cloud data;

[0022] In response to there being unmarked points, repeat the above steps, continue to initialize the laser segment and complete the separation; in response to there being no unmarked points, end the segmentation to obtain multiple laser segments.

[0023] In combination with the first aspect, further, the screening of effective laser segments by calculating the lengths of the multiple laser segments includes:

[0024] Extract any one of the multiple laser segments;

[0025] Calculate the distance between any two points in the laser segment, take the longest distance as the length of the laser segment, and calculate the mid - point coordinates of the longest distance as the mid - point of the laser segment;

[0026] In response to the length of the laser segment being greater than or equal to a preset length threshold, delete the laser segment; in response to the length of the laser segment being less than the length threshold, retain the laser segment;

[0027] Judge whether there are unextracted laser segments among the multiple laser segments;

[0028] In response to the existence of unextracted laser segments, repeat the above steps to continue extracting laser segments and complete the screening; in response to the non-existence of unextracted laser segments, end the screening and obtain the effective laser segments.

[0029] In combination with the first aspect, further, using a first scoring function for the effective laser segments to determine the key laser segments includes:

[0030] Calculate the lengths of the three sides of the template triangle according to the predicted length and width of the trolley:

[0031] ,

[0032] ,

[0033] ,

[0034] where, is the long right-angled side of the template triangle, is the short right-angled side of the template triangle, is the hypotenuse of the template triangle, is the length of the trolley, is the width of the trolley, is a function that returns the square root of the sum of the squares of two parameters;

[0035] Group the midpoints of the effective laser segments into three groups to form multiple triangles;

[0036] Extract any one of the multiple triangles;

[0037] Initialize the first array and the second array;

[0038] Calculate the lengths of the three sides of the triangle according to the coordinates of the three midpoints that make up the triangle ;

[0039] Put the lengths of the three sides of the triangle into the first array in ascending order, and put the lengths of the three sides of the template triangle into the second array in ascending order;

[0040] Use the first scoring function to score the triangle to obtain a first score, where the first scoring function is:

[0041] ,

[0042] where, is the first score, is a function that returns the absolute value, is the first array, is the second array;

[0043] In response to the first score being less than or equal to a preset first score threshold, sort the first score;

[0044] Determine whether there are unextracted triangles among the multiple triangles;

[0045] In response to the existence of unextracted triangles, repeat the above steps to continue extracting triangles and obtain the first score; in response to the non-existence of unextracted triangles, end the first round of scoring and record the three valid laser segments corresponding to the triangle with the lowest first score as the key laser segments.

[0046] Combined with the first aspect, further, using a second scoring function for the key laser segments to determine key points and output the midpoint pose, including:

[0047] Arbitrarily extract one point from each of the key laser segments to form a triangle;

[0048] Initialize the third array and the fourth array;

[0049] According to the coordinates of the three points forming the triangle, calculate the lengths of the three sides of the triangle ;

[0050] The lengths of the three sides of the triangle are placed in the third array in ascending order, and the lengths of the three sides of the template triangle are placed in the fourth array in ascending order;

[0051] Use the second scoring function to score the triangle to obtain a second score, where the second scoring function is:

[0052] ,

[0053] In the formula, is the second score, is the function to return the absolute value, is the third array, is the fourth array;

[0054] In response to the second score being less than or equal to a preset second score threshold, sort the second score;

[0055] Determine whether there are unextracted points in the first key laser segment;

[0056] In response to the existence of unextracted points, repeat the above steps to continue forming triangles and obtain a second score; in response to the non-existence of unextracted points, determine whether there are unextracted points in the second critical laser segment;

[0057] In response to the existence of unextracted points, repeat the above steps to continue forming triangles and obtain a second score; in response to the non-existence of unextracted points, determine whether there are unextracted points in the third critical laser segment;

[0058] In response to the existence of unextracted points, repeat the above steps to continue forming triangles and obtain a second score; in response to the non-existence of unextracted points, end the second-round scoring, and record the lengths of the three sides and the three points corresponding to the triangle with the lowest second score as the critical length and the critical points.

[0059] Combined with the first aspect, further, using the second scoring function for the critical laser segment to determine the critical points and output the midpoint pose further includes:

[0060] Sort the critical lengths in descending order to determine the hypotenuse and the long right-angled side among the critical lengths;

[0061] According to the coordinates of the critical point corresponding to the hypotenuse, calculate the abscissa of the midpoint of the hypotenuse , ordinate and the attitude angle , where the direction of the attitude angle is the direction in which the midpoint of the hypotenuse points vertically to the long right-angled side;

[0062] Output the midpoint pose .

[0063] Combined with the first aspect, further, matching the critical points with the critical coordinates of a pre-established template to obtain the relative pose of the midpoint pose and the center point of the trolley includes:

[0064] Taking the midpoint of the hypotenuse as the origin and the direction pointed by the attitude angle as the positive direction of the axis to establish a midpoint pose coordinate system;

[0065] Convert the coordinates of the critical points into the coordinates in the midpoint pose coordinate system , where the conversion formula is:

[0066] ,

[0067] ,

[0068] ,

[0069] ,

[0070] In the formula, is the abscissa of the th key point, is the abscissa of the midpoint pose, is the th key point's ordinate, is the ordinate of the midpoint pose, is the function for calculating the arctangent value, is the attitude angle of the midpoint pose, is the th key point's abscissa in the midpoint pose coordinate system, is the th key point's ordinate in the midpoint pose coordinate system, is the function for calculating the cosine value, is the function for calculating the sine value, ;

[0071] The iterative closest point algorithm is adopted to match the key points with the key coordinates of a pre - established template to obtain the matching points corresponding to the key points, and generate the relative pose of the trolley center point relative to the midpoint pose. Among them, the key coordinates are the leg position coordinates determined with the trolley center point as the origin, the matching point is the point closest to the key point among the leg position coordinates, and the relative pose is initially set to .

[0072] Combined with the first aspect, further, constructing a non - linear optimization problem based on the relative pose and solving to obtain the optimal solution of the relative pose includes:

[0073] Construct a non - linear optimization problem regarding the relative pose:

[0074] ,

[0075] ,

[0076] ,

[0077] In the formula, is the coordinate of the th key point in the midpoint pose coordinate system, is the coordinate of the relative pose in the midpoint pose coordinate system, is the th key point's corresponding matching point's coordinate, is the horizontal coordinate of the relative pose in the midpoint pose coordinate system, is the ordinate of the relative pose in the midpoint pose coordinate system, For the The horizontal coordinate of the matching point corresponding to the key point, For the The ordinate of the matching point corresponding to the key point, is the function for calculating the cosine value, is the function for calculating the sine value, is the function for calculating the inverse tangent value, is the attitude angle of the relative posture in the midpoint posture coordinate system, ;

[0078] Perform nonlinear optimization on the nonlinear optimization problem and solve it to obtain the optimal solution of the relative posture .

[0079] In combination with the first aspect, further, converting the optimal solution of the relative posture to position the trolley and achieve docking includes:

[0080] The coordinates of the optimal solution of the relative pose Convert to the coordinates of the laser radar coordinate system , where the conversion formula is:

[0081] ,

[0082] ,

[0083] ,

[0084] ,

[0085] In the formula, is the horizontal coordinate of the optimal solution of relative posture in the midpoint posture coordinate system, is the ordinate of the optimal solution of relative posture in the midpoint posture coordinate system, is the attitude angle of the optimal solution of relative posture in the midpoint posture coordinate system, is the horizontal coordinate of the midpoint pose, is the ordinate of the midpoint pose, is the attitude angle of the midpoint pose, is the function for calculating the inverse tangent value, is the function for calculating the cosine value, is the function for calculating the sine value;

[0086] According to the coordinates of the optimal solution of the relative posture in the laser radar coordinate system , positioning the trolley and achieving docking.

[0087] In a second aspect, the present invention further provides a computer device, including a storage medium and a processor;

[0088] The storage medium is used for storing instructions;

[0089] The processor is used for operating according to the instructions to execute the steps of the method according to any one of the first aspect.

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

[0091] The trolley docking and positioning method provided by the present invention does not require pre-recording key frames. Instead, by measuring the length and width of the trolley in advance, establishing a mathematical model, and confirming the matching template, it avoids the interference of the surrounding environment and improves the stability of the method. Moreover, during the subsequent calculation process, the point cloud data scanned by the lidar is segmented, screened, and scored twice to extract a few positioning key points that match the template, which can be unaffected by the observation angle and improves the positioning accuracy of the method. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0093] Figure 1 is a flowchart of a trolley docking and positioning method provided by an embodiment of the present invention;

[0094] Figure 2 is a flowchart of a segmentation method provided by an embodiment of the present invention;

[0095] Figure 3 is a flowchart of a screening method provided by an embodiment of the present invention;

[0096] Figure 4 is a flowchart of a first-round scoring method provided by an embodiment of the present invention;

[0097] Figure 5 is a flowchart of a second-round scoring method provided by an embodiment of the present invention;

[0098] Figure 6 is a flowchart of a method for outputting the pose of the midpoint provided by an embodiment of the present invention;

[0099] Figure 7 is a point cloud map actually detected and generated by an application example of the present invention;

[0100] Figure 8 It is an internal structure diagram of a computer device provided by an embodiment of the present invention. Detailed implementation manners

[0101] The present invention will be further described below with reference to the accompanying drawings. The following embodiments are only used to more clearly illustrate the technical solutions of the present invention and should not be used to limit the protection scope of the present invention.

[0102] Embodiment 1:

[0103] This embodiment provides a method for docking and positioning a trolley. As Figure 1 shown, it is a flowchart of the method provided in this embodiment, mainly including the following steps:

[0104] Step S100: Obtain the laser point cloud data during the relative positioning of the trolley;

[0105] Step S200: Perform segmented processing on the laser point cloud data to separate multiple laser segments;

[0106] Step S300: Screen out the effective laser segments by calculating the lengths of multiple laser segments;

[0107] Step S400: Use the first scoring function for the effective laser segments to determine the key laser segments;

[0108] Step S500: Use the second scoring function for the key laser segments to determine the key points and output the midpoint pose;

[0109] Step S600: Match the key points with the key coordinates of a pre-established template to obtain the relative pose between the midpoint pose and the center point of the trolley. Here, the template is a mathematical model established according to the pre-measured length and width of the trolley;

[0110] Step S700: Construct a non-linear optimization problem according to the relative pose and solve to obtain the optimal solution of the relative pose;

[0111] Step S800: Transform the optimal solution of the relative pose to locate the trolley and achieve docking.

[0112] It should be noted that before executing step S100, it is necessary to measure the length and width of the trolley in advance; the length and width of the trolley specifically refer to the distance between the inner sides of the trolley legs. Referring to Figure 7 the point cloud map actually detected in the application example of the present invention, the laser point cloud data obtained during the relative positioning of the trolley in step S100 is shown in Figure (a).

[0113] Furthermore, asFigure 2 As shown, it is a flowchart of the segmentation method provided in this embodiment. The following will further elaborate on how to perform segmentation processing on the laser point cloud data in step S200 to separate multiple laser segments: Figure 2 :

[0114] Step S201: Initialize a laser segment;

[0115] Step S202: Arbitrarily select an unmarked point from the laser point cloud data, add it to the laser segment, and mark it;

[0116] Step S203: Traverse the remaining unmarked points;

[0117] Step S204: Calculate the distance between the remaining unmarked point and the nearest point in the laser segment;

[0118] Step S205: In response to the distance being greater than or equal to the preset distance threshold, continue to traverse the next remaining unmarked point; in response to the distance being less than the distance threshold, add the remaining unmarked point to the laser segment and mark it;

[0119] Step S206: In response to a new point being added to the laser segment, continue the next round of traversal and judgment; in response to no new point being added, separate the laser segment;

[0120] Step S207: Determine whether there are unmarked points in the laser point cloud data; in response to there being unmarked points, repeat the above steps, continue to initialize the laser segment and complete the separation; in response to there being no unmarked points, end the segmentation to obtain multiple laser segments.

[0121] Specifically, first, a first laser segment seg1 can be declared. Arbitrarily select an unmarked point as the seed from the laser point cloud data, add the seed to the first laser segment seg1, and mark the seed as segmented. Then, traverse all the remaining unmarked points in the laser point cloud data, judge the distance between each point and the nearest point in the first laser segment seg1. If the distance of a certain point is greater than or equal to the preset distance threshold, continue to traverse the next remaining unmarked point; otherwise, add the point to the first laser segment seg1 and mark the point as segmented. Next, judge whether there is a new point added to the first laser segment seg1 during the above traversal. If there is a new point added, continue the next round of traversing the remaining points and judging the distance; if there is no new point added, separate the first laser segment seg1. Finally, judge whether there are still unmarked points in the laser point cloud data. If there are, repeat the above steps, continue to initialize the laser segment and complete the separation; if not, end the segmentation and separate multiple laser segments such as seg1, seg2, etc. As Figure 7As shown in (b), after the segmentation process, the laser point cloud data is divided into seven laser segments.

[0122] As an embodiment, as Figure 3 shown, it is the flowchart of the screening method provided by this embodiment. Next, in combination with Figure 3 , the screening method mainly includes the following processing steps:

[0123] Step S301: Arbitrarily extract one laser segment from multiple laser segments for processing;

[0124] Step S302: Calculate the distance between any two points in this laser segment, take the longest distance as the length of this laser segment, and calculate the midpoint coordinates of the longest distance as the midpoint of this laser segment;

[0125] Step S303: Judge the length of this laser segment and the preset length threshold. If the length of this laser segment is greater than or equal to the length threshold, delete this laser segment; otherwise, it is considered that the laser segment may describe the characteristics of the trolley leg, and this laser segment needs to be retained;

[0126] Step S304: Judge whether there are unextracted laser segments among multiple laser segments; if there are unextracted laser segments, repeat the above steps, continue to extract the remaining laser segments and complete the calculation and screening; otherwise, end the screening to obtain multiple effective laser segments; Refer to Figure 7 (b), after the screening process, among the seven laser segments, the uppermost and leftmost laser segments are deleted, and the remaining five laser segments are retained.

[0127] In this embodiment, by using the first scoring function for the multiple effective laser segments retained, the key laser segments are determined. As Figure 4 shown, it is the flowchart of the first-round scoring method provided by this embodiment. Next, in combination with Figure 4 , the method for determining the key laser segments in step S400 is further described in detail:

[0128] Step S401: According to the pre-measured length and width of the trolley, calculate the lengths of the three sides of the template triangle :

[0129] ,

[0130] ,

[0131] ,

[0132] In the formula, is the long right-angled side of the template triangle, is the short right-angled side of the template triangle, is the hypotenuse of the template triangle, is the length of the trolley, is the width of the trolley, is a function that returns the square root of the sum of the squares of two parameters;

[0133] Step S402: Group the midpoints of the remaining multiple valid laser segments into groups of three to form multiple triangles. Therefore, each triangle is composed of the midpoints of three valid laser segments;

[0134] Step S403: Extract any one of the multiple triangles;

[0135] Step S404: Initialize the first array and the second array ;

[0136] Step S405: Calculate the lengths of the three sides of the triangle according to the coordinates of the three midpoints that make up the triangle ;

[0137] Step S406: Place the lengths of the three sides of the triangle in ascending order into the first array , and place the lengths of the three sides of the template triangle in ascending order into the second array ;

[0138] Step S407: Score the triangle using the first scoring function to obtain the first score , where the first scoring function is:

[0139] ,

[0140] In the formula, is the first score, is the function that returns the absolute value, is the first array, is the second array;

[0141] Step S408: In response to the first score being less than or equal to the preset first score threshold , sort the first score ; otherwise, skip this step, do not sort, and continue to execute step S409;

[0142] Step S409: Determine whether there are any unextracted triangles among the multiple triangles; in response to the existence of unextracted triangles, repeat the above steps to continue extracting triangles and obtain the first score; in response to the non-existence of unextracted triangles, end the first-round scoring and record the three valid laser segments corresponding to the triangle with the lowest first score as the key laser segments.

[0143] It should be noted that in step S402, when grouping the midpoints of the valid laser segments into groups of three, all possible combination relationships should be considered to avoid omission. In addition, the lower the first score obtained in step S407, the closer it is to 0, indicating that the similarity between the formed triangle and the template triangle is higher.

[0144] Further, use the scoring function for the second time to score the points in the three valid laser segments to determine the key points for matching. As Figure 5 and Figure 6 shown, it is the flowchart of the second-round scoring method and the output midpoint pose method provided in this embodiment. The following combines Figure 5 and Figure 6 , and further elaborates in detail on how to use the second scoring function for the key laser segments in step S500 to determine the key points and output the midpoint pose method:

[0145] Step S501: Arbitrarily extract one point from each of the key laser segments , to form a triangle; that is, arbitrarily extract the first point from the first key laser segment, arbitrarily extract the second point from the second key laser segment, and arbitrarily extract the third point from the third key laser segment, and the three points form a triangle;

[0146] Step S502: Initialize the third array and the fourth array ;

[0147] Step S503: According to the coordinates of the three points forming the triangle, calculate the lengths of the three sides of the triangle ;

[0148] Step S504: Put the lengths of the three sides of the triangle into the third array in ascending order, and put the lengths of the three sides of the template triangle into the fourth array in ascending order;

[0149] Step S505: Use the second scoring function to score the triangle to obtain the second score , where the second scoring function is:

[0150] ,

[0151] wherein, is the second fraction, is the function to return the absolute value, is the third array, is the fourth array;

[0152] Step S506: In response to the second fraction being less than or equal to a preset second fraction threshold , sort the second fraction ; otherwise, skip this step, do not perform sorting, and continue to execute Step S507;

[0153] Step S507: Determine whether there are unextracted points in the first key laser segment; in response to the existence of unextracted points, repeat the above steps to continue forming triangles and obtain the second fraction; in response to the non - existence of unextracted points, determine whether there are unextracted points in the second key laser segment;

[0154] Step S508: In response to the existence of unextracted points, repeat the above steps to continue forming triangles and obtain the second fraction; in response to the non - existence of unextracted points, determine whether there are unextracted points in the third key laser segment;

[0155] Step S509: In response to the existence of unextracted points, repeat the above steps to continue forming triangles and obtain the second fraction; in response to the non - existence of unextracted points, end the second - round scoring, record the lengths of the three sides and the three points corresponding to the triangle with the lowest second fraction as the key lengths and key points ; referring to Figure 7 (c), after the second - round scoring process, the most representative key points are selected from the three key laser segments;

[0156] Step S510: Sort the key lengths in descending order to determine the hypotenuse and the long right - angled side among the key lengths;

[0157] Step S511: According to the coordinates of the key point corresponding to the hypotenuse, calculate the abscissa , ordinate and the attitude angle of the mid - point of the hypotenuse, wherein the direction of the attitude angle is the direction from the mid - point of the hypotenuse perpendicular to the long right - angled side;

[0158] Step S512: Output the mid - point pose ; referring to Figure 7 (d), the mid - point pose of the hypotenuse It is the rough estimated pose of the trolley.

[0159] It should be noted that in step S501, when arbitrarily extracting points from the three key laser segments for triangle combination, all possible combination relationships should be considered to avoid omission. In addition, the lower the second score obtained in step S505, the closer it is to 0, indicating that the similarity between the formed triangle and the template triangle is higher.

[0160] The trolley docking and positioning method provided in this embodiment screens the point cloud data related to the trolley legs in a segmented manner, and then through a two-round scoring mechanism, first screens out the key laser segments, and then screens out the key points from each key laser segment to determine the positioning key points for matching with the template. The determination of the positioning key points is not affected by the observation angle, improving the positioning accuracy of the method.

[0161] Figures 1 to 6 Only the logical order of the method described in this embodiment is shown. On the premise of no conflict, in other possible embodiments of the present invention, the steps shown or described may be completed in a different order from that Figures 1 to 6 shown. The trolley docking and positioning method provided in this embodiment can be applied to a terminal and can be executed by a trolley docking and positioning device, which can be implemented in a software and / or hardware manner, and the device can be integrated in the terminal, for example: any smart phone, tablet computer or computer device with a communication function.

[0162] Embodiment 2:

[0163] The trolley docking and positioning method provided in this embodiment is different from Embodiment 1 in that in step S600, the method of matching the key points with the key coordinates of the pre-established template to obtain the relative pose between the midpoint pose and the center point of the trolley mainly includes the following steps:

[0164] Step S601: Taking the midpoint of the hypotenuse as the origin, and the direction pointed by the attitude angle as the positive direction of the axis, establish a midpoint pose coordinate system;

[0165] Step S602: Convert the coordinates of the key points into the coordinates in the midpoint pose coordinate system, where the conversion formula is:

[0166] ,

[0167] ,

[0168] ,

[0169] ,

[0170] wherein, is the abscissa of the th key point, is the abscissa of the midpoint pose, is the th key point's ordinate, is the ordinate of the midpoint pose, is the function for calculating the arctangent value, is the attitude angle of the midpoint pose, is the th key point's abscissa in the midpoint pose coordinate system, is the th key point's ordinate in the midpoint pose coordinate system, is the function for calculating the cosine value, is the function for calculating the sine value, ;

[0171] Step S603: Use the Iterative Closest Point (ICP) algorithm to match the key point with the key coordinates of the pre-established template, obtain the corresponding matching point of the key point, and generate the relative pose of the center point of the trolley relative to the midpoint pose.

[0172] Specifically, the template is a mathematical model established according to the pre-measured length and width of the trolley; the key coordinates are the accurate position coordinates of the four inner points of the trolley legs determined with the center point of the trolley as the origin, which are respectively: ; the matching point is the point closest to the key point among the trolley leg position coordinates .

[0173] It should be noted that in the Iterative Closest Point (ICP) algorithm, the relative pose is initially set to , and the number of iterations can be set according to the computing efficiency and actual requirements of the robot. In addition, when measuring the length and width of the trolley, a measuring tool such as a tape measure can be used directly by humans, or it can be realized with an electronic measuring device. In practical applications, a suitable tool can be selected according to the actual situation.

[0174] As an optional embodiment, in step S700, the method of constructing a non-linear optimization problem based on the relative pose and solving to obtain the optimal solution of the relative pose mainly includes:

[0175] Step S701: Construct an optimization function about the relative pose The non - linear optimization problem:

[0176] ,

[0177] ,

[0178] ,

[0179] In the formula, is the coordinate of the th key point in the mid - point pose coordinate system, is the coordinate of the relative pose in the mid - point pose coordinate system, is the coordinate of the matching point corresponding to the th key point, is the abscissa of the relative pose in the mid - point pose coordinate system, is the ordinate of the relative pose in the mid - point pose coordinate system, is the abscissa of the matching point corresponding to the th key point, is the ordinate of the matching point corresponding to the th key point, is the function for calculating the cosine value, is the function for calculating the sine value, is the function for calculating the arctangent value, is the attitude angle of the relative pose in the mid - point pose coordinate system, ;

[0180] Step S702: Perform non - linear optimization and solution on the non - linear optimization problem to obtain the optimal solution of the relative pose .

[0181] It should be noted that the above non - linear optimization problem can be input into a non - linear optimization solver for solution to obtain the optimized value of the relative pose.

[0182] Furthermore, in step S800, the method of transforming the optimal solution of the relative pose to locate the trolley and achieve docking mainly includes:

[0183] Step S801: Transform the coordinates of the optimal solution of the relative pose into the coordinates in the lidar coordinate system, where the transformation formula is:

[0184] ,

[0185] ,

[0186] ,

[0187] ,

[0188] Wherein, is the abscissa of the optimal solution of the relative pose in the midpoint pose coordinate system, is the ordinate of the optimal solution of the relative pose in the midpoint pose coordinate system, is the attitude angle of the optimal solution of the relative pose in the midpoint pose coordinate system, is the abscissa of the midpoint pose, is the ordinate of the midpoint pose, is the attitude angle of the midpoint pose, is the function for calculating the arctangent value, is the function for calculating the cosine value, is the function for calculating the sine value;

[0189] Step S802: Locate the trolley and achieve docking according to the coordinates of the optimal solution of the relative pose in the lidar coordinate system. , and locate the trolley and achieve docking.

[0190] The trolley docking and positioning method provided in this embodiment does not require using an additional developed software tool to collect and record key frames in advance. Instead, by measuring the length and width of the trolley (the distance between the given trolley legs) in advance and describing it as a template of a mathematical model for direct calculation, the operation is simple; and it avoids the interference of the surrounding environment and improves the stability of the method.

[0191] The technical solution provided in the embodiment of the present invention determines key points by segmenting, screening, and two-round scoring of the point cloud data obtained by lidar scanning. The selected key points are then matched with the template to obtain the relative pose, and finally the relative pose is solved and transformed to locate the trolley and achieve docking. This method can determine key points, is not affected by the observation angle, and avoids the interference of the surrounding environment, improving the positioning accuracy and stability.

[0192] Embodiment Three:

[0193] This embodiment also provides a computer device, which can be a server, and its internal structure diagram can be as shown in Figure 8 . This computer device includes a processor, a memory, an input / output interface (Input / Output, abbreviated as I / O), and a communication interface. Among them, the processor, the memory, and the input / output interface are connected through a system bus, and the communication interface is connected to the system bus through the input / output interface. Figure 8 shown. This computer device includes a processor, a memory, an input / output interface (Input / Output, abbreviated as I / O), and a communication interface. Among them, the processor, the memory, and the input / output interface are connected through a system bus, and the communication interface is connected to the system bus through the input / output interface.

[0194] Among them, the processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program, and a database. The internal memory provides an environment for the operation of the operating system and the computer program in the non-volatile storage medium. The database of the computer device is used to store the data obtained and generated in the method for the robot to autonomously enter the packaging container. The input / output interface of the computer device is used to exchange information between the processor and external devices. The communication interface of the computer device is used to communicate with an external terminal through a network connection. When the computer program is executed by the processor, it implements the method of any of the foregoing embodiments.

[0195] Those skilled in the art can understand that Figure 8 the structure shown in

[0196] is only a block diagram of some structures related to the solution of this application, and does not constitute a limitation on the computer device to which the solution of this application is applied. The specific computer device may include more or fewer components than those shown in the figure, or combine some components, or have different component arrangements.

[0197] Those skilled in the art should understand that the embodiments of this application can be provided as a method, a system, or a computer program product. Therefore, this application can take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware aspects. Moreover, this application can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code. In addition, terms such as "first" and "second" are only used for descriptive purposes, and cannot be understood as indicating or implying relative importance or implicitly indicating the quantity of the indicated technical features. Thus, features defined with "first", "second", etc. may explicitly or implicitly include one or more of such features. In the description of the present invention, unless otherwise specified, the meaning of "plurality" is two or more.

[0198] This application is described with reference to the flowcharts and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the present application. It should be understood that each flow and / or block in the flowchart and / or block diagram, and the combination of flows and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, such that the instructions executed by the processor of the computer or other programmable data processing device produce a means for implementing the functions specified in one flow Figure 1 one flow or multiple flows and / or blocks Figure 1 or a means for implementing the functions specified in multiple blocks.

[0199] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a specific manner, such that the instructions stored in the computer-readable memory produce a manufactured article including an instruction means that implements the functions specified in one flow Figure 1 one flow or multiple flows and / or blocks Figure 1 or a means for implementing the functions specified in multiple blocks.

[0200] These computer program instructions can also be loaded onto a computer or other programmable data processing device, such that a series of operational steps are executed on the computer or other programmable device to generate a computer-implemented process, and thus the instructions executed on the computer or other programmable device provide steps for implementing the functions specified in one flow Figure 1 one flow or multiple flows and / or blocks Figure 1 or a means for implementing the functions specified in multiple blocks.

[0201] The embodiments of the present invention have been described above in conjunction with the accompanying drawings. However, the present invention is not limited to the above specific embodiments. The above specific embodiments are merely illustrative and not restrictive. Under the inspiration of the present invention, those of ordinary skill in the art can also make many forms without departing from the spirit and scope protected by the claims of the present invention. All of these fall within the protection scope of the present invention.

Claims

1. A trolley docking and positioning method, characterized in that: The method comprises: Obtain laser point cloud data during relative positioning of the trolley; Segmenting the laser point cloud data to separate multiple laser segments; Filter out effective laser segments by calculating the lengths of the plurality of laser segments; Using a first scoring function on the effective laser segments, determining a key laser segment; Using a second scoring function on the key laser segment, determining key points, and outputting midpoint poses; Matching the key points with the key coordinates of a pre-established template to obtain the relative pose of the midpoint pose and the center point of the cart, wherein the template is a mathematical model established based on the predicted length and width of the cart; According to the relative posture, a nonlinear optimization problem is constructed, and an optimal solution of the relative posture is obtained by solving the problem; The optimal solution of the relative posture is transformed to position the trolley and achieve docking.

2. The trolley docking and positioning method according to claim 1, characterized in that: The segmenting process of the laser point cloud data to separate a plurality of laser segments includes: Initialize a laser segment; Randomly select an unmarked point from the laser point cloud data, add it to the laser segment, and mark it; Traverse the remaining unmarked points; Calculating the distance between the remaining unmarked points and the nearest point in the laser segment; In response to the distance being greater than or equal to a preset distance threshold, continue to traverse the next remaining unmarked point; in response to the distance being less than the distance threshold, add the remaining unmarked point to the laser segment and mark it; In response to a new point being added to the laser segment, continuing the next round of traversal and judgment; in response to no new point being added, separating the laser segment; Determining whether there are any unmarked points in the laser point cloud data; In response to the presence of unmarked points, the above steps are repeated to continue initializing the laser segments and complete the separation; in response to the absence of unmarked points, the segmentation is terminated to obtain a plurality of laser segments.

3. The trolley docking and positioning method according to claim 1, characterized in that: The method of selecting effective laser segments by calculating the lengths of the plurality of laser segments comprises: extracting any one laser segment from the plurality of laser segments; Calculate the distance between any two points in the laser segment, take the longest distance as the length of the laser segment, and calculate the coordinates of the midpoint of the longest distance as the midpoint of the laser segment; In response to the length of the laser segment being greater than or equal to a preset length threshold, deleting the laser segment; in response to the length of the laser segment being less than the length threshold, retaining the laser segment; Determining whether there is any unextracted laser segment among the plurality of laser segments; In response to the existence of unextracted laser segments, the above steps are repeated to continue extracting laser segments and complete the screening; in response to the absence of unextracted laser segments, the screening is ended to obtain valid laser segments.

4. The trolley docking and positioning method according to claim 1, characterized in that: The step of using a first scoring function on the effective laser segments to determine the key laser segments comprises: According to the pre-measured length and width of the cart, calculate the lengths of the three sides of the template triangle: ， ， ， In the formula, is the long right-angled side of the template triangle, is the short right-angle side of the template triangle, is the hypotenuse of the template triangle, is the length of the cart, is the width of the cart, is a function that returns the square root of the sum of the squares of its two arguments; The midpoints of the effective laser segments are grouped into groups of three to form a plurality of triangles; Extracting any one triangle from the plurality of triangles; Initialize the first array and the second array; Calculate the lengths of the three sides of the triangle based on the coordinates of the three midpoints of the triangle ; The lengths of the three sides of the triangle Put them into the first array in ascending order, and add the lengths of the three sides of the template triangle Put them into the second array in ascending order; The triangle is scored using a first scoring function to obtain a first score, wherein the first scoring function is: ， In the formula, is the first score, is a function that returns an absolute value, is the first array, is the second array; In response to the first score being less than or equal to a preset first score threshold, sorting the first scores; Determine whether there is any unextracted triangle among the plurality of triangles; In response to the existence of unextracted triangles, repeat the above steps to continue extracting triangles and obtain a first score; in response to the absence of unextracted triangles, end the first round of scoring, and record the three valid laser segments corresponding to the triangle with the lowest first score as key laser segments.

5. The trolley docking and positioning method according to claim 4, characterized in that: The step of using a second scoring function on the key laser segment to determine a key point and outputting a midpoint pose includes: Extract a point arbitrarily from each of the key laser segments to form a triangle; Initialize the third array and the fourth array; Calculate the lengths of the three sides of the triangle based on the coordinates of the three points that make up the triangle ; The lengths of the three sides of the triangle Put them into the third array in ascending order, and change the lengths of the three sides of the template triangle Put into the fourth array in ascending order; The triangle is scored using a second scoring function to obtain a second score, wherein the second scoring function is: ， In the formula, is the second score, is a function that returns an absolute value, For the third array, is the fourth array; In response to the second score being less than or equal to a preset second score threshold, sorting the second scores; Determine whether there are unextracted points in the first key laser segment; In response to the existence of unextracted points, repeating the above steps to continue forming triangles and obtaining a second score; in response to the absence of unextracted points, determining whether there are unextracted points in the second key laser segment; In response to the existence of unextracted points, repeating the above steps to continue forming a triangle and obtaining a second score; in response to the absence of unextracted points, determining whether there are unextracted points in the third key laser segment; In response to the existence of unextracted points, repeat the above steps to continue to form triangles and obtain a second score; in response to the absence of unextracted points, end the second round of scoring, and record the three side lengths and three points corresponding to the triangle with the lowest second score as key lengths and key points.

6. The trolley docking and positioning method according to claim 5, characterized in that: The method uses a second scoring function on the key laser segment to determine the key point and output the midpoint pose, further comprising: Sorting the critical lengths in descending order to determine the hypotenuse and the long right-angled side in the critical lengths; According to the coordinates of the key points corresponding to the hypotenuse, calculate the horizontal coordinate of the midpoint of the hypotenuse , vertical coordinate and attitude angle , where the attitude angle The direction is the direction in which the midpoint of the hypotenuse points perpendicularly to the long right-angled side; Output midpoint pose .

7. The trolley docking and positioning method according to claim 6, characterized in that: The step of matching the key point with the key coordinates of the pre-established template to obtain the relative position and posture of the midpoint and the center point of the cart includes: Taking the midpoint of the hypotenuse as the origin, the attitude angle points in the direction Axis positive direction, establish the midpoint pose coordinate system; The coordinates of the key points Transformed into the coordinates of the midpoint pose coordinate system , where the conversion formula is: ， ， ， ， In the formula, For the The horizontal coordinates of the key points, is the horizontal coordinate of the midpoint pose, For the The vertical coordinates of the key points, is the ordinate of the midpoint pose, is the function for calculating the inverse tangent value, is the attitude angle of the midpoint pose, For the The horizontal coordinate of the key point in the midpoint pose coordinate system, For the The vertical coordinate of the key point in the midpoint pose coordinate system, is the function for calculating the cosine value, is the function for calculating the sine value, ; Using the iterative closest point algorithm, the key points Match the key coordinates of the pre-established template to obtain the matching points corresponding to the key points , and generate the relative pose of the cart center point relative to the midpoint pose , wherein the key coordinates are the leg position coordinates determined with the center point of the trolley as the origin, the matching point is the point in the leg position coordinates closest to the key point, and the relative pose is initially set to .

8. The trolley docking and positioning method according to claim 1, characterized in that: The step of constructing a nonlinear optimization problem according to the relative posture and solving the problem to obtain an optimal solution for the relative posture includes: Construct a nonlinear optimization problem about the relative pose: ， ， ， In the formula, For the The coordinates of the key points in the midpoint pose coordinate system, is the coordinate of the relative pose in the midpoint pose coordinate system, For the The coordinates of the matching points corresponding to the key points, is the horizontal coordinate of the relative pose in the midpoint pose coordinate system, is the ordinate of the relative pose in the midpoint pose coordinate system, For the The horizontal coordinate of the matching point corresponding to the key point, For the The ordinate of the matching point corresponding to the key point, is the function for calculating the cosine value, is the function for calculating the sine value, is the function for calculating the inverse tangent value, is the attitude angle of the relative posture in the midpoint posture coordinate system, ; Perform nonlinear optimization on the nonlinear optimization problem and solve it to obtain the optimal solution of the relative posture .

9. The trolley docking and positioning method according to claim 1, characterized in that: The step of converting the optimal solution of the relative posture to position the trolley and achieve docking includes: The coordinates of the optimal solution of the relative pose Convert to the coordinates of the laser radar coordinate system , where the conversion formula is: ， ， ， ， In the formula, is the horizontal coordinate of the optimal solution of relative posture in the midpoint posture coordinate system, is the ordinate of the optimal solution of relative posture in the midpoint posture coordinate system, is the attitude angle of the optimal solution of relative posture in the midpoint posture coordinate system, is the horizontal coordinate of the midpoint pose, is the ordinate of the midpoint pose, is the attitude angle of the midpoint pose, is the function for calculating the inverse tangent value, is the function for calculating the cosine value, is the function for calculating the sine value; According to the coordinates of the optimal solution of the relative posture in the laser radar coordinate system , positioning the trolley and achieving docking.

10. A computer device, characterized in that: including storage media and processors; The storage medium is used to store instructions; The processor is configured to operate according to the instructions to execute the steps of the method according to any one of claims 1 to 9.

Images