Method and system of angle estimation

Abstract

A method of angle estimation for use in a vehicle which is travelling on a surface. The vehicle includes a vehicle body having a first axis and being attached to at least two wheels. The method includes the steps of: providing a first height sensor for measuring h.sub.1, the height of the vehicle body with respect to the first wheel; providing a second height sensor for measuring h.sub.2, the height of the vehicle body with respect to the second wheel; providing a surface angle sensor for measuring .sub.road, the angle of the surface in relation to a horizontal plane; measuring the values of h.sub.1, h.sub.2 and .sub.road; using the values of h.sub.1 and h.sub.2 to calculate .sub.rel, the angle of the vehicle body relative to the surface; and calculating an estimate of .sub.glob, the angle between the first axis and the horizontal plane, from .sub.road and .sub.rel.

Claims

1. The method of angle estimation for use in operating a vehicle which is travelling on a surface, the vehicle comprising a vehicle body which is attached to at least two wheels, the vehicle body having a first axis, the method comprising estimating the angle of said vehicle by: calculating .sub.glob1 as a first estimate of the angle .sub.glob between the first axis and a horizontal plane; producing .sub.globtransient, an estimate of transient values of .sub.glob, by applying a high pass filter to .sub.glob1; calculating .sub.glob steady state as an estimate of steady state values .sub.glob; calculating a further estimate of .sub.glob from .sub.globtransient and .sub.globsteady state; and operating the vehicle using the further estimate of .sub.glob.

2. The method of angle estimation according to claim 1, the method comprising: providing a first height sensor for measuring h.sub.1, the height of the vehicle body with respect to the first wheel; providing a second height sensor for measuring h.sub.2, the height of the vehicle body with respect to the second wheel; providing a surface angle sensor for measuring .sub.road, the angle of the surface in relation to a horizontal plane; providing an orientation sensor for measuring .sub.g{dot over (l)}ob, .sub.g{dot over (l)}ob being the rate of change of .sub.glob with respect to time; measuring the values of h.sub.1, h.sub.2, .sub.road and .sub.g{dot over (l)}ob; calculating .sub.glob1 by integrating .sub.g{dot over (l)}ob with respect to time; producing .sub.globtransient by applying a high pass filter to .sub.glob1; calculating the angle .sub.rel of the first axis relative to the surface using the values of h.sub.1 and h.sub.2; calculating .sub.globsteady state by combining .sub.rel and .sub.road and applying at least one low pass filter; and calculating a further estimate of .sub.glob from .sub.globtransient and .sub.globsteady state.

3. The method of angle estimation according to claim 2, wherein the surface angle sensor comprises an accelerometer for measuring a.sub.x, the acceleration of the vehicle with respect to the first axis.

4. A method of angle estimation for use in a vehicle which is travelling on a surface, the vehicle comprising a vehicle body which is attached to at least two wheels, the vehicle body having a first axis, the method comprising: providing a first height sensor for measuring h.sub.1, the height of the vehicle body with respect to the first wheel; providing a second height sensor for measuring h.sub.2, the height of the vehicle body with respect to the second wheel; providing a surface angle sensor for measuring .sub.road, the angle of the surface in relation to a horizontal plane; measuring the values of h.sub.1, h.sub.2 and .sub.road; calculating the angle .sub.rel of the first axis relative to the surface using the values of h.sub.1 and h.sub.2; and calculating an estimate of .sub.glob, the angle between the first axis and the horizontal plane, from .sub.road and .sub.rel.

5. The method of angle estimation according to claim 4, wherein the surface angle sensor comprises an accelerometer for measuring a.sub.x, the acceleration of the vehicle with respect to the first axis.

6. The method of angle estimation according to claim 5, wherein measuring .sub.road comprises calculating an estimate .sub.road, using the relationship:
.sub.road=.sub.glob.sub.rel wherein .sub.glob and .sub.rel are estimates of .sub.glob and .sub.rel.

