JP3868416B2 - 角速度補正装置 - Google Patents
角速度補正装置 Download PDFInfo
- Publication number
- JP3868416B2 JP3868416B2 JP2003376993A JP2003376993A JP3868416B2 JP 3868416 B2 JP3868416 B2 JP 3868416B2 JP 2003376993 A JP2003376993 A JP 2003376993A JP 2003376993 A JP2003376993 A JP 2003376993A JP 3868416 B2 JP3868416 B2 JP 3868416B2
- Authority
- JP
- Japan
- Prior art keywords
- acceleration
- angular velocity
- vehicle
- angle
- calculated
- 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.)
- Expired - Fee Related
Links
Images
Landscapes
- Navigation (AREA)
- Traffic Control Systems (AREA)
- Instructional Devices (AREA)
- Gyroscopes (AREA)
Description
図1は、本発明の第一の実施形態に係る角速度補正装置の全体構成を示すブロック図である。図1において、角速度補正装置は、典型的には車両用ナビゲーションシステムに搭載され、ジャイロセンサ1と、加速度センサ2と、測距部3と、傾斜角算出部4と、角速度補正部5とを備える。
ここで、f1(v,r)は、遠心加速度成分を表す項であり、ジャイロセンサ1の出力値r、及び測距部3で算出された移動距離Dから導出可能な車両の移動速度vの関数である。このようなf1(v,r)の一例としては、r・f2(v)がある。ここで、f2(v)は、移動速度vの関数である。上式(1)を変形することにより、θ1は、次式(2)で表される。
また、Ax及びvは、移動距離Dの微分を用いて、次式(3)及び(4)に従って算出される。
Ax=dD2/dt2 …(3)
v=(dD/dt) …(4)
上式(5)を変形することにより、r’は、次式(6)で表される。
r’=r/cosθ1 …(6)
図3は、本発明の第二の実施形態に係る角速度補正装置の全体構成を示すブロック図である。図3において、角速度補正装置は、図1に示すものと比較すると、加速度センサ2、傾斜角算出部4及び角速度補正部5の代わりに、加速度センサ部6、傾斜角算出部7及び角速度補正部8を備える点で相違する。それ以外に両角速度補正装置の間に相違点は無いので、図3において、図1に示す構成に相当するものには、同一の参照符号を付け、それぞれの説明を省略する。
なお、周知のように、加速度センサには、互いに直交する2軸方向への加速度を検出可能なものもあるので、このような2軸加速度センサから加速度センサ部6は構成されても良い。
角速度補正部8は、傾斜角算出部7で算出されたピッチ角θ1及びロール角θ2を使って、ジャイロセンサ1から得られる角速度rを、鉛直軸周りの角速度r’に補正する。
ここで、f1(v,r)は、前述と同様の遠心加速度成分を表す項であり、出力値r及び移動速度vの関数である。ここで、本実施形態では、f1(v,r)は0でも良い。
gy≒f3(v,r)−Gcosθ1cosθ2 …(8)
gz≒Gcosθ1cosθ2…(9)
ここで、f3(v,r)は、遠心加速度成分を表す項であり、出力値r及び移動速度vの関数である。このようなf3(v,r)の一例としては、r・f4(v)がある。ここで、f4(v)は、移動速度vの関数である。
Ax=dD2/dt2 …(10)
v=(dD/dt) …(11)
θ1≒sin-1[{gx−Ax+f1(v,r)}/G] …(12)
また、上式(8)及び上式(12)から、θ2は次式(13)で表される。
θ2≒sin-1[{f1(v,r)−gy}/Gcosθ1] …(13)
傾斜角算出部7は、算出した移動速度v及びピッチ角θ1と、第二の加速度センサ10からの加速度gyと、ジャイロセンサ1からの角速度rとを、上式(13)に代入して、ロール角θ2を算出する。
θ2≒cos-1(gz/Gcosθ1) …(14)
傾斜角算出部7は、算出したピッチ角θ1と、第二の加速度センサ10からの加速度gzとを、上式(14)に代入して、ロール角θ2を算出する。ここで、上式(14)では、ロール角θ2の符号は失われてしまうが、一般的に、車両の旋回時におけるロール角θ2の符号変化は、ジャイロセンサ1が出力するヨー方向周りの角速度rの符号変化に一致するので、傾斜角算出部7は、ロール角θ2の符号を、ジャイロセンサ1からの角速度rの符号から決定すればよい。
gy≒f3(v,r)+gztanθ2 …(15)
上式(15)を変形することにより、θ2は、次式(16)で表される。
θ2≒tan-1[{gy−f3(v,r)}/gz] …(16)
θ1≒cos-1(gz/Gcosθ2) …(17)
r’=r/(cosθ1・cosθ2) …(18)
図6は、本発明の第三の実施形態に係る角速度補正装置の全体構成を示すブロック図である。図6において、角速度補正装置は、図3に示すものと比較すると、傾斜角算出部7の代わりに、ピッチ角導出部11、パラメータ算出部12及び傾斜角算出部13を備える点で相違する。それ以外に両角速度補正装置の間に相違点は無いので、図6において、図3に示す構成に相当するものには、同一の参照符号を付け、それぞれの説明を省略する。なお、本実施形態では、第一の加速度センサ9及び第二の加速度センサ10のいずれかは、車両の進行方向への加速度gxを検出する。
…(19)
ピッチ角導出部11は、上式(19)に、GPS_h(t)、GPS_h(t+1)及びGPS_Dを代入して、sin(GPS_θ)を導出する。
gx≒Ax+Gsin(GPS_θ)+K・f1(v,r) …(20)
上式(20)を変形することにより、Kは、次式(21)で表される。
K={Ax+Gsin(GPS_θ)−gx}/f1(v,r) …(21)
ここで、Ax及びvは、前式(3)及び(4)に従って算出される。
θ1≒sin-1((gx−Ax+K・f1(v,r))/G) …(22)
θ2≒sin-1((K・f6(v,r)−gy)/Gcosθ1) …(23)
ここで、f6(v,r)は、遠心加速度成分を表す項であり、ジャイロセンサ1の出力値r、及び測距部3で算出された移動距離Dから導出可能な車両の移動速度vの関数である。但し、f6(v,r)は、f1(v,r)とは異なる。
また、第一の加速度センサ9が第一の加速度g1として加速度gyを検出し、さらに第二の加速度センサ10が第二の加速度g2として加速度gzを検出する場合においては、傾斜角算出部13は、前式(16)におけるf3(v,r)の代わりに、K・f3(v,r)を代入することにより、ロール角θ2を算出する。
さらに、第三の実施形態は、第二の実施形態の変型例として記載されたが、これに限らず、第一の実施形態に、第三の実施形態の特徴である調整項Kを応用しても構わない。
図7は、本発明の第四の実施形態に係る角速度補正装置の全体構成を示すブロック図である。図7において、角速度補正装置は、図3に示すものと比較すると、傾斜角算出部7及び角速度補正部8の代わりに、取り付け角算出部14、傾斜角算出部15及び角速度補正部16を備える点で相違する。それ以外に両角速度補正装置の間に相違点は無いので、図7において、図3に示す構成に相当するものには、同一の参照符号を付け、それぞれの説明を省略する。なお、本実施形態では、第一の加速度センサ9は、車両の進行方向への加速度gxを検出し、第二の加速度センサ10は、車両の左右方向への加速度gyを検出すると仮定する。
角速度補正部16は、前述の角速度補正部9と比較すると、取り付け角算出部14で算出された取り付け角αをさらに用いて、補正値r’を算出する点で相違する。
上式(24)から、αは次式(25)で表される。
α=sin-1(g1/G)…(25)
g1=gx≒Axcosα+Gsin(θ1+α)+f1(v,r) …(26)
ここで、Ax及びf1(v,r)は、第一の実施形態で説明した通りである。
上式(26)を変形することにより、θ1は次式(27)で表される。
θ1≒sin-1{(gx−Axcosα+f1(v,r))/G}−α …(27)
ここで、f5(v,r,α)は、遠心加速度成分を表す項であり、出力値r、移動速度v及び取り付け角αの関数である。
上式(27)及び(28)より、θ2は次式(29)で表される。
θ2≒sin-1[{f5(v,r,α)−gy}/Gcosθ1] …(29)
r’=r・cosΘ …(30)
cosΘ=cos(α+θ1)−cosαcosθ1(1−cosθ2) …(31)
また、第四の実施形態は、第二の実施形態の変型例として記載されたが、これに限らず、第一の実施形態に、第四の実施形態の特徴である、取り付け角αに基づくピッチ角θ1、ロール角θ2及び補正値r’の算出処理を応用しても構わない。
2 加速度センサ
3 測距部
4,7,13,15 傾斜角算出部
5,8,16 角速度補正部
6 加速度センサ部
9 第一の加速度センサ
10 第二の加速度センサ
11 ピッチ角導出部
12 パラメータ算出部
14 取り付け角算出部
r ジャイロセンサ1の出力値
r’ 角速度補正部5,8又は16の出力値
gx 加速度センサ2又は加速度センサ部6により検出される車両の進行方向への加速度
D 測距部3で算出される車両の移動距離
v 移動距離Dを1階微分して得られる車両の移動速度
Ax 移動距離Dを2階微分して得られる車両の加速度
G 重力加速度
θ1 車両のピッチ角
gy 加速度センサ部6で検出される車両の左右方向への加速度
gz 加速度センサ部6で検出される車両の鉛直方向への加速度
θ2 車両のロール角
K 調整項
α ジャイロセンサ1の取り付け角
Claims (13)
- 角速度補正装置であって、
車両のヨー方向への角速度を検出するジャイロセンサと、
車両の所定方向への加速度を、第一の加速度として検出する加速度センサ部と、
車両の移動距離を算出する測距部と、
前記ジャイロセンサで検出された角速度と、前記加速度センサで検出された第一の加速度と、前記測距部で算出された移動距離とに基づいて、車両のピッチ方向への傾斜角を、ピッチ角として算出する傾斜角算出部と、
前記傾斜角算出部で算出されたピッチ角を用いて、前記ジャイロセンサで検出された角速度を補正する角速度補正部とを備える、角速度補正装置。 - 前記第一の加速度は、車両の進行方向への加速度である、請求項1に記載の角速度補正装置。
- 前記加速度センサ部は、車両について、前記第一の加速度とは異なる向きへの加速度を、第二の加速度として検出し、
前記傾斜角算出部は、前記加速度センサ部で検出された第二の加速度をさらに用いて、前記車両のロール方向への傾斜角をロール角として算出し、
前記角速度補正部は、前記傾斜角算出部で算出されたロール角をさらに用いて、前記ジャイロセンサで検出された角速度を補正し、
前記第一及び前記第二の加速度は、車両の進行方向、左右方向及び鉛直方向のいずれかである、請求項1に記載の角速度補正装置。 - 前記第一の加速度は車両の進行方向への加速度であり、前記第二の加速度は車両の左右方向への加速度であり、
前記傾斜角算出部は、
前記測距部で算出された移動距離から、車両の移動速度と、車両の進行方向への第三の加速度とを求め、さらに、求めた移動速度及び第三の加速度と、前記ジャイロセンサで検出された角速度と、前記加速度センサ部で検出された第一の加速度とから、前記ピッチ角を算出し、
算出したピッチ角及び移動速度と、前記ジャイロセンサで検出された角速度と、前記加速度センサ部で検出された第二の加速度とから、前記ロール角を算出する、請求項3に記載の角速度補正装置。 - 前記第一の加速度は車両の進行方向への加速度であり、前記第二の加速度は車両の鉛直方向への加速度であり、
前記傾斜角算出部は、
前記測距部で算出された移動距離から、車両の移動速度及び第三の加速度を求め、さらに、求めた移動速度及び第三の加速度と、前記ジャイロセンサで検出された角速度と、前記加速度センサ部で検出された第一の加速度とから、前記ピッチ角を算出し、
算出したピッチ角と、前記加速度センサ部で検出された第二の加速度とから、前記ロール角を算出する、請求項3に記載の角速度補正装置。 - 前記第一の加速度は車両の左右方向への加速度であり、前記第二の加速度は車両の鉛直方向への加速度であり、
前記傾斜角算出部は、
前記測距部で算出された移動距離から車両の移動速度を求め、さらに、求めた移動速度と、前記ジャイロセンサで検出された角速度と、前記加速度センサ部で検出された第一及び第二の加速度とから、前記ロール角を算出し、
算出したロール角と、前記加速度センサ部で検出された第二の加速度とから、前記ピッチ角を算出する、請求項3に記載の角速度補正装置。 - 前記傾斜角算出部は、所定の調整項をさらに用いて、前記ピッチ角を算出する、請求項1に記載の角速度補正装置。
- 前記加速度センサ部は、前記車両について、前記第一の加速度とは異なる向きへの加速度を、第二の加速度として検出し、
前記傾斜角算出部は、前記加速度センサ部で検出された第二の加速度及び前記調整項をさらに用いて、前記ロール角をさらに算出する、請求項7に記載の角速度補正装置。 - 前記傾斜角算出部とは異なる方法で、車両のピッチ方向への傾斜角を、ピッチ角として導出するピッチ角導出部と、
前記ピッチ角導出部で導出されたピッチ角と、前記ジャイロセンサで検出された角速度と、前記加速度センサで検出された第一の加速度と、前記測距部で算出された移動距離とに基づいて、前記調整項を算出するパラメータ算出部とをさらに備える、請求項7に記載の角速度補正装置。 - 前記調整項は、前記傾斜角算出部で算出されるピッチ角の誤差を補正可能な値である、請求項7に記載の角速度補正装置。
- 前記加速度センサ部で検出された第一の加速度に基づいて、前記ジャイロセンサの取り付け角度を算出する取り付け角算出部をさらに備え、
前記傾斜角算出部は、前記取り付け角算出部で算出された取り付け角度をさらに用いて、前記ピッチ角を算出し、
前記角速度補正部は、前記取り付け角算出部で算出された取り付け角をさらに用いて、前記ジャイロセンサで検出された角速度を補正する、請求項2に記載の角速度補正装置。 - 前記角速度補正部は、車両の種類毎で予め定められる、前記ジャイロセンサの取り付け角度をさらに用いて、前記ジャイロセンサで検出された角速度を補正する、請求項2に記載の角速度補正装置。
- 角速度算出方法であって、
車両のヨー方向への角速度を検出する角速度検出ステップと、
車両の進行方向への加速度を検出する加速度検出ステップと、
車両の移動距離を算出する測距ステップと、
前記角速度検出ステップで検出された角速度と、前記加速度検出ステップで検出された加速度と、前記測距ステップで算出された移動距離とに基づいて、車両のピッチ方向への傾斜角を、ピッチ角として算出する傾斜角算出ステップと、
前記傾斜角算出ステップで算出されたピッチ角を用いて、前記角速度検出ステップで検出された角速度を補正する角速度補正ステップとを備える、角速度算出方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2003376993A JP3868416B2 (ja) | 2003-11-06 | 2003-11-06 | 角速度補正装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2003376993A JP3868416B2 (ja) | 2003-11-06 | 2003-11-06 | 角速度補正装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2005140627A JP2005140627A (ja) | 2005-06-02 |
JP3868416B2 true JP3868416B2 (ja) | 2007-01-17 |
Family
ID=34687874
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2003376993A Expired - Fee Related JP3868416B2 (ja) | 2003-11-06 | 2003-11-06 | 角速度補正装置 |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP3868416B2 (ja) |
Families Citing this family (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2007003206A (ja) * | 2005-06-21 | 2007-01-11 | Alpine Electronics Inc | 車載用ナビゲーション装置 |
JP4655901B2 (ja) * | 2005-11-21 | 2011-03-23 | パナソニック株式会社 | 移動体の水平走行判定装置及び方法 |
JP4517298B2 (ja) * | 2005-11-30 | 2010-08-04 | アイシン・エィ・ダブリュ株式会社 | 方位センサの補正係数演算装置及び演算プログラム |
WO2008087778A1 (ja) * | 2007-01-18 | 2008-07-24 | Mitsubishi Electric Corporation | カーナビゲーション装置 |
TW200925946A (en) * | 2007-07-06 | 2009-06-16 | Sony Corp | Input device, controller, control system, control method, and hand-held device |
JP5354931B2 (ja) * | 2008-02-29 | 2013-11-27 | 古野電気株式会社 | 衛星航法/推測航法統合測位装置 |
JP2011064594A (ja) * | 2009-09-17 | 2011-03-31 | Sony Corp | 角速度補正装置、角速度補正方法、ナビゲーション装置及びナビゲーション機能付携帯電話機 |
JP5352922B2 (ja) * | 2009-10-07 | 2013-11-27 | 多摩川精機株式会社 | 角速度算出装置、ナビゲーション装置 |
JP2012141219A (ja) * | 2010-12-28 | 2012-07-26 | Pioneer Electronic Corp | 傾斜角検出装置、方法、プログラムおよび記録媒体 |
KR102219175B1 (ko) * | 2014-11-10 | 2021-02-23 | 한국단자공업 주식회사 | 차량의 제품 장착각도 측정장치 |
JP7406340B2 (ja) * | 2019-10-18 | 2023-12-27 | 株式会社小松製作所 | 加速度検出装置、作業機械および加速度検出方法 |
-
2003
- 2003-11-06 JP JP2003376993A patent/JP3868416B2/ja not_active Expired - Fee Related
Also Published As
Publication number | Publication date |
---|---|
JP2005140627A (ja) | 2005-06-02 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP4776570B2 (ja) | ナビゲーション装置、傾斜角を用いた角速度補正装置および方法 | |
US7363147B2 (en) | Navigation device and method for determining orientation of vehicle | |
EP2259023A1 (en) | Inertial navigation system error correction | |
US20090063051A1 (en) | Method And Apparatus Of Updating Vehicle Position And Orientation | |
JP2009075005A (ja) | ナビゲーション装置 | |
CN103245360A (zh) | 晃动基座下的舰载机旋转式捷联惯导***自对准方法 | |
JP3868416B2 (ja) | 角速度補正装置 | |
JP2009025012A (ja) | 加速度センサオフセット補正装置および方法 | |
KR102371985B1 (ko) | 관성센서 캘리브레이션 방법 | |
RU2324897C1 (ru) | Азимутальная ориентация платформы трехосного гиростабилизатора по углу прецессии гироблока | |
JP2004125689A (ja) | 自立航法用位置算出システム | |
JP4063149B2 (ja) | 車両角速度検出装置 | |
WO2010030565A1 (en) | Magnetic sensing device for navigation and detecting inclination | |
JP7333258B2 (ja) | 角度測定デバイスの基準方向を判定する方法およびデバイス | |
US20110126647A1 (en) | Rate of turn signal generator with drift compensation | |
JP2001194169A (ja) | 車両姿勢角度検出装置 | |
JP2005227026A (ja) | 角速度補正装置 | |
RU2624617C1 (ru) | Способ автономной азимутальной ориентации платформы трехосного гиростабилизатора по изменяющимся видимым уходам | |
JP3331865B2 (ja) | ナビゲーション装置 | |
US9671248B2 (en) | Method for calibrating an inertial navigation system with a limited mechanical turning range | |
JP2001264091A (ja) | ナビゲーション装置 | |
RU2649063C1 (ru) | Способ определения азимута платформы трёхосного гиростабилизатора по отклонению угла поворота гироскопа от расчётного значения | |
RU2408843C1 (ru) | Аналитический гирокомпас для квазистатических измерений | |
Rohac et al. | Inertial reference unit in a directional gyro mode of operation | |
JP4454442B2 (ja) | 角速度センサの感度誤差検出装置 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20051216 |
|
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: 20061010 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20061010 |
|
R150 | Certificate of patent or registration of utility model |
Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20091020 Year of fee payment: 3 |
|
FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20101020 Year of fee payment: 4 |
|
LAPS | Cancellation because of no payment of annual fees |