JP2010085196A - Object behavior estimation method and object behavior estimating device - Google Patents

Object behavior estimation method and object behavior estimating device Download PDF

Info

Publication number
JP2010085196A
JP2010085196A JP2008253513A JP2008253513A JP2010085196A JP 2010085196 A JP2010085196 A JP 2010085196A JP 2008253513 A JP2008253513 A JP 2008253513A JP 2008253513 A JP2008253513 A JP 2008253513A JP 2010085196 A JP2010085196 A JP 2010085196A
Authority
JP
Japan
Prior art keywords
acceleration
vector
acceleration sensor
equation
behavior estimation
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.)
Pending
Application number
JP2008253513A
Other languages
Japanese (ja)
Inventor
Masaaki Okamoto
昌明 岡本
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Toyota Motor Corp
Original Assignee
Toyota Motor Corp
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Toyota Motor Corp filed Critical Toyota Motor Corp
Priority to JP2008253513A priority Critical patent/JP2010085196A/en
Publication of JP2010085196A publication Critical patent/JP2010085196A/en
Pending legal-status Critical Current

Links

Images

Landscapes

  • Navigation (AREA)

Abstract

<P>PROBLEM TO BE SOLVED: To grasp accurately a behavior of a specific domain as a whole, including influence due to deformation. <P>SOLUTION: In this object behavior infer device 10, a translating acceleration acquisition part acquires each detection value of translating triaxial acceleration sensors 18 arranged on three or more spots nonexisting on one straight line, such as, the first acceleration sensor 18A to the seventh acceleration sensor 18G, in a vehicle 12 whose behavior is to be estimated. A behavior infer section infers the angular velocity vector and the angular acceleration vector of the vehicle 12, by applying an operation that uses a least-squares method on the acquired detection values. <P>COPYRIGHT: (C)2010,JPO&INPIT

Description

本発明は、物体挙動推定技術に関し、特に、物体の角速度ベクトルおよび角加速度ベクトルを推定する物体挙動推定技術に関する。   The present invention relates to an object behavior estimation technique, and more particularly to an object behavior estimation technique for estimating an angular velocity vector and an angular acceleration vector of an object.

例えば車両走行中に車両がどのような挙動を示すかを予め把握することは、車両の制動力制御や姿勢制御を適切に実施するために極めて重要である。このため、例えば、重力の方向を基準とする車両の傾斜角度を連続的に検出し、車両の角速度を積分して該車両の傾斜角度の変化量を連続的に検出し、これらの検出出力を加算して車両の傾斜角度を算出する車両の傾斜角度検出装置が提案されている（例えば、特許文献１参照）。また、例えば、自動車などの物体の表面に、特定の位置関係および検出方向をもって配置した複数の加速度センサーの検出値から物体の回転角速度、姿勢角、および位置標定用加速度情報を推定する姿勢検出装置が提案されている（例えば、特許文献２参照）。
特開２００１−７４４４９号公報 特開平６−１７４４８７号公報 For example, grasping in advance how the vehicle behaves while the vehicle is running is extremely important in order to appropriately perform the braking force control and the attitude control of the vehicle. For this reason, for example, the inclination angle of the vehicle with respect to the direction of gravity is continuously detected, the angular velocity of the vehicle is integrated, the change amount of the inclination angle of the vehicle is continuously detected, and these detection outputs are There has been proposed a vehicle inclination angle detection device that calculates the inclination angle of a vehicle by addition (see, for example, Patent Document 1). Also, for example, an attitude detection device that estimates rotational angular velocity, attitude angle, and positioning acceleration information of an object from detection values of a plurality of acceleration sensors arranged on the surface of the object such as an automobile with a specific positional relationship and detection direction Has been proposed (see, for example, Patent Document 2).
JP 2001-74449 A JP-A-6-174487

例えば車両などの挙動を推定すべき物体も、実際には移動中に微小に変形する。しかし、例えば特許文献１に示す技術ではロール角を検出するセンサを用いているため、検出個所におけるロール角を検出することはできるが、変形による影響も含む物体の特定領域全体の挙動を把握することは困難である。また、加速度センサなどによる検出には通常誤差を伴う。しかし、例えば特許文献２に示す技術ではこのような誤差を充分に考慮していないため、物体の姿勢を正確に求めることができないおそれがある。   For example, an object such as a vehicle whose behavior is to be estimated actually deforms slightly during movement. However, for example, the technique shown in Patent Document 1 uses a sensor that detects the roll angle, so that the roll angle at the detection location can be detected, but the behavior of the entire specific region of the object including the influence of deformation is grasped. It is difficult. Further, detection by an acceleration sensor or the like usually involves an error. However, for example, the technique disclosed in Patent Document 2 does not sufficiently consider such an error, and thus there is a possibility that the posture of the object cannot be obtained accurately.

そこで、本発明は上述した課題を解決するためになされたものであり、その目的は、変形による影響も含む物体の特定領域全体の挙動を正確に把握することにある。   Therefore, the present invention has been made to solve the above-described problems, and an object thereof is to accurately grasp the behavior of the entire specific region of the object including the influence of deformation.

上記課題を解決するために、本発明のある態様の物体挙動推定方法は、挙動を推定すべき物体において直線上にない３以上の個所に配置された並進３軸の加速度センサの各々の検出値を取得するステップと、取得した検出値に最小二乗法を用いた演算を施して物体の角速度ベクトルおよび角加速度ベクトルを推定するステップと、を備える。   In order to solve the above-described problems, an object behavior estimation method according to an aspect of the present invention is a detection value of each of translational triaxial acceleration sensors arranged at three or more locations that are not on a straight line in an object whose behavior is to be estimated And obtaining an angular velocity vector and an angular acceleration vector of the object by performing an operation using the least square method on the acquired detection value.

