Gradient estimation device and method

Abstract

The invention discloses a slope estimation device and method, and belongs to the technical field of road slope detection, and the device comprises a liquid storage bottle, a turbulent flow mechanism and a displacement sensor; the liquid storage bottle is mounted on a vehicle running on a road surface with a slope to be estimated, a bottle cap is arranged at the top of the liquid storage bottle, and measuring liquid with a set liquid level height is stored in the liquid storage bottle; the turbulent flow mechanism is located in the liquid storage bottle and divides the interior of the liquid storage bottle into a plurality of cavities. Every two adjacent cavities are communicated through a liquid flowing channel of the turbulent flow mechanism; the four displacement sensors are installed on the bottle cap in a penetrating mode in an annular distribution mode. The end part of the displacement sensor is connected with a floater floating on the liquid level of the measuring liquid; the gradient of the road surface on which the vehicle runs is estimated by acquiring the variable quantity of the height of each floater. According to the method, the gradient of the vehicle driving road can be estimated under different working conditions, and the requirement for the performance of the vehicle is low.

Description

Technical Field

[0001] The present invention belongs to the technical field of road slope detection, and in particular relates to a slope estimation device and method. Background Art

[0002] The slope of the road a vehicle is traveling on is an important vehicle state quantity, which is related to the control accuracy of the vehicle control system. At present, the slope estimation method based on kinematic model and dynamic model is mainly used to estimate the slope of the road a vehicle is traveling on.

[0003] The slope estimation method based on the kinematic model focuses on the motion characteristics of the object, such as shape, motion joints, motion equations, trajectory, displacement, velocity and acceleration. The accuracy of slope estimation is greatly affected when the vehicle posture changes. The slope estimation based on the dynamic model relies on the vehicle model and focuses on the motion state and force conditions of the object. It is greatly affected by various parameters of the vehicle itself, so there is a deviation in the slope estimation. Summary of the invention

[0004] In view of the deficiencies in the prior art, the present invention provides a slope estimation device and method, which can estimate the slope of a road on which a vehicle is traveling under different working conditions.

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

[0006] In a first aspect, a slope estimation device is provided, comprising: a liquid storage bottle, a spoiler mechanism and a displacement sensor;

[0007] The liquid storage bottle is installed on a vehicle traveling on a road surface whose slope is to be estimated, and a bottle cap is provided on the top of the liquid storage bottle, in which a measuring liquid with a set liquid level height is stored;

[0008] The spoiler mechanism is located in the liquid storage bottle and divides the interior of the liquid storage bottle into a plurality of chambers; two adjacent chambers are connected through the liquid flow channel of the spoiler mechanism;

[0009] There are four displacement sensors, which are installed on the bottle cap in a ring-shaped distribution form; the end of the displacement sensor inserted into the liquid storage bottle is connected to a float floating on the surface of the measuring liquid; when the liquid level of the measuring liquid changes, each of the floats drives the sensing element of the displacement sensor connected to it to produce displacement, so as to obtain the change in the height of each float, thereby realizing the estimation of the slope of the road surface on which the vehicle is traveling.

[0010] Optionally, there are four chambers, and the displacement sensors are inserted into the chambers one by one. The displacement sensors include a connecting rod and a voltage measuring device; one end of the connecting rod is connected to the float, and the other end extends into the voltage measuring device; when the float rises and falls, the connecting rod rises and falls in the voltage measuring device, and the voltage measuring device is installed on the bottle cap.

[0011] Optionally, the spoiler mechanism is an isolation fence, and the isolation fence is evenly distributed with spoiler holes to connect two adjacent chambers; the cross section of the isolation fence is a cross shape.

[0012] Optionally, the spoiler mechanism includes a spoiler and a plug-in plate; there are four plug-ins and they are arranged in a cross shape as a whole; the lower part of the plug-in plate is provided with a pin block extending outward; the bottom of the liquid storage bottle is provided with a slot adapted to the pin block; the spoiler is connected to the bottle cap, and there are four groups of spoilers and they are arranged in a cross shape as a whole; each group of spoilers includes two partitions distributed at a set interval; the upper part of the plug-in plate can be inserted between the two partitions of each group of spoilers, and the initial liquid level of the measuring liquid is located at the intersection of the plug-in plate and the partition.

[0013] Optionally, the longitudinal section of the bottle cap is T-shaped, the spoiler mechanism is provided with a mounting plate near the top of the bottle cap, and the bottle cap is detachably connected to the mounting plate;

[0014] A positioning pin is also provided at the end of the bottle cap inserted into the liquid storage bottle; there are multiple positioning pins; the liquid storage bottle is provided with positioning channels corresponding to the positioning pins one by one, and the positioning channels extend from the bottle mouth of the liquid storage bottle to the bottle body and are L-shaped; the positioning pin can move along the positioning channel under the action of external force.

[0015] In a second aspect, a slope estimation method is provided, based on the slope estimation device described in any one of the first aspects, comprising:

[0016] The slope estimation device is installed on a vehicle traveling on a road surface whose slope is to be estimated;

[0017] When the vehicle is stationary, a three-dimensional coordinate system of the slope estimation device is established and the normal vector of the XOY plane is defined.

[0018] According to the reading of each displacement sensor during the vehicle's driving process, the height change value of the four floats relative to the initial liquid level of the measuring liquid is obtained;

[0019] Based on the height change values ​​of the four floats and the installation positions of the four displacement sensors, the current coordinates of the four floats are obtained in the established three-dimensional coordinate system;

[0020] Based on the current coordinates of the four floats, the best estimated plane of the current liquid level of the measuring liquid and the normal vector of the best estimated plane are obtained. Normal vector and the normal vector The angle between the two is the slope angle of the road surface, so as to estimate the slope of the road surface.

[0021] Optionally, the three-dimensional coordinate system has the center of the initial liquid surface of the measuring liquid as the origin O, the ground upward as the positive direction of the Z axis, the vehicle forward direction as the positive direction of the X axis, and the right side of the vehicle forward direction as the positive direction of the Y axis; the normal vector The four floats are located in the positive direction of the X axis, the negative direction of the X axis, the positive direction of the Y axis, and the negative direction of the Y axis;

[0022] Based on the current coordinates of the four floats, the best estimated plane of the current liquid level of the measuring liquid and the normal vector of the best estimated plane are obtained. include:

[0023] Every three floats form an estimated plane, and the plane equation of the estimated plane is obtained;

[0024] A i x+B i y+A i z+D i =0

[0025] Among them, A i , B i , C i and D i are four constants of the i-th estimated plane; i = 1, 2, 3, 4;

[0026] Based on the plane equation of the estimated plane, obtaining the distance from the fourth float not used to construct the estimated plane to the estimated plane;

[0027] Among all the estimated planes, the estimated plane corresponding to the minimum distance is selected as the best estimated plane ε;

[0028] The slope angle θ of the driving road is:

[0029]

[0030] Among them, A0, B0, G0 and D0 are four constants of the best estimation plane ε.

[0031] In a third aspect, a slope estimation method is provided, based on the slope estimation device of the first aspect, comprising:

[0032] The slope estimation device is installed on a vehicle traveling on a road surface whose slope is to be estimated;

[0033] When the vehicle is stationary, a three-dimensional coordinate system of the slope estimation device is established; the three-dimensional coordinate system takes the center of the initial liquid surface of the measuring liquid as the origin O, the ground upward as the positive direction of the Z axis, the vehicle forward direction as the positive direction of the X axis, and the right side of the vehicle forward direction as the positive direction of the Y axis; the four floats are respectively located in the positive direction of the X axis, the negative direction of the X axis, the positive direction of the Y axis, and the negative direction of the Y axis;

[0034] According to the output value of the voltage measuring device in each displacement sensor during the vehicle driving process, the height change values ​​of the four floats are obtained respectively, and according to the installation positions of the four displacement sensors, the current coordinates of the four floats G1 (a, 0, h1), G2 (0, a, h2), G3 (-a, 0, h3), G4 (0, -a, h4) are obtained in the established three-dimensional coordinate system;

[0035] Combine the current coordinates of the four floats into a coordinate matrix H;

[0036]

[0037] Every three floats form an estimated plane, based on the coordinate matrix H and the coordinate change matrix K of each estimated plane. i , construct the inhomogeneous system of equations for all estimated planes;

[0038]

[0039] Among them, A i , B i , C i and D i are four constants of the i-th estimated plane; i = 1, 2, 3, 4;

[0040] Based on the constructed non-homogeneous equations of all estimated planes, the distance operator G of each estimated plane is obtained i ; Among them, the distance operator G i The smallest estimated plane is the best estimated plane ε;

[0041]

[0042]

[0043] Based on the plane equation parameters and distance operator of the best estimated plane, the slope angle θ is obtained to estimate the slope of the driving road surface;

[0044]

[0045] Among them, C0 is a parameter of the plane equation of the best estimated plane ε, and G0 is the distance operator of the best estimated plane.

[0046] Optionally, respectively obtaining the height change values ​​of the four floats includes obtaining the height change value of the mth float, which specifically includes:

[0047] When the vehicle is stationary, obtain the initial voltage output value U of the mth voltage measuring device 0m ;

