A method and system for detecting the included angle between a tractor and a trailer based on dual-antenna combination inertial navigation

Abstract

This invention discloses a method and system for detecting the angle between a tractor and trailer based on a dual-antenna combined inertial navigation system, applicable to vehicle systems. The method includes: acquiring the position difference vector between the main and auxiliary antennas using a dual-antenna GNSS system on the tractor; calculating the first coarse heading angle of the tractor; and then fusing IMU data and filtering it to obtain a high-precision first heading angle. The roll and pitch angles are calculated using accelerometer data from the trailer's IMU, and tilt compensation is performed on the magnetometer data to obtain the second heading angle of the trailer. Furthermore, the heading and lane curvature data from the tractor can be combined to dynamically compensate and correct the trailer's heading online. The difference between the first and second heading angles is calculated in real time to obtain the angle between them. This invention can maintain heading output using IMU and wheel speed data even when GNSS signals fail, and ensure trailer heading accuracy through dynamic compensation, thereby achieving stable and high-precision tractor-trailer angle measurement.

Description

Technical Field

[0001] This invention relates to the field of navigation technology, and in particular to a method and system for detecting the angle between a tractor and a trailer based on a dual-antenna combined inertial navigation system. Background Technology

[0002] The tractor-trailer angle, the angle formed between the tractor and trailer, is a crucial indicator for assessing the tractor's driving stability. An excessively large or small tractor-trailer angle can lead to safety hazards. An excessively large angle may cause the trailer to sway, affecting maneuverability; an excessively small angle may increase the risk of rollover. Especially with the development of intelligent driving technology, tractors are gradually developing L2+ functions such as high-speed NOA (Noise, Assessment, and Assist) and, in specific scenarios, achieving L4 autonomous driving technology. Therefore, obtaining highly accurate information on the tractor and trailer angles is essential for autonomous driving, preventing rollovers, and improving maneuverability and driving safety.

[0003] The mainstream existing technology uses LiDAR or camera vision detection. For example, LiDAR is installed in the rear and side-rear directions to detect the trailer angle, and the trailer pose is extracted through laser point cloud. Alternatively, a detection plate with multiple feature patterns is placed on the front side of the trailer, and a camera is placed on the rear side of the tractor to capture at least one feature pattern on the detection plate at any yaw angle. Summary of the Invention

[0004] The technical solution of this invention to solve the above-mentioned technical problems is to provide a method for detecting the angle between a tractor and a trailer based on a dual-antenna combined inertial navigation system, applicable to a vehicle system including a tractor and a trailer. The method includes the following steps: S1: Obtain the first heading angle of the tractor: Determine the position difference vector between the main antenna and the auxiliary antenna in the specified geographic coordinate system using a dual-antenna GNSS system; calculate the first coarse heading angle of the tractor based on the position difference vector; fuse the IMU data of the tractor with the first coarse heading angle, and output a high-precision first heading angle through a filtering algorithm; S2: Obtain the second heading angle of the trailer: Calculate the roll angle and pitch angle of the trailer using the accelerometer data; perform tilt compensation on the magnetometer data using the roll angle and pitch angle to obtain the magnetic field component on the horizontal plane; calculate the second heading angle of the trailer based on the compensated magnetic field component. S3: Calculate the angle between the tractor and the trailer: Subtract the second heading angle from the first heading angle to obtain the angle; when the heading angle is defined as the angle of clockwise rotation from geographic north to the direction of the vehicle's movement, and the North-East-Down (NED) coordinate system is used, the angle reflects the lateral connection angle of the tractor relative to the trailer.

[0005] Further, in step S1, when the specified geographic coordinate system is the North-East-Down (NED) coordinate system, the first coarse heading angle is calculated by the formula: φ=arctan2(ΔE,ΔN), where ΔE and ΔN are the components of the position difference vector in the east and north directions, respectively.

