Cage trolley docking method, cage trolley docking device and computer storage medium

Abstract

The invention provides a cage trolley docking method, a cage trolley docking device and a computer storage medium. The method comprises the following steps: acquiring an original point cloud of a cage trolley area; generating a plurality of candidate cage trolley trundle points according to a plurality of point cloud clusters in the original point cloud; screening out at least one candidate cage trolley trundle combination from the plurality of candidate cage trolley trundle points according to an issued preset cage trolley model; matching the candidate cage trolley trundle combination with each preset cage trolley model to obtain a matched cage trolley model and a matched cage trolley trundle combination corresponding to the matched cage trolley model; taking the trapezoidal orientation of the matched cage trolley model as the current orientation of the current cage trolley; generating the current lifting point of the current cage trolley according to the center point of the matched cage trolley trundle combination and the lifting position offset; and carrying out butt joint according to the current orientation of the current cage trolley and the current lifting point. Through the cage trolley butt joint method, the application scene that a plurality of cage trolleys with different sizes are placed on one site is supported, the corresponding cage trolley types can be distinguished, and the real positions of the cage trolleys can be estimated.

Description

Technical Field

[0001] The present application relates to the field of detection and identification technology, and in particular to a cage-car docking method, a cage-car docking device, and a computer storage medium. Background Art

[0002] In recent years, the rapid expansion of mobile robots in the industrial field has strongly promoted the progress of Industry 4.0. Compared with industrial robots, mobile robots are more flexible and can complete tasks autonomously in a certain scene environment, which greatly liberates productivity. Therefore, they have extremely high application value in the manufacturing industry, logistics industry and other fields. Mobile robots need to accurately determine the position and posture of cage cars so that they can accurately perform relevant operations on cage cars.

[0003] However, the current cage truck identification solution requires specifying the cage truck model to be identified. When there are multiple cage truck models of different sizes on site, it is unable to autonomously distinguish the corresponding cage truck models, resulting in low efficiency and accuracy in matching the cage truck with the model. Summary of the invention

[0004] In order to solve the above technical problems, the present application proposes a cage-car docking method, a cage-car docking device and a computer storage medium.

[0005] In order to solve the above technical problems, the present application proposes a cage car docking method, which comprises:

[0006] Get the original point cloud of the cage area;

[0007] Generate a number of candidate cage trolley caster points according to a number of point cloud clusters in the original point cloud;

[0008] From the plurality of candidate cage trolley caster points, at least one candidate cage trolley caster combination is screened out according to the issued preset cage trolley model;

[0009] Matching the candidate cage car caster combination with each preset cage car model to obtain a matching cage car model and its corresponding matching cage car caster combination;

[0010] The trapezoidal orientation of the matching cage car model is used as the current orientation of the current cage car;

[0011] Generating a current lifting point of the current cage vehicle according to the center point of the matching cage vehicle caster assembly and the lifting position offset;

[0012] Docking is performed according to the current orientation and the current lifting point of the current cage truck.

[0013] Wherein, after obtaining the original point cloud of the cage car area, the cage car docking method further includes:

[0014] Generate a screening interval according to the preset cage car model and the issued ideal position of the cage car;

[0015] The point cloud set of the original point cloud within the screening interval is used as the screening point cloud.

[0016] The step of selecting at least one candidate cage trolley caster combination according to the issued preset cage trolley model includes:

[0017] Determine the screening interval according to the longest side and the shortest side in the preset cage car model;

[0018] Traversing the combinations of the candidate cage car caster points in pairs, and taking the two candidate cage car caster points whose distances are located in the screening interval as the first candidate cage car caster combination;

[0019] Based on the first candidate cage trolley caster combination, other candidate cage trolley caster points are added to generate a second candidate cage trolley caster combination;

[0020] The second candidate cage trolley caster combination that meets the preset trapezoidal condition is used as the candidate cage trolley caster combination, and the candidate cage trolley caster points in the candidate cage trolley caster combination are sorted.

[0021] Wherein, the second candidate cage trolley caster combination is a cage trolley caster combination of three candidate cage trolley caster points and / or a cage trolley caster combination of four candidate cage trolley caster points;

[0022] The step of using the second candidate cage trolley caster combination that meets the preset trapezoidal condition as the candidate cage trolley caster combination comprises:

[0023] Determine whether there is a cage car caster combination of four candidate cage car caster points that meets the isosceles trapezoid condition;

[0024] If not, determine whether the cage car caster combination of the three candidate cage car caster points meets the specific length triangle condition of the trapezoidal model;

[0025] If so, the second candidate cage trolley caster combination that meets the specific length triangle condition of the trapezoidal model is used as the candidate cage trolley caster combination.

[0026] Wherein, after the second candidate cage car caster combination that meets the specific length triangle condition of the trapezoidal model is used as the candidate cage car caster combination, the cage car docking method further includes:

[0027] Determine the coordinates of the corner points of the trapezoidal model corresponding to the three candidate cage trolley caster points of the candidate cage trolley caster combination, as well as the coordinates of the remaining unidentified corner points to be determined;

[0028] Determine the trapezoidal model of the candidate cage trolley caster combination according to the coordinates of the corner points of the trapezoidal model;

[0029] The trapezoidal model is used to solve the coordinates of the corner point to be determined.

[0030] The method of using the second candidate cage car caster combination that meets the preset trapezoidal condition as the candidate cage car caster combination and sorting the candidate cage car caster points in the candidate cage car caster combination includes:

[0031] Obtain a first candidate edge and a second candidate edge in the second candidate cage trolley caster combination;

[0032] In response to the first candidate side and the second candidate side being parallel, and the length difference between the first connecting line and the second connecting line being less than a preset length threshold, confirming the second candidate cage trolley caster combination as the candidate cage trolley caster combination; wherein the first connecting line and the second connecting line have the same candidate cage trolley caster point;