この態様によれば、３以上の個所に配置された加速度センサの検出値を用いることにより、変形による影響も含む物体の特定領域全体の挙動を把握することが可能となる。また、最小二乗法を用いることで検出誤差の影響を抑制することが可能となり、物体の挙動を正確に把握することができる。   According to this aspect, by using the detection values of the acceleration sensors arranged at three or more locations, it is possible to grasp the behavior of the entire specific region of the object including the influence of deformation. Further, by using the least square method, it is possible to suppress the influence of detection error, and it is possible to accurately grasp the behavior of the object.

角速度ベクトルおよび角加速度ベクトルを推定するステップは、

Figure 2010085196
としたときに、
Figure 2010085196
 から、すべての加速度センサの平均位置ｐｍから位置ｐｉにある加速度センサのまでの相対位置ｑｉを用いて
Figure 2010085196
 として、最小二乗法を用いて誤差εｉの二乗和
Figure 2010085196
 を最少にする物体の角速度ベクトルおよび角加速度ベクトルを推定してもよい。 Estimating the angular velocity vector and the angular acceleration vector includes
Figure 2010085196
 And when
Figure 2010085196
 From using the relative positions q i from the average position p m of all the acceleration sensor to the acceleration sensor at position p i
Figure 2010085196
 As the sum of squares of error ε i using the least squares method
Figure 2010085196
 The angular velocity vector and the angular acceleration vector of the object that minimizes may be estimated.

この態様によれば、誤差εｉの二乗和を最小にすることにより、少ない誤差で角速度ベクトルおよび角加速度ベクトルを推定することができる。 According to this aspect, the angular velocity vector and the angular acceleration vector can be estimated with a small error by minimizing the sum of squares of the error ε i .

本発明の別の態様は、物体挙動推定装置である。この装置は、挙動を推定すべき物体において直線上にない３以上の個所に配置された並進３軸の加速度センサの各々の検出値を取得する並進加速度取得部と、物体の角速度ベクトルおよび角加速度ベクトルを推定する挙動推定部と、を備える。挙動推定部は、取得した検出値に最小二乗法を用いた演算を施して物体の角速度ベクトルおよび角加速度ベクトルを推定する。   Another aspect of the present invention is an object behavior estimation apparatus. This apparatus includes a translational acceleration acquisition unit that acquires detection values of translational triaxial acceleration sensors arranged at three or more points that are not on a straight line in an object whose behavior is to be estimated, and an angular velocity vector and an angular acceleration of the object. A behavior estimation unit for estimating a vector. The behavior estimation unit estimates the angular velocity vector and the angular acceleration vector of the object by performing an operation using the least square method on the acquired detection value.

この態様によれば、３以上の個所に配置された加速度センサの検出値を用いることにより、変形による影響も含む物体の特定領域全体の挙動を把握することが可能となる。また、最小二乗法を用いることで検出誤差の影響を抑制することが可能となり、物体の挙動を正確に把握することができる。   According to this aspect, it is possible to grasp the behavior of the entire specific region of the object including the influence of deformation by using the detection values of the acceleration sensors arranged at three or more locations. Further, by using the least square method, it is possible to suppress the influence of detection error, and it is possible to accurately grasp the behavior of the object.

挙動推定部は、

Figure 2010085196
としたときに、
Figure 2010085196
 から、すべての加速度センサの平均位置ｐｍから位置ｐｉにある加速度センサのまでの相対位置ｑｉを用いて
Figure 2010085196
 として、最小二乗法を用いて誤差εｉの二乗和
Figure 2010085196
 を最少にする物体の角速度ベクトルおよび角加速度ベクトルを推定してもよい。 The behavior estimation unit
Figure 2010085196
 And when
Figure 2010085196
 From using the relative positions q i from the average position p m of all the acceleration sensor to the acceleration sensor at position p i
Figure 2010085196
 As the sum of squares of error ε i using the least squares method
Figure 2010085196
 The angular velocity vector and the angular acceleration vector of the object that minimizes may be estimated.

この態様によれば、誤差εｉの二乗和を最小にすることにより、少ない誤差で角速度ベクトルおよび角加速度ベクトルを推定することができる。 According to this aspect, the angular velocity vector and the angular acceleration vector can be estimated with a small error by minimizing the sum of squares of the error ε i .

本発明によれば、変形による影響も含む物体の特定領域全体の挙動を正確に把握することができる。   According to the present invention, it is possible to accurately grasp the behavior of the entire specific region of the object including the influence of deformation.

以下、図面を参照して本発明の実施の形態（以下、「実施形態」という。）について詳細に説明する。   Hereinafter, embodiments of the present invention (hereinafter referred to as “embodiments”) will be described in detail with reference to the drawings.

図１は、本実施形態に係る物体挙動推定装置１０と、物体挙動推定装置１０による挙動推定の対象となる車両１２とを模式的に示す図である。なお、図１において物体挙動推定装置１０は車両１２内に設けられているように記載されているが、物体挙動推定装置１０は車両１２外に設けられていてもよい。   FIG. 1 is a diagram schematically illustrating an object behavior estimation apparatus 10 according to the present embodiment and a vehicle 12 that is a target of behavior estimation by the object behavior estimation apparatus 10. Although the object behavior estimation device 10 is described as being provided in the vehicle 12 in FIG. 1, the object behavior estimation device 10 may be provided outside the vehicle 12.

車両１２は、車両本体１４および４つの車輪によって構成される。以下、左前輪１６ＦＬ、右前輪１６ＦＲ、左後輪１６ＲＬ、および右後輪１６ＲＲを必要に応じて車輪１６と総称する。   The vehicle 12 includes a vehicle main body 14 and four wheels. Hereinafter, the left front wheel 16FL, the right front wheel 16FR, the left rear wheel 16RL, and the right rear wheel 16RR are collectively referred to as wheels 16 as necessary.