[0006] Furthermore, in step S1, the IMU data of the tractor vehicle is fused with the GNSS baseline vector information using a tight coupling or loose coupling method, and the attitude quaternion is corrected by an extended Kalman filter to output the first heading angle.

[0007] Furthermore, in step S2, the second heading angle is calculated using the formula: ,in, and This represents the magnetic field components on the horizontal surface after tilt compensation.

[0008] Furthermore, obtaining the trailer's second heading angle also includes dynamic compensation, which includes: S22a: Receive tractor heading data and lane curvature data transmitted by the tractor via the CAN bus; S22b: Based on the tractor's heading data and the lane curvature data, calculate the theoretical reference heading of the trailer; S22c: Using the theoretical reference heading as the observation benchmark, the zero bias of the gyroscope and / or the measurement deviation of the magnetometer in the trailer IMU are corrected online to obtain the second heading angle after dynamic compensation.

[0009] Furthermore, when the dual-antenna GNSS system signal of the tractor unit fails, the method further includes: S4: The tractor calculates and maintains its first heading angle output based on the gyroscope integration of the IMU and the wheel speed sensor data; S5: The trailer continuously uses the heading data sent by the tractor before the signal fails and the lane curvature data sent in real time to perform the dynamic compensation in order to maintain the accuracy of its second heading angle.

[0010] To address the aforementioned technical problems, this invention also proposes a detection system for implementing the method described above, comprising: The tractor unit includes a dual-antenna GNSS compact inertial navigation system mounted on the central axis of the tractor, used to output the first heading angle of the tractor; The trailer unit includes an inertial measurement unit containing a magnetometer mounted on the trailer for outputting a second heading angle of the trailer; The processing unit is communicatively connected to the tractor unit and the trailer unit, and is used to receive the first heading angle and the second heading angle, calculate the real-time angle between the tractor and the trailer, and perform the dynamic compensation step to correct the second heading angle.

[0011] The technical solution of this application achieves low-cost, high-precision, and high-reliability measurement of the tractor-trailer angle through an innovative architecture of "high-precision dual-antenna combined inertial navigation for tractor + low-cost IMU for trailer + dynamic collaborative compensation". This significantly improves the perception capability and driving safety of articulated vehicles in complex environments and has important engineering application value and market promotion prospects. Attached Figure Description

[0012] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the structures shown in these drawings without creative effort.

[0013] Figure 1 Diagram of the current integrated inertial navigation system scheme; Figure 2 This is a diagram of the upgraded combined inertial navigation scheme of the present invention; Figure 3 This is a schematic diagram of the 9-axle IMU scheme for trailers according to the present invention; Figure 4 This is an installation diagram of the combined inertial navigation system and IMU of the present invention; Figure 5 This is a flowchart illustrating the dynamic heading angle correction process of this invention. Figure 6 This is a flowchart illustrating the steps of a method for detecting the angle between a tractor and a trailer based on a dual-antenna combined inertial navigation system according to the present invention. Detailed Implementation

[0014] This invention proposes a method and system for detecting the angle between a tractor and a trailer based on a dual-antenna combined inertial navigation system. The aim is to design a technology based on high-precision tightly coupled dual-positioning antenna inertial navigation to achieve high-precision detection of the angle between the tractor and the trailer.

[0015] The method and system for detecting the angle between a tractor and a trailer based on a dual-antenna combined inertial navigation system, as proposed in this invention, will be described below in specific embodiments: Example 1: A method for detecting the angle between a tractor and a trailer based on a dual-antenna combined inertial navigation system is applied to vehicle systems including tractors and trailers, such as... Figure 6 As shown, the method includes the following steps: S1: Obtain the first heading angle of the tractor: Determine the position difference vector between the main antenna and the auxiliary antenna in the specified geographic coordinate system using a dual-antenna GNSS system; calculate the first coarse heading angle of the tractor based on the position difference vector; fuse the IMU data of the tractor with the first coarse heading angle, and output a high-precision first heading angle through a filtering algorithm; S2: Obtain the second heading angle of the trailer: Calculate the roll angle and pitch angle of the trailer using accelerometer data; perform tilt compensation on the magnetometer data using the roll angle and pitch angle to obtain the magnetic field component on the horizontal plane; calculate the second heading angle of the trailer based on the compensated magnetic field component. S3: Calculate the angle between the tractor and the trailer: Subtract the second heading angle from the first heading angle to obtain the angle; when the heading angle is defined as the angle of clockwise rotation from geographic north to the direction of the vehicle's movement, and the North-East-Down (NED) coordinate system is used, the angle reflects the lateral connection angle of the tractor relative to the trailer.