7. The method of angle estimation according to claim 6, the method comprising calculating .sub.glob using the equation: ${}_{\mathrm{glob}}^{}=\arcsin \left(\frac{a_x-\stackrel{.}{u}+{}_z{v}_y}{g}\right)$ wherein: .sub.z is the rate of yaw of the vehicle; v.sub.y is the velocity of the vehicle along a second axis, the second axis being perpendicular to the first axis and perpendicular to the direction of acceleration due to gravity; {dot over (u)} is the derivative with the respect to time of the speed of the vehicle along the first axis.

8. The method of angle estimation according to claim 6, the method comprising calculating .sub.rel using the equation: $\frac{{}_{\mathrm{rel}}^{}}{a_x}=\frac{M_s{h}_{\mathrm{cg}}}{K_{\mathrm{pitch}}+D_{\mathrm{pitch}}s}$ wherein: M.sub.s is the mass of the vehicle; h.sub.cg is the height of the centre of gravity of the vehicle; K.sub.pitch is the spring term associated with the vehicle suspension; and D.sub.pitchs is the damping term associated with the vehicle suspension.

9. An angle estimation system for use in operating a vehicle which is travelling on a surface, the vehicle comprising a vehicle body which is attached to at least two wheels, the vehicle body having a first axis, the angle estimation system comprising a control unit, the control unit being arranged to: calculate .sub.glob1 as a first estimate of the angle .sub.glob between the first axis and a horizontal plane; apply a high pass filter to .sub.glob1 to produce .sub.globtransient, an estimate of transient values of .sub.glob; calculate .sub.globsteady state, an estimate of steady state values of .sub.glob; and calculate a further estimate of .sub.glob from .sub.globtransient and .sub.glob steady state, wherein the system is configured to operate the vehicle using the further estimate of .sub.glob.

10. The angle estimation system according to claim 9, the angle estimation system further comprising: a first height sensor for measuring h.sub.1, the height of the vehicle body with respect to the first wheel; a second height sensor for measuring h.sub.2, the height of the vehicle body with respect to the second wheel; a surface angle sensor for measuring .sub.road, the angle of the surface in relation to a horizontal plane; and an orientation sensor for measuring .sub.g{dot over (l)}ob, .sub.g{dot over (l)}ob being the rate of change of .sub.glob with respect to time and .sub.glob being the angle between the first axis and the horizontal plane, the control unit being arranged to: receive signals from the sensors indicating the values of h.sub.1, h.sub.2, .sub.road and .sub.g{dot over (l)}ob; integrate .sub.g{dot over (l)}ob with respect to time to calculate .sub.glob1; apply a high pass filter to .sub.glob1 to produce .sub.globtransient; use the values of h.sub.1 and h.sub.2 to calculate .sub.rel, the angle of the first axis relative to the surface; combine .sub.rel and .sub.road, and apply a low pass filter, to produce .sub.globsteady state; and calculate a further estimate of .sub.glob from .sub.globtransient and .sub.globsteady state.

11. The angle estimation system according to claim 10, wherein the surface angle sensor comprises an accelerometer for measuring a.sub.x, the acceleration of the vehicle with respect to the first axis.

12. A vehicle comprising an angle estimation system according to claim 9.

13. An angle estimation system for use in a vehicle which is travelling on a surface, the vehicle comprising a vehicle body which is attached to at least two wheels, the vehicle body having a first axis, the angle estimation system comprising: a first height sensor for measuring h.sub.1, the height of the vehicle body with respect to the first wheel; a second height sensor for measuring h.sub.2, the height of the vehicle body with respect to the second wheel; a surface angle sensor for measuring .sub.road, the angle of the surface in relation to a horizontal plane; and a control unit, the control unit being arranged to: receive signals from the sensors indicating the values of h.sub.1, h.sub.2 and .sub.road; use the values of h.sub.1 and h.sub.2 to calculate .sub.rel, the angle of the first axis relative to the surface; and calculate an estimate of .sub.glob, the angle between the first axis and the horizontal plane, from .sub.road and .sub.rel.

14. The angle estimation system according to claim 13, wherein the surface angle sensor comprises an accelerometer for measuring a.sub.x, the acceleration of the vehicle with respect to the first axis.