[0033] Determine a candidate waistline based on the size relationship between the first connecting line and the second connecting line;

[0034] Obtaining a first up-down relationship between the candidate cage car caster point and the candidate waistline, and a second up-down relationship between the candidate cage car caster point and the first candidate edge;

[0035] The trapezoidal corner point sequence of the candidate cage trolley caster combination is determined according to the size relationship, the first up-down relationship, and the second up-down relationship.

[0036] The step of matching the candidate cage trolley caster combination with each preset cage trolley model to obtain a matching cage trolley model and its corresponding matching cage trolley caster combination includes:

[0037] According to the edge size matching of the candidate cage car caster combination with a preset cage car model whose edge size error value is less than a preset size threshold, a first candidate combination of the candidate caster combination and the candidate cage car model is obtained;

[0038] Acquire a first candidate combination with the smallest size error value, and a second candidate combination whose size error value difference with the first candidate combination is less than a preset difference threshold;

[0039] Obtain a candidate combination whose center position of the candidate caster combination is closest to the issued initial cage car center position in the first candidate combination and the second candidate combination, and obtain the matching cage car model and its corresponding matching cage car caster combination.

[0040] Wherein, after obtaining the matching cage car model and its corresponding matching cage car caster combination, the cage car docking method further includes:

[0041] Obtaining the trapezoidal center point of the matching cage trolley caster combination;

[0042] Obtaining a rotation matrix according to the order of the trapezoidal corner points of the matching cage trolley caster combination;

[0043] Using the size of the matching cage car model, the trapezoid center point and the rotation matrix, the coordinates of the first corner point of the upright cage car model are obtained;

[0044] Rotate the first corner point coordinates by 180 degrees to obtain the second corner point coordinates of the inverted cage car model;

[0045] Obtain the caster coordinates of the matching cage trolley caster combination and make differences with the first corner point coordinates and the second corner point coordinates respectively in the order of the trapezoid corner points, calculate the first error sum of the matching cage trolley caster combination and the upright cage trolley model, and the second error sum of the matching cage trolley caster combination and the inverted cage trolley model;

[0046] The trapezoidal model of the matching cage trolley caster combination is determined according to the smaller value of the first error sum and the second error sum.

[0047] In order to solve the above technical problems, the present application also proposes a cage truck docking device, which includes a memory and a processor coupled to the memory; wherein the memory is used to store program data, and the processor is used to execute the program data to implement the cage truck docking method as described above.

[0048] In order to solve the above technical problems, the present application also proposes a computer storage medium, which is used to store program data. When the program data is executed by a computer, it is used to implement the above cage truck docking method.

[0049] Compared with the prior art, the beneficial effects of the present application are as follows: the cage car docking device obtains the original point cloud of the cage car area; generates several candidate cage car caster points according to several point cloud clusters in the original point cloud; selects at least one candidate cage car caster combination from the several candidate cage car caster points according to the preset cage car model issued; matches the candidate cage car caster combination with each preset cage car model to obtain a matching cage car model and its corresponding matching cage car caster combination; uses the trapezoidal orientation of the matching cage car model as the current orientation of the current cage car; generates the current lifting point of the current cage car according to the center point of the matching cage car caster combination and the lifting position offset; docks according to the current orientation and current lifting point of the current cage car. The above-mentioned cage car docking method supports an application scenario in which multiple cage cars of different sizes are placed on site, and can distinguish the corresponding cage car types and estimate the actual position of the cage car. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0051] in:

[0052] Figure 1 It is a flow chart of an embodiment of a cage-car docking method provided by the present application;

[0053] Figure 2 It is a schematic diagram of the principle flow of cage wheel detection and precise docking provided by this application;

[0054] Figure 3 is a device schematic diagram of an embodiment of a mobile robot provided by the present application;

[0055] Figure 4 It is a schematic diagram of the definition of the angle orientation of the trapezoid and the order of the four corner points provided by the present application;

[0056] Figure 5 yes Figure 1 The specific flow diagram of step S13 of the cage-car docking method shown;

[0057] Figure 6 yes Figure 5 The specific flow diagram of step S134 of the cage-car docking method shown;

[0058] Figure 7 It is a schematic diagram of the order of the trapezoidal casters corresponding to the point cloud clusters provided by this application;

[0059] Figure 8 yes Figure 1 The specific flow diagram of step S14 of the cage-car docking method shown;

[0060] Fig. 9 is a schematic diagram of the inverted trapezoidal model provided by the present application and the corresponding corner point sequence;

[0061] Fig.10 It is a structural schematic diagram of an embodiment of a cage-car docking device provided by the present application;

[0062] Fig.11 It is a structural diagram of an embodiment of a computer storage medium provided by the present application. DETAILED DESCRIPTION

[0063] The following will be combined with the drawings in the embodiments of the present application to clearly and completely describe the technical solutions in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0064] The terms "first", "second", "third", "fourth", etc. (if any) in the specification and claims of the present application and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchangeable where appropriate, so that the embodiments of the present application described herein can, for example, be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "having" and any of their variations are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units that are clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0065] The technical concepts and technical terms involved in this application include but are not limited to:

[0066] (1) When a mobile robot is performing a task, such as moving objects in a warehouse, it needs to know its exact position in the changing environment so that it can perform the task accurately.

[0067] (2) Odometry: It is used to preliminarily estimate the changes in the angle and distance of the mobile robot. A common example is a wheel encoder. It can estimate the robot's current posture based on the change in posture between the previous and next moments and the posture at the previous moment.

[0068] (3) Two-dimensional LiDAR: A sensor used to obtain two-dimensional plane information and detect the two-dimensional plane contour information of the surrounding environment.