[0048] During the vehicle's movement, the continuous n voltage output values ​​U of the mth voltage measuring device are collected in real time. on ;

[0049] The average value of every five voltage output values ​​among the n voltage output values ​​is recorded as a voltage calculation value U j , and according to the voltage calculation value U j And the initial voltage output value U 0m , obtain the voltage change value ΔU of the mth voltage measuring device mj ;

[0050] According to the mth voltage change value ΔU mj And the set proportional coefficient, determine the height change value Δh of the mth float m ;

[0051] Δh m =k e ΔU mj

[0052] Among them, k e is the proportionality coefficient.

[0053] Optionally, the method further includes: obtaining an angle α between the vehicle's traveling direction and the direction in which the slope changes most rapidly based on the plane equation parameters of the best estimated plane and the set normal vector of the vehicle's traveling direction;

[0054]

[0055] Among them, A0 and B0 are two parameters of the plane equation of the best estimation plane ε.

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

[0057] (1) The slope estimation device of the present invention estimates the slope of the road on which the vehicle is traveling by measuring the change in the liquid level in the liquid storage bottle 1. The overall structure is simple, the manufacturing is convenient and the cost is low. At the same time, the spoiler mechanism 2 is provided in the liquid storage bottle 1 of the present invention, which can reduce the influence of the fluctuation of the measuring liquid level caused by the acceleration or deceleration of the vehicle, thereby improving the accuracy of the slope estimation, and no additional vehicle is required.

[0058] (2) The slope estimation method of the present invention does not require additional vehicle driving detection sensors, has low requirements on vehicle performance and driving conditions, is simple to calculate, and has high measurement accuracy, which is convenient for measuring basic road surface data in vehicle engineering, thereby effectively controlling the driving of vehicles on slopes. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] Figure 1 is a schematic diagram of the overall structure of the slope estimation device of the present invention;

[0060] Figure 2 is a schematic diagram of an exploded structure of a slope estimation device according to Embodiment 1 of the present invention;

[0061] Figure 3 It is a schematic structural diagram of the displacement sensor of the present invention;

[0062] Figure 4 It is a schematic structural diagram of the bottle cap of the present invention;

[0063] Figure 5 It is a schematic diagram of the position structure of the positioning channel of the present invention;

[0064] Figure 6 is a schematic structural diagram of a slot according to Embodiment 1 of the present invention;

[0065] Figure 7 It is a schematic structural diagram of a spoiler of the present invention;

[0066] Figure 8 It is a structural schematic diagram of the plug board of the present invention;

[0067] Fig. 9 is a schematic structural diagram of a spoiler mechanism according to Embodiment 1 of the present invention;

[0068] Fig.10 It is a schematic diagram of the structure of establishing a three-dimensional coordinate system based on the estimation device of Example 1 in the slope estimation method of the present invention;

[0069] Fig.11 is a schematic diagram of an exploded structure of a slope estimation device according to Embodiment 2 of the present invention;

[0070] Fig.12 A schematic structural diagram of a spoiler mechanism according to Embodiment 2 of the present invention;

[0071] Fig.13 It is a structural schematic diagram of the slope estimation method of the present invention establishing a three-dimensional coordinate system based on the estimation device of Example 2.

[0072] Markings in the figure are: 1 is a liquid storage bottle, 11 is a positioning channel, 12 is a slot, 2 is a spoiler mechanism, 21 is a barrier, 211 is a spoiler hole, 22 is a spoiler plate, 221 is a partition, 23 is a plug plate, 24 is a pin block, 3 is a displacement sensor, 31 is a connecting rod, 32 is a voltage measuring device, 4 is a bottle cap, 41 is a positioning pin, 5 is a float, and 6 is a mounting plate. DETAILED DESCRIPTION

[0073] The present invention will now be described in further detail with reference to the accompanying drawings.

[0074] It should be noted that the terms such as "upper", "lower", "left", "right", "front", "rear", etc. cited in the invention are only for the convenience of description and are not used to limit the scope of the invention. The change or adjustment of their relative relationship shall also be regarded as the scope of the invention without substantial change of technical content. The term "including" and any variation of it in the description and claims of the present invention are intended to cover non-exclusive inclusions.

[0075] Example 1

[0076] like Figure 1-Figure 10 , a slope estimation device is provided, including: a liquid storage bottle 1, a spoiler mechanism 2 and a displacement sensor 3.

