Trailer angle detection method and device, vehicle and medium

Abstract

The invention relates to the technical field of automatic driving, and discloses a trailer angle detection method and device, a vehicle and a medium, a first point cloud sensor is installed on the face, facing a trailer, of a vehicle head, and a reflector is installed at the position, facing the vehicle head, of the trailer. Generating a reflector point cloud plane corresponding to the reflector; the trailer deviation angle is determined according to the included angle of the reflector point cloud plane relative to the head advancing direction. According to the invention, the accuracy of vehicle trailer deflection angle detection is improved.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving technology, and in particular to a trailer angle detection method, device, vehicle and medium. Background Art

[0002] Autonomous driving technology is a technology that uses artificial intelligence, sensors, control and other technologies to enable vehicles to complete driving tasks autonomously without human intervention. Autonomous driving technology has been rapidly developed and widely used in recent years, especially in engineering fields such as ports and mines. The use of autonomous driving technology for container trucks is gradually being realized. A container truck is a heavy-duty transport vehicle consisting of a front truck and a trailer. It can load and transport goods in scenes such as ports and mines. However, the autonomous driving of container trucks also faces some challenges and difficulties, one of which is how to detect the angle of the trailer in real time. Because a certain angle will be generated between the front truck and the trailer during the turning process of the container truck, this will lead to blind spots in the field of vision, and it is easy to cause collisions and scratches between the trailer and obstacles. Therefore, real-time detection of the offset angle of the trailer relative to the front truck plays an important role in improving the perception and obstacle avoidance capabilities of the autonomous driving system for obstacles, pedestrians and other targets. Summary of the invention

[0003] In view of this, the present invention provides a trailer angle detection method, device, vehicle and medium to solve the problem of inaccurate detection of the deflection angle of the vehicle trailer.

[0004] In a first aspect, the present invention provides a method for detecting the angle of a trailer, wherein a first point cloud sensor is installed on a side of the front of the vehicle facing the trailer, and a reflector is installed at a position of the trailer facing the front of the vehicle, the method comprising: scanning the reflector by the first point cloud sensor to generate a reflector point cloud plane corresponding to the reflector; and determining the offset angle of the trailer based on the angle of the reflector point cloud plane relative to the forward direction of the front of the vehicle.

[0005] In an optional embodiment, a first reflector is installed on the front baffle of the trailer facing the front of the vehicle, and a second reflector is installed on the left and right sides near the trailer saddle, and scanning the reflector by the first point cloud sensor to generate a reflector point cloud corresponding to the reflector includes: scanning the first reflector and the second reflector by the first point cloud sensor to generate corresponding first reflector point cloud planes and second reflector point cloud planes, respectively.

[0006] In an optional embodiment, the trailer offset angle is determined according to the angle of the reflector point cloud plane relative to the forward direction of the vehicle head, including: determining the initial offset angle of the trailer relative to the vehicle head according to the angle of the first reflector point cloud plane relative to the forward direction of the vehicle head; judging whether the initial offset angle belongs to a small-angle rotation case; if it belongs to a small-angle rotation case, using the initial offset angle as the trailer offset angle; if it does not belong to a small-angle rotation case, determining an auxiliary angle according to the angle of the second reflector point cloud plane relative to the forward direction of the vehicle head; and calculating the trailer offset angle using the auxiliary angle based on the vertical plane relationship between the first reflector point cloud plane and the second reflector point cloud plane.

[0007] In an optional implementation, the determination of whether the initial trailer offset angle belongs to a small-angle rotation situation includes: determining whether a first angle change value is less than a first preset angle threshold, the first angle change value being the difference between the initial offset angle calculated in the current calculation cycle and the relative front angle of the trailer in a straight-ahead state; determining whether a second angle change value is less than a second preset angle threshold, the second angle change value being the difference between the initial offset angle calculated in the current calculation cycle and the trailer offset angle calculated in the previous calculation cycle; when the first angle change value calculated in the current calculation cycle is less than the first preset angle threshold, and the second angle change value is less than the second preset angle threshold, it is determined that it belongs to the small-angle rotation situation, otherwise it does not belong to the small-angle rotation situation.

[0008] In an optional embodiment, the first reflector and the second reflector are scanned by the first point cloud sensor to generate corresponding first reflector point cloud planes and second reflector point cloud planes, respectively, including: scanning the current reflector to obtain the initial point cloud of the current reflector; intercepting the point cloud of the region of interest from the initial point cloud of the current reflector based on pass-through filtering; processing the intercepted point cloud of the region of interest based on voxel filtering; obtaining the normal vector of each point from the processed point cloud of the region of interest; if the current reflector is the first reflector, retaining the points whose angles between the normal vector and the direction of travel of the vehicle head are less than a third preset angle threshold, and if the current reflector is the second reflector, removing the points whose angles between the normal vector and the direction of travel of the vehicle head are less than the third preset angle threshold; clustering the point cloud of the region of interest after normal vector processing; plane fitting the clustered point cloud of the region of interest based on a plane fitting method, and using the fitted plane as the current reflector point cloud plane.

[0009] In an optional embodiment, the plane fitting method is used to perform plane fitting on the clustered point cloud of the region of interest, and the fitted plane is used as the current reflector point cloud plane, including: using a random sampling consistency algorithm to perform plane fitting on the clustered point cloud of the region of interest to obtain a reflector plane equation and the number of outlier points; judging whether the number of outlier points is greater than a preset point threshold; if the number of outlier points is greater than the preset point threshold, filtering the outliers and returning to the step of using a random sampling consistency algorithm to perform plane fitting on the clustered point cloud of the region of interest; if the number of outlier points is not greater than the preset point threshold, judging whether it is greater than a preset number of iterations; if it is greater than the preset number of iterations, determining the reflector point cloud plane based on the current reflector plane equation; if it is not greater than the preset number of iterations, returning to the step of using a random sampling consistency algorithm to perform plane fitting on the clustered point cloud of the region of interest.

[0010] In an optional implementation, the method further includes: collecting an environmental point cloud; generating a body filter frame according to the geometric parameters of the vehicle and the trailer offset angle; and filtering the environmental point cloud using the body filter frame to remove the portion of the environmental point cloud that belongs to the body.

[0011] In an optional implementation, the environmental point cloud is collected by a second point cloud sensor installed on the front of the vehicle, and the body filter frame is generated according to the geometric parameters of the vehicle and the trailer offset angle, including: mapping the trailer offset angle under the detection coordinates of the first point cloud sensor to the detection coordinates of the second point cloud sensor according to the timestamp difference between the first point cloud sensor and the second point cloud sensor to obtain a mapping offset angle; and generating a body filter frame based on the geometric parameters of the vehicle and the mapping offset angle.

[0012] In an optional implementation, the method maps the trailer offset angle under the detection coordinates of the first point cloud sensor to the detection coordinates of the second point cloud sensor according to the timestamp difference between the first point cloud sensor and the second point cloud sensor to obtain the mapping offset angle, including: calculating the angle difference between the trailer offset angle detected by the first point cloud sensor in the current calculation cycle and the trailer offset angle detected in the previous calculation cycle; calculating the first timestamp difference between the timestamp of the current calculation cycle and the timestamp of the previous calculation cycle of the first point cloud sensor; calculating the trailer offset angular velocity in the first point cloud sensor through the angle difference and the first timestamp difference; calculating the second timestamp difference between the timestamp of the current calculation cycle of the first point cloud sensor and the timestamp of the current calculation cycle of the second point cloud sensor; calculating the mapping compensation angle through the product of the trailer offset angular velocity and the second timestamp difference; calculating the difference between the mapping compensation angle and the trailer offset angle detected by the first point cloud sensor in the current calculation cycle, and determining the corresponding mapping offset angle under the detection coordinates of the second point cloud sensor.

[0013] In a second aspect, the present invention provides a trailer angle detection device, wherein a first point cloud sensor is installed on a side of the front of the vehicle facing the trailer, and a reflector is installed at a position of the trailer facing the front of the vehicle, the device comprising: a reflector scanning module, used to scan the reflector through the first point cloud sensor to generate a reflector point cloud plane corresponding to the reflector; an angle calculation module, used to determine the trailer offset angle according to the angle between the reflector point cloud plane and the forward direction of the front of the vehicle.

[0014] In the third aspect, the present invention provides a vehicle, comprising a front of the vehicle, a trailer and a controller, wherein a first point cloud sensor is installed on a side of the front of the vehicle facing the trailer, a reflector is installed at a position of the trailer facing the front of the vehicle, and a second point cloud sensor is also installed on the front of the vehicle; the first point cloud sensor is used to detect the trailer offset angle relative to the front of the vehicle; the second point cloud sensor is used to collect environmental point clouds; the controller is installed inside the front of the vehicle, and is used to execute computer instructions, thereby implementing the method of the first aspect or any corresponding implementation method thereof.

[0015] In a fourth aspect, the present invention provides a computer-readable storage medium having computer instructions stored thereon, the computer instructions being used to cause a computer to execute the method of the first aspect or any corresponding embodiment thereof.

[0016] The technical solution provided by the present invention has the following advantages:

[0017] The detection device in the present invention is composed of a first point cloud sensor installed on the back of the front of the vehicle and a reflector posted at the front gear position of the trailer. By scanning the point cloud plane of the reflector and analyzing the angle between the point cloud plane of the reflector and the forward direction of the front of the vehicle, accurate detection of the offset angle of the trailer is achieved. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] 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.

[0019] Figure 1 is a flow chart of a method for detecting a trailer angle according to an embodiment of the present invention;

[0020] Figure 2 is a structural schematic diagram of a vehicle according to an embodiment of the present invention;

[0021] Figure 3 is another flow chart of a method for detecting a trailer angle according to an embodiment of the present invention;

[0022] Figure 4 is another flow chart of a method for detecting a trailer angle according to an embodiment of the present invention;

[0023] Figure 5 is another flow chart of a method for detecting a trailer angle according to an embodiment of the present invention;

[0024] Figure 6 is another flow chart of a method for detecting a trailer angle according to an embodiment of the present invention;

[0025] Figure 7 is another flow chart of a method for detecting a trailer angle according to an embodiment of the present invention;

[0026] Figure 8 4 is a schematic structural diagram of a trailer angle detection device according to an embodiment of the present invention. DETAILED DESCRIPTION

[0027] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of the present invention.

[0028] Autonomous driving of container trucks faces some challenges and problems, one of which is how to detect the angle of the trailer in real time. When a container truck turns, there will be a certain angle between the front of the truck and the trailer, which will lead to blind spots in the field of vision, making it easy for the trailer to collide with obstacles and other dangerous situations. Therefore, real-time detection of the rotation angle of the trailer relative to the front of the truck plays an important role in improving the autonomous driving system's ability to perceive and avoid obstacles such as obstacles and pedestrians.

[0029] In the existing trailer rotation angle detection scheme, the most commonly used technical method is to install a single or multiple angle encoders at the joint between the front of the vehicle and the trailer or between the two sides of the back of the front of the vehicle and the trailer to measure the rotation angle of the trailer. This implementation method usually has problems such as complex installation, high maintenance cost and high hardware cost.

[0030] In addition, there is also a method of using a camera to detect the angle of the trailer. However, the disadvantage of this method is that the calculation accuracy is greatly affected by the image quality and lighting conditions. It cannot be used in low-light environments such as at night and in reflective scenes. Its usage conditions are not suitable for the 24-hour continuous operation requirements in the field of autonomous driving.

[0031] Some related technologies have proposed trailer angle detection solutions based on neural networks. This type of solution installs QR codes on both sides of the trailer, uses a camera to shoot the QR codes in real time, and then uses a neural network to recognize the image to obtain information such as corner points, borders, and ID, thereby calculating the trailer angle. Since this type of method also uses a camera sensor, it is greatly affected by image quality and lighting conditions. In addition, the QR codes installed during use are easily blocked by dust, etc., resulting in unstable recognition, resulting in low detection accuracy.

[0032] There is also a related technology that proposes a trailer turning angle estimation method based on laser radar. High-reflective materials are installed at the four corners and two wheel brows of the trailer, and then the two main radars on the left and right in front of the front of the vehicle are used to scan the high-reflective materials. The turning angle of the trailer is calculated by extracting the corner points and comparing them with the trailer template. The disadvantage of this method is that the position between the two main radars and the high-reflective materials is too tricky, and the main radar harness must be sufficient to identify the reflector. In addition, since the wheel brow is at a low position, the reflective material is easily covered with dust and mud during long-term operation or driving in rainy days, making it impossible to extract stably.

[0033] Therefore, it is crucial to find a new method to accurately detect the trailer offset angle.

[0034] According to an embodiment of the present invention, an embodiment of a trailer angle detection method is provided. It should be noted that the steps shown in the flowchart of the accompanying drawings can be executed in a computer system such as a set of computer executable instructions, and although a logical order is shown in the flowchart, in some cases, the steps shown or described can be executed in an order different from that shown here.

[0035] In this embodiment, a method for detecting the angle of a trailer is provided. A first point cloud sensor is installed on the side of the front of the vehicle facing the trailer, and a reflector is installed on the position of the trailer facing the front of the vehicle. Figure 1 As shown, the process includes the following steps:

[0036] Step S101, scanning the reflector by a first point cloud sensor to generate a reflector point cloud plane corresponding to the reflector;

[0037] Step S102, determining the trailer offset angle according to the angle between the reflector point cloud plane and the forward direction of the vehicle head.

[0038] Specifically, the present invention provides a new method for detecting the offset angle of a trailer. In this embodiment, Figure 2 As shown, the detection device is composed of a first point cloud sensor installed on the back of the front of the vehicle and a reflector posted at the front position of the trailer, wherein the first point cloud sensor includes but is not limited to a multi-line laser radar, a millimeter wave radar, an ultrasonic radar, etc. The reflector is scanned by the first point cloud sensor to generate a reflector point cloud plane. When the vehicle turns and the trailer and the front of the vehicle are deflected, the reflector point cloud plane and the forward direction of the front of the vehicle will form an angle, so that the angle of the reflector point cloud plane relative to the forward direction of the front of the vehicle is geometrically deduced to determine the trailer offset angle relative to the forward direction of the front of the vehicle, thereby realizing the detection of the trailer offset angle. This solution solves the problem of unclear image detection, the problem of high maintenance cost of the angle encoder, and the problem that the reflective material is not easy to scan by radar scanning and the reflective material is easily blocked by mud, which significantly improves the detection accuracy of the trailer offset angle.

[0039] In some optional embodiments, Figure 2 As shown, a first reflector is installed on the front baffle of the trailer facing the front of the vehicle, and second reflectors are installed on the left and right sides near the trailer saddle. Step S101 includes:

[0040] Step a1: Scan the first reflector and the second reflector by using the first point cloud sensor to generate corresponding first reflector point cloud plane and second reflector point cloud plane respectively.

[0041] Step S102 includes:

[0042] Step a2, determining an initial offset angle of the trailer relative to the front of the vehicle according to an angle between the point cloud plane of the first reflector and the advancing direction of the front of the vehicle;

[0043] Step a3, determining whether the initial offset angle belongs to a small angle rotation situation;

[0044] Step a4: if the rotation is at a small angle, the initial offset angle is used as the trailer offset angle;

[0045] Step a5: if the rotation does not fall within a small angle, the auxiliary angle is determined according to the angle between the second reflector point cloud plane and the vehicle head forward direction;

[0046] Step a6, based on the vertical plane relationship between the first reflector point cloud plane and the second reflector point cloud plane, the trailer offset angle is calculated using the auxiliary angle.

[0047] Specifically, in the embodiment of the present invention, a first reflector is installed on the front baffle of the vehicle head, and a second reflector is installed on the left and right sides near the vehicle saddle, the first reflector is directly opposite to the first point cloud sensor, and the second reflector is at a 90-degree relationship with the first reflector. The detection of the trailer offset angle of the present invention is divided into two stages. In the first stage, the offset angle of the trailer relative to the vehicle head is mainly calculated based on the angle between the first reflector and the forward direction of the vehicle head. However, considering that when the offset angle of the trailer is too large, the first reflector can hardly be directly opposite to the first sensor, and the first reflector is scanned very narrowly, resulting in greater difficulty in measuring the angle between the first reflector and the forward direction of the vehicle head. Therefore, when the offset angle of the trailer is too large, the present invention calculates the offset angle of the trailer relative to the vehicle head through the second stage. In the second stage, the second reflector and the first point cloud sensor are closer to a directly opposite state, so it is easier to scan the second reflector and measure the angle between the second reflector and the forward direction of the vehicle head. Afterwards, based on the relationship that the second reflector and the first reflector are vertically arranged at 90 degrees, the angle between the first reflector and the forward direction of the vehicle head can be further inferred, and the offset angle of the trailer can be accurately calculated.

[0048] Based on this, the embodiment of the present invention first measures the initial offset angle of the trailer relative to the front of the vehicle through the first reflector. If the initial offset angle is a small angle, it is considered that the initial offset angle measurement is relatively accurate, and the initial offset angle can be directly used as the trailer offset angle; if the initial offset angle is too large and does not belong to the small angle rotation situation, the second reflector is continued to be scanned, and the angle between the second reflector point cloud plane and the forward direction of the vehicle head is calculated to obtain the auxiliary angle, and then the vertical plane relationship between the first reflector point cloud plane and the second reflector point cloud plane is used to continue to calculate the angle between the first reflector point cloud plane and the forward direction of the vehicle head through the auxiliary angle, and then the trailer offset angle is further calculated. The technical solution provided by the embodiment of the present invention can significantly improve the detection accuracy of the trailer offset angle.

[0049] In some optional implementations, the above step a3 includes:

[0050] Step a31, determining whether a first angle change value is less than a first preset angle threshold, the first angle change value being the difference between an initial offset angle calculated in the current calculation cycle and a relative front angle of the trailer in a straight-ahead state;

[0051] Step a32, determining whether a second angle change value is less than a second preset angle threshold, the second angle change value being the difference between an initial offset angle calculated in a current calculation cycle and a trailer offset angle calculated in a previous calculation cycle;

[0052] Step a33, when the first angle change value calculated in the current calculation cycle is less than the first preset angle threshold, and the second angle change value is less than the second preset angle threshold, it is determined to be a small angle rotation situation, otherwise it is not a small angle rotation situation.

[0053] Specifically, Figure 3 As shown, the embodiment of the present invention determines whether the trailer deflection angle belongs to a small-angle rotation situation mainly through two conditions. First, the difference between the initial offset angle of the current calculation cycle and the trailer offset angle calculated in the previous calculation cycle is calculated to obtain a second angle change value. The trailer deflection angle needs to be detected once every period of time, so the detection time is a calculation cycle. The embodiment of the present invention determines whether the second angle change value is less than the second preset angle threshold. The principle of this judgment condition is to prevent the situation where the vehicle turns at a fast speed and cannot accurately judge whether it is a small-angle scene or a large-angle scene due to false detection. If the second angle change value is less than the second preset angle threshold, it means that the current situation may belong to a small-angle rotation situation. After that, another condition is required for verification.

[0054] Another judgment condition is to first measure the angle A of the trailer relative to the front of the vehicle in the straight-ahead state (there will also be a small angle between the trailer and the front of the vehicle in the straight-ahead state, which is generally not 0 degrees), and then calculate the difference between the initial offset angle calculated in the current calculation cycle and the angle A to obtain a first angle change value, which is used to measure the degree of deflection of the current trailer relative to the straight-ahead state. Determine whether the first angle change value is less than the first preset angle threshold. If this judgment condition is also met, the current scene belongs to a small angle rotation situation. When one of the two judgment conditions is not met, it belongs to a large angle rotation situation. The solution provided by the embodiment of the present invention selects to use the first reflector point cloud or the second reflector point cloud plane to measure the trailer offset angle, which significantly improves the accuracy of identifying large and small angle rotation situations, thereby improving the accuracy of the trailer offset angle measurement. In addition, in this embodiment, the specific size of the first preset angle threshold and the second preset angle threshold can be defined based on the actual situation of the vehicle, and the embodiment of the present invention does not make specific limitations.

[0055] In some optional implementations, step a1 includes:

[0056] Step b1, scanning the current reflector to obtain the initial point cloud of the current reflector;

[0057] Step b2, extracting the point cloud of the area of ​​interest from the initial point cloud of the current reflector based on the straight-through filtering;

[0058] Step b3, processing the intercepted point cloud of the region of interest based on voxel filtering;

[0059] Step b4, obtaining the normal vector of each point from the processed point cloud of the region of interest;

[0060] Step b5, if the current reflector is the first reflector, then retain the points where the angle between the normal vector and the vehicle head direction is less than the third preset angle threshold; if the current reflector is the second reflector, then remove the points where the angle between the normal vector and the vehicle head direction is less than the third preset angle threshold;

[0061] Step b6, clustering the point cloud of the region of interest after normal vector processing;

[0062] Step b7, performing plane fitting on the clustered point cloud of the region of interest based on a plane fitting method, and using the fitted plane as the current reflector point cloud plane.

[0063] Specifically, Figure 4 As shown, the embodiment of the present invention performs multiple preprocessing when scanning the reflector point cloud. Among them, firstly, the scanned initial point cloud of the current reflector is subjected to straight-through filtering to intercept the point cloud of the area of ​​interest that can better represent the reflector area from the initial point cloud of the current reflector, and filter the point cloud of the irrelevant area.

[0064] Afterwards, the intercepted point cloud of the region of interest is processed based on voxel filtering to reduce the amount of calculation in subsequent steps. Then, the normal vector of each point is obtained from the processed point cloud of the region of interest. For the first reflector, the normal vector of the first reflector should be parallel to the forward direction of the vehicle head in the straight-ahead state. Even if the trailer deviates, the angle will not be too large. Therefore, it is necessary to retain the points whose angles between the normal vector and the forward direction of the vehicle head are less than the third preset angle threshold. For the points whose angles between the normal vector of the first reflector and the forward direction of the vehicle head are greater than the third preset angle threshold, they are considered to be noise in the point cloud of the first reflector and should be removed. For the second reflector, because the second reflector and the first reflector are 90 degrees, the normal vector of the second reflector should be perpendicular to the forward direction of the vehicle head in the straight-ahead state. Even if the trailer deviates, the angle will not be too small. Therefore, for the points whose angles between the normal vector of the second reflector and the forward direction of the vehicle head are greater than the third preset angle threshold, they should be retained. For the points whose angles between the normal vector of the second reflector and the forward direction of the vehicle head are less than the third preset angle threshold, they are considered to be noise and should be removed.

[0065] Afterwards, the filtered point cloud of the region of interest is clustered, and the cluster with the largest number of points is selected as the current point cloud of the reflector to further avoid abnormal situations and improve the stability of the algorithm. Then, the plane fitting is performed on the clustered point cloud of the region of interest based on the plane fitting method, and the fitting method includes but is not limited to least squares fitting, vector feature calculation, etc. Finally, the fitted plane is used as the current reflector point cloud plane, which significantly improves the accuracy and stability of the reflector point cloud plane.

[0066] In some optional implementations, the above step b7 includes:

[0067] Step c1, using a random sampling consistency algorithm to perform plane fitting on the clustered point cloud of the region of interest to obtain the plane equation of the reflector and the number of outliers;

[0068] Step c2, determining whether the number of outlier points is greater than a preset point threshold;

[0069] Step c3, if the number of outlier points is greater than the preset point number threshold, the outlier points are filtered, and the process returns to the step of plane fitting the clustered point cloud of the region of interest using a random sampling consensus algorithm;

[0070] Step c4, if the number of outlier points is not greater than the preset point threshold, determine whether it is greater than the preset number of iterations;

[0071] Step c5: if the number of iterations is greater than a preset number, the reflector point cloud plane is determined based on the current reflector plane equation;

[0072] Step c6: If the number of iterations is not greater than the preset number, return to the step of using the random sampling consistency algorithm to perform plane fitting on the clustered point cloud of the region of interest.

[0073] Specifically, Figure 5 As shown, in the process of plane fitting of the reflector point cloud to obtain the reflector point cloud plane, the embodiment of the present invention uses the Random Sample Consensus (RANSAC) algorithm and optimizes it during the fitting process. The reflector point cloud is optimized several times according to the preset number of iterations. In each optimization process, the plane fitting of the clustered area of ​​interest point cloud is performed using the random sampling consensus algorithm (the random sampling consensus algorithm is a prior art, and the specific principle is not repeated here), and the reflector plane equation and the number of outliers are obtained. If the number of outliers is too large and greater than the preset point threshold, the next iteration is directly entered, and the outliers are removed and the remaining points are used to re-fit the plane through the random sampling consensus algorithm. If the number of outliers is not greater than the preset point threshold, it is determined whether the preset number of iterations is reached. If the preset number of iterations is reached, it is stopped, otherwise it continues to refit. Finally, a more accurate reflector point cloud plane is obtained through multiple fittings of the random sampling consensus algorithm and outlier filtering. It should be noted that the preset point threshold and the preset number of iterations defined in the embodiment of the present invention can be flexibly set according to user needs, and the present invention does not make special restrictions on specific values.

[0074] In some optional implementations, the trailer angle detection method provided by the present invention further includes:

[0075] Step d1, collecting environment point cloud;

[0076] Step d2, generating a vehicle body filter frame according to the geometric parameters of the vehicle and the trailer offset angle;

[0077] Step d3, using the vehicle body filter frame to filter the environment point cloud, and remove the part of the environment point cloud that belongs to the vehicle body.

[0078] Specifically, the autonomous driving of container trucks often requires the use of radar to collect point cloud data of the external environment, so as to navigate and avoid obstacles through the environmental point cloud. Because when the container truck turns, there will be a certain angle between the front of the truck and the trailer, that is, the trailer offset angle, which will cause the shape of the container truck to be irregular. When the radar equipment on the container truck collects the environmental point cloud data, it is very likely to scan the body of the truck as well, resulting in inaccurate environmental point cloud data collection, which will cause problems for navigation and obstacle avoidance. Based on the above problems, such as Figure 6As shown, an embodiment of the present invention also provides a new method for collecting environmental point clouds. First, the environmental point cloud is collected by a radar on the vehicle body for collecting environmental information. Then, a vehicle body filter frame is generated according to geometric parameters such as the length, width, and height of the vehicle and the offset angle of the trailer. The vehicle body filter frame represents the outline of the vehicle. Then, the environmental point cloud is filtered using the vehicle body filter frame to remove the part of the environmental point cloud belonging to the vehicle body, and the remaining part is retained as the final environmental point cloud, which can significantly improve the accuracy of the environmental point cloud.

[0079] In some optional implementations, the environmental point cloud is collected by a second point cloud sensor installed on the front of the vehicle, and the above step d2 includes:

[0080] Step e1, mapping the trailer offset angle under the detection coordinates of the first point cloud sensor to the detection coordinates of the second point cloud sensor according to the timestamp difference between the first point cloud sensor and the second point cloud sensor, to obtain a mapped offset angle;

[0081] Step e2, generating a vehicle body filter frame based on the vehicle's geometric parameters and the mapping offset angle.

[0082] Specifically, Figure 2 As shown, the embodiment of the present invention can collect environmental point clouds through the second point cloud sensors installed on the left and right sides of the front of the vehicle. Because the second point cloud sensor and the first point cloud sensor are not the same device, there must be a time difference between the second point cloud sensor and the first point cloud sensor. If the body filter frame is calculated using the angle measured by the first point cloud sensor, and the body filter frame is directly placed in the environmental point cloud collected by the second point cloud sensor for filtering, there will be a problem of inaccurate point cloud filtering. This is because the time difference between the two sensors causes the body filter frame calculated by the first point cloud sensor to be offset in the data of the first point cloud sensor. Based on this, before using the body filter frame to filter the environmental point cloud, the embodiment of the present invention first maps the trailer offset angle under the detection coordinates of the first point cloud sensor to the detection coordinates of the second point cloud sensor according to the timestamp difference between the first point cloud sensor and the second point cloud sensor, and obtains the real angle between the trailer and the front of the vehicle in the second point cloud sensor after the time offset, that is, the mapping offset angle. Then, the body filter frame is calculated using the mapping offset angle and the geometric parameters of the body, thereby ensuring that the body filter frame overcomes the problem of inconsistent timestamps between the two sensors, thereby significantly improving the accuracy of subsequent environmental point cloud filtering.

[0083] In some optional implementations, the above step e1 includes:

[0084] Step f1, calculating the angle difference between the trailer offset angle detected by the first point cloud sensor in the current calculation cycle and the trailer offset angle detected in the previous calculation cycle;

[0085] Step f2, calculating a first timestamp difference between a current calculation cycle timestamp and a previous calculation cycle timestamp of the first point cloud sensor;

[0086] Step f3, calculating the trailer deviation angular velocity in the first point cloud sensor by using the angle difference and the first timestamp difference;

[0087] Step f4, calculating a second timestamp difference between the timestamp of the first point cloud sensor in the current calculation cycle and the timestamp of the second point cloud sensor in the current calculation cycle;

[0088] Step f5, calculating the mapping compensation angle by multiplying the trailer offset angular velocity and the second timestamp difference;

[0089] Step f6, calculating the difference between the mapping compensation angle and the trailer offset angle detected by the first point cloud sensor in the current calculation cycle, and determining the corresponding mapping offset angle under the detection coordinates of the second point cloud sensor.

[0090] Specifically, Figure 7 As shown, the embodiment of the present invention obtains the trailer offset angle C detected by the first point cloud sensor in the current calculation cycle, and obtains the trailer offset angle D detected by the first point cloud sensor in the previous calculation cycle, and then calculates the angle difference F=(CD) between the current cycle and the previous cycle. It represents the change in the trailer offset angle between the current cycle and the previous cycle. After that, the first timestamp difference backLidarTimeDiff between the current calculation cycle timestamp and the previous calculation cycle timestamp of the first point cloud sensor is calculated. The ratio obtained by dividing the angle difference F by the first timestamp difference backLidarTimeDiff represents the change speed of the trailer offset angle detected by the first point cloud sensor, that is, the trailer offset angular velocity. Then, the timestamp of the first point cloud sensor in the current calculation cycle and the timestamp of the second point cloud sensor in the current calculation cycle are obtained respectively, and the difference between the two is calculated to represent the time difference between the first point cloud sensor and the second point cloud sensor, that is, the second timestamp difference timeDiffOf2Lidar. Afterwards, the trailer offset angular velocity in the first point cloud sensor is multiplied by the second timestamp difference timeDiffOf2Lidar to determine the change in the trailer offset angle in the first point cloud sensor within the second timestamp difference timeDiffOf2Lidar, thereby obtaining the mapping compensation angle H.

[0091] H=F / backLidarTimeDiff*timeDiffOf2Lidar

[0092] By subtracting the mapping compensation angle H from the trailer offset angle C detected by the first point cloud sensor in the current calculation cycle, CH, the corresponding mapping offset angle under the detection coordinates of the second point cloud sensor can be determined.

[0093] Afterwards, the mapping offset angle is smoothed by filtering methods including but not limited to mean filtering, weighted averaging, Kalman filtering, etc., to further ensure the accuracy of the mapping offset angle. In this embodiment, combined with the advantage of Kalman filtering being more robust, the Kalman filtering method can be used preferentially. Through the technical solution provided by the embodiment of the present invention, the timestamp alignment of the first point cloud sensor and the second point cloud sensor is finally completed, significantly improving the accuracy of subsequent point cloud filtering.

[0094] The technical solution provided by the embodiment of the present invention has the following advantages:

[0095] 1. Laser radar is used to detect and calculate the trailer angle, which is less affected by lighting conditions and can work normally during the day when the light intensity is high and at night when the light intensity is low. It is suitable for operational needs in the field of autonomous driving.

[0096] 2. By adopting a differential calculation algorithm for the detection of small-angle and large-angle turns, and realizing automatic detection of turning angle scenes, the algorithm can adapt to high-precision detection of rotation angles in different rotation scenes, and realize adaptive high-precision detection of the entire scene.

[0097] 3. By adopting kinematic modeling, the detection results are time-stamped and registered for synchronous use between different radars, which can effectively solve the error problem caused by the time asynchrony of detection results between multiple sensors when used in devices without hard synchronization of timestamps.

[0098] 4. The iterative method used is to detect the point cloud plane and fit the plane equation, so as to effectively filter out noise points and abnormal points, thereby achieving high-precision fitting of the point cloud plane.

[0099] In this embodiment, a trailer angle detection device is also provided, which is used to implement the above-mentioned embodiments and preferred implementation modes, and the descriptions that have been made will not be repeated. As used below, the term "module" can be a combination of software and / or hardware that implements a predetermined function. Although the device described in the following embodiments is preferably implemented in software, the implementation of hardware, or a combination of software and hardware, is also possible and conceivable.

[0100] This embodiment provides a trailer angle detection device, wherein a first point cloud sensor is installed on the side of the front of the vehicle facing the trailer, and a reflector is installed on the position of the trailer facing the front of the vehicle. Figure 8 As shown, including:

[0101] The reflector scanning module 801 is used to scan the reflector through the first point cloud sensor to generate a reflector point cloud plane corresponding to the reflector;

[0102] The angle calculation module 802 is used to determine the trailer offset angle according to the angle between the reflector point cloud plane and the forward direction of the vehicle head.

[0103] In some optional embodiments, a first reflector is installed on the front baffle of the trailer facing the front of the vehicle, and second reflectors are installed on the left and right sides near the trailer saddle. The reflector scanning module 801 includes:

[0104] The scanning unit is used to scan the first reflector and the second reflector through the first point cloud sensor to generate the corresponding first reflector point cloud plane and second reflector point cloud plane respectively.

[0105] In some optional implementations, the angle calculation module 802 includes:

[0106] An initial offset angle calculation unit, used to determine an initial offset angle of the trailer relative to the front of the vehicle according to an angle between the point cloud plane of the first reflector and the advancing direction of the front of the vehicle;

[0107] A scene analysis unit, used to determine whether the initial offset angle belongs to a small angle rotation situation;

[0108] A first angle output unit, for taking the initial offset angle as the trailer offset angle if the rotation angle is small;

[0109] The auxiliary analysis unit is used to determine the auxiliary angle according to the angle between the point cloud plane of the second reflector and the forward direction of the vehicle head if the situation does not belong to the small angle rotation situation;

[0110] The second angle output unit is used to calculate the trailer offset angle using the auxiliary angle based on the vertical plane relationship between the first reflector point cloud plane and the second reflector point cloud plane.

[0111] In some optional implementations, the scene analysis unit includes:

[0112] A first threshold judgment unit, used to judge whether a first angle change value is less than a first preset angle threshold value, the first angle change value being a difference between an initial offset angle calculated in a current calculation cycle and a relative front angle of the trailer in a straight-ahead state;

[0113] A second threshold judgment unit, used to judge whether a second angle change value is less than a second preset angle threshold, the second angle change value being a difference between an initial offset angle calculated in a current calculation cycle and a trailer offset angle calculated in a previous calculation cycle;

[0114] The scene determination unit is used to determine that it belongs to a small angle rotation situation when the first angle change value calculated in the current calculation cycle is less than the first preset angle threshold and the second angle change value is less than the second preset angle threshold, otherwise it does not belong to a small angle rotation situation.

[0115] In some optional embodiments, the scanning unit comprises:

[0116] An initial scanning unit is used to scan the current reflector to obtain an initial point cloud of the current reflector;

[0117] A through filtering unit, used for intercepting a point cloud of an area of ​​interest from an initial point cloud of a current reflector based on through filtering;

[0118] A voxel filtering unit, used for processing the intercepted point cloud of the region of interest based on voxel filtering;

[0119] A normal vector calculation unit, used for obtaining a normal vector of each point from the processed point cloud of the region of interest;

[0120] a normal vector filtering unit, for retaining points where the angle between the normal vector and the vehicle head direction is less than a third preset angle threshold if the current reflector is the first reflector, and for removing points where the angle between the normal vector and the vehicle head direction is less than the third preset angle threshold if the current reflector is the second reflector;

[0121] A clustering unit, used for clustering the point cloud of the region of interest after normal vector processing;

[0122] The plane collection unit is used to perform plane fitting on the clustered point cloud of the region of interest based on a plane fitting method, and use the fitted plane as the current reflector point cloud plane.

[0123] In some optional implementations, the plane set unit includes:

[0124] The first fitting unit is used to perform plane fitting on the point cloud of the region of interest after clustering by using a random sampling consistency algorithm to obtain a plane equation of the reflector and the number of outlier points;

[0125] An outlier analysis unit, used to determine whether the number of outlier points is greater than a preset point threshold;

[0126] A filtering unit, for filtering the outlier points if the number of outlier points is greater than a preset point number threshold, and returning to the step of plane fitting the clustered point cloud of the region of interest using a random sampling consensus algorithm;

[0127] An iteration judgment unit, used to judge whether the number of outlier points is greater than a preset number of iterations if the number of outlier points is not greater than a preset point threshold;

[0128] A plane output unit, used for determining the reflector point cloud plane based on the current reflector plane equation if the number of iterations is greater than a preset number;

[0129] The return unit is used to return to the step of plane fitting the clustered region of interest point cloud using a random sampling consensus algorithm if the number of iterations is not greater than a preset number.

[0130] In some optional embodiments, the device further comprises:

[0131] Environmental point cloud acquisition module, used to collect environmental point clouds;

[0132] The filter frame calculation module generates the body filter frame according to the vehicle's geometric parameters and the trailer offset angle;

[0133] The filtering module uses the body filter frame to filter the environment point cloud and remove the part of the environment point cloud that belongs to the body.

[0134] The further functional description of each of the above modules and units is the same as that of the above corresponding embodiments and will not be repeated here.

[0135] The trailer angle detection device in this embodiment is presented in the form of a functional unit, where the unit refers to an ASIC (Application Specific Integrated Circuit) circuit, a processor and memory that executes one or more software or fixed programs, and / or other devices that can provide the above functions.

[0136] The embodiment of the present invention further provides a vehicle, such as Figure 2 As shown, it includes a vehicle head, a trailer and a controller, a first point cloud sensor is installed on the side of the vehicle head facing the trailer, a reflector is installed on the position of the trailer facing the vehicle head, and a second point cloud sensor is also installed on the vehicle head; the first point cloud sensor is used to detect the trailer offset angle relative to the vehicle head; the second point cloud sensor is used to collect environmental point clouds; the controller is installed inside the vehicle head, and is used to execute computer instructions, thereby executing the method of the aforementioned method embodiment.

[0137] The controller may be a central processing unit, a network processor or a combination thereof. The controller may further include a hardware chip. The hardware chip may be a dedicated integrated circuit, a programmable logic device or a combination thereof. The programmable logic device may be a complex programmable logic device, a field programmable gate array, a general purpose array logic or any combination thereof.

[0138] The embodiment of the present invention also provides a computer-readable storage medium. The method according to the embodiment of the present invention can be implemented in hardware, firmware, or can be implemented as a computer code that can be recorded in a storage medium, or can be implemented as a computer code that is originally stored in a remote storage medium or a non-temporary machine-readable storage medium and will be stored in a local storage medium through a network download, so that the method described herein can be stored in such software processing on a storage medium using a general-purpose computer, a dedicated processor, or programmable or dedicated hardware. Among them, the storage medium can be a magnetic disk, an optical disk, a read-only storage memory, a random access memory, a flash memory, a hard disk or a solid-state hard disk, etc.; further, the storage medium can also include a combination of the above types of memories. It can be understood that a computer, a processor, a microprocessor controller, or programmable hardware includes a storage component that can store or receive software or computer code. When the software or computer code is accessed and executed by a computer, a processor, or hardware, the method shown in the above embodiment is implemented.

[0139] A part of the present invention may be applied as a computer program product, such as a computer program instruction, which, when executed by a computer, can call or provide the method and / or technical solution according to the present invention through the operation of the computer. Those skilled in the art should understand that the existence of the computer program instruction in a computer-readable medium includes, but is not limited to, a source file, an executable file, an installation package file, etc., and accordingly, the way in which the computer program instruction is executed by the computer includes, but is not limited to: the computer directly executes the instruction, or the computer compiles the instruction and then executes the corresponding compiled program, or the computer reads and executes the instruction, or the computer reads and installs the instruction and then executes the corresponding installed program. Here, the computer-readable medium may be any available computer-readable storage medium or communication medium accessible to the computer.

[0140] Although the embodiments of the present invention have been described in conjunction with the accompanying drawings, those skilled in the art may make various modifications and variations without departing from the spirit and scope of the present invention, and such modifications and variations are all within the scope defined by the appended claims.

Claims

1. A method for detecting a trailer angle, characterized in that: A first point cloud sensor is installed on a side of the front of the vehicle facing the trailer, and a reflector is installed at a position of the trailer facing the front of the vehicle. The method includes: Scanning the reflector by the first point cloud sensor to generate a reflector point cloud plane corresponding to the reflector; The trailer offset angle is determined according to the angle between the reflector point cloud plane and the forward direction of the vehicle head.

2. The method according to claim 1, characterized in that A first reflector is installed on the front baffle of the trailer facing the front of the vehicle, and second reflectors are installed on the left and right sides near the trailer saddle. The reflector point cloud corresponding to the reflector is generated by scanning the reflector with the first point cloud sensor, including: The first reflector plate and the second reflector plate are scanned by the first point cloud sensor to generate corresponding first reflector plate point cloud plane and second reflector plate point cloud plane respectively.

