Angle detection method, device and electronic equipment between tractor and trailer

Abstract

The present invention provides a method, device and electronic device for detecting the angle between a tractor and a trailer. For each frame of point cloud, the current frame point clouds obtained by scanning various laser radars are merged to obtain a merged point cloud of the current frame; based on a preset angle resolution, multiple rotation angles corresponding to the merged point cloud of the current frame and the three-dimensional model of the trailer corresponding to each rotation angle are calculated; based on the target rotation angle corresponding to the three-dimensional model of the trailer that matches the largest number of point clouds, the angle detection result is determined. In this method, laser radars are respectively set between the tractor and the trailer, and on both sides of the front of the tractor, and the point clouds belonging to the same frame scanned by multiple laser radars are merged. The angle detection result is determined by judging the number of point clouds of the merged point cloud that matches the three-dimensional model of the trailer corresponding to different rotation angles. This method will not be affected by light and obstacles, so that the accuracy and precision of the angle detection between the tractor and the trailer can be effectively guaranteed.

Description

Technical Field

[0001] The present invention relates to the field of intelligent driving technology, and in particular to a method, device and electronic equipment for detecting the angle between a tractor and a trailer. Background Art

[0002] At present, autonomous driving technology has begun to be applied in the field of semi-trailers. Since semi-trailers usually include tractors and trailers, in order to ensure the safe driving of semi-trailers, it is necessary to accurately detect the angle between the tractor and the trailer during the driving process of the vehicle; in related technologies, several different QR codes are usually pasted on both sides of the trailer, and the pose of the QR code in the camera coordinate system is extracted according to the algorithm, and then the angle of the trailer in the vehicle body coordinate system is determined according to the pose in the camera coordinate system. This method requires pasting a QR code on the trailer, but in actual applications it is difficult to ensure that all trailers have a QR code pasted on them, and this method based on visual detection of the trailer angle is easily affected by light or obstacles, and it is difficult to ensure the accuracy and precision of the angle detection between the tractor and the trailer. Summary of the invention

[0003] The object of the present invention is to provide a method, device and electronic equipment for detecting the angle between a tractor and a trailer, so as to improve the accuracy and precision of the angle detection between the tractor and the trailer.

[0004] The present invention provides a method for detecting an angle between a tractor and a trailer. A first laser radar is arranged between the tractor and the trailer, and a second laser radar and a third laser radar are arranged on both sides of the front of the tractor respectively; the acquisition time of the first laser radar, the second laser radar and the third laser radar are synchronized; the method comprises: respectively obtaining each frame point cloud scanned by the first laser radar, the second laser radar and the third laser radar; for each frame point cloud, merging the current frame point cloud scanned by the first laser radar, the second laser radar and the third laser radar to obtain the current frame merged point cloud corresponding to the current frame point cloud; based on a preset angle resolution, calculating multiple rotation angles corresponding to the current frame merged point cloud and a trailer stereo model rotated according to the rotation angle corresponding to each rotation angle; determining the number of point clouds on the trailer stereo model corresponding to each rotation angle; and determining the angle detection result between the tractor and the trailer corresponding to the current frame merged point cloud based on the target rotation angle corresponding to the trailer stereo model with the largest number of matched point clouds.

[0005] Furthermore, based on a preset angle resolution, the step of calculating multiple rotation angles corresponding to the current frame merged point cloud and the three-dimensional model of the trailer rotated according to each rotation angle includes: if the current frame merged point cloud is the first frame merged point cloud, based on a preset first angle range and a preset angle resolution, calculating multiple rotation angles corresponding to the first frame merged point cloud and the three-dimensional model of the trailer rotated according to each rotation angle; if the current frame merged point cloud is a merged point cloud other than the first frame merged point cloud, based on the angle detection result corresponding to the previous frame merged point cloud of the current frame merged point cloud and a preset angle change amplitude, determining the second angle range corresponding to the current frame merged point cloud; based on the second angle range and the preset angle resolution, calculating multiple rotation angles corresponding to the current frame merged point cloud and the three-dimensional model of the trailer rotated according to each rotation angle.

[0006] Furthermore, the step of determining the number of point clouds on the trailer three-dimensional model that matches each rotation angle includes: for each rotation angle, respectively obtaining the number of point clouds on the front plane, left plane and right plane of the trailer three-dimensional model that match the rotation angle; adding the number of point clouds on the front plane, left plane and right plane of the trailer three-dimensional model that match the rotation angle to obtain the number of point clouds on the trailer three-dimensional model that matches the rotation angle.

[0007] Furthermore, for each rotation angle, the steps of respectively obtaining the number of point clouds that match the front plane, left plane and right plane of the trailer three-dimensional model corresponding to the rotation angle include: for each rotation angle, calculating the first distance from each feature point in the current frame merged point cloud to the front plane of the trailer three-dimensional model corresponding to the rotation angle, the second distance from the left plane, and the third distance from the right plane; based on the first distance, the second distance, the third distance and the preset distance threshold corresponding to each feature point, determining the matching result corresponding to each feature point; based on the matching result corresponding to each feature point, determining the number of point clouds that match the front plane, the left plane and the right plane of the trailer three-dimensional model corresponding to the rotation angle.

[0008] Furthermore, for each rotation angle, the step of calculating a first distance from a front plane of a trailer stereo model corresponding to the rotation angle, a second distance from a left plane, and a third distance from a right plane for each feature point in the current frame merged point cloud includes: for each rotation angle and each feature point in the current frame merged point cloud, determining whether the foot of a perpendicular from the feature point to a designated plane of the trailer stereo model corresponding to the rotation angle is located within the designated plane; wherein the designated plane includes: at least one of a front plane, a left plane, and a right plane; if located within the designated plane, determining a designated distance from the feature point to the designated plane based on the position coordinates of the feature point; wherein the designated distance includes: at least one of the first distance, the second distance, and the third distance; if located outside the designated plane, calculating the distance between the foot of the perpendicular and multiple boundary lines of the designated plane based on the position coordinates of the foot of the perpendicular, and determining the minimum distance value among the calculated multiple distance values ​​as the distance from the feature point to the designated plane.

[0009] Furthermore, based on the first distance, second distance, third distance and preset distance threshold corresponding to each feature point, the step of determining the matching result corresponding to each feature point includes: for each feature point in the merged point cloud of the current frame, if the first distance, second distance or third distance corresponding to the feature point meets the preset distance threshold, the plane corresponding to the distance value that meets the preset distance threshold is determined as the plane matched by the feature point; if at least two distance values ​​among the first distance, second distance and third distance corresponding to the feature point meet the preset distance threshold, the plane corresponding to the smallest distance value among the at least two distance values ​​is determined as the plane matched by the feature point; if the first distance, second distance and third distance corresponding to the feature point do not meet the preset distance threshold, it is determined that the feature point does not match the front plane, the left plane and the right plane.