[0016] Further, in step S1, when the specified geographic coordinate system is the North-East-Down (NED) coordinate system, the first coarse heading angle is calculated by the formula: φ=arctan2(ΔE,ΔN), where ΔE and ΔN are the components of the position difference vector in the east and north directions, respectively.

[0017] Furthermore, in step S1, the IMU data of the tractor vehicle is fused with the GNSS baseline vector information using a tight coupling or loose coupling method, and the attitude quaternion is corrected by an extended Kalman filter to output the first heading angle.

[0018] Furthermore, in step S2, the second heading angle is calculated using the formula: ,in, and This represents the magnetic field components on the horizontal surface after tilt compensation.

[0019] Furthermore, obtaining the trailer's second heading angle also includes dynamic compensation, which includes: S22a: Receive tractor heading data and lane curvature data transmitted by the tractor via the CAN bus; S22b: Based on the tractor's heading data and the lane curvature data, calculate the theoretical reference heading of the trailer; S22c: Using the theoretical reference heading as the observation benchmark, the zero bias of the gyroscope and / or the measurement deviation of the magnetometer in the trailer IMU are corrected online to obtain the second heading angle after dynamic compensation.

[0020] Furthermore, when the dual-antenna GNSS system signal of the tractor unit fails, the method further includes: S4: The tractor calculates and maintains its first heading angle output based on the gyroscope integration of the IMU and the wheel speed sensor data; S5: The trailer continuously uses the heading data sent by the tractor before the signal fails and the lane curvature data sent in real time to perform the dynamic compensation in order to maintain the accuracy of its second heading angle.

[0021] Combined inertial navigation system for tractor-trailer combined assisted driving, such as Figure 1 As shown, the system employs dual- or tri-frequency GNSS supporting RTK, combined with a 6-axis IMU, and then acquires differential data through the vehicle's TBox mobile communication network to achieve high-precision positioning and integrated inertial navigation functions. Other systems integrate the integrated inertial navigation hardware into the domain controller.

[0022] The integrated inertial navigation system (INS) is installed on the centerline of the tractor unit. Because it uses a single-antenna GNSS, even with an IMU, it cannot measure the vehicle's heading angle in static conditions. Therefore, to achieve heading angle detection in both static and dynamic conditions with guaranteed accuracy, the single-antenna GNSS needs to be upgraded to a tri-frequency GNSS supporting RTK. Additionally, a private CAN bus is added to the MCU to communicate with the trailer's IMU. This achieves heading angle detection of the tractor unit with minimal additional cost compared to the original system design. The upgraded integrated INS solution is as follows: Figure 2 As shown.

[0023] For the trailer, if the dual-antenna combined inertial navigation system of the tractor unit is also used, the trailer's heading angle can be measured. Considering cost factors, the trailer can adopt a 9-axis IMU solution, adding a 3-axis magnetometer function to the 6-axis (3-axis accelerometer + 3-axis gyroscope) system. The 9-axis IMU can also measure both static and dynamic heading angles. Figure 3 .

[0024] The combined inertial navigation system (INS) and IMU are preferentially installed on the centerline of the tractor and trailer. If offset, an additional offset calculation algorithm is required. Figure 4 The installation scheme shown depicts a combined inertial navigation system (INS) and an inertial measurement unit (IMU) connected and communicating via the trailer's CAN bus. The INS calculates the tractor's heading angle, the IMU calculates the trailer's heading angle, and the included angle between the two can then be determined.

