Vehicle front wheel turning angle detection method and application

Abstract

The invention belongs to an automatic control technology, mainly relates to the field of agricultural vehicle automation, and particularly relates to a vehicle front wheel turning angle detection method and application. A method of estimating a tire angle based on a rate of change of a steering wheel angle is provided. According to the method, the change rate of the steering wheel turning angle is used as an input quantity, a mathematical model of a vehicle steering system is established, and the vehicle body absolute turning angle calculated through vehicle kinematics is used as a measurement quantity to calculate the tire turning angle. Accurate and high-stability front wheel steering angle detection can be provided under the condition of high-speed operation of an agricultural vehicle.

Description

Technical Field

[0001] The present invention belongs to automatic control technology, mainly relates to the field of agricultural machinery vehicle automation, and in particular to a method for detecting a vehicle front wheel angle and its application. Background Art

[0002] In an autonomous driving system, real-time monitoring of vehicle driving information is crucial, and real-time monitoring of tire angles is particularly critical. Accurate detection of tire angles is an important prerequisite for vehicle control. Usually, front wheel angle detection can be achieved by installing a Hall angle sensor or two IMU solutions. However, both solutions require sensors to be installed on the front axle of the vehicle, which is prone to wear and tear, cable breakage, etc. during long-term operation. In addition, the cost of these two solutions is also relatively high.

[0003] In order to conveniently obtain the front wheel angle and reduce costs, Patent No. ZL202111455635.7 discloses a tire angle analysis method based on a gyroscope and a steering wheel angle, which uses an error model combined with a vehicle kinematic model and integrates a Kalman filter method to estimate the front wheel angle. Since the invention uses the encoder angle value instead of the angle difference to calculate the front wheel angle, the steering wheel occasionally swings violently when the vehicle starts, and the accuracy of the front wheel angle detected when the agricultural vehicle is operating at more than 10km / h is also reduced. Summary of the invention

[0004] Therefore, the present invention proposes a method and application for detecting the front wheel turning angle of a vehicle, which is mainly used in the field of automatic driving of agricultural machinery and is used to estimate the tire turning angle of agricultural machinery vehicles to solve the above-mentioned problems.

[0005] In order to solve the above technical problems, this paper proposes a method and application for detecting the front wheel turning angle of a vehicle.

[0006] The present invention provides the following technical solutions:

[0007] A method for detecting a front wheel turning angle of a vehicle comprises the following steps:

[0008] S100, establish a mathematical model of vehicle steering system

[0009] S200, calculate the absolute turning angle of the vehicle body based on kinematics

[0010] S300, estimate the front wheel angle:

[0011] According to the S100 and S200, the state equation and observation equation are obtained as follows:

[0012] x k =F k x k-1 +B k uk +w k

[0013] z k =H k x k +v k

[0014] In the formula

[0015]

[0016] H k = [0 1],

[0017] w k is the discrete time process noise with covariance Q k , v k is the observation noise, whose covariance is given by R k It means that δ k represents the front wheel turning angle at time k, represents the front wheel angular velocity at time k, Δe is the sampling time interval, τ is the time constant, K is the transmission ratio of the steering system, and u k represents the steering wheel change rate, z k is the observed value at time k, x k represents the state vector at time k, x k-1 represents the state vector at time k-1, F k represents the state transfer matrix, B k represents the input matrix, H k represents the output matrix;

[0018] Based on the above equation, the Kalman filter algorithm used includes two parts: prediction and update. The prediction part is:

[0019]

[0020] In the formula, - represents the estimated value, P is the state covariance matrix, and the algorithm for the update part is:

[0021]

[0022] Where I is the unit matrix, K k is the gain at time k, is the state vector at time k predicted based on the state vector at time k-1, is the covariance P at time k-1 k-1 The predicted covariance at time k, P k is the covariance at time k, and Represents F k and Hk The transposed matrix of the update part is x k This is the front wheel turning angle calculated by the Kalman filter algorithm.