車両１２には、第１加速度センサ１８Ａ〜第７加速度センサ１８Ｇが設けられている。以下、これらを必要に応じて加速度センサ１８と総称する。加速度センサ１８の各々は、互いに直交する３軸の各々の並進加速度を検出するよう設けられている。加速度センサ１８の各々は、車両１２の前後方向、左右方向、および上下方向の３軸の各々の並進加速度を検出することができるよう車両１２に配置される。なお、車両１２に配置される加速度センサ１８の数が７つに限られないことは勿論であり、３以上のいずれかの数であってもよい。   The vehicle 12 is provided with a first acceleration sensor 18A to a seventh acceleration sensor 18G. Hereinafter, these are collectively referred to as the acceleration sensor 18 as necessary. Each of the acceleration sensors 18 is provided to detect the translational acceleration of each of the three axes orthogonal to each other. Each of the acceleration sensors 18 is arranged in the vehicle 12 so as to detect the translational acceleration of each of the three axes of the vehicle 12 in the front-rear direction, the left-right direction, and the up-down direction. Of course, the number of acceleration sensors 18 arranged in the vehicle 12 is not limited to seven, and may be any number of three or more.

第１加速度センサ１８Ａは、車両１２の先端部における左右方向中央に配置される。第２加速度センサ１８Ｂは左前輪１６ＦＬの後方近傍に配置され、第３加速度センサ１８Ｃは右前輪１６ＦＲの後方近傍に配置される。このように、第１加速度センサ１８Ａ〜第３加速度センサ１８Ｃは、車両１２の前方側に分散して配置される。   The first acceleration sensor 18 </ b> A is disposed at the center in the left-right direction at the tip of the vehicle 12. The second acceleration sensor 18B is disposed in the vicinity of the rear of the left front wheel 16FL, and the third acceleration sensor 18C is disposed in the vicinity of the rear of the right front wheel 16FR. As described above, the first acceleration sensor 18 </ b> A to the third acceleration sensor 18 </ b> C are distributed and arranged on the front side of the vehicle 12.

第４加速度センサ１８Ｄは左後輪１６ＲＬの前方近傍に配置され、第５加速度センサ１８Ｅは右後輪１６ＲＲの前方近傍に配置される。第６加速度センサ１８Ｆは左後輪１６ＲＬの後方近傍に配置され、第７加速度センサ１８Ｇは右後輪１６ＲＲの後方近傍に配置される。このように、第４加速度センサ１８Ｄ〜第７加速度センサ１８Ｇは、車両１２の後方側に分散して配置される。なお、加速度センサ１８の各々は、３つ以上が一直線上に位置しないよう配置される。   The fourth acceleration sensor 18D is disposed near the front of the left rear wheel 16RL, and the fifth acceleration sensor 18E is disposed near the front of the right rear wheel 16RR. The sixth acceleration sensor 18F is disposed near the rear of the left rear wheel 16RL, and the seventh acceleration sensor 18G is disposed near the rear of the right rear wheel 16RR. As described above, the fourth acceleration sensor 18 </ b> D to the seventh acceleration sensor 18 </ b> G are distributed on the rear side of the vehicle 12. In addition, each of the acceleration sensors 18 is arrange | positioned so that three or more may not be located on a straight line.

図２は、本実施形態に係る物体挙動推定装置１０の構成を示す図である。なお、図２において物体挙動推定装置１０は、各種演算処理を実行するＣＰＵ、各種制御プログラムを格納するＲＯＭ、データ格納やプログラム実行のためのワークエリアとして利用されるＲＡＭなどのハードウェア、およびソフトウェアの連携によって実現される機能ブロックが描かれている。したがって、これらの機能ブロックはハードウェアおよびソフトウェアの組合せによって様々な形で実現することができる。   FIG. 2 is a diagram illustrating a configuration of the object behavior estimation apparatus 10 according to the present embodiment. 2, the object behavior estimation apparatus 10 includes hardware and software such as a CPU that executes various arithmetic processes, a ROM that stores various control programs, a RAM that is used as a work area for data storage and program execution, and the like. The functional blocks realized by the cooperation are drawn. Therefore, these functional blocks can be realized in various forms by a combination of hardware and software.

物体挙動推定装置１０は、並進加速度取得部３０、挙動推定部３２、および表示制御部３４を有する。物体挙動推定装置１０は加速度センサ１８の各々に接続されており、並進加速度取得部３０は、加速度センサ１８の各々の検出値を取得する。挙動推定部３２は、取得された加速度センサ１８の各々の検出値を利用して、車両１２の全体または一部の領域の角速度ベクトル、角加速度ベクトルなどの挙動を推定する。物体挙動推定装置１０はディスプレイ４０に接続されており、表示制御部３４は、挙動推定部３２による挙動推定結果をディスプレイ４０に表示させるよう表示データを生成する。   The object behavior estimation apparatus 10 includes a translational acceleration acquisition unit 30, a behavior estimation unit 32, and a display control unit 34. The object behavior estimation apparatus 10 is connected to each of the acceleration sensors 18, and the translational acceleration acquisition unit 30 acquires each detection value of the acceleration sensor 18. The behavior estimation unit 32 estimates behaviors such as an angular velocity vector and an angular acceleration vector of the whole or a partial region of the vehicle 12 by using the acquired detection values of the acceleration sensor 18. The object behavior estimation apparatus 10 is connected to the display 40, and the display control unit 34 generates display data so that the behavior estimation result by the behavior estimation unit 32 is displayed on the display 40.

以下、車両１２の全体または一部の領域の角速度ベクトル、角加速度ベクトルなどの挙動を推定するときの方法について詳細に説明する。   Hereinafter, a method for estimating the behavior of the entire region of the vehicle 12 such as an angular velocity vector and an angular acceleration vector will be described in detail.