[0069] (4) Cage trolley detection: After accurately detecting the wheels of the cage trolley through the obstacle avoidance laser, the corresponding cage trolley size can be distinguished from the multi-model trapezoidal cage trolleys and the actual lifting position and direction of the cage trolley can be estimated. The odometer navigation method is used to accurately find the cage trolley target point and lift the cage trolley.

[0070] The present application mainly relates to the autonomous identification of various cage car sizes in a mobile AGV (Automated Guided Vehicle) navigation environment, and in particular to screening out models that meet the observation from cage cars of multiple sizes based on radar data, calculating the lifting point and orientation of the cage car, and performing high-precision docking.

[0071] This application is based on the four wheels of the cage car that form a trapezoid. The point cloud clusters on the wheels are screened out from the laser data, and whether the trapezoidal requirements are met is determined based on whether the straight lines constructed by the point cloud clusters are parallel and whether the diagonals of the parallel sides are equal. The order of the point cloud clusters corresponding to the wheels of the trapezoid is determined to calculate the center and direction of the trapezoid, and the model that meets the observed trapezoidal size is screened out from the multi-model trapezoidal size issued, and then the lifting point is accurately calculated based on the relative position of the lifting point specified in the corresponding cage car model relative to the center of the trapezoid. After identifying the lifting point, the AGV is switched to the odometer mode for docking. During the docking process, continuous identification is used to eliminate odometer errors to ensure the final docking accuracy.

[0072] Please refer to Figure 1 and Figure 2 , Figure 1 is a flow chart of an embodiment of a cage-car docking method provided by the present application, Figure 2 It is a schematic diagram of the principle flow of cage wheel detection and precise docking provided by this application.

[0073] The cage car docking method of the present application is applied to a cage car docking device, wherein the cage car docking device of the present application can be a server, a terminal device, or a system composed of a server and a terminal device. Accordingly, the various parts of the cage car docking device, such as various units, subunits, modules, and submodules, can all be set in the server, can all be set in the terminal device, or can be set in the server and the terminal device respectively.

[0074] Furthermore, the above-mentioned server can be hardware or software. When the server is hardware, it can be implemented as a distributed server cluster consisting of multiple servers, or it can be implemented as a single server. When the server is software, it can be implemented as multiple software or software modules, such as software or software modules used to provide distributed servers, or it can be implemented as a single software or software module, which is not specifically limited here.

[0075] like Figure 1 As shown, the specific steps are as follows:

[0076] Step S11: Obtain the original point cloud of the cage car area.

[0077] In the embodiment of the present application, the cage car docking device collects the original point cloud of the cage car area through a mobile robot, such as the radar sensor carried on the AGV shown above. The cage car area is the area monitored by the AGV where at least one cage car is placed.

[0078] Specifically, this application provides a mobile robot based on an odometer and a 2D laser radar sensor. Figure 3, Figure 3 It is a device schematic diagram of an embodiment of a mobile robot provided by the present application.

[0079] The mobile robot used in this application includes a mobile chassis (including but not limited to: motion controller, motor, battery, embedded computer, odometer, etc.), a navigation laser radar (2D laser radar) and an obstacle avoidance laser radar (2D obstacle avoidance laser radar) installed on the vehicle body.

[0080] Figure 3 The basic functions of the mobile robot shown are introduced as follows: The robot uses the installed 2D laser radar to scan the environmental contours of walls, work machines, cage cars, building supports, etc. in workshops, warehouses and other environments to build an environmental map used for navigation. The mobile robot provided in this application uses an odometer to estimate the change in the robot's movement, and uses one or more 2D laser radars installed on the vehicle body to scan the environmental contours and match them with the grid map for positioning and navigation. When the robot enters the cage car identification area, it continues to identify the cage car, calculates the lifting position of the cage car, adjusts its own trajectory, and the odometer navigates to the lifting point to lift the cage car.

[0081] Cage car docking device Figure 3 The mobile robot shown reads the fork tip 2D lidar point cloud data and removes motion distortion according to the timestamp of each point cloud. The cage car docking device converts the issued trapezoid center position into the lidar coordinate system using the current robot posture and laser external parameters. The calculation formula is shown in Formula 1.

[0082]

[0083] in: That is, the position and posture of the cage car in the laser radar coordinate system; ——External parameters of the laser radar relative to the robot; That is, the position and posture of the robot relative to the map coordinate system; That is, the position and posture of the cage truck in the map coordinate system is identified this time.

[0084] The cage docking device initially filters the laser points. Specifically, the cage docking device uses the cage position in the preset map as the origin and the radius of γ times the length of the hypotenuse of the cage model with the longest hypotenuse in the model library to generate a circular screening interval and calculate the distance from each point cloud to the If the distance is greater than the radius of the circle, the points outside the circular area are filtered out. Secondly, the cage car docking device traverses the laser points according to the index and determines the distance between adjacent points. If the distance is less than the threshold d p It is considered to be the same point cloud cluster, the angles of adjacent points are calculated, and the tailing point cloud is removed.

[0085] Step S12: generating a number of candidate cage trolley caster points according to a number of point cloud clusters in the original point cloud.

[0086] In an embodiment of the present application, the cage trolley docking device can generate several candidate cage trolley caster points based on several point cloud clusters in the original point cloud. In order to obtain more accurate data, several candidate cage trolley caster points can also be generated based on several point cloud clusters in the preliminary filtered point cloud.

[0087] Specifically, after obtaining several point cloud clusters, the cage car docking device calculates whether the length of the point cloud cluster meets the threshold and whether the number of points in the point cloud cluster meets the threshold, thereby eliminating noise point cloud clusters. If the threshold is met, the center p of the point cloud cluster is calculated according to the following formula (2): c (x c ,y c ):

[0088]

