JP6007778B2 - Inclination angle detection apparatus, inclination angle detection method, and program - Google Patents

Description

  The present invention relates to a tilt angle detection technique, and more particularly to a tilt angle detection device, a tilt angle detection method, and a program for detecting a tilt angle of a vehicle.

  Conventionally, a car navigation apparatus combines a GPS (Global Positioning System) capable of obtaining an absolute position and a self-contained navigation that obtains a relative position calculated from a sensor such as a gyroscope or a speed pulse to obtain a vehicle position and traveling direction. Is calculated. In the self-contained navigation, it is possible to calculate a more accurate position by taking into account the attitude angle of the vehicle including the road inclination. The inclination angle, which is the attitude angle of the vehicle, is derived, for example, by Kalman filter processing based on a state equation derived from a model in which the inclination angle does not change in unit time (see, for example, Patent Document 1).

JP 2011-102784 A

  Even when the tilt angle changes, it is desirable to reduce the delay for calculating the tilt angle in order to be able to derive the tilt angle.

  The present invention has been made in view of such circumstances, and an object thereof is to provide a technique for reducing a delay for calculating an inclination angle.

  In order to solve the above problems, an inclination angle detection device according to an aspect of the present invention includes a first acquisition unit that acquires acceleration of a moving body, a second acquisition unit that acquires a moving distance of the moving body, and a second acquisition. Based on the movement distance acquired in the unit and the acceleration acquired in the first acquisition unit, an estimation unit for estimating the speed of the moving body, and a change in the speed estimated in the estimation unit are calculated. A speed difference calculating unit for deriving a speed difference, an acceleration integrating unit for deriving a second speed difference by integrating the acceleration acquired by the first acquiring unit, and a second speed difference derived by the acceleration integrating unit; An inclination angle calculation unit for deriving an inclination angle of the moving body based on the first speed difference derived by the speed difference calculation unit.

  Another aspect of the present invention is a tilt angle detection method. The method includes: acquiring an acceleration of the moving object; acquiring a moving distance of the moving object; estimating a speed of the moving object based on the acquired moving distance and the acquired acceleration; A step of deriving a first speed difference by calculating a change in the estimated speed; a step of deriving a second speed difference by integrating the acquired acceleration; and the derived second speed difference; And deriving the tilt angle of the moving body based on the derived first speed difference.

  It should be noted that any combination of the above-described constituent elements and a conversion of the expression of the present invention between a method, an apparatus, a system, a recording medium, a computer program, etc. are also effective as an aspect of the present invention.

  According to the present invention, the delay for calculating the tilt angle can be reduced.

It is a figure which shows the structure of the inclination-angle detection apparatus which concerns on the Example of this invention. It is a figure which shows the operation state of the vehicle carrying the inclination-angle detection apparatus of FIG. It is a flowchart which shows the output procedure of the inclination angle by the inclination angle detection apparatus of FIG.

  Before describing the present invention specifically, an outline will be given first. An embodiment of the present invention relates to a tilt angle detection device that detects a tilt angle of a vehicle in a self-contained navigation with a car navigation device. The tilt angle detection apparatus according to the embodiment executes the following processing in order to reduce the delay for calculating the tilt angle and detect the tilt angle with high accuracy. The tilt angle detection apparatus according to the present embodiment adjusts the delay of the acceleration sensor output so as to match the delay time due to the moving average of the vehicle speed pulses. In addition, the inclination angle detection device estimates the speed by executing a Kalman filter process using, as an observed value, a vehicle speed pulse that is a moving average, and using an acceleration calculated based on a delay-adjusted acceleration sensor output as an input value. Further, the tilt angle detection device detects the difference between the estimated speeds at two time points in a predetermined time interval (hereinafter referred to as “first speed difference”) and the integrated value of acceleration in the predetermined time interval (hereinafter referred to as “second speed”). The tilt angle is calculated based on the difference. Note that the state variable used in the Kalman filter processing also includes an inclination angle. However, the inclination angle is calculated from two speed differences separately, so that the delay is reduced. In addition, since the vehicle speed pulses are moving averaged, the accuracy of the inclination angle is improved.

  FIG. 1 shows a configuration of an inclination angle detection apparatus 100 according to an embodiment of the present invention. The inclination angle detection device 100 includes a first acquisition unit 14, a second acquisition unit 16, a ring buffer 18, an acceleration integration unit 20, an estimation unit 22, a speed difference calculation unit 24, an inclination angle calculation unit 26, and a control unit 28. The second acquisition unit 16 includes a distance meter 30 and a filter unit 32. In addition, the tilt angle detection device 100 is connected to the acceleration sensor 10 and the distance sensor 12. Furthermore, the tilt angle detection device 100 is mounted on a vehicle (not shown).

  First, a symbol for processing time is defined. Here, the time interval for counting vehicle speed pulses is ΔT1, the time interval for sampling by the acceleration sensor 10 is ΔT2, and the time interval for calculating the tilt angle is ΔT3. In some cases, ΔT1 is called a first time interval, and ΔT2 is called a second time interval. Each time interval has a relationship of ΔT2 ≦ ΔT1 ≦ ΔT3. In the following description, mΔT2 = mΔT1 = ΔT3. However, m is a positive integer.

  The acceleration sensor 10 detects acceleration generated in the main movement direction of the vehicle, for example, the front-rear direction of the vehicle. Here, FIG. 2 is used to explain the acceleration. FIG. 2 shows an operating state of the vehicle 50 on which the tilt angle detection device 100 is mounted. The vehicle 50 is equipped with the tilt angle detection device 100 of FIG. In addition, the vehicle 50 moves on the road surface 54. Here, it is assumed that the sensitive axis 56 of the acceleration sensor 10 (not shown) mounted on the vehicle 50 is parallel to the road surface 54 and coincides with the longitudinal direction of the vehicle 50. Further, the sensitive shaft 56 and the road surface 54 are inclined with respect to the horizontal plane 52 by an inclination angle θ. In the acceleration, the moving acceleration of the vehicle 50 and the gravity component corresponding to the inclination angle θ formed by the sensitive axis 56 and the horizontal plane 52 are superimposed. Returning to FIG. In addition, the detection in the acceleration sensor 10 is sequentially performed for every 2nd time interval. The acceleration sensor 10 outputs the detection result to the first acquisition unit 14. The distance sensor 12 detects a vehicle speed pulse for detecting the moving distance of the vehicle 50 and outputs the vehicle speed pulse to the distance meter 30.

  The first acquisition unit 14 is connected to the acceleration sensor 10 and receives an output from the acceleration sensor 10, that is, a detection result of the acceleration sensor 10. The detection result is accepted at a second time interval equal to or less than the first time interval. The first acquisition unit 14 calculates acceleration based on the detection result. Specifically, the first acquisition unit 14 converts the detection result of the acceleration sensor 10 into a unit of acceleration, and delays the conversion result so as to be equal to a delay time of the filter unit 32 described later. Hereinafter, the conversion result or the delayed conversion result may be referred to as acceleration. In the first acquisition unit 14, a filter may be used for delay, or a combination of a filter and a delay unit may be used. The first acquisition unit 14 outputs the acceleration to the ring buffer 18 and the estimation unit 22.

  The distance meter 30 is connected to the distance sensor 12 and receives a vehicle speed pulse from the distance sensor 12. The distance meter 30 sequentially calculates the distance at the first time interval based on the vehicle speed pulse. Specifically, the distance meter 30 calculates the movement distance within ΔT1 based on the number of vehicle speed pulses counted within ΔT1. The filter unit 32 derives the travel distance of the vehicle 50 by performing low-pass filter processing on the distance at the first time interval sequentially calculated by the distance meter 30. For example, the filter unit 32 performs a moving average on the output of the distance meter 30. As a result, the distance resolution is improved from one pulse unit to the reciprocal of the number of moving averages. The filter unit 32 outputs the movement distance to the estimation unit 22.

