Method, device, electronic device and storage medium for determining trailer driving state

Abstract

The present disclosure discloses a method, device, electronic device, and storage medium for determining the driving state of a trailer. The method includes: determining the measured values ​​of the center coordinates and the heading angle of the rear axle of a trailer towed by the tractor based on associated data from sensors installed on the tractor; determining a first driving state of the rear axle of the trailer using an extended Kalman filter algorithm based on the measured values ​​of the center coordinates and the heading angle of the rear axle of the trailer and the kinematic model of the trailer; determining whether the trailer is driving abnormally based on at least the first driving state of the rear axle of the trailer, the kinematic model of the trailer, and a set evaluation index; and triggering a trailer abnormality handling mechanism when it is determined that the trailer is driving abnormally. The present disclosure achieves the determination of the driving state of the trailer without installing relevant sensors on the trailer, providing a basis for ensuring the safe driving of the trailer, saving costs, and having strong practicality.

Description

Technical Field

[0001] The present disclosure relates to the field of autonomous driving technology, and in particular to a method, device, electronic device, and storage medium for determining a trailer driving state. Background Art

[0002] In unmanned logistics scenarios such as airports, factories, and indoor areas, autonomous vehicle trains typically consist of an autonomous tractor with one or more trailers or trolleys attached. Due to limitations in sensor performance, cost, mechanical structure, and power supply, the trailers or trolleys are typically not equipped with sensors for positioning or inertial navigation, making it impossible to directly obtain the trailer's or trolley's driving status (including speed, heading, and position). However, obtaining the trailer's or trolley's driving status is essential for ensuring its safe operation.

[0003] Therefore, when no relevant sensors are installed on the trailer or trailer, how to obtain the driving status of the trailer or trailer is a key technical problem that needs to be solved at present. Summary of the Invention

[0004] In order to solve the above technical problems or at least partially solve the above technical problems, the embodiments of the present disclosure provide a method, device, electronic device and storage medium for determining the driving status of a trailer. Without installing relevant sensors on the trailer, the driving status of the trailer can be determined, which provides a basis for ensuring the safe driving of the trailer, saves costs and has strong practicality.

[0005] In a first aspect, an embodiment of the present disclosure provides a method for determining a trailer driving state, the method comprising:

[0006] Determining a measured value of the center coordinate of the rear axle of a trailer towed by the tractor and a measured value of the heading angle based on associated data from sensors installed on the tractor;

[0007] Determining a first driving state of the trailer rear axle using an extended Kalman filter (EKF) algorithm based on the measured values ​​of the center coordinates of the trailer rear axle and the measured value of the heading angle, as well as the kinematic model of the trailer;

[0008] determining whether the trailer is driving abnormally based on at least a first driving state of the trailer rear axle, a kinematic model of the trailer, and a set evaluation index;

[0009] When it is determined that the trailer is running abnormally, a trailer abnormality processing mechanism is triggered.

[0010] In a second aspect, an embodiment of the present disclosure further provides a device for determining a trailer driving state, the device comprising:

[0011] a first determining module, configured to determine a measured value of a center coordinate of a rear axle of a trailer towed by the tractor and a measured value of a heading angle based on associated data of sensors installed on the tractor;

[0012] a second determining module, configured to determine a first driving state of the rear axle of the trailer using an extended Kalman filter algorithm based on the measured values ​​of the center coordinates of the rear axle of the trailer and the measured value of the heading angle, as well as the kinematic model of the trailer;

[0013] a third determining module, configured to determine whether the trailer is driving abnormally based on at least the first driving state of the rear axle of the trailer, the kinematic model of the trailer, and a set evaluation index;

[0014] The processing module is used to trigger a trailer abnormality processing mechanism when it is determined that the trailer is running abnormally.

[0015] In a third aspect, an embodiment of the present disclosure further provides an electronic device, comprising: one or more processors; a storage device for storing one or more programs; and when the one or more programs are executed by the one or more processors, the one or more processors implement the method for determining the trailer driving state as described above.

[0016] In a fourth aspect, an embodiment of the present disclosure further provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the method for determining the driving state of a trailer as described above.

[0017] The disclosed embodiment provides a method for determining the driving state of a trailer, which achieves the determination of the driving state of the trailer without installing relevant sensors on the trailer, thereby providing a basis for ensuring the safe driving of the trailer, saving costs, and having strong practicality. Specifically, based on the associated data of the sensors installed on the tractor, the measured values ​​of the center coordinates of the rear axle of the trailer and the measured values ​​of the heading angle are determined, and then, based on the measured values ​​of the center coordinates of the rear axle of the trailer and the measured values ​​of the heading angle, as well as the kinematic model of the trailer, the first driving state of the rear axle of the trailer is determined by an extended Kalman filter algorithm, and whether the trailer is driving abnormally is determined based on at least the first driving state of the rear axle of the trailer, the kinematic model of the trailer, and the set evaluation index; when it is determined that the trailer is driving abnormally, a trailer abnormality handling mechanism is triggered. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] The above and other features, advantages, and aspects of the various embodiments of the present disclosure will become more apparent with reference to the following detailed description in conjunction with the accompanying drawings. Throughout the drawings, the same or similar reference numerals represent the same or similar elements. It should be understood that the drawings are schematic and that the originals and elements are not necessarily drawn to scale.

[0019] Figure 1 This is a flow chart of a method for determining a trailer driving state in an embodiment of the present disclosure;

[0020] Figure 2 is a schematic diagram of a kinematic model of a full trailer in an embodiment of the present disclosure;

[0021] Figure 3 is a schematic diagram of a kinematic model of a semi-trailer in an embodiment of the present disclosure;

[0022] Figure 4 is a schematic diagram of a preset baffle in an embodiment of the present disclosure;

[0023] Figure 5 A schematic diagram of determining the measured values ​​of the center coordinates of the trailer rear axle and the measured values ​​of the heading angle according to the corresponding point cloud on the preset baffle in an embodiment of the present disclosure;

[0024] Figure 6 A schematic diagram of a flow chart for determining the driving state of a full trailer in an embodiment of the present disclosure;

[0025] Figure 7 A schematic diagram of a flow chart for determining the driving state of a semi-trailer in an embodiment of the present disclosure;

[0026] Figure 8 Schematic diagram of the structure of a device for determining the driving state of a trailer according to an embodiment of the present disclosure;

[0027] Figure 9 Schematic diagram of the structure of an electronic device in an embodiment of the present disclosure. DETAILED DESCRIPTION

[0028] The following describes embodiments of the present disclosure in more detail with reference to the accompanying drawings. Although certain embodiments of the present disclosure are shown in the accompanying drawings, it should be understood that the present disclosure can be implemented in various forms and should not be construed as limited to the embodiments described herein. Rather, these embodiments are provided to provide a more thorough and complete understanding of the present disclosure. It should be understood that the drawings and embodiments of the present disclosure are for illustrative purposes only and are not intended to limit the scope of protection of the present disclosure.

[0029] It should be noted that the concepts of "first" and "second" mentioned in this disclosure are only used to distinguish different devices, modules or units, and are not used to limit the order or interdependence of the functions performed by these devices, modules or units.

[0030] The names of the messages or information exchanged between multiple devices in the embodiments of the present disclosure are only used for illustrative purposes and are not used to limit the scope of these messages or information.

[0031] Typically, due to limitations in sensor performance, cost, mechanical structure, and power supply, sensors with positioning or inertial navigation functions are not installed on the trailer (trailer refers to a vehicle including a body structure) or trailer (trailer refers to a vehicle including only a chassis but not a body structure) towed by a tractor, making it impossible to directly obtain the driving status of the trailer or trailer (including speed, heading, position, etc.).

[0032] In response to the above problems, an embodiment of the present disclosure provides a method for determining the driving status of a trailer, which realizes the determination of the driving status of the trailer without installing relevant sensors on the trailer, provides a basis for ensuring the safe driving of the trailer, saves costs, and has strong practicality.

[0033] It can be understood that the method provided in the embodiment of the present disclosure is also applicable to determining the driving status of a trailer.