[0089] where x i ,y i It is the horizontal and vertical coordinates of the point in the point cloud cluster and correspondingly saves the coordinates of the point in the point cloud cluster.

[0090] The cage docking device places the center of each point cloud cluster p c (x c ,y c ), as a candidate cage caster point.

[0091] Step S13: at least one candidate cage trolley caster combination is selected from a plurality of candidate cage trolley caster points according to the issued preset cage trolley model.

[0092] In an embodiment of the present application, the cage trolley docking device combines a plurality of candidate cage trolley caster points into a plurality of candidate cage trolley caster combinations, and screens the candidate cage trolley caster combinations by the cage trolley size, cage trolley position and other conditions of the issued preset cage trolley model, thereby obtaining an accurate cage trolley caster combination, which is conducive to the accurate identification of the cage trolley.

[0093] Specifically, in robot navigation, there will be a topological map to mark the moving route of the mobile robot and the target points that the robot needs to pass and reach. At the same time, the cage car area will be marked, indicating that the cage car needs to be moved to the ideal location. When detecting the cage car, the robot first navigates through the odometer and lidar, allowing the equipment to reach the cage car identification area to start cage car identification. Then the upper layer, such as the management platform, sends down all the cage car models. Each model contains the outer edge length L and width w of the cage car, the long parallel side length TL, short parallel side length Ts, waist Tw and diagonal length Td of the trapezoid formed by the cage wheels, as well as the ideal position P of the cage car in the map and the ideal cage car angle in the map. Figure 4As shown, the angle of the cage car is the direction of the trapezoid when the long side is placed upright. Figure 4 It is a schematic diagram of the definition of the angle orientation of the trapezoid and the order of the four corner points provided in this application.

[0094] Among all candidate trapezoidal wheel centers, first select a combination of four points that meet the trapezoidal conditions, and compare them with the m cage car models in turn to select the cage car model with a higher matching degree.

[0095] Please continue to refer to the detailed screening process. Figure 5 , Figure 5 yes Figure 1 The specific flow chart of step S13 of the cage truck docking method is shown.

[0096] like Figure 5 As shown, the specific steps are as follows:

[0097] Step S131: Determine the screening interval according to the longest side and the shortest side in the preset cage car model.

[0098] Step S132: traverse the combinations of candidate cage trolley caster points in pairs, and take the two candidate cage trolley caster points whose distance between the two points is within the screening interval as the first candidate cage trolley caster combination.

[0099] In the embodiment of the present application, the cage car docking device takes the shortest side of the cage car in all cage car models as L min , take the longest side as L max When two candidate cage car caster points are obtained, calculate whether the distance between the two candidate cage car caster points is between (γ·L min ,δ·L max )(γ, δ are proportional coefficients respectively, for example, γ = 0.8, δ = 1.2)(Condition (1)). If it is satisfied, the edge formed by the two candidate cage car caster points may be a candidate cage car point cloud cluster, otherwise it is filtered.

[0100] Step S133: Based on the first candidate cage trolley caster combination, other candidate cage trolley caster points are added to generate a second candidate cage trolley caster combination.

[0101] In an embodiment of the present application, the cage trolley docking device continues to add one or two other candidate cage trolley caster points on the basis of the first candidate cage trolley caster combination to form a cage trolley caster combination of three candidate cage trolley caster points, or a cage trolley caster combination of four candidate cage trolley caster points.

[0102] At this time, the cage car docking device can use the set record to record the combination of three points or four points that have been traversed. If the indexes of the traversed four points are already in the historical combination, they are directly skipped to speed up the traversal search (condition (2)).

[0103] Furthermore, if the combination of the three candidate trolley caster points of the second candidate trolley caster combination is collinear, the three candidate trolley caster points are directly skipped, that is, the candidate trolley caster combination composed of the three candidate trolley caster points is filtered out (condition (3)).

[0104] Step S134: taking the second candidate cage trolley caster combination that meets the preset trapezoidal condition as the candidate cage trolley caster combination, and sorting the candidate cage trolley caster points in the candidate cage trolley caster combination.

[0105] Specifically, among the n candidate cage legs {p1, p2, p3, ..., p n}, the cage docking device selects the combination of four points that meet the conditions. First, take the two points {p1, p2} and calculate whether they meet condition (1). If not, select {p1, p3}... and so on. If it meets, select p3 to obtain the combination of {p1, p2, p3}, and calculate whether it meets condition (3). If it meets, select {p1, p2, p4}... and so on. If it does not meet, continue to select p4 to obtain {p1, p2, p3, p4}, and calculate whether it meets the trapezoid condition. For the conditions for judging the trapezoid, see the description of the isosceles trapezoid condition. If not, select {p1, p2, p3, p5}... and so on. According to this rule, all candidate cage casters are traversed in turn to obtain all combinations of four cage casters that meet the requirements, and complete a quick preliminary screening.

[0106] Furthermore, the cage car docking device determines that the combination of four casters satisfies the isosceles trapezoid condition.

[0107] The cage car docking device obtains several groups of four different point combinations, such as {p l1 , p l2 , p l3 , p l4}. l1 , p l2 and p l3 , p l4 As edge, with p l1 , p l3 and p l2 , p l4 For the edge and p l1 , p l4 and p l2 , p l3 As the side to determine whether it satisfies the trapezoid. l1 , p l2 and p l3 , p l4 Take the edge as an example to introduce the specific method:

[0108] 1) With p l1 , p l2 and p l3, p l4 For the sides, determine whether the two sides are parallel, that is, whether formula (3) is established. If established, continue to determine whether the difference between L13 and L14 is less than the length threshold L1_th (for example, 5cm), that is, L13 and L14 are either the length of the waist or the length of the diagonal. If they are equal, it is considered that the four casters can form an isosceles trapezoid;