ここで、上付きの添え字Ｔは、ベクトルまたは行列の転置を示す。また、下付きの添え字ｎはタイミングｎを示す。 The estimation unit 22 estimates the speed of the vehicle 50 based on the travel distance acquired by the second acquisition unit 16 and the acceleration acquired by the first acquisition unit 14. For example, the estimation unit 22 estimates the velocity by executing the Kalman filter process using the acceleration acquired by the first acquisition unit 14 as an input value and the movement distance acquired by the second acquisition unit 16 as an observation value. Here, the contents of the Kalman filter processing will be specifically described. The state variable x _n is assumed to have three elements: distance s _n , velocity v _n , and inclination angle θ _n as follows.

Here, the superscript T indicates a transposition of a vector or a matrix. A subscript n indicates a timing n.

ここで、入力ｕ_ｎは、加速度センサ出力ａ_ＯＵＴｎと加速度センサオフセットａ_ＯＦＳｎである。

なお、加速度センサ出力ａ_ＯＵＴｎは、第１取得部１４からの加速度に相当し、加速度センサオフセットａ_ＯＦＳｎは、公知の技術等によって導出されている。 The estimation unit 22 calculates the timing equation shown below based on the input u _n and the previous state variable, that is, the state variable x _n−1 at the timing n−1. Calculate the estimated value x _n ′ at _n . The state variable x _n−1 is held after being derived.

Here, the input _{u n} is an acceleration sensor output _{a OUTn} and the acceleration sensor offset _{a OFSn.}

The acceleration sensor output a _OUTn corresponds to the acceleration from the first acquisition unit 14, and the acceleration sensor offset a _OFSn is derived by a known technique or the like.

ここで、ｇは重力加速度、ｋは加速度単位（例えばｍ／ｓ^２）への変換係数、ΔＴはサンプリング間隔を示す。サンプリング間隔は、ΔＴ３に相当する。前述のごとく、加速度センサの感応軸は、車両５０の前後方向と一致するものとする。 Further, the system matrix A and the input matrix B in Expression (2) are expressed as Expression (4) and Expression (5).

Here, g is a gravitational acceleration, k is a conversion coefficient to an acceleration unit (for example, m / s ² ), and ΔT is a sampling interval. The sampling interval corresponds to ΔT3. As described above, the sensitive axis of the acceleration sensor is assumed to coincide with the longitudinal direction of the vehicle 50.

ここで、出力行列Ｃは次のように示される。

The estimation unit 22 uses the movement distance from the filter unit 32 as an observation value z _n , and sets each of the state variable speed and acceleration offset extracted from the estimated value x _n ′ calculated in Expression (2). The difference is obtained as follows.

Here, the output matrix C is shown as follows.

ここで、共分散行列Ｒ等からカルマンフィルタのゲインの導出には、公知の技術が使用されればよい。公知の技術は、例えば、ＭＯＨＩＮＤＥＲ Ｓ． ＧＲＥＷＡＬ、ＡＮＧＵＳ Ｐ． ＡＮＤＲＥＷＳ著「Ｋａｌｍａｎ Ｆｉｌｔｅｒｉｎｇ：Ｔｈｅｏｒｙ ａｎｄ Ｐｒａｃｔｉｃｅ Ｕｓｉｎｇ ＭＡＴＬＡＢ」、Ａ Ｗｉｌｅｙ−Ｉｎｔｅｒｓｃｉｅｎｃｅ Ｐｕｂｌｉｃａｔｉｏｎの１１６頁から１２０頁に示されている。本実施例の場合、カルマンゲインＫ_ｎは下式のように３個得られる。