[0034] Figure 1 This is a flow chart of a method for determining the driving state of a trailer according to an embodiment of the present disclosure. This method can be executed by a device for determining the driving state of a trailer, which can be implemented in software and / or hardware, and can be configured in an electronic device. Figure 1 As shown, the method may specifically include the following steps:

[0035] Step 110: Determine the measured values ​​of the center coordinates of the rear axle of the trailer towed by the tractor and the measured values ​​of the heading angle based on the associated data of the sensors installed on the tractor.

[0036] Optionally, the measured values ​​of the center coordinates of the rear axle of the trailer towed by the tractor and the measured values ​​of the heading angle are determined based on image data of the trailer captured by a camera installed on the tractor; or the measured values ​​of the center coordinates of the rear axle of the trailer towed by the tractor are determined based on scanning data of the trailer captured by a lidar installed on the tractor.

[0037] The following is further explained by taking as an example the measurement values ​​of the center coordinates of the rear axle and the heading angle of the trailer towed by the tractor based on the scanning data of the trailer by the laser radar installed on the tractor.

[0038] In one embodiment, the measured values ​​of the center coordinates of the rear axle of the trailer towed by the tractor and the measured values ​​of the heading angle can be determined by using the kinematic model of the trailer and the scanning data of the trailer by the laser radar installed on the tractor. The trailer towed by the tractor includes a full trailer and a semi-trailer. Figure 2 The schematic diagram of the kinematic model of a full trailer shown in FIG. Figure 3 A schematic diagram of the kinematic model of a semi-trailer is shown.

[0039] exist Figure 2In the equation, θ0, θ1, and θ2 represent the heading angles of the tractor 320, the front axle 312, and the rear axle 311 of the trailer, respectively; ω0, ω1, and ω2 represent the rates of change of the heading angles of the tractor 320, the front axle 312, and the rear axle 311 of the trailer, respectively. The angle between the tractor 320 and the front axle 312 of the trailer is The angle between the front axle 312 and the rear axle 311 of the full trailer is l fr0 and l fr1 They represent the wheelbases of the tractor 320 and the trailer 310 (i.e., the lengths marked by reference numeral 313), h Indicates the distance from the center of the tractor's rear axle D to the connection point A, l b It represents the distance from the center B of the front axle 312 of the full trailer (i.e. the position marked by the number 314) to the connection point A (i.e. the position marked by the number 321). f is the front wheel deflection angle of the tractor 320. v0 is the velocity of the tractor's rear axle center C. v1 is the velocity of the trailer's front axle center B, and v2 is the velocity of the trailer's rear axle center C. (x0, y0) are the coordinates of the tractor's rear axle center C, (x1, y1) are the coordinates of the trailer's front axle center B, and (x2, y2) are the coordinates of the trailer's rear axle center C. The component marked with reference numeral 330 may be referred to as a handle or connecting rod.

[0040] exist Figure 3 In the equation, θ1 and θ2 represent the heading angles of the rear axles of the tractor and semi-trailer, respectively. The angle between the tractor and the semi-trailer is l0 is the wheelbase of the tractor, l t is the distance from the tractor's rear axle center D to the connection point A, l1 is the distance from the semitrailer's rear axle center C to the connection point A. δ is the tractor's front wheel deflection angle. v1 is the velocity of the tractor's rear axle center D. v2 is the velocity of the semitrailer's rear axle center C. (x1, y1) are the coordinates of the tractor's rear axle center D, (x2, y2) are the coordinates of the semitrailer's rear axle center C, and δ is the tractor's front wheel deflection angle.

[0041] Based on the kinematic model of the trailer, the method of determining the measured values ​​of the center coordinates of the rear axle of the trailer towed by the tractor and the measured values ​​of the heading angle based on the scanning data of the trailer by the laser radar installed on the tractor includes:

[0042] Determine the corresponding point cloud of the laser emitted by the laser radar within a set period on the preset baffle of the trailer, the laser radar is installed on the roof of the tractor, and the preset baffle is vertically installed on the edge of the trailer close to the tractor. For example, refer to Figure 4A schematic diagram of a preset baffle is shown, wherein the preset baffle 410 is vertically mounted on an edge 421 of a trailer 420 close to a tractor, and the trailer 420 is connected to the tractor via a mop 422, so the edge 421 is the edge close to the tractor.

[0043] Based on the corresponding point cloud, the associated data of the laser radar, and the positional relationship between the preset baffle and the center of the rear axle of the trailer, the measured value of the coordinates of the center of the rear axle of the trailer and the measured value of the heading angle are determined. Specifically, a straight line parallel to the preset baffle is fitted according to the corresponding point cloud, and the first angle between the straight line and the rear axle of the tractor is determined; based on the first angle, the associated data of the laser radar, and the positional relationship between the preset baffle and the center of the rear axle of the trailer, the measured value of the coordinates of the center of the rear axle of the trailer and the measured value of the heading angle are determined. For example, refer to Figure 5 The diagram shows a method for determining the measured coordinates and heading angle of the trailer's rear axle center based on a corresponding point cloud on a preset baffle. Line 510 is fitted based on the corresponding point cloud on the preset baffle. A first angle α1 is defined between this line and the rear axle 521 of the tractor 520. Based on the positional and geometric relationship between the preset baffle and the trailer's rear axle center B, it can be determined that a second angle α2 between the tractor's central axis 522 and the central axis 531 of the trailer 530 is the same as the first angle α1. Therefore, the heading angle of the trailer's rear axle center B can be determined. The coordinates of point O can be determined based on the coordinate information of the corresponding point cloud on the preset baffle. Combined with the wheelbase 11 of the trailer 530, the coordinates of the trailer's rear axle center B can be determined. Thus, the measured coordinates and heading angle of the trailer's rear axle center are obtained.

[0044] It should be noted that Figure 5 The explanation is given using a semi-trailer as an example. The calculation process for a full trailer is the same as that for a semi-trailer.

[0045] Step 120: Determine a first driving state of the rear axle of the trailer using an extended Kalman filter algorithm based on the measured values ​​of the center coordinates of the rear axle of the trailer and the measured values ​​of the heading angle, as well as the kinematic model of the trailer.

[0046] Exemplarily, determining the first driving state of the trailer rear axle by using an extended Kalman filter algorithm based on the measured values ​​of the center coordinates of the trailer rear axle and the measured value of the heading angle, as well as the kinematic model of the trailer, includes:

[0047] The measured value of the center coordinate of the rear axle of the trailer, the speed, the measured value of the heading angle, and the rate of change of the heading angle are determined as a state vector; the measured value of the center coordinate of the rear axle of the trailer and the measured value of the heading angle are determined as an observation vector; a state equation of an extended Kalman filter algorithm is constructed based on the state vector and a relationship determined based on the kinematic model of the trailer; an observation equation of an extended Kalman filter algorithm is constructed based on the observation vector and the state equation; and a first driving state of the rear axle of the trailer is determined based on the state equation and the observation equation.

[0048] Specifically, for full trailer, refer to Figure 2 The schematic diagram of the kinematic model of a full trailer is shown. Furthermore, the kinematic relationship of the rear axle of the trailer can be expressed using the following formula (1):

[0049]

[0050] Where θ2 represents the heading angle of the rear axle of the trailer, ω2 represents the rate of change of the heading angle of the rear axle of the trailer, v2 represents the velocity of the rear axle center C of the trailer, and (x2, y2) represents the coordinates of the rear axle center C of the trailer.

[0051] Discretize formula (1) to get formula (2):

[0052]

[0053] In particular, when the vehicle is traveling in a straight line, the rate of change of the heading angle of the rear axle of the full trailer is ω2 = 0. In this case, the posture equation of the rear axle of the full trailer is as shown in the following expression (3):

[0054]

[0055] Similarly, the kinematic relationship of the front axle center of the full trailer can be expressed using the following formula (4):

[0056]

[0057] Where θ1 represents the heading angle of the front axle of the trailer, ω1 represents the rate of change of the heading angle of the front axle of the trailer, v1 is the speed of the center B of the front axle of the trailer, v2 is the speed of the center C of the rear axle of the trailer, and (x1, y1) are the coordinates of the center B of the front axle of the trailer.

