A laser radar based trailer corner estimation method

Abstract

The present application relates to a kind of trailer corner estimation method based on laser radar, which needs to install multiple line laser radar in front of tractor, and install a piece of high reflection material at four corner blocks and two wheel arches of trailer respectively;Then multiple line laser radar scans high reflection material to collect laser point cloud data, and extracts high reflection point in laser point cloud as vehicle detection corner point, and the detected corner point is numbered;Finally, the numbered corner point is compared with trailer template to calculate the turning angle of trailer.The advantage of the present application is that it does not need to increase laser radar on the tractor to monitor the driving condition of the trailer, and only two main radars provided by the tractor can realize the estimation of the turning angle of the trailer, and the calculation method is simpler and more practical.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of automatic driving, and particularly relates to a trailer turning angle estimation method based on a laser radar. BACKGROUND

[0002] With the development and progress of science and technology, automatic driving trailers can improve work efficiency and reduce traffic accidents caused by driver fatigue and other human factors in trunk line logistics and port logistics. The trailer turning angle refers to the angle between the trailer and the towing vehicle at the hook shaft due to turning in the forward direction. The driving state of the automatic driving trailer includes lane changing, turning, parking, etc. For the automatic driving trailer in port logistics, only the correct detection of the turning angle between the trailer and the towing vehicle can accurately control the vehicle to realize parking in the warehouse. In addition, when the vehicle turns and changes lanes, the trailer turning angle needs to be detected to adjust the vehicle state to avoid scratches when the vehicle turns.

[0003] A Chinese patent with publication number CN115042797A discloses a trailer pose estimation method, device and equipment, and a storage medium. The method detects the trailer rotation angle by installing a laser radar at the rear end of the towing vehicle, and then calculates the trailer turning angle. This method needs to increase a special laser radar on the towing vehicle to detect the trailer rotation angle, which is high in cost and complicated in operation. A Chinese patent with publication number CN114661039A discloses a logistics vehicle and its trailer pose determination, pre-collision detection and automatic driving method. The method installs four laser radars, five ultrasonic radars and four binocular cameras on the trailer. The angle and speed of the trailer are input into the kinematic model to estimate the pose of the trailer by using Kalman filtering. The calculation method is complicated. A device for detecting the shaft deflection angle of an unmanned driving trailer and a detection method thereof are disclosed in CN108278981A. The method detects the shaft deflection angle generated when the position of the trailer bearing shaft changes, i.e. the angle of deflection of the shaft position. SUMMARY

[0004] The present application aims to solve the problems existing in the prior art, and provides a trailer turning angle estimation method based on a laser radar. The method is mainly used to calculate the trailer turning angle when the automatic driving trailer is parking, turning and changing lanes, and then to realize accurate control of the trailer by the automatic driving vehicle.

[0005] To achieve the above purpose, the present application provides a trailer turning angle estimation method based on a laser radar, comprising the following steps:

[0006] Step 1: Install a multi-line laser radar in front of the towing vehicle, and install a piece of high-reflective material at each of the four corner blocks and two wheel arches of the trailer.

[0007] Step 2, the multi-line laser radar scans the high reflection material to collect laser point cloud data, and extracts the high reflection points in the laser point cloud as the corner points of vehicle detection, and the detected corner points are numbered;

[0008] Step 3, the numbered corner points are compared with the trailer template to calculate the trailer corner.

[0009] The present application adopts the multi-line laser radar to collect the cloud data of laser points, which can accurately detect the corner of the trailer when driving, is beneficial to real-time monitoring of the driving condition of the trailer, and realizes the functions of automatic parking and automatic driving.

[0010] The present application further adopts the following technical solutions:

[0011] In step 1, a multi-line laser radar is arranged on each side of the front of the towing vehicle.

[0012] Preferably, the multi-line laser radar is a 128-line laser radar.

[0013] In step 2, two multi-line laser radars are used to scan the automatic driving vehicle to collect the whole vehicle laser point cloud data, and the point cloud data collected by the two laser radars is synchronously processed.

[0014] In step 2, the high reflection points in the synchronously processed laser point cloud are extracted as corner points, and the corner points are numbered and recorded as P i , i =1,2,3,4,5,6; in different driving states of the vehicle, the corner point information that can be detected by the laser radar is different, when the vehicle is driving straight, the laser radar can detect six corner points at the left front corner block, the right front corner block, the left rear corner block, the right rear corner block, the left side wheel arch and the right side wheel arch, when the vehicle is changing lanes, the laser radar can detect four corner points at the left front corner block, the right front corner block, the right rear corner block and the right side wheel arch, and when the vehicle is making a right-angle turn or parking, the laser radar can detect three corner points at the left front corner block, the right rear corner block and the right side wheel arch.

[0015] The present application proposes a robust reflector extraction and numbering method, which can effectively improve the stability of the trailer high reflection point detection when high reflection interference occurs around the vehicle.

[0016] In step 2, the specific method for numbering the detected corner points includes:

[0017] (1) vehicle initialization to obtain an initialization corner, calculating the centers of six high reflection materials according to the initialization corner, then setting the template range of the high reflection material corner point with the high reflection material center coordinate as the center, and then numbering the template range of the six high reflection material corner points, respectively recorded as i , i =1,2,3,4,5,6;

[0018] (2) using multi-line laser radar to scan the vehicle to obtain laser point cloud data, and extracting high-reflective points in the laser point cloud data;

[0019] (3) screening the high-reflective points according to the template range of the high-reflective material corner point to determine whether the high-reflective points are in the template range of the high-reflective material corner point, if yes, numbering the high-reflective points according to the template range number of the high-reflective material corner point where the high-reflective points are located, otherwise discarding the high-reflective points.

[0020] In the step 3, the numbered corner points of the trailer are matched with the standard corner points in the trailer template, and then the trailer turning angle is calculated according to the following formula θ ,

[0021]

[0022] In the formula, is the coordinate of the laser point cloud data collected by the laser radar after being projected to the XY two-dimensional plane, wherein represents the high-reflective material serial number of the point cloud, represents the laser point serial number in the high-reflective material, is the number of laser points in the high-reflective material, is the standard corner point coordinate value of the trailer template, and =1, 2, 3, 4, 5, 6; is θ the corresponding rotation matrix.

[0023] The present application estimates the turning angle by detecting the reflector plates installed on the four corners of the trailer and the wheel arches, and then using the optimization algorithm (trailer rectangular fitting) to estimate the turning angle, which not only can resist the influence of noise, increase the angle estimation accuracy, but also can improve the detection stability and flexibility.

[0024] The present application has the advantages that it does not need to increase laser radar on the towing vehicle to monitor the driving condition of the trailer, but only needs the two main radars of the towing vehicle to realize the trailer turning angle estimation, the calculation method is simpler, and the practicality is stronger. BRIEF DESCRIPTION OF DRAWINGS

[0025] The present application will be further described below in combination with the drawings.

[0026] Figure 1 is a schematic diagram of the laser scanning range and corner points in the present application.

[0027] Figure 2 is a flow chart of the trailer turning angle estimation method of the present application.

[0028] Figure 3 is a schematic diagram of the corner block template range in the present application.

[0029] Figure 4Flow chart for numbering of trailer corner blocks in the present invention.

[0030] Figure 5 Principle diagram for trailer corner turning calculation method in the present invention.

[0031] Figure 6 Result schematic diagram for trailer pose estimation comparison in the present invention.

[0032] In the figure: 1.128 line laser radar, 2. corner point. DETAILED DESCRIPTION EMBODIMENT

[0033] This embodiment is based on the installation position of the 128 line laser radar in front of the towing vehicle and the shielding condition of the scanning range, and designs a trailer corner turning estimation method based on laser radar. The method is to install a piece of high-reflective material at the four corner blocks and two wheel arches of the trailer (wherein the corner blocks are installed at the four corners of the trailer body, and the wheel arch refers to the plated bright strip on the upper edge of the tire), and scan the high-reflective material through the 128 line laser radar 1 installed on both sides in front of the towing vehicle, to obtain laser point cloud data. Then, the laser point cloud data is processed to obtain the position coordinates of the six high-reflective materials of the trailer in the vehicle coordinate system, which is simply referred to as corner point 2 (see Figure 1 ). The laser point cloud data obtained by laser radar scanning is transmitted to an industrial computer. The industrial computer is installed with an algorithm module and a storage device. After the industrial computer receives external laser radar data information, the algorithm module in the industrial computer calculates the trailer turning angle, and the storage device is used to store one or more programs.

[0034] The trailer has different turning angles under different driving states of the vehicle, and the trailer corner points that can be detected by the front laser radar are also different. Laser scanning test is performed on the vehicle under different driving states, that is, the six high-reflective materials of the trailer are scanned through the laser radar in front of the towing vehicle. According to the test results, it is found that the corner point detection is more difficult when the vehicle is with a box. Table 1 shows all the bright and dark states of the corner point under different driving states when the vehicle is with a box. The black point indicates that the corner point is in the bright state, and the laser radar can collect the information of the corner point.

[0035] Table 1: Trailer corner point condition

[0036]

[0037] As can be seen from Table 1, at least two corner block corner points and / or one side wheel arch corner point of the vehicle can be stably observed during driving, which meets the condition for turning angle estimation.

[0038] The embodiment first extracts high-reflective points in the laser radar point cloud to obtain six corner point information of the trailer when calculating the trailer corner, then numbers the detected six corner points of the trailer according to rules, and records the six corner points of the trailer as P1, P2, P3, P4, P5 and P6 respectively. The numbered corner points are matched with the template corner points, and then the trailer corner is calculated.

[0039] The embodiment is a trailer corner estimation method based on a laser radar, and the specific steps are as shown in Figure 2

[0040] Step 1: One 128-line laser radar 1 is installed on the left and right sides of the tractor head respectively to ensure that the high-reflective points at the rear of the trailer can be monitored. A high-reflective material is installed at each of the four corner blocks and two wheel arches of the trailer, wherein a reflective sticker is installed at each of the four corner blocks, and a reflective plate material is installed above the two wheel arches.

[0041] Step 2: Two 128-line laser radars 1 respectively scan the vehicle to collect the laser point cloud data of the whole vehicle, and the point clouds collected by the two laser radars are synchronously processed. Here, the two laser radars with adjacent time stamps are taken as synchronous laser point clouds. Then, the high-reflective points in the laser point cloud are extracted as vehicle detection corner points 2 to realize the detection of the six corner points of the trailer. Finally, the detected corner points 2 are numbered, and the six corner points are recorded as P i , i = 1, 2, 3, 4, 5, 6, wherein P1 is the left front corner block corner point of the trailer, P2 is the right front corner block corner point of the trailer, P3 is the right rear corner block corner point of the trailer, P4 is the left rear corner block corner point of the trailer, P5 is the right side wheel arch corner point, and P6 is the left side wheel arch corner point. When the vehicle is in different states, the corner block detected by the laser radar is not consistent due to the shielding of the trailer compartment. Generally, when the vehicle is in a straight driving state, the laser radar can detect four corner block corner points and left and right wheel arch corner points. When the vehicle is changing lanes, the laser radar can detect three corner block corner points and the right side wheel arch corner point P5 except P4 and P6. When the vehicle is making a right-angle turn or parking, the laser radar can only detect two corner block corner points P1 and P3 and the right side wheel arch corner point P5. The corner point conditions of the vehicle at different trailer corners are shown in Table 2.

[0042] Table 2 Trailer corner point numbering

[0043]

[0044] When calculating the trailer corner, it is necessary to ensure that the numbering of the trailer corner block is correct. In view of the detection effect of the above trailer high-reflective material at different angles, the numbering method of the trailer high-reflective material is introduced here.

[0045] ​To ensure the correct numbering of high-reflective material point cloud and avoid the interference of high-reflective points outside the vehicle on the estimation of the turning angle, the high-reflective material range of the trailer is needed to be determined, and the high-reflective points are screened according to the range, and the high-reflective points are numbered according to the range. First, the range of each high-reflective material template is determined Figure 3 . Since the trailer is a rigid body, the distance between the six high-reflective materials and the saddle position at the origin is fixed, and only the six high-reflective materials and the saddle hook have rotational motion during the movement of the trailer. Therefore, after the initial turning angle is obtained, the center positions of the six high-reflective materials can be calculated according to the initial angle. According to the coordinates of the center positions of the high-reflective materials, the range of each high-reflective material template is set, and the high-reflective points are screened in the range, and each high-reflective point is numbered according to the numbering of the high-reflective material template. In this way, the high-reflective point cloud at the high-reflective material of the trailer is obtained. The specific process is shown in Figure 4 , which includes:

[0046] (1) Vehicle initialization, obtain the trailer initialization turning angle θ . The initialization is obtained when the trailer starts, and generally the trailer turning angle θ is small or directly zero. According to the trailer initialization turning angle, the corresponding initialization rotation matrix is calculated, the high-reflective material angle point template is multiplied by the rotation matrix, and the center positions of the six high-reflective material angle points at this time are obtained. According to the coordinates of the center positions of the high-reflective material angle points in the vehicle coordinate system, the template range of each high-reflective material angle point is set, as shown in Figure 3 , the range of 0.5 meters before and after and left and right of the center of the high-reflective material angle point is set as the template range of the high-reflective material angle point, and then the template ranges of the six high-reflective material angle points are numbered, respectively denoted as , =1, 2, 3, 4, 5, 6;

[0047] (2) Scan the vehicle with two laser radars to obtain laser point cloud data, and extract the high-reflective points in the laser point cloud data.

[0048] (3) Screen the high-reflective points according to the template range of the high-reflective material angle point, that is, judge whether the high-reflective point is in the template range of the high-reflective material angle point. If the high-reflective point is located in the template range of a numbered high-reflective material angle point, it is judged that the high-reflective point is a useful point, and the high-reflective point is numbered according to the template range number of the high-reflective material angle point, otherwise it is judged that the high-reflective point is a useless point and can be discarded.

[0049] Step 3, compare the numbered angle point 2 with the trailer template, and calculate the trailer turning angle.

[0050] When calculating the trailer turning angle, the numbered angle point of the trailer is matched with the trailer template, and the relative rotation angle between the two is calculated, which is the trailer turning angle. The trailer turning angle is shown in Figure 5 .

[0051] The trailer template refers to the laser point cloud data obtained by scanning the trailer rear in the straight-ahead state of the vehicle (i.e., when the trailer angle is 0), located in front of the towing vehicle, and the high-reflective points in the high-reflective material angle point template range in the laser point cloud data are extracted as standard angle points, the standard angle points are denoted as , and =1, 2, 3, 4, 5, 6.

[0052] The trailer angle is calculated based on the theoretical value of the zero-turn trailer angle point, and the specific calculation method is as follows:

[0053] Input:

[0054] = { , ,…, },with

[0055] = { , ,…, },with

[0056] = { , ,…, },with

[0057] = { , ,…, },with

[0058] = { , ,…, },with

[0059] = { , ,…, with

[0060] Optimization:

[0061]

[0062] Output: θ

[0063] In the formula, C1, C2, C3, C4, C5 and C6 are all theoretical coordinate values when the trailer angle is 0, P1, P2, P3, P4, P5 and P6 are all high reflection point sets actually extracted in the trailer movement process, and output θ is the trailer rotation angle result to be solved. is the coordinate of the laser point cloud data projected to the XY two-dimensional plane collected by the laser radar, wherein represents the high reflection material serial number of the point cloud, represents the laser point serial number in the high reflection material, is the number of laser points in the high reflection material, is the standard angle point coordinate value of the trailer template, and =1, 2, 3, 4, 5, 6; is θ The corresponding rotation matrix is used for two-dimensional rotation of the trailer, so that the height value (z) of all angle point clouds is removed, and the trailer rotation angle is calculated after dimension reduction.

[0064] Before the trailer rotation angle estimation, the autonomous driving truck has estimated the trailer rotation angle by increasing a special laser radar and has been applied to an actual autonomous driving truck. The trailer pose estimated by increasing the special laser radar is taken as the true value to compare the trailer pose estimation result of the present application. In the autonomous parking process of the autonomous driving truck, the comparison of the trailer pose calculation results and error conditions of the two is shown in Figure 6 . It is found through calculation that the maximum error between the trailer pose estimation of the present application and the true value is 1.67°, the average error is 0.48°, and the error standard deviation is 0.37°.

[0065] The present application effectively solves the problem of trailer pose estimation by improving the intelligent degree of the vehicle, improves the control accuracy of the autonomous driving truck on the vehicle, improves the working efficiency of the vehicle during autonomous parking, and increases the economic benefit of the vehicle per unit time.

[0066] The application does not increase the sensors of the automatic driving vehicle, and correct estimation of the turning angle and the pose of the trailer can be realized by only installing the reflective material at four corner blocks of the trailer, so that the cost is low and the adaptability is good.

[0067] It should be noted that the execution sequence of the above steps is determined by its inherent logic and function, and the execution sequence can complete the expected results of the technical solutions of the patent disclosure, and should not cause any limitation and constraint to the implementation of the application and the embodiments. In addition to the above embodiments, the application can also have other implementation manners. Any technical solution formed by equivalent replacement or equivalent transformation falls within the protection scope of the application.

Claims

1. A laser-radar based trailer corner estimation method, characterized by, The method comprises the following steps: Step 1, installing a multi-line laser radar in front of a tractor, and installing a piece of high-reflective material at each of four corner blocks and two wheel arches of a trailer; and setting a multi-line laser radar on each side of the head of the tractor; Step 2, scanning the high-reflective material by the multi-line laser radar to collect laser point cloud data, and extracting high-reflective points in the laser point cloud as vehicle detection corner points, and numbering the detected corner points; Step 3, comparing the numbered corner points with a trailer template to calculate a trailer turning angle. First, the numbered corner points of the trailer are matched with the standard corner points in the trailer template, and then the trailer turning angle is calculated according to the following formula The multi-line laser radar is a 128-line laser radar. , ， In the formula, is the coordinate of the laser point cloud data collected by the laser radar projected on the XY two-dimensional plane, wherein represents the high-reflective material serial number of the point cloud, represents the laser point serial number in the high-reflective material, is the number of laser points in the high-reflective material, is the standard corner point coordinate value of the trailer template, and =1, 2, 3, 4, 5, 6; is In the step 2, two multi-line laser radars are used to scan an autonomous vehicle to collect whole-vehicle laser point cloud data, and the point cloud data collected by the two laser radars are synchronously processed. is the corresponding rotation matrix.

2. The laser-radar based trailer corner estimation method of claim 1, wherein, The specific method for numbering the detected corner points in the step 2 comprises:

3. The laser-radar based trailer corner estimation method of claim 1, wherein, (2) scanning a vehicle by a multi-line laser radar to obtain laser point cloud data, and extracting high-reflective points in the laser point cloud data; 4. The laser radar based trailer corner estimation method of claim 3, wherein, In step 2, high-reflectivity points in the synchronized laser point cloud are extracted as corner points, and the corner points are numbered and denoted as P i , i =1,2,3,4,5,6; in different driving states of the vehicle, the corner point information that can be detected by the laser radar is different, when the vehicle is driving straight, the laser radar can detect six corner points at the left front corner block, the right front corner block, the left rear corner block, the right rear corner block, the left side wheel arch, and the right side wheel arch, when the vehicle is changing lanes, the laser radar can detect four corner points at the left front corner block, the right front corner block, the right rear corner block, and the right side wheel arch, and when the vehicle is making a right-angle turn or parking, the laser radar can detect three corner points at the left front corner block, the right rear corner block, and the right side wheel arch.

5. The laser radar based trailer corner estimation method of claim 4, wherein, (3) screening the high-reflective points according to a template range of high-reflective material corner points to determine whether the high-reflective points are within the template range of the high-reflective material corner points, and if yes, numbering the high-reflective points according to the template range of the high-reflective material corner points where the high-reflective points are located, and if not, discarding the high-reflective points. (1) The vehicle is initialized to obtain an initialization rotation angle, six high-refraction material centers are calculated according to the initialization rotation angle, a template range of a high-refraction material corner point is set with the high-refraction material center coordinates as the center, and then the template ranges of the six high-refraction material corner points are numbered, respectively recorded as i , i =1,2,3,4,5,6; ​ ​

Images