The estimation unit 22 calculates the Kalman gain K _n from the observation error δs _n of the distance meter 30 and the system error. The observation error represents the standard deviation, and the covariance matrix R _n is _expressed by the following equation.

Here, a known technique may be used to derive the gain of the Kalman filter from the covariance matrix R or the like. Known techniques are described, for example, in MOHINDER S.M. GREWAL, ANGUS P. It is shown on pages 116 to 120 of ANDREWS, “Kalman Filtering: Theory and Practice Using MATLAB”, A Wiley-Interscience Publication. In this embodiment, the Kalman gain K _n is obtained three by the following equation.

カルマンゲインＫ_ｎが小さければ推定値ｘ_ｎ’の重みが大きく、一方、カルマンゲインＫ_ｎが大きければ観測値Ｚ_ｎの重みが大きい。式（１０）により求められた推定値ｘ_ｎの速度成分が、求める速度になる。ここで、推定値ｘ_ｎには傾斜角も成分として含まれるが、この傾斜角は遅延が大きいため外部では使用しない。推定部２２は、カルマンフィルタ処理によって推定した速度を速度差演算部２４に出力する。 Estimation unit 22, the estimated value _{x n} ', and updates from the Kalman gain _{K n} and the observation residuals _{y n} estimates _{x n} as follows.

If the Kalman gain K _n is small, the weight of the estimated value x _n ′ is large. On the other hand, if the Kalman gain K _n is large, the weight of the observed value Z _n is large. The speed component of the estimated value _xn obtained by the equation (10) is the speed to be obtained. Here, the estimated value _xn includes a tilt angle as a component, but this tilt angle has a large delay and is not used outside. The estimation unit 22 outputs the speed estimated by the Kalman filter process to the speed difference calculation unit 24.

  The speed difference calculation unit 24 calculates the difference between the current speed estimated by the estimation unit 22 and the speed before ΔT3. The speed difference that is the calculation result corresponds to the first speed difference described above. That is, the speed difference calculation unit 24 derives the first speed difference by calculating the change in speed estimated by the estimation unit 22. The speed difference calculation unit 24 outputs the first speed difference to the tilt angle calculation unit 26. The ring buffer 18 receives acceleration from the first acquisition unit 14. The ring buffer 18 stores the acceleration of ΔT3. The acceleration past ΔT3 is sequentially rewritten to the latest acceleration. The ring buffer 18 outputs acceleration to the acceleration integration unit 20.

  The acceleration integrating unit 20 receives the acceleration from the ring buffer 18. The acceleration integration unit 20 integrates the acceleration of ΔT3 and derives a speed difference at two time points separated by ΔT3. The speed difference that is the integration result corresponds to the above-described second speed difference. That is, the acceleration integration unit 20 derives the second speed difference by integrating the acceleration acquired by the first acquisition unit 14. The acceleration integrating unit 20 outputs the second speed difference to the tilt angle calculating unit 26.

式（１１）の上式では、運動加速度ａ_ｉと車両５０の傾斜角θ_ｐに応じた重力加速度成分ｇｓｉｎ(θ_ｐ)を加えた加速度（左辺）が、加速度センサ１０で検出される（右辺）ことを表している。ここで、ｋ_ａは加速度センサ出力の単位から加速度単位への変換係数である。加速度センサ出力の単位は、例えば電圧Ｖｏｌｔであり、加速度単位は、例えばｍ／ｓ^２である。また、ａ_ＯＵＴは加速度センサ出力、ａ_ＯＦＳは加速度センサ１０のオフセットである。加速度センサ１０のオフセットは、車両５０が水平の場合の加速度センサ出力に相当する。なお、加速度センサ１０は、その感応軸が道路面に対して平行になるように車両５０に取付けられているものとする。 The inclination angle calculator 26 receives the first speed difference from the speed difference calculator 24 and the second speed difference from the acceleration integrator 20. The tilt angle calculation unit 26 derives the tilt angle of the vehicle 50 based on the first speed difference and the second speed difference. The following formula is established for acceleration.