[0058] Discretize equation (4) to get equation (5):

[0059]

[0060] Similarly, when the vehicle is traveling in a straight line, ω1 = 0. In this case, the posture equation of the front axle of the full trailer is as shown in the following expression (6):

[0061]

[0062] It should be noted that the semi-trailer does not have a front axle structure, and the kinematic relationship of the rear axle of the semi-trailer is the same as that of the rear axle of the full-trailer, that is, the kinematic relationship of the rear axle of the semi-trailer can be expressed using the above formulas (1)-(3).

[0063] Based on the above kinematic relationship, the extended Kalman filter algorithm is used to estimate the driving state of the trailer's rear axle, thereby obtaining a first driving state of the trailer's rear axle. Since the kinematic relationship of the rear axle of a full trailer is the same as that of the rear axle of a semi-trailer, the extended Kalman filter algorithm uses the same processing logic for both full and semi-trailer trailers, resulting in the same results.

[0064] Specifically, let the state vector be X2 = [x2 y2 θ2 v2 ω2] and the observation vector be Y2 = [x2 y2 θ2], then the state equation is as shown in the following expression (7), and the observation equation is as shown in the following expression (8):

[0065] X 2,k =f2(X 2,k )+Γ w2 w 2,k-1 (7)

[0066] Y 2,k =H2X 2,k +v 2,k (8)

[0067] When the vehicle is traveling in a straight line, f2(X 2,k ) is the above expression (3), when the vehicle is not traveling in a straight line, f2(X 2,k ) is the above expression (2). Γ w2 =I5, indicating the identity matrix with a dimension of 5×5. w 2,k-1 represents process noise, v 2,k Represents observation noise. Assuming that process noise and observation noise satisfy Gaussian zero-mean distribution and are independent of each other, we have the following relationship (9):

[0068]

[0069] Where Q2 represents the symmetric covariance matrix of process noise, and R2 represents the symmetric covariance matrix of observation noise.

[0070] Assume that the initial value of the state vector X2(0) obeys Gaussian distribution, and its initial variance matrix is The process of state estimation based on the extended Kalman filter algorithm is as follows:

[0071] 1) Based on the state equation, perform state prediction:

[0072]

[0073] Where, Represents the estimated value of the state vector of the previous cycle. It should be noted that Different modes are selected depending on whether the vehicle is traveling in a straight line.

[0074] 2) Get the predicted observation vector:

[0075] Y 2,k|k-1 =H2X 2,k|k-1 (11)

[0076] 3) Get the predicted covariance matrix:

[0077]

[0078] In formula (12),

[0079] 4) Get the Kalman filter gain:

[0080]

[0081] 5) Get the estimated value of the state vector:

[0082]

[0083] Where Y 2,k is the observation value of the observation vector at time k.

[0084] 6) Update the covariance matrix of the state vector:

[0085]

[0086] The estimated value of the state vector can be obtained from the above extended Kalman filter process. Further, the estimated value of the center coordinate of the rear axle of the full trailer can be obtained from it. and Estimated heading angle Estimated driving speed and an estimate of the rate of change of the heading angle In summary, the first driving state at least includes an estimated value of the center coordinate of the trailer rear axle, an estimated value of the heading angle, an estimated value of the driving speed, and an estimated value of the heading angle change rate.

[0087] Step 130: Determine whether the trailer is driving abnormally based on at least the first driving state of the rear axle of the trailer, the kinematic model of the trailer, and a set evaluation index.

[0088] Exemplarily, the determining whether the trailer is driving abnormally based on at least the first driving state of the rear axle of the trailer, the kinematic model of the trailer, and a set evaluation index includes:

[0089] The actual steering curvature of the trailer is determined based on an estimated value of the center speed of the trailer's rear axle and an estimated value of the rate of change of the heading angle; the nominal steering curvature of the trailer is determined based on at least the estimated value of the center heading angle of the trailer's rear axle and a kinematic model of the trailer; the real-time value of the set evaluation index is determined based on the actual steering curvature and the nominal steering curvature; and whether the trailer is driving abnormally is determined based on the real-time value of the set evaluation index and a set threshold.

[0090] The steering curvature is an indicator that characterizes the driving state, and the nominal steering curvature is a value that conforms to the kinematic model and can be considered the expected value of the steering curvature. If the actual steering curvature of the trailer does not match the nominal steering curvature, it is considered that the trailer's driving state may be abnormal. For example, when the real-time value of a set evaluation indicator determined based on the actual steering curvature and the nominal steering curvature is greater than or equal to a set threshold, it indicates that the trailer is driving abnormally. When the real-time value of the set evaluation indicator is less than the set threshold, it indicates that the trailer is driving normally. For another example, if the real-time value of the set evaluation indicator is greater than or equal to the set threshold for a set duration, it indicates that the trailer is driving abnormally.

[0091] For example, the real-time value of the set evaluation index can be determined based on the following formula (16):

[0092]

[0093] in, Indicates setting evaluation indicators, represents the actual turning curvature, represents the nominal steering curvature.

[0094] In some embodiments, the actual turning curvature of the trailer can be determined specifically according to the following formula (17):

[0095]

[0096] in, Indicates the estimated value of the rate of change of the rear axle heading angle of the trailer (including full trailer and semi-trailer), It indicates the estimated value of the rear axle center speed of the trailer (including full trailer and semi-trailer). Indicates the actual steering curvature.

[0097] In some embodiments, due to the different kinematic models of full trailers and semi-trailers, the nominal turning curvature is calculated differently for full trailers and semi-trailers. Specifically, when the trailer is a semi-trailer, determining the nominal turning curvature of the trailer based on at least the estimated value of the center heading angle of the trailer's rear axle and the trailer's kinematic model includes:

[0098] The nominal turning curvature of the trailer is determined based on an estimated value of the heading angle of the trailer's rear axle center, a measured value of the tractor's heading angle, and a distance from the trailer's rear axle center to a hitch point, where the tractor connects to the trailer. The distance from the trailer's rear axle center to the hitch point is determined based on a kinematic model of the trailer.

[0099] Optionally, the nominal turning curvature of the semi-trailer is determined based on the following formula (18):

[0100]

[0101] in, represents the nominal steering curvature, θ1 represents the measured value of the heading angle of the tractor, which can be referred to as Figure 3 The schematic diagram of the kinematic model of the semi-trailer shown in Represents the estimated value of the heading angle of the rear axle center of the trailer, l1 represents the distance from the rear axle center of the trailer to the connection point, that is, Figure 3 The distance from point C to point A is shown.

[0102] When the trailer is a full trailer, determining the nominal turning curvature of the trailer based on at least an estimated value of the center heading angle of the trailer's rear axle and a kinematic model of the trailer includes:

[0103] The nominal turning curvature of the trailer is determined based on an estimated value of the heading angle of the center of the rear axle of the trailer, an estimated value of the heading angle of the center of the front axle of the trailer (the estimated value is determined by an extended Kalman filter algorithm, which will be introduced later), and the wheelbase of the trailer, wherein the wheelbase of the trailer is determined based on a kinematic model of the trailer.

[0104] Optionally, the nominal turning curvature of the full trailer is determined based on the following formula (19):

[0105]

[0106] in, represents the nominal steering curvature, represents the estimated value of the heading angle of the center of the trailer front axle, Represents the estimated value of the heading angle of the trailer rear axle center, l fr1Indicates the wheelbase of the trailer, which can be used as a reference for Figure 2 Schematic diagram of the kinematic model of the full trailer shown.

[0107] Furthermore, when the trailer is a full trailer, the method further includes:

[0108] The observed values ​​of the center coordinates of the front axle of the trailer and the observed values ​​of the heading angle are determined based on the estimated values ​​of the center coordinates of the rear axle of the trailer and the estimated value of the heading angle, the coordinates and heading angle of the tractor, and the kinematic model of the trailer; and the second driving state of the front axle of the trailer is determined by an extended Kalman filter algorithm based on the observed values ​​of the center coordinates of the front axle of the trailer and the observed values ​​of the heading angle, and the kinematic model of the trailer.