[0025] To ensure the accuracy of the combined inertial navigation and IMU, it is necessary to select GNSS and IMU chips that meet the performance requirements. Since IMU chips generally use MEMS technology, temperature drift compensation calibration is also required at the end of the production line.

[0026] For integrated inertial navigation systems, the technical requirements for GNSS and IMU chips are as follows: Table 1: ; The performance requirements for the IMU of the combined inertial navigation system are as follows: Table 2: ; For trailer IMUs, the performance indicators of their 6-axis inertial navigation systems are the same as those of the combined inertial navigation system, as shown in the table above. The performance requirements for 3-axis magnetometers are as follows. In addition, trailer IMUs also need to be fully compensated for temperature drift and magnetically calibrated.

[0027] Table 3: ; Method for calculating heading angle: The trailer-mounted IMU is equipped with a magnetometer. The yaw / heading angle of the IMU is calculated using a North-East-Down (NED) coordinate system based on the sensor configuration. This is based on attitude quaternions and uses Madgwick, Mahony, or Kalman filtering algorithms to avoid the gimbaling problem of Euler angles. It is assumed that the attitude quaternions from the body coordinate system to the geographic coordinate system (such as NED) have already been obtained through filtering algorithms. The formula for calculating the heading angle (Yaw) is as follows: , Algorithm for calculating heading angle using dual antennas on a tractor: Dual-Antenna GNSS / INS is currently the mainstream solution for high-precision, magneto-independent yaw calculation, widely used in autonomous driving, drones, ships, agricultural machinery, and other fields. Its core idea is to directly calculate the absolute yaw angle of the vehicle using the direction of the baseline vector between the two GNSS antennas in the geographic coordinate system. This is then fused with the IMU through tight or loose coupling to achieve high dynamic, interference-resistant, and drift-free yaw output. Assuming antenna A is the primary antenna and antenna B is the secondary antenna, and their baseline vectors are known constants in the body coordinate system... (Obtained through calibration). GNSS provides real-time high-precision positions of the two antennas in the local horizontal coordinate system (such as NED) (RTK required).

[0028] Calculate the representation of the baseline vector in the geographic coordinate system, and convert the position difference between the two antennas to the local horizontal coordinate system (taking NED as an example): , Note: Using RTK, baseline calculation can directly output centimeter-level accuracy. .

[0029] Calculate the yaw angle: In the NED coordinate system, the yaw angle (the angle from north to the forward direction of the vehicle) is: , Consider the baseline mounting attitude: If the baseline is not strictly horizontal (e.g., mounted on an inclined platform), pitch and roll compensation provided by the IMU is required. , Fusion algorithm with IMU: Using a dual-antenna GNSS heading system alone suffers from low update rates (1–20 Hz) and signal obstruction failures, therefore it must be integrated with an IMU. A tightly coupled + baseline observation method is adopted.

[0030] Instead of directly using the heading angle, the projection of the baseline vector onto the geographic system is used as the observation. The observation model is as follows: , in It is a rotation matrix constructed from the current pose quaternion q.

[0031] EKF directly corrects the attitude quaternion, aligning the predicted baseline with the GNSS measurement baseline.

[0032] Trailer 9-Axis IMU (including magnetometer) heading angle calculation: The trailer uses a 9-axis IMU (including magnetometer), which can directly calculate the heading angle using the Earth's magnetic field. This method is very accurate under static or low-dynamic conditions, but it is easily affected by surrounding ferromagnetic interference, therefore requiring calibration with both hard and soft iron magnets. Calculation steps: Data Acquisition: Acquire accelerometer data and magnetometer data .

[0033] Calculate tilt compensation: Use Roll (Φ) and Pitch (θ) calculated from acceleration to "straighten" the magnetometer data and eliminate errors caused by tilt.