15. The angle estimation system according to claim 14, wherein measuring .sub.road comprises calculating an estimate .sub.road, using the relationship:
.sub.road=.sub.glob.sub.rel, wherein .sub.glob and .sub.rel are estimates of .sub.glob and .sub.rel.

16. The angle estimation system according to claim 15, the control unit being further arranged to calculate .sub.glob using the equation: ${}_{\mathrm{glob}}^{}=\arcsin \left(\frac{a_x-\stackrel{.}{u}+{}_z{v}_y}{g}\right)$ wherein: .sub.z is the rate of yaw of the vehicle; v.sub.y is the velocity of the vehicle along a second axis, the second axis being perpendicular to the first axis and perpendicular to the direction of acceleration due to gravity; {dot over (u)} is the derivative with the respect to time of the speed of the vehicle along the first axis.

17. The angle estimation system according to claim 15, the control unit being further arrange to calculate .sub.rel using the equation: $\frac{{}_{\mathrm{rel}}^{}}{a_x}=\frac{M_s{h}_{\mathrm{cg}}}{K_{\mathrm{pitch}}+D_{\mathrm{pitch}}s}$ wherein: M.sub.s is the mass of the vehicle; h.sub.cg is the height of the centre of gravity of the vehicle; K.sub.pitch is the spring term associated with the vehicle suspension; and D.sub.pitchs is the damping term associated with the vehicle suspension.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

(1) Embodiments of the invention will now be described, by way of example only, with reference to the accompanying drawings, in which:

(2) FIG. 1 is a diagram of a vehicle climbing a slope;

(3) FIG. 2 is a block diagram showing a system according to an embodiment of the invention; and

(4) FIG. 3 is a diagram illustrating a method according to an embodiment of the invention.

DETAILED DESCRIPTION OF THE ILLUSTRATED EMBODIMENTS

(5) FIG. 1 shows a vehicle 101 which is climbing a sloping surface 102. The vehicle 101 comprises a vehicle body 103 and four wheels 104, of which two are shown. Each wheel 104 is provided with a height sensor 105 (not shown in FIG. 1) which measures the height of the vehicle body 103 in relation to the wheel. As is indicated in FIG. 1, the height sensor 105 which is attached to the rear right wheel of the vehicle measures H.sub.RR, the height of a predetermined point on the vehicle body 103 with respect to the rear right wheel. Similarly, the height sensor 105 which is attached to the front right wheel of the vehicle measures H.sub.FR, the height of a predetermined point on the vehicle body 103 with respect to the front right wheel. The other two height sensors 105 measure the height of the vehicle body 103 with respect to the rear left wheel (H.sub.RL) and the front left (H.sub.FL) wheel respectively.

(6) The vehicle body 103 has a longitudinal axis 106. The longitudinal axis 106 is related to the surface 102 by an angle .sub.rel, as is indicated in FIG. 1. .sub.rel is defined as the angle between the longitudinal axis 106 and the surface 102 when measured on a vertical plane which is coincident with the longitudinal axis 106 along the entirety of its length. In FIG. 1, the vertical plane is therefore the plane of the drawing.

(7) The surface 102 is related to a horizontal plane 107 by an angle .sub.road, as is indicated in FIG. 1. The label .sub.road, is used since the surface is often a road, however the invention is not limited to uses on roads, and could be used for example by a vehicle during off-road use. .sub.road is defined as the angle between the surface 102 and the horizontal plane 107 when measured on the vertical plane which is coincident with the longitudinal axis 106 of the vehicle body 103.

(8) Both the horizontal and the vertical planes are defined with respect to gravity. The direction of acceleration due to gravity is illustrated in FIG. 1 by the line labeled g. The horizontal plane is perpendicular to the arrow g. The vertical plane is coincident with the line g along the entirety of its length.

(9) FIG. 2 shows an angle estimation system 201 according to the invention, which is provided for the vehicle 101 and comprises the four height sensors 105. The angle estimation system 201 comprises a control unit 202. The control unit receives input from the four height sensors 105, in the form of signals indicating the values of H.sub.RR, H.sub.FR, H.sub.RL and H.sub.FL.

