A method for estimating the centroid height of a counterbalanced forklift

Abstract

The present invention discloses a real-time estimation method for the centroid height of a counterbalanced forklift. First, it detects the rotational speed of the driving wheels, the rotational angle of the rear wheels, and the body roll angle of the forklift to determine the state of the forklift. When the forklift is in a static state, the static centroid height of the forklift is detected by a static centroid position measurement method. When the forklift is in a straight-line running condition, a forklift longitudinal dynamics model is used to estimate the straight-line centroid height of the forklift. When the forklift is in a turning condition, a forklift roll dynamics model is used to estimate the turning centroid height of the forklift. Finally, the centroid heights obtained by the forklift under static, straight-line, and turning conditions are fused together to obtain the centroid height of the forklift applicable to multiple working conditions. The present invention realizes the accuracy and real-time performance of obtaining the centroid height information of the forklift under different operating conditions, and then can improve the accuracy of forklift stability control, providing a guarantee for the operating safety of the forklift.

Description

Technical Field

[0001] The present invention relates to the technical field of forklifts, and particularly relates to a method for estimating the centroid height of a counterbalanced forklift. Background Art

[0002] With the wide application of counterbalanced forklifts in major transportation and logistics industries such as factories, ports, stations, and warehouses, their working environments are becoming increasingly complex.

[0003] With the development of automotive electronic technology, in order to avoid accidents, more and more active safety systems are applied to counterbalanced forklifts. However, the stability and reliability of the forklift active safety control system not only lie in the robustness of the control algorithm, but also depend on the accurate acquisition of forklift parameters. For example, the centroid height (referring to the position of the centroid in the vertical direction), which plays a very important role in the roll stability and handling stability of the vehicle, is an important parameter in power control. Vehicle parameters determine the intervention time and control input of the active safety control system, and play an important role in safety without driver intervention. Different from traditional vehicles and other construction machinery, forklifts have complex operating conditions and a wide range of centroid position changes. Especially in high-speed steering and high-lifting cargo conditions, they are prone to longitudinal and lateral instability and even rollover risks. Due to the large real-time changes in the forklift's load capacity and the large changes in the load position distribution, far exceeding those of traditional passenger vehicles, it is difficult for existing sensors or technologies to accurately observe the centroid position data. Currently, most forklift active safety control systems use constant centroid position parameters.

[0004] Currently, many passenger vehicles use parameter estimation methods to estimate centroid height information, but current technologies are not applicable to counterbalanced forklifts. This is because counterbalanced forklifts have a unique structure, which is a rear-wheel steering structure of "front drive axle + rear steering axle", and there is no suspension structure such as springs and dampers in traditional vehicles. Therefore, the centroid estimation method based on the vertical dynamics model is not applicable to counterbalanced forklifts. In addition, due to the rigid connection between the front drive axle of the forklift and the body, and the rear steering axle is connected to the body by means of hinges, the traditional passenger vehicle roll dynamics model and longitudinal dynamics model cannot be directly applied to counterbalanced forklifts. Summary of the Invention

[0005] In order to avoid the deficiencies of the above-mentioned prior art, the present invention proposes a method for estimating the centroid height of a counterbalanced forklift, in order to accurately and real-time estimate the centroid height information of the forklift under different operating conditions, and provide guarantee for forklift stability control and active safety.

[0006] The present invention adopts the following technical solutions to solve the technical problems:

[0007] A method for estimating the centroid height of a counterbalanced forklift, the steps are as follows:

[0008] Step 1: Detect the rotational speed w of the driving wheels of the forklift f , the rotational angle δ of the rear wheels, and the roll angle φ of the vehicle body;

[0009] Step 2: When w f = 0, the forklift is in a static state, and the static centroid height h' of the forklift is detected by the static centroid position measurement method;

[0010] Step 3: When w f ≠ 0 and δ = 0, the forklift is in a straight running condition, and the longitudinal dynamics model of the forklift is used to estimate the straight running centroid height h c ;

[0011] Step 4: When w f ≠ 0 and δ ≠ 0, the forklift is in a turning condition; the roll dynamics model of the forklift is used to estimate the turning centroid height h lat ;

[0012] Step 5: The centroid heights h', h c , h lat obtained by the forklift under static, straight running, and turning conditions are fused together through the following formula to obtain the centroid height h of the forklift applicable to multiple conditions int :