Figure 2010085196
としたとき、これらの関係は、
Figure 2010085196
 によって表される。これを数式１とする。
Figure 2010085196
 And these relationships are
Figure 2010085196
 Represented by This is represented by Equation 1.

この数式１において、角速度ベクトル、角加速度ベクトル、および基準位置の加速度が未知数となる。求めたいのは角速度ベクトルおよび角加速度ベクトルであることから、この数式１からまず基準位置の加速度を消去する。この式を加速度センサの設置位置（以下、「計測点」という）全点で総和を取ると、

Figure 2010085196
となる。これを数式２とする。したがって、
Figure 2010085196
 となる。これを数式３とする。 In Equation 1, the angular velocity vector, the angular acceleration vector, and the acceleration at the reference position are unknown. Since it is desired to obtain the angular velocity vector and the angular acceleration vector, the acceleration at the reference position is first erased from Equation 1. When this equation is summed at all points where the acceleration sensor is installed (hereinafter referred to as “measurement points”),
Figure 2010085196
 It becomes. This is represented by Formula 2. Therefore,
Figure 2010085196
 It becomes. This is represented by Equation 3.

ここで、全計測点の平均加速度ａｍは、

Figure 2010085196
となる。これを数式４とする。また、全計測点の平均位置ｐｍは、
Figure 2010085196
 となる。これを数式５とする。基準位置の加速度は、これらを利用して、
Figure 2010085196
 と表すことができる。これを数式６とする。この数式６を上述の数式１に代入すると、
Figure 2010085196
 となる。これを数式７とする。 The average acceleration a m of all measurement points,
Figure 2010085196
 It becomes. This is represented by Equation 4. The average position p m of all measurement points,
Figure 2010085196
 It becomes. This is represented by Equation 5. The acceleration of the reference position uses these,
Figure 2010085196
 It can be expressed as. This is represented by Equation 6. Substituting Equation 6 into Equation 1 above,
Figure 2010085196
 It becomes. This is represented by Equation 7.

ここで、基準位置の加速度に対するｉ番目の加速度センサ設置個所の相対加速度αｉは、

Figure 2010085196
で表すことができる。これを数式８とする。また、基準位置に対するｉ番目の加速度センサ設置個所の相対位置ｑｉは、
Figure 2010085196
 で表すことができる。これを数式９とする。これら数式８および数式９を上記の数式７に代入すると、
Figure 2010085196
 となる。これを数式１０とする。こうして、数式１から基準位置の加速度を削除することができる。 Here, the relative acceleration α i at the location where the i-th acceleration sensor is installed with respect to the acceleration at the reference position is:
Figure 2010085196
 Can be expressed as This is represented by Equation 8. The relative position q i of the i th acceleration sensor installation position with respect to the reference position is
Figure 2010085196
 Can be expressed as This is represented by Equation 9. Substituting these Equations 8 and 9 into Equation 7 above,
Figure 2010085196
 It becomes. This is represented by Equation 10. Thus, the acceleration at the reference position can be deleted from Equation 1.

この数式１０に対して、最小二乗法を用いて、

Figure 2010085196
を演算する。これを数式１１とする。これにより、誤差εｉの二乗和
Figure 2010085196
 を最少にする車両１２の角速度ベクトルおよび角加速度ベクトルを推定する。これを数式１２とする。このための同定モデルは、
Figure 2010085196
 となる。これを数式１３とする。 For this formula 10, using the least squares method,
Figure 2010085196
 Is calculated. This is expressed as Equation 11. This gives the sum of squares of error ε i
Figure 2010085196
 Estimate the angular velocity vector and the angular acceleration vector of the vehicle 12 that minimizes. This is expressed as Equation 12. The identification model for this is
Figure 2010085196
 It becomes. This is expressed as Equation 13.

しかし、数式１３は非線形微分方程式となっており、そのまま最小二乗法を用いるのは困難である。このため、数式１１をまず線形方程式に近似する。数式１３をテイラー展開すると、

Figure 2010085196
となる。これを数式１４とする。このため、線形モデルは、
Figure 2010085196
 となる。これを数式１５とする。 However, Formula 13 is a nonlinear differential equation, and it is difficult to use the least square method as it is. Therefore, Equation 11 is first approximated to a linear equation. When Equation 13 is expanded to Taylor,
Figure 2010085196
 It becomes. This is represented by Equation 14. For this reason, the linear model is
Figure 2010085196
 It becomes. This is expressed as Equation 15.

上述の数式１２は、

Figure 2010085196
と表すことができる。これを数式１６とする。一方、数式１５を行列で表すと、
Figure 2010085196
 となる。これを数式１７とする。数式１７において、
Figure 2010085196
 である。これを数式１８とする。また、数式１７において、
Figure 2010085196
 である。これを数式１９とする。したがって、これを数式１６は、
Figure 2010085196
 となる。これを数式２０とする。 Equation 12 above is
Figure 2010085196
 It can be expressed as. This is represented by Equation 16. On the other hand, when Expression 15 is represented by a matrix,
Figure 2010085196
 It becomes. This is represented by Equation 17. In Equation 17,
Figure 2010085196
 It is. This is represented by Equation 18. Also, in Equation 17,
Figure 2010085196
 It is. This is represented by Equation 19. Therefore, this is expressed by Equation 16.
Figure 2010085196
 It becomes. This is represented by Equation 20.

ここで、逐次線形近似最小二乗式は、

Figure 2010085196
で表される。これを数式２１とする。数式２１を変形することにより、
Figure 2010085196
 となる。これを数式２２とする。この数式２２において、γ１およびγ２は、それぞれ、