[0010] Furthermore, based on the target rotation angle corresponding to the trailer stereo model with the largest number of matched point clouds, the step of determining the angle detection result between the tractor and the trailer corresponding to the current frame merged point cloud includes: if the number of target rotation angles corresponding to the trailer stereo model with the largest number of matched point clouds is one, the target rotation angle is determined as the angle detection result between the tractor and the trailer corresponding to the current frame merged point cloud; if the number of target rotation angles corresponding to the trailer stereo model with the largest number of matched point clouds is multiple, the total distance between the matched feature points and the corresponding plane corresponding to each target rotation angle is calculated, and the target rotation angle corresponding to the minimum value of the calculated total distance is determined as the angle detection result between the tractor and the trailer corresponding to the current frame merged point cloud.

[0011] The present invention provides an angle detection device between a tractor and a trailer. A first laser radar is arranged between the tractor and the trailer, and a second laser radar and a third laser radar are arranged on both sides of the front of the tractor respectively; the acquisition time of the first laser radar, the second laser radar and the third laser radar are synchronized; the device comprises: an acquisition module, which is used to respectively acquire each frame of point cloud scanned by the first laser radar, the second laser radar and the third laser radar; a merging processing module, which is used to merge the current frame point cloud scanned by the first laser radar, the second laser radar and the third laser radar for each frame of point cloud, so as to obtain the current frame merged point cloud corresponding to the current frame point cloud; a calculation module, which is used to calculate multiple rotation angles corresponding to the current frame merged point cloud and the trailer stereo model rotated according to the rotation angle corresponding to each rotation angle based on a preset angle resolution; a first determination module, which is used to determine the number of point clouds matched to the trailer stereo model corresponding to each rotation angle; and a second determination module, which is used to determine the angle detection result between the tractor and the trailer corresponding to the current frame merged point cloud based on the target rotation angle corresponding to the trailer stereo model with the largest number of matched point clouds.

[0012] An electronic device provided by the present invention includes a processor and a memory, wherein the memory stores machine executable instructions that can be executed by the processor, and the processor executes the machine executable instructions to implement any of the above-mentioned methods for detecting an angle between a tractor and a trailer.

[0013] The present invention provides a machine-readable storage medium, which stores machine-executable instructions. When the machine-executable instructions are called and executed by a processor, the machine-executable instructions prompt the processor to implement any of the above-mentioned angle detection methods between a tractor and a trailer.

[0014] The angle detection method, device and electronic device between the tractor and the trailer provided by the present invention respectively obtain each frame point cloud obtained by scanning the first laser radar, the second laser radar and the third laser radar; for each frame point cloud, the current frame point cloud obtained by scanning the first laser radar, the second laser radar and the third laser radar are merged to obtain the current frame merged point cloud corresponding to the current frame point cloud; based on the preset angle resolution, multiple rotation angles corresponding to the current frame merged point cloud and the three-dimensional model of the trailer rotated according to the rotation angle corresponding to each rotation angle are calculated; the number of point clouds on the three-dimensional model of the trailer corresponding to each rotation angle is determined; based on the target rotation angle corresponding to the three-dimensional model of the trailer with the largest number of matched point clouds, the angle detection result between the tractor and the trailer corresponding to the current frame merged point cloud is determined. This method sets up laser radars between the tractor and the trailer, and on both sides of the front of the tractor, and merges the point clouds belonging to the same frame scanned by multiple laser radars. The angle detection result is determined by judging the number of point clouds of the trailer three-dimensional model that matches the merged point cloud at different rotation angles. This method will not be affected by light and obstacles, and can effectively ensure the accuracy and precision of the angle detection between the tractor and the trailer. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] In order to more clearly illustrate the specific implementation methods of the present invention or the technical solutions in the prior art, the drawings required for use in the specific implementation methods or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are some implementation methods of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0016] Figure 1 A schematic diagram of a trailer model provided by an embodiment of the present invention;

[0017] Figure 2 A schematic top view of a semitrailer provided in an embodiment of the present invention;

[0018] Figure 3 A flow chart of a method for detecting an angle between a tractor and a trailer provided in an embodiment of the present invention;

[0019] Figure 4 A flow chart of another method for detecting the angle between a tractor and a trailer provided by an embodiment of the present invention;

[0020] Figure 5 A flow chart of another method for detecting the angle between a tractor and a trailer provided by an embodiment of the present invention;

[0021] Figure 6 A schematic structural diagram of an angle detection device between a tractor and a trailer provided in an embodiment of the present invention;

[0022] Figure 7 A schematic diagram of the structure of an electronic device provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0023] The technical solution of the present invention will be clearly and completely described below in conjunction with the embodiments. Obviously, the described embodiments are part of the embodiments of the present invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0024] At present, when measuring the angle between a tractor and a trailer, the method usually adopted is to paste several different QR codes on both sides of the trailer, extract the position of the QR code in the camera coordinate system according to the algorithm, and determine the rotation angle of the trailer in the vehicle body coordinate system according to the different positions in the camera coordinate system. This method has three major defects: (1) The premise for the realization of this method is that the QR code must be pasted on the trailer. Doing so will not only affect the appearance of the trailer, but also greatly affect the scope of application of trailer angle detection, because in practical applications, it is impossible to ensure that all trailers are pasted with QR codes. (2) This method will be affected by night lighting, because there is often not enough light on both sides of the trailer at night, which greatly reduces the accuracy of trailer angle recognition. (3) In this method, the camera is easily blocked by external objects such as branches on the road or other nearby vehicles, especially when the trailer is close to the branches on the road or the trailer is close to other vehicles. At this time, the accuracy of trailer angle detection will also be greatly reduced. Based on this, the embodiment of the present invention provides a method, device and electronic device for angle detection between a tractor and a trailer. The technology can be applied to applications where the trailer angle needs to be detected.

