JP2020142568A - Estimation device - Google Patents
Estimation device Download PDFInfo
- Publication number
- JP2020142568A JP2020142568A JP2019038915A JP2019038915A JP2020142568A JP 2020142568 A JP2020142568 A JP 2020142568A JP 2019038915 A JP2019038915 A JP 2019038915A JP 2019038915 A JP2019038915 A JP 2019038915A JP 2020142568 A JP2020142568 A JP 2020142568A
- Authority
- JP
- Japan
- Prior art keywords
- vehicle
- angle
- equation
- motion
- roll
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Landscapes
- Control Of Driving Devices And Active Controlling Of Vehicle (AREA)
Abstract
Description
本発明は、推定装置に関する。 The present invention relates to an estimation device.
従来から、車両に取り付けた各種センサで計測した計測値と、車両の運動状態をモデル化した運動方程式を用いて車両の走行状態を推定する推定装置が提案されている（例えば、特許文献１〜特許文献３参照）。 Conventionally, an estimation device that estimates the running state of a vehicle by using the measured values measured by various sensors attached to the vehicle and the equation of motion that models the motion state of the vehicle has been proposed (for example, Patent Documents 1 to 1). See Patent Document 3).
車両の走行状態を推定する場合、路面の傾き（カント角）は車両の運動に影響を与える要素となることから、従来の推定装置の中には、例えばロールレートセンサで計測されたロールレートを用いて、進行方向と直交する車両の横方向の傾き（姿勢角）を推定するものが存在する。 When estimating the running state of a vehicle, the inclination of the road surface (cant angle) is a factor that affects the movement of the vehicle. Therefore, in some conventional estimation devices, for example, the roll rate measured by a roll rate sensor is used. There is a device that estimates the lateral inclination (attitude angle) of a vehicle that is orthogonal to the traveling direction.
しかしながら、例えば姿勢角には路面のカント角の他、路面に対する車両のロール角も含まれるため、路面のカント角を精度よく推定するためには車両のロール角も推定する必要があるが、従来の推定装置では路面のカント角と共に車両のロール角を推定する推定装置は提案されていない。 However, for example, since the posture angle includes not only the cant angle of the road surface but also the roll angle of the vehicle with respect to the road surface, it is necessary to estimate the roll angle of the vehicle in order to accurately estimate the cant angle of the road surface. No estimation device has been proposed for estimating the roll angle of the vehicle as well as the cant angle of the road surface.
本発明は、上記に示した問題点を鑑みてなされたものであり、路面のカント角と共に車両のロール角を推定することができる推定装置を提供することを目的とする。 The present invention has been made in view of the above-mentioned problems, and an object of the present invention is to provide an estimation device capable of estimating the roll angle of a vehicle together with the cant angle of a road surface.
上記の目的を達成するために、請求項１に記載の推定装置は、目標経路に対する車両のずれ量、前記目標経路の曲率、前記車両のヨーレート、及び前記車両の操舵角のそれぞれの計測値と、前記車両のずれ量、前記目標経路の曲率、前記車両のヨーレート、前記車両の操舵角、路面のカント角、及び前記車両のロール角を用いて前記車両の運動状態をモデル化した複数の運動方程式とを用いて、前記車両が走行する路面のカント角及び前記車両のロール角を含む前記車両の状態量を推定する推定部を備える。 In order to achieve the above object, the estimation device according to claim 1 is used to measure the amount of deviation of the vehicle with respect to the target route, the curvature of the target route, the yaw rate of the vehicle, and the steering angle of the vehicle. A plurality of motions that model the motion state of the vehicle using the amount of displacement of the vehicle, the curvature of the target path, the yaw rate of the vehicle, the steering angle of the vehicle, the cant angle of the road surface, and the roll angle of the vehicle. It is provided with an estimation unit that estimates the state amount of the vehicle including the cant angle of the road surface on which the vehicle travels and the roll angle of the vehicle using the equation.
請求項２記載の発明では、前記計測値は、更に前記車両のロールレート及び前記車両の横方向の加速度に関するそれぞれの計測値を含み、前記運動方程式は、前記車両のロールレート、及び前記車両の横方向の加速度、前記車両のロールレートを計測するロールレートセンサのオフセット、前記車両の加速度を計測する加速度センサのオフセット、及び前記車両の操舵角の外乱を更に用いてモデル化され、前記車両の状態量は、前記車両のロールレート、前記車両の横方向の加速度、前記車両のロールレートを計測するロールレートセンサのオフセット、前記車両の加速度を計測する加速度センサのオフセット、及び前記車両の操舵角の外乱を更に含む。 In the invention according to claim 2, the measured value further includes each measured value regarding the roll rate of the vehicle and the lateral acceleration of the vehicle, and the equation of motion is the roll rate of the vehicle and the roll rate of the vehicle. Modeled using further lateral acceleration, the offset of the roll rate sensor that measures the roll rate of the vehicle, the offset of the acceleration sensor that measures the acceleration of the vehicle, and the disturbance of the steering angle of the vehicle, the vehicle The amount of state is the roll rate of the vehicle, the lateral acceleration of the vehicle, the offset of the roll rate sensor that measures the roll rate of the vehicle, the offset of the acceleration sensor that measures the acceleration of the vehicle, and the steering angle of the vehicle. Including further disturbances.
請求項３記載の発明では、前記推定部は、前記運動方程式に基づいて、前回推定された前記車両の状態量から、次時刻の前記車両の状態量を予測する予測部と、観測方程式を用いて、予測された次時刻の前記車両の状態量に対応する前記それぞれの計測値を算出し、前記算出された前記それぞれの計測値と、計測された前記それぞれの計測値との差分、及び前回推定された前記車両の状態量に基づいて、前記予測部で予測された前記車両の状態量を更新する更新部と、を含む。 In the invention according to claim 3, the estimation unit uses a prediction unit that predicts the state quantity of the vehicle at the next time from the previously estimated state quantity of the vehicle based on the equation of motion, and an observation equation. Then, each of the measured values corresponding to the state quantity of the vehicle at the predicted next time is calculated, the difference between the calculated measured values and the measured measured values, and the previous measurement. It includes an update unit that updates the state quantity of the vehicle predicted by the prediction unit based on the estimated state quantity of the vehicle.
請求項４記載の発明では、前記運動方程式が、前記路面のカント角、前記車両のロール角、前記車両のずれ量、前記目標経路の曲率、前記車両のヨーレート、及び前記車両の操舵角を用いて前記車両の横方向の運動状態をモデル化した第１の運動方程式と、前記車両のずれ量、前記目標経路の曲率、前記車両のヨーレート、及び前記車両の操舵角を用いて前記車両のヨー方向の運動状態をモデル化した第２の運動方程式と、前記車両のロール角、前記車両のずれ量、前記目標経路の曲率、前記車両のヨーレート、及び前記車両の操舵角を用いて前記車両のロール方向の運動状態をモデル化した第３の運動方程式によって示される。 In the invention according to claim 4, the equation of motion uses the cant angle of the road surface, the roll angle of the vehicle, the displacement amount of the vehicle, the curvature of the target path, the yaw rate of the vehicle, and the steering angle of the vehicle. Using the first equation of motion that models the lateral motion state of the vehicle, the displacement amount of the vehicle, the curvature of the target path, the yaw rate of the vehicle, and the steering angle of the vehicle, the yaw of the vehicle is used. The second equation of motion, which models the motion state in the direction, and the roll angle of the vehicle, the displacement amount of the vehicle, the curvature of the target path, the yaw rate of the vehicle, and the steering angle of the vehicle are used to obtain the vehicle. It is shown by a third equation of motion that models the state of motion in the roll direction.
請求項５記載の発明では、前記目標経路に対する車両のずれ量、及び前記目標経路の曲率のそれぞれの計測値は、車両前方又は車両後方を撮像するカメラにより撮像された画像と、前記車両の状態量として前回推定された前記ロール角とに基づいて算出される。 In the invention according to claim 5, the measured values of the amount of deviation of the vehicle with respect to the target route and the curvature of the target route are an image captured by a camera that images the front or the rear of the vehicle and a state of the vehicle. The amount is calculated based on the previously estimated roll angle.
本発明によれば、路面のカント角と共に車両のロール角を推定することができる、という効果を有する。 According to the present invention, there is an effect that the roll angle of the vehicle can be estimated together with the cant angle of the road surface.
以下、本実施の形態について図面を参照しながら説明する。なお、同じ構成要素及び処理には全図面を通して同じ符合を付与し、重複する説明を省略するものとする。 Hereinafter, the present embodiment will be described with reference to the drawings. The same components and processes are given the same code throughout the drawings, and duplicate explanations are omitted.
図１は、走行中の車両２の状態量を推定する推定装置１０の機能ブロック図の一例である。図１に示すように、推定装置１０は、推定部１２及び車両位置演算部１４を含み、車両位置演算部１４には撮像装置２２が接続され、推定部１２には車両位置演算部１４、ＩＭＵ(Inertial Measurement Unit)２４、及び操舵装置２６がそれぞれ接続されている。 FIG. 1 is an example of a functional block diagram of the estimation device 10 that estimates the state quantity of the moving vehicle 2. As shown in FIG. 1, the estimation device 10 includes an estimation unit 12 and a vehicle position calculation unit 14, an imaging device 22 is connected to the vehicle position calculation unit 14, and a vehicle position calculation unit 14 and an IMU are connected to the estimation unit 12. The (Inertial Measurement Unit) 24 and the steering device 26 are connected to each other.
撮像装置２２は車両２の走行方向の画像（「前方画像」ともいう）を撮像する単眼カメラであり、車両位置演算部１４は撮像装置２２から前方画像を受け付けると、車両位置を前方画像から演算する車線認識処理を行う。具体的には、車両位置演算部１４は、目標経路４に対する車両２のずれ量及び目標経路４の曲率を車両２の位置情報として前方画像から演算する。車両位置演算部１４は、目標経路４に対する車両２のずれ量及び目標経路４の曲率を撮像装置２２による計測値として推定部１２に通知する。 The image pickup device 22 is a monocular camera that captures an image of the traveling direction of the vehicle 2 (also referred to as a "front image"), and when the vehicle position calculation unit 14 receives the front image from the image pickup device 22, the vehicle position is calculated from the front image. Performs lane recognition processing. Specifically, the vehicle position calculation unit 14 calculates the amount of deviation of the vehicle 2 with respect to the target route 4 and the curvature of the target route 4 from the front image as the position information of the vehicle 2. The vehicle position calculation unit 14 notifies the estimation unit 12 of the amount of deviation of the vehicle 2 with respect to the target route 4 and the curvature of the target route 4 as measured values by the imaging device 22.
また、車両位置演算部１４は、後述する推定部１２で推定された車両２の状態量のうち車両２のロール角を用いて、演算した車両２の位置情報が実際の車両２の位置情報に近づくように位置情報を補正する。すなわち、車両位置演算部１４は、本実施の形態に係る演算部の一例である。 Further, the vehicle position calculation unit 14 uses the roll angle of the vehicle 2 among the state quantities of the vehicle 2 estimated by the estimation unit 12 described later, and the position information of the vehicle 2 calculated is used as the actual position information of the vehicle 2. Correct the position information so that it approaches. That is, the vehicle position calculation unit 14 is an example of the calculation unit according to the present embodiment.
なお、撮像装置２２は必ずしも車両２に必要なセンサでなく、目標経路４に対する車両２のずれ量及び目標経路の曲率を計測することができれば、撮像装置２２の代わりに例えばレーダ装置を用いてもよい。また、撮像装置２２は前方画像の代わりに、車両２の走行方向とは逆方向の画像（「後方画像」ともいう）を撮影してもよい。車両位置演算部１４は後方画像からも車両２の位置情報を演算することができる。 The image pickup device 22 is not necessarily a sensor required for the vehicle 2, and if the deviation amount of the vehicle 2 with respect to the target path 4 and the curvature of the target path can be measured, for example, a radar device may be used instead of the image pickup device 22. Good. Further, the image pickup device 22 may take an image (also referred to as a "rear image") in the direction opposite to the traveling direction of the vehicle 2 instead of the front image. The vehicle position calculation unit 14 can also calculate the position information of the vehicle 2 from the rear image.
ＩＭＵ２４は、慣性センサを用いて、例えば車両２のロールレート及びヨーレートを計測すると共に、車両２の横加速度を計測する装置である。 The IMU 24 is a device that uses an inertial sensor to measure, for example, the roll rate and yaw rate of the vehicle 2 and also measures the lateral acceleration of the vehicle 2.
以降では、ロールレートを計測するセンサのオフセットを「ロールレートセンサのオフセット」、ヨーレートを計測するセンサのオフセットを「ヨーレートセンサのオフセット」、及び車両２の横加速度を計測するセンサのオフセットを「横加速度センサのオフセット」ということにする。なお、「オフセット」とは各々のセンサにおける計測誤差を意味する。 In the following, the offset of the sensor that measures the roll rate is the "offset of the roll rate sensor", the offset of the sensor that measures the yaw rate is the "offset of the yaw rate sensor", and the offset of the sensor that measures the lateral acceleration of the vehicle 2 is "horizontal". It will be called "offset of acceleration sensor". The "offset" means a measurement error in each sensor.
操舵装置２６は、車両２の操舵角を制御して車両２の進行方向を可変させる装置であり、制御した操舵角を推定部１２に通知する。 The steering device 26 is a device that controls the steering angle of the vehicle 2 to change the traveling direction of the vehicle 2, and notifies the estimation unit 12 of the controlled steering angle.
撮像装置２２と、ＩＭＵ２４に含まれるロールレートセンサ、ヨーレートセンサ、及び横加速度センサと、操舵装置２６は、各種センサの一例である。 The image pickup device 22, the roll rate sensor, the yaw rate sensor, the lateral acceleration sensor, and the steering device 26 included in the IMU 24 are examples of various sensors.
推定部１２は、前回推定した車両２の状態量、及び車両２に設置された各種センサによって計測された計測値を入力として、車両２の運動状態をモデル化した複数の運動方程式に基づく状態方程式、及び各種センサの計測値を車両２の状態量を用いて表した車両２の観測方程式を用いて、現時刻における車両２の状態量を推定する。 The estimation unit 12 receives the previously estimated state quantity of the vehicle 2 and the measured values measured by various sensors installed in the vehicle 2 as inputs, and the state equation based on a plurality of equations of motion modeling the motion state of the vehicle 2. And, the state quantity of the vehicle 2 at the current time is estimated by using the observation equation of the vehicle 2 which expresses the measured values of various sensors using the state quantity of the vehicle 2.
具体的には、推定部１２は、状態方程式に基づいて、前回推定された車両２の状態量から、次時刻の車両２の状態量を予測する図示しない予測部と、観測方程式を用いて、予測部で予測された次時刻の前記車両の状態量に対応する各種センサの計測値を算出し、算出した各種センサの計測値と、各種センサで実際に計測された計測値とのそれぞれの差分、及び推定部１２で前回推定された車両２の状態量に基づいて、車両２の状態量を更新する図示しない更新部を含む。 Specifically, the estimation unit 12 uses a prediction unit (not shown) that predicts the state quantity of the vehicle 2 at the next time from the previously estimated state quantity of the vehicle 2 based on the state equation, and an observation equation. Calculate the measured values of various sensors corresponding to the state quantity of the vehicle at the next time predicted by the prediction unit, and the difference between the calculated measured values of the various sensors and the measured values actually measured by the various sensors. , And an update unit (not shown) that updates the state quantity of the vehicle 2 based on the state quantity of the vehicle 2 previously estimated by the estimation unit 12.
車両の「状態量」とは、例えば車両の走行位置、走行方向、車両の傾き、走行する道路の形状、及び各種センサのオフセットのように、車両の走行状態を特定する場合に用いられる物理量の集合をいう。推定部１２は推定した車両２の状態量を、例えば状態量に基づいて車両２の制御を行う図示しない制御装置に出力する。 The "state quantity" of a vehicle is a physical quantity used to specify the traveling condition of a vehicle, such as the traveling position, traveling direction, inclination of the vehicle, the shape of the traveling road, and the offset of various sensors. Refers to a set. The estimation unit 12 outputs the estimated state quantity of the vehicle 2 to, for example, a control device (not shown) that controls the vehicle 2 based on the state quantity.
＜車両の運動方程式＞ <Vehicle equation of motion>
図２は、走行中の車両２における運動状態の一例を示す図である。図２（Ａ）に示すように、目標経路４に対する車両２の横位置偏差をｅ1、ヨー角偏差をｅ2、目標経路の曲率をρ、車両２の車速をＶとすれば、２輪モデルの運動を規定するヨーレートψ・と車両２の横方向速度ｙ・はそれぞれ（１）式及び（２）式で表される。なお、例えば（１）式ではｅ2の上にドット（・）が付加されているが、これはｅ2 ・とも表記され、ドットの数は微分の階数を表す。以降、各式のドットについては同様の対応付けが適用される。 FIG. 2 is a diagram showing an example of an exercise state of the moving vehicle 2. As shown in FIG. 2A, if the lateral position deviation of the vehicle 2 with respect to the target route 4 is e 1 , the yaw angle deviation is e 2 , the curvature of the target route is ρ, and the vehicle speed of the vehicle 2 is V, two wheels. The yaw rate ψ · that defines the motion of the model and the lateral velocity y · of the vehicle 2 are expressed by equations (1) and (2), respectively. For example, in equation (1), a dot (・) is added above e 2 , but this is also expressed as e 2 , and the number of dots represents the rank of differentiation. Hereinafter, the same correspondence is applied to the dots of each expression.
更に、推定部１２で用いる状態方程式は、（３）式〜（５）式で表される運動方程式に基づいて定められる。 Further, the equation of state used in the estimation unit 12 is determined based on the equations of motion represented by the equations (3) to (5).
ここでｌfは車両２の重心から前輪までの距離、ｌrは車両２の重心から後輪までの距離を表す（図２（Ａ）参照）。Ｋf及びＫrはそれぞれ車両２における前輪及び後輪のコーナリングスティフネスを表す。また、δは車両２の操舵角、ｍは車両２の質量、ｇは重力加速度、ｈはロールセンターから車両重心までの距離、ｋxはバネ上ロール剛性、Ｃrはバネ上ロール方向のダンピング定数、Ｉzはヨー方向の慣性モーメント、Ｉxはロール方向の慣性モーメントをそれぞれ表す。 Here, l f represents the distance from the center of gravity of the vehicle 2 to the front wheels, and l r represents the distance from the center of gravity of the vehicle 2 to the rear wheels (see FIG. 2 (A)). K f and K r represent the cornering stiffness of the front wheels and the rear wheels of the vehicle 2, respectively. Δ is the steering angle of the vehicle 2, m is the mass of the vehicle 2, g is the gravitational acceleration, h is the distance from the roll center to the center of gravity of the vehicle, k x is the spring roll rigidity, and C r is the damping in the spring roll direction. The constant, I z represents the moment of inertia in the yaw direction, and I x represents the moment of inertia in the roll direction.
また、φcは路面のカント角、及びφvは車両２のロール角をそれぞれ表す。図２（Ｂ）に示すように、カント角φcは水平面に対する路面の傾きを表し、ロール角φvは路面と直交する直交線に対する車両２の傾きを表す。 Further, φ c represents the cant angle of the road surface, and φ v represents the roll angle of the vehicle 2. As shown in FIG. 2B, the cant angle φ c represents the inclination of the road surface with respect to the horizontal plane, and the roll angle φ v represents the inclination of the vehicle 2 with respect to the orthogonal line orthogonal to the road surface.
（３）式で表される運動方程式は車両２の横方向の運動を表す運動方程式であり、本実施の形態に係る第１の運動方程式の一例である。（４）式で表される運動方程式は車両２のヨー方向の運動を表す運動方程式であり、本実施の形態に係る第２の運動方程式の一例である。（５）式で表される運動方程式は車両２のロール方向の運動を表す運動方程式であり、本実施の形態に係る第３の運動方程式の一例である。 The equation of motion represented by the equation (3) is an equation of motion representing the lateral motion of the vehicle 2, and is an example of the first equation of motion according to the present embodiment. The equation of motion represented by the equation (4) is an equation of motion representing the motion of the vehicle 2 in the yaw direction, and is an example of the second equation of motion according to the present embodiment. The equation of motion represented by the equation (5) is an equation of motion representing the motion of the vehicle 2 in the roll direction, and is an example of the third equation of motion according to the present embodiment.
なお、後述するように、車両２の横位置偏差ｅ1、ヨー角偏差ｅ2、及び目標経路４の曲率ρは撮像装置２２で撮像した前方画像又は後方画像から取得される。また、車速Ｖは図示しない車速センサから取得される。 As will be described later, the lateral position deviation e 1 of the vehicle 2, the yaw angle deviation e 2 , and the curvature ρ of the target path 4 are acquired from the front image or the rear image captured by the imaging device 22. Further, the vehicle speed V is acquired from a vehicle speed sensor (not shown).
＜操舵系の外乱＞ <Rudder system disturbance>
上記に示した車両２の運動方程式では車両２の操舵角をδで表したが、操舵装置２６から取得される操舵角、すなわち、計測された操舵角は、例えば操舵系が有するヒステリシス特性等による制御遅れや、ハンドルの遊び及び操舵系のがたつき等の外乱により、必ずしもタイヤの切れ角と一致しない場合がある。すなわち、計測された操舵角にはオフセットが含まれる。 In the motion equation of the vehicle 2 shown above, the steering angle of the vehicle 2 is represented by δ, but the steering angle acquired from the steering device 26, that is, the measured steering angle depends on, for example, the hysteresis characteristics of the steering system. It may not always match the turning angle of the tire due to disturbances such as control delay, steering wheel play, and steering system rattling. That is, the measured steering angle includes an offset.
操舵系の制御遅れは１次遅れとして近似可能であり、１次遅れの時定数をτ、操舵角のオフセットをγ’、時間をｔとすれば、計測された操舵角δγは（６）式で表される。 The control delay of the steering system can be approximated as a first-order delay. If the time constant of the first-order delay is τ, the steering angle offset is γ ' , and the time is t, the measured steering angle δ γ is (6). It is represented by an expression.
（６）式に対して、時間ｔによって変化する操舵角の外乱γを（７）式のように定義した場合、計測された操舵角δγは（８）式によって表されることになる。すなわち、（３）式〜（５）式に示した車両２の運動方程式では、操舵装置２６の過渡応答性による外乱量と操舵装置２６のオフセットによる外乱量が、操舵角の外乱γという１つのパラメータによって表されている。以降では、操舵角δは外乱が含まれない操舵角の真値を表す。 When the disturbance γ of the steering angle that changes with time t is defined as in the equation (7) with respect to the equation (6), the measured steering angle δ γ is expressed by the equation (8). That is, in the equations of motion of the vehicle 2 shown in Eqs. (3) to (5), the amount of disturbance due to the transient response of the steering device 26 and the amount of disturbance due to the offset of the steering device 26 are one of the disturbance γ of the steering angle. It is represented by a parameter. Hereinafter, the steering angle δ represents the true value of the steering angle that does not include disturbance.
＜状態方程式＞ <Equation of state>
車両２の状態量を（９）式のように定義する。 The state quantity of the vehicle 2 is defined as the equation (9).
（９）式の状態量において、αrはヨーレートセンサのオフセット、αφはロールレートセンサのオフセット、αGは横加速度センサのオフセットを表す。 In the state quantity of equation (9), α r represents the offset of the yaw rate sensor, α φ represents the offset of the roll rate sensor, and α G represents the offset of the lateral acceleration sensor.
そして、（３）式〜（５）式に示した車両２の運動方程式に基づいて、車両２の状態量ｘの状態方程式を行列式で表した場合、（１０）式のように表記される。 Then, when the equation of state of the state quantity x of the vehicle 2 is expressed by a determinant based on the equation of motion of the vehicle 2 shown in equations (3) to (5), it is expressed as equation (10). ..
ここでＥは単位行列であり、Ａは各状態量の係数を表す係数行列である。推定部１２は、カルマンフィルタを用いて（１１）式に示した状態方程式を予め定めた間隔で逐次演算することにより、車両２の状態量ｘを推定する。 Here, E is an identity matrix, and A is a coefficient matrix representing the coefficients of each state quantity. The estimation unit 12 estimates the state quantity x of the vehicle 2 by sequentially calculating the equation of state shown in the equation (11) using a Kalman filter at predetermined intervals.
実際にカルマンフィルタを用いて車両２の状態量ｘを推定するためには、（１１）式で表される状態方程式を離散化した状態方程式が用いられる。（１２）式は、（１１）式の状態方程式を離散化した状態方程式である。 In order to actually estimate the state quantity x of the vehicle 2 using the Kalman filter, a state equation obtained by discretizing the state equation represented by the equation (11) is used. Equation (12) is a discretized equation of state of equation (11).
変数ｋは時系列の順序を表す整数である。すなわち、ｘkは時刻ｋにおける車両２の状態量を表し、ｘk+1は時刻ｋの次の時刻（ｋ＋１）における車両２の状態量を表す。Ｄは状態行列と呼ばれ、Ｄ＝Ｅ-1Ａである。 The variable k is an integer representing the order of the time series. That is, x k represents the state quantity of the vehicle 2 at the time k, and x k + 1 represents the state quantity of the vehicle 2 at the time (k + 1) next to the time k. D is called the state matrix and D = E -1 A.
したがって、カルマンフィルタを用いて状態量ｘの各要素を推定するためには、要素毎に離散化した状態方程式（１３）式が用いられる。 Therefore, in order to estimate each element of the state quantity x using the Kalman filter, the equation of state (13) discretized for each element is used.
ここで（１３）式で表される各要素の状態方程式におけるｗは「システムノイズ」と呼ばれ、状態方程式の白色雑音として定義される分散値である。また、状態量ｘの各要素の分散値を行列値とする対角行列Ｑを、システムノイズを表す共分散行列という。 Here, w in the equation of state of each element represented by the equation (13) is called "system noise" and is a variance value defined as white noise in the equation of state. Further, a diagonal matrix Q in which the variance value of each element of the state quantity x is a matrix value is called a covariance matrix representing system noise.
（１３）式において、ｅ1,k ・・、ｅ2,k ・・、及びφv,k ・・は（３）式〜（５）式で示した車両２の運動方程式をそれぞれｅ1 ・・、ｅ2 ・・、φv ・・について変形した（１４）式〜（１６）式に示す運動方程式によって定義される。 (13) In the equation, e 1, k ··, e 2, k ··, and φ v, k ·· is (3) ~ (5) e 1 · each motion equation of the vehicle 2 shown by formula ·, e 2 ··, is defined by the equation of motion shown in phi v · · deformed about (14) to (16).
このように、本実施の形態に係る車両２の運動方程式には、目標経路４の曲率ρ及び曲率変化率ρ・が含まれる。 Thus, the equation of motion of the vehicle 2 according to this embodiment includes a curvature [rho and the curvature change rate [rho · target path 4.
以降では（１４）式〜（１６）式に示す運動方程式を車両２の運動方程式として用いることにする。 Hereinafter, the equations of motion shown in equations (14) to (16) will be used as the equation of motion of the vehicle 2.
＜観測方程式＞ <Observation equation>
推定部１２では各種センサで計測された計測値と、（１１）式に示した状態方程式を用いて予測した状態量とを比較する。このとき、予測した状態量を、各種センサの計測値に変換して比較する。例えばヨーレートセンサの計測値をｒとすれば、ヨーレート計測値ｒは、車両２の状態量に基づいて（１７）式の観測方程式から得られる。 The estimation unit 12 compares the measured values measured by various sensors with the state quantity predicted by using the equation of state shown in equation (11). At this time, the predicted state quantity is converted into the measured values of various sensors and compared. For example, assuming that the measured value of the yaw rate sensor is r, the yaw rate measured value r can be obtained from the observation equation of Eq. (17) based on the state quantity of the vehicle 2.
（１７）式によれば、目標経路４の曲率ρを加味した車両２のヨーレートを推定することができる。 According to the equation (17), the yaw rate of the vehicle 2 can be estimated in consideration of the curvature ρ of the target route 4.
一方、ロールレートセンサの計測値をφ・とすれば、ロールレート計測値φ・は、車両２の状態量に基づいて（１８）式の観測方程式から得られる。 On the other hand, if the measured value of the roll rate sensor is φ · , the roll rate measured value φ · can be obtained from the observation equation of Eq. (18) based on the state quantity of the vehicle 2.
（１８）式によれば、車両２のロールレートは、推定されたカント角の微分値φ・ c、ロール角の微分値φv ・、及びロールレートセンサのオフセットαφから推定することができる。 According to the equation (18), the roll rate of the vehicle 2 can be estimated from the estimated differential value φ · c of the cant angle, the differential value φ v · of the roll angle, and the offset α φ of the roll rate sensor. ..
更に、横加速度センサの計測値をａyとすれば、横加速度センサ計測値ａyは、車両２の状態量に基づいて（１９）式の観測方程式から得られる。 Further, assuming that the measured value of the lateral acceleration sensor is a y , the lateral acceleration sensor measured value a y can be obtained from the observation equation of Eq. (19) based on the state quantity of the vehicle 2.
（１９）式によれば、車両２の横方向にかかる加速度は、車両２の横位置偏差の２階微分ｅ1 ・・、車速Ｖ、曲率ρ、カント角φc、ロール角φv、及び横加速度センサのオフセットαGから推定することができる。また、（１９）式の右辺１項目で表される車両２の運動に伴って車両２の横方向にかかる加速度、及び右辺２項目で表される重力による加速度を分離して推定することも可能である。 (19) According to the equation, acceleration applied in the lateral direction of the vehicle 2, the second order derivative e 1 · · of transverse position of the vehicle 2, the vehicle speed V, the curvature [rho, cant angle phi c, a roll angle phi v and, It can be estimated from the offset α G of the lateral acceleration sensor. It is also possible to separately estimate the lateral acceleration of the vehicle 2 due to the movement of the vehicle 2 represented by one item on the right side of equation (19) and the acceleration due to gravity represented by the two items on the right side. Is.
車両２の状態量から算出される、各種センサの計測値ｙをｙ＝(ｅ1 ｅ2 ρ r φ・ ａy δγ)とすれば、時刻ｋにおける計測値ｙは（２０）式で表される。 If the measured values y of various sensors calculated from the state quantity of the vehicle 2 are y = (e 1 e 2 ρ r φ · a y δ γ ), the measured values y at time k are expressed by Eq. (20). Will be done.
行列Ｃは観測行列と呼ばれ、行列Ｒは観測方程式の観測ノイズを示す共分散行列である。 The matrix C is called an observation matrix, and the matrix R is a covariance matrix showing the observation noise of the observation equation.
＜推定装置の処理＞ <Processing of estimation device>
次に、推定装置１０における車両２の状態量ｘの推定方法について説明する。 Next, a method of estimating the state quantity x of the vehicle 2 in the estimation device 10 will be described.
図３は、例えば推定装置１０が推定の開始指示を受け付けた場合に、推定装置１０に備えられたＣＰＵによって実行される推定処理の流れの一例を示すフローチャートである。 FIG. 3 is a flowchart showing an example of a flow of estimation processing executed by a CPU provided in the estimation device 10, for example, when the estimation device 10 receives an instruction to start estimation.
推定処理を規定する推定プログラムは、例えば推定装置１０に備えられたＲＯＭに予め記憶されている。推定装置１０のＣＰＵは、ＲＯＭに記憶される推定プログラムを読み込んで推定処理を実行する。 The estimation program that defines the estimation process is stored in advance in, for example, a ROM provided in the estimation device 10. The CPU of the estimation device 10 reads the estimation program stored in the ROM and executes the estimation process.
なお、推定装置１０のＲＡＭには前回推定した時刻ｋにおける車両２の状態量ｘkが記憶されているものとする。前回推定した車両２の状態量ｘkが存在しない場合には、予め設定された状態量ｘの初期値が前回推定した時刻ｋにおける車両２の状態量ｘkとして用いられる。 It is assumed that the RAM of the estimation device 10 stores the state quantity x k of the vehicle 2 at the time k estimated last time. When the previously estimated state quantity x k of the vehicle 2 does not exist, the initial value of the preset state quantity x is used as the state quantity x k of the vehicle 2 at the time k estimated last time.
推定処理ではカルマンフィルタを用いた状態量ｘk+1の推定が行われる。そのため、ステップＳ１０において、推定部１２は撮像装置２２、ＩＭＵ２４、及び操舵装置２６の各種センサで計測された計測値を取得する。具体的には、推定部１２は、車両位置演算部１４を介して、撮像装置２２で撮像された画像から得られた車両２のずれ量の一例である横位置偏差ｅ1及びヨー角偏差ｅ2と、目標経路４の曲率ρを取得する。また、推定部１２は、車両２のロールレート計測値φ・、ヨーレート計測値ｒ、及び横加速度センサ計測値ａyをＩＭＵ２４から取得する。更に、推定部１２は計測された操舵角δγを操舵装置２６から取得する。すなわち、推定部１２は計測値ｙを取得する。 In the estimation process, the state quantity x k + 1 is estimated using the Kalman filter. Therefore, in step S10, the estimation unit 12 acquires the measured values measured by the various sensors of the image pickup device 22, the IMU 24, and the steering device 26. Specifically, the estimation unit 12 has a lateral position deviation e 1 and a yaw angle deviation e, which are examples of the deviation amount of the vehicle 2 obtained from the image captured by the image pickup device 22 via the vehicle position calculation unit 14. 2 and the curvature ρ of the target path 4 are acquired. Further, the estimation unit 12 acquires the roll rate measurement value φ · , the yaw rate measurement value r, and the lateral acceleration sensor measurement value a y of the vehicle 2 from the IMU 24. Further, the estimation unit 12 acquires the measured steering angle δ γ from the steering device 26. That is, the estimation unit 12 acquires the measured value y.
上述したように、車両位置演算部１４は撮像装置２２で撮像された画像に対して車線認識処理を行い、車両２の横位置偏差ｅ1、ヨー角偏差ｅ2、及び目標経路４の曲率ρを計測することから、車両位置演算部１４の処理について説明する。 As described above, the vehicle position calculation unit 14 performs lane recognition processing on the image captured by the image pickup device 22, the lateral position deviation e 1 of the vehicle 2, the yaw angle deviation e 2 , and the curvature ρ of the target path 4. The processing of the vehicle position calculation unit 14 will be described with reference to the measurement.
図４は、車両位置演算部１４で実行される、車両２の横位置偏差ｅ1、ヨー角偏差ｅ2、及び目標経路４の曲率ρを計測する車線認識処理の流れの一例を示すフローチャートである。車線認識処理を規定する車線認識プログラムも、例えば推定装置１０に備えられたＲＯＭに予め記憶されている。推定装置１０のＣＰＵは、ＲＯＭに記憶される車線認識プログラムを読み込んで車線認識処理を実行する。 FIG. 4 is a flowchart showing an example of the flow of the lane recognition process for measuring the lateral position deviation e 1 , the yaw angle deviation e 2 , and the curvature ρ of the target path 4 executed by the vehicle position calculation unit 14. is there. The lane recognition program that defines the lane recognition process is also stored in advance in, for example, a ROM provided in the estimation device 10. The CPU of the estimation device 10 reads the lane recognition program stored in the ROM and executes the lane recognition process.
ここでは一例として、車両２の目標経路４が路面上の車線に沿って設定されているものとする。 Here, as an example, it is assumed that the target route 4 of the vehicle 2 is set along the lane on the road surface.
ステップＳ１００において、車両位置演算部１４は、車両２の前方画像を撮像装置２２から取得する。車両位置演算部１４は後方画像を撮像装置２２から取得してもよいが、ここでは前方画像を取得する例について説明する。 In step S100, the vehicle position calculation unit 14 acquires a front image of the vehicle 2 from the image pickup device 22. The vehicle position calculation unit 14 may acquire a rear image from the image pickup device 22, but here, an example of acquiring a front image will be described.
ステップＳ１１０において、車両位置演算部１４は、公知のパターン認識又は画像処理を用いて、ステップＳ１００で取得した前方画像から車線を抽出する。 In step S110, the vehicle position calculation unit 14 extracts a lane from the front image acquired in step S100 by using known pattern recognition or image processing.
ステップＳ１２０において、車両位置演算部１４は、ステップＳ１１０で抽出した車線を、路面を車両２の真上から見た図（鳥瞰図）に投影する座標変換を実行する。 In step S120, the vehicle position calculation unit 14 executes coordinate conversion for projecting the lane extracted in step S110 onto a view (bird's-eye view) of the road surface viewed from directly above the vehicle 2.
ステップＳ１３０において、車両位置演算部１４は、例えば道路をモデル化したクロソイド曲線に車線の３次元位置を当てはめることで、路面上における車線の位置を推定する車線パラメータ推定を実行する。 In step S130, the vehicle position calculation unit 14 executes lane parameter estimation for estimating the position of the lane on the road surface by, for example, applying the three-dimensional position of the lane to the clothoid curve modeled on the road.
ステップＳ１４０において、車両位置演算部１４は、ステップＳ１３０で推定した路面上における車線の位置に基づいて、目標経路４に対する車両２のずれ量の一例である横位置偏差ｅ1及びヨー角偏差ｅ2を算出する。また、車両位置演算部１４は、ステップＳ１３０で推定した路面上における車線の位置に基づいて、目標経路４の曲率ρを算出する。 In step S140, the vehicle position calculation unit 14 determines the lateral position deviation e 1 and the yaw angle deviation e 2 which are examples of the deviation amount of the vehicle 2 with respect to the target route 4 based on the position of the lane on the road surface estimated in step S130. Is calculated. Further, the vehicle position calculation unit 14 calculates the curvature ρ of the target route 4 based on the position of the lane on the road surface estimated in step S130.
そして、車両位置演算部１４は、算出した車両２の横位置偏差ｅ1、ヨー角偏差ｅ2、及び目標経路４の曲率ρを推定部１２に通知する。 Then, the vehicle position calculation unit 14 notifies the estimation unit 12 of the calculated lateral position deviation e 1 , yaw angle deviation e 2 , and curvature ρ of the target path 4.
なお、車両位置演算部１４は、目標経路４の曲率ρの単位時間における変化量から曲率変化率ρ・を算出して車両位置演算部１４に通知してもよい。以上により、図４に示した車線認識処理を終了する。 The vehicle position calculation unit 14 may calculate the curvature change rate ρ · from the amount of change in the curvature ρ of the target path 4 in a unit time and notify the vehicle position calculation unit 14. As a result, the lane recognition process shown in FIG. 4 is completed.
図３のステップＳ２０において、推定部１２は、前回推定した車両２の状態量ｘkの各々を（１３）式に代入し、時刻（ｋ＋１）における車両２の状態量ｘk+1を予測する。なお、説明の便宜上、ステップＳ２０で予測した状態量ｘk+1を特にｘk+1 -と表すことにする。 In step S20 of FIG. 3, the estimation unit 12 substitutes each of the previously estimated state quantities x k of the vehicle 2 into the equation (13), and predicts the state quantity x k + 1 of the vehicle 2 at the time (k + 1). .. For convenience of explanation, the state quantity x k + 1 predicted in step S20 is expressed as x k + 1 − in particular.
上述したように（１３）式におけるｅ1,k ・・、ｅ2,k ・・、及びφv,k ・・は、（１４）式〜（１６）式に示す運動方程式から得られる。 As described above, e 1, k ... , e 2, k ... , And φ v, k ... in Eq. (13) are obtained from the equations of motion shown in Eqs . (14) to (16).
図５は、ｅ1,k ・・、ｅ2,k ・・、及びφv,k ・・が、（１４）式〜（１６）式に示す運動方程式から得られることを示す図である。 FIG. 5 is a diagram showing that e 1, k ... , e 2, k ... , And φ v, k ... Can be obtained from the equations of motion shown in equations (14) to (16).
車両２の横方向の運動方程式は（１４）式で表されることから、車両２の横位置偏差の２階微分値ｅ1 ・・は、状態量ｘkに含まれる操舵角δ、操舵角の外乱γ、車両２の横位置偏差の１階微分値ｅ1 ・、車両２のヨー角偏差ｅ2、車両２のヨー角偏差の１階微分値ｅ2 ・、目標経路４の曲率ρ、カント角φc、ロール角φv、及びロール角の１階微分値φv ・から推定される。 Since the equation of motion in the lateral direction of the vehicle 2 is expressed by the equation (14), the second derivative value e 1 ... of the lateral position deviation of the vehicle 2 is the steering angle δ and the steering angle included in the state quantity x k. disturbance gamma, first order differential value e 1 · the transverse position of the vehicle 2, a yaw angle deviation e 2 of the vehicle 2, the first derivative value e 2 · yaw angle deviation of the vehicle 2, the curvature of the target path 4 [rho, It is estimated from the cant angle φ c , the roll angle φ v , and the first derivative value φ v · of the roll angle.
また、車両２のヨー方向の運動方程式は（１５）式で表されることから、車両２のヨー角偏差の２階微分値ｅ2 ・・は、状態量ｘkに含まれる操舵角δ、操舵角の外乱γ、車両２の横位置偏差の１階微分値ｅ1 ・、車両２のヨー角偏差ｅ2、車両２のヨー角偏差の１階微分値ｅ2 ・、目標経路４の曲率ρ、及び目標経路４の曲率変化率ρ・から推定される。 Further, since the equation of motion of the vehicle 2 in the yaw direction is expressed by the equation (15), the second derivative value e 2 ... Of the yaw angle deviation of the vehicle 2 is the steering angle δ included in the state quantity x k . disturbance γ of the steering angle, first derivative e 1 ·, yaw angle deviation e 2 of the vehicle 2, the first derivative value e 2 · yaw angle deviation of the vehicle 2, the curvature of the target path 4 of transverse position of the vehicle 2 It is estimated from ρ and the curvature change rate ρ · of the target path 4.
更に、車両２のロール方向の運動方程式は（１６）式で表されることから、車両２のロール角の２階微分値φv ・・は、状態量ｘkに含まれる操舵角δ、操舵角の外乱γ、車両２の横位置偏差の１階微分値ｅ1 ・、車両２のヨー角偏差ｅ2、車両２のヨー角偏差の１階微分値ｅ2 ・、目標経路４の曲率ρ、ロール角φv、及びロール角の１階微分値φv ・から推定される。 Further, since the equation of motion in the roll direction of the vehicle 2 is expressed by the equation (16), the second derivative value φ v ··· of the roll angle of the vehicle 2 is the steering angle δ included in the state quantity x k , and the steering. disturbance γ corner, first derivatives e 1 · the transverse position of the vehicle 2, a yaw angle deviation e 2 of the vehicle 2, the first derivative value e 2 · yaw angle deviation of the vehicle 2, the curvature of the target path 4 [rho , Roll angle φ v , and the first derivative value φ v · of the roll angle.
（１３）式に示すように、車両２の横位置偏差の２階微分値ｅ1 ・・、車両２のヨー角偏差の２階微分値ｅ2 ・・、及び車両２のロール角の２階微分値φv ・・を積分した値と、各種センサで計測された計測値を用いてカルマンフィルタによるフィルタリングを行うことで、ｅ1、ｅ1 ・、ｅ2、ｅ2 ・、φv、φv ・がそれぞれ推定される。 (13) As shown in equation second order differential value e 2 · ·, and the second floor of the roll angle of the vehicle 2 in the second-order differential value e 1 · ·, yaw angle deviation of the vehicle 2 in the transverse position of the vehicle 2 a value obtained by integrating the differential value phi v · ·, by performing filtering by Kalman using measurement values measured by the various sensors, e 1, e 1 ·, e 2, e 2 ·, φ v, φ v・Are estimated respectively.
図５に示すように、カント角φcは車両２の横方向の運動のみに影響を与え、ロール角φvは車両２の横方向の運動方程式及びロール方向の運動に影響を与える。したがって、推定部１２は、カント角φcとロール角φvが影響を与える運動に違いがあることを利用して、カント角φcとロール角φvをそれぞれ分離して推定する。 As shown in FIG. 5, the cant angle φ c affects only the lateral motion of the vehicle 2, and the roll angle φ v affects the lateral equation of motion and the roll direction motion of the vehicle 2. Therefore, the estimation unit 12, by using the fact that there are differences in movement cant angle phi c and the roll angle phi v affects, estimated by separating each cant angle phi c and the roll angle phi v.
また、図５に示すように、カント角φcは車両２の横方向の運動のみに影響を与え、操舵角の外乱γは車両２の横方向の運動、及びヨー方向の運動に影響を与える。したがって、推定部１２は、カント角φcと操舵角の外乱γが影響を与える運動に違いがあることを利用して、カント角φcと操舵角の外乱γをそれぞれ分離して推定する。 Further, as shown in FIG. 5, the cant angle φ c affects only the lateral motion of the vehicle 2, and the disturbance γ of the steering angle affects the lateral motion and the yaw direction motion of the vehicle 2. .. Therefore, the estimation unit 12, by using the fact that there are differences in movement disturbances cant angle phi c and the steering angle γ influences, estimates separated respectively cant angle phi c steering angle disturbance γ a.
なお、（１６）式に示す車両２のロール方向の運動方程式には操舵角の外乱γが含まれるため、図５では操舵角の外乱γが車両２のロール方向の運動にも影響を与えているように見える。しかし、（５）式で示される変形前の車両２のロール方向の運動方程式には操舵角の外乱γは含まれないことから、操舵角の外乱γは車両２の横方向の運動、及びヨー方向の運動に影響を与えることになる。 Since the equation of motion of the vehicle 2 in the roll direction shown in equation (16) includes the disturbance γ of the steering angle, the disturbance γ of the steering angle also affects the movement of the vehicle 2 in the roll direction in FIG. It looks like there is. However, since the equation of motion in the roll direction of the vehicle 2 before deformation represented by the equation (5) does not include the disturbance γ of the steering angle, the disturbance γ of the steering angle is the lateral motion of the vehicle 2 and the yaw. It will affect the movement of the direction.
また、本実施の形態では、操舵角の外乱γが１つのパラメータでモデル化されているため、例えば操舵装置２６の過渡応答性による外乱量と操舵装置２６のオフセットによる外乱量をそれぞれ分離した運動方程式を用いる場合と比較して、カルマンフィルタを用いた車両２の状態量ｘkの推定が単純になり、車両２の状態量ｘkの推定に要する時間を短縮することができる。 Further, in the present embodiment, since the disturbance γ of the steering angle is modeled by one parameter, for example, the disturbance amount due to the transient response of the steering device 26 and the disturbance amount due to the offset of the steering device 26 are separated from each other. Compared with the case of using the equation, the estimation of the state quantity x k of the vehicle 2 using the Kalman filter becomes simple, and the time required for estimating the state quantity x k of the vehicle 2 can be shortened.
ステップＳ３０において、推定部１２は、前回推定した状態方程式の状態ノイズを示す共分散行列Ｐkを（２１）式に代入し、時刻（ｋ＋１）における共分散行列Ｐk+1を予測する。なお、説明の便宜上、ステップＳ３０で予測した共分散行列Ｐk+1を特にＰk+1 -と表すことにする。 In step S30, the estimation unit 12 substitutes the covariance matrix P k indicating the state noise of the previously estimated state equation into the equation (21), and predicts the covariance matrix P k + 1 at the time (k + 1). For convenience of explanation, the covariance matrix P k + 1 predicted in step S30 will be expressed as P k + 1 − in particular.
ステップＳ４０において、推定部１２は（２２）式によって、ステップＳ３０で予測した共分散行列Ｐk+1 -と、（２０）式における観測行列Ｃ及び共分散行列Ｒを用いてカルマンゲイン行列Ｇkを算出する。 In step S40, the estimation unit 12 uses the covariance matrix P k + 1 − predicted in step S30 by the equation (22), and the observation matrix C and the covariance matrix R in the equation (20) to use the Kalman gain matrix G k. Is calculated.
ステップＳ５０において、推定部１２は（２３）式に従って、ステップＳ２０で予測した状態量ｘk+1 -、ステップＳ４０で算出したカルマンゲイン行列Ｇk、ステップＳ１０で取得した計測値ｙ、及び計測値ｙの観測行列Ｃを用いて、ステップＳ２０で予測した状態量ｘk+1 -を更新する。更新後の状態量ｘk+1 -は状態量ｘk+1で表される。 In step S50, the estimation unit 12 determines the state quantity x k + 1 − predicted in step S20, the Kalman gain matrix G k calculated in step S40, the measured value y acquired in step S10, and the measured value according to equation (23). Using the observation matrix C of y, the state quantity x k + 1 − predicted in step S20 is updated. State of the updated x k + 1 - is represented by the state quantity x k + 1.
（２３）式に示すように、状態量ｘk+1はセンサの計測値ｙと、観測行列Ｃで重み付けを行った状態量ｘk+1 -の差分に対して、カルマンゲイン行列Ｇkを作用させることで更新される。 As shown in equation (23), the state quantity x k + 1 is the Kalman gain matrix G k for the difference between the measured value y of the sensor and the state quantity x k + 1 − weighted by the observation matrix C. It is updated by acting.
ステップＳ６０において、推定部１２は（２４）式に従って、ステップＳ４０で算出したカルマンゲイン行列Ｇk、ステップＳ１０で取得した計測値ｙの観測行列Ｃ、及びステップＳ３０で予測した前回の共分散行列Ｐk -を用いて、ステップＳ３０で予測した共分散行列Ｐk+1 -を更新する。更新後の共分散行列Ｐk+1 -は共分散行列Ｐk+1で表される。なお、Ｉは単位行列を表す。 In step S60, the estimation unit 12 determines the Kalman gain matrix G k calculated in step S40, the observation matrix C of the measured value y acquired in step S10, and the previous covariance matrix P predicted in step S30 according to equation (24). k - using the covariance matrix predicted in step S30 P k + 1 - to update. Covariance matrix of the updated P k + 1 - is represented by the covariance matrix P k + 1. In addition, I represents a unit matrix.
ステップＳ７０において、推定部１２はステップＳ５０で更新した状態量ｘk+1を出力する。 In step S70, the estimation unit 12 outputs the state quantity x k + 1 updated in step S50.
ステップＳ８０において、推定部１２は、推定処理の終了指示を受け付けたか否かを判定し、終了指示を受け付けていない場合にはステップＳ１０に移行して、現在のセンサの計測値ｙを取得する。ステップＳ１０〜Ｓ８０を繰り返し実行することで、時系列に沿った車両２の状態量ｘk+n(nは１以上の整数）を推定することができる。 In step S80, the estimation unit 12 determines whether or not the end instruction of the estimation process has been accepted, and if the end instruction has not been received, the process proceeds to step S10 to acquire the measured value y of the current sensor. By repeatedly executing steps S10 to S80, the state quantity x k + n (n is an integer of 1 or more) of the vehicle 2 along the time series can be estimated.
なお、ステップＳ８０の判定処理で終了指示を受け付けていないと判定された場合、予め定めた間隔だけ待機してからステップＳ１０に移行するようにしてもよい。この場合、待機時間を調整することで、例えば１００Ｈｚや１ｋＨｚというように予め定めた周期で車両２の状態量ｘk+nを推定することができる。推定装置１０で推定された状態量ｘk+nは未来の状態量を表すことから、状態量ｘk+nは例えば目標経路４に沿った車両２の自動運転に利用される。 If it is determined in the determination process of step S80 that the end instruction has not been accepted, the process may proceed to step S10 after waiting for a predetermined interval. In this case, by adjusting the standby time, the state quantity x k + n of the vehicle 2 can be estimated at a predetermined cycle such as 100 Hz or 1 kHz. Since the state quantity x k + n estimated by the estimation device 10 represents the future state quantity, the state quantity x k + n is used, for example, for the automatic driving of the vehicle 2 along the target route 4.
一方、ステップＳ８０の判定処理で終了指示を受け付けたと判定される場合には、図３に示す推定処理を終了する。 On the other hand, when it is determined in the determination process of step S80 that the end instruction has been accepted, the estimation process shown in FIG. 3 is terminated.
なお、単眼カメラを用いた車両位置演算部１４における車線認識処理では、単に前方画像だけから車両２の位置情報の演算する場合、路面に対する車両２のロール角φvが加味されない。したがって、車両位置演算部１４には、ステップＳ７０で出力された車両２の状態量のうちロール角φvが入力される。車両位置演算部１４は、入力されたロール角φvを用いて、前方画像だけから得られた車両２の横位置偏差ｅ1、ヨー角偏差ｅ2、及び目標経路４の曲率ρを補正する。 In the lane recognition process in the vehicle position calculation unit 14 using the monocular camera, the roll angle φ v of the vehicle 2 with respect to the road surface is not taken into consideration when the position information of the vehicle 2 is calculated only from the front image. Therefore, the roll angle φ v of the state quantity of the vehicle 2 output in step S70 is input to the vehicle position calculation unit 14. The vehicle position calculation unit 14 uses the input roll angle φ v to correct the lateral position deviation e 1 , the yaw angle deviation e 2 , and the curvature ρ of the target path 4 obtained only from the front image. ..
図４に示した車線認識処理において、ロール角φvを用いた車両２の横位置偏差ｅ1、ヨー角偏差ｅ2、及び目標経路４の曲率ρの補正位置は複数存在する。 In the lane recognition process shown in FIG. 4, there are a plurality of correction positions for the lateral position deviation e 1 of the vehicle 2 and the yaw angle deviation e 2 using the roll angle φ v , and the curvature ρ of the target path 4.
例えば、車両位置演算部１４はステップＳ１００において、撮像装置２２から取得した車両２の前方画像をロール角φvだけ回転させ、以降の車線認識処理では、回転補正した車両２の前方画像を用いるようにする。これにより、車両２の横位置偏差ｅ1、ヨー角偏差ｅ2、及び目標経路４の曲率ρが実際の車両２の位置情報に近づくように補正される。 For example, in step S100, the vehicle position calculation unit 14 rotates the front image of the vehicle 2 acquired from the image pickup device 22 by the roll angle φ v , and in the subsequent lane recognition processing, the rotation-corrected front image of the vehicle 2 is used. To. As a result, the lateral position deviation e 1 of the vehicle 2, the yaw angle deviation e 2 , and the curvature ρ of the target path 4 are corrected so as to approach the actual position information of the vehicle 2.
また、図４のステップＳ１２０において、ステップＳ１１０で抽出した車線を鳥瞰図に投影する座標変換を行う場合、本来であれば路面に対する車両２の姿勢角が必要になるため、車両２の姿勢角の一例であるロール角φvを加味した座標変換を行うことで、車線が精度よく鳥瞰図に投影されることになる。したがって、車両２の横位置偏差ｅ1、ヨー角偏差ｅ2、及び目標経路４の曲率ρが実際の車両２の位置情報に近づくように補正される。 Further, in step S120 of FIG. 4, when performing coordinate conversion for projecting the lane extracted in step S110 onto a bird's-eye view, the attitude angle of the vehicle 2 with respect to the road surface is normally required, so an example of the attitude angle of the vehicle 2. By performing coordinate transformation that takes into account the roll angle φ v , which is, the lane is accurately projected on the bird's-eye view. Therefore, the lateral position deviation e 1 of the vehicle 2, the yaw angle deviation e 2 , and the curvature ρ of the target path 4 are corrected so as to approach the actual position information of the vehicle 2.
また、推定装置１０の実機による実験や推定装置１０の設計仕様に基づくコンピュータシミュレーション等により、前方画像だけから算出した車両２の横位置偏差ｅ1、ヨー角偏差ｅ2、及び目標経路４の曲率ρと、ロール角φvを考慮して算出した車両２の横位置偏差ｅ1、ヨー角偏差ｅ2、及び目標経路４の曲率ρとのそれぞれの誤差（補正値）をロール角φv毎に予め定めた変換テーブルを準備して例えばＲＯＭに記憶しておき、当該変換テーブルを参照して、図４のステップＳ１４０で前方画像だけから算出した車両２の横位置偏差ｅ1、ヨー角偏差ｅ2、及び目標経路４の曲率ρを入力されたロール角φvに対応した補正値で補正してもよい。この場合においても、車両２の横位置偏差ｅ1、ヨー角偏差ｅ2、及び目標経路４の曲率ρが実際の車両２の位置情報に近づくように補正される。 In addition, the lateral position deviation e 1 , the yaw angle deviation e 2 , and the curvature of the target path 4 of the vehicle 2 calculated only from the front image by an experiment using the estimation device 10 and a computer simulation based on the design specifications of the estimation device 10. The error (correction value) between ρ and the lateral position deviation e 1 of the vehicle 2 calculated in consideration of the roll angle φ v , the yaw angle deviation e 2 , and the curvature ρ of the target path 4 is calculated for each roll angle φ v. A predetermined conversion table is prepared in, for example, stored in a ROM, and the lateral position deviation e 1 and the yaw angle deviation of the vehicle 2 calculated only from the front image in step S140 of FIG. 4 with reference to the conversion table. The curvature ρ of e 2 and the target path 4 may be corrected by the correction value corresponding to the input roll angle φ v . Also in this case, the lateral position deviation e 1 of the vehicle 2, the yaw angle deviation e 2 , and the curvature ρ of the target path 4 are corrected so as to approach the actual position information of the vehicle 2.
したがって、車両位置演算部１４は上述した何れかの補正位置において、車両２の横位置偏差ｅ1、ヨー角偏差ｅ2、及び目標経路４の曲率ρの補正を行えばよい。 Therefore, the vehicle position calculation unit 14 may correct the lateral position deviation e 1 , the yaw angle deviation e 2 , and the curvature ρ of the target path 4 at any of the above-mentioned correction positions.
これまでは、ＩＭＵ２４から車両２のロールレート、ヨーレート、及び横加速度を取得して車両２の状態量ｘを推定する推定装置１０の例について説明したが、カント角φcの推定には必ずしも車両２のヨーレート及び横加速度は必要ではなく、車両２のロールレートだけから推定することができる。ロールレート計測値φ・、カント角φc、及びロールレートセンサのオフセットαφがわかれば、（１８）式からカント角φcを推定することが可能である。 So far, an example of the estimation device 10 that acquires the roll rate, yaw rate, and lateral acceleration of the vehicle 2 from the IMU 24 and estimates the state quantity x of the vehicle 2 has been described, but the vehicle is not always estimated for the cant angle φ c. The yaw rate and lateral acceleration of 2 are not required and can be estimated only from the roll rate of vehicle 2. If the roll rate measurement value φ · , the cant angle φ c , and the offset α φ of the roll rate sensor are known, the cant angle φ c can be estimated from Eq. (18).
（１４）式〜（１６）式に示したように、推定装置１０で車両２の状態量ｘを推定するための状態方程式を定義する場合に用いられる車両２の運動方程式には、目標経路４の曲率ρ及び曲率変化率ρ・が含まれる。したがって、車両２の状態量ｘの推定に曲率ρ及び曲率変化率ρ・が含まれない運動方程式を用いる場合と比較して、推定装置１０は、カーブの走行時に車両２が傾いた場合であっても、遠心力の影響を排除して路面のカント角φcを精度よく推定することができる。 As shown in the equations (14) to (16), the target path 4 is included in the equation of motion of the vehicle 2 used when the equation of state for estimating the state quantity x of the vehicle 2 is defined by the estimation device 10. Curvature ρ and curvature change rate ρ · are included. Therefore, compared to the case where the equation of motion that does not include the curvature ρ and the curvature change rate ρ · is used for estimating the state quantity x of the vehicle 2, the estimation device 10 is a case where the vehicle 2 is tilted when traveling on a curve. However, the influence of centrifugal force can be eliminated and the cant angle φ c of the road surface can be estimated accurately.
また、従来ヨーレートセンサのオフセットαrは、撮像装置２２で撮像された画像から取得された曲率ρを用いて推定していたが、本実施の形態に係る推定装置１０は、車両２の運動状態を加味した運動方程式及び観測方程式から推定された曲率ρを用いる。したがって、撮像装置２２で撮像された画像から取得された曲率ρを用いる場合と比較して、ヨーレートセンサのオフセットαrを精度よく推定することができる。 Further, conventionally, the offset α r of the yaw rate sensor has been estimated by using the curvature ρ acquired from the image captured by the image pickup device 22, but the estimation device 10 according to the present embodiment has the motion state of the vehicle 2. The curvature ρ estimated from the equation of motion and the observation equation with the above in mind is used. Therefore, the offset α r of the yaw rate sensor can be estimated more accurately than the case where the curvature ρ acquired from the image captured by the image pickup apparatus 22 is used.
以上、実施の形態を用いて本発明について説明したが、本発明は実施の形態に記載の範囲には限定されない。本発明の要旨を逸脱しない範囲で実施の形態に多様な変更又は改良を加えることができ、当該変更又は改良を加えた形態も本発明の技術的範囲に含まれる。例えば、本発明の要旨を逸脱しない範囲で処理の順序を変更してもよい。 Although the present invention has been described above using the embodiments, the present invention is not limited to the scope described in the embodiments. Various changes or improvements can be made to the embodiments without departing from the gist of the present invention, and the modified or improved forms are also included in the technical scope of the present invention. For example, the order of processing may be changed without departing from the gist of the present invention.
また、実施の形態では、一例として推定処理及び車線認識処理をソフトウエアで実現する形態について説明したが、図３及び図４に示したフローチャートと同等の処理を、例えばＡＳＩＣ（Application Specific Integrated Circuit）に実装し、ハードウエアで処理させるようにしてもよい。この場合、ソフトウエアを用いた場合と比較して、推定処理及び車線認識処理を高速化することができる。 Further, in the embodiment, an embodiment in which estimation processing and lane recognition processing are realized by software has been described as an example, but processing equivalent to the flowcharts shown in FIGS. 3 and 4 can be performed by, for example, an ASIC (Application Specific Integrated Circuit). It may be implemented in the hardware and processed by hardware. In this case, the estimation process and the lane recognition process can be speeded up as compared with the case where software is used.
また、上述した実施の形態では、推定プログラム及び車線変更プログラムがＲＯＭにインストールされている形態を説明したが、これに限定されるものではない。本発明に係る推定プログラム及び車線変更プログラムは、コンピュータ読み取り可能な記憶媒体に記録された形態で提供することも可能である。例えば、本発明に係る推定プログラム及び車線変更プログラムを、ＣＤ(Compact Disc)−ＲＯＭ、又はＤＶＤ(Digital Versatile Disc)−ＲＯＭ等の光ディスクに記録した形態で提供してもよい。また、本発明に係る推定プログラム及び車線変更プログラムを、ＵＳＢメモリ及びフラッシュメモリ等の半導体メモリに記録した形態で提供してもよい。更に、推定装置１０は、例えばインターネット等の通信回線に接続された外部装置から、本発明に係る推定プログラム及び車線変更プログラムを取得するようにしてもよい。 Further, in the above-described embodiment, the mode in which the estimation program and the lane change program are installed in the ROM has been described, but the present invention is not limited to this. The estimation program and the lane change program according to the present invention can also be provided in a form recorded on a computer-readable storage medium. For example, the estimation program and the lane change program according to the present invention may be provided in the form of being recorded on an optical disk such as a CD (Compact Disc) -ROM or a DVD (Digital Versatile Disc) -ROM. Further, the estimation program and the lane change program according to the present invention may be provided in the form of being recorded in a semiconductor memory such as a USB memory and a flash memory. Further, the estimation device 10 may acquire the estimation program and the lane change program according to the present invention from an external device connected to a communication line such as the Internet.
２・・・車両
４・・・目標経路
１０・・・推定装置
１２・・・推定部
１４・・・車両位置演算部
２２・・・撮像装置
２４・・・ＩＭＵ
２６・・・操舵装置
Ｖ・・・車速
2 ... Vehicle 4 ... Target route 10 ... Estimating device 12 ... Estimating unit 14 ... Vehicle position calculation unit 22 ... Imaging device 24 ... IMU
26 ... Steering device V ... Vehicle speed
Claims (5)
を備えた推定装置。 The measured values of the amount of deviation of the vehicle with respect to the target route, the curvature of the target route, the yaw rate of the vehicle, and the steering angle of the vehicle, the amount of deviation of the vehicle, the curvature of the target route, the yaw rate of the vehicle, and the above. The cant angle of the road surface on which the vehicle travels and the cant angle of the vehicle using a plurality of equations of motion that model the motion state of the vehicle using the steering angle of the vehicle, the cant angle of the road surface, and the roll angle of the vehicle. An estimation device including an estimation unit that estimates the state amount of the vehicle including the roll angle.
前記運動方程式は、前記車両のロールレート、及び前記車両の横方向の加速度、前記車両のロールレートを計測するロールレートセンサのオフセット、前記車両の加速度を計測する加速度センサのオフセット、及び前記車両の操舵角の外乱を更に用いてモデル化され、
前記車両の状態量は、前記車両のロールレート、前記車両の横方向の加速度、前記車両のロールレートを計測するロールレートセンサのオフセット、前記車両の加速度を計測する加速度センサのオフセット、及び前記車両の操舵角の外乱を更に含む
請求項１記載の推定装置。 The measured values further include the respective measured values regarding the roll rate of the vehicle and the lateral acceleration of the vehicle.
The equations of motion are the roll rate of the vehicle, the lateral acceleration of the vehicle, the offset of the roll rate sensor for measuring the roll rate of the vehicle, the offset of the acceleration sensor for measuring the acceleration of the vehicle, and the offset of the vehicle. Modeled with further steering angle disturbances,
The amount of state of the vehicle includes the roll rate of the vehicle, the lateral acceleration of the vehicle, the offset of the roll rate sensor for measuring the roll rate of the vehicle, the offset of the acceleration sensor for measuring the acceleration of the vehicle, and the vehicle. The estimation device according to claim 1, further comprising a disturbance of the steering angle of the vehicle.
観測方程式を用いて、予測された次時刻の前記車両の状態量に対応する前記それぞれの計測値を算出し、前記算出された前記それぞれの計測値と、計測された前記それぞれの計測値との差分、及び前回推定された前記車両の状態量に基づいて、前記予測部で予測された前記車両の状態量を更新する更新部と、
を含む請求項１又は請求項２記載の推定装置。 The estimation unit includes a prediction unit that predicts the state quantity of the vehicle at the next time from the previously estimated state quantity of the vehicle based on the equation of motion.
Using the observation equation, each of the measured values corresponding to the state quantity of the vehicle at the predicted next time is calculated, and the calculated measured value and the measured measured value are set to each other. An update unit that updates the state quantity of the vehicle predicted by the prediction unit based on the difference and the state quantity of the vehicle previously estimated.
The estimation device according to claim 1 or 2, comprising the above.
請求項１〜請求項３の何れか１項に記載の推定装置。 The equation of motion uses the cant angle of the road surface, the roll angle of the vehicle, the displacement amount of the vehicle, the curvature of the target path, the yaw rate of the vehicle, and the steering angle of the vehicle to move the vehicle laterally. Using the first equation of motion that models the state, the amount of displacement of the vehicle, the curvature of the target path, the yaw rate of the vehicle, and the steering angle of the vehicle, the motion state of the vehicle in the yaw direction was modeled. The second equation of motion and the roll angle of the vehicle, the amount of displacement of the vehicle, the curvature of the target path, the yaw rate of the vehicle, and the steering angle of the vehicle are used to model the motion state of the vehicle in the roll direction. The estimation device according to any one of claims 1 to 3, which is represented by the third equation of motion.
請求項１〜請求項４の何れか１項に記載の推定装置。 The measured values of the amount of deviation of the vehicle with respect to the target route and the curvature of the target route are the image captured by the camera that images the front or the rear of the vehicle and the roll previously estimated as the state amount of the vehicle. The estimation device according to any one of claims 1 to 4, which is calculated based on the angle.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2019038915A JP7014197B2 (en) | 2019-03-04 | 2019-03-04 | Estimator |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2019038915A JP7014197B2 (en) | 2019-03-04 | 2019-03-04 | Estimator |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2020142568A true JP2020142568A (en) | 2020-09-10 |
| JP7014197B2 JP7014197B2 (en) | 2022-02-01 |
Family
ID=72355167
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2019038915A Active JP7014197B2 (en) | 2019-03-04 | 2019-03-04 | Estimator |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP7014197B2 (en) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2003026023A (en) * | 2001-07-12 | 2003-01-29 | Nissan Motor Co Ltd | Lane following travel control device |
| JP2005239012A (en) * | 2004-02-26 | 2005-09-08 | Mitsubishi Motors Corp | Power steering device for vehicle and disturbance estimating device for vehicle |
| JP2006236238A (en) * | 2005-02-28 | 2006-09-07 | Aisin Seiki Co Ltd | Lane traveling supporting device for vehicle |
| JP2013129289A (en) * | 2011-12-21 | 2013-07-04 | Toyota Motor Corp | Vehicle state quantity estimating device and vehicle steering control apparatus |
| US20160068166A1 (en) * | 2013-05-02 | 2016-03-10 | GM Global Technology Operations LLC | Integrated bank and roll estimation using a three-axis inertial-measuring device |
| JP2018079872A (en) * | 2016-11-18 | 2018-05-24 | 本田技研工業株式会社 | Vehicle control device |
-
2019
- 2019-03-04 JP JP2019038915A patent/JP7014197B2/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2003026023A (en) * | 2001-07-12 | 2003-01-29 | Nissan Motor Co Ltd | Lane following travel control device |
| JP2005239012A (en) * | 2004-02-26 | 2005-09-08 | Mitsubishi Motors Corp | Power steering device for vehicle and disturbance estimating device for vehicle |
| JP2006236238A (en) * | 2005-02-28 | 2006-09-07 | Aisin Seiki Co Ltd | Lane traveling supporting device for vehicle |
| JP2013129289A (en) * | 2011-12-21 | 2013-07-04 | Toyota Motor Corp | Vehicle state quantity estimating device and vehicle steering control apparatus |
| US20160068166A1 (en) * | 2013-05-02 | 2016-03-10 | GM Global Technology Operations LLC | Integrated bank and roll estimation using a three-axis inertial-measuring device |
| JP2018079872A (en) * | 2016-11-18 | 2018-05-24 | 本田技研工業株式会社 | Vehicle control device |
Also Published As
| Publication number | Publication date |
|---|---|
| JP7014197B2 (en) | 2022-02-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US10297033B2 (en) | Information processing apparatus and method | |
| US9753144B1 (en) | Bias and misalignment compensation for 6-DOF IMU using GNSS/INS data | |
| KR102440358B1 (en) | Inertial-based navigation device and Inertia-based navigation method based on relative preintegration | |
| WO2019114807A1 (en) | Multi-sensor target information fusion | |
| JP6110256B2 (en) | Object estimation apparatus and object estimation method | |
| JP5173854B2 (en) | Sensor drift estimation device | |
| JP5639874B2 (en) | Driving assistance device | |
| US9988082B2 (en) | Traveling path estimation apparatus | |
| JP7028649B2 (en) | Vehicle, vehicle motion state estimation device and vehicle motion state estimation method | |
| JP6776707B2 (en) | Own vehicle position estimation device | |
| JP4670528B2 (en) | Imaging device deviation detection method, imaging device deviation correction method, and imaging device | |
| KR102542377B1 (en) | Autonomous method and device for determining a global inclination of a motor vehicle | |
| JP6478318B2 (en) | Vehicle motion parameter estimation device | |
| JPH1123291A (en) | Picture processing device for car | |
| JP5402244B2 (en) | Vehicle physical quantity estimation device | |
| CN112874529B (en) | Vehicle mass center slip angle estimation method and system based on event trigger state estimation | |
| KR20200007879A (en) | Driving control method and driving control device | |
| JP4502733B2 (en) | Obstacle measuring method and obstacle measuring device | |
| JP7014197B2 (en) | Estimator | |
| JP2019007739A (en) | Self position estimation method and self position estimation device | |
| JP2019018773A (en) | Control system for suspension | |
| JP2021193340A (en) | Self-position estimation device | |
| JP5091047B2 (en) | Vehicle attitude angle estimation device and sensor drift estimation device | |
| WO2022203026A1 (en) | Driving control device | |
| JP4861897B2 (en) | Vehicle state estimation device |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20200727 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20210721 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20211005 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20211202 |
|
| TRDD | Decision of grant or rejection written | ||
| A01 | Written decision to grant a patent or to grant a registration (utility model) |
Free format text: JAPANESE INTERMEDIATE CODE: A01 Effective date: 20211221 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20220103 |