Figure 2010085196
 となる。これを数式２３とする。また、数式２２において、Ａ１１、Ａ１２、Ａ２１、およびＡ２２は、それぞれ、
Figure 2010085196
 となる。これを数式２４とする。 Here, the successive linear approximation least squares equation is
Figure 2010085196
 It is represented by This is expressed as Equation 21. By transforming Equation 21:
Figure 2010085196
 It becomes. This is represented by Equation 22. In Equation 22, γ1 and γ2 are respectively
Figure 2010085196
 It becomes. This is expressed as Equation 23. In Formula 22, A 11 , A 12 , A 21 , and A 22 are respectively
Figure 2010085196
 It becomes. This is represented by Equation 24.

一方、時間積分法より、

Figure 2010085196
によって角速度の近似解を求める。これを数式２５とする。陽解法では、
Figure 2010085196
 となる。これを数式２６とする。また、台形則では、
Figure 2010085196
 となる。これを数式２７とする。また、陰解法では、
Figure 2010085196
 となる。これを数式２８とする。また、
Figure 2010085196
 の関係がある。これを数式２９とする。 On the other hand, from the time integration method,
Figure 2010085196
 To obtain an approximate solution of angular velocity. This is represented by Equation 25. In the explicit method,
Figure 2010085196
 It becomes. This is represented by Equation 26. Also, with the trapezoidal rule,
Figure 2010085196
 It becomes. This is represented by Equation 27. In the implicit method,
Figure 2010085196
 It becomes. This is represented by Equation 28. Also,
Figure 2010085196
 There is a relationship. This is represented by Equation 29.

数式２１および数式２９により、

Figure 2010085196
となる。これを数式３０とする。数式３０において、
Figure 2010085196
 である。これを数式３１とする。また、数式３０において、
Figure 2010085196
 である。これを数式３２とする。 From Equation 21 and Equation 29,
Figure 2010085196
 It becomes. This is represented by Equation 30. In Equation 30,
Figure 2010085196
 It is. This is represented by Equation 31. Also, in Equation 30,
Figure 2010085196
 It is. This is expressed as Equation 32.

ここで、

Figure 2010085196
である。これを数式３３とする。したがって、
Figure 2010085196
 として角加速度修正量を算出することができる。これを数式３４とする。数式２５の近似解を更新する場合は、
Figure 2010085196
 を用いて更新する。これを数式３５とする。 here,
Figure 2010085196
 It is. This is expressed as Equation 33. Therefore,
Figure 2010085196
 The angular acceleration correction amount can be calculated as follows. This is represented by Equation 34. When updating the approximate solution of Equation 25,
Figure 2010085196
 Update using. This is expressed as Equation 35.

なお、数式３４において、反復行列Ａおよび残差ベクトルγ、定数行列Ｊは、それぞれ、

Figure 2010085196
となる。これを数式３６とする。こうして、取得した加速度センサ１８の検出値を利用して反復行列Ａおよび残差ベクトルγを求めることができる。このため、数式３４を利用して、
Figure 2010085196
 を満たすか否かを判定することが可能となる。これを数式３７とする。 In Equation 34, the iteration matrix A, the residual vector γ, and the constant matrix J are respectively
Figure 2010085196
 It becomes. This is represented by Equation 36. In this way, the iteration matrix A and the residual vector γ can be obtained using the acquired detection value of the acceleration sensor 18. For this reason, using Equation 34,
Figure 2010085196
 It is possible to determine whether or not the above is satisfied. This is represented by Equation 37.

図３は、本実施形態に係る物体挙動推定装置１０による挙動推定手順を示すフローチャートである。本フローチャートにおける処理は、ユーザにより挙動推定を開始させるためのスタートボタンがディスプレイ４０の画面上で計測作業者にクリックされるなどによって開始する。   FIG. 3 is a flowchart showing a behavior estimation procedure by the object behavior estimation apparatus 10 according to the present embodiment. The process in this flowchart starts when the measurement operator clicks a start button for starting behavior estimation by the user on the screen of the display 40.

なお、計測作業者は、挙動推定開始のためのスタートボタンを押す前に、加速度センサ１８の各々の設置位置ｐｉを物体挙動推定装置１０に入力する。また、計測作業者は、時間積分パラメータであるβも物体挙動推定装置１０に入力する。なお、角速度の初期値はω０に設定されており、角加速度の初期値はゼロに設定されている。 The measurement operator inputs each installation position p i of the acceleration sensor 18 to the object behavior estimation apparatus 10 before pressing the start button for behavior estimation start. Further, the measurement operator also inputs β, which is a time integration parameter, into the object behavior estimation apparatus 10. The initial value of the angular velocity is set to omega 0, the initial value of the angular acceleration is set to zero.

挙動推定部３２は、時間ｔにおいて加速度センサ１８から取得した検出値ａｉを利用して、数式４に示す平均加速度ａｍ、および数式８に示す相対加速度αｉを算出する。挙動推定部３２は、こうして算出した値を用い、時間積分法により数式２５を利用して角速度近似を行う。挙動推定部３２は、こうして求めた角速度を利用して反復行列Ａを算出し（Ｓ１０）、また、残差ベクトルγを算出する（Ｓ１２）。 The behavior estimation unit 32 calculates the average acceleration a m shown in Formula 4 and the relative acceleration α i shown in Formula 8 using the detection value a i acquired from the acceleration sensor 18 at time t. The behavior estimation unit 32 uses the calculated value and approximates the angular velocity by using the numerical formula 25 by the time integration method. The behavior estimation unit 32 calculates the iteration matrix A using the angular velocity thus obtained (S10), and calculates the residual vector γ (S12).