(10) The angle estimation system 201 further comprises three linear acceleration sensors 203, 204, 205. Each linear acceleration sensor is suitable for measuring the acceleration of the vehicle body 103 on a particular axis, x, y or z. The x axis linear acceleration sensor 203 measures a.sub.x, the acceleration of the vehicle body along the x axis, which is illustrated in FIG. 1 and is substantially coincident with the longitudinal axis of the vehicle body 103. As a result, a.sub.x is a measure of the acceleration forwards and backwards, as they would be thought of by a driver of the vehicle. Similarly, the y axis linear acceleration sensor 204 measures a.sub.y, the acceleration of the vehicle body along the y axis. The y axis is perpendicular to the x axis, and is substantially coincident with a lateral axis of the vehicle body. As a result, a.sub.y is a measure of the acceleration to the right and left as they would be thought of by a driver of the vehicle. Both the x axis and the y axis are substantially horizontal when the vehicle is at rest on a horizontal surface. The z axis linear acceleration sensor 205 measures a.sub.z, the acceleration of the vehicle body along the z axis, which is perpendicular to both the x and y axes. The measurement a.sub.z is a measure of the acceleration up and down as they would be thought of by a driver of the vehicle. The control unit receives input from the three linear acceleration sensors 203, 204, 205, in the form of signals indicating the values of a.sub.x, a.sub.y and a.sub.z.

(11) As such, the roll, pitch and yaw of the vehicle body can be thought of as rotations about the x axis, the y axis and the z axis, respectively.

(12) The angle estimation system 201 further comprises a wheel speed sensor 206, which measures the speed of rotation of at least two wheels of the vehicle and hence determines u, the speed of the vehicle 101 with respect to the surface 102. The control unit receives input from the wheel speed sensor 206, in the form of signals indicating the value of u.

(13) The angle estimation system 201 further comprises a gyroscope, which measures the rate of change in the pitch, roll and yaw of the vehicle body 103. The control unit 202 receives input from the gyroscope 207 in the form of signals indicating the value of .sub.g{dot over (l)}ob, which is the pitch rate. The control unit also receives input from the gyroscope 207 which indicates the values the roll rate and the yaw rate respectively.

(14) The control unit 202 uses the measurements described above to arrive at an estimate of .sub.glob, the global pitch of the vehicle. .sub.glob is indicated in FIG. 1, and is the angle of the longitudinal axis of the vehicle body 103 with respect to a horizontal plane, when measured in the vertical plane which is coincident with the longitudinal axis of the vehicle body 103. The method used for deriving an estimate of .sub.glob is illustrated in FIG. 3.

(15) An integral of the pitch rate provides the global pitch angle .sub.glob.
.sub.glob1=.sub.g{dot over (l)}obdt

(16) The pitch rate measurement is, however, subject to contamination due to rolling and yawing of the vehicle. Therefore the sensor measurement also needs to be compensated due to roll bias. As such the control unit 202 uses the formula:
.sub.glob1=.sub.g{dot over (l)}obdt
wherein
.sub.g{dot over (l)}ob=.sub.g{dot over (l)}ob.sub.z .sub.g{dot over (l)}ob: Sensed pitch rate .sub.z: Yaw rate : Roll angle estimate

(17) This is illustrated as S301 in FIG. 3. The yaw rate .sub.z may be measured by the gyroscope 207, or it may be calculated based on the position of the steering and the measured velocity of the wheels.

(18) The initial conditions for this integral are unknown. The values produced by the integral calculation drift over time, as small errors in the measurements and sensor offsets mount up when integrated. As such .sub.glob1 is an accurate indication of transient changes in .sub.glob, but does not provide an accurate measure of long term changes in .sub.glob.

(19) In order to compensate for these errors, .sub.glob1 is passed through a high pass filter at S302 to produce .sub.glob transient, which is an accurate estimate of the transient, or high frequency, variations in global pitch. This is of little use by itself, so the control unit 202 is configured to also produce .sub.glob steady state, an estimate of the steady state, or low frequency, variations in global pitch.

(20) At S303 the control unit 202 calculates an estimate of .sub.rel based upon the measurements of the height sensors. To begin with:

(21) ${}_{\mathrm{rel}1}=\mathrm{atan}\left(\frac{h_{\mathrm{RL}}-h_{\mathrm{FL}}}{L}\right)\left(\frac{h_{\mathrm{RL}}-h_{\mathrm{FL}}}{L}\right)\left(\mathrm{using}\mathrm{small}\mathrm{angle}\mathrm{aproximation}\right)$

(22) In the same way:

(23) ${}_{\mathrm{rel}2}=\left(\frac{h_{\mathrm{RR}}-h_{\mathrm{FR}}}{L}\right)$

(24) With: h.sub.FL,FR,RL,RR: Front left, front right, rear left, rear right height of body with respect to respective wheel, as provided by height sensors L: Length of vehicle wheel base

(25) As can be seen from these equations, .sub.rel1 and .sub.rel2 are estimates of .sub.rel based upon the measurements of the height sensors on the left hand side of the vehicle and the right hand side of the vehicle respectively. Taking the average of both we obtain a more accurate relative pitch calculation:

(26) ${}_{\mathrm{rel}}=\left(\frac{{}_{\mathrm{rel}1}+{}_{\mathrm{rel}2}}{2}\right)$

(27) This calculation is an accurate relative pitch estimate across the entire frequency spectrum. However for the purposes of this method, the estimate is low pass filtered at S304 to produce .sub.rel steady state, an accurate steady state relative pitch.

(28) In the event that a height sensor stops working, any two sensors can be used to provide an estimate of .sub.rel, provided that the two sensors are on different axles of the vehicle 101.

(29) As the vehicle pitches away from the horizontal, a component of the acceleration due to gravity can be measured by the x axis linear acceleration sensor. At S305, the control unit 202 calculates a global pitch estimate based upon the gravitational contamination of the x axis linear acceleration sensor, using the equation:

(30) ${}_{\mathrm{glob}}^{}=\arcsin \left(\frac{a_x-\stackrel{.}{u}+{}_z{v}_y}{g}\right)$

(31) With: a.sub.x: Sensed forward acceleration .sub.zv.sub.y: Longitudinal acceleration due to centrifugal acceleration (.sub.z: Yaw rate, v.sub.y: Lateral velocity) {dot over (u)}: Derivative with respect to time of the sensed forward speed u.

(32) In the above equation, the acceleration due to changing speed and the acceleration due to centrifugal force are removed from a.sub.x, leaving only the acceleration due to gravity, which can then be related to .sub.glob.

(33) This estimate will be accurate provided that the measured values are accurate. However large yaw rates can amplify any errors in the lateral velocity estimate.

(34) Since accelerometers are typically subject to a considerable amount of noise, due to unpredictable movements in the vehicle, this calculation is more accurate for steady states than for transitory changes.

(35) It is possible to model the vehicle as a linear spring damped system, since the suspension provides typically constant spring and damping characteristics. Therefore it is possible to calculate the relative pitch of the vehicle from the forward acceleration. This is done at S306.

(36) Specifically, there are two external moments applied to the vehicle body: the moment due to vertical suspension forces, denoted as M.sub.susp and the moment due to longitudinal tire force, denoted as M.sub.longforce.
M.sub.susp=K.sub.pitch.sub.yr+D.sub.pitch{dot over ()}.sub.yr
M.sub.longforce=M.sub.sa.sub.xh.sub.cg

(37) With: h.sub.cg: The height of the centre of gravity of the vehicle. This may be calculated, for example by monitoring measurements of the height sensors when the vehicle is at rest, or an estimate may be used such as a typical value for the vehicle with a driver and a standard load.

(38) Since the pitch angular rate is usually small, we may take it that:

(39) $M_{\mathrm{longforce}}-M_{\mathrm{susp}}0$$\mathrm{Hence},{}_{\mathrm{rel}}^{}+\frac{D_{\mathrm{pitch}}}{K_{\mathrm{pitch}}}{\stackrel{.}{}}_{\mathrm{rel}}^{}=\frac{M_s{a}_x{h}_{\mathrm{cg}}}{K_{\mathrm{pitch}}}$