3. The method according to claim 2, characterized in that Determining the trailer offset angle according to the angle between the reflector point cloud plane and the front moving direction of the vehicle includes: Determining an initial offset angle of the trailer relative to the front of the vehicle according to an angle between the point cloud plane of the first reflector and the advancing direction of the front of the vehicle; Determine whether the initial offset angle belongs to a small angle rotation situation; If it is a small angle rotation case, the initial offset angle is used as the trailer offset angle; If it is not a small angle rotation case, the auxiliary angle is determined according to the angle between the second reflector point cloud plane and the forward direction of the vehicle head; The trailer offset angle is calculated using the auxiliary angle based on a vertical plane relationship between the first reflector point cloud plane and the second reflector point cloud plane.

4. The method according to claim 3, characterized in that The determining whether the initial trailer offset angle is a small angle rotation situation includes: Determine whether a first angle change value is less than a first preset angle threshold, wherein the first angle change value is a difference between an initial offset angle calculated in a current calculation cycle and a relative front angle of the trailer in a straight-ahead state; Determining whether a second angle change value is less than a second preset angle threshold, wherein the second angle change value is a difference between an initial offset angle calculated in a current calculation cycle and a trailer offset angle calculated in a previous calculation cycle; When the first angle change value calculated in the current calculation cycle is less than the first preset angle threshold, and the second angle change value is less than the second preset angle threshold, it is determined to belong to the small angle rotation situation, otherwise it does not belong to the small angle rotation situation.

5. The method according to claim 2, characterized in that: The method of scanning the first reflector and the second reflector by the first point cloud sensor to generate corresponding first reflector point cloud plane and second reflector point cloud plane respectively includes: Scan the current reflector to obtain the initial point cloud of the current reflector; Extracting a point cloud of an area of ​​interest from the initial point cloud of the current reflector based on through filtering; Processing the intercepted point cloud of the region of interest based on voxel filtering; Get the normal vector of each point from the processed point cloud of the region of interest; If the current reflector is the first reflector, the points where the angle between the normal vector and the vehicle head direction is less than the third preset angle threshold are retained; if the current reflector is the second reflector, the points where the angle between the normal vector and the vehicle head direction is less than the third preset angle threshold are removed; Perform clustering on the point cloud of the region of interest after normal vector processing; Based on the plane fitting method, plane fitting is performed on the clustered point cloud of the region of interest, and the fitted plane is used as the current reflector point cloud plane.

6. The method according to claim 5, characterized in that The plane fitting method is used to perform plane fitting on the clustered point cloud of the region of interest, and the fitted plane is used as the current reflector point cloud plane, including: A random sampling consistency algorithm is used to perform plane fitting on the clustered point cloud of the region of interest to obtain a plane equation of the reflector and the number of outliers; Determine whether the number of outlier points is greater than a preset point threshold; If the number of outlier points is greater than the preset point number threshold, the outlier points are filtered, and the process of using the random sampling consensus algorithm to perform plane fitting on the clustered region of interest point cloud is returned; If the number of outlier points is not greater than the preset point threshold, determining whether it is greater than a preset number of iterations; If it is greater than the preset number of iterations, determining the reflector point cloud plane based on the current reflector plane equation; If it is not greater than the preset number of iterations, the process returns to the step of performing plane fitting on the clustered point cloud of the region of interest using a random sampling consistency algorithm.

7. The method according to any one of claims 1 to 6, characterized in that: The method further comprises: Collect environmental point cloud; generating a vehicle body filter frame according to the geometric parameters of the vehicle and the trailer offset angle; The vehicle body filtering frame is used to filter the environment point cloud to remove the part of the environment point cloud that belongs to the vehicle body.

8. The method according to claim 7, characterized in that The environmental point cloud is collected by a second point cloud sensor installed on the front of the vehicle, and the vehicle body filter frame is generated according to the geometric parameters of the vehicle and the offset angle of the trailer, including: According to the timestamp difference between the first point cloud sensor and the second point cloud sensor, mapping the trailer offset angle under the detection coordinates of the first point cloud sensor to the detection coordinates of the second point cloud sensor to obtain a mapped offset angle; A vehicle body filter frame is generated based on the geometric parameters of the vehicle and the mapping offset angle.

9. The method according to claim 8, characterized in that The method of mapping the trailer offset angle under the detection coordinates of the first point cloud sensor to the detection coordinates of the second point cloud sensor according to the timestamp difference between the first point cloud sensor and the second point cloud sensor to obtain the mapped offset angle includes: Calculate the angle difference between the trailer offset angle detected by the first point cloud sensor in the current calculation cycle and the trailer offset angle detected in the previous calculation cycle; Calculate a first timestamp difference between a current calculation cycle timestamp and a previous calculation cycle timestamp of a first point cloud sensor; Calculating the trailer deviation angular velocity in the first point cloud sensor by using the angle difference and the first timestamp difference; Calculate a second timestamp difference between the timestamp of the first point cloud sensor in the current calculation cycle and the timestamp of the second point cloud sensor in the current calculation cycle; Calculating a mapping compensation angle by multiplying the trailer offset angular velocity by the second timestamp difference; The difference between the mapping compensation angle and the trailer offset angle detected by the first point cloud sensor in the current calculation cycle is calculated to determine the corresponding mapping offset angle under the detection coordinates of the second point cloud sensor.

10. A trailer angle detection device, characterized in that: A first point cloud sensor is installed on a side of the front of the vehicle facing the trailer, and a reflector is installed at a position of the trailer facing the front of the vehicle, and the device includes: A reflector scanning module, used for scanning the reflector through the first point cloud sensor to generate a reflector point cloud plane corresponding to the reflector; The angle calculation module is used to determine the trailer offset angle according to the angle between the reflector point cloud plane and the forward direction of the vehicle head.

11. A vehicle, characterized in that: It includes a vehicle head, a trailer and a controller, wherein a first point cloud sensor is installed on a side of the vehicle head facing the trailer, a reflector is installed at a position where the trailer faces the vehicle head, and a second point cloud sensor is also installed on the vehicle head; the first point cloud sensor is used to detect the trailer offset angle relative to the vehicle head; the second point cloud sensor is used to collect environmental point clouds; the controller is installed inside the vehicle head and is used to execute computer instructions, thereby executing any one of claims 1 to 9.

12. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer instructions, and the computer instructions are used to enable a computer to execute the method according to any one of claims 1 to 9.

Images