A method, system, storage medium and vehicle for positioning a tractor and a trailer

Abstract

The present invention discloses a method, system, storage medium and vehicle for positioning a tractor and a trailer, comprising the following steps: a logo plate base is fixed on the front surface of a semi-trailer, a logo plate is arranged on the logo plate base, point cloud data of the semi-trailer and the tractor are collected in a detached state, a minimum bounding box is established, the point cloud data is calculated, and the first center coordinate of the logo plate is obtained; a small neighborhood search is set according to the first center coordinate of the logo plate obtained, a plane equation is fitted to the point cloud data obtained by the small neighborhood search, and the normal vector of the front surface of the trailer is calculated; a large neighborhood search is set according to the first center coordinate of the logo plate obtained, the point cloud data obtained by the large neighborhood search is classified and sorted, and the point cloud data on the plane and the harnesses where they are located are screened; the boundary points of each harness are found according to the width of the logo plate base, the harnesses located on the plane are screened, and the second center coordinate of the logo plate is calculated. In this way, the relative position of the tractor and the trailer can be quickly and accurately determined.

Description

Technical Field

[0001] The present invention relates to the field of positioning, and in particular to a positioning method, system, storage medium and vehicle for a tractor and a trailer. Background Art

[0002] The so-called "unmanned port operation" refers to the round-trip transportation from the cargo ship to the warehouse (unloading and loading). The traditional manual operation method has huge safety hazards, and various large equipment at the dock will threaten the personal safety of workers. In addition, with the continuous increase in port throughput, loading and unloading transportation work has become more and more complicated. The traditional semi-trailer tractor has become a bottleneck restricting the efficiency of loading and unloading. Therefore, it is urgent to solve the problem through automation technology. The connection process between the tractor and the semi-trailer is a crucial link. Only when the relative position of the two is accurately determined can the smooth completion of the automatic connection be guaranteed. Common positioning methods include GPS positioning, visual positioning, and laser radar positioning. GPS positioning has a low update frequency, is easily affected by the signal strength, and has low accuracy, which makes it difficult to meet the connection requirements. Visual positioning is greatly affected by ambient light and cannot work when the light is dark. Lidar positioning has high accuracy, high reliability and mature technology, and can be applied to the connection positioning of port tractors and semi-trailers. Summary of the invention

[0003] The purpose of the present invention is to solve the problem that the position of the existing unmanned tractor saddle is difficult to determine when it is connected to the semi-trailer towing pin, and to provide a tractor and trailer positioning method, system, storage medium and vehicle.

[0004] In order to achieve the above objectives, a first aspect of the present invention provides a method for positioning a tractor and a trailer, which specifically comprises the following steps:

[0005] Step 101: A logo plate base is fixed on the front surface of the trailer, and a logo plate is arranged on the logo plate base. Point cloud data of the trailer and the tractor are collected when they are uncoupled, a minimum bounding box is established, and the point cloud data is calculated to obtain the first center coordinates of the logo plate.

[0006] Step 102: setting a small neighborhood search according to the first center coordinate of the identification plate obtained in step 101, performing plane equation fitting on the point cloud data obtained by the small neighborhood search, and calculating the normal vector of the front surface of the trailer;

[0007] Step 103: setting a large neighborhood search according to the first center coordinate of the identification plate obtained in step 101, classifying and sorting the point cloud data obtained by the large neighborhood search, and screening the point cloud data on the plane and the line bundle on the radar where they are located;

[0008] Step 104: corresponding to the width of the base of the identification plate, find the boundary points of each beam on the radar, select the beam located on the plane, and calculate the second center coordinate of the identification plate.

[0009] In some possible implementations, in step 101, the “collecting point cloud data when the trailer and the tractor are uncoupled” specifically includes:

[0010] When the trailer is unhooked from the tractor, the radar is used to collect point cloud data at the position in the unhooked state. The coordinate system of the radar is L X L Y L Z L , where O L is the origin of the coordinate system, X L The Y axis coincides with the center axis of the multi-line radar laser. L Axis and Z L The axes are perpendicular to the X L axis, and the coordinate system O L X L Y L Z L Satisfies the right-hand criterion.

[0011] In some possible implementations, in step 101, the “establishing a minimum bounding box” specifically includes:

[0012] Perform straight-through filtering on the collected point cloud, select the three-axis coordinate range, remove outliers that are not within the required range, and call the voxel grid filter to downsample the point cloud;

