JP5686048B2 - 位置姿勢出力装置、位置姿勢出力プログラム及び位置姿勢出力方法 - Google Patents
位置姿勢出力装置、位置姿勢出力プログラム及び位置姿勢出力方法 Download PDFInfo
- Publication number
- JP5686048B2 JP5686048B2 JP2011128549A JP2011128549A JP5686048B2 JP 5686048 B2 JP5686048 B2 JP 5686048B2 JP 2011128549 A JP2011128549 A JP 2011128549A JP 2011128549 A JP2011128549 A JP 2011128549A JP 5686048 B2 JP5686048 B2 JP 5686048B2
- Authority
- JP
- Japan
- Prior art keywords
- odometry
- orientation
- unit
- posture
- moving body
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims description 58
- 238000004364 calculation method Methods 0.000 claims description 78
- 230000007704 transition Effects 0.000 claims description 6
- 238000009499 grossing Methods 0.000 claims 1
- 238000001914 filtration Methods 0.000 description 43
- 230000008569 process Effects 0.000 description 32
- 238000012545 processing Methods 0.000 description 29
- 238000010586 diagram Methods 0.000 description 22
- 230000006870 function Effects 0.000 description 5
- 238000013459 approach Methods 0.000 description 3
- 230000008859 change Effects 0.000 description 3
- 230000014509 gene expression Effects 0.000 description 3
- 230000010354 integration Effects 0.000 description 3
- 230000009466 transformation Effects 0.000 description 3
- 230000006399 behavior Effects 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000003384 imaging method Methods 0.000 description 2
- 238000013215 result calculation Methods 0.000 description 2
- 239000004065 semiconductor Substances 0.000 description 2
- 235000008733 Citrus aurantifolia Nutrition 0.000 description 1
- 235000011941 Tilia x europaea Nutrition 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000008094 contradictory effect Effects 0.000 description 1
- 238000007796 conventional method Methods 0.000 description 1
- 238000007667 floating Methods 0.000 description 1
- 239000004571 lime Substances 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 230000004044 response Effects 0.000 description 1
Images
Description
まず、本実施例に係る位置姿勢出力装置を含むロボットの機能的構成について説明する。図1は、実施例1に係るロボットの機能的構成を示すブロック図である。図1に示すロボット1は、オフィス、廊下やショッピングモール等の空間において所定の目標の位置、例えば予め定められた位置や外部から指示入力された位置に向かって自律走行する装置である。
続いて、本実施例に係る位置姿勢出力装置の機能的構成について説明する。図2は、実施例1に係る位置姿勢出力装置の機能的構成を示すブロック図である。図2に示すように、位置姿勢出力装置10は、第1の取得部11と、第2の取得部12と、記憶部13と、更新部14と、算出部15と、出力部16とを有する。
次に、内部状態量の更新およびフィルタリング結果の算出に関する計算例を説明する。なお、E(t)、E_odo(t)、f(t)、odo(t)、z(t)、dsのデータ型は、それぞれ浮動小数点型のx座標位置(m)、y座標位置(m)、姿勢θ(rad)を含んで構成されるものとする。さらに、ここでは、内部状態量として初期の位置・姿勢(x,y,θ)として(0,0,0)が設定される場合を想定して以下の説明を行う。加えて、以下では、これらデータの要素を区別して説明する場合には、「E(t).x」、「E(t).y」「E(t).θ」のように記載する場合がある。
θ(t’)=E(t’).θ-E_odo(t’).θ
dx=(E_odo(t’).x-E_odo(t’-1).x)・cosθ(t’)-(E_odo(t’).y-E_odo(t’-1).y)・sinθ(t’)
dy=(E_odo(t’).x-E_odo(t’-1).x)・sinθ(t’)+(E_odo(t’).y-E_odo(t’-1).y)・cosθ(t’)
すなわち、
z(t’-1).x+dx=5.0+1.0・cos(-1.56)-1.0・sin(-1.56)=5.0+0.01+1.00=6.01
z(t’-1).y+dy=5.0+1.0・sin(-1.56)+1.0・cos(-1.56)=5.0-1.00+0.01=4.01
z(t’-1).θ+E_odo(t’).θ-E_odo(t’-1).θ=1.45
となる。
すなわち、
z(t’).x=6.01-0.003=6.007
z(t’).y=4.01+0.66=4.67
z(t’).θ=1.45-0.003=1.497
と算出する。
θ(t’)=E(t’).θ(t’)-E_odo(t’).θ(t’)
dx=(odo(t_new).x-E_odo(t’).x)・cosθ(t’)-(odo(t_new).y-E_odo(t’).y)・sinθ(t’)
dy=(odo(t_new).x-E_odo(t’).x)・sinθ(t’)+(odo(t_new).y-E_odo(t’).y)・cosθ(t’)
の座標変換式を適用する。
f(t_new).x=6.007+0.5・cos(-1.56)-0.5・sin(-1.56)=6.507
f(t_new).y=4.67+0.5・cos(-1.56)+0.5・sin(-1.56)=4.17
f(t_new).θ=1.497-0.2=1.297
次に、本実施例に係る位置姿勢出力装置の処理の流れについて説明する。なお、ここでは、位置姿勢出力装置が実行する(1)出力処理を説明した後に、(2)算出処理を説明することとする。
図6は、実施例1に係る出力処理の手順を示すフローチャートである。この出力処理は、ロボット1の電源が作動した場合に起動する処理であり、電源がOFF状態になるまで繰り返し実行される。
図7は、実施例1に係る算出処理の手順を示すフローチャートである。この算出処理は、ロボット1の管理者によって定数ωの算出が指示された場合等に処理が起動する。
上述してきたように、本実施例に係る位置姿勢出力装置10は、オドメトリの差分に基づく内部状態量「z(t’-1)+E_odo(t’)-E_odo(t’-1)」から今回の位置・姿勢の推定結果「E(t’)」へシフト値dsの分、シフトさせる内部状態量z(t)の更新を実行する。これによって、位置・姿勢の推定結果がばらつくという弱点をオドメトリの差分によって補完することができる結果、内部状態量z(t)を実際の位置・姿勢と近い値に保つことができる。また、本実施例に係る位置姿勢出力装置10は、オドメトリの差分「odo(t_new)-E_odo(t’)」を用いてフィルタリング結果を算出することにより、位置・姿勢の推定結果が入力されずともロボット1の位置・姿勢を導出できる。このため、位置・姿勢の推定結果が入力されるのを待機せずに、位置・姿勢の推定結果が入力されるよりも短い周期、すなわちオドメトリが入力される周期でフィルタリング結果を出力できる。したがって、本実施例に係る位置姿勢出力装置10によれば、ロボット1の正確な位置および姿勢を時間遅れなく出力することが可能になる。
上記の実施例1では、第2の取得部12によって位置・姿勢の推定結果が取得される周期で内部状態量zを更新する場合を説明したが、開示の装置は先の例に限定されない。他の一例としては、第1の取得部11によってオドメトリが取得される周期で更新する内部状態量z’を新たに使用することもできる。これによって、フィルタリング結果が前回に得られたフィルタリング結果との間で極端に大きな変化をとる場合に、フィルタリング結果を時間的に滑らかに変化させることができる。なお、内部状態量z’のデータ型についても、内部状態量zと同様に、浮動小数点型のx座標位置(m)、y座標位置(m)、姿勢θ(rad)を含んで構成されるものとする。
また、図示した各装置の各構成要素は、必ずしも物理的に図示の如く構成されていることを要しない。すなわち、各装置の分散・統合の具体的形態は図示のものに限られず、その全部または一部を、各種の負荷や使用状況などに応じて、任意の単位で機能的または物理的に分散・統合して構成することができる。例えば、センサ5a、計算部3、推定部5、経路計画部7または移動制御部9をロボット1の外部装置としてネットワーク経由で接続するようにしてもよい。また、センサ5a、計算部3、推定部5、経路計画部7または移動制御部9を別の装置がそれぞれ有し、ネットワーク接続されて協働することで、上記のロボット1の機能を実現するようにしてもよい。
また、上記の実施例で説明した各種の処理は、予め用意されたプログラムをパーソナルコンピュータやワークステーションなどのコンピュータで実行することによって実現することができる。そこで、以下では、図15を用いて、上記の実施例と同様の機能を有する位置姿勢出力プログラムを実行するコンピュータの一例について説明する。
3 計算部
5 推定部
5a センサ
7 経路計画部
9 移動制御部
10 位置姿勢出力装置
11 第1の取得部
12 第2の取得部
13 記憶部
14 更新部
15 算出部
16 出力部
Claims (5)
- 移動体の位置および姿勢を記憶する記憶部と、
前記移動体のオドメトリを計算する計算部から、前記オドメトリを第1の間隔で取得する第1の取得部と、
前記オドメトリ以外の情報を用いて前記移動体の位置および姿勢を推定する推定部から、前記移動体の位置および姿勢の推定結果を前記第1の間隔よりも長い第2の時間間隔で取得する第2の取得部と、
前記第2の取得部によって前記推定結果が取得された場合に、更新後の移動体の位置および姿勢の値が、前記計算部により計算されるオドメトリのうち前記推定結果が取得された時点で前記第1の取得部により取得された第1のオドメトリ及び前記第1のオドメトリよりも以前に前記第1の取得部により取得された第2のオドメトリの差分を前記記憶部に記憶された移動体の位置および姿勢に加算することにより得られるオドメトリの差分に基づく位置および姿勢と、前記推定部による移動体の位置および姿勢の推定結果との間に遷移するように、前記記憶部に記憶された移動体の位置および姿勢を更新する更新部と、
前記第1の取得部によってオドメトリが前記第1の間隔で取得される度に、当該オドメトリと前記第1のオドメトリとの差分を前記記憶部に記憶された移動体の位置および姿勢に足し合わせた上で出力する出力部と
を有することを特徴とする位置姿勢出力装置。 - 前記オドメトリの差分に基づく位置および姿勢から前記推定部による移動体の位置および姿勢の推定結果へ前記更新後の移動体の位置および姿勢を遷移させる度合いを定める定数を算出する算出部をさらに有し、
前記更新部は、
前記オドメトリの差分に基づく位置および姿勢と前記推定部による移動体の位置および姿勢の推定結果との差分に前記算出部によって算出された定数を乗算した乗算値を用いて、前記記憶部に記憶された移動体の位置および姿勢を更新することを特徴とする請求項1に記載の位置姿勢出力装置。 - 前記記憶部は、
前記第2の取得部によって前記推定結果が取得された場合に更新される移動体の位置および姿勢とともに、前記第1の取得部によってオドメトリが取得された場合に更新される移動体の位置および姿勢をさらに記憶するものであって、
前記更新部は、
前記第1の取得部によってオドメトリが前記第1の間隔で取得される度に、前記第1の間隔と前記第2の間隔との比に基づいて、前記オドメトリの差分に基づく位置および姿勢と前記推定部による移動体の位置および姿勢の推定結果との差分を平滑化した上で、前記記憶部に記憶された移動体の位置および姿勢のうち前記第1の取得部によってオドメトリが取得された場合に更新される移動体の位置および姿勢に足し合わせる更新をさらに実行することを特徴とする請求項1または2に記載の位置姿勢出力装置。 - コンピュータに、
前記移動体のオドメトリを計算する計算部から、前記オドメトリを第1の間隔で取得し、
前記オドメトリ以外の情報を用いて前記移動体の位置および姿勢を推定する推定部から、前記移動体の位置および姿勢の推定結果を前記第1の間隔よりも長い第2の時間間隔で取得し、
前記移動体の位置および姿勢の推定結果が取得された場合に、更新後の移動体の位置および姿勢の値が、前記計算部により計算されるオドメトリのうち前記推定結果が取得された時点で取得された第1のオドメトリ及び前記第1のオドメトリよりも以前に取得された第2のオドメトリの差分を移動体の位置および姿勢を記憶する記憶部に記憶された移動体の位置および姿勢に加算することにより得られるオドメトリの差分に基づく位置および姿勢と、前記推定部による移動体の位置および姿勢の推定結果との間に遷移するように、前記記憶部に記憶された移動体の位置および姿勢を更新し、
前記オドメトリが前記第1の間隔で取得される度に、当該オドメトリと前記第1のオドメトリとの差分を前記記憶部に記憶された移動体の位置および姿勢に足し合わせた上で出力する
各処理を実行させることを特徴とする位置姿勢出力プログラム。 - コンピュータが、
前記移動体のオドメトリを計算する計算部から、前記オドメトリを第1の間隔で取得し、
前記オドメトリ以外の情報を用いて前記移動体の位置および姿勢を推定する推定部から、前記移動体の位置および姿勢の推定結果を前記第1の間隔よりも長い第2の時間間隔で取得し、
前記移動体の位置および姿勢の推定結果が取得された場合に、更新後の移動体の位置および姿勢の値が、前記計算部により計算されるオドメトリのうち前記推定結果が取得された時点で取得された第1のオドメトリ及び前記第1のオドメトリよりも以前に取得された第2のオドメトリの差分を移動体の位置および姿勢を記憶する記憶部に記憶された移動体の位置および姿勢に加算することにより得られるオドメトリの差分に基づく位置および姿勢と、前記推定部による移動体の位置および姿勢の推定結果との間に遷移するように、前記記憶部に記憶された移動体の位置および姿勢を更新し、
前記オドメトリが前記第1の間隔で取得される度に、当該オドメトリと前記第1のオドメトリとの差分を前記記憶部に記憶された移動体の位置および姿勢に足し合わせた上で出力する
各処理を実行することを特徴とする位置姿勢出力方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2011128549A JP5686048B2 (ja) | 2011-06-08 | 2011-06-08 | 位置姿勢出力装置、位置姿勢出力プログラム及び位置姿勢出力方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2011128549A JP5686048B2 (ja) | 2011-06-08 | 2011-06-08 | 位置姿勢出力装置、位置姿勢出力プログラム及び位置姿勢出力方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2012255705A JP2012255705A (ja) | 2012-12-27 |
JP5686048B2 true JP5686048B2 (ja) | 2015-03-18 |
Family
ID=47527400
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2011128549A Active JP5686048B2 (ja) | 2011-06-08 | 2011-06-08 | 位置姿勢出力装置、位置姿勢出力プログラム及び位置姿勢出力方法 |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP5686048B2 (ja) |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2015225450A (ja) * | 2014-05-27 | 2015-12-14 | 村田機械株式会社 | 自律走行車、及び自律走行車における物体認識方法 |
JP7040308B2 (ja) * | 2018-06-14 | 2022-03-23 | 株式会社明電舎 | 無人搬送車の走行制御装置及び走行制御方法 |
CN112462769A (zh) * | 2020-11-25 | 2021-03-09 | 深圳市优必选科技股份有限公司 | 机器人定位方法、装置、计算机可读存储介质及机器人 |
Family Cites Families (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH03118416A (ja) * | 1989-09-30 | 1991-05-21 | Aisin Seiki Co Ltd | 自動車用ナビゲーション装置 |
JPH07286853A (ja) * | 1994-04-15 | 1995-10-31 | Nissan Motor Co Ltd | 車両用位置検出装置 |
JP3173951B2 (ja) * | 1994-06-30 | 2001-06-04 | 松下電器産業株式会社 | ナビゲーション装置 |
KR100785784B1 (ko) * | 2006-07-27 | 2007-12-13 | 한국전자통신연구원 | 인공표식과 오도메트리를 결합한 실시간 위치산출 시스템및 방법 |
JP4878256B2 (ja) * | 2006-10-18 | 2012-02-15 | 株式会社日立製作所 | 分岐線認識装置およびカーナビゲーションシステム |
-
2011
- 2011-06-08 JP JP2011128549A patent/JP5686048B2/ja active Active
Also Published As
Publication number | Publication date |
---|---|
JP2012255705A (ja) | 2012-12-27 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP6667686B2 (ja) | 自動運転車両のための走行軌跡生成方法、システム及び機械可読媒体 | |
US9255989B2 (en) | Tracking on-road vehicles with sensors of different modalities | |
JP6668375B2 (ja) | 比例、積分及び微分(pid)コントローラを用いた自律走行車のステアリング制御方法及びシステム | |
US10407076B2 (en) | Method and system for determining road frictions of autonomous driving vehicles using learning-based model predictive control | |
US10838427B2 (en) | Vision-aided inertial navigation with loop closure | |
US11377119B2 (en) | Drifting correction between planning stage and controlling stage of operating autonomous driving vehicles | |
US20190187297A1 (en) | System and method for locating a moving object | |
KR101909953B1 (ko) | 라이다 센서를 이용한 차량의 자세 추정 방법 | |
US8812234B2 (en) | Device to aid navigation, notably inside buildings | |
KR102238522B1 (ko) | 3차원 공간에 대응되는 맵을 생성하는 차량 및 방법 | |
US9864067B2 (en) | Method for determining a current position of a motor vehicle in a geodetic coordinate system and motor vehicle | |
JP2020064056A (ja) | 位置推定装置及び方法 | |
JP2019501809A (ja) | 自律走行車の速度を追従する方法及びシステム | |
CN109141411B (zh) | 定位方法、定位装置、移动机器人及存储介质 | |
KR101985344B1 (ko) | 관성 및 단일 광학 센서를 이용한 슬라이딩 윈도우 기반 비-구조 위치 인식 방법, 이를 수행하기 위한 기록 매체 및 장치 | |
CN110637209B (zh) | 估计机动车的姿势的方法、设备和具有指令的计算机可读存储介质 | |
JP5686048B2 (ja) | 位置姿勢出力装置、位置姿勢出力プログラム及び位置姿勢出力方法 | |
JP2022081396A (ja) | 車両側位方法及び装置 | |
Koroglu et al. | Pedestrian inertial navigation with building floor plans for indoor environments via non-recursive Bayesian filtering | |
Katwe et al. | Map-based particle filter for localization: Autonomous vehicle | |
JP5045567B2 (ja) | 撮像方向決定プログラム | |
JP7105819B2 (ja) | グラフ構造ベースの移動体の位置推定方法およびそれを用いた移動体 | |
CN113218389B (zh) | 一种车辆定位方法、装置、存储介质及计算机程序产品 | |
KR20190079470A (ko) | 객체 측위 방법 및 장치 | |
JP2013159246A (ja) | 車両位置姿勢推定装置及びプログラム |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20140204 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20140918 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20141007 |
|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20141208 |
|
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: 20141224 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20150106 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 5686048 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |