JP2007041657A - Moving body control method, and moving body - Google Patents
Moving body control method, and moving body Download PDFInfo
- Publication number
- JP2007041657A JP2007041657A JP2005222179A JP2005222179A JP2007041657A JP 2007041657 A JP2007041657 A JP 2007041657A JP 2005222179 A JP2005222179 A JP 2005222179A JP 2005222179 A JP2005222179 A JP 2005222179A JP 2007041657 A JP2007041657 A JP 2007041657A
- Authority
- JP
- Japan
- Prior art keywords
- moving body
- path
- moving
- robot
- unit
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 88
- 230000033001 locomotion Effects 0.000 claims abstract description 70
- 238000003384 imaging method Methods 0.000 claims description 18
- 230000009471 action Effects 0.000 claims description 15
- 230000009194 climbing Effects 0.000 claims description 11
- 238000012545 processing Methods 0.000 abstract description 139
- 230000006870 function Effects 0.000 abstract description 31
- 230000008569 process Effects 0.000 description 60
- 238000001514 detection method Methods 0.000 description 41
- 238000005259 measurement Methods 0.000 description 26
- 238000010586 diagram Methods 0.000 description 24
- 210000002414 leg Anatomy 0.000 description 12
- 238000009826 distribution Methods 0.000 description 7
- 230000007613 environmental effect Effects 0.000 description 7
- 238000000605 extraction Methods 0.000 description 6
- 210000004394 hip joint Anatomy 0.000 description 6
- 239000000284 extract Substances 0.000 description 5
- 238000003860 storage Methods 0.000 description 5
- CIWBSHSKHKDKBQ-JLAZNSOCSA-N Ascorbic acid Chemical compound OC[C@H](O)[C@H]1OC(=O)C(O)=C1O CIWBSHSKHKDKBQ-JLAZNSOCSA-N 0.000 description 4
- 230000008451 emotion Effects 0.000 description 4
- 210000002683 foot Anatomy 0.000 description 4
- 102220111708 rs77809780 Human genes 0.000 description 4
- 102220182402 rs80133178 Human genes 0.000 description 4
- 230000000007 visual effect Effects 0.000 description 4
- 238000012790 confirmation Methods 0.000 description 3
- 210000000544 articulatio talocruralis Anatomy 0.000 description 2
- 230000008921 facial expression Effects 0.000 description 2
- 210000001624 hip Anatomy 0.000 description 2
- 230000010365 information processing Effects 0.000 description 2
- 210000001503 joint Anatomy 0.000 description 2
- 230000009191 jumping Effects 0.000 description 2
- 210000000323 shoulder joint Anatomy 0.000 description 2
- 210000001364 upper extremity Anatomy 0.000 description 2
- OVGWMUWIRHGGJP-WVDJAODQSA-N (z)-7-[(1s,3r,4r,5s)-3-[(e,3r)-3-hydroxyoct-1-enyl]-6-thiabicyclo[3.1.1]heptan-4-yl]hept-5-enoic acid Chemical compound OC(=O)CCC\C=C/C[C@@H]1[C@@H](/C=C/[C@H](O)CCCCC)C[C@@H]2S[C@H]1C2 OVGWMUWIRHGGJP-WVDJAODQSA-N 0.000 description 1
- 101100161473 Arabidopsis thaliana ABCB25 gene Proteins 0.000 description 1
- 238000012935 Averaging Methods 0.000 description 1
- 101000988961 Escherichia coli Heat-stable enterotoxin A2 Proteins 0.000 description 1
- 241000282412 Homo Species 0.000 description 1
- 101100096893 Mus musculus Sult2a1 gene Proteins 0.000 description 1
- 101150081243 STA1 gene Proteins 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 230000006399 behavior Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 210000002310 elbow joint Anatomy 0.000 description 1
- 210000003414 extremity Anatomy 0.000 description 1
- 210000000245 forearm Anatomy 0.000 description 1
- 210000003127 knee Anatomy 0.000 description 1
- 210000000629 knee joint Anatomy 0.000 description 1
- 210000003141 lower extremity Anatomy 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 210000000707 wrist Anatomy 0.000 description 1
- 210000003857 wrist joint Anatomy 0.000 description 1
Images
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
Abstract
Description
本発明は、撮像画像を基に移動する移動体を制御する移動体制御方法と、その移動体に関する。 The present invention relates to a moving body control method for controlling a moving body that moves based on a captured image, and the moving body.
例えば、自律型ロボットは、産業用ロボットと異なり、周囲の外部状態やロボット自身の内部状態に応じて自律的に動作を行うことのできるロボットである。例えば、自律型ロボットは、歩行動作では、外部の障害物を検出して障害物を回避するような行動経路を計画することができなくてはならない。従来のロボットのなかには、自身のおかれた環境の障害物地図を作成し、これに基づいて行動経路を決定できるロボットも提案されている。 For example, unlike an industrial robot, an autonomous robot is a robot that can operate autonomously according to the surrounding external state or the internal state of the robot itself. For example, an autonomous robot must be able to plan an action path that detects an external obstacle and avoids the obstacle in walking motion. Among conventional robots, a robot that can create an obstacle map of its own environment and determine an action route based on the obstacle map has been proposed.
ところで、従来のロボットは、自らが備える機能、例えば、通常歩行、階段走行、よじ登り動作、ジャンプ動作などを行う機能が正常であるとの前提でロボットが移動するパスを決定し、ロボットを動作させている。
ところで、上述した機能は、その一部が故障する場合があるが、従来のロボットでは、全ての機能を正常に動作するという前提で、パス上の移動過程で利用する機能を順次決定するため、故障した機能が選択され、ロボットがパスを移動できない場合がある。
By the way, a conventional robot determines a path on which the robot moves and operates the robot on the assumption that the functions that the robot itself has, such as normal walking, stairs running, climbing movement, jumping movement, etc. are normal. ing.
By the way, some of the functions described above may fail, but in conventional robots, on the assumption that all functions operate normally, in order to sequentially determine the functions to be used in the movement process on the path, In some cases, the malfunctioning function is selected and the robot cannot move along the path.
本発明は上述した従来技術の問題点を解決するために、移動体が利用可能な移動動作が変化した場合でも、適切な移動動作を選択してパス上を移動できる移動体制御方法および移動体を提供することを目的とする。 In order to solve the above-described problems of the prior art, the present invention provides a moving body control method and a moving body capable of selecting an appropriate moving action and moving on a path even when the moving action available to the moving body changes. The purpose is to provide.
上述した従来技術の問題点を解決し、上述した目的を達成するため、第1の観点の発明の移動体制御方法は、移動体が目標位置まで移動する動作を制御する移動体制御方法であって、前記移動体の現在位置から前記目標位置までのパスの状態を検出する第1の工程と、前記移動体が実行可能な複数の移動動作を特定する第2の工程と、前記第1の工程で検出したパスの状態に基づいて、前記第2の工程で特定した前記移動体が実行可能な複数の種類の移動動作のうち、前記移動体の位置の状態に適合した移動動作を順次選択し、当該選択した移動動作を前記移動体が行うように制御する第3の工程とを有する。 In order to solve the above-described problems of the prior art and achieve the above-described object, the mobile body control method of the first aspect of the invention is a mobile body control method for controlling the movement of the mobile body to the target position. A first step of detecting a state of a path from the current position of the moving body to the target position, a second step of identifying a plurality of moving operations that can be performed by the moving body, and the first step Based on the state of the path detected in the process, sequentially select a movement operation suitable for the position of the moving body from among a plurality of types of movement operations that can be executed by the moving body specified in the second step. And a third step of controlling the moving body to perform the selected moving operation.
第2の観点の発明の移動体は、異なる複数の撮像位置からの撮像対象領域の撮像結果を生成する撮像手段と、走行手段と、前記撮像手段が生成した前記撮像結果を基に、前記移動体の現在位置から前記目標位置までのパスの状態を検出し、前記移動体が実行可能な複数の移動動作を特定し、前記検出したパスの状態に基づいて、前記第2の工程で特定した前記移動体が実行可能な複数の種類の移動動作のうち、前記移動体の位置の状態に適合した移動動作を順次選択し、当該選択した移動動作を前記移動体が行うように前記走行手段を制御する制御手段とを有する。 The moving body according to the second aspect of the invention is based on the imaging result generated by the imaging means, the traveling means, and the imaging result generated by the imaging means. The state of the path from the current position of the body to the target position is detected, a plurality of movement operations that can be executed by the moving body are specified, and the second step is specified based on the detected state of the path Among the plurality of types of moving operations that can be executed by the moving body, the moving means that sequentially selects a moving operation that matches the position state of the moving body, and the traveling unit performs the selected moving operation. Control means for controlling.
本発明によれば、移動体が利用可能な移動動作が変化した場合でも、適切な移動動作を選択してパス上を移動できる移動体制御方法および移動体を提供することができる。 According to the present invention, it is possible to provide a moving body control method and a moving body capable of selecting an appropriate moving action and moving on a path even when a moving action that can be used by the moving body changes.
以下、本発明の実施形態に係わるロボットについて説明する。
先ず、本実施形態の構成要素と、本発明の構成要素との対応関係を説明する。
図5に示す環境クラス分類部34の処理が本発明の第1の工程の一例であり、図22に示すパス決定部36のステップST61の処理が本発明の第2の工程の一例であり、ステップST62〜ST64、図24および図25の処理が本発明の第3の工程の一例である。
また、図23の処理が、本発明の第4の工程の一例である。
Hereinafter, a robot according to an embodiment of the present invention will be described.
First, the correspondence between the components of the present embodiment and the components of the present invention will be described.
The process of the environment
Moreover, the process of FIG. 23 is an example of the 4th process of this invention.
また、図1に示すCCDカメラ10R,10Lが本発明の撮像手段の一例であり、図3および図4に示す脚部などが本発明の走行手段の一例であり、図1に示すCPU15が本発明の制御手段の一例である。
Further, the
本実施形態のロボット1は、ロボット1では、図5等に示すように、CPU15が機能として利用可能な処理部35_1〜35_6等を特定し(図22に示すステップST61)、その利用な処理部を基に、目標位置までの複数のパスを動的に特定する(ステップST62)。
これにより、例えば、処理部35_1〜35_6あるいはそれによって制御されるアクチュエータ群16などの機械部位が故障した場合に、その故障した機能に対応した処理部による制御を利用しないで目標位置に達するパスを選択することができる。
In the
Thereby, for example, when a machine part such as the processing units 35_1 to 35_6 or the
図1は、本発明の実施形態のロボット1の機能ブロック図である。
図1に示すように、ロボット1は、例えば、CCDカメラ10R,10L、ステレオ画像処理装置12、CPU15、アクチュエータ群16およびセンサ群17を有する。
CCDカメラ10R,10Lは、それぞれ撮像結果に応じた右画像データS10Rおよび左画像データS10Lをステレオ画像処理装置12に出力する。
FIG. 1 is a functional block diagram of a
As shown in FIG. 1, the
The
ステレオ画像処理装置12は、CCDカメラ10R,10Lから入力した右画像データS10Rと左画像データS10Lの視差情報(disparity data)(距離情報)を計算し、カラー画像データ(YUV:輝度Y、UV色差)S12a及び視差画像データ(YDR:輝度Y、視差D、信頼度R)S12bをフレーム毎に左右交互に算出する。
ここで、視差とは、空間中のある点が左側カメラ及び右側カメラに写像される点の違いを示し、そのカメラからの距離に応じて変化するものである。
カラー画像データS12a及び視差画像データS12bは、CPU(制御部)15に入力される。
CPU15は、ステレオ画像処理装置12から入力したカラー画像データS12a及び視差画像データS12bを用いて、後述するソフトウェア構成によって、画像処理を行う。
The stereo
Here, the parallax indicates a difference between points where a certain point in the space is mapped to the left camera and the right camera, and changes according to the distance from the camera.
The color image data S12a and the parallax image data S12b are input to a CPU (control unit) 15.
The
アクチュエータ群16は、ロボット1の動きを駆動する各種のアクチュエータである。
アクチュエータ群16は、CPU15からの制御信号S15によって駆動される。
センサ群17は、ロボット1の所定箇所に設置された赤外線センサなどの非接触型センサや接触型センサなどである。
センサ群17は、検出信号S17をCPU15に出力する。
The
The
The
The
以下、図1に示すロボット1の構成をさらに詳細に説明する。
図2に示すように、ロボット1は、2足歩行タイプであり、日常生活上の様々な場面における人的活動を支援する実用ロボットである。
ロボット1は、内部状態(怒り、悲しみ、喜び、楽しみ等)に応じて行動できるほか、人間が行う基本的な動作を表出できるエンターテインメントロボットである。ロボットは、3次元環境クラスデータを生成し、これを基に行動経路を決定する。
Hereinafter, the configuration of the
As shown in FIG. 2, the
The
図2に示すように、ロボット1は、体幹部ユニット202の所定の位置に頭部ユニット203が連結されるとともに、左右2つの腕部ユニット204R/Lと、左右2つの脚部ユニット205R/Lが連結されて構成されている。
As shown in FIG. 2, the
図2に示すロボット1が具備する関節自由度構成を図3に模式的に示す。
頭部ユニット203を支持する首関節は、首関節ヨー軸101と、首関節ピッチ軸102と、首関節ロール軸103という3自由度を有している。
A joint degree-of-freedom configuration of the
The neck joint that supports the
また、上肢を構成する各々の腕部ユニット204R/Lは、肩関節ピッチ軸107と、肩関節ロール軸108と、上腕ヨー軸109と、肘関節ピッチ軸110と、前腕ヨー軸111と、手首関節ピッチ軸112と、手首関節ロール軸113と、手部114とで構成される。
手部114は、実際には、複数本の指を含む多関節・多自由度構造体である。ただし、手部114の動作は、ロボット1の姿勢制御や歩行制御に対する寄与や影響が少ないので、本明細書では簡単のため、ゼロ自由度と仮定する。したがって、各腕部は7自由度を有するとする。
また、体幹部ユニット202は、体幹ピッチ軸104と、体幹ロール軸105と、体幹ヨー軸106という3自由度を有する。
Each
The
The
また、下肢を構成する各々の脚部ユニット205R/Lは、股関節ヨー軸115と、股関節ピッチ軸116と、股関節ロール軸117と、膝関節ピッチ軸118と、足首関節ピッチ軸119と、足首関節ロール軸120と、足底121とで構成される。
本明細書中では、股関節ピッチ軸116と股関節ロール軸117の交点は、ロボット1の股関節位置を定義する。人体の足底121は、実際には多関節・多自由度の足底を含んだ構造体であるが、本明細書においては、簡単のためロボット1の足底は、ゼロ自由度とする。したがって、各脚部は、6自由度で構成される。
以上を総括すれば、ロボット1全体としては、合計で3+7×2+3+6×2=32自由度を有することになる。ただし、エンターテインメント向けのロボット1が必ずしも32自由度に限定されるわけではない。設計・制作上の制約条件や要求仕様等に応じて、自由度すなわち関節数を適宜増減することができることはいうまでもない。
Each
In this specification, the intersection of the hip
In summary, the
上述したようなロボット1がもつ各自由度は、実際にはアクチュエータを用いて実装される。外観上で余分な膨らみを排してヒトの自然体形状に近似させること、2足歩行という不安定構造体に対して姿勢制御を行うこと等の要請から、アクチュエータは小型かつ軽量であることが好ましい。
なお、図3および図4に示すアクチュエータによって図1に示すアクチュエータ群16が構成され、図3および図4に示すセンサによって図1に示すセンサ群17が構成される。
Each degree of freedom of the
The
このようなロボットは、ロボット全体の動作を制御する制御システムを例えば体幹部ユニット202等に備える。
図3および図4は、本実施形態のロボット1の制御システム構成を示す模式図である。
図3および図4に示すように、制御システムは、ユーザ入力等に動的に反応して情緒判断や感情表現を司る思考制御モジュール200と、アクチュエータ350の駆動等、ロボット1の全身協調運動を制御する運動制御モジュール300とで構成される。
Such a robot includes a control system that controls the operation of the entire robot, for example, in the
3 and 4 are schematic views showing the control system configuration of the
As shown in FIG. 3 and FIG. 4, the control system performs the whole body cooperative movement of the
思考制御モジュール200は、情緒判断や感情表現に関する演算処理を実行するCPU(Central Processing Unit)211や、RAM(Random Access Memory)212、ROM(Read Only Memory)213及び外部記憶装置(ハード・ディスク・ドライブ等)214等で構成され、モジュール内で自己完結した処理を行うことができる、独立駆動型の情報処理装置である。
The
この思考制御モジュール200は、画像入力装置251から入力される画像データや音声入力装置252から入力される音声データ等、外界からの刺激等にしたがって、ロボット1の現在の感情や意思を決定する。すなわち、上述したように、入力される画像データからユーザの表情を認識し、その情報をロボット201の感情や意思に反映させることで、ユーザの表情に応じた行動を発現することができる。
ここで、画像入力装置251は、例えばCCD(Charge Coupled Device)カメラを複数備えており、これらのカメラにより撮像した画像から距離画像を得ることができる。
本実施形態では、画像入力装置251は、図1に示すCCDカメラ10R,10Lに対応している。
また、音声入力装置252は、例えばマイクロホンを複数備えている。
The
Here, the
In the present embodiment, the
The
思考制御モジュール200は、意思決定に基づいた動作又は行動シーケンス、すなわち四肢の運動を実行するように、運動制御モジュール300に対して指令を発行する。
The
運動制御モジュール300は、ロボット1の全身協調運動を制御するCPU311(図1に示すCPU15)や、RAM312、ROM313及び外部記憶装置(ハード・ディスク・ドライブ等)314等で構成され、モジュール内で自己完結した処理を行うことができる独立駆動型の情報処理装置である。また、外部記憶装置314には、例えば、オフラインで算出された歩行パターンや目標とするZMP軌道、その他の行動計画を蓄積することができる。
この運動制御モジュール300には、図2に示したロボット1の全身に分散するそれぞれの関節自由度を実現するアクチュエータ350、対象物との距離を測定する距離計測センサ(図示せず)、体幹部ユニット202の姿勢や傾斜を計測する姿勢センサ351、左右の足底の離床又は着床を検出する接地確認センサ352,353、足底121の足底121に設けられる荷重センサ、バッテリ等の電源を管理する電源制御装置354等の各種の装置が、バス・インターフェース(I/F)310経由で接続されている。
ここで、姿勢センサ351は、例えば加速度センサとジャイロ・センサの組合せによって構成され、接地確認センサ352,353は、近接センサ又はマイクロ・スイッチ等で構成される。
The
The
Here, the
思考制御モジュール200と運動制御モジュール300は、共通のプラットフォーム上
で構築され、両者間はバス・インターフェース210,310を介して相互接続されている。
The
運動制御モジュール300では、思考制御モジュール200から指示された行動を体現すべく、各アクチュエータ350による全身協調運動を制御する。すなわち、CPU311は、思考制御モジュール200から指示された行動に応じた動作パターンを外部記憶装置314から取り出し、又は、内部的に動作パターンを生成する。そして、CPU311は、指定された動作パターンにしたがって、足部運動、ZMP軌道、体幹運動、上肢運動、腰部水平位置及び高さ等を設定するとともに、これらの設定内容に従った動作を指示す
る指令値を各アクチュエータ350に転送する。
The
CPU311は、姿勢センサ351の出力信号によりロボット1の体幹部ユニット202の姿勢や傾きを検出するとともに、各接地確認センサ352,353の出力信号により各脚部ユニット205R/Lが遊脚又は立脚のいずれの状態であるかを検出することによって、ロボット1の全身協調運動を適応的に制御することができる。更に、CPU311は、ZMP位置が常にZMP安定領域の中心に向かうように、ロボット1の姿勢や動作を制御する。
The
また、運動制御モジュール300は、思考制御モジュール200において決定された意
思通りの行動がどの程度発現されたか、すなわち処理の状況を、思考制御モジュール20
0に返すようになっている。このようにしてロボット1は、制御プログラムに基づいて自己及び周囲の状況を判断し、自律的に行動することができる。
In addition, the
It returns to 0. In this way, the
以下、図1および図4に示すCPU15(CPU311)の機能を説明する。
図5は、図1および図4に示すCPU15の機能ブロック図である。
図5に示すように、CPU15は、例えば、距離データ生成部31、平面検出部32、グリッドマップ生成部33、環境クラス分類部34、通常走行処理部35_1、階段走行処理部35_2、注意走行処理部35_3、這動作処理部35_4、ステップオーバ処理部35_5、よじ登り/ジャンプ処理部35_6,パス決定部36、並びに動き制御部37を有する。
The function of the CPU 15 (CPU 311) shown in FIGS. 1 and 4 will be described below.
FIG. 5 is a functional block diagram of the
As illustrated in FIG. 5, the
[距離データ生成部31]
距離データ生成部31は、図1に示すステレオ画像処理装置12から入力したカラー画像データS12a及び視差画像データS12bを基に、ロボット1から画像(環境)内の対象物までの3次元距離を示す距離データ(ステレオデータ)S31を生成し、これを平面検出部32およびグリッドマップ生成部33に出力する。
距離データ生成部31は、例えば、図6に示すような画像(環境)内の距離データS31を生成する。図6に示す実空間S500には、ロボット1が登行可能な平面PL501とPL502とを備える階段(Staircase)ST503と、ロボット1が登行不可能な障害物(Obstacle)OB504,505とが示されている。
また、図7は、ロボット1が外界を撮影している様子を示す模式図である。
ここでは、床面(基準面)をx−y平面とし、高さ方向をz軸としたとき、図7に示すように、画像入力部(ステレオカメラ)を頭部ユニット203に有するロボット1の視野範囲は、ロボット1の前方の所定範囲となる。ロボット1は、視野範囲にある環境を認識して距離データS31を生成する。
[Distance data generator 31]
The distance
The distance
FIG. 7 is a schematic diagram showing a state in which the
Here, when the floor (reference plane) is the xy plane and the height direction is the z axis, the
[平面検出部32]
平面検出部32は、距離データ生成部31から入力した距離データS31から環境内に存在する複数の平面を検出し、それを示す平面データS32をグリッドマップ生成部33に出力する。
ここでの平面の検出方法としては、線分拡張法、ハフ変換等を利用した公知の平面検出技術を適用することができる。ノイズを含む距離データから階段のように複数平面を検出する代表例として、線分拡張法による平面検出を行うと正確に平面を検出することができる。
[Plane detection unit 32]
The
As a plane detection method here, a known plane detection technique using a line segment expansion method, a Hough transform, or the like can be applied. As a representative example of detecting a plurality of planes like staircases from distance data including noise, it is possible to accurately detect planes by performing plane detection by a line segment expansion method.
線分拡張法は、視野内において支配的な平面だけでなく、例えば階段など複数の平面が
存在する場合であっても確実に複数平面を検出することができ、平面を検出する際に抽出
する線分抽出において、距離データの点の分布に応じて適応的に線分をフィッティングさ
せることにより観測ノイズに対してロバストな平面検出結果を得ることができる。
The line segment expansion method can detect not only a dominant plane in the field of view but also a plurality of planes such as stairs, and can extract them when detecting a plane. In line segment extraction, a plane detection result that is robust against observation noise can be obtained by fitting a line segment adaptively according to the distribution of the points in the distance data.
平面検出部32は、図8に示すように、画像を構成する距離データから同一平面にあると推定される距離データ点群を選択し、この距離データ点群毎に線分を抽出する線分抽出部51と、画像内に含まれる、線分抽出部51よって抽出された全線分からなる線分群から、該画像内に存在する1又は複数の平面領域を検出する領域拡張部52とを有する。
領域拡張部52は、線分群から同一平面上に存在すると推定される任意の3本の線分を選択し、これらから基準平面を求める。そして、選択した3本の線分に隣接する線分がこの基準平面と同一平面に属するか否かを判定し、同一平面に属すると判定した場合にはその領域拡張用線分としての線分により基準平面を更新するとともに基準平面の領域を拡張する。
As shown in FIG. 8, the
The
線分抽出部51は、その距離画像における列又は行毎の各データ列において、3次元空間内で同一平面上にあると推定される距離データ点群を抽出し、この距離データ点群から距離データ点群の分布に応じて1以上の線分を生成する。すなわち、分布に偏りがあると判断された場合には、データ点群は同一平面上にないと判断し、データ点群を分割し、分割したデータ点群それぞれについて再度分布に偏りがあるかを判断する処理を繰り返し、分布に偏りがない場合にはそのデータ点群から線分を生成する。全てのデータ列について以上の処理を行い、生成した線分群S51を領域拡張部52に出力する。
The line
領域拡張部52は、この線分S51において、同一の平面に属すると推定される線分を3本選択し、これらから基準平面としての種となる平面を求める。この種となる平面の領域(領域種:seed region)に対して、該領域種と同一平面に属する線分を順次統合していくことで拡張していく領域拡張によって距離画像を複数の平面に分割し、平面群の平面データS32を出力する。
In this line segment S51, the
平面検出部32は、ハフ変換のように、取得した画像に含まれる支配的な平面を検出するのみならず、階段など複数平面が含まれている場合であっても平面を検出可能とするため、線分拡張法により平面を検出する。そして、この際、距離データ点の分布に応じて線分を生成することにより、計測ノイズに対してロバストな検出結果を得ることができるものである。
The
図9は、線分拡張法による平面検出方法を説明する図である。
線分拡張法による平面検出では、図9に示すように、まず、焦点Fから撮影された画像20において、行方向又は列方向のデータ列における処理をする。画像内の例えば行方向の画素列(image row:イメージロウ)において、距離データ点が同一の平面に属するならば直線となることを利用し、同一平面に属すると推定される距離データ点からなる線分を生成する。そして、得られた複数の線分からなる線分群において、同一平面を構成するとされる線分群に基づき平面を推定、検出する方法である。
FIG. 9 is a diagram for explaining a plane detection method based on a line segment expansion method.
In plane detection by the line segment expansion method, as shown in FIG. 9, first, processing is performed on a data string in the row direction or the column direction in the
図10は、線分拡張法による平面検出処理を示すフローチャートである。
図10に示すように、まず、平面検出部32は、距離データS31を入力し(ステップST1)、距離画像の行方向(又は列方向)の各画素列において同一平面に属すると推定されるデータ点から線分を求める(ステップST2)。
そして、平面検出部32は、これらの線分群のなかから同一平面に属すると推定される線分を抽出し、これらの線分からなる平面を求める(ステップST3)。このステップS3では、まず、平面検出部32は、平面の種となる領域(以下、領域種(seed region)という。)を選び、該当する領域種を選択する。この選択においては、上下隣接する行方向(又は左右隣接する列方向)の1ラインを含む3本の線分が同一平面にあることを条件とする。ここで、平面検出部32は、選択した3本の線分からなる領域種が属する平面を基準平面とし、3本の線分から平均して求まる平面を求めておく。また、3本の線分からなる領域を基準平面領域とする。
FIG. 10 is a flowchart showing plane detection processing by the line segment expansion method.
As shown in FIG. 10, first, the
Then, the
そして、平面検出部32は、選択した領域種に隣接する行方向(又は列方向)の画素列からなる直線と上記基準平面とが同じ平面であるかどうかを空間的な距離を比較することで判断し、同じ平面である場合には、その隣接する線分を基準平面領域に追加し(領域拡張処理)、追加した線分を含めたものとして上記基準平面を更新し(平面更新処理)、これを平面領域に隣接するデータ列に同一平面の線分が存在しなくなるまで繰り返し行う。そして、以上領域種を検索して平面更新及び領域拡張処理を、種となる領域(3本の線分)が存在しなくなるまで繰り返し実行する。最後に、得られた複数の領域群のなかから同一平面を構成するものを連結する。そして、本実施の形態においては、得られた平面に属する線分群のうち、平面から所定の閾値以上外れる線分を除いて再度平面を求める平面再算出処理をステップST4として更に設け、最終的な平面とするが、詳細は後述する。
Then, the
ここで、3次元距離データから線分を検出し、これを同一平面毎にまとめた領域を1つの平面とする処理は従来の線分拡張法による平面検出処理であるが、本実施の形態においては、ステップST2における線分抽出方法が従来とは異なる。すなわち、上述したように、距離データ点から線分を求めて距離データ点にできるだけフィットするように線分を生成しようとしても、距離データの精度に応じて閾値を変更しなければ、over-segmentation又はunder-segmentationなどもの問題が生じてしまう。そこで、本実施の形態においては、この線分抽出において、距離データの分布を解析することで、距離データの精度、ノイズに応じて適応的に閾値を変更する手法を導入するものとする。 Here, the process of detecting a line segment from the three-dimensional distance data and combining the areas into the same plane as one plane is a plane detection process by the conventional line segment expansion method. Is different from the conventional line segment extraction method in step ST2. That is, as described above, even if an attempt is made to generate a line segment so as to fit the distance data point as much as possible, if the threshold value is not changed according to the accuracy of the distance data, over-segmentation Or problems such as under-segmentation occur. Therefore, in the present embodiment, a method of adaptively changing the threshold value according to the accuracy of distance data and noise is analyzed by analyzing the distribution of distance data in this line segment extraction.
上述した平面検出処理によって検出される平面について、図11、図12を用いて説明する。平面検出部32によって検出される平面が、例えば、図11、図12に示す階段STAの場合について説明する。図11(a)、図12(a)は階段を正面からみた図であり、図11(b)、図12(b)は階段を側面からみた図であり、図11(c)、図12(c)は階段を斜めからみた図である。
The plane detected by the plane detection process described above will be described with reference to FIGS. The case where the plane detected by the
本明細書においては、人間、ロボットなどが階段を昇降するために使用する面(足又は可動脚部を載せる面)を踏面といい、一の踏面からその次の踏面までの高さ(1段の階段の高さ)を蹴り上げということとする。また、階段は、地面に近い方から1段目、2段目とカウントすることとする。 In this specification, a surface (surface on which a foot or a movable leg is placed) used by a human, a robot, etc. to move up and down stairs is called a tread surface, and the height (one step) from one tread surface to the next tread surface. The height of the stairs) is to be kicked up. The stairs are counted as the first and second steps from the side closer to the ground.
図11に示す階段STA1は、段数が3段の階段であり、蹴り上げ4cm、1,2段面の踏面の大きさは幅30cm、奥行き10cm、最上段である3段目の踏面のみ、幅30cm、奥行き21cmとなっている。また、図12に示す階段STA2も段数が3段の階段であり、蹴り上げ3cm、1,2段面の踏面の大きさは幅33cm、奥行き12cm、最上段である3段目の踏面のみ、幅33cm、奥行き32cmとなっている。 The staircase STA1 shown in FIG. 11 is a staircase having three steps, the kicking height is 4 cm, the step size of the first and second steps is 30 cm wide, the depth is 10 cm, and only the third step tread is the width of the uppermost step. It is 30 cm and the depth is 21 cm. Further, the staircase STA2 shown in FIG. 12 is also a three-step staircase, the kicking height is 3 cm, the size of the tread surface of the first and second steps is 33 cm wide, the depth is 12 cm, and only the third step tread, which is the uppermost step, It is 33 cm wide and 32 cm deep.
[グリッドマップ生成部33]
グリッドマップ生成部33は、距離データ生成部31から入力した距離データS31と平面検出部32から入力した上記平面検出データS32とに基づいて、3次元グリッドマップデータGMおよびフロア高さデータFHを生成し、これらを環境クラス分類部34に出力する。
[Grid Map Generation Unit 33]
The grid
先ず、グリッドマップ生成部33による3次元グリッドマップデータGMの生成処理について説明する。
3次元グリッドマップデータGMは、周囲の環境を水平方向及び垂直方向に所定の大きさの格子(グリッド;Grid)に分割して表したものである。
グリッドマップ生成部33は、距離データ生成部31からの距離データS31と、平面検出部32からの平面データS32とに基づいて、例えば、4メートル四方、高さ1メートルの空間を、水平解像度4センチ及び垂直解像度1センチの直方体空間を1グリッド単位(1セルと記す。)とし、その集合によって表現された3次元グリッドマップデータGMを生成する。
グリッドマップ生成部33は、各セルに物体によって占有されているか否かの確率を付与し、観測データに基づいて各セルの占有確率を変更させている。また、グリッドマップ生成部33は、占有確率に閾値を設け、占有確率が閾値より大きいか否かによって環境の状態を地図として表現している。
本具体例では、高さ方向の解像度がセル分解能に依存しないため、垂直解像度は、1セ
ンチよりも粗くできる。
First, generation processing of the three-dimensional grid map data GM by the grid
The three-dimensional grid map data GM is obtained by dividing the surrounding environment into a grid (Grid) having a predetermined size in the horizontal direction and the vertical direction.
Based on the distance data S31 from the distance
The grid
In this specific example, since the resolution in the height direction does not depend on the cell resolution, the vertical resolution can be coarser than 1 cm.
グリッドマップ生成部33は、3次元グリッドで表される実空間S500における全ての単位グリッド(以下、セルと記す。)p(x,y,z)に対して次の処理を行う。
グリッドマップ生成部33は、撮像画像内に像として映し出された測定点(3次元グリッド)と観測中心(ステレオカメラ中心)とを結ぶ直線上には障害物が存在しないというアルゴリズムに基づいて3次元グリッドマップデータGMを作成及び/又は更新している。
図13は、3次元グリッドマップデータGMが観測結果に応じて更新される様子をy軸方向からの側面図として模式的に表したものである。ロボット1の視野領域(Field of View)にないセル(斜線のセル)は、まだ測定していないセルを表す。ロボット1によって測定されたセルは黒塗りで表され、測定点にあたるセルとの間のセルは、白塗りで表されている。ハッチング領域は、未確認領域であることを示している。
The grid
The grid
FIG. 13 schematically shows how the three-dimensional grid map data GM is updated according to the observation result as a side view from the y-axis direction. Cells that are not in the field of view of the robot 1 (hatched cells) represent cells that have not yet been measured. The cells measured by the
グリッドマップ生成部33は、上述のアルゴリズムに基づいて、測定対象点であるセルpとロボットのステレオカメラ11L,11Rとを結ぶ線分の間に存在するセルに対して、エンプティ(Empty)処理を行う。続いて、測定点pにあたるセルに対してオキュパイド(Occupied)処理を行う。
3次元グリッドマップデータGMは、セルCに対して障害物の存在確率(障害物によって占有されている確率)p(c)を保持しており、このエンプティ処理及びオキュパイド処理は、これら各セルに対して統計的な処理である。
エンプティ処理は、障害物の存在確率を低下させる処理であり、オキュパイド処理は障害物の存在確率を増加させる処理である。本具体例では、エンプティ処理及びオキュパイド処理の存在確率を算出する手法の一例として、ベイズの更新則を用いる。
Based on the algorithm described above, the grid
The three-dimensional grid map data GM holds an obstacle existence probability (probability occupied by an obstacle) p (c) with respect to the cell C, and the empty process and the occupy process are performed on each cell. On the other hand, it is a statistical process.
The empty process is a process for reducing the existence probability of an obstacle, and the occupy process is a process for increasing the existence probability of an obstacle. In this specific example, Bayesian update rules are used as an example of a method for calculating the existence probability of the empty process and the occupy process.
グリッドマップ生成部33は、エンプティ処理において式(1)により占有確率を減少させ、オキュパイド処理において式(2)により占有確率を増加させる。
式(1)は、セルCの占有確率を示すp(c)が“占有”という条件下にあるセルCの確率を表しており、式(1)又は式(2)においてセルCが占有されているか又は占有されていないかを表す確率p(occ|・・・)、p(empty|・・・)は、予め決められた閾値thである。
The grid
Equation (1) represents the probability of cell C under the condition that p (c) indicating the occupation probability of cell C is “occupied”, and cell C is occupied in equation (1) or equation (2). The probabilities p (occ |...) And p (empty |...) That indicate whether or not they are occupied are predetermined threshold values th.
[数1]
p(c)←p(c|empty)=p(empty|c)p(c)/{p(empty|c)p(c)+p(empty|c~)p(c~)}
…(1)
[Equation 1]
p (c) ← p (c | empty) = p (empty | c) p (c) / {p (empty | c) p (c) + p (empty | c ~) p (c ~)}
... (1)
[数2]
p(c)←p(c|occ)=p(occ|c)p(c)/{p(occ|c)p(c)+p(occ|c~)p(c~)}
…(2)
[Equation 2]
p (c) ← p (c | occ) = p (occ | c) p (c) / {p (occ | c) p (c) + p (occ | c ~) p (c ~)}
... (2)
このように障害物の存在確率が与えられた3次元グリッドマップデータGMが示す内容を図14に示す。本具体例では、3次元グリッドとして表される領域は、例えば、4メートル四方、水平解像度4センチ及び垂直解像度1センチを1セルとした3次元グリッドで表現されている。 The contents indicated by the three-dimensional grid map data GM given the obstacle existence probability are shown in FIG. In this specific example, the area represented as a three-dimensional grid is represented by a three-dimensional grid, for example, 4 meters square, with a horizontal resolution of 4 cm and a vertical resolution of 1 cm as one cell.
図15では、説明の便宜上、図6に示す実空間S500と同じ領域が3次元グリッドとして表されているが、実際のロボット1は、例えば1秒間に30回というように所定間隔毎に周囲の状態を取得していくため、ロボット1が移動している場合には、3次元グリッドとして表現される空間S500は、時々刻々と変化することになる。取得されたセルは、見えたところ1、見えないところ0.5とする。占有確率は、1秒間に30回測定するうちに徐々に更新されていく。
In FIG. 15, for convenience of explanation, the same area as the real space S500 shown in FIG. 6 is represented as a three-dimensional grid. However, the
グリッドマップ生成部33が生成する3次元グリッドマップデータGMは、例えば、3次元座標(x、y、z)の各セルについて、その占有確率OG(x、y、z)(=p(c))を示している。
The three-dimensional grid map data GM generated by the grid
以下、グリッドマップ生成部33によるフロア高さデータFHの生成処理を説明する。
グリッドマップ生成部33は、2次元高さ地図(2D Height Map)のセルに対して、3次元グリッドマップデータGMから得られるセルの占有確率データ(障害物存在確率データ)を用いてフロア高さデータFHを検証する。
Hereinafter, generation processing of floor height data FH by the grid
The grid
本具体例では、グリッドマップ生成部33において、フロア高さデータFHを、3次元グリッドマップデータGMを用いて生成する。
3次元グリッドマップデータGMは、幾何学的に測定点と観測中心(ステレオカメラ中心)とを結ぶ直線上には、障害物は存在しないというアルゴリズムに基づいて作成及び/又は更新される。
グリッドマップ生成部33は、フロア高さデータFHのセルの高さ情報を3次元グリッドのセルに対応付けしたときの占有確率を3次元グリッドのセルの占有確率を参照して更新する処理を行う。
In this specific example, the grid
The three-dimensional grid map data GM is created and / or updated based on an algorithm that there is no obstacle on a straight line that geometrically connects the measurement point and the observation center (stereo camera center).
The grid
図16は、フロア高さデータFHを示す。
フロア高さデータFHは、矩形状格子で表される2次元グリッドの各セルに対して、高さ情報hの情報と無効フラグ(invalid Flag)とを保持している。
図16では、図6における平面PL501に対応するセルに高さh=30が、平面PL502に対応するセルに高さh=60が与えられている。また、このセルがロボット1には登行不可能な高さ情報をもっている場合には、無効フラグ(inv)が与えられる。
FIG. 16 shows the floor height data FH.
The floor height data FH holds height information h information and an invalid flag for each cell of a two-dimensional grid represented by a rectangular grid.
In FIG. 16, the height h = 30 is given to the cell corresponding to the plane PL501 in FIG. 6, and the height h = 60 is given to the cell corresponding to the plane PL502. If this cell has height information that the
フロア高さデータFHも3次元グリッドマップデータGMと同様に、例えば、4メートル四方、水平解像度4センチを1セルとした2次元グリッドで表現される。フロア高さデータFHでは、垂直方向の解像度は、考慮されておらず、各セルに対して高さ情報が割り当てられている。無効フラグとは、ロボット1に対して、障害物にあたることを示しているが、無効フラグを用意する代わりに、例えば、−10000などのように空間を表現するに非現実的な値を与えることで無効、すなわちロボット1に対する障害物であることを表してもよい。
Similarly to the three-dimensional grid map data GM, the floor height data FH is expressed by, for example, a two-dimensional grid having one cell of 4 meters square and a horizontal resolution of 4 cm. In the floor height data FH, the vertical resolution is not considered, and height information is assigned to each cell. The invalid flag indicates that the
図17は、グリッドマップ生成部33によるフロア高さデータFHの生成処理を説明するフローチャートである。
グリッドマップ生成部33は、対基準面高さ地図としてのフロア高さデータFHにおける高さ情報を取得し、この高さ情報を3次元グリッドマップデータGMの高さ座標に対応させ、対応した3次元グリッドのセルの占有確率が3次元グリッドにおいて障害物と認識するか否かを決めている閾値より大きいか否かを比較し、閾値より小さい場合、このセルは障害物でないと判断し、閾値より大きい場合、このセルは障害物であると判断し、対応する対基準面高さ地図の高さ情報を更新している。
FIG. 17 is a flowchart for describing generation processing of floor height data FH by the grid
The grid
グリッドマップ生成部33は、全セル(x、y)に対してフロア高さデータFHを参照してグリッドの高さを取得する(ステップST11)。
高さ情報hに無効フラグが立っていた場合、処理を終了する(ステップST12;yes)。
The grid
If the invalid flag is set in the height information h, the process is terminated (step ST12; yes).
一方、高さ情報hが有効であった場合、すなわち高さ情報hがあてはめられていた場合、高さ情報hを3次元グリッドに対応させて3次元グリッドにおけるインデックスkを求める(ステップST13)。続いて、3次元グリッド内のセル(x、y、z)の障害物の存在確率p(Cxyz)が3次元グリッドにおいて障害物と認識するか否かを決めている閾値thより大きいか否かを比較する(ステップST14)。 On the other hand, when the height information h is valid, that is, when the height information h is applied, the height information h is made to correspond to the three-dimensional grid to obtain an index k in the three-dimensional grid (step ST13). Subsequently, whether or not the obstacle existence probability p (Cxyz) of the cell (x, y, z) in the three-dimensional grid is larger than a threshold th that determines whether or not the obstacle is recognized in the three-dimensional grid. Are compared (step ST14).
閾値より小さい場合(ステップST14;no)、このセルは障害物でないと判断する。
このとき、グリッドマップ生成部33においてセル(x、y)を3次元グリッドマップデータGMを作成するアルゴリズムに基づいて更新する。閾値より大きい場合(ステップST14;yes)、このセルは障害物であると判断し処理を終了する。すなわち、この場合には、フロア高さデータFHの高さ情報hを更新しない。
If it is smaller than the threshold (step ST14; no), it is determined that this cell is not an obstacle.
At this time, the grid
以上の処理は、図13に示したように、グリッドのうち視野角領域に相当するセル領域
にのみ適用することで計算量の削減が可能となる。図13では、この視野角領域に相当するセル領域をバウンディングボックス(Bounding Box)と記している。
As shown in FIG. 13, the amount of calculation can be reduced by applying the above process only to the cell region corresponding to the viewing angle region in the grid. In FIG. 13, the cell region corresponding to this viewing angle region is described as a bounding box.
グリッドマップ生成部33は、平面検出部32による平面データS32と、3次元グリッドマップデータGMとからフロア高さデータFHを更新している。
ロボット1の視野領域にある計測空間内に含まれる全ての計測点(セル)pに対して、計測点pが属する平面が水平面であって、且つ計測点pに対応する3次元グリッドマップデータGMのセルの障害物存在確率が閾値thよりも高い場合に、計測点pに対応する高さ情報を更新する。
The grid
For all measurement points (cells) p included in the measurement space in the visual field region of the
グリッドマップ生成部33によるフロア高さデータFHの更新処理を図18に示す。
グリッドマップ生成部33は、まず、視野領域にある全計測点(セル)pに対して、計測点pが属する平面情報を取得する(ステップST21)。
平面情報としては、平面検出部32による平面検出結果である平面データS32を用いる。具体的には、計測点pが検出されたどの平面に属するかを表した平面ID(p_id)を取得し、このp_idが有効である場合には(ステップST22;yes)、このpidをキーとして検出された平面群のなかから計測点pが属する平面情報を検索する(ステップST23)。
The update processing of the floor height data FH by the grid
First, the grid
As the plane information, plane data S32 that is a plane detection result by the
次に、グリッドマップ生成部33は、計測点pが属する平面が水平面であるかどうかを検証し、水平面でない場合(ステップST24;no)には終了する。一方、水平面である場合(ステップST24;yes)、計測点pに対応する3次元グリッドのセルの障害物存在確率を参照する(ステップST25)。
具体的には、グリッドマップ生成部33は、フロア高さデータFHにおける計測点pの位置情報(x,y,z)から3次元グリッドの位置情報としてのインデックス(i,j,k)を求め、このインデックスに基づいて3次元グリッドのセルを参照する。
インデックス(i,j,k)を求める処理において、本実施例では、各計測点のノイズの影響を低減するため、高さ情報hとして、計測点pの実計測高さではなく、計測点pが属する平面の高さ(平均化された高さ;plane.m_z)を利用している。勿論、計測点pの高さをそのまま利用してもよい。
Next, the grid
Specifically, the grid
In the processing for obtaining the index (i, j, k), in this embodiment, in order to reduce the influence of noise at each measurement point, the height information h is not the actual measurement height of the measurement point p but the measurement point p. The height of the plane to which the file belongs (averaged height; plane.m_z) is used. Of course, the height of the measurement point p may be used as it is.
続いて、グリッドマップ生成部33は、計測点pに対応する高さ情報hを更新するか否かを決定する(ステップST26)。ここでは、計測点pに対応する3次元グリッドの占有確率が障害物が存在することを示す閾値thよりも大きい場合(ステップST26;yes)、フロア高さデータFHの対応するセル(i,j)を高さ情報hで更新する(ステップST27)。
Subsequently, the grid
フロア高さデータFHにおけるセル(i,j)の更新則としては、幾つかの方法があげられる。例えば、上書き更新とする場合、計測された最大値を保持する場合等があげられる。上書き更新とする場合には、セル(i,j)の高さを取得されたhによって常に上書きする。また、最大値を保持する場合には、セル(i,j)の高さが今までの高さ情報hより低いとき、又はセル(i,j)に無効フラグが立っている場合にのみ、新しい値hで更新する。このほかに、今までの高さ情報hと新しい値との平均をとってもよい。 There are several methods for updating the cell (i, j) in the floor height data FH. For example, when overwriting is updated, the measured maximum value is held, and the like. In the case of overwriting update, the height of the cell (i, j) is always overwritten by the acquired h. When the maximum value is held, only when the height of the cell (i, j) is lower than the height information h so far, or when the invalid flag is set in the cell (i, j). Update with new value h. In addition, the average of the height information h so far and the new value may be taken.
図19は、この一連の処理によって作成された環境地図を表している。上述したように、フロア高さデータFHを3次元グリッドマップデータGMによって補完する。環境地図は、2次元グリッドのセルに対して高さ情報h又は障害物(obs)か否かがあてはめられたものになっている。 FIG. 19 shows an environment map created by this series of processing. As described above, the floor height data FH is complemented by the three-dimensional grid map data GM. In the environment map, whether or not the height information h or an obstacle (obs) is applied to the cells of the two-dimensional grid.
グリッドマップ生成部33が生成するフロア高さデータFHは、例えば、2次元座標(x、y)の各々について、そのフロア高さFloor(x,y)を示している。
The floor height data FH generated by the grid
[環境クラス分類部34]
環境クラス分類部34は、グリッドマップ生成部33から3次元グリッドマップデータGMおよびフロア高さデータFHを入力し、これらを基に、2次元セル(x,y)に対応する位置を、例えば、障害物、でこぼこ、トンネル、未知、フロア、階段、境界部のいずれかに分類した環境クラス分類データである変数env_type(x,y)を生成し、これをパス決定部35に出力する。
[Environmental Class Classification Unit 34]
The environment
環境クラス分類部34は、先ず、フロア高さデータFHを基に、2次元座標(x、y)のセル(x、y)のフロア高さFloor(x,y)を取得し、これをhfに代入する。
なお、フロア高さFloor(x,y)=−∞(inv)の場合には、フロアが無い、あるいはフロアではないことを示している。
次に、環境クラス分類部34は、2次元座標(x、y)上の「z>hf」を満たす3次元座標(x、y、z)の占有確率OG(x、y、z)を、3次元グリッドマップデータGMから取得する。
そして、環境クラス分類部34は、上記セル(x、y)上記取得した占有確率OG(x、y、z)のうち、しきい値Poccを超えるものを特定し、そのうち最も高いzを障害物の高さを示すObstacle(x,y,hf)とする。
そして、上記セル(x、y)の「z>hf」の全てのzにおいて、占有確率OG(x、y、z)が所定しきい値Pocc(例えば、0.8)を超えていない場合に、Obstacle(x,y,hf)=−∞(障害ではない)とする。
The environment
Note that when the floor height Floor (x, y) = − ∞ (inv), there is no floor or no floor.
Next, the environment
Then, the environmental
When the occupation probability OG (x, y, z) does not exceed a predetermined threshold value Pocc (for example, 0.8) in all z of “z> hf” of the cell (x, y). , Obstable (x, y, hf) = − ∞ (not a failure).
また、環境クラス分類部34は、上記セル(x、y)の上記取得した占有確率OG(x、y、z)のうち、しきい値Poccを超えるものを特定し、そのうち最も低いzを天井の高さを示すCeiling(x,y,hf)とする。
そして、上記セル(x、y)の「z>hf」の全てのzにおいて、占有確率OG(x、y、z)が所定しきい値Pocc(例えば、0.8)を超えていない場合に、Ceiling(x,y,hf)=−∞(天井ではない)とする。
In addition, the environment
When the occupation probability OG (x, y, z) does not exceed a predetermined threshold value Pocc (for example, 0.8) in all z of “z> hf” of the cell (x, y). , Ceiling (x, y, hf) = − ∞ (not the ceiling).
また、環境クラス分類部34は、フロア高さデータFHが示す上記セル(x、y)のフロア高さFloor(x,y)と、当該セルに隣接する2次元座標(x’、y’)の隣接セル(x’、y’)のフロア高さFloor(x’,y’)との差分の絶対値の最大値をΔh(x、y)として算出する。
The environment
図20は、環境クラス分類部34による各セル(x、y)の環境クラス分類処理を説明するためのフローチャートである。
先ず、環境クラス分類部34は、上述したように、グリッドマップ生成部33から入力した3次元グリッドマップデータGMおよびフロア高さデータFHを基に、各セル(x、y)の上記フロア高さFloor(x,y)、上記Obstacle(x,y,hf)、Ceiling(x,y,hf)をそれぞれ取得あるいは生成し、それぞれを変数f,o,cに代入する(ステップST41)。
FIG. 20 is a flowchart for explaining the environment class classification process of each cell (x, y) by the environment
First, the environment
次に、環境クラス分類部34は、「変数o>f」の条件を満たすか否かを判断し、満たすと判断するとステップST43に進み、そうでない場合にはステップST51に進む(ステップST42)。
ここで、条件「変数o>f」を満たさない場合(ステップST51に進む場合)は、例えば、「Obstacle(x,y,hf)=−∞(障害ではない)」と設定された場合である。
Next, the environment
Here, when the condition “variable o> f” is not satisfied (when the process proceeds to step ST51), for example, “Obscale (x, y, hf) = − ∞ (not a failure)” is set. .
次に、環境クラス分類部34は、「変数fがinv(無効)ではない」且つ「c>f+d_tunnel」の条件を満たすか否かを判断し(ステップST43)、満たすと判断すると、セル(x、y)はトンネルであると判定し(ステップST44)、そうでない場合にはステップST45に進む。
ここで、「d_tunnel」は、ロボット1が、天井下において這いながら走行可能な最小高さを示している。すなわち、トンネル内の最小高さを示している。
Next, the environment
Here, “d_tunnel” indicates the minimum height at which the
そして、環境クラス分類部34は、「変数oが所定のしきい値を超えていない」という条件を満たすか否かを判断し、当該条件を満たすと判断すると、セル(x,y)は荒れた面(clutter)であると判定する(ステップST47)。
ここで、「変数oが所定のしきい値を超えていない」という条件は、例えば、変数oが示す障害物の高さが、ロボット1が乗り越えられた程度に小さい場合に満たされる。
一方、環境クラス分類部34は、「変数oが所定のしきい値を超えていない」という条件を満たさないと判断すると、セル(x,y)は障害物(obstacle)であると判定する(ステップST46)。
なお、上記荒れた面は、ロボット1が簡単に乗り越えることができる程度の障害しかない面を意味している。
また、上記障害物は、ロボット1が走行できない程度に大きい障害物を意味している。
Then, the environment
Here, the condition that the variable o does not exceed a predetermined threshold is satisfied, for example, when the height of the obstacle indicated by the variable o is small enough to get over the
On the other hand, if the environment
The rough surface means a surface having only an obstacle that the
The obstacle means an obstacle that is large enough that the
環境クラス分類部34は、ステップST51において、変数fが無効(inv)であるか否かを判断し(ステップST51)、無効であると判断するとセル(x、y)は未知(unknown)であると判断し(ステップST52)、無効ではないと判断するとステップST53に進む。
In step ST51, the environment
環境クラス分類部34は、ステップST53において、前述したように、セル(x、y)のフロア高さFloor(x,y)と、当該セルに隣接する2次元座標(x’、y’)の隣接セル(x’、y’)のフロア高さFloor(x’,y’)との差分の絶対値の最大値をΔh(x、y)として算出する。そして、環境クラス分類部34は、変数dに上記算出したΔh(x、y)を代入する(ステップST52)。
次に、環境クラス分類部34は、条件「d<d_step」を満たすか否かを判断し、満たすと判断すると、セル(x、y)はフロア(floor)であると判定し(ステップST55)、そうでない場合にはステップST56に進む。
ここで、「d_step」は、ロボット1が通常歩行できる段差の最大高さを示し、予め設定される。
また、フロアは、ロボット1が容易に通常歩行できる平面である。
In step ST53, as described above, the environmental
Next, the environment
Here, “d_step” indicates the maximum height of the step where the
The floor is a plane on which the
環境クラス分類部34は、ステップST54において、条件「d<d_stairs」を満たすか否かを判断し、満たすと判断すると、セル(x、y)は階段(stairs)であると判定し(ステップST57)、そうでない場合にはセル(x,y)は境界部(border)であると判定する(ステップST58)。
ここで、「d_stairs」は、ロボット1が階段上り歩行可能な段差の高さの最大値を示し、予め設定される。
In step ST54, the environmental
Here, “d_stays” indicates the maximum value of the height of the step where the
環境クラス分類部34による上述した処理により、3次元座標(x,y,z)で規定されるセルの状態が、例えば、図21に示すように分類(判定)される。
環境クラス分類部34は、上述したように、全セル(x,y)について判定した状態の結果を変数env_type(x,y)に格納し、これをパス決定部36に出力する。
なお、環境クラス分類部34は、変数env_type(x,y)をパス決定部36に出力する前に、所定のメモリ(例えば、図4に示すRAR1312や外部記憶装置314)に書き込み、それを読み出してパス決定部36に出力する。
By the above-described processing by the environment
As described above, the environment
The environment
なお、上述した例では、セル(x、y)の状態として、floor、obstacle,unknown,stairs,border,tunnel,clutterを例示したが、それ以外にも、環境クラス分類部34は、障害物が接近した狭い領域を意味するnarrow、傾斜面を意味するramp、人間等を意味するhuman、階段を意味するladder、通常のドアを意味するdoor、エレベータを意味するelevator、エスカレータを意味するescalatorなどのその他の状態を判定してもよい。
In the example described above, the state of the cell (x, y) is exemplified as floor, obstable, unknown, stays, border, tunnel, and clutter. However, in addition to the above, the environment
[動作機能部]
CPU15は、例えば、デフォルトとして、通常走行処理部35_1、階段走行処理部35_2、注意走行処理部35_3、這動作処理部35_4、ステップオーバ処理部35_5、よじ登り/ジャンプ処理部35_6を、例えば、プログラムを実行して実現する。
上記処理部35_1〜35_6の各々には、例えば、(name、info,types,cost,sensors,excuter)という属性データが割り当てられている。
nameは、その処理部(モジュール)の名前を示す。
infoは、その処理部の機能の記述、例えば、プログラムコードを示す。
typesは、その処理部によって対応可能なセルの状態を示す。
costは、ロボット1が移動する最適パス(経路)を決定する際に、その処理部の処理に割り当てるコストを示す。当該コストは、その処理部の制御による動作をロボット1が行った場合に、その位置を通過するのに要する時間に比例した値を持つ。
sensorsは、その処理部の処理に入力として必要なセンサを示す。
excuteは、その処理部の処理を実行するシステム(ハードウェア)を示す。
[Operation function section]
For example, the
For example, attribute data (name, info, types, cost, sensors, excuter) is assigned to each of the processing units 35_1 to 35_6.
“name” indicates the name of the processing unit (module).
info indicates a description of the function of the processing unit, for example, a program code.
“types” indicates a state of a cell that can be handled by the processing unit.
“cost” indicates the cost assigned to the processing of the processing unit when determining the optimum path (route) for the
Sensors indicates a sensor necessary as an input for processing of the processing unit.
Execute indicates a system (hardware) that executes the processing of the processing unit.
通常走行処理部35_1は、例えば、ロボット1が平坦なフロアを2足歩行する一連の動作を動き制御部37に指示する処理を行う。
階段走行処理部35_2は、例えば、ロボット1が階段を2足歩行する一連の動作を動き制御部37に指示する処理を行う。
注意走行処理部35_3は、例えば、ロボット1がセンサ群17からの検出信号を基に表面状態を確認しながら、ゆっくりとした動作で慎重に歩行する一連の動作を動き制御部37に指示する処理を行う。
這動作処理部35_4は、例えば、高い障害物がある場合に、ロボット1が腰を屈めて頭を避けて歩いたり、膝をついて歩いたり、四足で這いつくばって歩く一連の這動作を制御する。
ステップオーバ処理部35_5は、ロボット1が小さい障害物を跨いで移動する一連の動作を動き制御部37に指示する。
よじ登り/ジャンプ処理部35_6は、ロボット1が障害物を手を利用してよじ登ったり、境界部をジャンプしたりする一連の動作を動き制御部37に指示する。
For example, the normal travel processing unit 35_1 performs a process of instructing the
The staircase traveling processing unit 35_2 performs, for example, processing for instructing the
The attention travel processing unit 35_3 is a process for instructing the
For example, when there is a high obstacle, the heel motion processing unit 35_4 controls a series of heel motions in which the
The step over processing unit 35_5 instructs the
The climbing / jump processing unit 35_6 instructs the
CPU15が実現する処理部としては、上述したものの他にも、横方向にゆっくり歩く動作(sideways)を規定する処理部、人間と会話をする動作(dialog)を規定する処理部、障害物を移動する動作(manupulate)を規定する処理部、ドアを開ける動作(open door)を規定する処理部、傾斜面を移動する動作(slide)を規定する処理部、エレベータを使う動作(use elevator)を規定する処理部、エスカレータを使う動作(use escalator)を規定する処理部などがある。
As the processing unit realized by the
CPU15は、上述した処理部のうち、何れかを予めデフォルトとして備え、それらはユーザからの指示などにより削除可能である。
また、CPU15は、デフォルトとして備えた処理部に加えて、新たな処理部を追加可能である。
The
Further, the
[パス決定部36]
パス決定部36は、環境クラス分類部34から入力した、全セル(x,y)について判定した状態の結果を示す変数env_type(x,y)と、上述した利用可能な動作機能部のリストとに基づいて、ロボット1が現在地から目的地に移動する最適なパスを決定する。
また、パス決定部36は、利用可能な動作機能部のリストを常時更新する。
また、パス決定部36は、上記決定したパスとロボット1の移動状況に基づいて、適切な処理部を順次選択し、ロボット1の動作を制御させる。
[Path determination unit 36]
The
The
In addition, the
先ず、パス決定部36による最適パスの決定処理を説明する。
図22は、図5に示すパス決定部36による最適パスの決定処理を説明するためのフローチャートである。
パス決定部36は、利用可能機能リストを参照して、利用可能な処理部を特定する(ステップST61)。
本実施形態では、一例として、図5に示すように、利用可能な処理部が、通常走行処理部35_1、階段走行処理部35_2、注意走行処理部35_3、這動作処理部35_4、ステップオーバ処理部35_5、並びによじ登り/ジャンプ処理部35_6である場合を説明する。
First, the optimum path determination process by the
FIG. 22 is a flowchart for explaining optimum path determination processing by the
The
In this embodiment, as an example, as illustrated in FIG. 5, usable processing units are a normal traveling processing unit 35_1, a staircase traveling processing unit 35_2, a caution traveling processing unit 35_3, a saddle movement processing unit 35_4, and a step over processing unit. The case of 35_5 and the climbing / jump processing unit 35_6 will be described.
次に、パス決定部36は、環境クラス分類部34から入力した変数env_type(x,y)を基に、ロボット1の現在位置から目標位置に達する複数のパス(経路)を特定する(ステップST62)。
Next, the
次に、パス決定部36は、ステップST62で特定した複数のパスの各々について、そのパス上の座標(x、y)と、その変数env_type(x,y)が示す状態と、その状態を移動するロボット1の機能部に割り当てられた上記コストcostとを基に、コストを算出する(ステップST63)。
次に、パス決定部36は、ステップST62で特定した複数のパスのうち、ステップST63で算出したコストが最小のパスを決定する(ステップST64)。
具体的には、パス決定部36は、上記複数のパスの各々について、当該パス上の座標(x,y)に対応する上記コストcostの総和を計算し、その総和が最小のパスを決定する。
Next, the
Next, the
Specifically, the
以下、パス決定部36による利用可能機能リストの更新処理を説明する。
図23は、図5に示すパス決定部36による利用可能機能リスト更新処理を説明するためのフローチャートである。
パス決定部36は、ロボット1の動作を規定する新たな処理部(機能)が追加されたか否かを判断し(ステップST61)、追加されたと判断するとステップST62に進み、そうでない場合にはステップST63に進む。
パス決定部36は、利用可能機能リストに、新たに追加された機能(処理部)のエントリを追加する(ステップST62)。
ここで、ロボット1のCPU15への上記機能の追加は、プログラムモジュールの追加や、あるいは専用ハードウェアの追加によって実現される。
Hereinafter, the update process of the available function list by the
FIG. 23 is a flowchart for explaining an available function list update process by the
The
The
Here, the addition of the above functions to the
パス決定部36は、利用可能機能リストのエントリとなっている機能が削除されたか、あるいは、故障したかを判断し(ステップST63)、そうであると判断するとステップST64に進み、そうではないと判断するとステップST61に戻る。
パス決定部36は、利用可能機能リストから、上記削除あるいは故障した機能のエントリを削除する(ステップST64)。
The
The
以下、パス決定部36による処理部の選択処理を説明する。
図24および図25は、図5に示すパス決定部36による処理部の選択処理を説明するためのフローチャートである。
パス決定部36は、動き制御部37の制御の進行状況、CCDカメラ10R,10Lの撮像結果、並びにセンサ群27からの検出信号の少なくとも一つに基づいて、ロボット1が次に移動するセル(x、y)を特定する(ステップST71)。
次に、パス決定部36は、ステップST71で特定したセル(x、y)の変数env_type(x,y)を変数tに代入する(ステップST72)。
Hereinafter, processing unit selection processing by the
24 and 25 are flowcharts for explaining processing unit selection processing by the
The
Next, the
次に、パス決定部36は、変数tがfloorを示すか否かを判断し(ステップST73)、floorを示す場合に通常走行処理部35_1に対して動作指示を出力し(ステップST74)、そうでない場合にステップST75に進む。
ステップST74において、動き制御部37が、通常走行処理部35_1からの指示に基づいて、ロボット1に通常走行動作を行わせるようにアクチュエータ群16を制御する。
Next, the
In step ST74, the
次に、パス決定部36は、変数tがstairsを示すか否かを判断し(ステップST75)、stairsを示す場合に階段走行処理部35_2に対して動作指示を出力し(ステップST76)、そうでない場合にステップST77に進む。
ステップST76において、動き制御部37が、階段走行処理部35_2からの指示に基づいて、ロボット1に階段走行動作を行わせるようにアクチュエータ群16を制御する。
Next, the
In step ST76, the
次に、パス決定部36は、変数tがunknownを示すか否かを判断し(ステップST77)、unknownを示す場合に注意走行処理部35_3に対して動作指示を出力し(ステップST78)、そうでない場合にステップST79に進む。
ステップST78において、動き制御部37が、注意走行処理部35_3からの指示に基づいて、ロボット1に注意走行動作を行わせるようにアクチュエータ群16を制御する。
Next, the
In step ST78, the
次に、パス決定部36は、変数tがtunnelを示すか否かを判断し(ステップST79)、tunnelを示す場合に這動作処理部35_4に対して動作指示を出力し(ステップST80)、そうでない場合にステップST81に進む。
ステップST80において、動き制御部37が、這動作処理部35_4からの指示に基づいて、ロボット1に這動作を行わせるようにアクチュエータ群16を制御する。
Next, the
In step ST80, the
次に、パス決定部36は、変数tがclutterを示すか否かを判断し(ステップST81)、clutterを示す場合にステップオーバ処理部35_5に対して動作指示を出力し(ステップST82)、そうでない場合にステップST83に進む。
ステップST82において、動き制御部37が、ステップオーバ処理部35_5からの指示に基づいて、ロボット1にステップオーバ動作を行わせるようにアクチュエータ群16を制御する。
Next, the
In step ST82, the
次に、パス決定部36は、変数tがborderを示すか否かを判断し(ステップST83)、borderを示す場合によじ登り/ジャンプ処理部35_6に対して動作指示を出力し(ステップST84)、そうでない場合にステップST85に進む。
ステップST84において、動き制御部37が、よじ登り/ジャンプ処理部35_6からの指示に基づいて、ロボット1によじ登り動作あるいはジャンプ動作を行わせるようにアクチュエータ群16を制御する。
Next, the
In step ST84, the
次に、パス決定部36は、動き制御部37の制御の進行状況、CCDカメラ10R,10Lの撮像結果、並びにセンサ群27からの検出信号の少なくとも一つに基づいて、目的地に到達したか否かを判断し(ステップST85)、目的地に到達したと判断すると処理を終了し、そうでない場合にはステップST71に戻る。
Next, whether the
パス決定部36は、例えば、図26に示すように、領域AR1,AR3,AR5はfloorであり通常走行処理部35_1に制御を行わせ、領域AR2はstairsであり階段走行処理部35_2に制御を行わせ、領域AR4は境界部でありよじ登り/ジャンプ処理部35_6に制御を行わせ、領域AR6はunknownであり注意走行処理部35_3に制御を行わせる。
また、パス決定部36は、例えば、図27に示すように、ロボット1の現在位置(Robot)から目的地(Goal)までの最適パスを決定する。
For example, as shown in FIG. 26, the
In addition, for example, as illustrated in FIG. 27, the
[動き制御部37]
動き制御部37は、通常走行処理部35_1、階段走行処理部35_2、注意走行処理部35_3、這動作処理部35_4、ステップオーバ処理部35_5、よじ登り/ジャンプ処理部35_6などからの指示に基づいて、アクチュエータ群16を駆動し、所望の動作をロボット1に実行させる。
[Motion control unit 37]
The
[ロボット1の全体動作]
以下、図1に示すロボット1の全体動作例を説明する。
CCDカメラ10R,10Lが、それぞれ撮像結果に応じた右画像データS10Rおよび左画像データS10Lをステレオ画像処理装置12に出力する。
そして、ステレオ画像処理装置12が、CCDカメラ10R,10Lから入力した右画像データS10Rと左画像データS10Lの視差情報(disparity data)(距離情報)を計算し、カラー画像データ(YUV:輝度Y、UV色差)S12a及び視差画像データ(YDR:輝度Y、視差D、信頼度R)S12bをフレーム毎に左右交互に算出し、これをCPU15(図5に示す距離データ生成部31)に出力する。
[Whole movement of robot 1]
Hereinafter, an example of the overall operation of the
The
Then, the stereo
次に、距離データ生成部31が、ステレオ画像処理装置12から入力したカラー画像データS12a及び視差画像データS12bを基に、画像(環境)内の対象物についての3次元の距離データ(ステレオデータ)S31を生成し、これを平面検出部32およびグリッドマップ生成部33に出力する。
次に、平面検出部32は、距離データ生成部31から入力した距離データS31から環境内に存在する複数の平面を検出し、それを示す平面データS32をグリッドマップ生成部33に出力する。
次に、グリッドマップ生成部33が、距離データ生成部31から入力した距離データS31と平面検出部32から入力した上記平面検出データS32とに基づいて、3次元グリッドマップデータGMおよびフロア高さデータFHを生成し、これらを環境クラス分類部34に出力する。
Next, the distance
Next, the
Next, based on the distance data S31 input from the distance
次に、環境クラス分類部34が、グリッドマップ生成部33から3次元グリッドマップデータGMおよびフロア高さデータFHを入力し、これらを基に、2次元セル(x,y)に対応する位置を、例えば、障害物、でこぼこ、トンネル、未知、フロア、階段、境界部のいずれかに分類した環境クラス分類データである変数env_type(x,y)を生成し、これをパス決定部35に出力する。
次に、パス決定部36が、環境クラス分類部34から入力した、全セル(x,y)について判定した状態の結果を示す変数env_type(x,y)と、上述した利用可能な動作機能部のリストとに基づいて、ロボット1が現在地から目的地に移動する最適なパスを決定する。
また、パス決定部36は、利用可能な動作機能部のリストを常時更新する。
また、パス決定部36は、上記決定したパスとロボット1の移動状況に基づいて、適切な処理部35_1〜35_6を順次選択する。
そして、選択された処理部35_1〜35_6が動き制御部37に指示を出す。
次に、動き制御部37が、上記選択された処理部35_1〜35_6からの指示に基づいて、アクチュエータ群16を駆動し、所望の動作をロボット1に実行させる。
Next, the environment
Next, the variable env_type (x, y) indicating the result of the determination for all the cells (x, y) input from the environment
The
Further, the
Then, the selected processing units 35_1 to 35_6 issue instructions to the
Next, the
以上説明したように、ロボット1では、図5等に示すように、CPU15が機能として利用可能な処理部35_1〜35_6等を特定し(図22に示すステップST61)、その利用な処理部を基に、目標位置までの複数のパスを動的に特定する(ステップST62)。
これにより、例えば、処理部35_1〜35_6あるいはそれによって制御されるアクチュエータ群16などの機械部位が故障した場合に、その故障した機能に対応した処理部による制御を利用しないで目標位置に達するパスを選択することができる。
また、新たな処理部を追加して利用可能にした場合に、その追加した処理部の機能を利用してロボット1が走行可能なパスを選択枠に入れることができる。
また、ロボット1は、それが位置する環境に応じて利用可能な処理部を動的に変更して最適パスを決定することも可能である。
また、ロボット1では、上記複数のパスの各々について、そのパス上の2次元座標の変数env_type(x,y)と、それに対応付けられたコスト(時間コスト)とを基に、コストの和を最小にするパスを選択するため、最短時間で到達できるパスを高い信頼性で選択できる。
As described above, in the
Thereby, for example, when a machine part such as the processing units 35_1 to 35_6 or the
Further, when a new processing unit is added and made available, a path that can be traveled by the
In addition, the
Further, in the
また、ロボット1では、環境クラス分類部34において、3次元グリッドマップデータGMおよびフロア高さデータFHを基に、2次元座標(x、y)の各々についてその状態を分類した変数env_type(x,y)を生成する。
そして、パス決定部36において、ロボット1の移動過程において、ロボット1の2次元位置(x、y)に応じて、それに対応した上記状態を特定し、その状態に予め対応付けられた処理部35_1〜35_6を選択してロボット1の動作を駆動させる。
ロボット1では、上述したように、予め2次元座標(x、y)の状態を分類した変数env_type(x,y)を生成し、これを図4に示すRAM312などに記憶する。
これにより、ロボット1の動作時に、パス決定部36は、環境クラス分類部34から変数env_type(x,y)を読み出すだけで、ロボット1が次に移動する位置の状態を取得でき、それを基にロボット1の次の動作を短時間で決定できると共に、処理負荷を従来に比べて軽減できる。
また、ロボット1では、次元座標(x、y)の状態を分類した変数env_type(x,y)をRAM312などのメモリに記憶していれば十分であり、変数env_type(x,y)を生成後に、グリッドマップデータGMおよびフロア高さデータFHを保持する必要が無く、メモリを効率的に使用できる。
In the
Then, the
As described above, the
As a result, during the operation of the
In the
本発明は上述した実施形態には限定されない。
すなわち、当業者は、本発明の技術的範囲またはその均等の範囲内において、上述した実施形態の構成要素に関し、様々な変更、コンビネーション、サブコンビネーション、並びに代替を行ってもよい。
例えば、上述した実施形態では、本発明の移動体として、2足歩行のロボット1を例示したが、移動体として、例えば、車輪による移動体、あるいは1足や3足以上のロボットを用いてもよい。
The present invention is not limited to the embodiment described above.
That is, those skilled in the art may make various modifications, combinations, subcombinations, and alternatives regarding the components of the above-described embodiments within the technical scope of the present invention or an equivalent scope thereof.
For example, in the above-described embodiment, the
10L,10R…CCDカメラ、12…ステレオ画像処理装置、15…CPU、16…アクチュエータ群、17…センサ群、31…距離データ生成部、32…平面検出部、33…グリッドマップ生成部、34…環境クラス分類部、35_1…通常走行処理部、35_2…階段走行処理部、35_3…注意走行処理部、35_4…這動作処理部、35_5…ステップオーバ処理部、35_6…よじ登り/ジャンプ処理部、36…パス決定部、37…動き制御部
10L, 10R ... CCD camera, 12 ... stereo image processing device, 15 ... CPU, 16 ... actuator group, 17 ... sensor group, 31 ... distance data generation unit, 32 ... plane detection unit, 33 ... grid map generation unit, 34 ... Environmental class classification unit, 35_1 ... normal travel processing unit, 35_2 ... stair travel processing unit, 35_3 ... caution travel processing unit, 35_4 ... saddle motion processing unit, 35_5 ... step over processing unit, 35_6 ... climbing / jump processing unit, 36 ... Path determination unit, 37 ... motion control unit
Claims (11)
前記移動体の現在位置から前記目標位置までのパスの状態を検出する第1の工程と、
前記移動体が実行可能な複数の移動動作を特定する第2の工程と、
前記第1の工程で検出したパスの状態に基づいて、前記第2の工程で特定した前記移動体が実行可能な複数の種類の移動動作のうち、前記移動体の位置の状態に適合した移動動作を順次選択し、当該選択した移動動作を前記移動体が行うように制御する第3の工程と
を有する移動体制御方法。 A moving body control method for controlling movement of a moving body to a target position,
A first step of detecting a state of a path from the current position of the moving body to the target position;
A second step of identifying a plurality of movement operations that the movable body can execute;
Based on the state of the path detected in the first step, the movement suitable for the state of the position of the moving body among a plurality of types of moving operations that can be executed by the moving body specified in the second step And a third step of controlling the selected moving operation so that the moving body performs the selected moving operation.
をさらに有し、
前記第2の工程は、前記第4の工程で追加または削除された後の前記実行可能な移動動作を特定する
請求項1に記載の移動体制御方法。 A fourth step of adding or deleting the moving operation executable by the moving body,
The moving body control method according to claim 1, wherein the second step specifies the executable movement operation after being added or deleted in the fourth step.
前記移動体の現在位置から前記目標位置までの複数のパスの各々について、当該パスを前記移動体を走行した場合の走行コストの総和を計算し、当該総和を最小にする前記パスを、前記移動体が移動するパスとして選択する
請求項1に記載の移動体制御方法。 The third step includes
For each of a plurality of paths from the current position of the mobile body to the target position, the total of travel costs when traveling the mobile body on the path is calculated, and the path that minimizes the total is transferred to the path. The moving body control method according to claim 1, wherein the moving body is selected as a path along which the body moves.
前記第3の工程は、前記移動体の位置について前記判定した状態に予め対応付けられた移動動作を選択する
請求項1に記載の移動体制御方法。 The first step determines the state of the position on the path based on the imaging results of the imaging target regions from different imaging positions,
The moving body control method according to claim 1, wherein the third step selects a moving action previously associated with the determined state with respect to the position of the moving body.
撮像対象領域内に規定された各3次元グリッドの対象物による占有状態を示す3次元グリッドデータを生成し、
前記生成した3次元グリッドデータを基に、前記3次元グリッドの2次元位置の各々について、予め決められた複数の状態のうち何れの状態であるかを判定し、
当該判定した状態に応じて前記パスの状態を検出する
請求項1に記載の移動体制御方法。 The first step includes
Generating 3D grid data indicating the occupation state of each 3D grid object defined in the imaging target area;
Based on the generated three-dimensional grid data, for each of the two-dimensional positions of the three-dimensional grid, it is determined which of a plurality of predetermined states,
The moving body control method according to claim 1, wherein the state of the path is detected according to the determined state.
請求項5に記載の移動体制御方法。 The movement according to claim 5, wherein the first step determines whether each of the two-dimensional positions is a floor or a staircase on which the movable body can move and an obstacle on which the movable body cannot move. Body control method.
前記複数の種類の移動動作は、通常走行、階段走行、注意走行、這動作、ステップオーバ、よじ登り動作、並びにジャンプ動作のうち何れかである
請求項1に記載の移動体制御方法。 The moving body is a humanoid robot having two legs,
The moving body control method according to claim 1, wherein the plurality of types of movement operations are any one of normal travel, stair travel, caution travel, dredging operation, step over, climbing operation, and jump operation.
走行手段と、
前記撮像手段が生成した前記撮像結果を基に、前記移動体の現在位置から前記目標位置までのパスの状態を検出し、前記移動体が実行可能な複数の移動動作を特定し、前記検出したパスの状態に基づいて、前記第2の工程で特定した前記移動体が実行可能な複数の種類の移動動作のうち、前記移動体の位置の状態に適合した移動動作を順次選択し、当該選択した移動動作を前記移動体が行うように前記走行手段を制御する制御手段と
を有する移動体。 Imaging means for generating imaging results of imaging target regions from different imaging positions;
Traveling means;
Based on the imaging result generated by the imaging means, the state of the path from the current position of the moving body to the target position is detected, and a plurality of moving operations that can be executed by the moving body are identified and detected. Based on the state of the path, from among a plurality of types of moving operations that can be executed by the moving body specified in the second step, sequentially select a moving operation that matches the position state of the moving body, and perform the selection. And a control means for controlling the traveling means so that the moving body performs the moving operation.
請求項8に記載の移動体。 The moving body according to claim 8, wherein the control unit adds or deletes the moving operation that can be executed by the moving body.
請求項8に記載の移動体。 The control means calculates, for each of a plurality of paths from the current position of the moving body to the target position, a sum of traveling costs when traveling on the path along the path, and minimizes the sum. The moving body according to claim 8, wherein a path is selected as a path along which the moving body moves.
人間型のロボットである
請求項8に記載の移動体。
The traveling means is two legs,
The moving body according to claim 8, which is a humanoid robot.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2005222179A JP2007041657A (en) | 2005-07-29 | 2005-07-29 | Moving body control method, and moving body |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2005222179A JP2007041657A (en) | 2005-07-29 | 2005-07-29 | Moving body control method, and moving body |
Publications (1)
Publication Number | Publication Date |
---|---|
JP2007041657A true JP2007041657A (en) | 2007-02-15 |
Family
ID=37799609
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2005222179A Pending JP2007041657A (en) | 2005-07-29 | 2005-07-29 | Moving body control method, and moving body |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP2007041657A (en) |
Cited By (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2010009236A (en) * | 2008-06-25 | 2010-01-14 | Toyota Central R&D Labs Inc | Plane area estimation device and program |
JP2014078254A (en) * | 2009-08-31 | 2014-05-01 | Neato Robotics Inc | Method and apparatus for simultaneous localization and mapping of mobile robot environment |
JP2018142316A (en) * | 2017-02-28 | 2018-09-13 | トヨタ自動車株式会社 | Observability grid based autonomous environment search |
JP2019053367A (en) * | 2017-09-13 | 2019-04-04 | 株式会社アルファメディア | Image processing device and movement support device |
CN110941258A (en) * | 2018-09-21 | 2020-03-31 | 日本电产株式会社 | Control method and control system for mobile body |
US10816974B2 (en) | 2017-02-28 | 2020-10-27 | Toyota Jidosha Kabushiki Kaisha | Proactive acquisition of data for maintenance of appearance model by mobile robot |
CN114243544A (en) * | 2022-02-25 | 2022-03-25 | 国网辽宁省电力有限公司电力科学研究院 | Robot-based GIS vertical cavity overhauling system and overhauling method |
-
2005
- 2005-07-29 JP JP2005222179A patent/JP2007041657A/en active Pending
Cited By (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2010009236A (en) * | 2008-06-25 | 2010-01-14 | Toyota Central R&D Labs Inc | Plane area estimation device and program |
JP2014078254A (en) * | 2009-08-31 | 2014-05-01 | Neato Robotics Inc | Method and apparatus for simultaneous localization and mapping of mobile robot environment |
JP2018142316A (en) * | 2017-02-28 | 2018-09-13 | トヨタ自動車株式会社 | Observability grid based autonomous environment search |
US10754351B2 (en) | 2017-02-28 | 2020-08-25 | Toyota Jidosha Kabushiki Kaisha | Observability grid-based autonomous environment search |
US10816974B2 (en) | 2017-02-28 | 2020-10-27 | Toyota Jidosha Kabushiki Kaisha | Proactive acquisition of data for maintenance of appearance model by mobile robot |
JP2019053367A (en) * | 2017-09-13 | 2019-04-04 | 株式会社アルファメディア | Image processing device and movement support device |
JP7101350B2 (en) | 2017-09-13 | 2022-07-15 | 株式会社アルファメディア | Image processing device and movement support device |
CN110941258A (en) * | 2018-09-21 | 2020-03-31 | 日本电产株式会社 | Control method and control system for mobile body |
CN114243544A (en) * | 2022-02-25 | 2022-03-25 | 国网辽宁省电力有限公司电力科学研究院 | Robot-based GIS vertical cavity overhauling system and overhauling method |
CN114243544B (en) * | 2022-02-25 | 2022-05-20 | 国网辽宁省电力有限公司电力科学研究院 | Robot-based GIS vertical cavity overhauling system and overhauling method |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP2007041656A (en) | Moving body control method, and moving body | |
JP4479372B2 (en) | Environmental map creation method, environmental map creation device, and mobile robot device | |
JP3994950B2 (en) | Environment recognition apparatus and method, path planning apparatus and method, and robot apparatus | |
JP4636016B2 (en) | Plane detection apparatus, plane detection method, and robot apparatus equipped with plane detection apparatus | |
JP4618247B2 (en) | Robot apparatus and operation control method thereof | |
US7769491B2 (en) | Obstacle avoiding apparatus, obstacle avoiding method, obstacle avoiding program, and mobile robot apparatus | |
KR102192791B1 (en) | Mobile robot and control method of mobile robot | |
CN112666939B (en) | Robot path planning algorithm based on deep reinforcement learning | |
US7386163B2 (en) | Obstacle recognition apparatus and method, obstacle recognition program, and mobile robot apparatus | |
JP2007041657A (en) | Moving body control method, and moving body | |
KR20210068446A (en) | Terrain Awareness Step Planning System | |
CN114503043A (en) | Restricted mobility mapping | |
JP2009165823A (en) | Path planning method of mobile robot and device thereof | |
CN112097769A (en) | Homing pigeon brain-hippocampus-imitated unmanned aerial vehicle simultaneous positioning and mapping navigation system and method | |
CN115143964A (en) | Four-footed robot autonomous navigation method based on 2.5D cost map | |
Chignoli et al. | Rapid and reliable quadruped motion planning with omnidirectional jumping | |
Gao et al. | A vision-based irregular obstacle avoidance framework via deep reinforcement learning | |
KR102246236B1 (en) | How to update a map in Fusion Slam and a robot that implements it | |
US20200293051A1 (en) | Route determination method | |
CN107970610B (en) | Planning method and device for 3D scene vertical space flight route | |
Baek et al. | A Mobile Robot Framework in Industrial Disaster for Human Rescue | |
JP2003346150A (en) | Floor surface recognition device, floor surface recognition method, and robot device | |
Belter et al. | On-Board Perception and Motion Planning for Legged Locomotion over Rough Terrain. | |
WO2023021734A1 (en) | Movement device, movement device control method, and program | |
JP7250573B2 (en) | Inverted pendulum type robot |