In the above equation (11), the acceleration (left side) obtained by adding the gravitational acceleration component gsin (θ _p ) corresponding to the motion acceleration a _i and the inclination angle θ _p of the vehicle 50 is detected by the acceleration sensor 10 (right side) ). Here, k _a is a conversion coefficient from the unit of acceleration sensor output to the acceleration unit. The unit of the acceleration sensor output is, for example, the voltage Volt, and the acceleration unit is, for example, m / s ² . Further, a _OUT is an acceleration sensor output, and a _OFS is an offset of the acceleration sensor 10. The offset of the acceleration sensor 10 corresponds to an acceleration sensor output when the vehicle 50 is horizontal. The acceleration sensor 10 is assumed to be attached to the vehicle 50 so that its sensitive axis is parallel to the road surface.

傾斜角θ_ｐが小さい場合は、式（１２）の下式に変形される。ここで、傾斜角θ_ｐの単位はラジアンである。制御部２８は、傾斜角検出装置１００の動作を制御する。 In the following equation (11), the sum of multiplications of the motion acceleration a _i and ΔT2 within the interval time ΔT3 of the calculation of the tilt angle is added to the velocity V _n−1 at the time _n−1 , It represents that the speed is V _n . The interval time between time n and time n-1 is equal to ΔT3. The equation (11) is rearranged with respect to the inclination angle θ _p as follows.

When the inclination angle θ _p is small, the following expression (12) is transformed. Here, the unit of the inclination angle θ _p is radians. The control unit 28 controls the operation of the tilt angle detection device 100.

  This configuration can be realized in terms of hardware by a CPU, memory, or other LSI of any computer, and in terms of software, it can be realized by a program loaded in the memory, but here it is realized by their cooperation. Draw functional blocks. Accordingly, those skilled in the art will understand that these functional blocks can be realized in various forms by hardware only, software only, or a combination thereof.

The operation of the tilt angle detection apparatus 100 having the above configuration will be described. FIG. 3 is a flowchart showing a tilt angle output procedure by the tilt angle detection apparatus 100. The first acquisition unit 14 acquires an output from the acceleration sensor 10 (S10). The first acquiring unit 14, by multiplying the conversion factor k _a the difference between the offset of the acceleration sensor output and the acceleration sensor 10, calculates the acceleration (S12). Here, the output timing of acceleration is adjusted in a FIFO (Forst In First Out) memory or the like so as to be the same as the delay time from the distance meter 30 to the filter unit 32. The ring buffer 18 stores the acceleration output from the first acquisition unit 14 (S14). The acceleration accumulating unit 20 derives the second speed difference by multiplying the acceleration between ΔT3 stored in the ring buffer 18 and ΔT2 and then accumulating (S16). The third speed difference corresponds to a speed difference within ΔT3 time. However, the second speed difference is affected by gravitational acceleration according to the tilt angle.

  The distance meter 30 calculates the distance at the first time interval by multiplying the value of the vehicle speed pulse counted during ΔT1 by the distance coefficient (S18). Here, the distance coefficient corresponds to the movement distance per pulse. The filter unit 32 derives the movement distance by the low-pass filter process (S20). The estimation unit 22 estimates the speed from the movement distance per ΔT1 output from the filter unit 32 and the acceleration output from the first acquisition unit 14 (S22). The speed difference calculation unit 24 derives a difference between the current speed and the speed before ΔT3 hours, that is, a first speed difference (S24). The tilt angle calculation unit 26 derives the tilt angle based on the first speed difference and the second speed difference (S26).

  According to the embodiment of the present invention, since the tilt angle is derived separately from the process of estimating the vehicle speed, the delay for calculating the tilt angle can be reduced. In addition, since the delay for calculating the tilt angle is reduced, the tilt angle can be derived even when the tilt angle changes. Further, since the distance at the first time interval sequentially calculated by the distance meter is subjected to the low-pass filter processing, the accuracy of deriving the tilt angle can be improved. Further, since the acceleration and speed timings are estimated together, the calculation accuracy of the tilt angle can be improved. In addition, without installing a new expensive barometric pressure sensor, the tilt angle can be derived easily and with a small delay only from a standard sensor in a car navigation system. This can suppress an increase in cost. Further, since the Kalman filter is used, the speed can be estimated with high accuracy.

  In the above, this invention was demonstrated based on the Example. This embodiment is an exemplification, and it will be understood by those skilled in the art that various modifications can be made to the combination of each component and each processing process, and such modifications are also within the scope of the present invention. .

  In the embodiment of the present invention, the tilt angle detection device 100 is mounted on the vehicle 50. However, the present invention is not limited thereto, and for example, the tilt angle detection device 100 may be carried by a pedestrian. According to this modification, the tilt angle detection device 100 can be applied to various products.

  In the embodiment of the present invention, the estimation unit 22 estimates the speed by executing the Kalman filter process. However, the present invention is not limited to this. For example, the estimation unit 22 may estimate the speed by executing a process other than the Kalman filter process. Specifically, a filtering system based on probability theory such as an H infinity filter or a particle filter may be used. According to this modification, the degree of freedom of the configuration of the estimation unit 22 can be improved.

  DESCRIPTION OF SYMBOLS 10 Acceleration sensor, 12 Distance sensor, 14 1st acquisition part, 16 2nd acquisition part, 18 Ring buffer, 20 Acceleration integration part, 22 Estimation part, 24 Speed difference calculation part, 26 Inclination angle calculation part, 28 Control part, 30 Distance meter, 32 filter section, 100 tilt angle detection device.