[0023] Preferably, in the above S200, the absolute turning angle of the vehicle body can be calculated by the following formula:

[0024]

[0025] In the formula, is the vehicle body yaw angular velocity, L is the wheelbase of the agricultural machinery vehicle, V is the vehicle speed, and z is the absolute turning angle of the vehicle body, which is the observation value of the subsequent Kalman filter.

[0026] The discretized observation equation can be expressed as:

[0027]

[0028] In the formula, z k is the observed value at time k, δ k represents the front wheel turning angle at time k, represents the angular velocity of the front wheel at time k.

[0029] Preferably, in the above S100, the transfer function of the vehicle steering system is:

[0030]

[0031] Where s represents the s domain, corresponding to t in the time domain, δ is the front wheel angle, τ is the time constant, u is the steering wheel change rate, K is the transmission ratio of the steering system,

[0032] The expression of the transfer function in the time domain is:

[0033]

[0034] In the formula, is the front wheel angular velocity, is the first-order derivative with respect to time,

[0035] Discretize the expression in the time domain into:

[0036]

[0037] Where Δt is the sampling time interval, δ k represents the front wheel turning angle at time k, δ k-1 Represents the front wheel turning angle at time k-1. The discretized formula is the state equation used by the subsequent Kalman filter.

[0038] Preferably, the vehicle is equipped with a GNSS antenna and an IMU, and the steering wheel motor is equipped with an incremental encoder.

[0039] An application of a method for detecting a front wheel turning angle of a vehicle, running any of the above-mentioned methods for detecting a front wheel turning angle of a vehicle, is used to continuously estimate the tire turning angle when the agricultural machinery is operating normally.

[0040] An automatic driving agricultural machine, comprising:

[0041] Memory for storing computer programs;

[0042] A processor is used to implement the method for detecting the front wheel turning angle of a vehicle as described in any of the above items when executing the computer program.

[0043] A computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the method for detecting the front wheel angle of a vehicle as described in any of the above items is implemented.

[0044] Compared with the prior art, the present invention has the following beneficial effects:

[0045] In order to solve the problems that the existing methods for measuring tire angles are expensive, and the existing low-cost solutions are not accurate enough and unstable when agricultural machinery is operating at high speed, the present invention provides a method for estimating tire angles based on the rate of change of the steering wheel angle. The present invention uses the rate of change of the steering wheel angle as input, establishes a mathematical model of the vehicle steering system, and uses the absolute body angle calculated by vehicle kinematics as the measurement quantity to calculate the tire angle. It can provide accurate and highly stable front wheel angle detection when agricultural machinery vehicles are operating at high speed. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] The accompanying drawings are used to provide a further understanding of the present invention and constitute a part of the specification. Together with the embodiments of the present invention, they are used to explain the present invention and do not constitute a limitation of the present invention. In the accompanying drawings:

[0047] Figure 1 It is a schematic diagram of the process of the present invention. DETAILED DESCRIPTION

[0048] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0049] like Figure 1 As shown, Figure 1 It is a flowchart of an embodiment of the present application.

[0050] The agricultural machinery body needs to be equipped with a GNSS antenna and an IMU, and the steering wheel motor needs to be equipped with an incremental encoder. The GNSS antenna is used to obtain the position information of the vehicle's east and north coordinates, the IMU is used to obtain the vehicle's yaw angular velocity, and the encoder is used to obtain the steering wheel angle.

[0051] 1. Mathematical model of vehicle steering system

[0052] The transfer function of the vehicle steering system is:

[0053]

[0054] In the formula, s represents the s domain, which corresponds to t in the time domain, δ is the front wheel angle, τ is the time constant, u is the steering wheel change rate, and K is the transmission ratio of the steering system. The transfer function includes a transfer function from the steering wheel change rate u to the front wheel angular velocity A first-order delay system and an angular velocity from the front wheel The integral part to the front wheel steering angle δ. is the first-order derivative with respect to time. The steering wheel change rate can be calculated by the angle recorded by the encoder and the sampling frequency. The transmission ratio can be obtained by calibrating the relationship between the steering wheel angle and the front wheel angle. The expression of the transfer function in the time domain is:

[0055]

[0056] In order to use this model in a real system, the expression in the time domain is discretized as:

[0057]

[0058] Where Δt is the sampling time interval, δ k represents the front wheel turning angle at time k, δ k-1 Represents the front wheel turning angle at time k-1. The discretized formula is the state equation used by the subsequent Kalman filter.

[0059] 2. Calculate the absolute turning angle of the vehicle based on kinematics

[0060] Kalman filtering requires measurement values ​​to update the filtering algorithm process. The measurement value used in the present invention is the absolute turning angle of the vehicle body calculated based on the vehicle kinematics, which can be calculated by the following formula:

[0061]

[0062] In the formula is the vehicle body yaw angular velocity, L is the wheelbase of the agricultural machinery vehicle, and V is the vehicle speed. z is the absolute turning angle of the vehicle body, which is the observation value of the subsequent Kalman filter.

[0063] The discretized observation equation can be expressed as:

[0064]

[0065] In the formula, z k is the observed value at time k.

[0066] 3. Estimate the front wheel angle

[0067] Combining equation 3 and equation 5, the state equation and observation equation can be obtained as follows:

[0068] x k =F k x k-1 +B k u k +w k

[0069] z k =H k x k +v k

[0070] In the formula

[0071]

[0072] H k =[0 1].

[0073] w k is the discrete time process noise with covariance Q k .v k is the observation noise, and its covariance R k Indicates. k represents the front wheel turning angle at time k, represents the front wheel angular velocity at time k, Δt is the sampling time interval, τ is the time constant, K is the transmission ratio of the steering system, and u k represents the steering wheel change rate, z k is the observed value at time k. k represents the state vector at time k, x k-1 represents the state vector at time k-1, F k represents the state transfer matrix, B k represents the input matrix, H k Represents the output matrix.

[0074] Based on the above equation, the Kalman filter algorithm used in the present invention includes two parts: prediction and update. The prediction part is:

[0075]

[0076] In the formula, ^ represents the estimated value, and P is the state covariance matrix. The algorithm for the update part is:

[0077]

[0078] Where I is the unit matrix, K k is the gain at time k, and the x calculated by the update part k This is the front wheel turning angle calculated by the Kalman filter algorithm. is the state vector at time k predicted based on the state vector at time k-1, is the covariance P at time k-1 k-1 The predicted covariance at time k, P k is the covariance at time k. and Represents F k and H k The transposed matrix of .

[0079] By using the above algorithm, the tire angle can be continuously estimated when the agricultural machinery is operating normally.

[0080] The above process can be summarized as follows:

[0081] Install GNSS antenna and IMU on the agricultural machinery body, and install incremental encoder on the steering wheel motor. The GNSS antenna is used to obtain the vehicle body coordinate position information and speed information, the IMU is used to obtain the vehicle body yaw angular velocity, and the encoder is used to obtain the steering wheel angle.

[0082] This solution needs to measure the wheelbase L of the agricultural machinery vehicle before starting operation. According to vehicle kinematics, the absolute turning angle z of the vehicle body during movement can be calculated as

[0083]

[0084] In the formula is the vehicle yaw angular velocity, L is the wheelbase of the agricultural machinery vehicle, and V is the vehicle's driving speed. The calculated turning angle value is the observation value of the subsequent filtering algorithm.

[0085] According to the steering wheel angle value read by the encoder and the sampling frequency, the steering wheel change rate can be calculated. This value is used as the input value of the state equation.

[0086] Based on the above observations and input values, a Kalman filter algorithm can be designed to estimate the front wheel steering angle of the vehicle. The mathematical model used in the algorithm is

[0087]

[0088] Where Δt is the sampling time interval, δ k represents the front wheel turning angle at time k, is the front wheel angular velocity at time k, τ represents the time constant in the steering model, and u k is the steering wheel change rate, K is the transmission ratio, z k is the observed value at time k.