[0013] h int = [(1 - K')h lat + K'L int (h c - h lat )](1 - K)+ Kh'

[0014] In the formula: K and K' are weight coefficients, and the value range is (0, 1);

[0015] The values of the weight coefficients K and K' are obtained by collecting the rotational speed w of the driving wheels f , the rotational angle δ of the rear wheels, and the roll angle φ of the vehicle body. h lat is the centroid height estimated based on the roll dynamics model of the forklift, h c is the centroid height estimated based on the longitudinal dynamics model of the forklift, and h' is the statically measured centroid height of the forklift;

[0016] L int is the cut-off frequency, and the purpose of this parameter is to eliminate high-frequency noise; then L int = 2πf int , where f int is set as the base frequency of 5 Hz.

[0017] For a further solution, in Step 1, the rotational speed w of the driving wheels of the forklift fIt is obtained by a wheel speed sensor installed on the front drive wheels of the forklift; the rear wheel rotation angle δ is obtained by a corner sensor installed on the rear steering wheels of the forklift; the vehicle body roll angle φ is obtained by a gyroscope sensor installed on the forklift body.

[0018] The detection method of the static centroid position measurement method is as follows:

[0019] (1) Pressure sensors are arranged at the front and rear ends of the forklift forks to obtain the mass and pressure information of the goods;

[0020] A height sensor is set in the vertical direction of the forklift forks to obtain the height position information of the forks;

[0021] A mast tilt sensor is added at the fixed position between the forklift mast and the front axle to obtain the front and rear tilt angle information of the forklift mast;

[0022] (2) Lower the fork height to the lowest point and keep the forks horizontal, and the pressure sensors collect the pressure values F 1 、F 2 ;

[0023] (3) After tilting the forks to an angle of α 0 , collect the pressure values F 3 、F 4 at the front and rear ends of the forks again through the pressure sensors;

[0024] (4) Calculate the coordinates of the goods in the horizontal state: Establish a fork coordinate system. When the forks are in the horizontal state, assume the coordinates of the center of mass of the goods in the goods coordinate system are G 0 (x 0 , y 0 );

[0025] (5) When the forks are raised by a certain height Δh and tilted by a certain angle α, calculate the coordinates of the center of mass of the goods in the vehicle coordinate system: Establish a vehicle coordinate system for the forklift, and assume the coordinates of the center of mass of the goods in the vehicle coordinate system are G 0 ′(x 0 ′, y 0 ′);

[0026] (6) Calculate the combined centroid coordinates of the forklift vehicle: Assume the body centroid coordinates of the forklift when leaving the factory are G(x, y), and the combined centroid coordinates of the forklift after loading the goods are G′(x′, y′); then the static centroid height h’ = y’.

[0027] In a further solution, in step 3, a forklift longitudinal dynamics model is used to estimate the centroid height h c of the forklift, which is based on a simplified linear tire model and a forklift longitudinal dynamics model, and a nonlinear H ∞ observer is used to observe the centroid height information.

[0028] For a further solution, the formula for the tire normal force in the simplified linear tire model is as follows:

[0029]

[0030]

[0031] In the formula, m represents the forklift weight, Fzf, F zr respectively represent the normal forces of the front and rear wheels of the forklift, represents the equivalent longitudinal acceleration of the forklift, λ c represents the longitudinal position coefficient of the forklift's center of mass, L f 、L r respectively represent the horizontal distances from the forklift's center of mass to the front and rear axles, h c represents the height of the center of mass estimated by the forklift's longitudinal dynamics model;

[0032] The forklift longitudinal dynamics model is:

[0033]

[0034] In the formula, m represents the forklift weight, I f represents the moment of inertia of the front drive wheels of the forklift, T tf 、T bf respectively represent the traction torque and braking torque of the front drive wheels of the forklift, r f represents the radius of the front drive wheels; w f represents the rotational speed of the front drive wheels, represents the first derivative of the rotational speed of the front drive wheels, represents the first derivative of the longitudinal speed;

[0035] F x 、F θ respectively represent the driving longitudinal force and the component of the forklift gravity parallel to the road surface.

[0036] For a further solution, in step 4, the forklift roll dynamics model is used to estimate the height h of the forklift's center of mass lat is based on the forklift roll dynamics model, and then the least squares method with a forgetting factor is used to calculate the height of the center of mass.

[0037] For a further solution, the forklift roll dynamics model is:

[0038]

[0039] In the formula, I xx represents the moment of inertia of the vehicle body about the center of mass, l s represents the wheelbase of the forklift, φ represents the vehicle body roll angle, h RIt represents the vertical height of the forklift's center of mass from the rear axle connection point. k and c respectively represent the tire stiffness coefficient and damping coefficient, and M f represents the resistance moment provided by the rubber bushing at the hinge point, and a y represents the lateral acceleration of the forklift, and m represents the weight of the forklift;

[0040] Then, the vertical height h of the forklift's center of mass from the rear axle connection point is calculated by the least squares method with a forgetting factor R , then the turning center of mass height h of the forklift lat = h R + h r , where h r is the vertical height of the rear axle hinge point.

[0041] For a further solution, the weight coefficients K and K' in step 5 are selected according to the following rules:

[0042] (1) The rotational speeds w f of the drive wheels and the rotational angle δ of the rear wheels are detected respectively. When w f = 0 and the forklift is in a static state, then the weight coefficient K = 1 and K' = 0;

[0043] (2) When w f ≠ 0 and the forklift is in a dynamic state, the weight coefficient K = 0; at this time, when the rotational angle of the rear wheel is detected again, when δ = 0, the forklift is in a straight running condition, then the weight coefficient K' = 1 is set;

[0044] When δ ≠ 0 and is less than the steering angle threshold δ th , that is, 0 < δ < δ th , at this time the forklift is in a mixed running condition, then the weight coefficient K' = 1 - δ / δ th ;

[0045] When the detected rotational angle δ of the rear wheel is greater than the steering angle threshold δ th , that is, δ > δ th , at this time the forklift is in a turning and rolling condition, and the weight coefficient K' = 0 is set.

[0046] For an even further solution, the selection of the steering angle threshold δ th is carried out according to the following rules:

[0047] (1) When the detected roll angle φ of the forklift body is less than the roll angle error value φ min , that is, φ < φ min , it is determined that the forklift is in a ramp driving or slow turning condition at this time. At this time, the method for estimating the center of mass height based on the roll dynamics model fails. Therefore, the maximum value is assigned to δ th , making the weight coefficient K' tend to 1, that is, K' ≈ 1 (the roll angle error value φ minObtained through testing and calibration, the general value range is 1-3°).

[0048] (2) When it is detected that the forklift body roll angle φ is greater than the roll angle error value φ min and less than the roll angle φ when the forklift body contacts the limit block installed on the rear axle of the forklift max , that is, φ min < φ < φ max At this time, it is determined that the forklift is in a normal operating state. At this time, the actual maximum steering angle of the forklift rear wheels is assigned to δ th ;

[0049] (3) When it is detected that the forklift body roll angle φ is greater than the roll angle φ when the body contacts the limit block max , that is, φ max < φ, at this time, it is determined that the forklift is in a severe roll stage, and set δ th = 0. At this time, δ > δ th holds, and the weight coefficient K' = 0.

[0050] Compared with the prior art, the beneficial effects of the present invention are reflected in:

[0051] (1) The invention proposes a method for real-time estimation of the centroid height applicable to forklifts in view of the unique chassis layout structure of counterbalanced forklifts, that is, the structure with the front wheels as drive wheels and the rear wheels as steering wheels.

[0052] (2) The invention respectively considers the different motion states and dynamic characteristics of forklifts under static, straight-line and turning conditions, proposes a longitudinal dynamic model and a roll dynamic model applicable to forklifts, and respectively uses parameter estimation algorithms applicable to the current working conditions on different model bases to estimate the centroid height.

[0053] (3) The invention innovatively proposes a centroid height data fusion calculation method, which comprehensively considers the operating states of forklifts under different working conditions and fuses the centroid height data under different working conditions. Compared with the current commonly used technical methods for estimating centroid height parameters, because this estimation method comprehensively considers different working conditions and fuses the centroid heights solved under different working conditions through weight coefficients, that is, under complex operating conditions, the forklift can accurately estimate the centroid height. Therefore, the centroid height parameters after fusion in the present invention have the characteristics of high accuracy and strong robustness. Description of the Drawings

[0054] Figure 1 It is a diagram of the sensor installation position in the static measurement method of the forklift of the present invention;

[0055] Figure 2 It is a schematic diagram of the forklift fork coordinate system in the static measurement method of the forklift of the present invention;

[0056] Figure 3 Schematic diagram of the forklift's vehicle coordinate system in the forklift static measurement method of the present invention;

[0057] Figure 4 Schematic diagram of the longitudinal dynamics model of the forklift of the present invention;

[0058] Figure 5 Schematic diagram of the roll dynamics model of the forklift of the present invention;

[0059] Figure 6 Side view of the rear structure of the forklift of the present invention;

[0060] Figure 7 Flowchart of the centroid height data fusion method of the present invention. Specific embodiments

[0061] The present invention will be further described below in conjunction with the accompanying drawings and specific embodiments.

[0062] As Figure 1 shown is the sensor installation position in the forklift static measurement method, specifically:

[0063] Pressure sensors 3 are arranged at the front and rear ends of the forklift forks 4 to obtain the mass and pressure information of the goods;

[0064] A height sensor 1 is set in the vertical direction of the forklift forks 4 to obtain the height position information of the forks;

[0065] A mast inclination sensor 2 is installed at the fixed position between the forklift mast and the front axle to obtain the front and rear tilt angle information of the forklift mast.

[0066] As Figure 2 shown is the schematic diagram of the fork coordinate system in the forklift static measurement method, where F 1 , F 2 are respectively the pressure values at the front and rear ends of the forks collected by the pressure sensors when the forks are horizontal; F 3 , F 4 are respectively the pressure values at the front and rear ends of the forks collected by the pressure sensors after the forks are tilted at a certain angle, α 0 is the fork tilt angle, G represents the centroid of the goods, L 1 , L 2 are respectively the distances between the centroid G of the goods and the pressure sensors at the front and rear ends.

[0067] As Figure 3The following is a schematic diagram of the forklift's vehicle coordinate system in the forklift static measurement method of the present invention. In the figure, G0’(x0’, y0’) is the coordinate of the cargo's center of mass in the vehicle coordinate system, G(x, y) is the body coordinate of the forklift when it leaves the factory, and G’(x’, y’) is the combined center of mass coordinate of the forklift after loading the cargo.

[0068] As Figure 4 shown is a schematic diagram of the longitudinal dynamics model of the forklift of the present invention. Among them, CG represents the center of mass of the forklift, v x is the longitudinal speed of the forklift, L f represents the distance from the center of mass to the front axle, L r represents the distance from the center of mass to the rear axle, w f represents the rotational speed of the front drive wheel (collected by the wheel speed sensor), θ represents the road slope angle, F zf represents the normal force of the front wheel, F zr represents the normal force of the rear wheel, F xf represents the longitudinal force of the front wheel, h c represents the estimated center of mass height under the longitudinal dynamics model.

[0069] Figure 5 is a schematic diagram of the roll dynamics model of the forklift of the present invention. Among them, φ represents the roll angle of the forklift body, a y represents the lateral acceleration, g represents the acceleration due to gravity, M f represents the resistance moment provided by the rubber bushing at the hinge point, h R represents the distance from the center of mass to the rear axle hinge, h 0 represents the vertical height of the rear axle hinge point.

[0070] As Figure 6 shown is a side view of the rear structure of the forklift of the present invention. Among them, 5 is the limit block installed on the rear axle of the forklift, 6 is the forklift body, 7 is the forklift rear axle, 8 is the hinge point connecting the rear axle and the body, and the roll angle φ is generated when the forklift body 6 contacts the limit block 5 max .

[0071] Example 1:

[0072] As Figure 7 shown, in this example, a real-time estimation method for the center of mass height applicable to counterbalanced forklifts is applied to the forklift. The real-time estimation method for the center of mass height is carried out according to the following steps:

[0073] Step 1: Install a wheel speed sensor at the front drive wheel of the forklift to obtain the rotational speed w of the forklift drive wheel f ;

[0074] Install a corner sensor at the rear wheel corner of the forklift to obtain the rotational angle δ of the forklift rear wheel;

[0075] By detecting the gyroscope information of the forklift body, the roll angle φ of the forklift body is obtained by the gyroscope sensor installed on the forklift body;

[0076] Step 2. Determine whether the forklift is in a static state by detecting the rotational speed w of the forklift drive wheels f Whether it is zero:

[0077] When w f = 0, when the forklift is in a static state, the centroid position h' of the forklift is detected by the static centroid position measurement method; The detection steps by the lifting method are as follows:

[0078] (1) Pressure sensors are arranged at the front and rear ends of the forklift forks to obtain the mass and pressure information of the goods;

[0079] A height sensor is set in the vertical direction of the forklift forks to obtain the height position information of the forks;

[0080] A mast inclination sensor is added at the fixed position between the forklift mast and the front axle to obtain the front and rear tilt angle information of the forklift mast (the installation schematic diagram is as Figure 1 shown)

[0081] (2) Lower the fork height to the lowest point and keep the forks horizontal. The pressure sensors collect the pressure values F 1 、F 2 ;

[0082] (3) After tilting the forks to the α 0 angle, the pressure sensors are used to collect the pressure values F 3 、F 4 (as Figure 2 );

[0083] (4) Calculate the coordinates of the goods in the horizontal state: Establish a fork coordinate system. When the forks are in the horizontal state, assume the coordinates of the center of mass of the goods in the goods coordinate system are G 0 (x 0 ,y 0 ). The distance between the two pressure sensors is L. As in Figure 2 where L = L 1 +L 2 , then based on steps (2) and (3), the coordinates of the goods in the horizontal state can be expressed by the following formula:

[0084]

[0085]

[0086] (5) When the forklift forks are raised by a certain height Δh, the forks are tilted at an angle α; calculate the coordinates of the center of mass of the goods in the vehicle coordinate system: establish the vehicle coordinate system of the forklift, and assume that the coordinates of the center of mass of the goods in the vehicle coordinate system are G 0 ′(x 0 ′,y 0 ′). Then the coordinates of the goods after being raised by a certain height in the vehicle coordinate system can be expressed by the following calculation formula:

[0087]

[0088]

[0089] (6) Calculate the combined center of mass coordinates of the forklift vehicle: assume that the body center of mass coordinates of the forklift at the time of leaving the factory are G(x,y), the body mass is G, and the goods mass is G 0 , then G 0 =F 1 +F 2 . The combined center of mass coordinates of the forklift after loading the goods are G′(x′,y′), as Figure 3 shown. Then the static center of mass height h’ = y’;

[0090] The combined center of mass coordinates can be expressed by the following formula:

[0091]

[0092]

[0093] Step 3: When the rotational speed of the forklift drive wheels is not zero, determine whether the forklift is in a straight running condition by detecting whether the rear wheel angle of the forklift is zero:

[0094] That is, when w f ≠0 and δ = 0, the forklift is in a straight running condition. At this time, a straight running center of mass height estimation method based on the forklift longitudinal dynamics model is carried out, that is: based on the simplified linear tire model and the forklift longitudinal dynamics model, use the nonlinear H ∞ observer to observe the center of mass height information. The specific calculation steps are as follows:

[0095] (1) Simplify the linear tire model. The formula for the tire normal force is:

[0096]

[0097]

[0098] Among them, L f and L r respectively represent the horizontal distances from the center of mass of the forklift to the front and rear axles, h c represents the center of mass height estimated by the forklift longitudinal dynamics model, represents the equivalent vertical acceleration of the forklift represents the equivalent longitudinal acceleration of the forklift

[0099] and can be expressed by the following formulas respectively:

[0100]

[0101]

[0102] where, a z represents the normal acceleration of the forklift, a x represents the longitudinal acceleration of the forklift, and θ represents the road slope angle. The normal acceleration of the forklift can be approximately expressed by the following formula:

[0103] a z ≈a x θ + v x β

[0104] where, the longitudinal acceleration a x of the forklift and the longitudinal speed v x can be directly measured. β represents the ramp change rate. The road slope angle θ and the ramp change rate β are considered as known parameters.

[0105] Define the longitudinal position coefficient λ c of the forklift's center of mass, and the calculation formula is set as follows:

[0106]

[0107] Then the formula for the normal force of the tire is simplified to:

[0108]

[0109]

[0110] (2) The longitudinal dynamics model of the forklift, as Figure 4 shown:

[0111] When the rolling resistance of the wheels and the air resistance of the forklift body are ignored, the longitudinal dynamics equation of the forklift is as follows:

[0112]

[0113] In the formula, m represents the weight of the forklift, I f represents the moment of inertia of the front drive wheels of the forklift, T tf and T bf represent the traction torque and braking torque of the front drive wheels of the forklift respectively, and r f represents the radius of the front drive wheels;

[0114] w f represents the rotational speed of the front drive wheel, represents the first derivative of the rotational speed of the front drive wheel, represents the first derivative of the longitudinal speed;

[0115] F x and F θ respectively represent the driving longitudinal force and the component force of the forklift gravity parallel to the road surface, and can be expressed by the following formulas respectively:

[0116]

[0117] F θ = mgsinθ ≈ mgθ

[0118] In the formula, θ represents the road slope angle, σ represents the longitudinal slip ratio, represents the tire-road slip angle, and the above parameters are determined by the tire performance and the tire-road friction coefficient.

[0119] (3) The nonlinear H ∞ observer estimates the center of mass height of the forklift in a straight-ahead state

[0120] According to the forklift longitudinal dynamics motion equation derived in the previous step, let the forklift state vector x s = [v x , w f T , the prediction parameter vector is The forklift system measurement is z = x s = [v x , w f T , and the control input vector is u = [T tf , T bf T . Here, since the position of the tire-road contact condition is unknown, the tire-road slip angle is estimated together with the forklift center of mass position.

[0121] The vehicle dynamics model can be represented by a nonlinear time-varying state space model:

[0122]

[0123] Among them, ω represents the process noise vector, v represents the measurement noise vector, and H represents the identity matrix. Here, f(x s , x p , u) can be expressed as:

[0124]

[0125] ​​​In order to transform the continuity problem into a discrete problem that can be processed by a computer, the Euler method is used for discrete processing to obtain the following formula:

[0126]

[0127] Where H k represents the identity matrix, k represents the current time step, The Euler method can be calculated through the following formula:

[0128]

[0129] In the formula, T s represents the sampling time.

[0130] Since the forklift state parameters can be measured by sensors installed on the vehicle body, therefore, the state parameter vector is used as the measurement quantity Regarding the predicted forklift centroid position parameter as a random walk, the forklift longitudinal dynamics system equation can be listed as follows:

[0131]

[0132] In the formula, since the tire-road surface slip angle is not the focus of this study, so set

[0133] The goal of HIF is to comprehensively consider the system prediction noise measurement noise and the initial state value while minimizing the estimation error The cost function can be written in the following form:

[0134]

[0135] In the formula and respectively represent the predicted output value and the predicted value of the state value, S k , P 0 , Q k and R k are real symmetric matrices defined for different object problems. In order to minimize the cost function, the performance upper bound parameter ε used in the literature is adopted to make it satisfy:

[0136] J < 1 / ε

[0137] For NHIF, the nonlinear state space model can be linearized to identify the forklift parameters. Among them, the Jacobian matrix can be expressed by the following formula:

[0138]

[0139] Therefore, the NHIF can be iteratively calculated by the following formula:

[0140]

[0141]

[0142]

[0143]

[0144] Step 4: When the rotational speed of the forklift drive wheel is not zero and the detected steering angle of the rear wheel of the forklift is not zero, it is determined that the forklift is in a turning condition. That is, when w f ≠0 and δ≠0, the forklift is in a turning condition, and a method for estimating the centroid height based on the forklift roll dynamics model is

[0145] performed: Based on the forklift roll dynamics model, the least squares method with a forgetting factor is used to observe the centroid height information.

[0146] The specific calculation steps are as follows:

[0147] (1) Establish a forklift roll dynamics model (as shown in Figure 5 ), and the forklift roll dynamics model can be expressed by the following formula:

[0148]

[0149] In the formula, I xx represents the moment of inertia of the vehicle body about the centroid, l s represents the wheelbase of the forklift, φ represents the roll angle of the vehicle body, h R represents the vertical height of the forklift centroid from the rear axle hinge point, k and c respectively represent the tire stiffness coefficient and damping coefficient, M f represents the resistance moment provided by the rubber bushing at the hinge point, a y represents the lateral acceleration of the forklift, and m represents the weight of the forklift; is the second derivative of the roll angle of the vehicle body.

[0150] (2) Under steady-state steering and transient steering, simplify the forklift dynamics model

[0151] 1) When the forklift is in steady-state steering (the rear wheel steering angle is fixed), at this time, the roll angular acceleration and the lateral acceleration can be regarded as constants. In this case, the formula of the forklift roll dynamics model can be simplified as:

[0152]

[0153] This can be further written in the form of parameter identification:

[0154] y(t) = ψ T (t)θ(t)

[0155] where:

[0156]

[0157] ψ(t) = ma y cosφ + mgsinφ

[0158] θ(t) = h R

[0159] (2) When the forklift performs non-steady steering (the rear wheel steering angle value changes), the forklift roll dynamics model formula can be written in the following form:

[0160]

[0161] ψ(t) = ma y cosφ + mgsinφ

[0162] θ(t) = h R

[0163] In the formula, s represents the Laplace operator, ignoring a term and considering it much smaller than I xx .

[0164] (3) Use the least squares method with forgetting factor (FFRLS) to estimate the centroid height h of the forklift under turning conditions lat .

[0165] The basic formula of FFRLS is:

[0166]

[0167] In the formula, e(t) represents the estimation error, which is usually considered to be white noise with a mean of 0. represents the estimation parameter vector, y(t) represents the system output vector, and ψ(t) represents the system input vector.

[0168] FFRLS can be iteratively calculated through the following formula:

[0169]

[0170]

[0171]

[0172] θ(k) = θ(k - 1) + K(k)e(k)

[0173] The estimated vertical height h of the forklift's center of mass from the rear axle joint is solved by FFRLS R , so the estimated height of the forklift's center of mass is h lat is:

[0174] h lat = h R + h r

[0175] where h r is the vertical height of the rear axle hinge point and can be directly measured.

[0176] Step 5: Through the center of mass height data fusion method, fuse the center of mass height information obtained under the three working conditions of the forklift being static, going straight, and turning to obtain a method for estimating the center of mass height of the forklift applicable to multiple working conditions. The center of mass height data fusion formula is:

[0177] h int = [(1 - K')h lat + K'L int (h c - h lat )](1 - K) + Kh'

[0178] In the formula: K, K' are weight coefficients, h lat is the center of mass height during turning estimated based on the forklift roll dynamics model, h c is the center of mass height during straight running estimated based on the forklift longitudinal dynamics model, h' is the static center of mass height measured statically for the forklift. L int is the cut-off frequency, and the purpose of this parameter is to eliminate high-frequency noise. Through frequency domain analysis, the cut-off frequency L int = 2πf int . Considering the trade-off between the convergence speed and the low-pass filtering, the base frequency f int is set to 5 Hz. Even if the weight coefficient K' suddenly changes between 0 and 1, the integrated observer can still output stable forklift center of mass height information.

[0179] The above weight coefficients K, K' are taken according to the following rules:

[0180] (1) By detecting whether the rotational speed w f of the front drive wheel is zero, it is judged whether the forklift is completely static. When it is detected that the rotational speed w f of the front drive wheel = 0 and the forklift is in a static state, then the weight coefficient K = 1 and K' = 0;

[0181] (2) When the angular velocity w f≠0, the forklift is in dynamic state, and the weight coefficient K = 0. At this time, the height of the forklift's center of mass is calculated by the center of mass estimation method of the longitudinal dynamics model and the center of mass height estimation method of the roll dynamics model;

[0182] 2.1 When the detected rear wheel rotation angle δ = 0, it is determined that the forklift is in a straight running condition, and at this time, the weight coefficient K' = 1 is set;

[0183] 2.2 When the detected rear wheel rotation angle δ ≠ 0 and is less than the steering angle threshold δ th , that is, 0 < δ < δ th ), it is determined that the forklift is in a mixed running condition, and at this time, the weight coefficient K' = 1 - δ / δ th ;

[0184] 2.3 When the detected rear wheel rotation angle δ is greater than the steering angle threshold δ th , that is, δ > δ th , it is determined that the forklift is in a turning roll condition, and at this time, the weight coefficient K' = 0 is set.

[0185] δ in the above steps th is taken according to the following rules.

[0186] (1) When the detected roll angle φ of the forklift body is less than the roll angle error value φ min , that is, φ < φ min , it is determined that the forklift is in a ramp driving or slow turning condition at this time. At this time, the center of mass height estimation method based on the roll dynamics model fails. Therefore, the maximum value that the processor can handle is assigned to δ th , so that the weight coefficient K' tends to 1, that is, K' ≈ 1 (the roll angle error value φ min is obtained through test calibration, and the general value range is 1 - 3°);

[0187] (2) When the detected roll angle φ of the forklift body is greater than the roll angle error threshold φ th and less than the roll angle φ when the forklift body contacts the limit block max , that is, φ min < φ < φ max , it is determined that the forklift is in a normal running state at this time. At this time, the actual maximum steering angle of the forklift rear wheel is assigned to δ th ;

[0188] (3) When the detected roll angle φ of the forklift body is greater than the roll angle φ when the body contacts the limit block max , that is, φ max < φ, it is determined that the forklift is in a severe roll stage at this time. The center of mass height estimation method based on the roll dynamics model plays the greatest role. Set δ th = 0 at this time. At this time, δ > δ thIt holds, and the weight coefficient K' = 0.

[0189] Embodiment 2:

[0190] Apply the forklift anti - rollover control method of the present invention to a certain type of 3 - ton counterbalanced forklift. The total vehicle mass m of this counterbalanced forklift is 4639 kg, the body mass m s = 4300 kg, the moment of inertia I of the whole vehicle about the Z - axis z = 6129 kg·m 2 The moment of inertia I of the vehicle frame about the X - axis x = 3100 kg·m 2 The distance a between the front axle and the center of mass is 1 m, the distance b between the rear axle and the center of mass is 0.7 m, the wheelbase B is 1 m, the actual maximum steering angle of the rear wheels of the forklift is 82°, set φ min = 2°, φ max = 6°. The operating conditions of the forklift are to first measure the static center - of - mass height, then go straight and accelerate, and then make a right - angle turn.

[0191] The method for real - time estimating the center - of - mass height of the forklift mentioned in the present invention is carried out according to the following steps:

[0192] Step 1: Lower the fork height to the lowest point and keep the forks horizontal. The pressure sensors collect the pressure values at both ends of the forks. Denote the pressure values at the front and rear ends as F 1 and F 2 . After tilting the forks to a certain α 0 angle and height, collect the pressure values F 3 and F 4 at the front and rear ends again through the pressure sensors, and collect the fork tilt angle with an angle sensor and the fork height information with a height sensor. Measure the static center - of - mass height h' = 0.75 m of the forklift through the static measurement method proposed in this invention patent. The wheel speed sensor collects the rotational speed information of the front - wheel drive wheels of the forklift, and the rear - wheel steering angle sensor of the forklift collects the rear - wheel steering angle information.

[0193] Step 2: Under the straight - line acceleration condition, by detecting that the rotational speed of the forklift wheels is zero and the rear - wheel steering angle value is zero, determine the forklift turning condition. Therefore, adopt a simplified linear tire model and a forklift longitudinal dynamics model, and use a non - linear H ∞ observer to observe the center - of - mass height information;

[0194] Step 3: Under the turning condition, by detecting that the rotational speed of the forklift wheels is not zero and the rear - wheel steering angle value is not zero, determine the forklift turning condition. Therefore, adopt a forklift roll dynamics model and use the least - squares method with a forgetting factor to observe the center - of - mass height information;

[0195] Step 4: Combine the information of the front driving wheel speed, rear wheel steering angle and forklift roll angle of the forklift truck to obtain the weight coefficients K and K' calculated in real time. Adopt the centroid height data fusion algorithm to obtain the fused centroid height of 0.9 m.

[0196] In summary, by adopting the real-time centroid height estimation method for a counterbalanced forklift truck of the present invention, the real-time centroid height of the forklift truck under different operating conditions can be effectively identified, providing guarantee for the lateral stability and active safety of the forklift truck.

Claims

1. A method for estimating the centroid height of a counterbalanced forklift, characterized in that: The steps are as follows: Step 1, detect the rotational speed w of the driving wheels of the forklift f , the rotational angle δ of the rear wheels and the roll angle φ of the vehicle body; Step 2. When w f = 0, the forklift is in a static state, and the static centroid height h' of the forklift is detected by the static centroid position measurement method at this time; Step 3. When w f ≠ 0 and δ = 0, the forklift is in the straight running condition, and the longitudinal dynamics model of the forklift is used to estimate the straight running centroid height h c ; Step 4. When w f ≠ 0 and δ ≠ 0, the forklift is in a turning condition; a forklift roll dynamics model is used to estimate the turning center of mass height h lat ; Step 5: Combine the centroid heights h′ and h obtained for the forklift under static, straight-ahead, and turning conditions c , h lat together through the following formula to obtain the centroid height h of the forklift applicable to multiple conditions int : h int = [(1 - K')h lat + K'L int (h c - h lat )](1 - K)+ Kh' In the formula: K and K' are weight coefficients, and the value range is (0, 1); L int is the cut-off frequency, then L int = 2πf int where f int is set as the base frequency of 5 Hz.

2. The method for estimating the centroid height of a counterbalanced forklift according to claim 1, characterized in that: The rotational speed w of the driving wheels of the forklift in Step 1 f is obtained by a wheel speed sensor installed on the front driving wheels of the forklift; the rotational angle δ of the rear wheels is obtained by a rotational angle sensor installed on the rear steering wheels of the forklift; and the body roll angle φ is obtained by a gyroscope sensor installed on the forklift body.

3. The method for estimating the centroid height of a counterbalanced forklift according to claim 1, characterized in that: The steps of the static centroid measurement method are as follows: (1) Pressure sensors are arranged at the front and rear ends of the forklift forks to obtain the mass and pressure information of the goods; A height sensor is set in the vertical direction of the forklift forks to obtain the height position information of the forks; A mast inclination sensor is added at the fixed position of the forklift mast and the front axle to obtain the front and rear tilt angle information of the forklift mast; (2) Lower the fork height to the lowest point and keep the forks horizontal. The pressure sensors collect the pressure values F 1 and F 2 ; (3) Tilt the forklift forks to an angle of α 0 After that, collect the pressure values F 3 and F 4 at the front and rear ends of the forklift forks again through the pressure sensor; (4) Calculate the coordinates of the goods in the horizontal state: Establish a forklift tine coordinate system. When the forklift tines are in the horizontal state, assume that the coordinates of the center of mass of the goods in the goods coordinate system are G 0 (x 0 ,y 0 ); (5) When the fork is raised by a certain height Δh and tilted by a certain angle α, calculate the coordinates of the center of mass of the goods in the vehicle coordinate system: Establish the vehicle coordinate system of the forklift, and assume that the coordinates of the center of mass of the goods in the vehicle coordinate system are G′ 0 (x′ 0 ,y′ 0 ); (6) Calculate the combined centroid coordinates of the entire forklift: Let the body centroid coordinates of the forklift when leaving the factory be G(x, y), and the combined centroid coordinates of the forklift after loading the goods be G'(x', y'); then the static centroid height h' of the forklift = y'.

4. The method for estimating the centroid height of a counterbalanced forklift according to claim 1, characterized in that: The straight running centroid height h of the forklift in Step 3 c is based on a simplified linear tire model and a forklift longitudinal dynamics model, and uses a non-linear H ∞ observer to observe the centroid height information.

5. The method for estimating the centroid height of a counterbalanced forklift according to claim 4, characterized in that: The formula for the tire normal force in the simplified linear tire model is: where m represents the weight of the forklift, F zf and F zr represent the normal forces on the front and rear wheels of the forklift respectively, represents the equivalent longitudinal acceleration of the forklift, λ c represents the longitudinal position coefficient of the center of mass of the forklift, L f and L r represent the horizontal distances from the center of mass of the forklift to the front and rear axles respectively, h c represents the height of the center of mass estimated by the longitudinal dynamics model of the forklift; The longitudinal dynamics model of the forklift is: Wherein, m represents the weight of the forklift, and I f represents the moment of inertia of the front drive wheels of the forklift, and T tf and T bf respectively represent the traction torque and braking torque of the front drive wheels of the forklift, and r f represents the radius of the front drive wheels; w f represents the rotational speed of the front drive wheels, represents the first derivative of the rotational speed of the front drive wheels, represents the first derivative of the longitudinal speed; F x and F θ respectively represent the driving longitudinal force and the component force of the gravity of the forklift along the direction parallel to the road surface.

6. The method for estimating the centroid height of a counterbalanced forklift according to claim 1, characterized in that: The turning centroid height h of the forklift in Step 4 lat is based on the forklift roll dynamics model, and then the least squares method with a forgetting factor is used to calculate the centroid height.

7. The method for estimating the centroid height of a counterbalanced forklift according to claim 6, characterized in that: The roll dynamics model of the forklift is: Where, I xx represents the moment of inertia of the vehicle body rotating about the center of mass, l s represents the forklift wheelbase, φ represents the vehicle body roll angle, h R represents the vertical height of the forklift center of mass from the rear axle connection point, k and c respectively represent the tire stiffness coefficient and damping coefficient, M f represents the resistance moment provided by the rubber bushing at the hinge point, a y represents the lateral acceleration of the forklift, and m represents the weight of the forklift; The vertical height h from the center of mass of the forklift to the rear axle joint point is calculated by the least squares method with a forgetting factor. R , then the turning center of mass height h of the forklift lat = h R + h r , where h r is the vertical height of the rear axle hinge point.

8. The method for estimating the centroid height of a counterbalanced forklift according to claim 1, characterized in that: The weight coefficients K and K' in step 5 are taken according to the following rules: (1) Detect the rotational speed w of the driving wheel separately f and the rotational angle δ of the rear wheel. When w f = 0 and the forklift is in a static state, the weight coefficient K = 1 and K' = 0; (2) When w f ≠ 0, the forklift is in motion and the weight coefficient K = 0; when the rear wheel rotation angle is detected again at this time, when δ = 0, the forklift is in a straight running condition, then the weight coefficient K' = 1 is set; When δ≠0 and is less than the steering angle threshold δ th , that is, 0 < δ < δ th , at this time the forklift is in the hybrid operation condition, then set the weight coefficient K’ = 1 - δ / δ th ; When it is detected that the rear wheel rotation angle δ is greater than the steering angle threshold δ th , that is, δ > δ th , at this time, the forklift is in the turning and rolling condition, and the weight coefficient K' is set to 0.

9. The method for estimating the centroid height of a counterbalanced forklift according to claim 8, characterized in that: The steering angle threshold δ th is selected according to the following rules: (1) When the detected roll angle φ of the forklift body is less than the roll angle error value φ min , that is, φ < φ min , it is determined that the forklift is in the ramp driving or slow turning condition at this time. At this time, the centroid height estimation method based on the roll dynamics model fails. Therefore, the maximum value is assigned to δ th , so that the weight coefficient K’ tends to 1, that is, K’ ≈ 1; (2) When it is detected that the forklift body roll angle φ is greater than the roll angle error threshold φ min and less than the roll angle φ when the forklift body contacts the limit block installed on the rear axle of the forklift max , that is, φ min < φ < φ max at this time, it is determined that the forklift is in a normal operating state, and at this time, the actual maximum steering angle of the rear wheels of the forklift is assigned to δ th ; (3) When it is detected that the forklift body roll angle φ is greater than the roll angle φ when the body contacts the limit block max that is, φ max < φ, at this time, it is determined that the forklift is in the severe roll stage, and set δ th = 0, at this time, δ > δ th holds, and the weight coefficient K' = 0.

Images