Claims (5)

A first acquisition unit for acquiring acceleration of the moving body;
A second acquisition unit for acquiring a moving distance of the moving body;
An estimation unit that estimates the speed of the moving body based on the movement distance acquired in the second acquisition unit and the acceleration acquired in the first acquisition unit;
A speed difference calculation unit for deriving a first speed difference by calculating a change in speed estimated by the estimation unit;
An acceleration integration unit for deriving a second speed difference by integrating the acceleration acquired in the first acquisition unit;
An inclination angle calculation unit for deriving an inclination angle of the moving body based on the second speed difference derived in the acceleration integration unit and the first speed difference derived in the speed difference calculation unit;
An inclination angle detection device comprising: The second acquisition unit includes
A distance meter that sequentially calculates the distance at the first time interval based on the vehicle speed pulse;
A filter unit for deriving a moving distance by performing a low-pass filter process on the distance at the first time interval sequentially calculated in the distance meter,
The said 1st acquisition part is an output in the 2nd time interval below a 1st time interval, Comprising: The acceleration is calculated based on the output from an acceleration sensor, The Claim 1 characterized by the above-mentioned. Inclination angle detector. The estimation unit estimates a speed by executing a Kalman filter process using an output from an acceleration sensor as an input value and using a moving distance acquired in the second acquisition unit as an observation value. 2. The tilt angle detection device according to 2. Obtaining the acceleration of the moving object;
Obtaining a moving distance of the moving body;
Estimating the speed of the moving object based on the acquired moving distance and the acquired acceleration;
Deriving a first speed difference by computing an estimated change in speed;
Deriving a second speed difference by integrating the acquired acceleration;
Deriving an inclination angle of the moving body based on the derived second speed difference and the derived first speed difference;
An inclination angle detection method comprising: Obtaining the acceleration of the moving object;
Obtaining a moving distance of the moving body;
Estimating the speed of the moving object based on the acquired moving distance and the acquired acceleration;
Deriving a first speed difference by computing an estimated change in speed;
Deriving a second speed difference by integrating the acquired acceleration;
Deriving an inclination angle of the moving body based on the derived second speed difference and the derived first speed difference;
A program that causes a computer to execute.

Images