[0109] In summary, due to the limitation of sensor configuration, it is difficult to directly obtain the coordinates of the center of the front axle of the trailer and its heading angle. To address this problem, in the embodiment of the present disclosure, the estimated value of the coordinates of the center of the rear axle of the trailer is obtained by estimating the driving state of the rear axle of the full trailer through the extended Kalman filter algorithm. and and the estimated heading angle of the trailer's rear axle On this basis, according to the wheelbase of the trailer, combined with Figure 2 The kinematic model of the full trailer shown can obtain the observed values ​​of the center coordinates of the trailer's front axle.

[0110] Specifically, determining the observed values ​​of the center coordinates of the front axle of the trailer and the observed value of the heading angle based on the estimated values ​​of the center coordinates of the rear axle of the trailer and the estimated value of the heading angle, the coordinates and heading angle of the tractor, and the kinematic model of the trailer includes:

[0111] The observed value of the center coordinate of the front axle of the trailer is determined according to the estimated value of the center coordinate of the rear axle of the trailer, the estimated value of the heading angle, and the wheelbase of the trailer.

[0112] Optionally, the observed value of the center coordinate of the front axle of the trailer is determined based on the following formula (20):

[0113]

[0114] Among them, (x1, y1) is the observed value of the center coordinate of the front axle of the full trailer, and is the estimated value of the center coordinate of the trailer rear axle, is the estimated value of the heading angle of the trailer rear axle, l fr1 It is the wheelbase of the trailer.

[0115] Furthermore, the coordinates of the connection point are determined based on the coordinates and heading angle of the tractor and the distance from the center of the rear axle of the tractor to the connection point, where the tractor is connected to the trailer; and the observed value of the heading angle of the center of the front axle of the trailer is determined based on the observed value of the coordinates of the center of the front axle of the trailer, the coordinates of the connection point, the measured value of the coordinates of the center of the rear axle of the trailer, the estimated value of the heading angle of the center of the rear axle of the trailer, and the kinematic model of the trailer; wherein the wheelbase of the trailer and the distance from the center of the rear axle of the tractor to the connection point are determined based on the kinematic model of the trailer.

[0116] Optionally, the connection point (i.e. Figure 2 The coordinates of point A in :

[0117]

[0118] Among them, (x A ,y A ) is the coordinate of the connection point A, (x0, y0) is the coordinate of the tractor, θ0 is the heading angle of the tractor, l h It is the distance from the center of the rear axle of the tractor to the connection point A.

[0119] In some embodiments, determining the observed value of the trailer front axle center heading angle based on the observed value of the trailer front axle center coordinate, the coordinates of the coupling point, the measured value of the trailer rear axle center coordinate, the estimated value of the trailer rear axle center heading angle, and the trailer kinematic model includes:

[0120] A first vector from the center of the trailer rear axle to the center of the trailer front axle is determined based on the measured values ​​of the center coordinates of the trailer rear axle and the observed values ​​of the center coordinates of the trailer front axle; a second vector from the center of the trailer front axle to the connection point is determined based on the observed values ​​of the center coordinates of the trailer front axle and the coordinates of the connection point; a cross product operation is performed on the first vector and the second vector to obtain the angle between the trailer front axle and the trailer rear axle; and an observed value of the heading angle of the trailer front axle center is determined based on the angle between the trailer front axle and the trailer rear axle, an estimated value of the heading angle of the trailer rear axle center, and a preset angle relationship, wherein the preset angle relationship is determined based on a kinematic model of the trailer.

[0121] Specifically, the first vector from the trailer rear axle center C to the trailer front axle center B can be expressed as The second vector from the center of the trailer front axle B to the connection point A can be expressed as According to the vector cross product relationship (as shown in the following formula (22)), the angle between the front axle and the rear axle of the trailer can be obtained:

[0122]

[0123] The observed value of the heading angle of the trailer front axle center is given by the following formula (23):

[0124]

[0125] Among them, θ1 is the observed value of the heading angle of the front axle center of the trailer, is the estimated value of the heading angle of the trailer rear axle center, It is the angle between the front axle of the trailer and the rear axle of the trailer.

[0126] At this point, when the front axle posture of the trailer cannot be directly detected, the observation values ​​of the center coordinates and the heading angle of the front axle of the trailer are obtained.

[0127] Furthermore, in order to obtain an estimated value of the center coordinates of the trailer front axle and an estimated value of the heading angle, it is necessary to estimate the driving state of the trailer front axle using the extended Kalman filter algorithm based on the observed values ​​of the center coordinates of the trailer front axle and the observed values ​​of the heading angle and the kinematic relationship of the trailer front axle as shown in Expressions (4)-(6), thereby obtaining a second driving state of the trailer front axle. The process of estimating the driving state of the trailer front axle using the extended Kalman filter algorithm is similar to the process of estimating the driving state of the trailer rear axle using the extended Kalman filter algorithm.

[0128] Specifically, let the state vector be X1 = [x1 y1 θ1 v1 ω1] and the observation vector be Y1 = [x1 y1 θ1], then the state equation is as shown in the following expression (24), and the observation equation is as shown in the following expression (25):

[0129] X 1,k =f1(X 1,k )+Γ w1 w 1,k-1 (twenty four)

[0130] Y 1,k =H1X 1,k +v 1,k (25)

[0131] When the vehicle is traveling in a straight line, f1(X 1,k ) is the above expression (6), when the vehicle is not traveling in a straight line, f1(X 1,k ) is the above expression (5). Γ w1 =I5, indicating the identity matrix with a dimension of 5×5. w 1,k-1 represents process noise, v 1,kRepresents observation noise. Assuming that process noise and observation noise satisfy Gaussian zero-mean distribution and are independent of each other, we have the following relationship (26)

[0132]

[0133] Where Q1 represents the symmetric covariance matrix of process noise, and R1 represents the symmetric covariance matrix of observation noise.

[0134] Assume that the initial value of the state vector X1(0) obeys Gaussian distribution, and its initial variance matrix is The process of state estimation based on the extended Kalman filter algorithm is as follows:

[0135] 1) Based on the state equation, perform state prediction:

[0136]

[0137] Where, Represents the estimated value of the state vector of the previous cycle. It should be noted that Different modes are selected depending on whether the vehicle is traveling in a straight line.

[0138] 2) Get the predicted observation vector:

[0139] Y 1,k|k-1 =H1X 1,k|k-1 (28)

[0140] 3) Get the predicted covariance matrix:

[0141]

[0142] In formula (29),

[0143] 4) Get the Kalman filter gain:

[0144]

[0145] 5) Get the estimated value of the state vector:

[0146]

[0147] Where Y 1,k is the observation value of the observation vector at time k.

[0148] 6) Update the covariance matrix of the state vector:

[0149]

[0150] The estimated value of the state vector of the front axle of the trailer can be obtained by the above extended Kalman filter process. Further, the estimated value of the center coordinate of the front axle of the full trailer can be obtained from it. and Estimated heading angle Estimated driving speed and an estimate of the rate of change of the heading angle Generally speaking, the second driving state at least includes an estimated value of the heading angle of the center of the front axle of the trailer.

[0151] Step 140: When it is determined that the trailer is running abnormally, trigger a trailer abnormality handling mechanism.

[0152] Among them, the trailer abnormality handling mechanism includes but is not limited to: vehicle deceleration, roadside parking, emergency braking, etc.

[0153] Specifically, to set evaluation indicators Taking the absolute value of the difference between the actual steering curvature and the nominal steering curvature as an example, according to a large number of real vehicle data statistics, when the trailer is in normal driving state, The value will not exceed 0.2. When the trailer is in abnormal condition, The value is usually above 0.4. Therefore, a threshold value ρ can be set to trigger the abnormal driving state of the trailer. t1 Determined to be 0.4, that is, ρ t1 =0.4.

[0154] if It is judged as an E1 level error, and the trailer's driving state is considered abnormal. At this time, the vehicle turns on the double flashes to warn, and the vehicle reduces to a lower speed.