[0034] Calculate heading: , Note: Here It is the projection component on the horizontal plane after tilt compensation; Dynamic errors can cause accelerometers to inaccurately estimate the direction of gravity during motions with large accelerations or angular velocities, leading to tilt compensation failure and consequently affecting magnetic heading calculations. In such cases, heading calculations rely primarily on gyroscope integration, which deteriorates rapidly due to drift. Therefore, a dynamic compensation method was designed. During dynamic driving, the tractor unit transmits its heading ψ via the CAN bus. tThe lane curvature identified by the intelligent driving system's perception system, or the trajectory curvature κ calculated from wheel speed in the absence of lane markings, is sent to the trailer in real time. The trailer utilizes ψ t The theoretical heading ψᵣ of the trailer is calculated using κ, and this is used as a reference to correct the gyro zero bias or magnetometer deviation of the trailer IMU online.

[0035] To ensure the real-time performance and accuracy of the trailer's heading angle, the trailer's IMU system needs to periodically execute... Figure 5 The flowchart shown dynamically corrects the heading angle.

[0036] Heading angle calculation: Taking a left turn by the tractor as an example, calculate the angle between it and the trailer. Define the coordinate system: such as... Figure 4 As shown, the NED coordinate system is as follows: X-axis (East): horizontal to the right. Y-axis (North): vertically upward. Z-axis (Ground): perpendicular to the paper and inward (usually omitted in the diagram according to the right-hand rule).

[0037] Determine the direction vector and heading angle: Trailer direction: consistent with the positive direction of the X-axis (east), rotate 90° clockwise from north (Y-axis) to reach east. Therefore, without considering accuracy error, the heading angle of the trailer calculated by the combined inertial navigation is equal to 90°.

[0038] Tractor direction: The tractor direction shown in the diagram is 45° clockwise relative to the trailer direction. Therefore, the tractor direction is from east and then 45° clockwise, i.e., southeast. Therefore, without considering IMU accuracy error, the calculated NED coordinate system, viewed clockwise from north, is: 90° + 45° = 135°.

[0039] Calculate the included angle: The included angle between the tractor and the trailer: This angle is the lateral deflection angle at the connection point of the two vehicles, also known as the connection angle, which is 135° - 90° = 45°.

[0040] The current technical solution employs a dual-antenna inertial navigation system (INS) + IMU (with RTK support). When the GNSS signal is stable, the heading angle accuracy can reach 0.1 degrees. The vehicle is equipped with wheel speed sensors, and even if GNSS data is lost for 30 seconds, the accuracy can still reach approximately 0.15 degrees. The trailer uses an IMU (with a magnetometer), which can achieve an accuracy of <±1 degree under static conditions. For dynamic environments, calibration data from the tractor unit is required to achieve higher accuracy.

[0041] Table 4: ; As shown in Table 4, the IMU (including magnetometer) + tractor data calibration scheme can achieve an accuracy within ±2° in different scenarios, which can basically meet the requirements. If higher accuracy is required, the trailer can also adopt a dual-antenna combined inertial navigation scheme.

[0042] For example, a tractor-trailer equipped with this system was traveling on a highway when it entered a tunnel approximately 3 kilometers long (where GNSS signal was completely lost). Before entering the tunnel, the system measured the tractor's heading angle at 45° (relative to north) based on dual-antenna GNSS, while the trailer's IMU measured a heading angle of 44.5°, with an included angle of 0.5°. After entering the tunnel, the tractor's inertial navigation system maintained its heading using gyroscopes and wheel speed sensors. When the trailer's IMU experienced a 5° deviation due to interference from the steel reinforcement within the tunnel, the system immediately activated a dynamic compensation algorithm, using the tractor's heading angle and lane curvature (κ=0.001 rad / m) for correction. Three minutes later, after exiting the tunnel, the system's included angle output remained within ±1.5°, while the uncompensated trailer IMU's heading angle had drifted by more than 10°. This example demonstrates the effectiveness and reliability of this solution under extreme conditions.