[0025] To facilitate understanding of this embodiment, a method for detecting the angle between a tractor and a trailer disclosed in an embodiment of the present invention is first described in detail; in this method, a first laser radar is arranged between the tractor and the trailer, and a second laser radar and a third laser radar are arranged on both sides of the front of the tractor respectively; the acquisition time of the first laser radar, the second laser radar and the third laser radar are synchronized; the number of the first laser radar, the second laser radar and the third laser radar can be one or more, and can be set according to actual needs; refer to Figure 1 A schematic diagram of a trailer model is shown, and Figure 2 The top view of a semi-trailer is shown in FIG. 1 , wherein the coordinates of the four vertices of the trailer are (x0, y0), (x1, y1), (x2, y2) and (x3, y3), and the coordinates of the connection point between the tractor and the trailer are (x c ,y c), the following assumptions are made in this embodiment: (1) the trailer is assumed to be an invariable cube; (2) the connection between the trailer and the tractor is assumed to be a connection point, and the connection point remains stationary relative to the tractor; (3) the trailer is assumed to only move around Figure 1 The axis of rotation is a straight line perpendicular to the ground and passing through the connection points; as a schematic, Figure 2 In the embodiment, a rear-facing laser radar (corresponding to the first laser radar) is installed in the middle of the tractor and the trailer, and the rear-facing laser radar faces the front of the trailer. A left laser radar and a right laser radar (corresponding to the second laser radar and the third laser radar) are installed on the left and right sides of the front of the tractor respectively. The left laser radar can be installed on the left front of the front of the tractor, and the right laser radar can be installed on the right front of the front of the tractor. The specific installation position can be selected according to actual needs. Figure 2 In the figure, the front of the vehicle corresponds to the positive direction of the x-axis, the left side of the vehicle corresponds to the positive direction of the y-axis, and the positive direction of the z-axis is vertically upward. The laser emission time of the first laser radar, the second laser radar and the third laser radar are synchronized, and laser beams can be emitted simultaneously to scan the trailer; Figure 3 As shown, the method comprises the following steps:

[0026] Step S302, respectively obtain each frame of point cloud scanned by the first laser radar, the second laser radar and the third laser radar.

[0027] The above-mentioned laser radar is a radar system that emits laser beams to detect the position, speed and other characteristic quantities of the target. The above-mentioned point cloud can be understood as a set of point data on the surface of the measured target obtained by scanning the laser radar; in actual implementation, the laser radars installed at three different positions of the semi-trailer can respectively emit laser beams to scan the trailer to obtain each frame of point cloud corresponding to the first laser radar, the second laser radar and the third laser radar.

[0028] Step S304, for each frame point cloud, the current frame point cloud obtained by scanning the first laser radar, the second laser radar and the third laser radar is merged to obtain a current frame merged point cloud corresponding to the current frame point cloud.

[0029] Since the first laser radar, the second laser radar and the third laser radar emit lasers in synchronization, each laser radar can obtain the point cloud of the same frame at the same time. For each frame of point cloud, the point clouds of the same frame of the three laser radars are merged to form a merged point cloud; for example, the first frames of point clouds corresponding to the three laser radars are merged to obtain the first frame of merged point cloud.

[0030] Step S306, based on the preset angle resolution, calculate multiple rotation angles corresponding to the merged point cloud of the current frame, and the three-dimensional model of the trailer rotated according to each rotation angle.

[0031] The above-mentioned angular resolution can be pre-set according to actual needs. For example, the angular resolution can be set to 0.5°, etc.; for different angular ranges, based on the angular resolution, multiple rotation angles can be calculated. For example, when the angle range is 5°-25°, if the angular resolution is 0.5°, 40 rotation angles can be obtained. By rotating the trailer three-dimensional model according to each rotation angle, 40 trailer three-dimensional models can be obtained after rotating at 40 rotation angles.

[0032] Step S308, determining the number of point clouds on the trailer stereo model that match each rotation angle.

[0033] For each frame of merged point cloud, after obtaining the trailer three-dimensional models corresponding to different rotation angles, the number of point clouds that fall on the trailer three-dimensional model corresponding to each rotation angle can be further determined; for example, considering the installation positions of the first laser radar, the second laser radar and the third laser radar, the sum of the number of point clouds that fall on the front plane, the left plane and the right plane of the trailer three-dimensional model corresponding to each rotation angle can be confirmed as the number of point clouds matched to the trailer three-dimensional model corresponding to each rotation angle.

[0034] Step S310, based on the target rotation angle corresponding to the trailer stereo model that matches the largest number of point clouds, determine the angle detection result between the tractor and the trailer corresponding to the merged point cloud of the current frame.

[0035] The number of the above-mentioned target rotation angles may be one or more; for each frame of merged point cloud, after determining the number of point clouds on the trailer stereo model corresponding to each rotation angle, the target rotation angle corresponding to the trailer stereo model matching the largest number of point clouds may be screened out, and the corresponding trailer angle detection result may be determined based on the target rotation angle; for example, if there is only one target rotation angle, it means that the target rotation angle best matches the actual angular position of the trailer, and the target rotation angle may be directly determined as the angle detection result between the tractor and the trailer.

[0036] The above-mentioned angle detection method between the tractor and the trailer obtains each frame of point cloud scanned by the first laser radar, the second laser radar and the third laser radar respectively; for each frame of point cloud, the current frame point cloud scanned by the first laser radar, the second laser radar and the third laser radar are merged to obtain the current frame merged point cloud corresponding to the current frame point cloud; based on the preset angle resolution, multiple rotation angles corresponding to the current frame merged point cloud and the trailer stereo model rotated according to the rotation angle corresponding to each rotation angle are calculated; the number of point clouds on the trailer stereo model corresponding to each rotation angle is determined; based on the target rotation angle corresponding to the trailer stereo model with the largest number of matched point clouds, the angle detection result between the tractor and the trailer corresponding to the current frame merged point cloud is determined. In this method, laser radars are respectively set between the tractor and the trailer and on both sides of the front of the tractor, and the point clouds belonging to the same frame scanned by multiple laser radars are merged, and the angle detection result is determined by judging the number of point clouds of the trailer stereo model corresponding to different rotation angles matched by the merged point cloud. This method will not be affected by light and obstacles, so as to effectively ensure the accuracy and precision of the angle detection between the tractor and the trailer.

[0037] The embodiment of the present invention also provides another method for detecting the angle between the tractor and the trailer, which is implemented on the basis of the method of the above embodiment; the method focuses on describing the specific process of calculating multiple rotation angles corresponding to the merged point cloud of the current frame based on the preset angle resolution, and the three-dimensional model of the trailer rotated according to the rotation angle corresponding to each rotation angle, which specifically corresponds to the following steps S406 to S410; Figure 4 As shown, the method comprises the following steps:

[0038] Step S402, respectively obtain each frame of point cloud scanned by the first laser radar, the second laser radar and the third laser radar.

[0039] Step S404, for each frame point cloud, the current frame point cloud obtained by scanning the first laser radar, the second laser radar and the third laser radar is merged to obtain a current frame merged point cloud corresponding to the current frame point cloud.

[0040] Step S406, if the current frame merged point cloud is the first frame merged point cloud, based on the preset first angle range and the preset angle resolution, calculate multiple rotation angles corresponding to the first frame merged point cloud, and the three-dimensional model of the trailer rotated according to each rotation angle.