[0155] if It is judged as an E2 level error, and the trailer's driving state is considered to be seriously abnormal. At this time, the vehicle pulls over and waits for manual intervention.

[0156] Among them, the threshold value ρ for judging the abnormal driving state of the trailer is t1 It can be adjusted based on the actual vehicle data obtained from different trailers.

[0157] It should be noted that if a tractor tows multiple trailers, the trailer referred to in the disclosed embodiments refers to the first trailer towed by the tractor. When the driving state of another trailer (e.g., the third trailer) is abnormal, the driving state of the first trailer will also be abnormal. In other words, an abnormal driving state of any trailer towed by the tractor will cause an abnormal driving state of the first trailer. In other words, when it is determined that the driving state of the first trailer is abnormal, it may be due to the abnormal driving state of the first trailer itself, or it may be caused by the abnormal driving state of another trailer. This is determined by the interaction forces between the different trailers due to the physical connection relationship between the different trailers.

[0158] The present embodiment provides a method for determining the driving state of a trailer, which fully utilizes the sensing capabilities of sensors carried by a tractor, and estimates the motion state of the trailer based on partial detection information of the trailer. Based on the trailer motion state estimation result, an abnormal evaluation index for the trailer driving trajectory is designed, and abnormal monitoring of the trailer driving trajectory is achieved, thereby better distinguishing between abnormal and normal states. Based on the abnormal monitoring results of the trailer driving trajectory, an abnormality handling mechanism is established to ensure the driving safety of the vehicle. Especially in unmanned logistics application scenarios, the disclosed solution has strong practicality.

[0159] Based on the above embodiments, in summary, refer to Figure 6 The flowchart shown is a schematic diagram of determining the driving state of a full trailer, the method specifically comprising:

[0160] Step 610: Establish a kinematic model of the full trailer.

[0161] Step 620: Determine the measured values ​​of the center coordinates of the rear axle of the trailer and the measured values ​​of the heading angle based on the sensors carried by the tractor.

[0162] Step 630: Determine a first driving state of the trailer rear axle through an extended Kalman filter based on the measured values ​​of the trailer rear axle center coordinates and the measured values ​​of the heading angle, where the first driving state at least includes an estimated value of the trailer rear axle center coordinates and an estimated value of the heading angle.

[0163] Step 640: Determine the observed value of the center coordinate of the front axle of the trailer and the observed value of the heading angle based on the estimated value of the center coordinate of the rear axle of the trailer and the estimated value of the heading angle.

[0164] Step 650: Determine a second driving state of the front axle of the trailer using an extended Kalman filter algorithm based on the observed values ​​of the center coordinates of the front axle of the trailer and the observed values ​​of the heading angle.

[0165] Step 660: Calculate the trailer driving state abnormality evaluation index.

[0166] Step 670: Mechanism for judging and handling abnormality of trailer driving state.

[0167] Based on the above embodiments, in summary, refer to Figure 7 The flowchart shown is a schematic diagram of determining the driving state of a semi-trailer, the method specifically comprising:

[0168] Step 710: Establish a kinematic model of the semi-trailer.

[0169] Step 720: Determine the measured values ​​of the center coordinates of the rear axle of the trailer and the measured values ​​of the heading angle based on the sensors carried by the tractor.

[0170] Step 730: Determine a first driving state of the trailer rear axle through an extended Kalman filter based on the measured values ​​of the center coordinates of the trailer rear axle and the measured values ​​of the heading angle.

[0171] Step 740: Calculate the trailer driving state abnormality evaluation index.

[0172] Step 750: Mechanism for judging and handling abnormality of trailer driving state.

[0173] Figure 8 FIG. 1 is a schematic diagram of a structure of a device for determining the driving state of a trailer according to an embodiment of the present disclosure. Figure 8 As shown, the device includes: a first determination module 810, a second determination module 820, a third determination module 830 and a processing module 840.

[0174] Among them, the first determination module 810 is used to determine the measured values ​​of the center coordinates of the rear axle of the trailer towed by the tractor and the measured values ​​of the heading angle based on the associated data of the sensors installed on the tractor; the second determination module 820 is used to determine the first driving state of the rear axle of the trailer through the extended Kalman filter algorithm based on the measured values ​​of the center coordinates of the rear axle of the trailer and the measured values ​​of the heading angle, as well as the kinematic model of the trailer; the third determination module 830 is used to determine whether the trailer is driving abnormally based on at least the first driving state of the rear axle of the trailer, the kinematic model of the trailer, and the set evaluation index; the processing module 840 is used to trigger the trailer abnormality processing mechanism when it is determined that the trailer is driving abnormally.

[0175] Optionally, the second determination module 820 is specifically used to determine the measured value of the center coordinate of the rear axle of the trailer, the speed, the measured value of the heading angle, and the rate of change of the heading angle as a state vector; determine the measured value of the center coordinate of the rear axle of the trailer and the measured value of the heading angle as an observation vector; construct a state equation of the extended Kalman filter algorithm based on the state vector and a relationship determined based on the kinematic model of the trailer; construct an observation equation of the extended Kalman filter algorithm based on the observation vector and the state equation; and determine the first driving state of the rear axle of the trailer based on the state equation and the observation equation.

[0176] Optionally, the first driving state includes at least an estimated value of the coordinates of the center of the trailer's rear axle, an estimated value of the heading angle, an estimated value of the driving speed, and an estimated value of the heading angle change rate; the third determination module 830 includes: a first determination unit, used to determine the actual steering curvature of the trailer based on the estimated value of the driving speed of the center of the trailer's rear axle and the estimated value of the heading angle change rate; a second determination unit, used to determine the nominal steering curvature of the trailer based on at least the estimated value of the heading angle of the center of the trailer's rear axle and the kinematic model of the trailer; a third determination unit, used to determine the real-time value of the set evaluation index based on the actual steering curvature and the nominal steering curvature; a fourth determination unit, used to determine whether the trailer is driving abnormally based on the real-time value of the set evaluation index and a set threshold.

[0177] Optionally, when the trailer is a semi-trailer, the second determination unit is specifically used to determine the nominal turning curvature of the trailer based on an estimated value of the heading angle of the center of the trailer's rear axle, a measured value of the heading angle of the tractor, and a distance from the center of the trailer's rear axle to a connection point, wherein the connection point is a point at which the tractor is connected to the trailer, and the distance from the center of the trailer's rear axle to the connection point is determined based on a kinematic model of the trailer.

[0178] Optionally, when the trailer is a full-trailer trailer, the second determination unit is specifically used to determine the nominal turning curvature of the trailer based on an estimated value of the center heading angle of the trailer's rear axle, an estimated value of the center heading angle of the trailer's front axle, and the wheelbase of the trailer, wherein the wheelbase of the trailer is determined based on a kinematic model of the trailer.

[0179] Optionally, when the trailer is a full trailer, the device further comprises:

[0180] a fourth determination module, configured to determine the observed values ​​of the center coordinates of the front axle of the trailer and the observed values ​​of the heading angle based on the estimated values ​​of the center coordinates of the rear axle of the trailer and the estimated value of the heading angle, the coordinates and heading angle of the tractor, and the kinematic model of the trailer; and a fifth determination module, configured to determine the second driving state of the front axle of the trailer by using an extended Kalman filter algorithm based on the observed values ​​of the center coordinates of the front axle of the trailer and the observed value of the heading angle, and the kinematic model of the trailer.

[0181] Optionally, the fourth determination module includes: a fifth determination unit, used to determine the observed value of the center coordinate of the front axle of the trailer based on the estimated value of the center coordinate of the rear axle of the trailer, the estimated value of the heading angle, and the wheelbase of the trailer; a sixth determination unit, used to determine the coordinates of the connection point based on the coordinates and heading angle of the tractor and the distance from the center of the rear axle of the tractor to the connection point, wherein the connection point is the point at which the tractor and the trailer are connected; a seventh determination unit, used to determine the observed value of the heading angle of the center of the front axle of the trailer based on the observed value of the center coordinate of the front axle of the trailer, the coordinates of the connection point, the measured value of the center coordinate of the rear axle of the trailer, the estimated value of the heading angle of the center of the rear axle of the trailer, and the kinematic model of the trailer; wherein the wheelbase of the trailer and the distance from the center of the rear axle of the tractor to the connection point are determined based on the kinematic model of the trailer.