[0013] The remaining point cloud after preprocessing is segmented into separate point cloud clusters through Euclidean clustering. The positions of the edges of the obstacles are detected and the minimum bounding box is established, corresponding to the size of the actual sign base, from which the minimum bounding box of the sign base is selected.

[0014] In some possible implementations, in step 101, the “calculating the point cloud data to obtain the first center coordinates of the identification plate” specifically includes:

[0015] Output the positions of the vertices of the minimum bounding box of the identification plate. The four vertices close to the radar side are A(x1, y1, z1), B(x2, y2, z2), C(x3, y3, z3) and D(x4, y4, z4). The first center coordinate of the identification plate is obtained by calculation. A ,y A ,z A ), the first center coordinate of the identification plate A=(x A ,y A ,z A) is the coordinate in the multi-line laser radar coordinate system;

[0016] Among them, x A =x1=x2=x3=x4,y A =(x1+x2) / 2=(x3+x4) / 2, z A =(x1+x3) / 2=(x2+x4) / 2.

[0017] In some possible implementations, in step 102, "setting a small neighborhood search according to the first center coordinates of the identification plate obtained in step 101" specifically includes:

[0018] Set the neighborhood search radius radius=r1 of the plane fitting, create a KdTree structure, and use the nearest neighbor search algorithm based on KdTree for calculation. The k-dimensional tree constructed in the nearest neighbor search algorithm based on KdTree is a three-dimensional tree, and each node is a three-dimensional binary tree. For each data point, start from the root node of KdTree and compare it with the set threshold r1, and continue downward until reaching a child node. Then the node is the nearest neighbor point, and then search the remaining child nodes, compare their distance values ​​with the current nearest neighbor point distance value and continuously update the nearest neighbor point until all searches are completed, and the final nearest neighbor point is obtained to form the small neighborhood obtained by the search.

[0019] In some possible implementations, in step 102, “performing plane equation fitting on the point cloud obtained by searching the small neighborhood” specifically includes:

[0020] The RANSAC algorithm is used to fit the plane equation of the point cloud obtained by the small neighborhood search. Specifically, it is assumed that the point cloud consists of "inner points" and "outer points". A preliminary plane model is first obtained from a group of inner points, the number of inner points is calculated, and the plane model is iterated until the optimal value is reached. The final output plane equation is: ax+by+cz+d=0;

[0021] Among them, a, b, c and d are four coefficients of the plane equation ax+by+cz+d=0.

[0022] In some possible implementations, in step 102, the “calculating the normal vector of the front surface of the trailer” specifically includes:

[0023] The plane equation ax+by+cz+d=0 is used to obtain the normal vector of the sign plate plane, that is, the normal vector of the front surface of the semitrailer (a, b, c), and the relative angle between the tractor and the trailer is calculated as follows:

[0024]

[0025] Wherein, a and b are coefficients of the plane equation ax+by+cz+d=0.

[0026] In some possible embodiments, in step 103, the "setting a large neighborhood search according to the first center coordinates of the identification board obtained in step 101" specifically includes:

[0027] Set the neighborhood search radius radiusplus = r2 for plane fitting, create a KdTree structure. The k-dimensional tree constructed by the KdTree-based nearest neighbor search algorithm is a three-dimensional tree, and each of its nodes is a three-dimensional binary tree. For each data point, start from the root node of the KdTree and compare it with the set threshold r2, continuously go down until reaching a certain child node, then this node is the nearest neighbor point. After that, search the remaining child nodes, compare their distance values with the distance value of the current nearest neighbor point and continuously update the nearest neighbor point until all searches are completed, obtaining the final nearest neighbor point, which constitutes the large neighborhood obtained by the search.

[0028] In some possible embodiments, in step 103, the "classifying and sorting the point cloud data obtained from the large neighborhood search" specifically includes:

[0029] Define the radar beam number N_SCANS = N. According to the Ring value output by the point cloud coordinates of the radar itself, put the searched point cloud into the corresponding radar frame channel, and sort the point cloud in each radar frame channel in ascending order according to the horizontal rotation angle of the point cloud.

[0030] In some possible embodiments, in step 103, the "screening the point cloud data on the plane and its corresponding beam" specifically includes:

[0031] Calculate the distance from the point (x0, y0, z0) in each beam to the identification board plane ax + by + cz + d = 0 through the formula:

[0032]

