Real-time calculation method, system, device and medium for articulation angle of autonomous driving truck

Abstract

The present invention discloses a method, system, device and medium for real-time calculation of the articulation angle of an autonomous driving truck, the method comprising the following steps: respectively constructing a follow-up coordinate system for a tractor and a trailer; in response to before the truck runs, calibrating a laser radar and calculating a conversion matrix from the laser radar to the follow-up coordinate system; dividing the region of interest of the laser radar, and judging the registration status of the point cloud of the laser radar according to the point cloud features of the overlapping part of the region of interest; outputting articulation angle estimation parameters or performing an articulation angle calculation operation based on the registration status of the point cloud of the laser radar according to the registration status of the point cloud; the present invention can calculate the conversion matrix between the two follow-up coordinate systems based on the registration operation of the laser radar, and realize the calculation of the articulation angle of the truck by converting the conversion matrix, with high calculation efficiency, small calculation amount, accurate results, and strong applicability and convenience.

Description

Technical Field

[0001] The present invention relates to the field of measurement technology. Specifically, the present invention is applied to the field of self-driving trucks, and in particular to a method, system, device and medium for real-time measurement of the articulation angle of an self-driving truck. Background Art

[0002] A truck consists of a tractor and a trailer. The articulation angle is the angle between the axes of the tractor and the trailer. The articulation point is located at the connection between the two vehicles. The angle is difficult to measure directly with conventional tools due to its special location. However, the articulation angle is of great significance to self-driving trucks. It is directly related to the prediction of the sweep area of ​​the vehicle body in the future and affects the planning results of the vehicle's driving trajectory.

[0003] In the prior art, the method for measuring the articulation angle includes: installing an angle sensor, a magnetic sensor and a laser radar, as follows:

[0004] For example, Chinese patent CN219736324U discloses a method for measuring the articulation angle by fixing an angle sensor at one end of the articulation mechanism. This solution uses a parallel four-link structure and an angle sensor to measure the articulation angle. Although the measurement is accurate, the cost is high. The sensor needs to be maintained frequently, and the installation will change the vehicle structure, affecting stability and safety.

[0005] For example, Chinese patent CN214565630U discloses a method for measuring the articulation angle using a magnetic sensor. This solution uses a magnetic sensor to measure the articulation angle. This solution requires the installation of multiple sensors and corresponding associated modules, and the articulation angle is calculated by heading angle calculation. However, this solution has the problem of magnetic interference, which affects other devices, and the data processing is complicated.

[0006] For example, Chinese patent CN115131365A discloses a method for measuring articulation angle using laser radar. This method uses laser radar and trailer identification plate to measure the articulation angle. The method is simple but relies on the position of the identification plate and lacks accuracy verification, which may lead to inaccurate results.

[0007] To sum up, among the existing solutions, the angle sensor method requires regular maintenance, is costly and affects vehicle stability; the magnetic sensor method is prone to interfere with other equipment and data processing is complex; the lidar method relies on the location of the signboard and has limited accuracy; given the importance of the articulation angle to the safety of autonomous driving, it is necessary to develop an articulation angle measurement solution with high accuracy, applicability and convenience. Summary of the invention

[0008] The object of the present invention is to provide a method, system, device and medium for real-time measurement of the articulation angle of an autonomous driving truck, thereby solving all or one of the above-mentioned problems existing in the prior art.

[0009] In order to solve the above technical problems, the specific technical solutions of the present invention are as follows:

[0010] In one aspect, the present invention provides a method for real-time calculation of the articulation angle of an autonomous driving truck, comprising the following steps:

[0011] Coordinate system construction steps:

[0012] Construct the follow-up coordinate systems of the tractor and trailer respectively;

[0013] Initialization calibration steps:

[0014] In response to the laser radar being calibrated before the truck is running, a transformation matrix of the laser radar to the following coordinate system is calculated;

[0015] Point cloud screening steps:

[0016] Divide the region of interest of the laser radar, and determine the registration status of the point cloud of the laser radar according to the point cloud features of the overlapping parts of the region of interest;

[0017] Articulation angle calculation steps:

[0018] According to the registration status of the point cloud, the articulation angle estimation parameters are output, or the articulation angle calculation operation based on the laser radar point cloud registration is performed.

[0019] In one embodiment, respectively constructing the follow-up coordinate systems for the tractor and the trailer includes:

[0020] The articulation point of the tractor and trailer is taken as the coordinate origin, the front direction of the tractor is taken as the OX axis, the OZ axis is set to be perpendicular to the driving plane of the tractor, and its positive direction points to the upper position of the tractor. The first follower coordinate system for the tractor is established based on the right-hand rule.

[0021] The hinge point of the tractor and trailer is taken as the coordinate origin, the front direction of the trailer is taken as the OX axis, the OZ axis direction is set perpendicular to the trailer's driving plane, and its positive direction points to the position above the trailer. Based on the right-hand rule, a second moving coordinate system for the trailer is established.

[0022] In one embodiment, the laser radar includes: a first radar and a second radar installed on the tractor, and a third radar and a fourth radar installed on the trailer;

[0023] The calculation of the conversion matrix of the laser radar to the following coordinate system includes:

[0024] Calculate a first transformation matrix from the first radar to the first following coordinate system;

[0025] Calculate a second transformation matrix from the second radar to the first following coordinate system;

[0026] Calculating a third transformation matrix from the third radar to the second following coordinate system;

[0027] A fourth transformation matrix from the fourth radar to the second tracking coordinate system is calculated.

[0028] In one embodiment, dividing the region of interest of the laser radar includes:

[0029] Obtain the distance from the front of the tractor to the hinge point and the distance from the hinge point to the rear of the trailer; obtain the hinge angle at the previous moment; calculate the coordinates of the laser radar at the previous moment according to the distance from the front of the tractor to the hinge point, the distance from the hinge point to the rear of the trailer and the hinge angle at the previous moment;

[0030] The sum of the distance from the front of the tractor to the articulation point and the distance from the articulation point to the rear of the trailer is taken as the radius, the laser radar is taken as the origin, and the tractor body or trailer body where the laser radar is located is taken as the boundary, and the fan-shaped areas with angles corresponding to the articulation angle at the previous moment are divided as the areas of interest of the laser radar.

[0031] In one embodiment, the area of ​​interest includes: a first area of ​​the first radar, a second area of ​​the second radar, a third area of ​​the third radar, and a fourth area of ​​the fourth radar;

[0032] The step of judging the registration status of the laser radar point cloud according to the point cloud features of the overlapping part of the region of interest includes:

[0033] Setting the overlapping portion of the first area and the third area as a first overlapping area;

[0034] Setting the overlapping portion of the second area and the fourth area as a second overlapping area;

[0035] extracting and identifying point cloud features of the first overlapping area and the second overlapping area using a feature-based point cloud registration method;

[0036] For the first overlapping area, when a large number of scattered point clouds in the first overlapping area have common features, or a truncated dense point cloud with a large number of common feature point clouds appears in the first overlapping area, it is determined that the first radar and the third radar can be registered; when there is no point cloud with a large number of common features in the truncated dense point cloud in the first overlapping area, it is determined that the first radar and the third radar cannot be registered;

[0037] For the second overlapping area, when a large number of scattered point clouds in the second overlapping area have common features, or a truncated dense point cloud with a large number of common feature point clouds appears in the second overlapping area, it is determined that the second radar and the fourth radar can be registered; when there is no point cloud with a large number of common features in the truncated dense point cloud in the second overlapping area, it is determined that the second radar and the fourth radar cannot be registered;

[0038] If the first radar and the third radar cannot be registered, and the second radar and the fourth radar cannot be registered, setting the point cloud registration condition to the first registration condition;

[0039] If the first radar and the third radar can be registered, and the second radar and the fourth radar cannot be registered, setting the point cloud registration condition to the second registration condition;

[0040] If the second radar and the fourth radar can be registered, and the first radar and the third radar cannot be registered, setting the point cloud registration condition to the third registration condition;

[0041] If the second radar and the fourth radar can be registered, and the first radar and the third radar can be registered, the point cloud registration condition is set to the fourth registration condition.

[0042] In one embodiment, outputting the articulation angle estimation parameters according to the registration status of the point cloud, or performing the articulation angle calculation operation based on the laser radar point cloud registration, includes:

[0043] In response to the first registration situation, outputting the articulation angle at the previous moment as the articulation angle estimation parameter, and performing a region of interest calculation of the laser radar at the next moment based on the articulation angle estimation parameter;

[0044] In response to the second registration situation, performing point cloud registration on the point cloud data of the first radar and the third radar to obtain a first coordinate system conversion matrix; calculating the articulation angle according to the first coordinate system conversion matrix and the articulation angle calculation formula;

[0045] In response to the third registration situation, performing point cloud registration on the point cloud data of the second radar and the fourth radar to obtain a second coordinate system conversion matrix; calculating the articulation angle according to the second coordinate system conversion matrix and the articulation angle calculation formula;

[0046] In response to the fourth registration situation, point cloud registration is performed on the point cloud data of the first radar and the third radar to obtain the first coordinate system conversion matrix; point cloud registration is performed on the point cloud data of the second radar and the fourth radar to obtain the second coordinate system conversion matrix; the articulation angle is calculated according to the first coordinate system conversion matrix, the second coordinate system conversion matrix and the articulation angle calculation formula to obtain two articulation angle data; and the average of the two articulation angle data is taken as the final articulation angle.

[0047] In one implementation, specifically, the point cloud registration operation is as follows:

[0048] Get the LiDAR pose matching matrix;

[0049] Obtain a first pose matching matrix of the first following coordinate system and a second pose matching matrix of the second following coordinate system;

[0050] According to the conversion matrix from the laser radar to the servo coordinate system, the posture matching matrix of the laser radar, the first posture matching matrix and the second posture matching matrix, the point cloud data registration conversion of the laser radar on the tractor and the laser radar on the trailer is performed to obtain the servo coordinate system conversion matrix between the first posture matching matrix and the second posture matching matrix;

[0051] The follow-up coordinate system conversion matrix includes: the first coordinate system conversion matrix obtained based on the alignment of the first radar and the third radar, and the second coordinate system conversion matrix obtained based on the alignment of the second radar and the fourth radar.

[0052] On the other hand, the present invention also provides a real-time articulation angle measurement system for an autonomous driving truck, comprising:

[0053] A coordinate system construction module is used to: construct follow-up coordinate systems for the tractor and trailer respectively;

[0054] The initialization calibration module is used to: calibrate the laser radar before the truck runs, and calculate the transformation matrix of the laser radar to the following coordinate system;

[0055] The point cloud screening module is used to: divide the region of interest of the laser radar, and determine the registration status of the point cloud of the laser radar according to the point cloud features of the overlapping part of the region of interest;

[0056] The articulation angle calculation module is used to: output the articulation angle estimation parameters according to the registration status of the point cloud, or perform the articulation angle calculation operation based on the laser radar point cloud registration.

[0057] On the other hand, the present invention also provides a computer-readable storage medium, on which a computer program is stored. When the computer program is executed by a processor, the steps of the method for real-time measurement of the articulation angle of the autonomous driving truck are implemented.

[0058] On the other hand, the present invention further provides a computer device, comprising a processor, a communication interface, a memory and a communication bus, wherein the processor, the communication interface and the memory communicate with each other via the communication bus; wherein:

[0059] The memory is used to store computer programs;

[0060] The processor is used to execute the steps of the method for real-time measurement of the articulation angle of the autonomous driving truck by running the program stored in the memory.

[0061] The beneficial effects of the technical solution of the present invention are:

[0062] 1. The real-time measurement method of the articulation angle of the self-driving truck described in the present invention can realize the laser radar-based alignment operation, calculate the transformation matrix between the two follow-up coordinate systems, and convert the transformation matrix to realize the calculation of the articulation angle of the truck; the entire calculation process does not require the installation of other sensors or computing modules, has high calculation efficiency, and small calculation amount, and mutual alignment is performed through the truck's own laser radar to ensure the accuracy of the results, and has strong applicability and convenience.

[0063] 2. The real-time articulation angle measurement system of the autonomous driving truck described in the present invention can realize the real-time articulation angle measurement method of the autonomous driving truck described in the present invention through the mutual cooperation of system modules.

[0064] 3. The computer-readable storage medium described in the present invention can cooperate with the guidance system module to realize the real-time measurement method of the articulation angle of the autonomous driving truck described in the present invention, and the computer-readable storage medium described in the present invention also effectively improves the operability of the real-time measurement method of the articulation angle of the autonomous driving truck.

[0065] 4. The computer device described in the present invention can store and execute the computer-readable storage medium, thereby realizing the real-time measurement method of the articulation angle of the autonomous driving truck described in the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0067] Figure 1 is a flow chart of a method for real-time calculation of the articulation angle of an autonomous driving truck according to Embodiment 1 of the present invention;

[0068] Figure 2 is a detailed flowchart of the method for real-time calculation of the articulation angle of an autonomous driving truck according to Embodiment 1 of the present invention;

[0069] Figure 3 is a planar schematic diagram of the follow-up coordinate system described in the method for real-time calculation of the articulation angle of an automatic driving truck described in Example 1 of the present invention;

[0070] Figure 4 is a plan schematic diagram of the region of interest described in the method for real-time calculation of the articulation angle of an autonomous driving truck described in Example 1 of the present invention;

[0071] Figure 5 2 is a schematic diagram of the architecture of the real-time articulation angle measurement system for an autonomous driving truck according to Embodiment 2 of the present invention;

[0072] Figure 6 is a schematic diagram of the structure of the computer device described in Example 4 of the present invention;

[0073] The symbols in the accompanying drawings are explained as follows:

[0074] 1501, processor; 1502, communication interface; 1503, memory; 1504, communication bus. DETAILED DESCRIPTION

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

[0076] In the description of the present invention, it should be noted that the embodiments described in the present invention are only part of the embodiments of the present invention, rather than all of the embodiments; based on the embodiments in the present invention, all other embodiments obtained by ordinary technicians in the field without making creative work are within the scope of protection of the present invention.

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

[0078] Embodiment 1, this embodiment provides a method for real-time calculation of the articulation angle of an autonomous driving truck, such as Figure 1~Figure 4 As shown, the following steps are included:

[0079] S100, coordinate system construction steps:

[0080] In this step, the follow-up coordinate systems of the tractor and trailer are constructed respectively;

[0081] Specifically, the hinge point of the tractor and the trailer is taken as the coordinate origin, the front direction of the tractor is taken as the OX axis, the direction of the OZ axis is set to be perpendicular to the driving plane of the tractor, and its positive direction points to the position above the tractor. According to the right-hand rule, the first moving coordinate system A (OXYZ) about the tractor is established.

[0082] Specifically, the hinge point of the tractor and the trailer is taken as the coordinate origin, the front direction of the trailer is taken as the OX axis, the OZ axis direction is set perpendicular to the driving plane of the trailer, and its positive direction points to the position above the trailer. According to the right-hand rule, the second moving coordinate system B (Oxyz) about the trailer is established.

[0083] Specifically, the lidar has its own lidar coordinate system.

[0084] Specifically, according to the different angles between the tractor and the trailer, the two tracking coordinate systems have the following tracking relationship: when the tractor and the trailer are in a straight line, the tracking coordinate system A coincides with the tracking coordinate system B; when there is an angle between the tractor and the trailer, the tracking coordinate system B rotates around the origin O relative to the tracking coordinate system A, and the angle of rotation of its z-axis is equal to the articulation angle.

[0085] S200, initialization calibration steps:

[0086] In this step, before the truck runs, the laser radar is calibrated to ensure the measurement accuracy; at the same time, the transformation matrix between different coordinate systems is calculated based on the point cloud coordinates of each coordinate system;

[0087] Specifically, laser radars 1 (ie, the first radar) and 2 (ie, the second radar) are installed on the tractor, and their positions in the follow-up coordinate system A are fixed.

[0088] Specifically, laser radars 3 (ie, the third radar) and 4 (ie, the fourth radar) are installed on the trailer, and their positions in the tracking coordinate system B are fixed.

[0089] Specifically, according to the position data of the point cloud coordinates in each coordinate system, the first transformation matrix M1 from the laser radar 1 to the follow-up coordinate system A is calculated.

[0090] Specifically, according to the position data of the point cloud coordinates in each coordinate system, the second transformation matrix M2 from the laser radar 2 to the follow-up coordinate system A is calculated.

[0091] Specifically, according to the position data of the point cloud coordinates in each coordinate system, the third transformation matrix M3 from the laser radar 3 to the follow-up coordinate system B is calculated.

[0092] Specifically, according to the position data of the point cloud coordinates in each coordinate system, the fourth transformation matrix M4 from the laser radar 4 to the follow-up coordinate system B is calculated.

[0093] S300, point cloud screening steps:

[0094] In this step, obstacles that block point cloud information are identified, and obstacle judgment is performed based on point cloud features. The corresponding point cloud data is discarded according to the obstacle judgment situation, and the possibility of registration between point clouds is confirmed, thereby ensuring the smooth progress of subsequent point cloud registration and the accuracy of articulation angle calculation. It should be noted that when using laser radar for point cloud registration during truck driving, if the line of sight between two laser radars is blocked by obstacles, especially when these obstacles are located between the tractor and the trailer, the collected point cloud information will be incomplete or blocked; using point cloud frames containing occlusion information for registration will directly affect the accuracy of articulation angle calculation.

[0095] Specifically, the region of interest is first divided:

[0096] (1) Obtain the following known information: the distance a from the front of the tractor to the articulation point and the distance b from the articulation point to the rear of the trailer.

[0097] (2) Based on the above parameters and the articulation angle calculated at time t-1 (when performing the first calculation, substitute the articulation angle at time t-1 = 0), the coordinates of each lidar at time t-1 are obtained.

[0098] (3) With the laser radar (1, 2, 3, 4) itself as the origin, a+b as the radius, and the body of the tractor or trailer where the laser radar (1, 2, 3, 4) is located as the boundary, according to the articulation angle at time t-1 The corresponding fan-shaped area is determined by the angle, and the fan-shaped area is set as the area of ​​interest corresponding to the laser radar (1, 2, 3, 4) at time t (i.e., the first area E1, the second area E2, the third area E3, and the fourth area E4); in the above-mentioned area of ​​interest, all input point clouds are P={p1,p2,p3…p k}, where k is the number of point clouds and the coordinates of each point are p i ={x i ,y i ,z i}(i=1,2,3…k).

[0099] Specifically, the overlapping part of the overlapping part of the regions of interest E1 and E3 of the laser radars 1 and 3 is detected and named E 13 ;

[0100] (1) Extract and identify the features of point clouds using feature-based point cloud registration methods;

[0101] (2) If E 13 If a large number of scattered point clouds in E have common features, it can be judged that LiDAR 1 and LiDAR 3 are 13 A large amount of scattered point cloud information can be detected in the image, and it is determined that this frame of point cloud data can be used for registration;

[0102] (3) If E 13 If a truncated dense point cloud appears in the 13 Whether there is a point cloud with a large number of common features in the image; if so, it is judged that the position where the truncated dense point cloud appears is the same obstacle, and it is judged that this frame of point cloud data can be used for registration; if not, it is judged that the truncated dense point clouds identified by LiDARs 1 and 3 are two different obstacles, and it is judged that this frame of point cloud data cannot be registered, and this frame of point cloud is discarded.

[0103] Specifically, the overlapping part of the regions of interest E2 and E4 of the laser radars 2 and 4 is detected and named as E 24 ;

[0104] (1) Extract and identify the features of point clouds using feature-based point cloud registration methods;

[0105] (2) If E 24 If a large number of scattered point clouds in E have common features, it can be judged that LiDAR 2 and LiDAR 4 are 24 A large amount of scattered point cloud information can be detected in the image, and it is determined that this frame of point cloud data can be used for registration;

[0106] (3) If E 24 If a truncated dense point cloud appears in the 24Whether there is a point cloud with a large number of common features in the image; if so, it is determined that the position where the truncated dense point cloud appears is the same obstacle, and it is determined that this frame of point cloud data can be used for registration; if not, it is determined that the truncated dense point clouds identified by LiDARs 2 and 4 are two different obstacles, and it is determined that this frame of point cloud data cannot be registered, and this frame of point cloud is discarded.

[0107] Specifically, determine the point cloud registration between LiDARs 1 to 4:

[0108] (1) If LiDARs 1 and 3 cannot be registered, and LiDARs 2 and 4 cannot be registered, it is determined that obstacles that block LiDAR registration appear in all four ROIs, and the first registration situation is set;

[0109] (2) If laser radars 1 and 3 can be registered, but laser radars 2 and 4 cannot be registered, it is determined that at least one of E1 and E3 has no obstacles, and obstacles exist in both E2 and E4, and the second registration situation is set;

[0110] (3) If laser radars 2 and 4 can be registered, but laser radars 1 and 3 cannot be registered, it is determined that at least one of E2 and E4 has no obstacles, and obstacles exist in both E1 and E3, and the third registration situation is set;

[0111] (4) If laser radars 2 and 4 can be registered, and laser radars 1 and 3 can also be registered, it is determined that at least one area within E2 and E4 does not have an obstacle, and at least one area within E1 and E3 does not have an obstacle, and this is set as the fourth registration situation.

[0112] S400, articulation angle calculation steps:

[0113] In this step, according to the different obstacle conditions obtained in S300, the point cloud of the laser radar that can be registered is selected for registration to obtain a conversion matrix; the articulation angle is calculated according to the conversion matrix and the calculation formula of the articulation angle;

[0114] Specifically, the point cloud registration operation is as follows:

[0115] (1) The point cloud data collected by the corresponding lidar in real time is registered with the point cloud data at the previous moment through the ICP algorithm to obtain the pose matching matrix (N1, N2, N3, N4) of the corresponding lidar (1, 2, 3, 4) at time t0.

[0116] (2) According to the pose matching matrix of the corresponding laser radar and the transformation matrix (M1~M4) from the corresponding laser radar to the two following coordinate systems, calculate the pose matching matrix N of the following coordinate system A AAnd the pose matching matrix N of the moving coordinate system B B ;in, ; .

[0117] (3) According to the pose matching matrix N of the moving coordinate system A A And the pose matching matrix N of the moving coordinate system B B Calculate the conversion matrix of the moving coordinate system after point cloud registration (i.e. matrix T and matrix K):

[0118] (3.1) When performing point cloud data registration of LiDAR 1 and LiDAR 3, calculate the pose matching matrix N A And pose matching matrix N B The first coordinate system transformation matrix T between them is calculated as follows:

[0119] ,Right now ;

[0120] ;

[0121] Among them: R1, the matrix after geometric transformation such as scale, rotation, shearing, etc.; S1=[m 14 m 24 m 34 ], corresponding to the translation transformation matrix; 0, represents [0 0 0]=[m 41 m 42 m 43 ], corresponding to the projection transformation matrix; 1, represents [m 44 ], reflecting the transformation matrix of the overall proportion; the coordinate transformation studied in this method is a rigid transformation, so the definition [m 41 m 42 m 43 ] = [0 0 0], [m 44 ]=1;

[0122] (3.2) When performing point cloud data registration of LiDAR 2 and LiDAR 4, calculate the pose matching matrix N A And pose matching matrix N B The second coordinate system transformation matrix K between them is calculated as follows:

[0123] ,Right now ;

[0124] ;

[0125] Among them: R2, the matrix after geometric transformation such as scale, rotation, shearing, etc.; S2=[n 14 n 24 n34 ], corresponding to the translation transformation matrix; 0, represents [0 0 0]=[n 41 n 42 n 43 ], corresponding to the projection transformation matrix; 1, represents [n 44 ], reflecting the transformation matrix of the overall proportion; the coordinate transformation studied in this method is a rigid transformation, so it is defined as [n 41 n 42 n 43 ] = [0 0 0], [n 44 ]=1;

[0126] Specifically, the hinge angle calculation formula is as follows:

[0127] ;

[0128] ;

[0129] ;

[0130] ;

[0131] Among them, due to the coordinate transformation between the two three-dimensional space moving coordinate systems, there are 6 degrees of freedom variables, which are the rotation variables of the moving coordinate system around the three coordinate axes. , and , and three translation variables t around the coordinate axes x ,t y and t z ; However, since the coordinate origins of the moving coordinate system A and the moving coordinate system B are the same, there is no translation variable t x ,t y and t z ; then for other variables, It is the rotation angle of the moving coordinate system B around the X axis. It is the rotation angle of the moving coordinate system B around the Y axis. This is the rotation angle of the follower coordinate system B around the Z axis, which is the final calculated articulation angle; when the follower coordinate system B rotates around the X axis , rotate around the Y axis , rotate around the Z axis After that, the moving coordinate system A can be obtained; therefore, is the rotation matrix of the moving coordinate system B around the X axis, is the rotation matrix of the moving coordinate system B around the Y axis, is the rotation matrix of the moving coordinate system B around the Z axis; That is the size of the rotation matrix between the two moving coordinate systems;

[0132] In addition, in the process of point cloud registration, according to the transformation matrix T or K of the coordinate system, the rotation matrix R1 or R2 between the two moving coordinate systems can be obtained in the corresponding case; therefore, based on the corresponding relationship between the above rotation matrices, it can be obtained that R1 or R2 = Therefore, based on the above-mentioned hinge angle calculation formula and the point cloud registration calculation process, we can , and obtain the corresponding articulation angle data.

[0133] Specifically, the calculation steps of the articulation angle are as follows:

[0134] (1) In response to the first registration situation, LiDARs 1 and 3 cannot be registered, and LiDARs 2 and 4 cannot be registered. At this time, the articulation angle at time t cannot be calculated. Therefore, the point cloud frame at time t is discarded, and the articulation angle data at time t-1 is output as the reference articulation angle at time t. Based on this reference articulation angle, the regions of interest of each LiDAR point cloud at time t+1 are determined, and the articulation angle is calculated.

[0135] (2) In response to the second registration situation, laser radars 1 and 3 can be registered, but laser radars 2 and 4 cannot be registered. At this time, the point cloud data of laser radars 1 and 3 are registered to obtain the corresponding first transformation matrix T. The articulation angle is calculated based on the transformation matrix T and the articulation angle calculation formula. The calculation process is as follows:

[0136] (2.1) According to the registration transformation between LiDARs 1 and 3, the coordinate system transformation matrix T is obtained;

[0137] (2.2) According to the coordinate system transformation matrix T, the rotation matrix R1 between the moving coordinate system B and the moving coordinate system A is obtained:

[0138] ;

[0139] (2.3) Then the hinge angle .

[0140] (3) In response to the third registration situation, laser radars 2 and 4 can be registered, but laser radars 1 and 3 cannot be registered. At this time, the point cloud data of laser radars 2 and 4 are registered to obtain the corresponding second transformation matrix K. The articulation angle is calculated based on the transformation matrix K and the articulation angle calculation formula. The calculation process is as follows:

[0141] (3.1) According to the registration transformation between laser radars 2 and 4, the coordinate system transformation matrix K is obtained;

[0142] (3.2) According to the coordinate system transformation matrix K, the rotation matrix R2 between the moving coordinate system B and the moving coordinate system A is obtained:

[0143] ;

[0144] (3.3) Then the hinge angle .

[0145] (4) In response to the fourth registration situation, laser radars 2 and 4 can be registered, and laser radars 1 and 3 can also be registered. At this time, the point cloud data of laser radars 1 and 3 and laser radars 2 and 4 are respectively registered to obtain the corresponding transformation matrix T and transformation matrix K. The articulation angle is calculated according to the transformation matrix T, the transformation matrix K and the articulation angle calculation formula. , take the average of the two obtained hinge angles and output them. The calculation process is as follows:

[0146] (4.1) According to the registration transformation between LiDARs 1 and 3, the coordinate system transformation matrix T is obtained;

[0147] (4.2) According to the registration transformation between laser radars 2 and 4, the coordinate system transformation matrix K is obtained;

[0148] (4.3) According to the coordinate system transformation matrix T, the rotation matrix R1 between the moving coordinate system B and the moving coordinate system A is obtained: ; ;

[0149] (4.4) According to the coordinate system transformation matrix K, the rotation matrix R2 between the moving coordinate system B and the moving coordinate system A is obtained: ; ;

[0150] (4.5) Then the hinge angle .

[0151] Specifically, in response to the automatic driving truck moving forward, steps S300 to S400 are continuously repeated, thereby achieving real-time acquisition of the truck's articulation angle.

[0152] It should be noted that the above examples are only for explaining the present invention and cannot limit the protection scope of the present invention.

[0153] Embodiment 2, based on the same inventive concept as the method for real-time calculation of the articulation angle of an autonomous driving truck described in Embodiment 1, this embodiment provides a real-time calculation system for the articulation angle of an autonomous driving truck, such as Figure 5 As shown, including:

[0154] A coordinate system construction module is used to: construct follow-up coordinate systems for the tractor and trailer respectively;

[0155] The initialization calibration module is used to: calibrate the laser radar before the truck runs, and calculate the transformation matrix of the laser radar to the following coordinate system;

[0156] The point cloud screening module is used to: divide the region of interest of the laser radar, and determine the registration status of the point cloud of the laser radar according to the point cloud features of the overlapping part of the region of interest;

[0157] The articulation angle calculation module is used to: output the articulation angle estimation parameters according to the registration status of the point cloud, or perform the articulation angle calculation operation based on the laser radar point cloud registration.

[0158] Embodiment 3: This embodiment provides a computer-readable storage medium, including:

[0159] The storage medium is used to store computer software instructions used to implement the real-time measurement method for the articulation angle of the autonomous driving truck described in the above-mentioned embodiment 1, which includes a program for executing the above-mentioned program set for the real-time measurement method for the articulation angle of the autonomous driving truck; specifically, the executable program can be built into the real-time measurement system for the articulation angle of the autonomous driving truck described in embodiment 2, so that the real-time measurement system for the articulation angle of the autonomous driving truck can implement the real-time measurement method for the articulation angle of the autonomous driving truck described in embodiment 1 by executing the built-in executable program.

[0160] In addition, the computer-readable storage medium of this embodiment may adopt any combination of one or more computer-readable storage media, wherein the computer-readable storage medium includes electrical, optical, electromagnetic, infrared or semiconductor systems, devices or components, or any combination thereof.

[0161] Embodiment 4, this embodiment provides an electronic device, such as Figure 6 As shown, the electronic device may include: a processor 1501 , a communication interface 1502 , a memory 1503 and a communication bus 1504 , wherein the processor 1501 , the communication interface 1502 , and the memory 1503 communicate with each other via the communication bus 1504 .

[0162] Memory 1503, used for storing computer programs;

[0163] Processor 1501 is used to implement the steps of the real-time measurement method of the articulation angle of the autonomous driving truck described in the above-mentioned embodiment 1 when executing the computer program stored in the memory 1503.

[0164] As an embodiment of the present invention, the communication bus mentioned in the above terminal can be a Peripheral Component Interconnect (PCI) bus or an Extended Industry Standard Architecture (EISA) bus. The communication bus can be divided into an address bus, a data bus, a control bus, etc. For ease of representation, Figure 6 Only one thick line is used in the diagram, but this does not mean that there is only one bus or only one type of bus.

[0165] As an implementation mode of the present invention, the communication interface is used for communication between the above-mentioned terminal and other devices.

[0166] As an embodiment of the present invention, the memory may include a random access memory (RAM) or a non-volatile memory, such as at least one disk memory. Optionally, the memory may also be at least one storage device located away from the aforementioned processor.

[0167] As an embodiment of the present invention, the above-mentioned processor can be a general-purpose processor, including a central processing unit (CPU), a network processor (NP), etc.; it can also be a digital signal processor (DSP), an application specific integrated circuit (ASIC), a field programmable gate array (FPGA) or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components.

[0168] Different from the prior art, the present application provides a method, system, device and medium for real-time measurement of the articulation angle of an autonomous driving truck. It can calculate the transformation matrix between two follow-up coordinate systems based on the laser radar alignment operation, and calculate the size of the truck's articulation angle through conversion through the transformation matrix. The entire calculation process does not require the installation of additional sensors or computing modules, has high computing efficiency and low computational complexity, and uses the truck's own laser radar for mutual alignment to ensure the accuracy of the results. It has strong applicability and convenience.

[0169] It should be understood that in the various embodiments of this document, the size of the serial numbers of the above-mentioned processes does not mean the order of execution. The execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of this document.

[0170] It should also be understood that in the embodiments of this document, the term "and / or" is only a description of the association relationship of the associated objects, indicating that three relationships may exist. For example, A and / or B can represent: A exists alone, A and B exist at the same time, and B exists alone. In addition, the character " / " in this document generally indicates that the associated objects before and after are in an "or" relationship.

[0171] Those of ordinary skill in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of the two. In order to clearly illustrate the interchangeability of hardware and software, the composition and steps of each example have been generally described in terms of function in the above description. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of this article.

[0172] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working processes of the systems, devices and units described above can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0173] In the several embodiments provided herein, it should be understood that the disclosed systems, devices and methods can be implemented in other ways. For example, the device embodiments described above are only schematic, for example, the division of the units is only a logical function division, and there may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. In addition, the mutual coupling or direct coupling or communication connection shown or discussed can be an indirect coupling or communication connection through some interfaces, devices or units, or can be electrical, mechanical or other forms of connection.

[0174] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the embodiments of this article.

[0175] In addition, each functional unit in each embodiment of this invention may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above integrated unit may be implemented in the form of hardware or in the form of software functional unit.

[0176] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this article is essentially or the part that contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including a number of instructions to enable a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of this article. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk and other media that can store program codes.

[0177] The above descriptions are merely embodiments of the present invention and are not intended to limit the patent scope of the present invention. Any equivalent structure or equivalent process transformation made using the contents of the present invention specification and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. A method for real-time calculation of the articulation angle of an autonomous driving truck, characterized in that: The following steps are involved: Coordinate system construction steps: Construct the follow-up coordinate systems of the tractor and trailer respectively; Initialization calibration steps: In response to the laser radar being calibrated before the truck is running, a transformation matrix of the laser radar to the following coordinate system is calculated; Point cloud screening steps: Divide the region of interest of the laser radar, and determine the registration status of the point cloud of the laser radar according to the point cloud features of the overlapping parts of the region of interest; Articulation angle calculation steps: Outputting articulation angle estimation parameters or performing articulation angle calculation operations based on laser radar point cloud registration according to the point cloud registration availability; The said constructing the follow-up coordinate systems for the tractor and the trailer respectively comprises: taking the articulation point of the tractor and the trailer as the coordinate origin, taking the front direction of the tractor as the OX axis, setting the direction of the OZ axis perpendicular to the driving plane of the tractor, and its positive direction pointing to the upper position of the tractor, and establishing a first follow-up coordinate system for the tractor based on the right-hand rule; taking the articulation point of the tractor and the trailer as the coordinate origin, taking the front direction of the trailer as the OX axis, setting the direction of the OZ axis perpendicular to the driving plane of the trailer, and its positive direction pointing to the upper position of the trailer, and establishing a second follow-up coordinate system for the trailer based on the right-hand rule; The laser radar comprises: a first radar and a second radar installed on the tractor, and a third radar and a fourth radar installed on the trailer; the calculation of the conversion matrix from the laser radar to the follow-up coordinate system comprises: calculating the first conversion matrix from the first radar to the first follow-up coordinate system; calculating the second conversion matrix from the second radar to the first follow-up coordinate system; calculating the third conversion matrix from the third radar to the second follow-up coordinate system; calculating the fourth conversion matrix from the fourth radar to the second follow-up coordinate system; The dividing of the laser radar's area of ​​interest includes: obtaining the distance from the front of the tractor to the hinge point and the distance from the hinge point to the rear of the trailer; obtaining the hinge angle at the previous moment; calculating the coordinates of the laser radar at the previous moment according to the distance from the front of the tractor to the hinge point, the distance from the hinge point to the rear of the trailer, and the hinge angle at the previous moment; taking the sum of the distance from the front of the tractor to the hinge point and the distance from the hinge point to the rear of the trailer as the radius, taking the laser radar as the origin, taking the tractor body or the trailer body where the laser radar is located as the boundary, and dividing the fan-shaped areas with angles corresponding to the hinge angle at the previous moment as the area of ​​interest of the laser radar; The area of ​​interest includes: a first area of ​​the first radar, a second area of ​​the second radar, a third area of ​​the third radar, and a fourth area of ​​the fourth radar; The method of judging the registration status of the point cloud of the laser radar according to the point cloud features of the overlapping part of the area of ​​interest includes: setting the overlapping part of the first area and the third area as the first overlapping area; setting the overlapping part of the second area and the fourth area as the second overlapping area; using a feature-based point cloud registration method to extract and identify the point cloud features of the first overlapping area and the second overlapping area; for the first overlapping area, when a large number of scattered point clouds in the first overlapping area have common features, or a truncated dense point cloud with a large number of common feature point clouds appears in the first overlapping area, it is judged that the first radar and the third radar support registration; when there is no point cloud with a large number of common features in the truncated dense point cloud in the first overlapping area, it is judged that the first radar and the third radar cannot be registered; for the second overlapping area, when a large number of scattered point clouds in the second overlapping area have common features, or a large number of common features point clouds appear in the second overlapping area When the truncated dense point cloud of the point cloud is acquired, it is determined that the second radar and the fourth radar support registration; when there is no point cloud with a large number of common features in the truncated dense point cloud in the second overlapping area, it is determined that the second radar and the fourth radar cannot be registered; if the first radar and the third radar cannot be registered, and the second radar and the fourth radar cannot be registered, the point cloud registration situation is set to the first registration situation; if the first radar and the third radar support registration, and the second radar and the fourth radar cannot be registered, the point cloud registration situation is set to the second registration situation; if the second radar and the fourth radar support registration, and the first radar and the third radar cannot be registered, the point cloud registration situation is set to the third registration situation; if the second radar and the fourth radar support registration, and the first radar and the third radar also support registration, the point cloud registration situation is set to the fourth registration situation; The output of the articulation angle estimation parameter or the execution of the articulation angle calculation operation based on the laser radar point cloud registration according to the point cloud registration condition includes: in response to the first registration condition, outputting the articulation angle at the previous moment as the articulation angle estimation parameter, and calculating the region of interest of the laser radar at the next moment based on the articulation angle estimation parameter; in response to the second registration condition, performing point cloud registration on the point cloud data of the first radar and the third radar to obtain a first coordinate system transformation matrix; calculating the articulation angle according to the first coordinate system transformation matrix and the articulation angle calculation formula; in response to the third registration condition, performing point cloud registration on the point cloud data of the second radar and the fourth radar to obtain a first coordinate system transformation matrix; calculating the articulation angle according to the first coordinate system transformation matrix and the articulation angle calculation formula; The point cloud data of the radar are point cloud registered to obtain a second coordinate system conversion matrix; the articulation angle is calculated according to the second coordinate system conversion matrix and the articulation angle calculation formula; in response to the fourth registration situation, the point cloud data of the first radar and the third radar are point cloud registered to obtain the first coordinate system conversion matrix; the point cloud data of the second radar and the fourth radar are point cloud registered to obtain the second coordinate system conversion matrix; the articulation angle is calculated according to the first coordinate system conversion matrix, the second coordinate system conversion matrix and the articulation angle calculation formula to obtain two articulation angle data; the average of the two articulation angle data is taken as the final articulation angle.

2. The method for real-time calculation of the articulation angle of an autonomous driving truck according to claim 1, characterized in that: Specifically, the point cloud registration operation is as follows: Get the LiDAR pose matching matrix; Obtain a first pose matching matrix of the first following coordinate system and a second pose matching matrix of the second following coordinate system; According to the conversion matrix from the laser radar to the servo coordinate system, the posture matching matrix of the laser radar, the first posture matching matrix and the second posture matching matrix, the point cloud data registration conversion of the laser radar on the tractor and the laser radar on the trailer is performed to obtain the servo coordinate system conversion matrix between the first posture matching matrix and the second posture matching matrix; The follow-up coordinate system conversion matrix includes: the first coordinate system conversion matrix obtained based on the alignment of the first radar and the third radar, and the second coordinate system conversion matrix obtained based on the alignment of the second radar and the fourth radar.

3. A real-time articulation angle measurement system for an autonomous driving truck, characterized in that: include: A coordinate system construction module is used to: construct follow-up coordinate systems for the tractor and trailer respectively; The coordinate system construction module is further used to: take the articulation point of the tractor and the trailer as the coordinate origin, take the front direction of the tractor as the OX axis, set the OZ axis direction to be perpendicular to the driving plane of the tractor, and its positive direction points to the upper position of the tractor, and establish a first follow-up coordinate system for the tractor based on the right-hand rule; take the articulation point of the tractor and the trailer as the coordinate origin, take the front direction of the trailer as the OX axis, set the OZ axis direction to be perpendicular to the driving plane of the trailer, and its positive direction points to the upper position of the trailer, and establish a second follow-up coordinate system for the trailer based on the right-hand rule; The initialization calibration module is used to: calibrate the laser radar before the truck runs, and calculate the conversion matrix from the laser radar to the follow-up coordinate system; the laser radar includes: a first radar and a second radar installed on the tractor, and a third radar and a fourth radar installed on the trailer; the initialization calibration module is also used to: calculate the first conversion matrix from the first radar to the first follow-up coordinate system; calculate the second conversion matrix from the second radar to the first follow-up coordinate system; calculate the third conversion matrix from the third radar to the second follow-up coordinate system; calculate the fourth conversion matrix from the fourth radar to the second follow-up coordinate system; The point cloud screening module is used to: divide the area of ​​interest of the laser radar, and determine the registration status of the point cloud of the laser radar according to the point cloud features of the overlapping part of the area of ​​interest; the point cloud screening module is also used to: obtain the distance from the front of the tractor to the hinge point and the distance from the hinge point to the rear of the trailer; obtain the articulation angle at the previous moment; calculate the coordinates of the laser radar at the previous moment according to the distance from the front of the tractor to the hinge point, the distance from the hinge point to the rear of the trailer and the articulation angle at the previous moment; take the sum of the distance from the front of the tractor to the hinge point and the distance from the hinge point to the rear of the trailer as the radius, take the laser radar as the origin, and take the tractor body or trailer body where the laser radar is located as the boundary, respectively. Divide a fan-shaped area with an angle corresponding to the articulation angle at the previous moment as an area of ​​interest of the laser radar; the area of ​​interest includes: a first area of ​​the first radar, a second area of ​​the second radar, a third area of ​​the third radar and a fourth area of ​​the fourth radar; the point cloud screening module is further used to: set the overlapping part of the first area and the third area as a first overlapping area; set the overlapping part of the second area and the fourth area as a second overlapping area; use a feature-based point cloud registration method to extract and identify point cloud features of the first overlapping area and the second overlapping area; for the first overlapping area, when a large number of scattered point clouds in the first overlapping area have common features, or the first overlapping area When a truncated dense point cloud with a large number of common feature point clouds appears in an overlapping area, it is judged that the first radar and the third radar support registration; when there is no point cloud with a large number of common features in the truncated dense point cloud in the first overlapping area, it is judged that the first radar and the third radar cannot be registered; for the second overlapping area, when a large number of scattered point clouds in the second overlapping area have common features, or a truncated dense point cloud with a large number of common feature point clouds appears in the second overlapping area, it is judged that the second radar and the fourth radar support registration; when there is no point cloud with a large number of common features in the truncated dense point cloud in the second overlapping area, it is judged that the second radar and the fourth radar cannot be registered; If the first radar and the third radar cannot be registered, and the second radar and the fourth radar cannot be registered, the point cloud registration condition is set to the first registration condition; if the first radar and the third radar support registration, and the second radar and the fourth radar cannot be registered, the point cloud registration condition is set to the second registration condition; if the second radar and the fourth radar support registration, and the first radar and the third radar cannot be registered, the point cloud registration condition is set to the third registration condition; if the second radar and the fourth radar support registration, and the first radar and the third radar also support registration, the point cloud registration condition is set to the fourth registration condition; The articulation angle calculation module is used to: output the articulation angle estimation parameters according to the point cloud registration situation, or perform the articulation angle calculation operation based on the laser radar point cloud registration; the articulation angle calculation module is also used to: in response to the first registration situation, output the articulation angle at the previous moment as the articulation angle estimation parameter, and calculate the area of ​​interest of the laser radar at the next moment based on the articulation angle estimation parameter; in response to the second registration situation, perform point cloud registration on the point cloud data of the first radar and the third radar to obtain a first coordinate system transformation matrix; calculate the articulation angle according to the first coordinate system transformation matrix and the articulation angle calculation formula; in response to the third registration situation, The point cloud data of the second radar and the fourth radar are point cloud registered to obtain a second coordinate system conversion matrix; the articulation angle is calculated according to the second coordinate system conversion matrix and the articulation angle calculation formula; in response to the fourth registration situation, the point cloud data of the first radar and the third radar are point cloud registered to obtain the first coordinate system conversion matrix; the point cloud data of the second radar and the fourth radar are point cloud registered to obtain the second coordinate system conversion matrix; the articulation angle is calculated according to the first coordinate system conversion matrix, the second coordinate system conversion matrix and the articulation angle calculation formula to obtain two articulation angle data; the average of the two articulation angle data is taken as the final articulation angle.

4. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and when the computer program is executed by the processor, the steps of the method for real-time measurement of the articulation angle of an autonomous driving truck according to any one of claims 1 to 2 are implemented.

5. A computer device, characterized in that: It includes a processor, a communication interface, a memory and a communication bus, wherein the processor, the communication interface and the memory communicate with each other through the communication bus; wherein: The memory is used to store computer programs; The processor is used to execute the steps of the method for real-time measurement of the articulation angle of an autonomous driving truck as described in any one of claims 1 to 2 by running the program stored in the memory.

Images