[0089] Based on the above mathematical model, the extended Kalman filter algorithm can be used to estimate the front wheel angle. By repeating the above process, the tire angle can be continuously estimated when the agricultural machinery is operating normally.

[0090] It should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device.

[0091] Finally, it should be noted that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art can still modify the technical solutions described in the aforementioned embodiments or replace some of the technical features therein by equivalents. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A method for detecting a front wheel turning angle of a vehicle, characterized in that: The following steps are involved: S100, establish a mathematical model of vehicle steering system S200, calculate the absolute turning angle of the vehicle body based on kinematics S300, estimate the front wheel angle: According to the S100 and S200, the state equation and observation equation are obtained as follows: x k =F k x k-1 +B k u k +w k z k =H k x k +v k In the formula H k ＝[0 1]， w k is the discrete time process noise with covariance Q k , v k is the observation noise, whose covariance is given by R k It means that δ k represents the front wheel turning angle at time k, represents the front wheel angular velocity at time k, Δt is the sampling time interval, τ is the time constant, K is the transmission ratio of the steering system, and u k represents the steering wheel change rate, z k is the observed value at time k, x k represents the state vector at time k, x k-1 represents the state vector at time k-1, F k represents the state transfer matrix, B k represents the input matrix, H k represents the output matrix; Based on the above equation, the Kalman filter algorithm used includes two parts: prediction and update. The prediction part is: In the formula, ^ represents the estimated value, P is the state covariance matrix, and the algorithm for the update part is: Where I is the unit matrix, K k is the gain at time k, is the state vector at time k predicted based on the state vector at time k-1, is the covariance P at time k-1 k-1 The predicted covariance at time k, P k is the covariance at time k, and Represents F k and H k The transposed matrix of the update part is x k This is the front wheel turning angle calculated by the Kalman filter algorithm.

2. A method for detecting a vehicle front wheel turning angle according to claim 1, characterized in that: In the above S200, the absolute turning angle of the vehicle body can be calculated by the following formula: In the formula, is the vehicle body yaw angular velocity, L is the wheelbase of the agricultural machinery vehicle, V is the vehicle speed, and z is the absolute turning angle of the vehicle body, which is the observation value of the subsequent Kalman filter. The discretized observation equation can be expressed as: In the formula, z k is the observed value at time k, δ k represents the front wheel turning angle at time k, represents the angular velocity of the front wheel at time k.

3. The method for detecting the front wheel turning angle of a vehicle according to claim 1, characterized in that: In the above-mentioned S100, the transfer function of the vehicle steering system is: Where s represents the s domain, corresponding to t in the time domain, δ is the front wheel angle, τ is the time constant, u is the steering wheel change rate, K is the transmission ratio of the steering system, The expression of the transfer function in the time domain is: In the formula, is the front wheel angular velocity, is the first-order derivative with respect to time, Discretize the expression in the time domain into: Where Δt is the sampling time interval, δ k represents the front wheel turning angle at time k, δ k-1 Represents the front wheel turning angle at time k-1. The discretized formula is the state equation used by the subsequent Kalman filter.

4. The method for detecting the front wheel turning angle of a vehicle according to claim 1, characterized in that: The vehicle is equipped with a GNSS antenna and an IMU, and an incremental encoder is installed on the steering wheel motor.

5. An application of a method for detecting a front wheel turning angle of a vehicle, characterized in that: A method for detecting the front wheel turning angle of a vehicle as described in any one of claims 1 to 4 is used to continuously estimate the tire turning angle when the agricultural machinery is operating normally.

6. An automatic driving agricultural machine, characterized in that: include: Memory for storing computer programs; A processor, configured to implement the method for detecting the front wheel turning angle of a vehicle as claimed in any one of claims 1 to 4 when executing the computer program.

7. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the method for detecting the front wheel turning angle of a vehicle as claimed in any one of claims 1 to 4 is implemented.

Images