[0033] where a, b, c, and d are the four coefficients of the plane equation ax + by + cz + d = 0; set the distance threshold d1 from the point to the plane, and the judgment condition is: d 0-plane < d1. The points that meet the requirements of the distance threshold d1 are the point cloud on the identification board plane, thereby obtaining the number of point cloud on the plane in each channel.

[0034] In some possible embodiments, in step 104, the "finding the boundary points of each beam on the radar and screening the beams located on the plane" specifically includes:

[0035] Set the boundary threshold d2 according to the width of the identification board base, and find the two boundary points P(x of each beam located on the planep ,y p ,z p ) and Q(x q ,y q ,z q ), judged by the Euclidean distance between two points, the judgment condition is:

[0036]

[0037] The wiring harness that meets the boundary threshold d2 requirement is the wiring harness on the plane of the identification plate.

[0038] In some possible implementations, in step 104, the "calculating and obtaining the second center coordinates of the identification plate" specifically includes:

[0039] The number of radar beams located on the plane obtained by searching is N. The second center coordinates of the identification plate are calculated by the following formula based on the average value of the coordinates of each pair of boundary points:

[0040]

[0041] Among them, x and y are the center of the sign plate, X L Axis, Y L The coordinate value of the axis, x p 、x q ,y p and q Each boundary point X L Axis, Y L The coordinate values ​​of the axis.

[0042] A third aspect of the present invention provides a radar-based tractor and trailer positioning system, comprising a processor and a memory:

[0043] The memory is used to store program code and transmit the program code to the processor;

[0044] The processor is used to execute the above-mentioned method for positioning a tractor and a trailer according to the instructions in the program code.

[0045] According to a fourth aspect of the present invention, a computer-readable storage medium is provided, wherein the computer-readable storage medium is used to store program codes, and the program codes are used to execute the above-mentioned method for positioning a tractor and a trailer.

[0046] A fifth aspect of the present invention provides a vehicle, comprising the above-mentioned method for positioning a tractor and a trailer or the above-mentioned system for positioning a tractor and a trailer.

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

[0048] 1. By sticking a sign plate base on the front surface of the trailer, using a multi-line laser radar to collect point cloud data when the trailer and the tractor are detached, and by establishing a minimum bounding box, the roughly calculated first center coordinates of the sign plate are obtained. The first center coordinates of the sign plate are processed by the nearest neighbor search algorithm and plane equation fitting, and the accurate second center coordinates of the sign plate are obtained through the steps of searching, fitting and searching, so as to realize the rapid and accurate determination of the relative position of the tractor and the trailer.

[0049] 2. The center coordinates of the identification plate are determined by plane equation fitting and identification plate boundary point search. This does not require a large amount of equipment and infrastructure investment in the early stages, and is suitable for dock positioning scenarios, solving the existing problems of low accuracy and low efficiency in manual hooking.

[0050] 3. Based on the existing PCL library encapsulation algorithm, an algorithm for classifying and sorting point clouds according to radar beams is introduced, which effectively improves the accuracy of boundary point search.

[0051] 4. Compared with the existing GPS positioning and visual positioning methods, the laser radar positioning technology used in the present invention is not easily affected by environmental factors and has higher positioning accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] Figure 1 A schematic diagram of the positions of a marking plate and a laser radar in a method for positioning a tractor and a trailer according to an embodiment of the present invention;

[0053] Figure 2 This is a rendering of a minimum bounding box established in a method for positioning a tractor and a trailer according to an embodiment of the present invention;

[0054] Figure 3 This is a point cloud processing effect diagram of a positioning method for a tractor and a trailer according to an embodiment of the present invention;

[0055] Figure 4 A flowchart of the steps of a method for positioning a tractor and a trailer according to an embodiment of the present invention;

[0056] Figure 5 This is a flowchart of step 101 in a method for positioning a tractor and a trailer according to an embodiment of the present invention;

[0057] Figure 6 This is a flowchart of step 102 in a method for positioning a tractor and a trailer according to an embodiment of the present invention;

[0058] Figure 7 This is a flowchart of step 103 in a method for positioning a tractor and a trailer according to an embodiment of the present invention;

[0059] Figure 8This is a flowchart of step 104 in a method for positioning a tractor and a trailer according to an embodiment of the present invention;

[0060] Fig. 9 This is an algorithm flow chart of a method for positioning a tractor and a trailer according to an embodiment of the present invention;

[0061] Fig.10 The present invention is a structural diagram of a positioning system for a tractor and a trailer according to an embodiment of the present invention.

[0062] In the figure: 51, processor; 52, memory. DETAILED DESCRIPTION