[0043] Example 2: A detection system for implementing the method of Embodiment 1 includes: The tractor unit includes a dual-antenna GNSS compact inertial navigation system mounted on the central axis of the tractor, used to output the first heading angle of the tractor; The trailer unit includes an inertial measurement unit containing a magnetometer mounted on the trailer for outputting a second heading angle of the trailer; The processing unit is communicatively connected to the tractor unit and the trailer unit, and is used to receive the first heading angle and the second heading angle, calculate the real-time angle between the tractor and the trailer, and perform the dynamic compensation step to correct the second heading angle.

[0044] The above description is merely a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A method for detecting the angle between a tractor and a trailer based on a dual-antenna combined inertial navigation system, characterized in that, The method includes the following steps: S1: Obtain the first heading angle of the tractor: Determine the position difference vector between the main antenna and the auxiliary antenna in the specified geographic coordinate system using a dual-antenna GNSS system; calculate the first coarse heading angle of the tractor based on the position difference vector; fuse the IMU data of the tractor with the first coarse heading angle, and output the first heading angle through a filtering algorithm; S2: Obtain the second heading angle of the trailer: Calculate the roll and pitch angles of the trailer using accelerometer data; use the roll and pitch angles to perform tilt compensation on the magnetometer data to obtain the magnetic field components on the horizontal plane; calculate the second heading angle of the trailer based on the compensated magnetic field components. S3: Calculate the angle between the tractor and the trailer: Subtract the second heading angle from the first heading angle to obtain the angle.

2. The method according to claim 1, characterized in that, In step S1, when the specified geographic coordinate system is the North-East-Down coordinate system, the first coarse heading angle is calculated using the formula: φ = arctan2(ΔE, ΔN), where ΔE and ΔN are the components of the position difference vector in the east and north directions, respectively.

3. The method according to claim 1, characterized in that, In step S1, the IMU data of the tractor vehicle is fused with the GNSS baseline vector information using a tight coupling or loose coupling method, and the attitude quaternion is corrected by an extended Kalman filter to output the first heading angle.

4. The method according to claim 1, characterized in that, In step S2, the second heading angle is calculated using the formula: ,in, and This represents the magnetic field components on the horizontal surface after tilt compensation.

5. The method according to claim 1, characterized in that, Obtaining the trailer's second heading angle also includes dynamic compensation, which includes: S22a: Receive tractor heading data and lane curvature data transmitted by the tractor via the CAN bus; S22b: Based on the tractor's heading data and the lane curvature data, calculate the theoretical reference heading of the trailer; S22c: Using the theoretical reference heading as the observation benchmark, the zero bias of the gyroscope and / or the measurement deviation of the magnetometer in the trailer IMU are corrected online to obtain the second heading angle after dynamic compensation.

6. The method according to claim 1, characterized in that, When the dual-antenna GNSS system signal of the tractor unit fails, the method further includes: S4: The tractor calculates and maintains its first heading angle output based on the gyroscope integration of the IMU and the wheel speed sensor data; S5: The trailer continuously uses the heading data sent by the tractor before the signal fails and the lane curvature data sent in real time to perform the dynamic compensation in order to maintain the accuracy of its second heading angle.

7. A detection system for implementing the method according to any one of claims 1 to 6, characterized in that, include: The tractor unit includes a dual-antenna GNSS compact inertial navigation system mounted on the central axis of the tractor, used to output the first heading angle of the tractor; The trailer unit includes an inertial measurement unit containing a magnetometer mounted on the trailer for outputting a second heading angle of the trailer; The processing unit is communicatively connected to the tractor unit and the trailer unit, and is used to receive the first heading angle and the second heading angle, calculate the real-time angle between the tractor and the trailer, and perform the dynamic compensation step to correct the second heading angle.

Images