残差ベクトルγが算出されると、挙動推定部３２は、数式３７に示すように、角加速度修正量が誤差より以下か否かを判定する（Ｓ１４）。角加速度修正量が誤差より大きい場合（Ｓ１４のＮ）、挙動推定部３２は、数式３５を用いて近似解を更新し（Ｓ１６）、Ｓ１０に戻って再び反復行列Ａおよび残差ベクトルγを算出する。   When the residual vector γ is calculated, the behavior estimation unit 32 determines whether or not the angular acceleration correction amount is less than the error as shown in Expression 37 (S14). When the angular acceleration correction amount is larger than the error (N in S14), the behavior estimation unit 32 updates the approximate solution using Equation 35 (S16), returns to S10, and calculates the iteration matrix A and the residual vector γ again. To do.

角加速度修正量が誤差より以下と判定された場合（Ｓ１４のＹ）、挙動推定部３２は、その時間における角速度および角加速度を決定する（Ｓ１８）。このとき、表示制御部３４は、決定した角速度および角加速度を示す図や数値をディスプレイ４０に表示してもよい。その時間における角速度および角加速度が決定されると、挙動推定部３２は、時間ｔが計測完了時間Ｔに達したか否かを判定する（Ｓ２０）。時間ｔが計測完了時間Ｔに達していない場合（Ｓ２０のＮ）、挙動推定部３２は、時間ｔをｔ＋Δｔに更新して（Ｓ２２）、Ｓ１０に再び移行する。時間ｔが計測完了時間Ｔに達した場合（Ｓ２０のＹ）、本フローチャートにおける挙動推定処理を終了する。   When it is determined that the angular acceleration correction amount is less than the error (Y in S14), the behavior estimating unit 32 determines the angular velocity and the angular acceleration at that time (S18). At this time, the display control unit 34 may display on the display 40 a figure or numerical value indicating the determined angular velocity and angular acceleration. When the angular velocity and angular acceleration at that time are determined, the behavior estimating unit 32 determines whether or not the time t has reached the measurement completion time T (S20). When the time t has not reached the measurement completion time T (N in S20), the behavior estimation unit 32 updates the time t to t + Δt (S22), and proceeds to S10 again. When the time t reaches the measurement completion time T (Y in S20), the behavior estimation process in this flowchart is terminated.

図４は、挙動推定に用いる加速度センサ１８の取付位置とその平均位置ｐｍの一例を示す図である。図４に示す例では、第２加速度センサ１８Ｂ〜第５加速度センサ１８Ｅの４つによる計測値を用いて、車両１２の略中央のｐｍにおける角速度ベクトルおよび角加速度ベクトルを推定する。 Figure 4 is a diagram showing an example of the mounting position and the average position p m of the acceleration sensor 18 used in the behavior estimation. In the example shown in FIG. 4, using a measurement value measured by the four second acceleration sensor 18B~ fifth acceleration sensor 18E, it estimates the angular velocity vector and the angular acceleration vector in p m of substantially the center of the vehicle 12.

図５（ａ）は、第２加速度センサ１８Ｂの検出値を示す図であり、図５（ｂ）は、第３加速度センサ１８Ｃの検出値を示す図である。また、図５（ｃ）は、第４加速度センサ１８Ｄの検出値を示す図であり、図５（ｄ）は、第５加速度センサ１８Ｅの検出値を示す図である。以下、車両前後方向に伸びる軸をｘ軸、車両左右方向に伸びる軸をｙ軸、車両上下方向に伸びる軸をｚ軸として説明する。図５（ａ）〜図５（ｄ）において、Ａｘはｘ軸方向の並進加速度を示し、Ａｙはｙ軸方向の並進加速度示し、Ａｚはｚ軸方向の並進加速度を示す。   FIG. 5A is a diagram showing the detection value of the second acceleration sensor 18B, and FIG. 5B is a diagram showing the detection value of the third acceleration sensor 18C. FIG. 5C is a diagram showing the detection value of the fourth acceleration sensor 18D, and FIG. 5D is a diagram showing the detection value of the fifth acceleration sensor 18E. Hereinafter, an axis extending in the vehicle front-rear direction will be described as an x axis, an axis extending in the vehicle left-right direction as a y axis, and an axis extending in the vehicle vertical direction as a z axis. 5A to 5D, Ax represents the translational acceleration in the x-axis direction, Ay represents the translational acceleration in the y-axis direction, and Az represents the translational acceleration in the z-axis direction.

図６は、図５（ａ）〜図５（ｄ）に示す検出を利用して推定した平均位置ｐｍにおける角加速度を示す図である。図６において、ｄωｘはｘ軸回りの角加速度を示し、ｄωｙはｙ軸回りの角加速度を示し、ｄωｚはｚ軸回りの角加速度を示す。このように、本実施形態に係る挙動推定方法によれば、並進加速度の検出値を利用して、車両１２における角加速度ベクトルおよび角加速度ベクトルを精度良く算出することができる。 Figure 6 is a diagram showing the angular acceleration in the mean position p m estimated based on the detection shown in FIG. 5 (a) ~ FIG 5 (d). In FIG. 6, dωx represents angular acceleration about the x axis, dωy represents angular acceleration about the y axis, and dωz represents angular acceleration about the z axis. As described above, according to the behavior estimation method according to the present embodiment, the angular acceleration vector and the angular acceleration vector in the vehicle 12 can be accurately calculated by using the detected value of translational acceleration.

