A slope estimation device and method

Abstract

The application discloses a kind of slope estimation device and method, belong to the technical field of road slope detection, device includes liquid storage bottle, spoiler mechanism and displacement sensor;Liquid storage bottle is installed on the vehicle that runs in the slope road surface to be estimated, the top of liquid storage bottle is provided with bottle cap, and the inside is stored with the measuring liquid of setting liquid level;Spoiler mechanism is located in liquid storage bottle, and the inside of the liquid storage bottle is divided into multiple chambers;Two adjacent chambers are communicated by the liquid flow channel of spoiler mechanism;Displacement sensor has four, and is installed in the form of annular distribution on the bottle cap;The end of displacement sensor is connected with the float that floats in the liquid level of measuring liquid;By obtaining the change amount of the height of each float, the slope estimation of vehicle running road surface is realized.The application can estimate the slope of vehicle running road under different working conditions, and the performance requirement of vehicle is low.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of road slope detection, and particularly relates to a slope estimation device and method. BACKGROUND

[0002] The slope of a road on which a vehicle travels is an important vehicle state quantity, and is related to the control accuracy of a control system of the vehicle. At present, slope estimation of a road on which a vehicle travels mainly adopts a slope estimation method based on a kinematics model and a slope estimation method based on a dynamics model.

[0003] The slope estimation method based on the kinematics model pays attention to motion characteristics such as the shape of an object, a motion joint, a motion equation, a trajectory, displacement, velocity and acceleration, and the estimation accuracy of the slope is greatly affected when the vehicle posture changes. The slope estimation based on the dynamics model depends on a vehicle model, pays attention to the motion state and force condition of an object, and is greatly affected by various parameters of the vehicle itself, so that the slope estimation value is deviated. SUMMARY

[0004] The present application provides a slope estimation device and method, which can estimate the slope of a road on which a vehicle travels under different working conditions.

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

[0006] In a first aspect, a slope estimation device is provided, comprising a liquid storage bottle, a flow disturbance mechanism and displacement sensors.

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

[0008] The flow disturbance mechanism is located in the liquid storage bottle, and divides the interior of the liquid storage bottle into multiple chambers. Two adjacent chambers are communicated through a liquid flow channel of the flow disturbance mechanism.

[0009] The displacement sensors are four in number, and are installed in a ring-shaped distribution on the bottle cap. The end of each displacement sensor inserted into the liquid storage bottle is connected with a float floating on the liquid surface of the measuring liquid. When the liquid level of the measuring liquid changes, each float drives the sensing element of the displacement sensor connected therewith to produce displacement, so as to obtain the change amount of the height of each float, and further realize slope estimation of the road surface on which the vehicle travels.

[0010] Optionally, the chamber has four, the displacement sensor one-one corresponding to the insertion into the chamber, the displacement sensor includes connecting rod and voltage measurement device; the connecting rod is connected with the float at one end, the other end extends into the voltage measurement device; when the float rises and falls, the connecting rod rises and falls in the voltage measurement device, the voltage measurement device is installed on the bottle cap.

[0011] Optionally, the spoiler mechanism is a partition grid, and the partition grid is uniformly distributed with spoiler holes to communicate adjacent two chambers; the cross section of the partition grid is a cross shape.

[0012] Optionally, the spoiler mechanism includes spoiler plate and plug plate; the plug plate is four and is cross-distributed as a whole; the lower part of the plug plate is provided with an outwardly extending plug block; the bottom of the liquid storage bottle is provided with a plug slot matched with the plug block; the spoiler plate is connected with the bottle cap, and the spoiler plate has four groups and is cross-distributed as a whole; each group of the spoiler plate includes two partition plates distributed at a set interval; the upper part of the plug plate can be inserted between the two partition plates of each group of the spoiler plate, and the initial liquid level of the measuring liquid is located at the intersection part of the plug plate and the partition plate.

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