[0109]

[0110] Among them: L12 is the length of the straight line composed of plp2; L34 is the length of the straight line composed of p3p4; |*| is the vector modulus or the absolute value of the value.

[0111] Furthermore, the cage car docking device sorts the caster points of the candidate cage car caster combinations that meet the isosceles trapezoid condition. Figure 6 , Figure 6 yes Figure 5 The specific flow chart of step S134 of the cage truck docking method is shown.

[0112] like Figure 6 As shown, the specific steps are as follows:

[0113] Step S1341: Obtain the first candidate edge and the second candidate edge in the second candidate cage trolley caster combination.

[0114] Step S1342: In response to the first candidate side and the second candidate side being parallel, and the length difference between the first connecting line and the second connecting line being less than a preset length threshold, confirming the second candidate cage trolley caster combination as the candidate cage trolley caster combination.

[0115] Among them, the first connecting line and the second connecting line have the same candidate cage car caster point

[0116] Step S1343: Determine a candidate waistline based on the size relationship between the first connecting line and the second connecting line.

[0117] Step S1344: Obtain a first up-down relationship between the candidate cage trolley caster point and the candidate waistline, and a second up-down relationship between the candidate cage trolley caster point and the first candidate edge.

[0118] Step S1345: Determine the order of the trapezoidal corner points of the candidate cage trolley caster combination according to the size relationship, the first up-down relationship, and the second up-down relationship.

[0119] In the embodiment of the present application, the cage car docking device determines whether L13 (the straight line composed of p1p3) is greater than L14. If so, p1p3 is considered to be a diagonal line, otherwise p1p4 is considered to be a diagonal line. The order of the trapezoidal corner points corresponding to the four point cloud clusters is determined by determining whether p4 is above or below the coordinate system composed of p1p2 and whether p2 is above or below the coordinate system composed of p1p4.

[0120] Specific as Figure 7 As shown, Figure 7 This is a schematic diagram of the order of the trapezoidal casters corresponding to the point cloud cluster provided by this application. Let P12 be the vector formed by p1 pointing to p2, then the angle of the vector is:

[0121]

[0122] Among them, θ 12 The corresponding rotation matrix is ​​denoted as R 12 , then the coordinates of the vector in the coordinate system constructed with P14 and P12 as the x-axis are:

[0123]

[0124] if If the y value of is greater than zero, P4 is above P12; if it is less than zero, P4 is below P12. Similarly, use formulas (4) and (5) to calculate whether P2 is above or below the coordinate system formed by P14.

[0125] Specific as Figure 7 As shown, if p1p3 are diagonal sides, the order of the trapezoid corner points is as follows:

[0126] p4 is above p1p2, p2 is below p1p4 and L12 is greater than L34, then the order of the trapezoid corner points corresponding to the four points is 1234.

[0127] p4 is above p1p2, p2 is below p1p4 and L12 is less than or equal to L34, then the order of the trapezoid corner points corresponding to the four points is 3412.

[0128] p4 is below p1p2, p2 is above p1p4 and L12 is greater than L34, then the order of the trapezoid corner points corresponding to the four points is 2143.

[0129] p4 is below p1p2, p2 is above p1p4 and L12 is less than or equal to L34, then the order of the trapezoid corner points corresponding to the four points is 4321.

[0130] If p1p3 is the waist of the trapezoid, the order of the corner points of the trapezoid is as follows:

[0131] p3 is above p1p2, p2 is below p1p3 and L12 is greater than L34, then the order of the trapezoid corner points corresponding to the four points is 1243.

[0132] p3 is above p1p2, p2 is below p1p3 and L12 is less than or equal to L34, then the order of the trapezoid corner points corresponding to the four points is 4312.

[0133] p3 is below p1p2, p2 is above p1p3 and L12 is greater than L34, then the order of the trapezoid corner points corresponding to the four points is 2134.

[0134] p3 is below p1p2, p2 is above p1p3 and L12 is less than or equal to L34, then the order of the trapezoid corner points corresponding to the four points is 3421.

[0135] Step S14: Match the candidate cage trolley caster combination with each preset cage trolley model to obtain a matching cage trolley model and its corresponding matching cage trolley caster combination.

[0136] In the embodiment of the present application, the cage car docking device determines multiple candidate cage car caster combinations using step S13, and at least determines the candidate cage car caster points of each candidate cage car caster combination for matching with each preset cage car model issued. Figure 5 and Figure 6 Further screening multiple candidate cage car caster combinations according to the isosceles trapezoid condition, and sorting the corner point order of the candidate cage car caster points of the candidate cage car caster combinations that meet the isosceles trapezoid condition, so as to match them with each preset cage car model, can effectively improve the matching efficiency and accuracy.

[0137] Please continue to read Figure 8 , Figure 8 yes Figure 1 The specific flow chart of step S14 of the cage truck docking method is shown.

[0138] like Figure 8 As shown, the specific steps are as follows:

[0139] Step S141: Matching the edge size of the candidate cage trolley caster combination with a preset cage trolley model whose edge size error value is less than a preset size threshold according to the edge size of the candidate cage trolley caster combination, to obtain a first candidate combination of the candidate caster combination and the candidate cage trolley model.

[0140] In the embodiment of the present application, the cage car docking device determines the order of the trapezoidal corner points corresponding to the point cloud, and then compares it with all the cage car models {{TL1, TS1, TW1}, ..., {TL m ,TS m ,TW m}}, M1 is the long side of L12 and L34, M2 is the short side of L12 and L34, and M3 is the waist length of the trapezoid, then it must satisfy:

[0141]

[0142] Save the cage car leg combination and the model that meets the requirements, for example, {{p l1 ,p l2 ,p l3 ,p l4},{TL1,TS1,TW1}},{{p l2 , p l3 , p l4 , p l5}, {TL2, TS2, TW2}}.