[0063] The preferred embodiments of the present invention are described in detail below in conjunction with the accompanying drawings so that the advantages and features of the present invention can be more easily understood by those skilled in the art, thereby making a clearer and more definite definition of the protection scope of the present invention.

[0064] In the first aspect of this embodiment, refer to the attached Fig. 9 As shown, a method for positioning a tractor and a trailer is provided, which specifically includes the following steps:

[0065] Step 101: Refer to the attached Figure 1 As shown, a signboard base with a size of 38×38×20 (cm) is fixed on the front surface of the trailer, and a signboard with a size of 38×38 (cm) is arranged on the signboard base. Figure 5 As shown in the figure, the point cloud data of the trailer and tractor are collected by using the Sagitar RS-Ruby_Lite multi-line laser radar, and the minimum bounding box is established. The point cloud data is calculated to obtain the first center coordinates of the identification plate. The center coordinates of the identification plate are related to the radar position. When the radar position changes, the center coordinates will also change.

[0066] The “collection of point cloud data when the semi-trailer and the tractor are uncoupled” specifically includes:

[0067] When the semi-trailer is uncoupled from the tractor, an 80-line laser radar is used to collect point cloud data at the position in the uncoupled state. The coordinate system of the 80-line laser radar is 0 L X L Y L Z L , where O L is the origin of the coordinate system, X L The Y axis coincides with the central axis of the 80-line radar laser. L Axis and Z L The axes are perpendicular to the X L axis, and the coordinate system O L X L Y L ZL Satisfies the right-hand criterion.

[0068] The “establishing a minimum bounding box” specifically includes:

[0069] The collected point cloud is filtered directly, the three-axis coordinate range is selected, outliers that are not within the required range are quickly removed, and the voxel grid filter is called to downsample the point cloud. The density of the point cloud is reduced while ensuring that the original point cloud structure remains unchanged, which is convenient for subsequent segmentation and clustering. The remaining point cloud after preprocessing is segmented into individual point cloud clusters through Euclidean clustering, and the positions of the edges of the obstacles are detected and the minimum bounding box is established, corresponding to the size of the actual sign base, from which the minimum bounding box of the sign base is selected. The purpose of establishing the minimum bounding box is to select the point cloud corresponding to the sign. Refer to the attached Figure 2 As shown, Figure 2 This is the effect diagram of the minimum bounding box.

[0070] Applied to this embodiment: the collected point cloud is through-filtered, and the three-axis coordinate range is determined according to the relative position of the sign plate and the radar. In the X-axis direction, that is, the radar depth direction, point clouds that are too far away are removed. In the Y-axis and Z-axis directions, corresponding to the approximate position of the sign plate, outliers on the top, bottom and sides that are not within the required range are removed; the voxel grid filter is called to set the size of a single voxel to 2cm×2cm×2cm, and the point cloud is downsampled to reduce the point cloud density while ensuring that the original point cloud structure remains unchanged, so as to facilitate subsequent segmentation and clustering; the remaining point cloud after preprocessing is segmented into separate point cloud clusters through Euclidean clustering, and a minimum bounding box is established to detect the positions of the edges of the obstacle, corresponding to the size of the actual sign plate base, and the minimum bounding box of the sign plate base is screened out.

[0071] The “calculating the point cloud data to obtain the first center coordinates of the identification plate” specifically includes:

[0072] Output the positions of the vertices of the minimum bounding box of the identification plate. The four vertices close to the 80-line laser radar are A(x1, y1, z1), B(x2, y2, z2), C(x3, y3, z3) and D(x4, y4, z4). The first center coordinate of the identification plate is obtained by calculation. A ,y A ,z A ), the first center coordinate of the identification plate A=(x A ,y A ,z A ) is the coordinate in the 80-line laser radar coordinate system, where x A =x1=x2=x3=x4,y A =(x1+x2) / 2=(x3+x4) / 2, z A=(x1+x3) / 2=(x2+x4) / 2. Refer to Appendix 3, Figure 3 This is a diagram showing the effect of point cloud processing in an embodiment of the present invention.

[0073] Step 102: Refer to the attached Figure 6 As shown, a small neighborhood search is set according to the first center coordinate of the identification plate obtained in step 101, and a plane equation is fitted to the point cloud data obtained by the small neighborhood search to calculate the normal vector of the front surface of the trailer. The purpose of setting the small neighborhood search is to obtain the plane equation of the plane where the identification plate is located, and the normal vector of the front surface of the trailer can be calculated through the plane equation of the plane where the identification plate is located.