[0182] Optionally, the seventh determination unit is specifically used to: determine a first vector from the center of the trailer rear axle to the center of the trailer front axle based on the measured value of the center coordinates of the trailer rear axle and the observed value of the center coordinates of the trailer front axle; determine a second vector from the center of the trailer front axle to the connection point based on the observed value of the center coordinates of the trailer front axle and the coordinates of the connection point; perform a cross product operation on the first vector and the second vector to obtain the angle between the trailer front axle and the trailer rear axle; determine the observed value of the heading angle of the trailer front axle based on the angle between the trailer front axle and the trailer rear axle, the estimated value of the heading angle of the trailer rear axle center and a preset angle relationship, wherein the preset angle relationship is determined based on the kinematic model of the trailer.

[0183] Optionally, the sensor includes a laser radar installed on the roof of the tractor, and the first determination module 810 includes: an eighth determination unit, used to determine the corresponding point cloud of the laser emitted by the laser radar within a set period on the preset baffle of the trailer; a ninth determination unit, used to determine the measured value of the coordinate of the center of the rear axle of the trailer and the measured value of the heading angle based on the corresponding point cloud, the associated data of the laser radar, and the positional relationship between the preset baffle and the center of the rear axle of the trailer.

[0184] Optionally, the ninth determination unit is specifically used to: fit a straight line parallel to the preset baffle based on the corresponding point cloud, wherein the preset baffle is vertically installed on the edge of the trailer close to the tractor; determine a first angle between the straight line and the rear axle of the tractor; determine the measured value of the coordinates of the center of the trailer rear axle and the measured value of the heading angle based on the first angle, the associated data of the lidar, and the positional relationship between the preset baffle and the center of the trailer rear axle.

[0185] The trailer driving state determination device provided in the embodiment of the present disclosure can execute the steps of the trailer driving state determination method provided in the method embodiment of the present disclosure, and the execution steps and beneficial effects are no longer repeated here.

[0186] Figure 9 This is a schematic diagram of the structure of an electronic device in the embodiment of the present disclosure. Figure 9 , which shows a structural diagram of an electronic device 500 suitable for implementing the embodiments of the present disclosure. Figure 9 The electronic device shown is only an example and should not limit the functions and scope of use of the embodiments of the present disclosure.

[0187] like Figure 9 As shown, the electronic device 500 may include a processing device (e.g., a central processing unit, a graphics processing unit, etc.) 501, which can perform various appropriate actions and processes to implement the methods of the embodiments described in the present disclosure according to the program stored in the read-only memory (ROM) 502 or the program loaded from the storage device 508 into the random access memory (RAM) 503. Various programs and data required for the operation of the electronic device 500 are also stored in the RAM 503. The processing device 501, the ROM 502, and the RAM 503 are connected to each other via a bus 504. An input / output (I / O) interface 505 is also connected to the bus 504.

[0188] In particular, according to embodiments of the present disclosure, the processes described above with reference to the flowcharts can be implemented as computer software programs. For example, embodiments of the present disclosure include a computer program product comprising a computer program carried on a non-transitory computer-readable medium, the computer program containing program code for executing the method illustrated in the flowcharts, thereby implementing the trailer driving state determination method described above. In such embodiments, the computer program can be downloaded and installed from a network via the communication device 509, or installed from the storage device 508, or installed from the ROM 502. When executed by the processing device 501, the computer program performs the aforementioned functions defined in the method of the embodiments of the present disclosure.

[0189] It should be noted that the computer-readable medium mentioned above in the present disclosure may be a computer-readable signal medium or a computer-readable storage medium, or any combination of the two. A computer-readable storage medium may be, for example, but not limited to, an electrical, magnetic, optical, electromagnetic, infrared, or semiconductor system, device, or component, or any combination of the above. More specific examples of computer-readable storage media may include, but are not limited to: an electrical connection with one or more wires, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the above. In the present disclosure, a computer-readable storage medium may be any tangible medium that contains or stores a program that can be used by or in conjunction with an instruction execution system, device, or component. In the present disclosure, a computer-readable signal medium may include a data signal propagated in baseband or as part of a carrier wave, which carries computer-readable program code. Such a propagated data signal may take a variety of forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination of the above. A computer-readable signal medium may also be any computer-readable medium other than a computer-readable storage medium that can transmit, propagate, or transport a program for use by or in conjunction with an instruction execution system, apparatus, or device. The program code contained on the computer-readable medium may be transmitted using any suitable medium, including but not limited to wires, optical cables, RF (radio frequency), etc., or any suitable combination thereof.

[0190] The computer-readable medium may be included in the electronic device, or may exist independently and not incorporated into the electronic device. The computer-readable medium carries one or more programs that, when executed by the electronic device, cause the electronic device to execute the method for determining the trailer driving state of the present disclosure.

[0191] Optionally, when the above one or more programs are executed by the electronic device, the electronic device may also execute other steps described in the above embodiments.

[0192] In the context of the present disclosure, a machine-readable medium can be a tangible medium that can contain or store a program for use by or in conjunction with an instruction execution system, device or equipment. A machine-readable medium can be a machine-readable signal medium or a machine-readable storage medium. A machine-readable medium can include, but is not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, device or equipment, or any suitable combination of the foregoing. A more specific example of a machine-readable storage medium can include an electrical connection based on one or more lines, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.

[0193] Solution 1: A method for determining a trailer driving state, the method comprising:

[0194] Determining a measured value of the center coordinate of the rear axle of a trailer towed by the tractor and a measured value of the heading angle based on associated data from sensors installed on the tractor;

[0195] Determining a first driving state of the trailer rear axle by using an extended Kalman filter algorithm based on the measured values ​​of the center coordinates of the trailer rear axle and the measured value of the heading angle, as well as the kinematic model of the trailer;

[0196] determining whether the trailer is driving abnormally based on at least a first driving state of the trailer rear axle, a kinematic model of the trailer, and a set evaluation index;

[0197] When it is determined that the trailer is running abnormally, a trailer abnormality processing mechanism is triggered.

[0198] Solution 2: The method according to Solution 1, wherein determining the first driving state of the trailer rear axle using an extended Kalman filter algorithm based on the measured values ​​of the center coordinates of the trailer rear axle and the measured value of the heading angle, and the kinematic model of the trailer, comprises:

[0199] Determining the measured value of the center coordinate of the rear axle of the trailer, the speed, the measured value of the heading angle, and the rate of change of the heading angle as a state vector;

[0200] Determine the measured values ​​of the center coordinates of the rear axle of the trailer and the measured values ​​of the heading angle as an observation vector;

[0201] Constructing a state equation of an extended Kalman filter algorithm according to the state vector and a relationship determined based on a kinematic model of the trailer;

[0202] Constructing an observation equation of an extended Kalman filter algorithm according to the observation vector and the state equation;

[0203] A first driving state of the trailer rear axle is determined according to the state equation and the observation equation.

[0204] Solution 3: According to the method of Solution 1, the first driving state includes at least an estimated value of the center coordinate of the trailer rear axle, an estimated value of the heading angle, an estimated value of the driving speed, and an estimated value of the heading angle change rate;

[0205] The determining whether the trailer is driving abnormally based on at least the first driving state of the rear axle of the trailer, the kinematic model of the trailer, and a set evaluation index includes:

[0206] determining an actual turning curvature of the trailer according to an estimated value of the center speed of the trailer's rear axle and an estimated value of the rate of change of the heading angle;

[0207] determining a nominal turning curvature of the trailer based on at least an estimated value of a heading angle of a rear axle center of the trailer and a kinematic model of the trailer;

[0208] Determining a real-time value of the set evaluation index according to the actual steering curvature and the nominal steering curvature;

[0209] Whether the trailer is driving abnormally is determined according to the real-time value of the set evaluation index and the set threshold.