[0077] The liquid storage bottle 1 is installed on a vehicle traveling on a road surface whose slope is to be estimated. A bottle cap 4 is provided on the top of the liquid storage bottle 1, and a measuring liquid with a set liquid level height is stored inside. The liquid storage bottle 1 is an open barrel-shaped structure. In order to facilitate the installation of the liquid storage bottle 1, a connecting plate can also be provided at the bottom of the liquid storage bottle 1, and the connecting plate is installed on the vehicle. As an option, the measuring liquid is water or vehicle coolant.

[0078] The spoiler mechanism 2 is located in the liquid storage bottle 1 and divides the interior of the liquid storage bottle 1 into a plurality of chambers; two adjacent chambers are connected through the liquid flow channel of the spoiler mechanism 2 .

[0079] There are four displacement sensors 3, which are installed on the bottle cap 4 in a ring-shaped distribution form; the end of the displacement sensor 3 inserted into the liquid storage bottle 1 is connected to a float 5 floating on the surface of the measuring liquid; when the liquid level of the measuring liquid changes, each float 5 drives the sensing element of the displacement sensor 3 connected to it to produce displacement, so as to obtain the change in the height of each float 5, thereby realizing the estimation of the slope of the road surface on which the vehicle is traveling.

[0080] The provision of the spoiler mechanism 2 can effectively reduce excessive fluctuations in the measuring fluid caused by different driving conditions of the vehicle (constant speed driving, acceleration and deceleration), thereby ensuring the detection accuracy of the displacement sensor 3.

[0081] In this embodiment, there are four chambers, and the displacement sensors 3 are inserted into the chambers one by one. Figure 3 As shown, the displacement sensor 3 includes a connecting rod 31 and a voltage measuring device 32; one end of the connecting rod 31 is connected to the float 5, and the other end extends into the voltage measuring device 32; when the float 5 rises and falls, the connecting rod 31 rises and falls in the voltage measuring device 32, and the voltage measuring device 32 is installed on the bottle cap 4; the displacement sensor 3 can be optionally a resistive displacement sensor 3; when the float 5 rises and falls, the voltage value output by the voltage measuring device 32 changes.

[0082] like Figure 4 and 5 As shown, the longitudinal section of the bottle cap 4 is T-shaped, and a mounting plate 6 is provided near the top of the bottle cap 4 by the spoiler mechanism 2, and the bottle cap 4 and the mounting plate 6 are detachably connected by a bolt structure; a positioning pin 41 is also provided at the end of the bottle cap 4 inserted into the liquid storage bottle 1; there are multiple positioning pins 41, and they surround the bottle cap 4 for a week; a positioning channel 11 corresponding to the positioning pin 41 is provided on the liquid storage bottle 1, and the positioning channel 11 extends from the bottle mouth of the liquid storage bottle 1 to the bottle body and is L-shaped; the positioning pin 41 can move along the positioning channel 11 under the action of external force, that is, the positioning pin 41 moves along the positioning channel 11 and stays at the end of the horizontal section of the positioning channel 11, and the setting of the positioning pin 41 and the positioning channel 11 can limit the connection position of the bottle cap 4 and the bottle body of the liquid storage bottle 1, so as to facilitate the accurate control of the installation position of the displacement sensor 3; it is worth noting that the cross-sectional area of ​​the mounting plate 6 is smaller than the cross-sectional area of ​​the liquid storage bottle 1, that is, the position of the mounting plate 6 does not affect the installation and operation of the displacement sensor 3.

[0083] like Figure 6-Figure 9 As shown, in this embodiment, the spoiler mechanism 2 includes a spoiler plate 22 and a plug plate 23; there are four plug plates 23, which are arranged in a cross shape as a whole; a pin block 24 extending outward is provided at the lower part of the plug plate 23; a slot 12 adapted to the pin block 24 is provided at the bottom of the liquid storage bottle 1, and the pin block 24 and the plug plate 23 are integrally formed; the spoiler plate 22 is detachably connected to the bottle cap 4 through the mounting plate 6, that is, the mounting plate 6 is mounted on the top of the spoiler plate 22; there are four groups of spoilers 22, which are arranged in a cross shape as a whole; each group of spoilers 22 includes two The upper part of the plug plate 23 can be inserted between the two partitions 221 of each group of spoilers 22, that is, the plug plate 23 and the spoiler 22 are partially crossed in the axial direction of the bottle body, and the initial liquid level of the measuring liquid is located at the intersection of the plug plate 23 and the partition 221; the plugging method of the plug plate 23 and the spoiler 22 can refer to the prior art; when the vehicle climbs a slope, the liquid storage bottle 1 tilts, and the measuring liquid flows from the gap between the plug plate 23 and the bottom of the bottle and the gap between the partition 221 and the plug plate 23, thereby changing the height of the four floats 5.

[0084] Example 2

[0085] like Fig.11 and Fig.12 As shown, the only difference between Example 2 and Example 1 is that the spoiler mechanism 2 is a fence 21, and the fence 21 is evenly distributed with spoiler holes 211 to connect two adjacent chambers; the cross section of the fence 21 is a cross shape.

[0086] Example 3

[0087] A slope estimation method is provided, based on any one of the slope estimation devices in Embodiment 1 or Embodiment 2, comprising the following steps:

[0088] S1: Installing a slope estimation device on a vehicle traveling on a road surface whose slope is to be estimated.

[0089] Specifically, the slope fixing device is installed on the vehicle by screws or the like.

[0090] S2: When the vehicle is stationary, establish a three-dimensional coordinate system for the slope estimation device and define the normal vector of the XOY plane

[0091] like Fig.10 Or as shown in 13, the three-dimensional coordinate system takes the center of the initial liquid surface of the measuring liquid as the origin O, the ground upward as the positive direction of the Z axis, the vehicle forward direction as the positive direction of the X axis, and the right side of the vehicle forward direction as the positive direction of the Y axis; the normal vector The four floats 5 are respectively located in the positive direction of the X axis, the negative direction of the X axis, the positive direction of the Y axis, and the negative direction of the Y axis.

[0092] S3: According to the reading of each displacement sensor 3 during the vehicle driving process, the height change value of the four floats 5 relative to the initial liquid level of the measuring liquid is obtained.

[0093] S4: Based on the height change values ​​of the four floats 5 and the installation positions of the four displacement sensors 3 , the current coordinates of the four floats 5 are obtained in the established three-dimensional coordinate system.

[0094] Since the initial liquid level center of the measuring liquid is the origin O, the current coordinates of the four floats 5 are G1 (a, 0, h1), G2 (0, a, h2), G3 (-a, 0, h3), and G4 (0, -a, h4), where a is the distance between the float 5 and the origin O, h1 is the height change value of the first float 5, h2 is the height change value of the second float 5, h3 is the height change value of the third float 5, and h4 is the height change value of the fourth float 5; the height change values ​​of the four floats 5 are obtained by reading the voltage indication of the voltage measuring device 32, and the specific method can refer to the prior art.

[0095] S5: Based on the current coordinates of the four floats 5, the best estimated plane of the current liquid level of the measuring liquid and the normal vector of the best estimated plane are obtained. Normal vector and the normal vector The angle between the two is the slope angle of the road surface, so as to estimate the slope of the road surface.

[0096] Specifically, S51: every three floats 5 form an estimated plane, and the plane equation of the estimated plane is obtained;

[0097] A i x+B i y+C i z+D i =0

[0098] Among them, A i , B i , C i and D i are four constants for the i-th estimated plane; i=1,2,3,4.

[0099] S52: Based on the plane equation of the estimated plane, obtain the distance from the fourth float 5 that is not used to construct the estimated plane to the estimated plane.

[0100] Specifically, floats G1, G2 and G3 form an estimated plane, and the general equation of the estimated plane α is: A1x+B1y+C1z+D1=0; the coordinates of floats G1, G2 and G3 are substituted into the general equation of the estimated plane α, and the parameters A1, B1, C1 and D1 of the general equation of the estimated plane α are obtained, and the distance d1 from float G4 to the estimated plane α is calculated as:

[0101]

[0102] Similarly, the general equation of the estimated plane β where the three points of floats G2, G3, and G4 are located is A2x+B2y+C2z+D2=0, and the distance from G1(a,0,h1) to plane β is

[0103] The general equation of the estimated plane γ where the three floats G1, G3, and G4 are located is A3x+B3y+C3z+D3=0. The distance from G2(0,a,h2) to the estimated plane γ is

[0104] The general equation of the estimated plane δ where the three points of floats G1, G2, and G4 are located is A4x+B4y+C4z+D4=0. The distance from G3 (-a, 0, h3) to the estimated plane δ is

[0105] S53: Among all the estimated planes, select the estimated plane corresponding to the minimum distance as the best estimated plane ε.

[0106] The estimated plane corresponding to d=min{d1, d2, d3, d4} is the optimal estimated plane ε.

[0107] The slope angle θ of the driving road is:

[0108]

[0109] Among them, A0, B0, C0 and D0 are four constants of the best estimation plane ε.

[0110] The best estimated plane is one of the estimated planes α, β, γ, and δ.

[0111] The slope angle θ of the driving road is expressed as a percentage Q, and the slope Q(t) is obtained by median filtering.

[0112] Q = tanθ×100%.

[0113] Example 4