[0074] The “setting of a small neighborhood search according to the first center coordinate of the identification plate obtained in step 101” specifically includes:

[0075] With the center of the identification plate as the center of the circle, set the neighborhood search radius of the plane fitting radius = r1, and create a KdTree structure. KdTree is kd-tree (short for k-dimensional tree), which is a tree data structure that stores instance points in k-dimensional space for fast retrieval. It is mainly used in the search of key data in multidimensional space (such as range search and nearest neighbor search). In computer science, kd tree (short for k-dimensional tree) is a data structure for organizing points in k-dimensional Euclidean space. Kd tree can be used in a variety of applications, such as multidimensional key value search (for example: range search and nearest neighbor search). Kd tree is a special case of binary space partitioning.

[0076] The nearest neighbor search algorithm based on KdTree is used to calculate and find the point in the tree that is closest to the input point. The k-dimensional tree constructed in the nearest neighbor search algorithm based on KdTree is a three-dimensional tree, and each node of the tree is a three-dimensional binary tree.

[0077] The process of the kd tree nearest neighbor search is as follows: for each data point, starting from the root node of the KdTree, it is compared with the set threshold r1, and continues downward until it reaches a child node, then the node is the nearest neighbor point, and then the remaining child nodes are searched, and their distance values ​​are compared with the current nearest neighbor point distance value and the nearest neighbor points are continuously updated until all searches are completed, and the final nearest neighbor points are obtained to form the small neighborhood obtained by the search.

[0078] The “performing plane equation fitting on the point cloud obtained by searching the small neighborhood” specifically includes:

[0079] RANSAC is the abbreviation of Random Sample Consensus. It is an algorithm that calculates the mathematical model parameters of the data based on a set of sample data sets containing abnormal data to obtain valid sample data. The RANSAC algorithm is used to fit the plane equation to the point cloud obtained by searching in a small neighborhood. Specifically, it is assumed that the point cloud consists of "inner points" and "outer points". A preliminary plane model is first obtained from a set of inner points, the number of inner points is calculated, and the plane model is iterated until the optimal value is reached. The final output plane equation is: ax+by+cz+d=0;

[0080] Among them, a, b, c and d are four coefficients of the plane equation ax+by+cz+d=0.

[0081] The “calculating the normal vector of the front surface of the trailer” specifically includes:

[0082] The plane equation ax+by+cz+d=0 is used to obtain the normal vector of the sign plate plane, that is, the normal vector of the front surface of the trailer (a, b, c). The relative angle between the tractor and the trailer is calculated as follows:

[0083]

[0084] Among them, a and b are the coefficients of the plane equation ax+by+cz+d=0. The center coordinates of the sign plate are determined by plane equation fitting and sign plate boundary point search, which does not require a large amount of equipment and infrastructure investment in the early stage, and is suitable for dock positioning scenarios, solving the existing problems of low accuracy and low efficiency of manual hooking.

[0085] Step 103: Refer to the attached Figure 7 As shown, a large neighborhood search is set according to the first center coordinate of the identification plate obtained in step 101, and the point cloud data obtained by the large neighborhood search is classified and sorted, and the point cloud data on the plane and the wiring harness to which it is located are screened. The purpose of setting a large neighborhood search is to classify and sort the point clouds obtained by the search, and to screen the point clouds located on the plane and their corresponding wiring harnesses. In order to subsequently find the boundary points of the wiring harness on the plane of the identification plate, the set neighborhood range must be able to completely include the identification plate. In addition, setting large and small neighborhood searches separately can also facilitate distinction during visualization.

[0086] The “setting of a large neighborhood search according to the first center coordinates of the identification plate obtained in step 101” specifically includes:

[0087] With the center of the identification plate as the center of the circle, set the neighborhood search radius of the plane fitting radiusplus=r2, and create a KdTree structure. Kd-tree (short for k-dimensional tree) is a tree data structure that stores instance points in k-dimensional space for fast retrieval. It is mainly used in the search of key data in multidimensional space (such as range search and nearest neighbor search). In computer science, kd tree (short for k-dimensional tree) is a data structure for organizing points in k-dimensional Euclidean space. Kd tree can be used in a variety of applications, such as multidimensional key value search (e.g. range search and nearest neighbor search). Kd tree is a special case of binary space partitioning.