なお、本実施形態に係る挙動推定方法では、例えば第１加速度センサ１８Ａ〜第３加速度センサ１８Ｃの検出値を利用することにより、車両１２の前方側を中心とした角速度ベクトルおよび角加速度ベクトルを算出することができる。また、第４加速度センサ１８Ｄ〜第７加速度センサ１８Ｇの検出値を利用することにより、車両１２の後方側を中心とした角速度ベクトルおよび角加速度ベクトルを算出することができる。このため、例えば車両前方と後方との間で車両１２の筐体に変形が生じた場合においても、その影響を含む車両１２の挙動を精度良く推定することが可能となる。   In the behavior estimation method according to the present embodiment, for example, the angular velocity vector and the angular acceleration vector centered on the front side of the vehicle 12 are calculated by using detection values of the first acceleration sensor 18A to the third acceleration sensor 18C. can do. Further, by using the detection values of the fourth acceleration sensor 18D to the seventh acceleration sensor 18G, the angular velocity vector and the angular acceleration vector centered on the rear side of the vehicle 12 can be calculated. For this reason, for example, even when the housing of the vehicle 12 is deformed between the front and rear of the vehicle, the behavior of the vehicle 12 including the influence can be accurately estimated.

本発明は上述の実施形態に限定されるものではなく、本実施形態の各要素を適宜組み合わせたものも、本発明の実施形態として有効である。また、当業者の知識に基づいて各種の設計変更等の変形を本実施形態に対して加えることも可能であり、そのような変形が加えられた実施形態も本発明の範囲に含まれうる。   The present invention is not limited to the above-described embodiment, and an appropriate combination of the elements of this embodiment is also effective as an embodiment of the present invention. Various modifications such as various design changes can be added to the present embodiment based on the knowledge of those skilled in the art, and the embodiments with such modifications can be included in the scope of the present invention.

本実施形態に係る物体挙動推定装置と、物体挙動推定装置による挙動推定の対象となる車両とを模式的に示す図である。It is a figure which shows typically the object behavior estimation apparatus which concerns on this embodiment, and the vehicle used as the object of the behavior estimation by an object behavior estimation apparatus. 本実施形態に係る物体挙動推定装置の構成を示す図である。It is a figure which shows the structure of the object behavior estimation apparatus which concerns on this embodiment. 本実施形態に係る物体挙動推定装置による挙動推定手順を示すフローチャートである。It is a flowchart which shows the behavior estimation procedure by the object behavior estimation apparatus which concerns on this embodiment. 挙動推定に用いる加速度センサの取付位置とその平均位置ｐｍの一例を示す図である。It is a diagram illustrating an example of a mounting position of the acceleration sensor used in the behavior estimation and its average position p m. （ａ）は、第２加速度センサの検出値を示す図であり、（ｂ）は、第３加速度センサの検出値を示す図であり、（ｃ）は、第４加速度センサの検出値を示す図であり、（ｄ）は、第５加速度センサの検出値を示す図である。(A) is a figure which shows the detection value of a 2nd acceleration sensor, (b) is a figure which shows the detection value of a 3rd acceleration sensor, (c) shows the detection value of a 4th acceleration sensor. It is a figure and (d) is a figure which shows the detected value of a 5th acceleration sensor. 図５（ａ）〜図５（ｄ）に示す検出を利用して推定した平均位置ｐｍにおける角速度を示す図である。Is a diagram showing an angular velocity in the mean position p m estimated based on the detection shown in FIG. 5 (a) ~ FIG 5 (d).

符号の説明Explanation of symbols

１０ 物体挙動推定装置、 １２ 車両、 １４ 車両本体、 １６ 車輪、 １６ＦＬ 左前輪、 １６ＦＲ 右前輪、 １６ＲＬ 左後輪、 １６ＲＲ 右後輪、 １８ 加速度センサ、 １８Ａ 第１加速度センサ、 １８Ｂ 第２加速度センサ、 １８Ｃ 第３加速度センサ、 １８Ｄ 第４加速度センサ、 １８Ｅ 第５加速度センサ、 １８Ｆ 第６加速度センサ、 １８Ｇ 第７加速度センサ、 ３０ 並進加速度取得部、 ３２ 挙動推定部、 ３４ 表示制御部、 ４０ ディスプレイ。   DESCRIPTION OF SYMBOLS 10 Object behavior estimation apparatus, 12 vehicle, 14 vehicle main body, 16 wheel, 16FL left front wheel, 16FR right front wheel, 16RL left rear wheel, 16RR right rear wheel, 18 acceleration sensor, 18A 1st acceleration sensor, 18B 2nd acceleration sensor, 18C 3rd acceleration sensor, 18D 4th acceleration sensor, 18E 5th acceleration sensor, 18F 6th acceleration sensor, 18G 7th acceleration sensor, 30 Translation acceleration acquisition part, 32 Behavior estimation part, 34 Display control part, 40 Display.

Claims (4)

挙動を推定すべき物体において直線上にない３以上の個所に配置された並進３軸の加速度センサの各々の検出値を取得するステップと、
取得した検出値に最小二乗法を用いた演算を施して前記物体の角速度ベクトルおよび角加速度ベクトルを推定するステップと、
を備えることを特徴とする物体挙動推定方法。 Obtaining each detection value of a translational triaxial acceleration sensor arranged at three or more locations that are not on a straight line in the object whose behavior is to be estimated;
Performing an operation using a least-squares method on the obtained detection value to estimate an angular velocity vector and an angular acceleration vector of the object;
An object behavior estimation method comprising: 前記角速度ベクトルおよび角加速度ベクトルを推定するステップは、
Figure 2010085196
 としたときに、
Figure 2010085196
 から、すべての加速度センサの平均位置ｐｍから位置ｐｉにある加速度センサのまでの相対位置ｑｉを用いて
Figure 2010085196
 として、最小二乗法を用いて誤差εｉの二乗和
Figure 2010085196
 を最少にする前記物体の角速度ベクトルおよび角加速度ベクトルを推定することを特徴とする請求項１に記載の物体挙動推定方法。 Estimating the angular velocity vector and the angular acceleration vector,
Figure 2010085196
 And when
Figure 2010085196
 From using the relative positions q i from the average position p m of all the acceleration sensor to the acceleration sensor at position p i