[0114] A slope estimation method is provided, based on the slope estimation device described in any one of Embodiment 1 or Embodiment 2, comprising the following steps:

[0115] D1: Install the slope estimation device on a vehicle traveling on a road surface whose slope is to be estimated.

[0116] D2: When the vehicle is stationary, establish a three-dimensional coordinate system for the slope estimation device; Fig.10 Or as shown in 13, the three-dimensional coordinate system takes the center of the initial liquid surface of the measuring liquid as the origin O, the ground upward as the positive direction of the Z axis, the vehicle forward direction as the positive direction of the X axis, and the right side of the vehicle forward direction as the positive direction of the Y axis; the four floats 5 are respectively located in the positive direction of the X axis, the negative direction of the X axis, the positive direction of the Y axis and the negative direction of the Y axis.

[0117] D3: According to the output value of the voltage measuring device 32 in each displacement sensor 3 during the vehicle driving process, the height change values ​​of the four floats 5 are obtained respectively, and according to the installation positions of the four displacement sensors 3, the current coordinates G1 (a, 0, h1), G2 (0, a, h2), G3 (-a, 0, h3), G4 (0, -a, h4) of the four floats 5 are obtained in the established three-dimensional coordinate system.

[0118] In this embodiment, respectively obtaining the height change values ​​of the four floats 5 includes obtaining the height change value of the mth float 5, which specifically includes:

[0119] D31: When the vehicle is stationary, obtain the initial voltage output value U of the mth voltage measuring device 320m ;

[0120] D32: During the vehicle's travel, real-time collection of n consecutive voltage output values ​​U of the mth voltage measuring device 32 on ;

[0121] The number n can be determined empirically.

[0122] D33: The average value of every five voltage output values ​​among n voltage output values ​​is recorded as a voltage calculation value U j , and according to the voltage calculation value U j And the initial voltage output value U 0m , obtain the voltage change value ΔU of the mth voltage measuring device 32 mj ;

[0123] Specifically, the voltage calculation value U j (j=1,2,3,,,), The number of voltage calculation values ​​is determined according to the number of voltage output values.

[0124] ΔU mj =U j -U 0m .

[0125] D34: According to the mth voltage change value ΔU mj and the set proportional coefficient to determine the height change Δh of the mth float 5 m ;

[0126] Δh m =k e ΔU mj

[0127] Among them, k e is the proportionality coefficient, which is determined according to expert experience; the height change value of each float 5 is the Z-axis value of the coordinate of each float 5.

[0128] D4: Combine the current coordinates of the four floats 5 into a coordinate matrix H;

[0129]

[0130] D5: Every three floats 5 form an estimated plane, based on the coordinate matrix H and the coordinate change matrix K of each estimated plane i , construct the inhomogeneous system of equations for all estimated planes;

[0131]

[0132] Among them, A i , B i , Ci and D i are four constants of the i-th estimated plane; i = 1, 2, 3, 4;

[0133] and

[0134] D6: Based on the constructed non-homogeneous equations of all estimated planes, obtain the distance operator G for each estimated plane i ; Among them, the distance operator G i The smallest estimated plane is the best estimated plane ε;

[0135]

[0136] Specifically,

[0137]

[0138] The Hadamard product F1 of the vector is obtained by calculation:

[0139]

[0140] The distance operator G1 of the first estimated plane is:

[0141]

[0142] Similarly,

[0143]

[0144] The plane corresponding to the distance operator G=min{G1,G2,G3,G4} is the best estimated plane.

[0145] D7: Based on the plane equation parameters and distance operator of the best estimated plane, the slope angle θ is obtained to estimate the slope of the driving road;

[0146]

[0147] Among them, C0 is a parameter of the plane equation of the best estimated plane ε, and G0 is the distance operator of the best estimated plane.

[0148] The other three parameters of the plane equation for the best estimate plane ε are A0, B0 and D0.

[0149] The slope angle θ of the driving road is expressed as a percentage Q, and the slope Q(t) is obtained by median filtering.

[0150] Q = tanθ×100%.

[0151] In this embodiment, the slope estimation method further includes: obtaining an angle α between the vehicle's traveling direction and the direction in which the slope changes most rapidly based on the plane equation parameters of the best estimated plane and the normal vector of the set vehicle's traveling direction;

[0152]

[0153] From the above formula, we can obtain the direction vector perpendicular to the direction with the fastest slope change:

[0154] Normal vector of the vehicle's direction of travel

[0155]