[0088] The nearest neighbor search algorithm based on KdTree is used to calculate and find the point in the tree that is closest to the input point. The k-dimensional tree constructed in the nearest neighbor search algorithm based on KdTree is a three-dimensional tree, and each node of the tree is a three-dimensional binary tree.

[0089] The process of the kd tree nearest neighbor search is as follows: for each data point, starting from the root node of the KdTree, it is compared with the set threshold r2, and continues downward until it reaches a child node. This node is the nearest neighbor. Then the remaining child nodes are searched, and their distance values ​​are compared with the current nearest neighbor distance value. The nearest neighbor points are continuously updated until all searches are completed, and the final nearest neighbor points are obtained to form the large neighborhood obtained by the search.

[0090] The “classifying and sorting the point cloud data obtained by the large neighborhood search” specifically includes:

[0091] Using the Sagitar RS-Ruby_Lite 80-line radar, define the 80-line laser radar beam N_SCANS=80, and output the Ring value based on the point cloud coordinates of the multi-line laser radar. The so-called Ring value is how many beams of light are emitted from each point. Put the searched point cloud into the corresponding radar frame channel, and sort the point cloud in each radar frame channel from small to large according to the horizontal rotation angle of the point cloud.

[0092] The “screening of point cloud data on a plane and the line beam on the radar on which it is located” specifically includes:

[0093] The distance from the point (x0, y0, z0) in each harness to the plane of the identification plate ax+by+cz+d=0 is calculated by the formula:

[0094]

[0095] Among them, a, b, c, and d are the four coefficients of the plane equation ax + by + cz + d = 0; a distance threshold d1 from a set point to the plane is set, and the judgment condition is: d 0-plane <d1, the points that meet the requirement of the distance threshold d1 are the point clouds on the plane of the identification plate, so as to obtain the number of point clouds on the plane in each channel. Introducing an algorithm for classifying and sorting point clouds according to radar beam bundles effectively improves the accuracy of finding the boundary points of beam bundles on the radar.

[0096] Step 104: Refer to the appendix Figure 8 As shown, corresponding to the width of the base of the identification plate, find the boundary points of each beam bundle on the radar, screen the beam bundles located on the plane, and calculate the second center coordinates of the identification plate.

[0097] The "finding the boundary points of each beam bundle on the radar and screening the beam bundles located on the plane" specifically includes:

[0098] Set a boundary threshold d2 according to the width of the base of the identification plate, and find the two boundary points P(x p , y p , z p ) and Q(x q , y q , z q ) of each beam bundle on the radar located on the plane. Judge by the Euclidean distance between two points, and the judgment condition is:

[0099]

[0100] Those that meet the requirement of the boundary threshold d2 are the beam bundles on the plane of the identification plate.

[0101] The "calculating the second center coordinates of the identification plate" specifically includes:

[0102] The number of lidar beam bundles located on the plane obtained by searching is N. From the average values of the coordinates of each pair of boundary points, the second center coordinates of the identification plate are calculated by the formula:

[0103]

[0104] Among them, x and y are the coordinate values of the X L axis and Y L axis of the center of the identification plate, and x p , x q , y p and y q are the coordinate values of the X L axis and Y L axis of each boundary point respectively.

[0105] In the third aspect of this embodiment, refer to the appendix Fig.10As shown, a radar-based tractor and trailer positioning system is provided, including a processor 51 and a memory 52:

[0106] The memory 52 is used to store program codes and transmit the program codes to the processor;

[0107] The processor 51 is used to execute the above-mentioned method for positioning a tractor and a trailer according to the instructions in the program code.

[0108] The processor 51 may also be referred to as a CPU (Central Processing Unit). The processor 51 may be an integrated circuit chip having the ability to process signals. The processor 51 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 gate or transistor logic devices, discrete hardware components. The general-purpose processor may be a microprocessor or the processor may also be any conventional processor, etc.

[0109] In a fourth aspect of the present embodiment, a computer-readable storage medium is provided, wherein the computer-readable storage medium is used to store program codes, and the program codes are used to execute the above-mentioned method for positioning a tractor and a trailer.

[0110] The storage medium stores program instructions that can implement all the above methods, wherein the program instructions can be stored in the above storage medium in the form of a software product, including several instructions for enabling a computer device (which can be a personal computer, server, or network device, etc.) or a processor to perform all or part of the steps of the methods described in each embodiment of the present invention. 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 magnetic disk or an optical disk, or a terminal device such as a computer, a server, a mobile phone, or a tablet.