(40) Applying the Laplace transform we obtain the transfer function:

(41) 0$\frac{{}_{\mathrm{rel}}^{}}{a_x}=\frac{M_s{h}_{\mathrm{cg}}}{K_{\mathrm{pitch}}+D_{\mathrm{pitch}}s}$

(42) Again, this calculation is most valid in a steady state condition.

(43) As can be seen from FIG. 1, .sub.glob=.sub.rel+.sub.road. Therefore, at S307 the control unit 202 combines .sub.glob and .sub.rel to calculate .sub.road using:
.sub.road=.sub.glob.sub.rel

(44) Therefore .sub.road can be calculated using the measurement a.sub.x, and as such the x axis linear acceleration sensor is here being used as a surface angle sensor, that is a sensor which can be used in estimating the angle between the surface and a horizontal plane. At S308, .sub.road is low pass filtered to produce .sub.road steady state, an estimate of the steady state surface pitch.

(45) At S309, the control unit 202 calculates .sub.globsteady state using:
.sub.globsteady state=.sub.relsteady state+.sub.roadsteady state

(46) Having now calculated estimates of both the steady state and the transient variations in the global pitch, the control unit 202 combines them as S310 to produce .sub.glob, an estimate of the global pitch which can be used, for example, in managing rollover prevention systems.

(47) Out of necessity, the three stages in calculating .sub.glob have been described above in a particular order. However, they can be completed by the control unit 202 in any desired order, or in parallel in any combination. Control unit 202 can also switch between different stages as desired. For example the control unit 202 may calculate the values of any combination of .sub.glob1, .sub.road and .sub.rel before beginning to apply any of the filters in steps S302, S304 and S308.

(48) Ideally, the high and low pass filters should all use a similar cut off frequency in order to obtain pitch information across the frequency spectrum. Typically, the lower this cut off frequency is the more accurate the global pitch calculation will be, provided that the cut off is still high enough to filter out the integral drift in .sub.glob1.

(49) It may be particularly advantageous for L2(s), the low pass filter in S308, to have a very low cut off frequency in order to filter inaccuracies in the pitch estimation derived from the forward accelerometer sensor. Therefore L2(s) may be provided with a lower cut off frequency than the other two filters.

(50) In the event that the gyroscope 207 malfunctions, the control unit 202 may still obtain an estimate for .sub.glob by combining .sub.rel and .sub.road:
.sub.glob=.sub.rel+.sub.road

(51) Some noise filtering is still required in such a calculation. Similarly, in the event of a malfunction of the height sensors 105, the control unit 202 can use .sub.glob in place of .sub.glob. In the event of a malfunction of the wheel speed sensor 206, or the x axis linear acceleration sensor 203, the control unit 202 can use .sub.glob1 in place of .sub.glob.

(52) The description above describes using the technique to arrive at a pitch angle estimate .sub.glob. However, the same method can be used by the control unit 202 to provide a roll angle estimate, simply by switching the longitudinal axis with the lateral axis of the vehicle.

(53) Throughout the description and claims of this specification, the words comprise and contain and variations of them mean including but not limited to, and they are not intended to (and do not) exclude other moieties, additives, components, integers or steps. Throughout the description and claims of this specification, the singular encompasses the plural unless the context otherwise requires. In particular, where the indefinite article is used, the specification is to be understood as contemplating plurality as well as singularity, unless the context requires otherwise.

(54) Features, integers, characteristics, compounds, chemical moieties or groups described in conjunction with a particular aspect, embodiment or example of the invention are to be understood to be applicable to any other aspect, embodiment or example described herein unless incompatible therewith. All of the features disclosed in this specification (including any accompanying claims, abstract and drawings), and/or all of the steps of any method or process so disclosed, may be combined in any combination, except combinations where at least some of such features and/or steps are mutually exclusive. The invention is not restricted to the details of any foregoing embodiments. The invention extends to any novel one, or any novel combination, of the features disclosed in this specification (including any accompanying claims, abstract and drawings), or to any novel one, or any novel combination, of the steps of any method or process so disclosed.