[0041] Taking into account the lack of reference trailer angles when detecting trailer angles based on the first frame merged point cloud, and the angle between the tractor and the trailer may be any angle when the lidar is turned on, the first angle range is usually set to -180°~180°; for example, still taking the angle resolution of 0.5° as an example, 720 rotation angles and 720 trailer stereo models corresponding to the 720 rotation angles can be obtained.

[0042] Step S408, if the current frame merged point cloud is a merged point cloud other than the first frame merged point cloud, determine the second angle range corresponding to the current frame merged point cloud based on the angle detection result corresponding to the previous frame merged point cloud of the current frame merged point cloud and the preset angle change amplitude.

[0043] In actual implementation, since the change of the trailer angle is slow relative to the frequency of the laser radar, the range of the trailer angle corresponding to the merged point cloud of the current frame can be inferred based on the trailer angle detected by the merged point cloud of the previous frame. In general, the frame rate of the point cloud is 10HZ, and the trailer angle change amplitude is usually any value between 5° and 20°. For example, the trailer angle detected by the merged point cloud of the previous frame is 15°, and the trailer angle change amplitude is set to 10°, then the trailer angle corresponding to the merged point cloud of the current frame is in the range of 5° to 25°, which is the second angle range mentioned above.

[0044] Step S410, based on the second angle range and the preset angle resolution, calculate multiple rotation angles corresponding to the merged point cloud of the current frame, and the three-dimensional model of the trailer rotated according to each rotation angle.

[0045] Continuing with the example in the previous step, within the second angle range of 5° to 25°, if the angle resolution is 0.5°, 40 rotation angles and 40 trailer stereo models corresponding to the 40 rotation angles can be obtained.

[0046] Step S412, determining the number of point clouds on the trailer stereo model that match each rotation angle.

[0047] Step S414, based on the target rotation angle corresponding to the trailer stereo model that matches the largest number of point clouds, determine the angle detection result between the tractor and the trailer corresponding to the merged point cloud of the current frame.

[0048] In the above-mentioned angle detection method between the tractor and the trailer, if the current frame merged point cloud is the first frame merged point cloud, based on the preset first angle range and the preset angle resolution, multiple rotation angles corresponding to the first frame merged point cloud and the three-dimensional model of the trailer rotated according to the rotation angle corresponding to each rotation angle are calculated. If the current frame merged point cloud is a merged point cloud other than the first frame merged point cloud, based on the angle detection result corresponding to the previous frame merged point cloud of the current frame merged point cloud and the preset angle change amplitude, the second angle range corresponding to the current frame merged point cloud is determined. Based on the second angle range and the preset angle resolution, multiple rotation angles corresponding to the current frame merged point cloud and the three-dimensional model of the trailer rotated according to the rotation angle corresponding to each rotation angle are calculated. For merged point clouds other than the first frame merged point cloud, this method can confirm the corresponding multiple rotation angles and the three-dimensional model of the trailer corresponding to each rotation angle based on the angle detection result corresponding to the previous frame merged point cloud, and then confirm the angle between the tractor and the trailer, which can effectively reduce the amount of calculation and improve the efficiency of angle detection.

[0049] The embodiment of the present invention also provides another method for detecting the angle between the tractor and the trailer, which is implemented on the basis of the method of the above embodiment; the method focuses on describing the specific process of determining the number of point clouds on the trailer stereo model corresponding to each rotation angle, which specifically corresponds to the following steps S508 to S514; Figure 5 As shown, the method comprises the following steps:

[0050] Step S502, respectively obtain each frame of point cloud scanned by the first laser radar, the second laser radar and the third laser radar.

[0051] Step S504, for each frame point cloud, the current frame point cloud obtained by scanning the first laser radar, the second laser radar and the third laser radar is merged to obtain a current frame merged point cloud corresponding to the current frame point cloud.

[0052] Step S506, based on the preset angle resolution, calculate multiple rotation angles corresponding to the merged point cloud of the current frame, and the three-dimensional model of the trailer rotated according to each rotation angle.

[0053] Step S508: for each rotation angle, respectively obtain the number of point clouds of the front plane, the left plane and the right plane of the trailer stereo model that match the rotation angle.

[0054] Step S508 can be implemented by the following steps 1 to 3:

[0055] Step 1: For each rotation angle, calculate the first distance of each feature point in the merged point cloud of the current frame from the front plane of the trailer stereo model corresponding to the rotation angle, the second distance from the left plane, and the third distance from the right plane.

[0056] In actual implementation, you can also set the priority for matching according to actual needs. For example, since the point cloud may be distorted, considering that the distance of the front plane is relatively short and the distortion is relatively small, you can first determine whether the feature point matches the front plane. If the feature point matches the front plane, you do not need to determine whether the feature point matches the left and right planes. Of course, you can continue to determine whether the feature point matches the two planes as an auxiliary detection. If the point does not match the front plane, it is usually necessary to continue to determine whether the feature point matches the left and right planes.

[0057] This step 1 can be specifically implemented by the following steps A to C:

[0058] Step A, for each rotation angle and each feature point in the merged point cloud of the current frame, determine whether the foot of the perpendicular from the feature point to the specified plane of the trailer stereo model corresponding to the rotation angle is located in the specified plane; wherein the specified plane includes: at least one of: a front plane, a left plane and a right plane.

[0059] The above-mentioned designated plane may also be referred to as a trailer plane, which includes at least one of a front plane, a left plane, and a right plane of the trailer stereo model. In actual implementation, it is assumed that the plane equation of the designated plane is:

[0060] ax+by+cz+d=0 (1)

[0061] in,

[0062] x min <x<x max (2)

[0063] y min <y<y max (3)

[0064] z min <z<z max (4)

[0065] x max ,y max 、z max is the maximum value of the specified plane on the x, y, and z axes, x min ,y min 、z min is the minimum value of the specified plane on the x, y, and z axes, and a, b, c, and d are preset coefficients.

[0066] Since the designated planes of the trailer, such as the front plane, the left plane and the right plane, are all finite planes, when calculating the distance from each feature point to the designated plane of the trailer stereo model, it is necessary to first determine whether the foot of the perpendicular from the feature point to the designated plane falls within the designated plane, that is, to determine whether the foot of the perpendicular to the feature point satisfies the above expressions (2)-(4).

[0067] Step B, if the feature point is located in the specified plane, determining a specified distance from the feature point to the specified plane based on the position coordinates of the feature point; wherein the specified distance includes: at least one of a first distance, a second distance and a third distance;

[0068] If it is determined that the foot of the perpendicular from the feature point to the designated plane of the trailer stereo model corresponding to the rotation angle is located in the designated plane, the feature point is represented by q as an example, and the distance from the feature point q to the designated plane can be calculated according to the following formula:

[0069]

[0070] Among them, x q ,y q and z q are the position coordinates of the feature point q in the x-axis direction, y-axis direction and z-axis direction respectively.

[0071] Step C: If the feature point is outside the specified plane, the distance between the foot of the perpendicular and multiple boundary lines of the specified plane is calculated based on the position coordinates of the foot of the perpendicular, and the minimum distance value among the calculated multiple distance values ​​is determined as the distance from the feature point to the specified plane.

[0072] In actual implementation, if the foot of the perpendicular falls outside the specified plane, assuming that the foot of the perpendicular to the feature point q is represented by f, and the specified plane passes through the straight line (x=x0, y=y0) and the straight line (x=x1, y=y1), then the distance from the feature point q to the specified plane can be calculated according to the following formula:

[0073]

[0074] Among them, x f and f are the position coordinates of the foot of perpendicular f in the x-axis direction and the y-axis direction respectively.

[0075] Step 2: determining a matching result corresponding to each feature point based on the first distance, the second distance, the third distance and a preset distance threshold corresponding to each feature point;

[0076] The step 2 can be specifically implemented by the following steps E to G:

[0077] In step E, for each feature point in the merged point cloud of the current frame, if the first distance, the second distance or the third distance corresponding to the feature point meets the preset distance threshold, the plane corresponding to the distance value meeting the preset distance threshold is determined as the plane matched by the feature point.

[0078] The above-mentioned preset distance threshold can be set according to actual needs; during the trailer detection process, it is necessary to simultaneously calculate the distance between each feature point and the front plane, left plane and right plane of the trailer three-dimensional model, that is, to obtain the first distance, second distance and third distance corresponding to each feature point. For each feature point, if any one of the three distance values ​​calculated is less than the preset distance threshold, it can be determined that the feature point matches the plane corresponding to the distance value. For example, if the first distance corresponding to the feature point meets the preset distance threshold, and the second distance and the third distance do not meet the preset distance threshold, it can be determined that the feature point matches the front plane of the trailer three-dimensional model, but does not match the left plane and the right plane.

[0079] Step F: If at least two distance values ​​among the first distance, the second distance and the third distance corresponding to the feature point meet the preset distance threshold, the plane corresponding to the smallest distance value among the at least two distance values ​​is determined as the plane matched by the feature point.

[0080] For each feature point, if two or more of the three distances calculated are less than the preset distance threshold, the plane corresponding to the smallest distance value among the multiple distance values ​​is usually determined as the plane matched by the feature point; for example, if the first distance and the second distance corresponding to the feature point both meet the preset distance threshold, and the third distance does not meet the preset distance threshold, the first distance and the second distance are compared. If the first distance is less than the second distance, it can be determined that the feature point matches the plane corresponding to the first distance, that is, it matches the front plane of the trailer three-dimensional model, and does not match the left plane and the right plane.

[0081] Step G: If the first distance, the second distance, and the third distance corresponding to the feature point do not meet the preset distance threshold, it is determined that the feature point does not match the front plane, the left plane, and the right plane.

[0082] Step three, based on the matching results corresponding to each feature point, determine the number of point clouds that match the front plane, the left plane, and the right plane of the trailer stereo model corresponding to the rotation angle.

[0083] After determining the matching result corresponding to each feature point, the number of point clouds of the front plane, left plane and right plane of the trailer stereo model corresponding to the current rotation angle can be determined; for example, 100 feature points match the front plane, 50 feature points match the left plane, 50 feature points match the right plane, and so on.

[0084] Step S510, summing up the number of point clouds on the front plane, the left plane and the right plane of the trailer stereo model that matches the rotation angle, to obtain the number of point clouds on the trailer stereo model that matches the rotation angle.

[0085] For each rotation angle, after determining the number of point clouds on the front plane, left plane and right plane of the trailer three-dimensional model corresponding to the rotation angle, these numbers can be added to obtain the number of point clouds on the trailer three-dimensional model corresponding to the rotation angle; for example, there are 100 feature points matching the front plane, 50 feature points matching the left plane, and 50 feature points matching the right plane, then the number of point clouds on the trailer three-dimensional model matching the rotation angle is 100+50+50=200.

[0086] Step S512: If the number of target rotation angles corresponding to the trailer stereo model with the largest number of matched point clouds is one, the target rotation angle is determined as the angle detection result between the tractor and the trailer corresponding to the merged point cloud of the current frame.

[0087] If the number of target rotation angles corresponding to the trailer stereo model with the largest number of matched point clouds is one, for example, when the rotation angle is 15°, the trailer stereo model corresponding to the rotation angle matches the largest number of matched point clouds, then it can be determined that the angle detection result between the tractor and the trailer is 15°. By determining which trailer stereo model has the most point clouds, the trailer stereo model with the most matched point clouds is the matched trailer stereo model, and the angle between the orientation angle of this trailer stereo model and the x-axis is the angle of the trailer.

[0088] Step S514: If there are multiple target rotation angles corresponding to the trailer stereo model that matches the largest number of point clouds, calculate the total distance between the matched feature points and the corresponding plane for each target rotation angle, and determine the target rotation angle corresponding to the minimum value in the calculated total distance as the angle detection result between the tractor and the trailer corresponding to the merged point cloud of the current frame.

[0089] If the number of target rotation angles corresponding to the trailer stereo model with the largest number of matched point clouds is multiple, for example, when the rotation angles are 15° and 15.5° respectively, the trailer stereo models corresponding to the two rotation angles match the largest number of point clouds, and the number of matched point clouds is 200. At this time, the distances between the 200 matched feature points and the matched planes are usually calculated respectively when the rotation angles are 15° and 15.5°, and the total distances corresponding to the two rotation angles are added up. The target rotation angle corresponding to the minimum value of the calculated total distance is determined as the final trailer angle detection result. For example, when the rotation angle is 15°, the total distance between the 200 matched feature points and the corresponding plane is 20 mm, and when the rotation angle is 15.5°, the total distance between the 200 matched feature points and the corresponding plane is 30 mm. It can be determined that the angle between the tractor and the trailer detected according to the merged point cloud of the current frame is 15°.

[0090] After determining the angle between the tractor and the trailer, the actual position of the trailer can be determined. It is known that the front plane of the trailer passes through the straight line (x=x0, y=y0) and the straight line (x=x3, y=y3), then the equation of the front plane of the trailer is:

[0091] (y3-y0)(x-x0)-(x3-x0)(y-y0)=0 (7)

[0092] The plane on the left side of the trailer passes through the straight line (x=x2, y=y2) and the straight line (x=x3, y=y3), so the equation of the plane on the left side of the trailer is:

[0093] (y3-y2)(x-x2)-(x3-x2)(y-y2)=0 (8)

[0094] The plane on the right side of the trailer passes through the straight line (x=x0, y=y0) and the straight line (x=x1, y=y1), so the equation of the plane on the right side of the trailer is:

[0095] (y1-y0)(x-x0)-(x1-x0)(y-y0)=0 (9)

[0096] After the trailer rotates by angle θ, the front plane of the trailer passes through the straight line (x=x0', y=y0') and the straight line (x=x3', y=y3'), the left plane of the trailer passes through the straight line (x=x2', y=y2') and the straight line (x=x3', y=y3'), and the right plane of the trailer passes through the straight line (x=x0', y=y0') and the straight line (x=x1', y=y1'), where:

[0097] x0'=(x0-x c )cosθ-(y0-y c )sinθ+x c(10)

[0098] y0'=(x0-x c )sinθ+(y0-y c )cosθ+y c (11)

[0099] x1'=(x1-x c )cosθ-(y1-y c )sinθ+x c (12)

[0100] y1'=(x1-x c )sinθ+(y1-y c )cosθ+y c (13)

[0101] x2'=(x2-x c )cosθ-(y2-y c )sinθ+x c (14)

[0102] y2'=(x2-x c )sinθ+(y2-y c )cosθ+y c (15)

[0103] x3'=(x3-x c )cosθ-(y3-y c )sinθ+x c (16)

[0104] y3'=(x3-x c )sinθ+(y3-y c )cosθ+y c (17)

[0105] Among them, Figure 2 As shown, x c and c are the position coordinates of the connection point in the x-axis direction and the y-axis direction respectively; after rotating by an angle θ, the plane equation of the front part of the trailer is:

[0106] (y3'-y0')(x-x0')-(x3'-x0')(y-y0')=0 (18)

[0107] The plane equation of the left side of the trailer is:

[0108] (y3'-y2')(x-x2')-(x3'-x2')(y-y2')=0 (19)

[0109] The plane equation of the right side of the trailer is:

[0110] (y1'-y0')(x-x0')-(x1'-x0')(y-y0')=0 (20)

[0111] The above-mentioned angle detection method between the tractor and the trailer uses laser radar instead of a camera to detect the position of the trailer. It relies on the shape of the trailer and the position of the connection point. It mainly detects the shape of the trailer instead of the pattern on the surface of the trailer. It does not need to stick a QR code on the surface of the trailer, which will not affect the appearance of the trailer and can be adapted to trailers with various coatings. This method uses rearward and lateral laser radars to detect trailers. Since there are almost no external obstacles between the rearward laser radar and the trailer, the trailer detection effect of this method will not be affected by external obstacles, and the reliability of the detection results is higher. In addition, the camera-based trailer angle detection scheme in the related art relies on external light sources. In the case of insufficient light, the trailer detection accuracy will decrease. In this scheme, since the light source of the laser radar is spontaneous, the point cloud will not be affected by external light sources and can operate around the clock. Therefore, it can be detected normally during the day or at night, and maintain a good detection effect. Compared with a single rear-facing laser radar, this method installs laser radars between the tractor and the trailer, and on both sides of the front of the tractor, and uses rear-facing and side-facing laser radars to detect the front plane and left and right planes of the trailer to determine the angle of the trailer. Since the left and right planes of the trailer are longer than the front plane, detecting the front plane and both side planes of the trailer at the same time can greatly improve the detection accuracy of the trailer's rotation angle.

[0112] Corresponding to the above method embodiment, the present invention also provides an angle detection device between a tractor and a trailer, wherein a first laser radar is arranged between the tractor and the trailer, and a second laser radar and a third laser radar are arranged on both sides of the front of the tractor respectively; the acquisition time of the first laser radar, the second laser radar and the third laser radar are synchronized; Figure 6 As shown, the device includes: an acquisition module 60, which is used to respectively acquire each frame of point cloud obtained by scanning the first laser radar, the second laser radar and the third laser radar; a merging processing module 61, which is used to merge the current frame point cloud obtained by scanning the first laser radar, the second laser radar and the third laser radar for each frame of point cloud, and obtain the current frame merged point cloud corresponding to the current frame point cloud; a calculation module 62, which is used to calculate multiple rotation angles corresponding to the current frame merged point cloud based on a preset angle resolution, and a three-dimensional model of the trailer rotated according to the rotation angle corresponding to each rotation angle; a first determination module 63, which is used to determine the number of point clouds on the three-dimensional model of the trailer corresponding to each rotation angle; a second determination module 64, which is used to determine the angle detection result between the tractor and the trailer corresponding to the current frame merged point cloud based on the target rotation angle corresponding to the three-dimensional model of the trailer with the largest number of matched point clouds.

[0113] The angle detection device between the tractor and the trailer obtains each frame of point cloud scanned by the first laser radar, the second laser radar and the third laser radar respectively; for each frame of point cloud, the current frame point cloud scanned by the first laser radar, the second laser radar and the third laser radar are merged to obtain the current frame merged point cloud corresponding to the current frame point cloud; based on the preset angle resolution, multiple rotation angles corresponding to the current frame merged point cloud and the three-dimensional model of the trailer rotated according to the rotation angle corresponding to each rotation angle are calculated; the number of point clouds matched to the three-dimensional model of the trailer corresponding to each rotation angle is determined; based on the target rotation angle corresponding to the three-dimensional model of the trailer with the largest number of matched point clouds, the angle detection result between the tractor and the trailer corresponding to the current frame merged point cloud is determined. The device sets laser radars between the tractor and the trailer and on both sides of the front of the tractor, merges the point clouds belonging to the same frame scanned by multiple laser radars, and determines the angle detection result by judging the number of point clouds of the merged point cloud matched to the three-dimensional model of the trailer corresponding to different rotation angles. The device will not be affected by light and obstacles, so as to effectively ensure the accuracy and precision of the angle detection between the tractor and the trailer.

[0114] Furthermore, the calculation module is also used to: if the current frame merged point cloud is the first frame merged point cloud, based on a preset first angle range and a preset angle resolution, calculate multiple rotation angles corresponding to the first frame merged point cloud, and the three-dimensional model of the trailer rotated according to each rotation angle; if the current frame merged point cloud is a merged point cloud other than the first frame merged point cloud, based on the angle detection result corresponding to the previous frame merged point cloud of the current frame merged point cloud, and a preset angle change amplitude, determine the second angle range corresponding to the current frame merged point cloud; based on the second angle range and the preset angle resolution, calculate multiple rotation angles corresponding to the current frame merged point cloud, and the three-dimensional model of the trailer rotated according to each rotation angle.