[0111] A fifth aspect of the present embodiment provides a vehicle, including the above-mentioned method for positioning a tractor and a trailer or the above-mentioned system for positioning a tractor and a trailer.

[0112] The above implementation modes are only for illustrating the technical concept and features of the present invention, and their purpose is to enable people familiar with this technology to understand the content of the present invention and implement it, and they cannot be used to limit the protection scope of the present invention. Any equivalent changes or modifications made according to the spirit of the present invention should be included in the protection scope of the present invention.

Claims

1. A method for positioning a tractor and a trailer, characterized in that: The specific steps include: Step 101: A logo plate base is fixed on the front surface of the trailer, and a logo plate is arranged on the logo plate base. Point cloud data of the trailer and the tractor are collected when they are uncoupled, a minimum bounding box is established, and the point cloud data is calculated to obtain the first center coordinates of the logo plate. Step 102: setting a small neighborhood search according to the first center coordinate of the identification plate obtained in step 101, performing plane equation fitting on the point cloud data obtained by the small neighborhood search, and calculating the normal vector of the front surface of the trailer; Step 103: setting a large neighborhood search according to the first center coordinate of the identification plate obtained in step 101, classifying and sorting the point cloud data obtained by the large neighborhood search, and screening the point cloud data on the plane and the line bundle on the radar where they are located; Step 104: corresponding to the width of the base of the identification plate, find the boundary points of each beam on the radar, select the beam located on the plane, and calculate the second center coordinate of the identification plate.

2. A method for positioning a tractor and a trailer according to claim 1, characterized in that: In step 101, the "collecting point cloud data when the trailer and the tractor are uncoupled" specifically includes: When the trailer is unhooked from the tractor, the radar is used to collect point cloud data at the position in the unhooked state. The coordinate system of the radar is L X L Y L Z L , where O L is the origin of the coordinate system, X L The Y axis coincides with the center axis of the multi-line radar laser. L Axis and Z L The axes are perpendicular to the X L axis, and the coordinate system O L X L Y L Z L Satisfies the right-hand criterion.

3. A method for positioning a tractor and a trailer according to claim 1, characterized in that: In step 101, the "establishing a minimum bounding box" specifically includes: Perform straight-through filtering on the collected point cloud, select the three-axis coordinate range, remove outliers that are not within the required range, and call the voxel grid filter to downsample the point cloud; The remaining point cloud after preprocessing is segmented into separate point cloud clusters through Euclidean clustering. The positions of the edges of the obstacles are detected and the minimum bounding box is established, corresponding to the size of the actual sign base, from which the minimum bounding box of the sign base is selected.

4. A method for positioning a tractor and a trailer according to claim 1, characterized in that: In step 101, the "calculating the point cloud data to obtain the first center coordinates of the identification plate" specifically includes: Output the positions of the vertices of the minimum bounding box of the identification plate. The four vertices close to the radar side are A(x1, y1, z1), B(x2, y2, z2), C(x3, y3, z3) and D(x4, y4, z4). The first center coordinate of the identification plate is obtained by calculation. A ,y A ,z A ), the first center coordinate of the identification plate A=(x A ,y A ,z A ) is the coordinate in the multi-line laser radar coordinate system; where x A = x1 = x2 = x3 = x4, y A = (x1 + x2) / 2 = (x3 + x4) / 2, z A = (x1 + x3) / 2 = (x2 + x4) / 2.

5. A method for positioning a tractor and a trailer according to claim 1, characterized in that: In step 102, the "setting of a small neighborhood search according to the first center coordinates of the identification plate obtained in step 101" specifically includes: Set the neighborhood search radius radius=r1 of the plane fitting, create a KdTree structure, and use the nearest neighbor search algorithm based on KdTree for calculation. The k-dimensional tree constructed in the nearest neighbor search algorithm based on KdTree is a three-dimensional tree, and each node is a three-dimensional binary tree. For each data point, start from the root node of KdTree and compare it with the set threshold r1, and continue downward until reaching a child node. Then the node is the nearest neighbor point, and then search the remaining child nodes, compare their distance values ​​with the current nearest neighbor point distance value and continuously update the nearest neighbor point until all searches are completed, and the final nearest neighbor point is obtained to form the small neighborhood obtained by the search.