[0014] The end of the bottle cap inserted into the liquid storage bottle is further provided with a positioning pin; the positioning pin has a plurality of; the liquid storage bottle is provided with a positioning channel corresponding to the positioning pin, the positioning channel extends from the bottle mouth to the bottle body and is L-shaped; the positioning pin can move along the positioning channel under the action of external force.

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

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

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

[0018] According to the indication of each displacement sensor during the running of the vehicle, the height change values of the four floats relative to the initial liquid level of the measuring liquid are 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, a best estimation plane of the current liquid level of the measuring liquid and a normal vector of the best estimation plane are obtained Normal vector The included angle between the normal vector and the normal vector is the slope angle of the driving surface, and the slope estimation of the driving surface is realized.

[0021] Optionally, the three-dimensional coordinate system takes the initial liquid level center of the measuring liquid as the origin O, takes the ground upward as the positive direction of the Z axis, takes the forward direction of the vehicle as the positive direction of the X axis, and takes the right side of the forward direction of the vehicle as the positive direction of the Y axis; the normal vector 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.

[0022] Based on the current coordinates of the four floats, a best estimation plane of the current liquid level of the measuring liquid and a normal vector of the best estimation plane are obtained Comprise:

[0023] Each three floats constitute an estimation plane, and a plane equation of the estimation plane is obtained.

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

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

[0026] Based on the plane equation of the estimation plane, the distance from the fourth float not used to construct the estimation plane to the estimation plane is obtained.

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

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

[0029]

[0030] Wherein, 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 driving on a road surface whose slope is to be estimated.

[0033] A three-dimensional coordinate system of the slope estimation device is established when the vehicle is stationary; the three-dimensional coordinate system takes the initial liquid surface center of the measuring liquid as the origin O, takes the ground upward as the positive direction of the Z axis, takes the vehicle forward direction as the positive direction of the X axis, and takes the right side of the vehicle forward direction as the positive direction of the Y axis; 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 values of the voltage measuring devices 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 G1(a, 0, h1), G2(0, a, h2), G3(-a, 0, h3), and G4(0, -a, h4) of the four floats are obtained in the established three-dimensional coordinate system;

[0035] The current coordinates of the four floats are combined into a coordinate matrix H;

[0036]

[0037] Each three floats constitute an estimation plane, and based on the coordinate matrix H and the set coordinate change matrix K of each estimation plane i , a non-homogeneous equation group of all estimation planes is constructed;

[0038]

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

[0040] Based on the constructed non-homogeneous equation group of all estimation planes, the distance operator G i of each estimation plane is obtained; wherein, the estimation plane with the smallest distance operator G i is the best estimation plane ε;

[0041]

[0042]

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

[0044]

[0045] Wherein, C0 is a parameter of the plane equation of the best estimation plane ε, and G0 is the distance operator of the best estimation plane.

[0046] Optionally, obtaining the height change values ​​of the four floats respectively includes obtaining the height change value of the m-th float, specifically including:

[0047] With the vehicle stationary, acquire the initial voltage output value U of the m-th voltage measuring device. 0m ;

[0048] During vehicle movement, the n consecutive voltage output values ​​U of the m-th voltage measuring device are collected in real time. on ;

[0049] The average of every five voltage output values ​​out of n voltage output values ​​is denoted as a voltage calculation value U. j And based on the voltage calculation value U j and initial voltage output value U 0m Obtain the voltage change value ΔU of the m-th voltage measuring device. mj ;

[0050] Based on the m-th voltage change value ΔU mj Based on the set proportional coefficient, determine the height change value Δh of the m-th float. m ;

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

[0052] Where, k e This is the proportionality coefficient.

[0053] Optionally, it also includes: obtaining the angle α between the vehicle's direction of travel and the direction of the most rapid slope change based on the plane equation parameters of the best estimated plane and the normal vector of the set vehicle travel direction;

[0054]

[0055] Here, A0 and B0 are two parameters of the plane equation for the best estimate of plane ε.

[0056] Compared with the prior art, the beneficial effects of the present invention are:

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

[0058] (2) The slope estimation method of the present application does not need additional vehicle running detection sensors, has low requirements on the performance and running conditions of the vehicle, is simple in calculation, has high measurement precision, is convenient for road surface basic data measurement in vehicle engineering, and thus effectively controls the running of the vehicle on a slope. BRIEF DESCRIPTION OF DRAWINGS

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

[0060] Figure 2 is a schematic diagram of the structure of the slope estimation device of embodiment 1 of the present application;

[0061] Figure 3 is a schematic diagram of the structure of the displacement sensor of the present application;

[0062] Figure 4 is a schematic diagram of the structure of the bottle cap of the present application;

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

[0064] Figure 6 is a schematic diagram of the structure of the slot of embodiment 1 of the present application;

[0065] Figure 7 is a schematic diagram of the structure of the spoiler of the present application;

[0066] Figure 8 is a schematic diagram of the structure of the plugboard of the present application;

[0067] Figure 9 is a schematic diagram of the structure of the spoiler mechanism of embodiment 1 of the present application;

[0068] Figure 10 is a schematic diagram of the structure of the three-dimensional coordinate system established by the estimation device of the slope estimation method of the present application based on embodiment 1;

[0069] Figure 11 is a schematic diagram of the structure of the slope estimation device of embodiment 2 of the present application;

[0070] Figure 12 is a schematic diagram of the structure of the spoiler mechanism of embodiment 2 of the present application;

[0071] Figure 13 is a schematic diagram of the structure of the three-dimensional coordinate system established by the estimation device of the slope estimation method of the present application based on embodiment 2.

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

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

[0074] It should be noted that the terms such as "upper", "lower", "left", "right", "front", "back" and the like cited in the application are only for the convenience of clear description, and are not intended to limit the scope of the application. The change or adjustment of the relative relationship without substantial change of the technical content is also regarded as the implementation scope of the application. The term "comprising" and any variation thereof in the specification and claims is intended to cover non-exclusive inclusion.

[0075] Example 1

[0076] As Figures 1-10 , a slope estimation device is provided, comprising a liquid storage bottle 1, a disturbance mechanism 2 and a displacement sensor 3.

[0077] The liquid storage bottle 1 is installed on a vehicle running on a slope to be estimated, the top of the liquid storage bottle 1 is provided with a bottle cap 4, and the inside stores a measuring liquid with a set liquid level; the liquid storage bottle 1 is an open barrel 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 disturbance mechanism 2 is located in the liquid storage bottle 1, and the inside of the liquid storage bottle 1 is divided into multiple chambers by the disturbance mechanism 2; adjacent two chambers are communicated through the liquid flow channel of the disturbance mechanism 2.

[0079] The displacement sensor 3 has four, and is installed in a ring distribution form on the bottle cap 4; the end of the displacement sensor 3 inserted into the liquid storage bottle 1 is connected with a float 5 floating on the liquid level 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 thereto to produce displacement, so as to obtain the change amount of the height of each float 5, and then realize the estimation of the slope of the vehicle running road.

[0080] The disturbance mechanism 2 can effectively reduce the excessive fluctuation of the measuring liquid caused by different working conditions of the vehicle (uniform speed running, acceleration and deceleration), so as to ensure the detection accuracy of the displacement sensor 3.

[0081] In the embodiment, the chamber has four, displacement sensor 3-one-one corresponding to the insertion chamber, as shown in Figure 3 The displacement sensor 3 includes a connecting rod 31 and a voltage measuring device 32; one end of the connecting rod 31 is connected with 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 is an optional resistance displacement sensor 3; when the float 5 rises and falls, the voltage value output by the voltage measuring device 32 changes.

[0082] As shown in Figure 4 and 5 , the longitudinal section of the bottle cap 4 is T-shaped, and the installation plate 6 is arranged near the top of the bottle cap 4; the bottle cap 4 and the installation plate 6 are detachably connected by a bolt structure; the bottle cap 4 inserted into the end of the liquid storage bottle 1 is also provided with a plurality of positioning pins 41; the positioning pins 41 are arranged around the bottle cap 4; the liquid storage bottle 1 is provided with a positioning channel 11 corresponding to the positioning pin 41; the positioning channel 11 extends from the bottle mouth 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 stays at the end of the horizontal section of the positioning channel 11 after moving along the positioning channel 11; the positioning 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, thereby facilitating the accurate control of the installation position of the displacement sensor 3; it is worth noting that the cross-sectional area of the installation plate 6 is smaller than that of the liquid storage bottle 1, that is, the position of the installation plate 6 does not affect the installation and work of the displacement sensor 3.

[0083] As shown in Figures 6-9 , in the embodiment, the spoiler mechanism 2 includes a spoiler plate 22 and a plug plate 23; the plug plate 23 is four and is cross-distributed as a whole; the lower part of the plug plate 23 is provided with an outwardly extending plug block 24; the bottom of the liquid storage bottle 1 is provided with a plug groove 12 matched with the plug block 24, and the plug block 24 is integrally formed with the plug plate 23; the spoiler plate 22 is detachably connected with the bottle cap 4 through the installation plate 6, that is, the installation plate 6 is installed on the top of the spoiler plate 22; the spoiler plate 22 has four groups and is cross-distributed as a whole; each group of spoiler plates 22 includes two spaced apart partitions 221; the upper part of the plug plate 23 can be inserted between the two partitions 221 of each group of spoiler plates 22, that is, the plug plate 23 and the spoiler plate 22 partially intersect in the axial direction of the bottle body, and the initial liquid level of the measuring liquid is located at the intersection part of the plug plate 23 and the partition 221; the plug plate 23 and the spoiler plate 22 can be combined in the manner of referring to the prior art; when the vehicle climbs, the liquid storage bottle 1 is inclined, and the measuring liquid flows from the gap between the plug plate 23 and the bottle bottom and the gap between the partition 221 and the plug plate 23, so that the height of the four floats 5 changes.

[0084] Embodiment 2

[0085] As Figure 11 and Figure 12 shown, the difference between Example 2 and Example 1 is only that the flow disturbing mechanism 2 is a partition grid 21, and the partition grid 21 is uniformly distributed with flow disturbing holes 211 to connect two adjacent chambers; the cross section of the partition grid 21 is cross-shaped.

[0086] Example 3

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

[0088] S1: Install the slope estimation device on a vehicle running on the 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: In the state that the vehicle is stationary, establish a three-dimensional coordinate system of the slope estimation device, and define the normal vector of the XOY plane

[0091] As Figure 10 or 13 shown, the three-dimensional coordinate system takes the center of the initial liquid surface of the measuring liquid as the origin O, takes the upward direction of the ground as the positive direction of the Z axis, takes the forward direction of the vehicle as the positive direction of the X axis, and takes the right side of the vehicle in the forward direction as the positive direction of the Y axis; the normal vector of the XOY plane is 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 readings of each displacement sensor 3 during the running of the vehicle, obtain the height change values of the four floats 5 relative to the initial liquid surface of the measuring liquid.

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

[0094] Since the center of the initial liquid surface 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 of the float 5 from 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 readings of the voltage measuring device 32, and the specific manner can refer to the prior art.

[0095] S5: Obtain the best estimation plane of the current liquid level of the measuring liquid and the normal vector of the best estimation plane based on the current coordinates of the four floats 5 Normal vector The included angle between the normal vector and the vector is the slope angle of the driving surface, and the slope estimation of the driving surface is realized.

[0096] Specifically, S51: three floats 5 constitute an estimation plane, and the plane equation of the estimation plane is obtained;

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

[0098] Wherein, A i , B i , C i and D i are four constants of the i-th estimation plane; i=1, 2, 3, 4.

[0099] S52: Obtain the distance from the fourth float 5 not used to construct the estimation plane to the estimation plane based on the plane equation of the estimation plane.

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

[0101]

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

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

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

[0105] ​S53: In all estimation planes, select the estimation plane corresponding to the minimum distance as the best estimation plane ε.

[0106] d = min{d1, d2, d3, d4} corresponds to the best estimation plane ε.

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

[0108]

[0109] Where A0, B0, C0and D0are four constants of the best estimation plane ε.

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

[0111] The slope angle θ of the driving surface is expressed in 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 of any one of embodiments 1 or 2, comprising the following steps:

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

[0116] D2: Establish a three-dimensional coordinate system of the slope estimation device when the vehicle is stationary; as shown in FIG. 13, the three-dimensional coordinate system takes the initial liquid surface center of the measuring liquid as the origin O, takes the upward direction of the ground as the positive direction of the Z axis, takes the forward direction of the vehicle as the positive direction of the X axis, and takes the right side of the forward direction of the vehicle 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. Figure 10

[0117] D3: According to the output value of the voltage measuring device 32 in each displacement sensor 3 during the driving process of the vehicle, the height change value of each float 5 is obtained, and according to the installation position 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, the height change value of each float 5 is obtained, including obtaining the height change value of the mth float 5, specifically including:

[0119] D31: In the stationary state of the vehicle, the initial voltage output value U of the mth voltage measuring device 32 is obtained.​0m ;

[0120] D32: Collecting n continuous voltage output values U of the mth voltage measuring device 32 in real time during the vehicle running process on ;

[0121] The number of n can be determined empirically.

[0122] D33: Taking the average value of every five voltage output values as a voltage operation value U j , and according to the voltage operation value U j and the initial voltage output value U 0m , obtaining the voltage change value ΔU mj of the mth voltage measuring device 32 ;

[0123] Specifically, the voltage operation value U j (j = 1, 2, 3,,,) The number of voltage operation 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, determining the height change value Δh m of the mth float 5 ;

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

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

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

[0129]

[0130] D5: Each three floats 5 constitutes an estimation plane, and based on the coordinate matrix H and the set coordinate change matrix K i of each estimation plane, a non-homogeneous equation group of all estimation planes is constructed ;

[0131]

[0132] Wherein, 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 equation set of all estimated planes, obtain the distance operator G of each estimated plane i ; wherein 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 vectors is obtained by operation:

[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 the distance operator of the best estimated plane, obtain the slope angle θ, and realize the slope estimation of the driving road surface;

[0146]

[0147] wherein 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 of the best estimated plane ε are A0, B0 and D0.

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

[0150] Q = tan θ × 100%.

[0151] In the embodiment, the slope estimation method further comprises: obtaining an angle α between the vehicle travel direction and the direction with the fastest slope change based on the plane equation parameters of the best estimation plane and the normal vector of the set vehicle travel direction;

[0152]

[0153] The direction vector perpendicular to the direction with the fastest slope change can be obtained from the above formula

[0154] The normal vector of the vehicle travel direction

[0155]

[0156] Wherein, A0 and B0 are two parameters of the plane equation of the best estimation plane ε; the angle α between the vehicle travel direction and the direction with the fastest slope change and the direction vector perpendicular to the direction with the fastest slope change The estimation method can help to determine whether the direction with the fastest slope change is the vehicle travel direction or the left and right directions of the vehicle, i.e. to determine whether it is a transverse slope or a longitudinal slope, and the estimation method is more simple and accurate.

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

[0158] The above is only the preferred embodiment of the present application, and the protection scope of the present application is not limited to the above-mentioned embodiments, and any technical solution falling within the concept of the present application shall fall within the protection scope of the present application. It should be noted that, for ordinary skilled in the art, some improvements and refinements without departing from the principles of the present application shall be considered as the protection scope of the present application.

Claims

1. A slope estimation method, based on a slope estimation device, characterized in that, include: The slope estimation device is installed on a vehicle traveling on the slope of the road surface to be estimated; With the vehicle stationary, establish a three-dimensional coordinate system for the slope estimation device and define the normal vector of the XOY plane. Based on the readings of each displacement sensor (3) during vehicle operation, the height change values ​​of the four floats (5) relative to the initial liquid surface of the measuring liquid are 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), obtain the optimal estimation plane of the current liquid surface and the normal vector of the optimal estimation plane. normal vector and normal vector The included angle is the slope angle of the driving road surface, which enables the estimation of the slope of the driving road surface; The three-dimensional coordinate system has its origin O at the initial center of the measured liquid surface, with the positive Z-axis pointing upwards from the ground, the positive X-axis pointing in the direction of vehicle movement, and the positive Y-axis pointing to the right of the direction of vehicle movement; the normal vector... The four floats (5) are located in the positive X-axis direction, the negative X-axis direction, the positive Y-axis direction, and the negative Y-axis direction, respectively; Based on the current coordinates of the four floats (5), the optimal estimation plane of the current liquid surface and the normal vector of the optimal estimation plane are obtained. include: Every three floats (5) form an estimation plane, and the plane equation of the estimation 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 These are the four constants for the i-th estimated plane; i = 1, 2, 3, 4; Based on the plane equation of the estimated plane, the distance from the fourth float (5) not used to construct the estimated plane to the estimated plane is obtained; 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 surface is: Among them, A0, B0, C0, and D0 are four constants for the optimal estimation of the plane ε; The slope estimation device includes: a liquid storage bottle (1), a flow disturbance mechanism (2), and a displacement sensor (3); The liquid storage bottle (1) is installed on a vehicle traveling on a road surface with an estimated slope. The top of the liquid storage bottle (1) is provided with a bottle cap (4), and the inside stores a measuring liquid with a set liquid level. The flow disturbance mechanism (2) is located inside the liquid storage bottle (1) and divides the interior of the liquid storage bottle (1) into multiple chambers; two adjacent chambers are connected through the liquid flow channel of the flow disturbance mechanism (2); There are four displacement sensors (3), which are installed on the bottle cap (4) in a ring-shaped arrangement. 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 height of the measuring liquid changes, each float (5) drives the sensing element of the displacement sensor (3) connected to it to generate displacement, so as to obtain the change in height of each float (5) and thus realize the slope estimation of the road surface on which the vehicle is traveling. The flow-dispersing mechanism (2) includes a flow-dispersing plate (22) and an insert plate (23); there are four insert plates (23) arranged in a cross shape as a whole; the lower part of the insert plate (23) is provided with an outwardly extending insert block (24); the bottom of the liquid storage bottle (1) is provided with a slot (12) adapted to the insert block (24); the flow-dispersing plate (22) is connected to the bottle cap (4), and there are four sets of flow-dispersing plates (22) arranged in a cross shape as a whole; each set of flow-dispersing plates (22) includes two partitions (221) distributed at a set interval; the upper part of the insert plate (23) can be inserted between the two partitions (221) of each set of flow-dispersing plates (22), and the initial liquid level of the measuring liquid is located at the intersection of the insert plate (23) and the partition (221).

2. The slope estimation method 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 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). The voltage measuring device (32) is installed on the bottle cap (4).

3. The slope estimation method according to claim 1, characterized in that, The bottle cap (4) has a T-shaped longitudinal section. The turbulence mechanism (2) is provided with a mounting plate (6) near the top of the bottle cap (4). 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 multiple 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) extend from the bottle mouth of the liquid storage bottle (1) to the bottle body and are L-shaped; the positioning pins (41) can move along the positioning channels (11) under the action of external force.

Images