[0210] Solution 4: According to the method of Solution 3, when the trailer is a semi-trailer, determining the nominal turning curvature of the trailer based on at least an estimated value of the center heading angle of the trailer's rear axle and a kinematic model of the trailer comprises:

[0211] The nominal turning curvature of the trailer is determined based on an estimated value of the heading angle of the trailer's rear axle center, a measured value of the tractor's heading angle, and a distance from the trailer's rear axle center to a hitch point, where the tractor connects to the trailer. The distance from the trailer's rear axle center to the hitch point is determined based on a kinematic model of the trailer.

[0212] Solution 5: According to the method of Solution 3, when the trailer is a full trailer, the method further includes:

[0213] Determining the observed values ​​of the center coordinates of the front axle of the trailer and the observed values ​​of the heading angle based on the estimated values ​​of the center coordinates of the rear axle of the trailer and the estimated value of the heading angle, the coordinates and heading angle of the tractor, and the kinematic model of the trailer;

[0214] The second driving state of the front axle of the trailer is determined by an extended Kalman filter algorithm based on the observed values ​​of the center coordinates of the front axle of the trailer and the observed values ​​of the heading angle, as well as the kinematic model of the trailer.

[0215] Solution 6. The method according to Solution 5, wherein determining the observed values ​​of the center coordinates of the front axle of the trailer and the observed values ​​of the heading angle based on the estimated values ​​of the center coordinates of the rear axle of the trailer and the estimated value of the heading angle, the coordinates and heading angle of the tractor, and the kinematic model of the trailer comprises:

[0216] Determining the observed value of the center coordinate of the front axle of the trailer based on the estimated value of the center coordinate of the rear axle of the trailer, the estimated value of the heading angle, and the wheelbase of the trailer;

[0217] Determining the coordinates of the connection point based on the coordinates and heading angle of the tractor and the distance from the center of the rear axle of the tractor to the connection point, where the tractor is connected to the trailer;

[0218] Determining an observed value of the center heading angle of the trailer's front axle based on the observed values ​​of the center coordinates of the trailer's front axle, the coordinates of the connection point, the measured values ​​of the center coordinates of the trailer's rear axle, the estimated value of the center heading angle of the trailer's rear axle, and a kinematic model of the trailer;

[0219] The wheelbase of the trailer and the distance from the center of the rear axle of the tractor to the connection point are determined based on a kinematic model of the trailer.

[0220] Solution 7. The method according to Solution 6, wherein determining the observed value of the trailer front axle center heading angle based on the observed value of the trailer front axle center coordinate, the coordinates of the connection point, the measured value of the trailer rear axle center coordinate, the estimated value of the trailer rear axle center heading angle, and the kinematic model of the trailer comprises:

[0221] Determining a first vector from the center of the trailer rear axle to the center of the trailer front axle based on the measured values ​​of the center coordinates of the trailer rear axle and the observed values ​​of the center coordinates of the trailer front axle;

[0222] Determining a second vector from the center of the trailer front axle to the connection point based on the observed values ​​of the center coordinates of the trailer front axle and the coordinates of the connection point;

[0223] Performing a cross product operation on the first vector and the second vector to obtain an angle between the front axle of the trailer and the rear axle of the trailer;

[0224] An observed value of the center heading angle of the front axle of the trailer is determined based on an angle between the front axle of the trailer and the rear axle of the trailer, an estimated value of the center heading angle of the rear axle of the trailer, and a preset angular relationship, wherein the preset angular relationship is determined based on a kinematic model of the trailer.

[0225] Solution 8: The method according to Solution 5, wherein the second driving state includes at least an estimated value of the center heading angle of the trailer front axle;

[0226] The determining of the nominal turning curvature of the trailer based on at least the estimated value of the center heading angle of the trailer rear axle and the kinematic model of the trailer comprises:

[0227] A nominal turning curvature of the trailer is determined based on an estimated value of a heading angle of a rear axle center of the trailer, an estimated value of a heading angle of a front axle center of the trailer, and a wheelbase of the trailer, wherein the wheelbase of the trailer is determined based on a kinematic model of the trailer.

[0228] Solution 9: The method according to any one of Solutions 1-8, wherein the sensor includes a lidar mounted on the roof of the tractor, and the method of determining the measured value of the center coordinate of the rear axle of the trailer towed by the tractor and the measured value of the heading angle based on the correlation data of the sensor mounted on the tractor includes:

[0229] Determine a corresponding point cloud of the laser emitted by the laser radar within a set period on a preset baffle of the trailer;

[0230] The measured values ​​of the coordinates of the rear axle center of the trailer and the measured values ​​of the heading angle are determined based on the corresponding point cloud, the associated data of the laser radar, and the positional relationship between the preset baffle and the rear axle center of the trailer.

[0231] Solution 10: According to the method of Solution 9, determining the measured value of the coordinates of the center of the trailer rear axle and the measured value of the heading angle based on the corresponding point cloud, the associated data of the laser radar, and the positional relationship between the preset baffle and the center of the trailer rear axle includes:

[0232] Fitting a straight line parallel to the preset baffle according to the corresponding point cloud, wherein the preset baffle is vertically installed on the edge of the trailer close to the tractor;

[0233] determining a first angle between the straight line and the rear axle of the tractor;

[0234] The measured value of the coordinates of the rear axle center of the trailer and the measured value of the heading angle are determined based on the first angle, the associated data of the laser radar, and the positional relationship between the preset baffle and the center of the rear axle of the trailer.

[0235] Solution 11: A device for determining a trailer driving state, comprising:

[0236] a first determining module, configured to determine a measured value of a center coordinate of a rear axle of a trailer towed by the tractor and a measured value of a heading angle based on associated data of sensors installed on the tractor;

[0237] a second determining module, configured to determine a first driving state of the rear axle of the trailer using an extended Kalman filter algorithm based on the measured values ​​of the center coordinates of the rear axle of the trailer and the measured value of the heading angle, as well as the kinematic model of the trailer;

[0238] a third determining module, configured to determine whether the trailer is driving abnormally based on at least the first driving state of the rear axle of the trailer, the kinematic model of the trailer, and a set evaluation index;

[0239] The processing module is used to trigger a trailer abnormality processing mechanism when it is determined that the trailer is running abnormally.

[0240] Solution 12. An electronic device, comprising:

[0241] one or more processors;

[0242] a storage device for storing one or more programs;

[0243] When the one or more programs are executed by the one or more processors, the one or more processors implement the method as described in any one of solutions 1-10.

[0244] Solution 13: A computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the method as described in any one of Solutions 1-10.

[0245] The above description is merely a preferred embodiment of the present disclosure and an illustration of the technical principles employed. Those skilled in the art should understand that the scope of disclosure involved in the present disclosure is not limited to the technical solutions formed by the specific combination of the above-mentioned technical features, but also includes other technical solutions formed by any combination of the above-mentioned technical features or their equivalents without departing from the above-mentioned disclosed concepts. For example, a technical solution formed by replacing the above-mentioned features with (but not limited to) technical features with similar functions disclosed in this disclosure.

Claims

1. A method for determining a trailer driving state, characterized in that: The method comprises: Determining, based on correlation data from sensors installed on the tractor, measured values ​​of the center coordinates of the rear axle of the trailer towed by the tractor and a measured value of the heading angle, without installing sensors on the trailer; Determining a first driving state of the trailer rear axle by using an extended Kalman filter algorithm based on the measured values ​​of the center coordinates of the trailer rear axle and the measured value of the heading angle, as well as the kinematic model of the trailer; determining whether the trailer is driving abnormally based on at least a first driving state of the trailer rear axle, a kinematic model of the trailer, and a set evaluation index; When it is determined that the trailer is running abnormally, triggering a trailer abnormality handling mechanism; Determining the first driving state of the trailer rear axle by using an extended Kalman filter algorithm based on the measured values ​​of the center coordinates of the trailer rear axle and the measured value of the heading angle, as well as the kinematic model of the trailer, includes: Determining the measured value of the center coordinate of the rear axle of the trailer, the speed, the measured value of the heading angle, and the rate of change of the heading angle as a state vector; Determine the measured values ​​of the center coordinates of the rear axle of the trailer and the measured values ​​of the heading angle as an observation vector; Constructing a state equation of an extended Kalman filter algorithm according to the state vector and a relationship determined based on a kinematic model of the trailer; Constructing an observation equation of an extended Kalman filter algorithm according to the observation vector and the state equation; A first driving state of the trailer rear axle is determined according to the state equation and the observation equation.