[0143] Step S142: obtaining a first candidate combination having a minimum size error value and a second candidate combination having a size error value whose difference with the first candidate combination is less than a preset difference threshold.

[0144] Step S143: Obtain the candidate combination whose center position of the candidate caster combination is closest to the center position of the issued initial cage car in the first candidate combination and the second candidate combination, and obtain the matching cage car model and its corresponding matching cage car caster combination.

[0145] In the embodiment of the present application, the cage car docking device selects three error sums (err sum ) the smallest combination, and traverse to find whether there is |min(err sum )-err sumi |<γ err If there are multiple combinations, the cage car docking device selects the combination and model whose center position of the cage car leg combination is closest to the initial cage car center as the final matching model M c .

[0146] Step S144: Obtain the trapezoidal center point that matches the cage trolley caster combination.

[0147] In the embodiment of the present application, since the cage car may be an inverted model, that is, Fig. 9 As shown, Fig. 9 This is a schematic diagram of the inverted trapezoidal model and the corresponding corner point sequence provided by this application. It is necessary to determine whether it is an inverted model. The specific method is:

[0148] The cage car docking device calculates the distance between two parallel lines, that is, the distance from P1 to P3P4 can be obtained by formula (7):

[0149] d=| P41x*P31y-P31x*P41y| / L34 (7)

[0150] Assuming the model is upright, the center point of the trapezoid is:

[0151] T cen =(p1+p2+p3+p4) / 4

[0152] Step S145: Obtain the rotation matrix according to the order of the trapezoidal corner points that match the cage trolley caster combination.

[0153] Step S146: using the size, trapezoid center point and rotation matrix of the matching cage car model, the coordinates of the first corner point of the upright cage car model are obtained.

[0154] In the embodiment of the present application, the cage-car docking device uses the order of the corner points of the trapezoid to obtain the direction of the trapezoid as T by subtracting corner point 1 from corner point 2. θ , the corresponding rotation matrix is ​​R Tθ , use the rotation matrix and the TL, TS and D of the trapezoidal model to calculate the positions of the four corner points. The calculation formulas are shown in (8)-(11):

[0155] The coordinates of the lower left corner are:

[0156]

[0157] The coordinates of the lower right corner are:

[0158]

[0159] The coordinates of the upper right corner are:

[0160]

[0161] The coordinates of the upper left corner are:

[0162]

[0163] Step S147: Rotate the first corner point coordinates by 180 degrees to obtain the second corner point coordinates of the inverted cage car model.

[0164] In the embodiment of the present application, assuming that the trapezoid is inverted, the angle is rotated 180° and the coordinates of the four corner points of the inverted trapezoid are obtained by substituting them into the above formulas (8)-(11) and denoted as Z LD , Z RD , Z RU , Z LU .

[0165] Step S148: Obtain the caster coordinates of the matching cage trolley caster combination and make differences with the first corner point coordinates and the second corner point coordinates in the order of the trapezoidal corner points, calculate the first error sum between the matching cage trolley caster combination and the upright cage trolley model, and the second error sum between the matching cage trolley caster combination and the inverted cage trolley model.

[0166] Step S149: Determine a trapezoidal model that matches the cage trolley caster combination according to the smaller value of the first error sum and the second error sum.

[0167] In an embodiment of the present application, the cage trolley docking device calculates the sum of errors of the caster coordinates p1, p2, p3 and p4 obtained in the above steps and the coordinates of the corner points of the upright and inverted trapezoids respectively. The one with the smaller sum of errors is the corresponding trapezoid model, and the corresponding trapezoid orientation is the orientation of the current cage trolley.

[0168] Step S15: The trapezoidal orientation of the matching cage car model is used as the current orientation of the current cage car.

[0169] Step S16: Generate the current lifting point of the current cage car according to the center point of the matching cage car caster assembly and the lifting position offset.

[0170] In the embodiment of the present application, after obtaining the center and orientation of the trapezoid, since the lifting position is offset relative to the center of the trapezoid, the offset relative to the center of the trapezoid is recorded as SL and SW, then the coordinates of the lifting point are:

[0171]

[0172] Step S17: docking is performed according to the current orientation and the current lifting point of the cage truck.

[0173] In the embodiment of the present application, the cage car docking device is T LI The laser external parameters and the device posture are converted to the map coordinate system. At this time, the device uses the odometer mode to navigate to the lifting point for cage docking. The method of calculating the lifting center through four wheels in this application is also applicable to rectangular cage identification. Since the rectangle is a symmetrical shape, there is no need to consider whether it needs to be flipped.

[0174] Furthermore, due to the influence of the laser radar angular resolution, the position of the cage wheel, the recognition distance, etc., in some scenarios, only three casters of the cage car can be recognized. If there is no combination of 4 points that meet the conditions, the combination that meets the conditions of the specific length triangle of the sent trapezoidal model is selected, and the combination of the long side, waist or diagonal line and the short side with the smallest length corresponding to the sent model is found as the candidate cage car caster.

[0175] This application takes p1, p2, and p3 as an example, corresponding to the lower left corner, lower right corner, and upper right corner of the trapezoid respectively. If the formula:

[0176]

[0177] If it holds, then p1, p2, and p3 are considered to be the corner points at positions 1, 2, and 3 of the trapezoid respectively.

[0178] The cage car docking device uses formulas (8)-(11) and the trapezoidal model to calculate the virtual fourth point. For example, as in the above example, formula (11) can be used to calculate the virtual fourth trapezoidal corner point as the upper left corner point p4′, and p1, p2, p3 and p4′ are put into step S14 and subsequent steps to calculate the cage car center and cage car orientation.