Figure 2010085196
 As the sum of squares of error ε i using the least squares method
Figure 2010085196
 The object behavior estimation method according to claim 1, wherein an angular velocity vector and an angular acceleration vector of the object that minimize the frequency are estimated. 挙動を推定すべき物体において直線上にない３以上の個所に配置された並進３軸の加速度センサの各々の検出値を取得する並進加速度取得部と、
前記物体の角速度ベクトルおよび角加速度ベクトルを推定する挙動推定部と、
を備え、
前記挙動推定部は、取得した検出値に最小二乗法を用いた演算を施して前記物体の角速度ベクトルおよび角加速度ベクトルを推定することを特徴とする物体挙動推定装置。 A translational acceleration acquisition unit that acquires detection values of the translational triaxial acceleration sensors arranged at three or more locations that are not on a straight line in the object whose behavior is to be estimated;
A behavior estimation unit for estimating an angular velocity vector and an angular acceleration vector of the object;
With
The object behavior estimation apparatus, wherein the behavior estimation unit estimates an angular velocity vector and an angular acceleration vector of the object by performing an operation using a least square method on the acquired detection value. 前記挙動推定部は、
Figure 2010085196
 としたときに、
Figure 2010085196
 から、すべての加速度センサの平均位置ｐｍから位置ｐｉにある加速度センサのまでの相対位置ｑｉを用いて
Figure 2010085196
 として、最小二乗法を用いて誤差εｉの二乗和
Figure 2010085196
 を最少にする前記物体の角速度ベクトルおよび角加速度ベクトルを推定することを特徴とする請求項３に記載の物体挙動推定装置。 The behavior estimation unit
Figure 2010085196
 And when
Figure 2010085196
 From using the relative positions q i from the average position p m of all the acceleration sensor to the acceleration sensor at position p i
Figure 2010085196
 As the sum of squares of error ε i using the least squares method
Figure 2010085196
 The object behavior estimation apparatus according to claim 3, wherein an angular velocity vector and an angular acceleration vector of the object that minimize the frequency are estimated.
JP2008253513A 2008-09-30 2008-09-30 Object behavior estimation method and object behavior estimating device Pending JP2010085196A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2008253513A JP2010085196A (en) 2008-09-30 2008-09-30 Object behavior estimation method and object behavior estimating device

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2008253513A JP2010085196A (en) 2008-09-30 2008-09-30 Object behavior estimation method and object behavior estimating device

Publications (1)

Publication Number Publication Date
JP2010085196A true JP2010085196A (en) 2010-04-15

Family

ID=42249302

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2008253513A Pending JP2010085196A (en) 2008-09-30 2008-09-30 Object behavior estimation method and object behavior estimating device

Country Status (1)

Country Link
JP (1) JP2010085196A (en)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2017009347A (en) * 2015-06-18 2017-01-12 株式会社豊田中央研究所 Steering state estimation device and program
CN112304312A (en) * 2020-09-17 2021-02-02 合肥赛为智能有限公司 Unmanned aerial vehicle attitude calculation method and system based on least square method and EKF
WO2022037370A1 (en) * 2020-08-18 2022-02-24 华为技术有限公司 Motion estimation method and apparatus

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2017009347A (en) * 2015-06-18 2017-01-12 株式会社豊田中央研究所 Steering state estimation device and program
WO2022037370A1 (en) * 2020-08-18 2022-02-24 华为技术有限公司 Motion estimation method and apparatus
CN112304312A (en) * 2020-09-17 2021-02-02 合肥赛为智能有限公司 Unmanned aerial vehicle attitude calculation method and system based on least square method and EKF
CN112304312B (en) * 2020-09-17 2022-09-13 合肥赛为智能有限公司 Unmanned aerial vehicle attitude calculation method and system based on least square method and EKF

Similar Documents

Publication Publication Date Title
JP6576578B2 (en) Method, system and non-transitory computer readable memory for controlling a vehicle
EP2841874B1 (en) Estimation of the relative attitude and position between a vehicle body and an implement operably coupled to the vehicle body
JP6922641B2 (en) Angular velocity derivation device and angular velocity derivation method
JP5736106B2 (en) Moving state detection device
Du et al. Online robot calibration based on hybrid sensors using Kalman Filters
JP5618066B2 (en) Force control robot calibration apparatus and method
KR20090008924A (en) Method for measuring pose of a mobile robot and method and apparatus for measuring position of the mobile robot using the method
KR20090009172A (en) Method and apparatus for measuring position of the mobile robot
US9802619B2 (en) Moving amount estimating apparatus, autonomous mobile body, and moving amount estimating method
JP5570902B2 (en) Vehicle pitch angle estimation device
JP6795540B2 (en) Devices, methods and programs for estimating load weight and center of gravity position using a robot
CN106990836B (en) Method for measuring spatial position and attitude of head-mounted human input device
JP5419268B2 (en) 3D attitude estimation device, 3D attitude estimation method, and 3D attitude estimation program
JP2010085196A (en) Object behavior estimation method and object behavior estimating device
CN104819717B (en) A kind of multi-rotor aerocraft attitude detecting method based on MEMS inertial sensor group
JP2010143238A (en) Control device for underwater moving body
JP5849477B2 (en) Robot, robot control apparatus, robot control method, and program
EP2249123B1 (en) Surface texture measuring device, surface texture measuring method, and program
KR20180085916A (en) Method for estimating position of wearable devices and apparatus using the same
JP7406390B2 (en) Calibration device and its control method
JP2006329758A (en) Magnetic search system
CN109674480B (en) Human motion attitude calculation method based on improved complementary filtering
JP2017188743A (en) Camera calibration device and camera calibration method
JP2015215179A (en) System and method for magnetic detection
JP7186571B2 (en) Apparatus and method for determining operation completion time of robot