[0115] Furthermore, the first determination module is also used to: for each rotation angle, respectively obtain the number of point clouds of the front plane, left plane and right plane of the trailer three-dimensional model that match the rotation angle; add the number of point clouds of the front plane, left plane and right plane of the trailer three-dimensional model that match the rotation angle to obtain the number of point clouds on the trailer three-dimensional model that matches the rotation angle.

[0116] Furthermore, the first determination module is also used to: calculate, for each rotation angle, a first distance from the front plane of the trailer three-dimensional model corresponding to the rotation angle, a second distance from the left plane, and a third distance from the right plane of each feature point in the merged point cloud of the current frame; determine the matching result corresponding to each feature point based on the first distance, second distance, third distance and a preset distance threshold corresponding to each feature point; determine the number of point clouds matched to the front plane, left plane and right plane of the trailer three-dimensional model corresponding to the rotation angle based on the matching result corresponding to each feature point.

[0117] Furthermore, the first determination module is also used to: for each rotation angle, and each feature point in the merged point cloud of the current frame, determine whether the foot of the perpendicular from the feature point to the specified plane of the trailer three-dimensional model corresponding to the rotation angle is located within the specified plane; wherein the specified plane includes: at least one of a front plane, a left plane and a right plane; if it is located within the specified plane, determine the specified distance from the feature point to the specified plane based on the position coordinates of the feature point; wherein the specified distance includes: at least one of a first distance, a second distance and a third distance; if it is located outside the specified plane, calculate the distance between the foot of the perpendicular and multiple boundary lines of the specified plane based on the position coordinates of the foot of the perpendicular, and determine the minimum distance value among the calculated multiple distance values ​​as the distance from the feature point to the specified plane.

[0118] Furthermore, the first determination module is also used to: for each feature point in the merged point cloud of the current frame, if the first distance, the second distance or the third distance corresponding to the feature point meets the preset distance threshold, the plane corresponding to the distance value that meets the preset distance threshold is determined as the plane matched by the feature point; if at least two distance values ​​among the first distance, the second distance and the third distance corresponding to the feature point meet the preset distance threshold, the plane corresponding to the smallest distance value among the at least two distance values ​​is determined as the plane matched by the feature point; if the first distance, the second distance and the third distance corresponding to the feature point do not meet the preset distance threshold, it is determined that the feature point does not match the front plane, the left plane and the right plane.

[0119] Furthermore, the second determination module is also used for: if the number of target rotation angles corresponding to the trailer stereo model that matches the largest number of point clouds is one, determining the target rotation angle as the angle detection result between the tractor and the trailer corresponding to the current frame merged point cloud; if the number of target rotation angles corresponding to the trailer stereo model that matches the largest number of point clouds is multiple, calculating the total distance between the matched feature points and the corresponding plane corresponding to each target rotation angle, and determining the target rotation angle corresponding to the minimum value of the calculated total distance as the angle detection result between the tractor and the trailer corresponding to the current frame merged point cloud.

[0120] The angle detection device between the tractor and the trailer provided in the embodiment of the present invention has the same implementation principle and technical effects as those in the aforementioned embodiment of the angle detection method between the tractor and the trailer. For the sake of brief description, for matters not mentioned in the embodiment of the angle detection device between the tractor and the trailer, reference may be made to the corresponding contents in the aforementioned embodiment of the angle detection method between the tractor and the trailer.

[0121] The embodiment of the present invention further provides an electronic device, see Figure 7 As shown, the electronic device includes a processor 130 and a memory 131. The memory 131 stores machine executable instructions that can be executed by the processor 130. The processor 130 executes the machine executable instructions to implement the above-mentioned angle detection method between the tractor and the trailer.

[0122] Further, Figure 7 The electronic device shown further includes a bus 132 and a communication interface 133 , and the processor 130 , the communication interface 133 and the memory 131 are connected via the bus 132 .

[0123] The memory 131 may include a high-speed random access memory (RAM), and may also include a non-volatile memory, such as at least one disk storage. The communication connection between the system network element and at least one other network element is realized through at least one communication interface 133 (which may be wired or wireless), and the Internet, wide area network, local area network, metropolitan area network, etc. may be used. The bus 132 may be an ISA bus, a PCI bus, or an EISA bus, etc. The bus may be divided into an address bus, a data bus, a control bus, etc. For ease of representation, Figure 7 Only one bidirectional arrow is used in the diagram, but this does not mean that there is only one bus or only one type of bus.

[0124] The processor 130 may be an integrated circuit chip with signal processing capabilities. In the implementation process, each step of the above method can be completed by the hardware integrated logic circuit or software instructions in the processor 130. The above processor 130 can be a general-purpose processor, including a central processing unit (CPU), a network processor (NP), etc.; it can also be 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, discrete hardware components. The methods, steps and logic block diagrams disclosed in the embodiments of the present invention can be implemented or executed. The general-purpose processor can be a microprocessor or the processor can also be any conventional processor, etc. The steps of the method disclosed in conjunction with the embodiments of the present invention can be directly embodied as a hardware decoding processor for execution, or a combination of hardware and software modules in the decoding processor for execution. The software module may be located in a storage medium mature in the art, such as a random access memory, a flash memory, a read-only memory, a programmable read-only memory, or an electrically erasable programmable memory, a register, etc. The storage medium is located in the memory 131, and the processor 130 reads the information in the memory 131 and completes the steps of the method of the above embodiment in combination with its hardware.

[0125] An embodiment of the present invention also provides a machine-readable storage medium, which stores machine-executable instructions. When the machine-executable instructions are called and executed by a processor, the machine-executable instructions prompt the processor to implement the above-mentioned angle detection method between the tractor and the trailer. The specific implementation can be found in the method embodiment, which will not be repeated here.

[0126] The computer program product of the angle detection method, device and electronic device between the tractor and the trailer provided in the embodiments of the present invention includes a computer-readable storage medium storing program code. The instructions included in the program code can be used to execute the method described in the previous method embodiments. The specific implementation can be found in the method embodiments, which will not be repeated here.

[0127] If the functions 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 invention, or the part that contributes to the prior art or the part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium, including several instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the methods described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk, etc., which can store program codes.

[0128] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for detecting the angle between a tractor and a trailer, characterized in that: A first laser radar is arranged between the tractor and the trailer, and a second laser radar and a third laser radar are arranged on both sides of the front of the tractor respectively; the acquisition time of the first laser radar, the second laser radar and the third laser radar is synchronized; the method comprises: Respectively obtain each frame of point cloud scanned by the first laser radar, the second laser radar, and the third laser radar; For each frame of point cloud, merging the current frame point cloud scanned by the first laser radar, the second laser radar and the third laser radar to obtain a current frame merged point cloud corresponding to the current frame point cloud; Based on a preset angle resolution, calculate a plurality of rotation angles corresponding to the merged point cloud of the current frame, and a three-dimensional model of the trailer rotated according to each rotation angle; Determine the number of point clouds on the trailer stereo model that match each of the rotation angles; Based on the target rotation angle corresponding to the trailer stereo model that matches the largest number of point clouds, the angle detection result between the tractor and the trailer corresponding to the merged point cloud of the current frame is determined.