[0179] Specifically, the cage car docking device calculates the virtual fourth point through formulas (8)-(11) and the trapezoidal model, and then determines the corner point sequence and the cage car orientation according to the above four candidate cage car caster points.

[0180] It should be noted that the cage car docking device compares the length of a parallel side, the length of the waist, and the length of the diagonal line in each cage car model with the triangle formed by the three known candidate cage car caster points. A cage car model with the smallest three length differences of the triangle formed by the three candidate cage car caster points is obtained as the trapezoidal model matching the three candidate cage car caster points, which is used to calculate the virtual fourth point.

[0181] This application estimates the position and posture of the mobile robot based on the odometer, laser sensor and prior map, and performs cage truck recognition and docking when entering the recognition area.

[0182] The cage car docking method of the present application first detects all cage wheels that meet certain conditions, and then determines whether it conforms to the trapezoidal model based on the parallelism and diagonal length of the straight line composed of the cage wheels, and selects the order of the trapezoidal corner points corresponding to the cage wheel point cloud cluster according to the relative position relationship of the three points in the wheel point cloud cluster, the length relationship of the parallel sides, and the diagonal side is longer than the waist. The direction of the longest side is used as the direction of the cage car; then the optimal model that meets the conditions is selected from the issued multiple models based on the length of the long and short sides and the waist. The virtual coordinates of the four corners of the trapezoid are calculated based on the long and short sides of the optimal model, the spacing of the parallel lines, and the direction of the long side. It is determined whether the distance between the coordinates of the currently observed point and the virtual coordinates is too large to determine whether the trapezoid is inverted, thereby deciding to flip the trapezoid angle. Finally, the lifting point is calculated using the relative length of the issued lifting point relative to the center of the trapezoid and the direction of the trapezoid, and the odometer mode is switched to navigate under the cage car. During the docking process, continuous identification is used to eliminate the odometer error to ensure the final docking accuracy.

[0183] The cage car docking method of the present application proposes a simple and fast screening mechanism for candidate cage car casters, designs multiple quick judgment conditions, and can greatly reduce the search time. A screening strategy combining cage car model and cage car position information is proposed, which can accurately identify cage cars and corresponding models when multiple cage cars of different sizes are placed in the load area.

[0184] The cage car docking method of the present application proposes a method of using whether the straight line formed by the casters is parallel to the right side, the relative position relationship between the caster coordinates, and the long and short sides to determine whether it conforms to the trapezoid and the order of the four caster coordinates corresponding to the corner points of the trapezoidal model to calculate the current cage car's orientation. By comparing the coordinates of the four corner points calculated theoretically by the trapezoid orientation and the trapezoidal model with the observed corresponding caster coordinates, it is determined whether the angle needs to be flipped. Finally, the lifting position is inferred by the desired trapezoid center, trapezoid orientation, and the offset relative to the center. This method is compatible with regular rectangular models.

[0185] The cage truck docking method of the present application, when a caster is blocked, uses whether the triangle formed by the three casters meets the length of the issued model to determine which three points of the trapezoid are the three casters, and then uses the corresponding trapezoidal model to virtually generate the fourth point. These four points are used to calculate the lifting center and direction using the trapezoidal judgment method of this patent, thereby improving the success rate of recognition.

[0186] Those skilled in the art will appreciate that, in the above method of specific implementation, the order in which the steps are written does not imply a strict execution order and does not constitute any limitation on the implementation process. The specific execution order of the steps should be determined by their functions and possible internal logic.

[0187] In order to realize the above cage car docking method, the present application also proposes a cage car docking device, please refer to Fig.10 , Fig.10 It is a structural schematic diagram of an embodiment of a cage-car docking device provided in the present application.

[0188] The cage-trolley docking device 400 of this embodiment includes a processor 41 , a memory 42 , an input / output device 43 , and a bus 44 .

[0189] The processor 41, the memory 42, and the input / output device 43 are respectively connected to the bus 44. The memory 42 stores program data, and the processor 41 is used to execute the program data to implement the cage-car docking method described in the above embodiment.

[0190] In the embodiment of the present application, the processor 41 may also be referred to as a CPU (Central Processing Unit). The processor 41 may be an integrated circuit chip having the ability to process signals. The processor 41 may also be a general-purpose processor, a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field programmable gate array (FPGA), or other programmable logic devices, discrete gates or transistor logic devices, or discrete hardware components. A general-purpose processor may be a microprocessor or the processor 41 may also be any conventional processor, etc.

[0191] This application also provides a computer storage medium, please continue to refer to Fig.11 , Fig.11 It is a structural diagram of an embodiment of a computer storage medium provided in the present application. The computer storage medium 600 stores a computer program 61. When the computer program 61 is executed by a processor, it is used to implement the cage truck docking method of the above embodiment.

[0192] When the embodiments of the present application are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application is essentially or the part that contributes to the prior art or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including several instructions to enable a computer device (which can be a personal computer, a server, or a network device, etc.) or a processor to perform all or part of the steps of the method described in each embodiment of the present application. The aforementioned storage medium includes: various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a disk or an optical disk.

[0193] The above description is only an implementation method of the present application, and does not limit the patent scope of the present application. Any equivalent structure or equivalent process transformation made using the contents of the present application specification and drawings, or directly or indirectly used in other related technical fields, are also included in the patent protection scope of the present application.

Claims

1. A cage car docking method, characterized in that: The cage car docking method comprises: Get the original point cloud of the cage area; Generate a number of candidate cage trolley caster points according to a number of point cloud clusters in the original point cloud; From the plurality of candidate cage trolley caster points, at least one candidate cage trolley caster combination is screened out according to the issued preset cage trolley model; Matching the candidate cage car caster combination with each preset cage car model to obtain a matching cage car model and its corresponding matching cage car caster combination; The trapezoidal orientation of the matching cage car model is used as the current orientation of the current cage car; Generating a current lifting point of the current cage vehicle according to the center point of the matching cage vehicle caster assembly and the lifting position offset; Docking is performed according to the current orientation and the current lifting point of the current cage truck.