[0156] Among them, A0 and B0 are two parameters of the plane equation of the best estimated plane ε; the angle α between the vehicle's direction of travel and the direction in which the slope changes most rapidly and the direction vector perpendicular to the direction in which the slope changes most rapidly It can help determine whether the direction in which the current slope changes fastest is the vehicle's forward direction or the vehicle's left and right direction, that is, determine whether it is a horizontal slope or a longitudinal slope. The estimation method of the present invention is simpler and more accurate.

[0157] The various embodiments in this specification are described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the various embodiments can be referenced to each other.

[0158] The above are only preferred embodiments of the present invention, and the protection scope of the present invention is not limited to the above embodiments. All technical solutions under the concept of the present invention belong to the protection scope of the present invention. It should be pointed out that for ordinary technicians in this technical field, some improvements and modifications without departing from the principle of the present invention should be regarded as the protection scope of the present invention.

Claims

1. A slope estimation device, characterized in that: include: Liquid storage bottle (1), flow disturbance mechanism (2) and displacement sensor (3); The liquid storage bottle (1) is installed on a vehicle traveling on a road surface whose slope is to be estimated, and a bottle cap (4) is provided on the top of the liquid storage bottle (1), wherein a measuring liquid with a set liquid level height is stored inside; The flow-disturbing mechanism (2) is located in the liquid storage bottle (1) and divides the interior of the liquid storage bottle (1) into a plurality of chambers; two adjacent chambers are connected via a liquid flow channel of the flow-disturbing mechanism (2); There are four displacement sensors (3) which are installed on the bottle cap (4) in a circular distribution form; the end of the displacement sensor (3) inserted into the liquid storage bottle (1) is connected to a float (5) floating on the surface of the measuring liquid; when the liquid level of the measuring liquid changes, each of the floats (5) drives the sensing element of the displacement sensor (3) connected thereto to generate displacement, so as to obtain the change in the height of each float (5), thereby realizing the estimation of the slope of the road surface on which the vehicle is traveling.

2. The slope estimation device according to claim 1, characterized in that: There are four chambers, and the displacement sensors (3) are inserted into the chambers one by one. The displacement sensors (3) include a connecting rod (31) and a voltage measuring device (32). One end of the connecting rod (31) is connected to the float (5), and the other end extends into the voltage measuring device (32). When the float (5) rises and falls, the connecting rod (31) rises and falls in the voltage measuring device (32), and the voltage measuring device (32) is installed on the bottle cap (4).

3. The slope estimation device according to claim 1, characterized in that: The flow disturbance mechanism (2) is an isolation grille (21), and the isolation grille (21) is evenly distributed with flow disturbance holes (211) to connect two adjacent chambers; the cross section of the isolation grille (21) is in a cross shape.

4. The slope estimation device according to claim 1, characterized in that: The spoiler mechanism (2) comprises a spoiler (22) and a plugging plate (23); the plugging plates (23) are four in number and are arranged in a cross shape as a whole; a pin block (24) extending outward is arranged at the lower part of the plugging plate (23); a slot (12) adapted to the pin block (24) is arranged at the bottom of the liquid storage bottle (1); the spoiler (22) is connected to the bottle cap (4); the spoiler (22) comprises four groups and are arranged in a cross shape as a whole; each group of the spoiler (22) comprises two partitions (221) arranged at a set interval; the upper part of the plugging plate (23) can be inserted between the two partitions (221) of each group of spoilers (22), and the initial liquid level of the measuring liquid is located at the intersection of the plugging plate (23) and the partition (221).

5. The slope estimation device according to claim 1, characterized in that: The longitudinal section of the bottle cap (4) is T-shaped, the spoiler mechanism (2) is provided with a mounting plate (6) near the top of the bottle cap (4), and the bottle cap (4) is detachably connected to the mounting plate (6); The end of the bottle cap (4) inserted into the liquid storage bottle (1) is also provided with a positioning pin (41); there are a plurality of positioning pins (41); the liquid storage bottle (1) is provided with positioning channels (11) corresponding to the positioning pins (41) one by one, the positioning channels (11) extending from the bottle mouth of the liquid storage bottle (1) to the bottle body and being L-shaped; the positioning pins (41) can move along the positioning channels (11) under the action of an external force.

6. A slope estimation method, based on the slope estimation device according to any one of claims 1 to 5, characterized in that: include: The slope estimation device is installed on a vehicle traveling on a road surface whose slope is to be estimated; When the vehicle is stationary, a three-dimensional coordinate system of the slope estimation device is established and the normal vector of the XOY plane is defined. According to the reading of each displacement sensor (3) during the vehicle's running process, the height change value of the four floats (5) relative to the initial liquid level of the measuring liquid is obtained; Based on the height change values ​​of the four floats (5) and the installation positions of the four displacement sensors (3), the current coordinates of the four floats (5) are obtained in the established three-dimensional coordinate system; Based on the current coordinates of the four floats (5), the best estimated plane of the current liquid level of the measuring liquid and the normal vector of the best estimated plane are obtained. Normal vector and the normal vector The angle between the two is the slope angle of the road surface, so as to estimate the slope of the road surface.