2. The method according to claim 1, characterized in that The step of calculating multiple rotation angles corresponding to the merged point cloud of the current frame based on the preset angle resolution, and the three-dimensional model of the trailer rotated according to each rotation angle includes: If the current frame merged point cloud is the first frame merged point cloud, based on a preset first angle range and the preset angle resolution, calculate a plurality of rotation angles corresponding to the first frame merged point cloud, and a three-dimensional model of the trailer rotated according to each rotation angle; If the current frame merged point cloud is a merged point cloud other than the first frame merged point cloud, based on the angle detection result corresponding to the previous frame merged point cloud of the current frame merged point cloud and a preset angle change amplitude, determine a second angle range corresponding to the current frame merged point cloud; Based on the second angle range and the preset angle resolution, multiple rotation angles corresponding to the current frame merged point cloud and a three-dimensional model of the trailer rotated according to each rotation angle are calculated.

3. The method according to claim 1, characterized in that The step of determining the number of point clouds on the trailer stereo model that match each rotation angle comprises: For each rotation angle, respectively obtain the number of point clouds of the front plane, the left plane, and the right plane of the trailer stereo model that match the rotation angle; The number of point clouds on the front plane, the left plane, and the right plane of the trailer stereo model that match the rotation angle are added together to obtain the number of point clouds on the trailer stereo model that match the rotation angle.

4. The method according to claim 3, characterized in that The step of respectively obtaining the number of point clouds of the front plane, the left plane and the right plane of the trailer stereo model that match the rotation angle for each rotation angle comprises: For each rotation angle, calculating a first distance from each feature point in the merged point cloud of the current frame to the front plane of the trailer stereo model corresponding to the rotation angle, a second distance from the left plane, and a third distance from the right plane; Determine a matching result corresponding to each feature point based on the first distance, the second distance, the third distance and a preset distance threshold corresponding to each feature point; Based on the matching result corresponding to each feature point, the number of point clouds matching the front plane, the left plane and the right plane of the trailer stereo model corresponding to the rotation angle is determined.

5. The method according to claim 4, characterized in that The step of calculating, for each rotation angle, a first distance from each feature point in the merged point cloud of the current frame to the front plane of the trailer stereo model corresponding to the rotation angle, a second distance from the left plane, and a third distance from the right plane comprises: For each rotation angle and each feature point in the merged point cloud of the current frame, determine whether the foot of the perpendicular from the feature point to the designated plane of the trailer stereo model corresponding to the rotation angle is located in the designated plane; wherein the designated plane includes: at least one of the front plane, the left plane and the right plane; If the feature point is located in the specified plane, determining a specified distance from the feature point to the specified plane based on the position coordinates of the feature point; wherein the specified distance includes: at least one of the first distance, the second distance and the third distance; If it is outside the specified plane, the distance between the foot of the perpendicular and multiple boundary lines of the specified plane is calculated based on the position coordinates of the foot of the perpendicular, and the minimum distance value among the calculated multiple distance values ​​is determined as the distance from the feature point to the specified plane.

6. The method according to claim 4, characterized in that The step of determining the matching result corresponding to each feature point based on the first distance, the second distance, the third distance and a preset distance threshold corresponding to each feature point includes: For each feature point in the merged point cloud of the current frame, if the first distance, the second distance, or the third distance corresponding to the feature point meets the preset distance threshold, a plane corresponding to the distance value meeting the preset distance threshold is determined as the plane matched by the feature point; If at least two distance values ​​among the first distance, the second distance and the third distance corresponding to the feature point meet the preset distance threshold, a plane corresponding to the smallest distance value among the at least two distance values ​​is determined as the plane matched by the feature point; If the first distance, the second distance, and the third distance corresponding to the feature point do not meet the preset distance threshold, it is determined that the feature point does not match the front plane, the left plane, and the right plane.

7. The method according to claim 1, characterized in that The step of determining the angle detection result between the tractor and the trailer corresponding to the merged point cloud of the current frame based on the target rotation angle corresponding to the trailer stereo model that matches the largest number of point clouds comprises: If the number of target rotation angles corresponding to the trailer stereo model that matches the largest number of point clouds is one, the target rotation angle is determined as the angle detection result between the tractor and the trailer corresponding to the merged point cloud of the current frame; If there are multiple target rotation angles corresponding to the trailer stereo model that matches the largest number of point clouds, calculate the total distance between the matched feature points and the corresponding plane corresponding to each target rotation angle, and determine the target rotation angle corresponding to the minimum value in the calculated total distance as the angle detection result between the tractor and the trailer corresponding to the merged point cloud of the current frame.

8. An angle detection device between a tractor and a trailer, characterized in that: A first laser radar is arranged between the tractor and the trailer, and a second laser radar and a third laser radar are arranged on both sides of the front of the tractor respectively; the acquisition time of the first laser radar, the second laser radar and the third laser radar is synchronized; the device comprises: An acquisition module, used to respectively acquire each frame of point cloud scanned by the first laser radar, the second laser radar, and the third laser radar; A merging processing module is used to merge the current frame point cloud scanned by the first laser radar, the second laser radar and the third laser radar for each frame point cloud, so as to obtain a current frame merged point cloud corresponding to the current frame point cloud; A calculation module, used for calculating, based on a preset angle resolution, a plurality of rotation angles corresponding to the merged point cloud of the current frame, and a three-dimensional model of the trailer rotated according to each rotation angle; A first determination module is used to determine the number of point clouds on the trailer stereo model that match each rotation angle; The second determination module is used to determine the angle detection result between the tractor and the trailer corresponding to the merged point cloud of the current frame based on the target rotation angle corresponding to the trailer stereo model that matches the largest number of point clouds.

9. An electronic device, characterized in that: It comprises a processor and a memory, wherein the memory stores machine executable instructions that can be executed by the processor, and the processor executes the machine executable instructions to implement the angle detection method between the tractor and the trailer as described in any one of claims 1-7.

10. A machine-readable storage medium, characterized in that: The machine-readable storage medium stores machine-executable instructions. When the machine-executable instructions are called and executed by the processor, the machine-executable instructions prompt the processor to implement the angle detection method between the tractor and the trailer as described in any one of claims 1-7.

Images