JP2019168287A - 姿勢角演算装置、移動装置、姿勢角演算方法、およびプログラム - Google Patents

姿勢角演算装置、移動装置、姿勢角演算方法、およびプログラム Download PDF

Info

Publication number
JP2019168287A
JP2019168287A JP2018054961A JP2018054961A JP2019168287A JP 2019168287 A JP2019168287 A JP 2019168287A JP 2018054961 A JP2018054961 A JP 2018054961A JP 2018054961 A JP2018054961 A JP 2018054961A JP 2019168287 A JP2019168287 A JP 2019168287A
Authority
JP
Japan
Prior art keywords
angular velocity
acceleration
output
attitude angle
vertical axis
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
Application number
JP2018054961A
Other languages
English (en)
Other versions
JP7155562B2 (ja
Inventor
貴裕 中山
Takahiro Nakayama
貴裕 中山
一博 美馬
Kazuhiro Mima
一博 美馬
浩司 尾藤
Koji Bito
浩司 尾藤
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 JP2018054961A priority Critical patent/JP7155562B2/ja
Priority to US16/254,782 priority patent/US11230019B2/en
Priority to EP19154103.6A priority patent/EP3543647A1/en
Priority to CN201910163027.5A priority patent/CN110293557B/zh
Publication of JP2019168287A publication Critical patent/JP2019168287A/ja
Application granted granted Critical
Publication of JP7155562B2 publication Critical patent/JP7155562B2/ja
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C1/00—Measuring angles
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B25J13/08—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices
    • B25J13/088—Controls for manipulators by means of sensing devices, e.g. viewing or touching devices with position, velocity or acceleration sensors
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Manufacturing & Machinery (AREA)
  • Human Computer Interaction (AREA)
  • Gyroscopes (AREA)
  • Manipulator (AREA)
  • Length Measuring Devices With Unspecified Measuring Means (AREA)

Abstract

【課題】加速度センサとジャイロセンサのデータから検出誤差を抑制して姿勢角を演算する姿勢角演算装置等を提供すること。【解決手段】本発明に係る姿勢角演算装置１７０は、移動面に沿って移動する移動装置の移動面に対する垂直軸方向の加速度を出力するように設置された加速度センサ１５１の出力と、垂直軸周りの角速度を出力するように設置されたジャイロセンサ１５２の出力と、をそれぞれ取得する取得部１７１を有する。また、姿勢角演算装置１７０は、加速度が予め設定された基準加速度Ｒｇより大きく、角速度が予め設定された基準角速度Ｒｗより小さい場合に、角速度ωｚをゼロとみなして移動装置の垂直軸周りの姿勢角を演算する演算部１７２を有する。また、姿勢角演算装置１７０は、演算した姿勢角のデータを出力する出力部１７３を有する。【選択図】図２

Description

本発明は姿勢角演算装置、移動装置、姿勢角演算方法、およびプログラムに関する。
移動装置の姿勢角を検出する手法としては、ジャイロセンサで検出された角速度を積分することにより姿勢角を求める方法や、複数の加速度センサで検出された加速度から幾何学的に姿勢角を求める方法が知られている。また、姿勢角を検出する精度を向上させる技術が提案されている。
特許文献１に記載されている姿勢角検出装置は、測定対象部分の回転角速度を検出するジャイロセンサと、測定対象部分における直交二方向の加速度をそれぞれ検出する加速度センサとを備える。また、姿勢角検出装置は、ジャイロセンサおよび加速度センサからの検出値に基づいて、測定対象部分の姿勢角を演算する演算手段を備えている。演算手段は、ジャイロセンサの検出値から、所定周波数以上の高周波成分における対象方向の回転角度を求める角速度処理部と、加速度センサの検出値から、所定周波数未満の低周波成分における対象方向の回転角度を求める加速度処理部とを備える。演算手段は、これら角速度処理部および加速度処理部で求めた各回転角度を加算して姿勢角を求める姿勢角決定部を備えている。
特開２００７−０６４８５４号公報
しかしながら、特許文献１に記載の技術を利用してもなお、ジャイロセンサによる角速度の検出は、ジャイロセンサが有するバイアス出力や温度変化などによって誤差が生じる場合がある。一方、角速度の検出に生じる誤差を排除するために、例えば、検出した角速度をゼロとみなすフィルタリング処理を行うと、姿勢角検出装置の感度が低下し、移動装置の姿勢角を検出することが困難になる場合がある。
本発明は、このような課題を解決するためになされたものであり、加速度センサとジャイロセンサのデータから検出誤差を抑制して姿勢角を演算する姿勢角演算装置等を提供するものである。
本発明に係る姿勢角演算装置は、移動面に沿って移動する移動装置の前記移動面に対する垂直軸方向の加速度を出力するように設置された加速度センサの出力と、前記垂直軸周りの角速度を出力するように設置されたジャイロセンサの出力と、をそれぞれ取得する取得部を有する。また、姿勢角演算装置は、前記加速度が予め設定された基準加速度より大きく、前記角速度が予め設定された基準角速度より小さい場合に、前記角速度をゼロとみなして前記移動装置の前記垂直軸周りの姿勢角を演算する演算部を有する。また、姿勢角演算装置は、演算した前記姿勢角のデータを出力する出力部を有する。
このような構成により、姿勢角演算装置は、移動装置の姿勢が比較的に安定している場合に、角速度の値を調整し、移動装置の姿勢が比較的に安定していない場合に、角速度の値を調整する処理を実行しない。
本発明に係る姿勢角演算方法は、移動面に沿って移動する移動装置の前記移動面に対する垂直軸方向の加速度を出力するように設置された加速度センサの出力と、前記垂直軸周りの角速度を出力するように設置されたジャイロセンサの出力と、をそれぞれ取得するステップを有する。また、姿勢角演算方法は、前記加速度が予め設定された基準加速度より大きく、前記角速度が予め設定された基準角速度より小さい場合に、前記角速度をゼロとみなして前記移動装置の前記垂直軸周りの姿勢角を演算するステップを有する。また、姿勢角演算方法は、演算した前記姿勢角のデータを出力するステップを有する。
このような構成により、姿勢角演算方法は、移動装置の姿勢が比較的に安定している場合に、角速度の値を調整し、移動装置の姿勢が比較的に安定していない場合に、角速度の値を調整する処理を実行しない。
本発明に係るプログラムは、コンピュータに以下の方法を実行させるものであって、前記方法は、移動面に沿って移動する移動装置の前記移動面に対する垂直軸方向の加速度を出力するように設置された加速度センサの出力と、前記垂直軸周りの角速度を出力するように設置されたジャイロセンサの出力と、をそれぞれ取得するステップを有する。また、前記方法は、前記加速度が予め設定された基準加速度より大きく、前記角速度が予め設定された基準角速度より小さい場合に、前記角速度をゼロとみなして前記移動装置の前記垂直軸周りの姿勢角を演算するステップを有する。また、前記方法は、演算した前記姿勢角のデータを出力するステップを有する。
このような構成により、姿勢角演算方法は、移動装置の姿勢が比較的に安定している場合に、角速度の値を調整し、移動装置の姿勢が比較的に安定していない場合に、角速度の値を調整する処理を実行しない。
本発明により、加速度センサとジャイロセンサのデータから検出誤差を抑制して姿勢角を演算する姿勢角演算装置等を提供することができる。
実施の形態に係る移動システムの概略構成を示す構成図である。 実施の形態に係る姿勢角演算装置を含む移動制御装置の構成を示す図である。 実施の形態に係る姿勢角演算装置の処理を示すフローチャートである。
説明の明確化のため、以下の記載および図面は、適宜、省略、および簡略化がなされている。また、様々な処理を行う機能ブロックとして図面に記載される各要素は、ハードウェア的には、ＣＰＵ（Central Processing Unit）、メモリ、その他の回路で構成することができ、ソフトウェア的には、メモリにロードされたプログラムなどによって実現される。したがって、これらの機能ブロックがハードウェアのみ、ソフトウェアのみ、又はそれらの組合せによっていろいろな形で実現できることは当業者には理解されるところであり、いずれかに限定されるものではない。よって、以下の説明に回路として例示した構成は、ハードウェア又はソフトウェアのいずれか又はその両方によって実現することが可能であり、ある機能を実現する回路として示された構成は、同様の機能を実現するソフトウェアの一部としても示され得る。例えば、制御回路と記載された構成は、制御部として記載され得る。なお、各図面において、同一の要素には同一の符号が付されており、必要に応じて重複説明は省略されている。
また、上述したプログラムは、様々なタイプの非一時的なコンピュータ可読媒体を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（Random Access Memory））を含む。また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、および電磁波を含む。一時的なコンピュータ可読媒体は、電線および光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。
＜実施の形態＞
以下、図面を参照しながら本発明の実施の形態に係る姿勢角演算装置等について説明する。なお、図１は、右手系の直交座標系が付されている。図１において、ｘ軸およびｙ軸は床面Ｇと平行である。ｚ軸は、床面Ｇに対して垂直である。
図１は、実施の形態に係る姿勢角演算装置を搭載する移動システムの概略構成を示す構成図である。図１に示す移動システム１は、自律移動式の移動ロボット１０を含むシステムである。移動ロボット１０は、種々のセンサを有し、センサの出力に基づいて駆動部を制御する。移動ロボット１０は、姿勢角を演算する姿勢角演算装置を有している。姿勢角演算装置は、センサの誤差を抑制する機能を有している。移動システム１は、主な構成として、移動ロボット１０および操作装置９０を有している。
操作装置９０は、移動ロボット１０を操作するための機能を有する装置である。操作装置９０は、例えばタブレット端末、スマートフォン、パーソナルコンピュータ等である。操作装置９０は、移動ロボット１０と通信可能に接続しており、移動ロボット１０から種々の情報を受け取る。また、操作装置９０は、使用者に移動ロボット１０の状態を通知するための表示部、使用者が移動ロボット１０の動作を指示するための入力部、および移動ロボット１０との間で情報の送受信をするための通信制御部等を有している。
移動ロボット１０は操作装置９０と通信可能に接続しており、操作装置９０の指示を受けて床面Ｇに沿って移動する。移動ロボット１０は、主な構成として、台車１１および本体１２を有している。台車１１は、下面が床面Ｇに接し、上面が本体１２を支持している。本体１２は、主な構成として、イメージセンサ１３、ロボットアーム１４、モーションセンサ１５、およびロボット制御装置１７を有している。
台車１１は、床面Ｇの上を移動する機能、および台車１１が支持する本体１２を旋回させる機能を有する全方位台車である。全方位台車は、全方向台車、全方位移動台車、又は全方向移動台車と称されてもよい。台車１１は主な構成として、車輪駆動部１１０Ｌ、車輪駆動部１１０Ｒ、車輪１１１Ｌ、車輪１１１Ｒ、旋回駆動部１１２、およびフレーム１１３を有している。
車輪駆動部１１０Ｌおよび車輪駆動部１１０Ｒは、床面Ｇに接する車輪１１１Ｌおよび車輪１１１Ｒをそれぞれ駆動するためのアクチュエータである。車輪１１１Ｌおよび車輪１１１Ｒは、同軸上に離間して配置され、それぞれ独立して駆動可能に設けられている。したがって、車輪１１１Ｌおよび車輪１１１Ｒが同じ方向に同期して回転すると、台車１１は直進する。また、車輪１１１Ｌと車輪１１１Ｒとの回転数又は回転方向に差が生じると、台車１１は差に応じて旋回しながら進んだり、移動せずに旋回のみを行ったりする。車輪駆動部１１０Ｌと車輪駆動部１１０Ｒとは、それぞれフレーム１１３に設けられている。なお、台車１１は、図示しない非駆動輪を有している。非駆動輪は、台車１１を床面Ｇに支持するとともに、車輪１１１Ｌおよび車輪１１１Ｒに従動する。
旋回駆動部１１２は、本体１２を旋回させる機能を有している。旋回駆動部１１２は、フレーム１１３に設けられ、床面Ｇに対して垂直なｚ軸方向に沿った旋回軸を有する。また旋回駆動部１１２は、旋回駆動部１１２を回転させるためのアクチュエータを有している。したがって、旋回駆動部１１２が回転すると、移動ロボット１０の本体１２は、旋回軸周りに姿勢角を変化させる。
イメージセンサ１３は、本体部が有するロボットアーム１４を含む画像を撮像する。イメージセンサ１３が撮像した画像のデータは、移動ロボット１０の周囲の物体を認識するのに利用される。また、イメージセンサ１３が撮像した画像のデータは、ロボットアーム１４の制御に利用される。
ロボットアーム１４は、本体１２に設けられた多関節アームである。ロボットアーム１４は本体１２から延伸し、先端部にはロボットハンド１４０を有している。ロボットアーム１４およびロボットハンド１４０はそれぞれの関節部にアクチュエータを有しており、操作装置９０から受けた指示に従って物体を掴んだり離したりする動作を行い得る。
モーションセンサ１５は、移動ロボット１０の移動や姿勢の変化を検出する機能を有する。モーションセンサ１５は、加速度センサ１５１と、ジャイロセンサ１５２とを有している。加速度センサ１５１は、ｘ軸方向の加速度ａｘ、ｙ軸方向の加速度ａｙ、およびｚ軸方向の加速度ａｚを検出し、検出した加速度の値を出力する。加速度センサ１５１は、例えば、ピエゾ抵抗方式、静電容量検出方式、又は熱検知方式等の加速度センサである。ジャイロセンサ１５２は、ｘ軸周りの角速度ωｘ、ｙ軸周りの角速度ωｙ、およびｚ軸周りの角速度ωｚを検出し、検出した角速度の値を出力する。ジャイロセンサ１５２は、例えば、コリオリ力を利用して角速度を検出する振動式ジャイロセンサである。
ロボット制御装置１７は、各構成から信号を受け取り、受け取った信号を処理し、処理結果に基づいて適宜各構成に指示を送る。ロボット制御装置１７は、複数の半導体等が実装された基板により構成される。例えば、ロボット制御装置１７は、操作装置９０からの指示を受け取り、受け取った指示に基づいて、移動ロボット１０の各構成を動作させる。
図２を参照しながら、ロボット制御装置１７の詳細について説明する。図２は、実施の形態に係るロボット制御装置の構成を示す図である。ロボット制御装置１７は、主な構成として、姿勢角演算装置１７０、台車制御装置１８０、およびメモリ１９０を有している。
姿勢角演算装置１７０は、モーションセンサ１５の出力を受け取り、受け取った出力に基づいて移動ロボット１０の姿勢角を演算し、演算結果を台車制御装置に出力する。姿勢角演算装置１７０は、取得部１７１、演算部１７２、および出力部１７３を有している。
取得部１７１は、モーションセンサ１５からの出力を受け取るインタフェースである。取得部１７１は、加速度センサ１５１の出力と、ジャイロセンサ１５２の出力とをそれぞれ取得する。取得部１７１は、取得した各センサの出力を演算部１７２に供給する。
演算部１７２は、取得部１７１から各センサの出力を受け取り、移動ロボット１０の姿勢角を演算する。演算部１７２はメモリ１９０に接続しており、適宜メモリ１９０が記憶するデータを取得する。具体的には、例えば、演算部１７２は、メモリ１９０が記憶するプログラムを取得して、取得したプログラムに基づいて姿勢角の演算を行う。演算部１７２は、演算した姿勢角のデータを出力部１７３に供給する。
出力部１７３は、演算部１７２が演算した姿勢角のデータを受け取り、台車制御装置１８０に出力するインタフェースである。
台車制御装置１１０は、台車１１が有している車輪駆動部１１０Ｌ、車輪駆動部１１０Ｒ、および旋回駆動部１１２の駆動をそれぞれ制御する機能を有している。すなわち、台車制御装置１１８は、台車１１の姿勢角を制御する旋回制御部１８１を有している。旋回制御部１８１は、車輪駆動部１１０Ｌ、車輪駆動部１１０Ｒ、および旋回駆動部１１２をそれぞれ制御することにより、台車１１の姿勢角を変化させる機能を担っている。
メモリ１９０は、演算部１７２に接続し、適宜記憶しているデータを演算部１７２に供給する。メモリ１９０は、例えばＤＲＡＭ（Dynamic Random Access Memory）、フラッシュメモリ、又はＳＳＤ（Solid State Drive）等の揮発性若しくは不揮発性又はこれらの組合せによって構成される記憶装置である。
次に、図３を参照しながら、姿勢角演算装置の処理について説明する。姿勢角演算装置１７０は、予め設定された条件下において、モーションセンサ１５が有している誤差を抑制する処理を行う。図３は、実施の形態に係る姿勢角演算装置の処理を示すフローチャートである。
まず、姿勢角演算装置１７０は、モーションセンサ１５の出力を取得する（ステップＳ１０）。取得したモーションセンサ１５の出力は、上述のように、演算部１７２に供給される。
次に、姿勢角演算装置１７０は、取得した出力の内、ｚ軸方向の加速度ａｚが予め設定された基準加速度Ｒｇより大きく、ｚ軸周りの角速度ωｚの絶対値が予め設定された基準角速度Ｒｗより小さいか否かを演算する（ステップＳ１１）。すなわち、姿勢角演算装置１７０は、以下の式（１）および式（２）が成立するか否かを判定する。
Figure 2019168287
Figure 2019168287
ここで、姿勢角演算装置１７０が式（１）を演算する理由について説明する。姿勢角演算装置１７０は、式（１）を演算することにより、床面Ｇが傾斜しているか否かを判定する。すなわち、移動面に沿って移動する移動ロボット１０において、移動ロボット１０が水平面に位置する場合に、移動面である床面Ｇに対して垂直なｚ軸方向は、重力方向と一致する。そのため、この場合、ｚ軸方向の加速度ａｚは、約９．８ｍ／ｓ２である。また、移動ロボット１０が位置する床面Ｇが傾斜している場合、ｚ軸と重力方向とが床面Ｇの傾斜分に相当する角度を成す。そのため、この場合、加速度ａｚは、約９．８ｍ／ｓ２より小さい値になる。したがって、基準加速度Ｒｇは、例えば、静止状態の重力加速度の９０パーセントと設定することができる。基準加速度Ｒｇを静止状態の重力加速度の９０パーセントと設定することにより、姿勢角演算装置１７０は、加速度センサ１５１から取得したｚ軸方向の加速度の出力に基づいて、移動ロボット１０が、比較的水平に近い床面Ｇに位置していることを判定する。
次に、姿勢角演算装置１７０が式（２）を演算する理由について説明する。姿勢角演算装置１７０は、式（２）を演算することにより、移動ロボット１０が旋回動作を行っているか否かを判定する。すなわち、移動ロボット１０が旋回動作を行っている場合、ジャイロセンサ１５２から取得したｚ軸周りの角速度ωｚは、旋回動作に応じた出力となっている。しかしながら、ジャイロセンサ１５２から出力は、誤差を含んでいる場合がある。例えば、振動式のＭＥＭＳ（Microelectromechanical systems）技術を利用したジャイロセンサは、角速度が発生していない状態で、わずかな出力を発生させる場合がある。このような出力をそのまま用いて姿勢角の演算を行うと、正しい演算を行うことができない。また、姿勢角演算装置１７０は、ジャイロセンサ１５２から出力された角速度を積分することによって角度を演算する。そのため、ジャイロセンサ１５２からの誤差を伴う出力をそのまま用い続けると、少しずつ誤差が積算されることになる可能性がある。そこで、姿勢角演算装置１７０は、後述の処理を行って上述した誤差を抑制するために、ジャイロセンサ１５２からの出力の大きさ（絶対値）が基準角速度Ｒｗより小さいか否かを判定する。基準角速度Ｒｗは、例えば、０．６ｄｅｇ／ｓ（毎秒０．６度）である。
ステップＳ１１において、式（１）又は式（２）の少なくともいずれかが成立しない場合（ステップＳ１１：Ｎｏ）、姿勢角演算装置１７０は、姿勢角の演算処理を行う（ステップＳ１３）。
一方、ステップＳ１１において、式（１）および式（２）のいずれもが成立する場合（ステップＳ１１：Ｙｅｓ）、姿勢角演算装置１７０は、ｚ軸周りの角速度ωｚをゼロとする処理を行う（ステップＳ１２）。つまり、姿勢角演算装置１７０は、以下の式（３）の処理を行う。
Figure 2019168287
 すなわち、姿勢角演算装置１７０は、ｚ軸周りの角速度ωｚをゼロとみなす、角速度ωｚの値を調整する処理を行う。
次に、姿勢角演算装置１７０は、角速度ωｚをゼロとみなして姿勢角の演算を行う（ステップＳ１４）。次に、姿勢角演算装置１７０は、演算結果を出力する（ステップＳ１４）。
このように、姿勢角演算装置１７０は、加速度ａｚが予め設定された基準加速度Ｒｇより大きく、角速度ωｚが予め設定された基準角速度Ｒｗより小さい場合に、角速度ωｚをゼロとみなして移動ロボット１０の垂直軸周りの姿勢角を演算する。
なお、旋回制御部１８１は、移動ロボット１０の姿勢角を変更する場合に、基準角速度の１０倍以上の角速度により旋回するように旋回駆動部を制御するのが好ましい。つまり、基準角速度Ｒｗは、旋回制御部１８１が旋回駆動部を制御する際の角速度より十分に小さいことが好ましい。このように設定しておくことにより、移動ロボット１０は、上述の処理を行った場合にも、旋回制御部１８１が姿勢角を変化させた場合と、旋回制御部１８１が姿勢角を変化させずにジャイロセンサ１５２の出力に含まれる誤差のみが出力されている場合との区別をつけることができる。
このような構成により、実施の形態に係る姿勢角演算装置は、移動ロボットの姿勢が比較的に安定している場合に、角速度の値を調整し、移動装置の姿勢が比較的に安定していない場合に、角速度の値を調整する処理を実行しない。よって、実施の形態によれば、加速度センサとジャイロセンサのデータから検出誤差を抑制して姿勢角を演算する姿勢角演算装置等を提供することができる。
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で以下のように適宜変更することが可能である。
例えば、実施の形態は、移動ロボットについて説明したが、移動面に沿って移動する移動装置であれば、例えば、自動車、自転車、船舶、又はホバークラフト等であってもよい。
また、実施の形態は、３軸の加速度センサ１５１と、３軸のジャイロセンサについて説明したが、加速度センサは、ｚ軸方向の加速度を出力する加速度センサであってもよい。また、ジャイロセンサは、ｚ軸周りの角速度を出力するジャイロセンサであってもよい。その場合、姿勢角演算装置は、ｚ軸に対して直交する軸方向の加速度と、ｚ軸に対して直交する軸周りの角速度とは、別途取得してもよい。
また、実施の形態は、姿勢角演算装置１７０が移動ロボット１０に含まれる構成としたが、姿勢角演算装置１７０は、移動ロボット１０に含まれず、移動ロボット１０と通信可能に接続している操作装置９０等に含まれてもよい。
１ 移動システム
１０ 移動ロボット
１１ 台車
１２ 本体
１５ モーションセンサ
１７ ロボット制御装置
９０ 操作装置
１１０ 台車制御装置
１１０Ｌ、１１０Ｒ 車輪駆動部
１１１Ｌ、１１１Ｒ 車輪
１１２ 旋回駆動部
１１３ フレーム
１１８ 台車制御装置
１５１ 加速度センサ
１５２ ジャイロセンサ
１７０ 姿勢角演算装置
１７１ 取得部
１７２ 演算部
１７３ 出力部
１８０ 台車制御装置
１８１ 旋回制御部
１９０ メモリ
ａｘ、ａｙ、ａｚ 加速度
Ｇ 床面
Ｒｇ 基準加速度
Ｒｗ 基準角速度
ωｘ、ωｙ、ωｚ 角速度

Claims (7)

  1. 移動面に沿って移動する移動装置の前記移動面に対する垂直軸方向の加速度を出力するように設置された加速度センサの出力と、前記垂直軸周りの角速度を出力するように設置されたジャイロセンサの出力と、をそれぞれ取得する取得部と、
    前記加速度が予め設定された基準加速度より大きく、前記角速度が予め設定された基準角速度より小さい場合に、前記角速度をゼロとみなして前記移動装置の前記垂直軸周りの姿勢角を演算する演算部と、
    演算した前記姿勢角のデータを出力する出力部と、を備える
    姿勢角演算装置。
  2. 前記取得部は、前記垂直軸と前記垂直軸に直交する２軸とからなる３軸方向のそれぞれの加速度と、前記３軸周りのそれぞれの角速度を取得し、
    前記演算部は、前記３軸方向のそれぞれの加速度と、前記３軸周りのそれぞれの角速度と、に基づいて、前記移動装置の前記３軸周りの姿勢角を演算する、
    請求項１に記載の姿勢角演算装置。
  3. 前記基準加速度は、静止状態の重力加速度の９０パーセントである、
    請求項１又は２に記載の姿勢角演算装置。
  4. 前記基準角速度は、０．６ｄｅｇ／ｓである、
    請求項１〜３のいずれか一項に記載の姿勢角演算装置。
  5. 請求項１〜４のいずれか一項に記載の姿勢角演算装置と、
    前記加速度を検出し、検出した前記加速度を前記姿勢角演算装置に出力する加速度センサと、
    前記角速度を検出し、検出した前記角速度を前記姿勢角演算装置に出力するジャイロセンサと、
    前記垂直軸周りに旋回運動を行う旋回駆動部と、前記旋回駆動部を駆動して前記姿勢角を制御する旋回制御部と、を備える
    移動装置。
  6. 移動面に沿って移動する移動装置の前記移動面に対する垂直軸方向の加速度を出力するように設置された加速度センサの出力と、前記垂直軸周りの角速度を出力するように設置されたジャイロセンサの出力と、をそれぞれ取得するステップと、
    前記加速度が予め設定された基準加速度より大きく、前記角速度が予め設定された基準角速度より小さい場合に、前記角速度をゼロとみなして前記移動装置の前記垂直軸周りの姿勢角を演算するステップと、
    演算した前記姿勢角のデータを出力するステップと、を備える
    姿勢角演算方法。
  7. 移動面に沿って移動する移動装置の前記移動面に対する垂直軸方向の加速度を出力するように設置された加速度センサの出力と、前記垂直軸周りの角速度を出力するように設置されたジャイロセンサの出力と、をそれぞれ取得するステップと、
    前記加速度が予め設定された基準加速度より大きく、前記角速度が予め設定された基準角速度より小さい場合に、前記角速度をゼロとみなして前記移動装置の前記垂直軸周りの姿勢角を演算するステップと、
    演算した前記姿勢角のデータを出力するステップと、
    を備える姿勢角演算方法をコンピュータに実行させる
    プログラム。
JP2018054961A 2018-03-22 2018-03-22 姿勢角演算装置、移動装置、姿勢角演算方法、およびプログラム Active JP7155562B2 (ja)

Priority Applications (4)

Application Number Priority Date Filing Date Title
JP2018054961A JP7155562B2 (ja) 2018-03-22 2018-03-22 姿勢角演算装置、移動装置、姿勢角演算方法、およびプログラム
US16/254,782 US11230019B2 (en) 2018-03-22 2019-01-23 Posture angle calculation apparatus, moving apparatus, posture angle calculation method, and program
EP19154103.6A EP3543647A1 (en) 2018-03-22 2019-01-29 Posture angle calculation apparatus, moving apparatus, posture angle calculation method, and program
CN201910163027.5A CN110293557B (zh) 2018-03-22 2019-03-05 姿势角计算装置、移动装置、姿势角计算方法和程序

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2018054961A JP7155562B2 (ja) 2018-03-22 2018-03-22 姿勢角演算装置、移動装置、姿勢角演算方法、およびプログラム

Publications (2)

Publication Number Publication Date
JP2019168287A true JP2019168287A (ja) 2019-10-03
JP7155562B2 JP7155562B2 (ja) 2022-10-19

Family

ID=65243354

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2018054961A Active JP7155562B2 (ja) 2018-03-22 2018-03-22 姿勢角演算装置、移動装置、姿勢角演算方法、およびプログラム

Country Status (4)

Country Link
US (1) US11230019B2 (ja)
EP (1) EP3543647A1 (ja)
JP (1) JP7155562B2 (ja)
CN (1) CN110293557B (ja)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113701696A (zh) * 2020-05-20 2021-11-26 豪夫迈·罗氏有限公司 样品容器弹簧检查设备

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP7310358B2 (ja) * 2019-06-27 2023-07-19 セイコーエプソン株式会社 オーバーシュート量検出方法、オーバーシュート量検出システム、ロボットシステムおよびオーバーシュート量調整方法
JP2022019093A (ja) 2020-07-17 2022-01-27 セイコーエプソン株式会社 オーバーシュート量検出方法およびロボットシステム

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2007064854A (ja) * 2005-08-31 2007-03-15 Univ Waseda 姿勢角検出装置及び姿勢角検出用プログラム
US20090254276A1 (en) * 2008-04-08 2009-10-08 Ensco, Inc. Method and computer-readable storage medium with instructions for processing data in an internal navigation system
JP2015217126A (ja) * 2014-05-16 2015-12-07 住友理工株式会社 角度センサ装置
JP2017110998A (ja) * 2015-12-16 2017-06-22 日立建機株式会社 建設機械
WO2018012064A1 (ja) * 2016-07-15 2018-01-18 日立オートモティブシステムズ株式会社 角度計測装置

Family Cites Families (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB2426827B (en) * 2002-11-15 2007-03-07 Alan George Rock Level, angle and distance measuring device
JP3795498B2 (ja) 2004-03-12 2006-07-12 日本航空電子工業株式会社 姿勢角検出装置
WO2006022201A1 (ja) * 2004-08-25 2006-03-02 Kabushiki Kaisha Yaskawa Denki ロボットの評価システム及び評価方法
JP2007041733A (ja) * 2005-08-01 2007-02-15 Toyota Motor Corp 運動体の姿勢角検出装置
US7957898B2 (en) * 2007-12-28 2011-06-07 Alpine Electronics, Inc Computational scheme for MEMS inertial navigation system
JP2010225139A (ja) * 2009-02-27 2010-10-07 Toshiba Corp 移動機器
JP5614527B2 (ja) * 2010-03-05 2014-10-29 セイコーエプソン株式会社 姿勢情報算出装置、姿勢情報算出システム、姿勢情報算出方法及び姿勢情報算出プログラム
US9170105B1 (en) * 2010-12-23 2015-10-27 Lockheed Martin Corporation System and method for improved accuracy gyro rate sensing
JP2013095306A (ja) 2011-11-02 2013-05-20 Yupiteru Corp 自転車用電子システム及びプログラム
JP2015094631A (ja) * 2013-11-11 2015-05-18 セイコーエプソン株式会社 位置算出装置及び位置算出方法
US9846040B2 (en) 2015-05-08 2017-12-19 Sharp Laboratories Of America, Inc. System and method for determining the orientation of an inertial measurement unit (IMU)
FR3060114B1 (fr) * 2016-12-13 2019-05-17 Commissariat A L'energie Atomique Et Aux Energies Alternatives Procede d'aide a la navigation, produit programme d'ordinateur et centrale de navigation inertielle associes
JP2019082454A (ja) * 2017-10-31 2019-05-30 セイコーエプソン株式会社 ジャイロセンサー装置、姿勢制御システムおよびカメラ装置

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2007064854A (ja) * 2005-08-31 2007-03-15 Univ Waseda 姿勢角検出装置及び姿勢角検出用プログラム
US20090254276A1 (en) * 2008-04-08 2009-10-08 Ensco, Inc. Method and computer-readable storage medium with instructions for processing data in an internal navigation system
JP2015217126A (ja) * 2014-05-16 2015-12-07 住友理工株式会社 角度センサ装置
JP2017110998A (ja) * 2015-12-16 2017-06-22 日立建機株式会社 建設機械
WO2018012064A1 (ja) * 2016-07-15 2018-01-18 日立オートモティブシステムズ株式会社 角度計測装置

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113701696A (zh) * 2020-05-20 2021-11-26 豪夫迈·罗氏有限公司 样品容器弹簧检查设备

Also Published As

Publication number Publication date
US11230019B2 (en) 2022-01-25
CN110293557A (zh) 2019-10-01
US20190291279A1 (en) 2019-09-26
CN110293557B (zh) 2022-04-29
EP3543647A1 (en) 2019-09-25
JP7155562B2 (ja) 2022-10-19

Similar Documents

Publication Publication Date Title
CN110293557B (zh) 姿势角计算装置、移动装置、姿势角计算方法和程序
JP2008089531A (ja) 傾斜角推定機構を有する移動体
WO2007148818A1 (ja) 姿勢角検出装置と姿勢角検出方法
CN101617199B (zh) 一种用于补偿位置测量设备中的漂移的方法
JP6191580B2 (ja) 移動体のセンサ校正方法
JP2013116684A (ja) 倒立移動体及び角速度センサの出力値補正方法
CN108871323B (zh) 一种低成本惯性传感器在机动环境下的高精度导航方法
JP6604175B2 (ja) ピッチ角速度補正値算出装置、姿勢角算出装置およびピッチ角速度補正値算出方法
JP7091733B2 (ja) 位置推定システム、位置検出方法、およびプログラム
KR20120042394A (ko) 주행 장치를 위한 위치측정 시스템 및 방법
KR102038482B1 (ko) 관성 센서 향상
JP4410051B2 (ja) 角速度計測装置および脚式移動ロボット
JP2013104665A (ja) 姿勢角演算装置、姿勢角演算方法、及びプログラム
JP6662121B2 (ja) 飛行体の重心位置表示方法および飛行体の重心位置表示システム
JP2017090448A (ja) 移動体制御システム及び移動体
JP2005147696A (ja) 取り付け角度算出装置
CN110637266A (zh) 云台的控制方法和云台
KR102005405B1 (ko) 플랫폼의 동적 평형 유지 방법, 이를 이용한 로봇 및 로봇의 제어 방법
JP2010271209A (ja) 加速度センサのオフセット誤差を補正する携帯型情報機器、方法及びプログラム
JP6632727B2 (ja) 角度計測装置
US20230314462A1 (en) Inertial sensor
JP6454857B2 (ja) 姿勢検出装置及び姿勢検出方法
JP2022144935A (ja) 姿勢制御装置及び姿勢制御方法
JP3884742B2 (ja) 方位計
KR101676470B1 (ko) 주행 로봇의 진행각 추정 장치 및 그 방법, 그 방법을 수행하기 위한 프로그램이 기록된 컴퓨터 판독 가능한 기록 매체

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20201026

A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20210930

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20211207

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20211215

A02 Decision of refusal

Free format text: JAPANESE INTERMEDIATE CODE: A02

Effective date: 20220510

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20220607

C60 Trial request (containing other claim documents, opposition documents)

Free format text: JAPANESE INTERMEDIATE CODE: C60

Effective date: 20220607

A911 Transfer to examiner for re-examination before appeal (zenchi)

Free format text: JAPANESE INTERMEDIATE CODE: A911

Effective date: 20220614

C21 Notice of transfer of a case for reconsideration by examiners before appeal proceedings

Free format text: JAPANESE INTERMEDIATE CODE: C21

Effective date: 20220621

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: 20220906

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20220919

R151 Written notification of patent or utility model registration

Ref document number: 7155562

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R151