2. The method according to claim 1, characterized in that The first driving state includes at least an estimated value of the center coordinate of the trailer rear axle, an estimated value of the heading angle, an estimated value of the driving speed, and an estimated value of the heading angle change rate; The determining whether the trailer is driving abnormally based on at least the first driving state of the rear axle of the trailer, the kinematic model of the trailer, and a set evaluation index includes: determining an actual turning curvature of the trailer according to an estimated value of the center speed of the trailer's rear axle and an estimated value of the rate of change of the heading angle; determining a nominal turning curvature of the trailer based on at least an estimated value of a heading angle of a rear axle center of the trailer and a kinematic model of the trailer; Determining a real-time value of the set evaluation index according to the actual steering curvature and the nominal steering curvature; Whether the trailer is driving abnormally is determined according to the real-time value of the set evaluation index and the set threshold.

3. The method according to claim 2, characterized in that When the trailer is a semi-trailer, determining the nominal turning curvature of the trailer based on at least an estimated value of the center heading angle of the trailer's rear axle and a kinematic model of the trailer includes: The nominal turning curvature of the trailer is determined based on an estimated value of the heading angle of the trailer's rear axle center, a measured value of the tractor's heading angle, and a distance from the trailer's rear axle center to a hitch point, where the tractor connects to the trailer. The distance from the trailer's rear axle center to the hitch point is determined based on a kinematic model of the trailer.

4. The method according to claim 2, characterized in that When the trailer is a full trailer, the method further includes: Determining the observed values ​​of the center coordinates of the front axle of the trailer and the observed values ​​of the heading angle based on the estimated values ​​of the center coordinates of the rear axle of the trailer and the estimated value of the heading angle, the coordinates and heading angle of the tractor, and the kinematic model of the trailer; The second driving state of the front axle of the trailer is determined by an extended Kalman filter algorithm based on the observed values ​​of the center coordinates of the front axle of the trailer and the observed values ​​of the heading angle, as well as the kinematic model of the trailer.

5. The method according to claim 4, characterized in that The step of determining the observed values ​​of the center coordinates of the front axle of the trailer and the observed values ​​of the heading angle based on the estimated values ​​of the center coordinates of the rear axle of the trailer and the estimated value of the heading angle, the coordinates and heading angle of the tractor, and the kinematic model of the trailer includes: Determining the observed value of the center coordinate of the front axle of the trailer based on the estimated value of the center coordinate of the rear axle of the trailer, the estimated value of the heading angle, and the wheelbase of the trailer; Determining the coordinates of the connection point based on the coordinates and heading angle of the tractor and the distance from the center of the rear axle of the tractor to the connection point, where the tractor is connected to the trailer; Determining an observed value of the center heading angle of the trailer's front axle based on the observed values ​​of the center coordinates of the trailer's front axle, the coordinates of the connection point, the measured values ​​of the center coordinates of the trailer's rear axle, the estimated value of the center heading angle of the trailer's rear axle, and a kinematic model of the trailer; The wheelbase of the trailer and the distance from the center of the rear axle of the tractor to the connection point are determined based on a kinematic model of the trailer.

6. The method according to claim 5, characterized in that The step of determining the observed value of the center heading angle of the front axle of the trailer based on the observed value of the center coordinate of the front axle of the trailer, the coordinate of the connection point, the measured value of the center coordinate of the rear axle of the trailer, the estimated value of the center heading angle of the rear axle of the trailer, and the kinematic model of the trailer includes: Determining a first vector from the center of the trailer rear axle to the center of the trailer front axle based on the measured values ​​of the center coordinates of the trailer rear axle and the observed values ​​of the center coordinates of the trailer front axle; Determining a second vector from the center of the trailer front axle to the connection point based on the observed values ​​of the center coordinates of the trailer front axle and the coordinates of the connection point; Performing a cross product operation on the first vector and the second vector to obtain an angle between the front axle of the trailer and the rear axle of the trailer; An observed value of the center heading angle of the front axle of the trailer is determined based on an angle between the front axle of the trailer and the rear axle of the trailer, an estimated value of the center heading angle of the rear axle of the trailer, and a preset angular relationship, wherein the preset angular relationship is determined based on a kinematic model of the trailer.

7. The method according to claim 4, characterized in that The second driving state includes at least an estimated value of the center heading angle of the trailer front axle; The determining of the nominal turning curvature of the trailer based on at least the estimated value of the center heading angle of the trailer rear axle and the kinematic model of the trailer comprises: A nominal turning curvature of the trailer is determined based on an estimated value of a heading angle of a rear axle center of the trailer, an estimated value of a heading angle of a front axle center of the trailer, and a wheelbase of the trailer, wherein the wheelbase of the trailer is determined based on a kinematic model of the trailer.

8. The method according to any one of claims 1 to 7, characterized in that The sensor includes a laser radar installed on the roof of the tractor. The method of determining the measured value of the center coordinate of the rear axle of the trailer towed by the tractor and the measured value of the heading angle based on the associated data of the sensor installed on the tractor includes: Determine a corresponding point cloud of the laser emitted by the laser radar within a set period on a preset baffle of the trailer; The measured values ​​of the coordinates of the rear axle center of the trailer and the measured values ​​of the heading angle are determined based on the corresponding point cloud, the associated data of the laser radar, and the positional relationship between the preset baffle and the rear axle center of the trailer.

9. The method according to claim 8, characterized in that The determining of the measured value of the coordinates of the center of the rear axle of the trailer and the measured value of the heading angle based on the corresponding point cloud, the associated data of the laser radar, and the positional relationship between the preset baffle and the center of the rear axle of the trailer includes: Fitting a straight line parallel to the preset baffle according to the corresponding point cloud, wherein the preset baffle is vertically installed on the edge of the trailer close to the tractor; determining a first angle between the straight line and the rear axle of the tractor; The measured value of the coordinates of the rear axle center of the trailer and the measured value of the heading angle are determined based on the first angle, the associated data of the laser radar, and the positional relationship between the preset baffle and the center of the rear axle of the trailer.

10. A device for determining the driving state of a trailer, characterized in that: include: a first determining module configured to determine, under a condition in which no sensor is installed on the trailer, a measured value of a center coordinate of a rear axle and a measured value of a heading angle of a trailer towed by the tractor based on associated data of a sensor installed on the tractor; a second determining module, configured to determine a first driving state of the rear axle of the trailer using an extended Kalman filter algorithm based on the measured values ​​of the center coordinates of the rear axle of the trailer and the measured value of the heading angle, as well as the kinematic model of the trailer; a third determining module, configured to determine whether the trailer is driving abnormally based on at least the first driving state of the rear axle of the trailer, the kinematic model of the trailer, and a set evaluation index; a processing module, configured to trigger a trailer abnormality processing mechanism when determining that the trailer is traveling abnormally; The second determination module is specifically used to determine the measured value of the center coordinate of the rear axle of the trailer, the speed, the measured value of the heading angle, and the rate of change of the heading angle as a state vector; determine the measured value of the center coordinate of the rear axle of the trailer and the measured value of the heading angle as an observation vector; construct a state equation of the extended Kalman filter algorithm based on the state vector and a relationship determined based on the kinematic model of the trailer; construct an observation equation of the extended Kalman filter algorithm based on the observation vector and the state equation; and determine the first driving state of the rear axle of the trailer based on the state equation and the observation equation.

11. An electronic device, characterized in that: The electronic device comprises: one or more processors; a storage device for storing one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors implement the method according to any one of claims 1 to 9.

12. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the method according to any one of claims 1 to 9 is implemented.

Images