6. A method for positioning a tractor and a trailer according to claim 1, characterized in that: In step 102, the “performing plane equation fitting on the point cloud obtained by searching the small neighborhood” specifically includes: The RANSAC algorithm is used to fit the plane equation of the point cloud obtained by the small neighborhood search. Specifically, the point cloud is assumed to be composed of "inner points" and "outer points". A preliminary plane model is first obtained from a group of inner points, the number of inner points is calculated, and the plane model is iterated until the optimal value is reached. The final output plane equation is: ax+by+cz+d=0; Among them, a, b, c and d are four coefficients of the plane equation ax+by+cz+d=0.

7. A method for positioning a tractor and a trailer according to claim 1, characterized in that: In step 102, the "calculating the normal vector of the front surface of the trailer" specifically includes: The plane equation ax+by+cz+d=0 is used to obtain the normal vector of the sign plate plane, that is, the normal vector of the front surface of the semitrailer (a, b, c), and the relative angle between the tractor and the trailer is calculated as follows: Wherein, a and b are coefficients of the plane equation ax+by+cz+d=0.

8. A method for positioning a tractor and a trailer according to claim 1, characterized in that: In step 103, the "setting of a large neighborhood search according to the first center coordinates of the identification plate obtained in step 101" specifically includes: Set the neighborhood search radius radiusplus=r2 of the plane fitting, create a KdTree structure, and use the nearest neighbor search algorithm based on KdTree to perform calculations. The k-dimensional tree constructed by the nearest neighbor search algorithm based on KdTree is a three-dimensional tree, and each node is a three-dimensional binary tree. For each data point, start from the root node of KdTree and compare it with the set threshold r2, and continue downward until a child node is reached. Then the node is the nearest neighbor point, and then search the remaining child nodes, compare their distance values ​​with the current nearest neighbor point distance value and continuously update the nearest neighbor point until all searches are completed, and the final nearest neighbor point is obtained to form the large neighborhood obtained by the search.

9. A method for positioning a tractor and a trailer according to claim 1, characterized in that: In step 103, the “classifying and sorting the point cloud data obtained by the large neighborhood search” specifically includes: Define the radar beam N_SCANS=N, and put the searched point cloud into the corresponding radar frame channel according to the Ring value output by the radar's own point cloud coordinates. For the point cloud in each radar frame channel, sort it from small to large according to the horizontal rotation angle of the point cloud.

10. A method for positioning a tractor and a trailer according to claim 1, characterized in that: In step 103, the "screening of point cloud data on a plane and its wiring harness" specifically includes: The distance from the point (x0, y0, z0) in each harness to the plane of the identification plate ax+by+cz+d=0 is calculated by the formula: Among them, a, b, c, and d are the four coefficients of the plane equation ax + by + cz + d = 0; a distance threshold d1 from a set point to the plane is set, and the judgment condition is: d 0-plane < d1. The points that meet the requirement of the distance threshold d1 are the point clouds on the identification plate plane, thereby obtaining the number of point clouds on the plane in each channel.

11. A method for positioning a tractor and a trailer according to claim 1, characterized in that: In step 104, the "searching for boundary points of each beam on the radar and screening the beams located on the plane" specifically includes: According to the width of the base of the identification plate, the boundary threshold d2 is set, and the two boundary points P (x p ,y p ,z p ) and Q(x q ,y q ,z q ), judged by the Euclidean distance between two points, the judgment condition is: The wiring harness that meets the boundary threshold d2 requirement is the wiring harness on the plane of the identification plate.

12. A method for positioning a tractor and a trailer according to claim 1, characterized in that: In step 104, the "calculating and obtaining the second center coordinates of the identification plate" specifically includes: The number of radar beams located on the plane obtained by searching is N. The second center coordinates of the identification plate are calculated by the following formula based on the average value of the coordinates of each pair of boundary points: Where x and y are the center of the sign plate, X L Axis, Y L The coordinate value of the axis, x p 、x q ,y p and q Each boundary point X L Axis, Y L The coordinate values ​​of the axis.

13. A positioning system for a tractor and a trailer, characterized in that: It includes a processor (51) and a memory (52): The memory (52) is used to store program code and transmit the program code to the processor; The processor (51) is used to execute a method for positioning a tractor and a trailer according to any one of claims 1 to 12 according to instructions in the program code.

14. A computer-readable storage medium, characterized in that: The computer-readable storage medium is used to store program codes, and the program codes are used to execute a method for positioning a tractor and a trailer according to any one of claims 1-12.

15. A vehicle, characterized in that: It comprises a control device, wherein the control device comprises a positioning method for a tractor and a trailer as described in any one of claims 1 to 12 or a positioning system for a tractor and a trailer as described in claim 13.

Images