2. The cage car docking method according to claim 1, characterized in that: After obtaining the original point cloud of the cage car area, the cage car docking method further includes: Generate a screening interval according to the preset cage car model and the issued ideal position of the cage car; The point cloud set of the original point cloud within the screening interval is used as the screening point cloud.

3. The cage car docking method according to claim 1, characterized in that: The step of selecting at least one candidate cage trolley caster combination according to the issued preset cage trolley model includes: Determine the screening interval according to the longest side and the shortest side in the preset cage car model; Traversing the combinations of the candidate cage car caster points in pairs, and taking the two candidate cage car caster points whose distances are located in the screening interval as the first candidate cage car caster combination; Based on the first candidate cage trolley caster combination, other candidate cage trolley caster points are added to generate a second candidate cage trolley caster combination; The second candidate cage trolley caster combination that meets the preset trapezoidal condition is used as the candidate cage trolley caster combination, and the candidate cage trolley caster points in the candidate cage trolley caster combination are sorted.

4. The cage car docking method according to claim 3, characterized in that: The second candidate cage trolley caster combination is a cage trolley caster combination of three candidate cage trolley caster points and / or a cage trolley caster combination of four candidate cage trolley caster points; The step of using the second candidate cage trolley caster combination that meets the preset trapezoidal condition as the candidate cage trolley caster combination comprises: Determine whether there is a cage car caster combination of four candidate cage car caster points that meets the isosceles trapezoid condition; If not, determine whether the cage car caster combination of the three candidate cage car caster points meets the specific length triangle condition of the trapezoidal model; If so, the second candidate cage trolley caster combination that meets the specific length triangle condition of the trapezoidal model is used as the candidate cage trolley caster combination.

5. The cage-car docking method according to claim 4, characterized in that: After the second candidate cage car caster combination that meets the specific length triangle condition of the trapezoidal model is used as the candidate cage car caster combination, the cage car docking method further includes: Determine the coordinates of the corner points of the trapezoidal model corresponding to the three candidate cage trolley caster points of the candidate cage trolley caster combination, as well as the coordinates of the remaining unidentified corner points to be determined; Determine the trapezoidal model of the candidate cage trolley caster combination according to the coordinates of the corner points of the trapezoidal model; The trapezoidal model is used to solve the coordinates of the corner point to be determined.

6. The cage-car docking method according to claim 3, characterized in that: The method of using the second candidate cage car caster combination that meets the preset trapezoidal condition as the candidate cage car caster combination, and sorting the candidate cage car caster points in the candidate cage car caster combination, comprises: Obtain a first candidate edge and a second candidate edge in the second candidate cage trolley caster combination; In response to the first candidate side and the second candidate side being parallel, and the length difference between the first connecting line and the second connecting line being less than a preset length threshold, confirming the second candidate cage trolley caster combination as the candidate cage trolley caster combination; wherein the first connecting line and the second connecting line have the same candidate cage trolley caster point; Determine a candidate waistline based on the size relationship between the first connecting line and the second connecting line; Obtaining a first up-down relationship between the candidate cage car caster point and the candidate waistline, and a second up-down relationship between the candidate cage car caster point and the first candidate edge; The trapezoidal corner point sequence of the candidate cage trolley caster combination is determined according to the size relationship, the first up-down relationship, and the second up-down relationship.

7. The cage-car docking method according to claim 1, characterized in that: The step of matching the candidate cage trolley caster combination with each preset cage trolley model to obtain a matching cage trolley model and its corresponding matching cage trolley caster combination includes: According to the edge size matching of the candidate cage car caster combination with a preset cage car model whose edge size error value is less than a preset size threshold, a first candidate combination of the candidate caster combination and the candidate cage car model is obtained; Acquire a first candidate combination with the smallest size error value, and a second candidate combination whose size error value difference with the first candidate combination is less than a preset difference threshold; Obtain a candidate combination whose center position of the candidate caster combination is closest to the issued initial cage car center position in the first candidate combination and the second candidate combination, and obtain the matching cage car model and its corresponding matching cage car caster combination.

8. The cage-car docking method according to claim 1, characterized in that: After obtaining the matching cage car model and its corresponding matching cage car caster combination, the cage car docking method further includes: Obtaining the trapezoidal center point of the matching cage trolley caster combination; Obtaining a rotation matrix according to the order of the trapezoidal corner points of the matching cage trolley caster combination; Using the size of the matching cage car model, the trapezoid center point and the rotation matrix, the coordinates of the first corner point of the upright cage car model are obtained; Rotate the first corner point coordinates by 180 degrees to obtain the second corner point coordinates of the inverted cage car model; Obtain the caster coordinates of the matching cage trolley caster combination and make differences with the first corner point coordinates and the second corner point coordinates respectively in the order of the trapezoid corner points, calculate the first error sum of the matching cage trolley caster combination and the upright cage trolley model, and the second error sum of the matching cage trolley caster combination and the inverted cage trolley model; The trapezoidal model of the matching cage trolley caster combination is determined according to the smaller value of the first error sum and the second error sum.

9. A cage car docking device, characterized in that: The cage-car docking device includes a memory and a processor coupled to the memory; Wherein, the memory is used to store program data, and the processor is used to execute the program data to implement the cage truck docking method as described in any one of claims 1 to 8.

10. A computer storage medium, characterized in that: The computer storage medium is used to store program data, and when the program data is executed by a computer, it is used to implement the cage-car docking method according to any one of claims 1 to 8.

Images