7. The slope estimation method according to claim 6, characterized in that: The three-dimensional coordinate system takes the center of the initial liquid surface of the measuring liquid as the origin O, the ground upward as the positive direction of the Z axis, the vehicle forward direction as the positive direction of the X axis, and the right side of the vehicle forward direction as the positive direction of the Y axis; the normal vector The four floats (5) are respectively located in the positive direction of the X axis, the negative direction of the X axis, the positive direction of the Y axis and the negative direction of the Y axis; The method obtains the best estimated plane of the current liquid level of the measuring liquid and the normal vector of the best estimated plane based on the current coordinates of the four floats (5). include: Every three floats (5) form an estimated plane, and the plane equation of the estimated plane is obtained; A i x+B i y+C i z+D i =0 Among them, A i , B i , C i and D i are four constants of the i-th estimated plane; i = 1, 2, 3, 4; Based on the plane equation of the estimated plane, obtaining the distance from the fourth float (5) not used to construct the estimated plane to the estimated plane; Among all the estimated planes, the estimated plane corresponding to the minimum distance is selected as the best estimated plane ε; The slope angle θ of the driving road is: Among them, A0, B0, C0 and D0 are four constants of the best estimation plane ε.

8. A slope estimation method, based on the slope estimation device according to claim 2, characterized in that: include: The slope estimation device is installed on a vehicle traveling on a road surface whose slope is to be estimated; When the vehicle is stationary, a three-dimensional coordinate system of the slope estimation device is established; The three-dimensional coordinate system takes the center of the initial liquid surface of the measuring liquid as the origin O, the ground upward as the positive direction of the Z axis, the vehicle forward direction as the positive direction of the X axis, and the right side of the vehicle forward direction as the positive direction of the Y axis; the four floats (5) are respectively located in the positive direction of the X axis, the negative direction of the X axis, the positive direction of the Y axis and the negative direction of the Y axis; According to the output value of the voltage measuring device (32) in each displacement sensor (3) during the vehicle driving process, the height change values ​​of the four floats (5) are respectively obtained, and according to the installation positions of the four displacement sensors (3), the current coordinates G1 (a, 0, h1), G2 (0, a, h2), G3 (-a, 0, h3), G4 (0, -a, h4) of the four floats (5) are obtained in the established three-dimensional coordinate system; Combine the current coordinates of the four floats (5) into a coordinate matrix H; Every three floats (5) form an estimated plane, based on the coordinate matrix H and the coordinate change matrix K of each estimated plane i , construct the inhomogeneous system of equations for all estimated planes; Among them, A i , B i , C i and D i are four constants of the i-th estimated plane; i = 1, 2, 3, 4; Based on the constructed non-homogeneous equations of all estimated planes, the distance operator G of each estimated plane is obtained i ; Among them, the distance operator G i The smallest estimated plane is the best estimated plane ε; Based on the plane equation parameters and distance operator of the best estimated plane, the slope angle θ is obtained to estimate the slope of the driving road surface; Among them, C0 is a parameter of the plane equation of the best estimated plane ε, and G0 is the distance operator of the best estimated plane.

9. The slope estimation method according to claim 8, characterized in that: The step of respectively obtaining the height change values ​​of the four floats (5) includes obtaining the height change value of the mth float (5), and specifically includes: When the vehicle is stationary, an initial voltage output value U of the mth voltage measuring device (32) is obtained. 0m ; During the movement of the vehicle, n consecutive voltage output values ​​U of the mth voltage measuring device (32) are collected in real time. on ; The average value of every five voltage output values ​​among the n voltage output values ​​is recorded as a voltage calculation value U j , and according to the voltage calculation value U j And the initial voltage output value U 0m , obtain the voltage change value ΔU of the mth voltage measuring device (32) mj ; According to the mth voltage change value ΔU mj and the set proportionality coefficient to determine the height change Δh of the mth float (5) m ; Δh m =k e ΔU mj Among them, k e is the proportionality coefficient.

10. The slope estimation method according to claim 8, characterized in that: Also includes: Based on the plane equation parameters of the best estimated plane and the normal vector of the set vehicle travel direction, the angle α between the vehicle travel direction and the direction where the slope changes most rapidly is obtained; Among them, A0 and B0 are two parameters of the plane equation of the best estimation plane ε.

Images