JP6260509B2

JP6260509B2 - robot

Info

Publication number: JP6260509B2
Application number: JP2014205561A
Authority: JP
Inventors: 国松橋本
Original assignee: Toyota Motor Corp
Current assignee: Toyota Motor Corp
Priority date: 2014-10-06
Filing date: 2014-10-06
Publication date: 2018-01-17
Anticipated expiration: 2034-10-06
Also published as: JP2016074053A

Description

本発明はロボットに関し、特に、物体との距離情報に基づいて自己位置を推定するロボットに関する。 The present invention relates to a robot, and more particularly to a robot that estimates its own position based on distance information with respect to an object.

環境地図を参照することにより、障害物等を避けて移動するロボットが知られている。また、このようなロボットにおいて、距離センサを用いて物体までの距離を計測し、計測結果を環境情報とマッチングすることにより自己位置を推定する技術が知られている。 A robot that moves while avoiding obstacles by referring to an environmental map is known. In such a robot, a technique is known in which the distance to an object is measured using a distance sensor, and the self-position is estimated by matching the measurement result with environmental information.

例えば、特許文献１は、ロボットの周辺に存在する物体までの距離を計測するセンサによる計測結果を環境地図に幾何学的に合わせこむことで自己位置を推定する技術について開示している。 For example, Patent Document 1 discloses a technique for estimating a self-position by geometrically matching a measurement result obtained by a sensor that measures a distance to an object existing around a robot with an environment map.

特開２０１２−９３８１１号公報JP 2012-93811 A

生活支援ロボット（HSR：Human Support Robot）などのように、マニピューレータにより対象物を操作する場合、操作に応じて対象物の位置が変動する。距離センサにより対象物までの距離を検知し、この検知により得られた距離情報に基づいて自己位置を推定する場合、距離センサにより取得される距離情報には、自身が操作している対象物の動的な位置情報が含まれることとなる。このため、結果として自己位置の推定精度が劣化するという問題がある。 When an object is operated by a manipulator such as a human support robot (HSR), the position of the object varies according to the operation. When the distance to the object is detected by the distance sensor and the self-position is estimated based on the distance information obtained by this detection, the distance information acquired by the distance sensor includes the object being operated by the distance sensor. Dynamic position information will be included. For this reason, there is a problem that the estimation accuracy of the self-position deteriorates as a result.

本発明は、上記した事情を背景としてなされたものであり、対象物を操作している場合における、自己位置の推定精度の劣化を抑制することができるロボットを提供することを目的とする。 The present invention has been made against the background of the above-described circumstances, and an object thereof is to provide a robot capable of suppressing deterioration in self-position estimation accuracy when an object is being operated.

本発明にかかるロボットは、把持対象を把持する把持手段と、検出領域内の物体との距離情報を検出する距離検出手段と、前記距離情報に基づいて、自己位置を推定する自己位置推定手段とを有し、前記自己位置推定手段は、前記距離情報のうち、前記把持手段の座標系を基準とした過去の前記距離情報からの変化量が予め定められた閾値以下の前記距離情報を除いて自己位置を推定する。 A robot according to the present invention includes a gripping unit that grips a gripping target, a distance detection unit that detects distance information from an object in a detection region, and a self-position estimation unit that estimates a self-position based on the distance information. And the self-position estimating means excludes the distance information in which the amount of change from the past distance information with reference to the coordinate system of the gripping means is less than or equal to a predetermined threshold. Estimate self-position.

このようなロボットによれば、把持手段による操作により位置が移動する対象物についての距離情報である動的な距離情報が除かれた距離情報に基づいて自己位置の推定が行われる。したがって、自身の操作により位置が変動している物体の影響が排除され、自己位置の推定精度の劣化を抑制することができる。 According to such a robot, self-position estimation is performed based on distance information from which dynamic distance information, which is distance information about an object whose position is moved by an operation by the gripping means, is removed. Therefore, the influence of the object whose position is changed by its own operation is eliminated, and deterioration of the self-position estimation accuracy can be suppressed.

本発明によれば、対象物を操作している場合における、自己位置の推定精度の劣化を抑制することができるロボットを提供することができる。 ADVANTAGE OF THE INVENTION According to this invention, when operating the target object, the robot which can suppress the deterioration of the self-position estimation precision can be provided.

実施形態に係るロボットの外観を示す斜視図である。It is a perspective view which shows the external appearance of the robot which concerns on embodiment. 実施形態に係るロボットの概略的システム構成を示すブロック図である。1 is a block diagram illustrating a schematic system configuration of a robot according to an embodiment. 実施形態に係る制御装置の機能構成の一例を示すブロック図である。It is a block diagram which shows an example of a function structure of the control apparatus which concerns on embodiment. タンスのタンス本体から引き出しが、ハンドによる操作によって引き出される様子を示す模式図である。（ａ）は、タイミングｔ−１における様子として、ハンドにより引き出しが引き出される前の状態を示しており、（ｂ）は、タイミングｔにおける様子として、ハンドにより引き出しが引き出された状態を示している。It is a schematic diagram which shows a mode that drawer | drawer is pulled out by the operation by a hand from the chiffon main body of stance. (A) shows a state before the drawer is pulled out by the hand as a state at timing t-1, and (b) shows a state where the drawer is pulled out by the hand as a state at timing t. . 図４に示されるハンドの操作前後におけるハンドの座標系を基準とした距離情報について示す模式図である。FIG. 5 is a schematic diagram showing distance information based on a hand coordinate system before and after operation of the hand shown in FIG. 4. 距離センサの検出領域と操作対象との関係を示す模式図であり、（ａ）は、操作対象であるドアなどの建具が検出領域内に侵入している様子を示し、（ｂ）は、操作対象であるタンスが検出領域内に侵入している様子を示している。It is a schematic diagram which shows the relationship between the detection area | region of a distance sensor, and an operation object, (a) shows a mode that fittings, such as a door which is an operation object, have penetrate | invaded in the detection area, (b) is operation The state that the target chiffon has entered the detection area is shown. 実施の形態に係る制御装置の動作の一例を示すフローチャートである。It is a flowchart which shows an example of operation | movement of the control apparatus which concerns on embodiment.

以下、図面を参照して本発明の実施の形態について説明する。
図１は、実施形態に係るロボット１の外観を示す斜視図である。図２は、実施形態に係るロボット１の概略的システム構成を示すブロック図である。
ロボット１は、台車１０と、ロボット本体１１と、ロボット本体１１に支持されたアーム１２と、アーム１２の先端に設けられたハンド１３と、距離センサ１４と、制御装置１５とを備えている。 Embodiments of the present invention will be described below with reference to the drawings.
FIG. 1 is a perspective view showing an appearance of a robot 1 according to the embodiment. FIG. 2 is a block diagram illustrating a schematic system configuration of the robot 1 according to the embodiment.
The robot 1 includes a carriage 10, a robot body 11, an arm 12 supported by the robot body 11, a hand 13 provided at the tip of the arm 12, a distance sensor 14, and a control device 15.

台車１０は、例えば、図示しないキャスタと、図示しない一対の対向する駆動車輪とが設けられており、制御信号にしたがって移動する。具体的には、台車１０には、モータなどにより構成された台車用駆動機構１００が設けられており、制御装置１５からの制御信号に基づいて台車用駆動機構１００が動作することにより、台車１０が移動する。 For example, the cart 10 is provided with a caster (not shown) and a pair of opposed driving wheels (not shown), and moves according to a control signal. Specifically, the carriage 10 is provided with a carriage drive mechanism 100 configured by a motor or the like, and the carriage drive mechanism 100 operates based on a control signal from the control device 15, whereby the carriage 10. Move.

また、台車１０には、台車状態検出センサ１０１が設けられている。台車状態検出センサ１０１は、例えばエンコーダなどによって、駆動車輪の回転情報を検出することにより、台車１０の速さ、移動方向、移動軌跡などの状態を検出する。台車状態検出センサ１０１による検出結果は、制御装置１５に出力される。 Further, the carriage 10 is provided with a carriage state detection sensor 101. The trolley state detection sensor 101 detects the state of the trolley 10 such as the speed, the moving direction, and the moving locus by detecting rotation information of the driving wheels using an encoder, for example. The detection result by the cart state detection sensor 101 is output to the control device 15.

ロボット本体１１は、台車１０上に設置されており、台車１０の移動にともない移動する。なお、ロボット本体１１は、台車１０に対して固定されて設置されていてもよいし、台車１０に対して変動可能に設置されていてもよい。例えば、ロボット本体１１は、台車１０上で旋回可能に設置されていてもよい。また、ロボット本体１１自体の位置姿勢を変更するための駆動機構が設けられていてもよい。 The robot body 11 is installed on the carriage 10 and moves as the carriage 10 moves. The robot body 11 may be fixedly installed with respect to the carriage 10 or may be installed so as to be variable with respect to the carriage 10. For example, the robot body 11 may be installed so as to be able to turn on the carriage 10. In addition, a drive mechanism for changing the position and orientation of the robot body 11 itself may be provided.

アーム１２は、例えば、多関節型アームとして構成されている。アーム１２は、例えば、ロボット本体１１の前面に設けられている。アーム１２には、アーム用駆動機構１２０が設けられており、アーム用駆動機構１２０の動作によりアーム１２の位置姿勢が変化する。アーム用駆動機構１２０は、図示しないモータを備え、制御装置１５からの制御信号にしたがって動作する。 The arm 12 is configured as an articulated arm, for example. The arm 12 is provided on the front surface of the robot body 11, for example. The arm 12 is provided with an arm drive mechanism 120, and the position and orientation of the arm 12 change by the operation of the arm drive mechanism 120. The arm drive mechanism 120 includes a motor (not shown) and operates according to a control signal from the control device 15.

また、アーム１２には、アーム状態検出センサ１２１が設けられている。アーム状態検出センサ１２１は、例えばエンコーダなどによって、関節部分の回転情報を検出することにより、アーム１２の姿勢や位置といった状態を検出する。アーム状態検出センサ１２１による検出結果は、制御装置１５に出力される。 The arm 12 is provided with an arm state detection sensor 121. The arm state detection sensor 121 detects the state such as the posture and position of the arm 12 by detecting the rotation information of the joint portion using, for example, an encoder. The detection result by the arm state detection sensor 121 is output to the control device 15.

ハンド１３は、把持手段に相当し、操作対象物（把持対象）の把持等を行う操作機構として構成されている。ハンド１３は、例えば、家具や建具などの操作対象を操作する。なお、ハンド１３は、操作時、操作対象物を固定的に把持しており、操作時においてハンド１３と操作対象物との幾何学的な相対位置関係は固定されている。ハンド１３には、ハンド用駆動機構１３０が設けられており、ハンド用駆動機構１３０の動作によりハンド１３の位置姿勢が変化する。ハンド用駆動機構１３０は、図示しないモータを備え、制御装置１５からの制御信号にしたがって動作する。 The hand 13 corresponds to a gripping means, and is configured as an operation mechanism that grips an operation target (grip target). For example, the hand 13 operates an operation target such as furniture or joinery. The hand 13 holds the operation target in a fixed manner during the operation, and the geometric relative positional relationship between the hand 13 and the operation target is fixed during the operation. The hand 13 is provided with a hand drive mechanism 130, and the position and orientation of the hand 13 change according to the operation of the hand drive mechanism 130. The hand drive mechanism 130 includes a motor (not shown) and operates according to a control signal from the control device 15.

また、ハンド１３には、ハンド状態検出センサ１３１が設けられている。ハンド状態検出センサ１３１は、例えばエンコーダなどによって関節部分の回転情報を検出することにより、ハンド１３の姿勢や位置といった状態を検出する。ハンド状態検出センサ１３１による検出結果は、制御装置１５に出力される。 The hand 13 is provided with a hand state detection sensor 131. The hand state detection sensor 131 detects a state such as the posture and position of the hand 13 by detecting rotation information of the joint portion using an encoder or the like, for example. The detection result by the hand state detection sensor 131 is output to the control device 15.

距離センサ１４は、距離検出手段に相当し、ロボット１周辺の検出領域Ｓ内にある環境（物体）とロボット１との距離情報を検出する。なお、図において、検出領域Ｓは、平面的な領域として図示されているが、検出領域Ｓは２次元に限らず、３次元であってもよい。距離センサ１４は、例えば、レーザレンジファインダが用いられる。例えば、レーザレンジファインダは、ロボット１の前方の検出領域Ｓへ放射状にレーザ光を放射し、その検出領域Ｓ内の物体からの反射光を受光することで、その物体の距離情報を検出することができる。なお、距離センサ１４は、レーザレンジファインダに限られない。例えば、デプスセンサ、超音波センサ、赤外線センサ、カメラ等の任意の距離センサを用いることができる。 The distance sensor 14 corresponds to a distance detection unit, and detects distance information between the environment (object) in the detection region S around the robot 1 and the robot 1. In the figure, the detection area S is illustrated as a planar area, but the detection area S is not limited to two dimensions, and may be three dimensions. As the distance sensor 14, for example, a laser range finder is used. For example, the laser range finder radiates laser light radially to the detection area S in front of the robot 1 and detects the distance information of the object by receiving reflected light from the object in the detection area S. Can do. The distance sensor 14 is not limited to the laser range finder. For example, an arbitrary distance sensor such as a depth sensor, an ultrasonic sensor, an infrared sensor, or a camera can be used.

距離センサ１４は、ロボット１に設けられており、距離センサ１４の検出領域Ｓとハンド１３の動作領域との少なくとも一部が重複するよう配置されている。例えば、距離センサ１４は、台車１０又はロボット本体１１の外周のうち、アーム１２が取り付けられている方向を検出できる位置に設けられている。 The distance sensor 14 is provided in the robot 1 and is arranged so that at least a part of the detection area S of the distance sensor 14 and the operation area of the hand 13 overlap. For example, the distance sensor 14 is provided at a position where the direction in which the arm 12 is attached can be detected on the outer periphery of the cart 10 or the robot body 11.

制御装置１５は、例えばロボット本体１１に内蔵されているが、台車１０などの他の構成部分に設けられていてもよい。制御装置１５は、例えば、制御処理、演算処理等を行うＣＰＵ（Central Processing Unit）１５ａ、ＣＰＵ１５ａによって実行される制御プログラム、演算プログラム、処理データ等を記憶する、ＲＯＭ（Read Only Memory）及びＲＡＭ（Random Access Memory）などにより構成される記憶部１５ｂ等からなるマイクロコンピュータを中心にして、ハードウェア構成されている。 The control device 15 is built in, for example, the robot body 11, but may be provided in other components such as the carriage 10. The control device 15 includes, for example, a CPU (Central Processing Unit) 15a that performs control processing, arithmetic processing, and the like, a ROM (Read Only Memory) and a RAM (RAM) that store control programs, arithmetic programs, processing data, and the like executed by the CPU 15a. Random Access Memory) and the like are configured with a hardware configuration centering on a microcomputer including a storage unit 15b and the like.

制御装置１５は、作業内容や移動内容などの指示が含まれる指示情報、並びに、台車状態検出センサ１０１、アーム状態検出センサ１２１、ハンド状態検出センサ１３１及び距離センサ１４からの検出結果などに基づいて、上述の各駆動機構を制御するとともに、後述する図３に示される処理を実行する。
また、記憶部１５ｂには、ロボット１が動作する環境についての地図情報が記憶されている。地図情報として、例えば、ロボット１が動作するエリア全体に略一定間隔に配置された格子点を結ぶグリッド線を仮想的に描写することで得られるグリッドマップが記憶されている。障害物の存在の有無を示す障害物情報が、予め又はリアルタイムに各グリッドに対して設定される。なお、地図情報には、障害物情報に限らず、路面情報、経路情報などの他の情報が含まれていてもよい。 The control device 15 is based on instruction information including instructions such as work contents and movement contents, and detection results from the carriage state detection sensor 101, the arm state detection sensor 121, the hand state detection sensor 131, and the distance sensor 14. In addition to controlling each of the drive mechanisms described above, the processing shown in FIG.
The storage unit 15b stores map information about the environment in which the robot 1 operates. As the map information, for example, a grid map obtained by virtually depicting grid lines connecting lattice points arranged at substantially constant intervals in the entire area where the robot 1 operates is stored. Obstacle information indicating the presence or absence of an obstacle is set for each grid in advance or in real time. The map information is not limited to the obstacle information, and may include other information such as road surface information and route information.

図３は、実施形態に係る制御装置１５の機能構成の一例を示すブロック図である。なお、図３に示される各構成は、例えば、上記制御プログラム又は演算プログラムがＣＰＵ１５ａにより実行されることで実現されてもよいし、ハードウェアにより実現されてもよい。 FIG. 3 is a block diagram illustrating an example of a functional configuration of the control device 15 according to the embodiment. Each configuration illustrated in FIG. 3 may be realized, for example, by the CPU 15a executing the control program or the arithmetic program, or may be realized by hardware.

制御装置１５は、ハンド位置姿勢情報取得部１５０と、距離情報取得部１５１と、距離情報選択部１５２と、自己位置推定部１５３とを有している。 The control device 15 includes a hand position / posture information acquisition unit 150, a distance information acquisition unit 151, a distance information selection unit 152, and a self-position estimation unit 153.

ハンド位置姿勢情報取得部１５０は、距離センサ１４の座標系を基準としたハンド１３の位置姿勢についての情報を取得する。ここで、距離センサ１４の座標系とは、距離センサ１４に固定された座標系である。ハンド位置姿勢情報取得部１５０は、例えば、ハンド状態検出センサ１３１による測定値及びアーム状態検出センサ１２１による測定値から、距離センサ１４の座標系を基準としたハンド１３の位置姿勢についての情報を生成する。 The hand position and orientation information acquisition unit 150 acquires information about the position and orientation of the hand 13 with reference to the coordinate system of the distance sensor 14. Here, the coordinate system of the distance sensor 14 is a coordinate system fixed to the distance sensor 14. For example, the hand position / orientation information acquisition unit 150 generates information about the position and orientation of the hand 13 based on the coordinate system of the distance sensor 14 from the measurement value obtained by the hand state detection sensor 131 and the measurement value obtained by the arm state detection sensor 121. To do.

距離情報取得部１５１は、検出領域Ｓ内にある物体とロボット１との距離情報であって、距離センサ１４の座標系を基準とした距離情報を距離センサ１４から取得する。距離情報取得部１５１は、例えば、検出領域Ｓを分割した各領域である、複数の部分領域ごとに、距離情報を取得する。本実施形態では、距離情報取得部１５１は、一度にＮ個の距離情報を取得するものとする。また、距離情報取得部１５１は、予め定められたタイミングにより、距離情報を取得する。例えば、距離情報取得部１５１は、予め定められた時間間隔ごとに、Ｎ個の距離情報を取得してもよいし、ロボット１の予め定められた移動距離ごとに、Ｎ個の距離情報を取得してもよい。なお、距離情報取得部１５１は、距離センサ１４の測定値から距離センサ１４の座標系を基準とした距離情報を生成してもよい。 The distance information acquisition unit 151 acquires distance information between the object in the detection region S and the robot 1 and distance information based on the coordinate system of the distance sensor 14 from the distance sensor 14. The distance information acquisition unit 151 acquires distance information for each of a plurality of partial areas that are areas obtained by dividing the detection area S, for example. In the present embodiment, the distance information acquisition unit 151 acquires N pieces of distance information at a time. The distance information acquisition unit 151 acquires distance information at a predetermined timing. For example, the distance information acquisition unit 151 may acquire N distance information for each predetermined time interval, or acquire N distance information for each predetermined movement distance of the robot 1. May be. Note that the distance information acquisition unit 151 may generate distance information based on the coordinate system of the distance sensor 14 from the measurement value of the distance sensor 14.

距離情報選択部１５２は、距離情報取得部１５１が取得した距離情報のうち、ハンド１３が現在操作中の対象物についての距離情報以外のものを選択する。具体的には、距離情報選択部１５２は、距離情報取得部１５１により取得された距離情報のうち、ハンド１３の座標系を基準とした過去の距離情報からの変化量が予め定められた閾値以下の距離情報を除き、残った距離情報を自己位置の推定のために用いるものとして選択する。なお、ハンド１３の座標系とは、ハンド１３に固定された座標系である。 The distance information selection unit 152 selects information other than the distance information about the target object currently being operated by the hand 13 from the distance information acquired by the distance information acquisition unit 151. Specifically, the distance information selection unit 152 has a change amount from the past distance information based on the coordinate system of the hand 13 out of the distance information acquired by the distance information acquisition unit 151 equal to or less than a predetermined threshold. The remaining distance information is selected to be used for the self-position estimation. The coordinate system of the hand 13 is a coordinate system fixed to the hand 13.

距離情報選択部１５２は、まず、距離情報取得部１５１が取得した、距離センサ１４の座標系を基準とした距離情報について、ハンド１３の座標系を基準とした距離情報へと変換する。距離情報選択部１５２は、例えば、下記の式（１）のように算出する。ここで、Ｄ_{ｔ，ｓｅｎｓｏｒ}（ｉ）は、タイミングｔにおける、距離センサ１４の座標系を基準とした距離情報のデータを示している。なお、ｉは、距離情報のデータのインデックスを表し、０＜ｉ≦Ｎを満たす整数である。また、_{ｓｅｎｓｏｒ}Ｔ_ｈａｎｄ ^−１は、距離センサ１４の座標系を基準としたハンド１３の位置姿勢データの逆行列である。また、Ｄ_{ｔ，ｈａｎｄ}（ｉ）は、タイミングｔにおける、ハンド１３の座標系を基準とした距離情報のデータを示している。 The distance information selection unit 152 first converts the distance information acquired by the distance information acquisition unit 151 based on the coordinate system of the distance sensor 14 into distance information based on the coordinate system of the hand 13. The distance information selection unit 152 calculates, for example, as in the following equation (1). Here, D _{t, sensor} (i) indicates distance information data at the timing t with reference to the coordinate system of the distance sensor 14. Note that i represents an index of data of distance information, and is an integer satisfying 0 <i ≦ N. _{Also,: sensor} T _hand ^-1 is the inverse matrix of the position and orientation data of the hand 13 relative to the coordinate system of the distance sensor 14. D _{t, hand} (i) indicates distance information data at the timing t with reference to the coordinate system of the hand 13.

Ｄ_{ｔ，ｈａｎｄ}（ｉ）＝_{ｓｅｎｓｏｒ}Ｔ_ｈａｎｄ ^−１Ｄ_{ｔ，ｓｅｎｓｏｒ}（ｉ）・・・（１） D _{t, hand} (i) = _sensor T _hand ⁻¹ D _{t, sensor} (i) (1)

距離情報選択部１５２は、式（１）により、１からＮの全ての距離情報のデータについて、変換を行う。
距離情報選択部１５２は、次に、変換して得られたハンド１３の座標系を基準とした距離情報について、前回のタイミングにおけるハンド１３の座標系を基準とした距離情報と比較する。そして、距離情報選択部１５２は、前回のタイミングにおけるハンド１３の座標系を基準とした距離情報との変動量が予め定められた閾値以下の距離情報については、ハンド１３により操作されている対象であるとみなして、選択候補から除外する。 The distance information selection unit 152 performs conversion on all the distance information data 1 to N according to the equation (1).
Next, the distance information selection unit 152 compares the distance information based on the coordinate system of the hand 13 obtained by the conversion with the distance information based on the coordinate system of the hand 13 at the previous timing. Then, the distance information selection unit 152 selects the distance information whose fluctuation amount with respect to the distance information with reference to the coordinate system of the hand 13 at the previous timing is equal to or less than a predetermined threshold as a target operated by the hand 13. It is considered that there is, and is excluded from the selection candidates.

距離情報選択部１５２は、例えば、下記の式（２）のように表される変動量ｄｉｆｆを閾値ｔｈと比較する。なお、式（２）において、変動量ｄｉｆｆ（ｉ）は、インデックスがｉである距離情報についての変動量を示す。また、Ｄ_{ｔ−１，ｈａｎｄ}（ｉ）は、タイミングｔ−１における、ハンド１３の座標系を基準とした距離情報のデータを示している。なお、タイミングｔ−１は、タイミングｔの１周期分前のタイミングである。また、ａｂｓ（）は、（）内の絶対値を演算する関数である。 The distance information selection unit 152 compares, for example, a variation amount diff expressed as the following equation (2) with a threshold th. In equation (2), the fluctuation amount diff (i) indicates the fluctuation amount for the distance information whose index is i. Further, D _{t−1, hand} (i) indicates data of distance information with reference to the coordinate system of the hand 13 at the timing t−1. Note that the timing t-1 is a timing one cycle before the timing t. Also, abs () is a function that calculates the absolute value in ().

ｄｉｆｆ（ｉ）＝ａｂｓ（Ｄ_{ｔ，ｈａｎｄ}（ｉ）−Ｄ_{ｔ−１，ｈａｎｄ}（ｉ））・・・（２） diff (i) = abs (D _{t, hand} (i) −D _{t−1, hand} (i)) (2)

距離情報選択部１５２は、式（２）により、１からＮの全ての距離情報のデータについて、変動量（差分）を算出する。そして、距離情報選択部１５２は、変動量が閾値ｔｈよりも大きい場合には、当該距離情報のデータ、すなわちＤ_{ｔ，ｈａｎｄ}（ｉ）を、自己位置の推定のために用いるものとして採用し、変動量が閾値ｔｈ以下の場合には、自己位置の推定のために用いるものとして採用しない。例えば、距離情報選択部１５２は、自己位置の推定のために用いるものとして選択したデータを、リストに保持する。このように、距離情報選択部１５２は、１からＮの全ての距離情報のデータについて、変動量に基づいて、採用又は不採用が決定する。距離情報選択部１５２は、採用すると決定したデータ集合、すなわち距離情報選択部１５２により選択されたデータ集合を、自己位置推定部１５３に出力する。このようにして、自己位置推定部１５３に入力されるデータ集合は、ハンド１３により位置が変動する動的な対象物に対する距離情報が除かれた静的な環境情報となる。 The distance information selection unit 152 calculates the amount of variation (difference) for all pieces of distance information data 1 to N using Equation (2). And the distance information selection part 152 employ | adopts that the data of the said distance information, ie _{, Dt, hand} (i), are used for estimation of a self-position, when the variation | change_quantity is larger than the threshold value th, When the fluctuation amount is equal to or less than the threshold th, it is not adopted as one used for estimating the self-position. For example, the distance information selection unit 152 holds the data selected to be used for estimating the self position in a list. In this way, the distance information selection unit 152 determines whether to adopt or not adopt all the distance information data from 1 to N based on the fluctuation amount. The distance information selection unit 152 outputs the data set determined to be adopted, that is, the data set selected by the distance information selection unit 152 to the self-position estimation unit 153. In this way, the data set input to the self-position estimation unit 153 becomes static environment information from which distance information for a dynamic object whose position is changed by the hand 13 is removed.

ここで、距離情報選択部１５２による距離情報の選択について、図４及び図５を用いて補足説明する。なお、図４及び図５に示す例では、２次元で図示されているが、距離情報は３次元の情報であってもよい。 Here, the selection of distance information by the distance information selection unit 152 will be supplementarily described with reference to FIGS. 4 and 5. In the example shown in FIGS. 4 and 5, the distance information may be three-dimensional information although it is illustrated in two dimensions.

図４は、タンス２のタンス本体２０から引き出し２１が、ハンド１３による操作によって引き出される様子を示す模式図である。なお、図４に示した例では、タンス２は、コの字型に配置された壁３に寄せて置かれている。また、図４において、破線は、距離センサ１４により検出される物体（タンス２及び壁３）の位置を示している。したがって、図４における破線は、距離情報選択部１５２に入力される距離情報のデータに対応している。また、図４（ａ）は、タイミングｔ−１における様子を示しており、ハンド１３により引き出し２１が引き出される前の状態を示している。これに対し、図４（ｂ）は、タイミングｔにおける様子を示しており、ハンド１３により引き出し２１が引き出された状態を示している。図４に示されるように、ハンド１３の操作により、タンス２についての距離情報に関し、引き出し２１に対する操作前後で、引き出し２１部分の距離情報が変化している。 FIG. 4 is a schematic diagram showing a state in which the drawer 21 is pulled out from the chase body 20 of the chase 2 by an operation with the hand 13. In the example shown in FIG. 4, the dresser 2 is placed close to the wall 3 arranged in a U-shape. In FIG. 4, the broken line indicates the position of the object (the chest 2 and the wall 3) detected by the distance sensor 14. Therefore, the broken line in FIG. 4 corresponds to the distance information data input to the distance information selection unit 152. FIG. 4A shows a state at the timing t−1 and shows a state before the drawer 21 is pulled out by the hand 13. On the other hand, FIG. 4B shows a state at the timing t, and shows a state in which the drawer 21 is pulled out by the hand 13. As shown in FIG. 4, by the operation of the hand 13, the distance information on the drawer 21 is changed before and after the operation on the drawer 21 with respect to the distance information about the chest 2.

次に、図５について説明する。図５は、図４に示されるハンド１３の操作前後におけるハンド１３の座標系を基準とした距離情報について示す模式図である。図５において、破線は、タイミングｔ−１（図４（ａ）に対応）におけるハンド１３の座標系を基準とした距離情報を模式的に示したものであり、実線は、タイミングｔ（図４（ｂ）に対応）におけるハンド１３の座標系を基準とした距離情報を模式的に示したものである。図５に示されるように、ハンド１３の座標系を基準とした場合には、操作前後において、タンス２が設けられた壁面に対する距離情報は変動するものの、引き出し２１に対する距離情報（図中の二点鎖線で囲まれた部分）は変動しない。ここで、引き出し２１に対する距離情報は、上述の変動量が閾値以下の距離情報に対応している。 Next, FIG. 5 will be described. FIG. 5 is a schematic diagram showing distance information based on the coordinate system of the hand 13 before and after the operation of the hand 13 shown in FIG. In FIG. 5, a broken line schematically shows distance information based on the coordinate system of the hand 13 at timing t-1 (corresponding to FIG. 4A), and a solid line indicates timing t (FIG. 4). 6 schematically shows distance information based on the coordinate system of the hand 13 in (corresponding to (b)). As shown in FIG. 5, when the coordinate system of the hand 13 is used as a reference, the distance information for the wall surface on which the chase 2 is provided varies before and after the operation, but the distance information for the drawer 21 (two in the figure). The part surrounded by the dotted line does not change. Here, the distance information with respect to the drawer 21 corresponds to the distance information in which the above-described variation amount is equal to or less than the threshold value.

このように、距離情報選択部１５２は、変動量が閾値以下の距離情報であるか否かを確認することで、ハンド１３の操作よる操作対象の距離情報であるか否かを判別している。 As described above, the distance information selection unit 152 determines whether or not the distance information is the distance information of the operation target by the operation of the hand 13 by confirming whether or not the variation amount is distance information equal to or less than the threshold. .

自己位置推定部１５３は、距離センサ１４による検出により取得された距離情報に基づいて、ロボット１の自己位置を推定する。具体的には、自己位置推定部１５３は、距離センサ１４による検出により取得された距離情報と、上述の地図情報とを比較してマッチング処理を行うことで自己位置推定処理を行う。なお、自己位置推定部１５３は、例えばオドメトリによって取得された自己位置を、距離情報選択部１５２から出力されたデータ集合と上述の地図情報とを比較してマッチング処理の結果にしたがって補正してもよい。 The self-position estimating unit 153 estimates the self-position of the robot 1 based on the distance information acquired by detection by the distance sensor 14. Specifically, the self-position estimation unit 153 performs the self-position estimation process by comparing the distance information acquired by the detection by the distance sensor 14 with the above-described map information and performing the matching process. For example, the self-position estimation unit 153 may correct the self-position acquired by odometry, for example, by comparing the data set output from the distance information selection unit 152 with the above-described map information according to the result of the matching process. Good.

ここで、自己位置推定部１５３は、距離センサ１４による検出により取得された距離情報のうち、ハンド１３の座標系を基準とした過去の距離情報からの変化量が予め定められた閾値以下の距離情報を除いて自己位置を推定する。すなわち、自己位置推定部１５３は、距離情報選択部１５２から出力されたデータ集合を用いて、自己位置推定処理を行う。 Here, the self-position estimation unit 153 includes a distance in which the amount of change from the past distance information based on the coordinate system of the hand 13 out of the distance information acquired by the detection by the distance sensor 14 is equal to or less than a predetermined threshold. Estimate self-location without information. That is, the self-position estimation unit 153 performs self-position estimation processing using the data set output from the distance information selection unit 152.

このため、自己位置推定部１５３は、ハンド１３により位置が変動する動的な対象物についての距離情報が除かれた距離情報のみを用いて、自己推定処理を行うこととなる。ハンド１３が操作対象を操作している際には、例えば、図６に示されるように、距離センサ１４の検出領域Ｓ内に、ハンド１３の操作によって位置が変化する動的な対象物が含まれることがある。その場合、距離情報取得部１５１が取得する距離情報には、ハンド１３の操作によって位置が変化する動的な対象物についての距離情報も含まれることとなる。そして、動的な対象物についての距離情報を含んだデータに基づいて自己位置推定処理を行う場合には、正確な自己位置の計算を行うことが阻害されることとなる。なお、図６（ａ）は、操作対象であるドアなどの建具４が検出領域Ｓ内に侵入している様子を示す模式図であり、図６（ｂ）は、操作対象であるタンス２が検出領域Ｓ内に侵入している様子を示す模式図である。 For this reason, the self-position estimation unit 153 performs the self-estimation process using only the distance information from which the distance information about the dynamic object whose position is changed by the hand 13 is removed. When the hand 13 is operating the operation target, for example, as illustrated in FIG. 6, a dynamic target whose position is changed by the operation of the hand 13 is included in the detection region S of the distance sensor 14. May be. In this case, the distance information acquired by the distance information acquisition unit 151 includes distance information about a dynamic object whose position changes due to the operation of the hand 13. And when performing a self-position estimation process based on the data containing the distance information about a dynamic target object, it will be inhibited from calculating an exact self-position. FIG. 6A is a schematic diagram showing a state in which a fitting 4 such as a door to be operated enters the detection region S, and FIG. FIG. 4 is a schematic diagram showing a state in which a detection area S has entered.

しかしながら、本実施形態にかかるロボット１においては、上述のとおり、距離情報選択部１５２によって、距離情報取得部１５１が取得した距離情報のうち、ハンド１３が現在操作中の対象物についての距離情報以外のものが選択され、自己位置推定部１５３は、選択された距離情報に基づいて自己位置の推定処理を行う。このため、ロボット１自身が操作している動的な物体の影響を排除することができる。したがって、ハンド１３が操作している対象物の距離情報を含んで自己位置推定を行う場合に比べ、正確な自己位置の推定を行うことができる。ハンド１３が操作対象を操作している際には、距離情報取得部１５１が取得する距離情報の大部分が、操作対象の距離情報であることも想定されるが、そのような外乱により自己位置の正確な推定が困難な状況にあっても、本実施形態にかかるロボット１は、自己位置の推定の精度を向上することができる。 However, in the robot 1 according to the present embodiment, as described above, out of the distance information acquired by the distance information acquisition unit 151 by the distance information selection unit 152, other than the distance information about the object that the hand 13 is currently operating. Are selected, and the self-position estimation unit 153 performs self-position estimation processing based on the selected distance information. For this reason, it is possible to eliminate the influence of a dynamic object operated by the robot 1 itself. Therefore, it is possible to estimate the self-position more accurately than in the case where the self-position estimation is performed including the distance information of the object operated by the hand 13. When the hand 13 is operating the operation target, it is assumed that most of the distance information acquired by the distance information acquisition unit 151 is the distance information of the operation target. Even in a situation where it is difficult to estimate accurately, the robot 1 according to the present embodiment can improve the accuracy of self-position estimation.

また、本実施形態にかかるロボット１においては、距離情報取得部１５１が取得した距離情報を選別することで、ハンド１３が現在操作中の対象物についての距離情報を除外している。このため、例えば、予め家具などのモデルデータを登録するなどの作業も発生しない。さらに、距離情報のうち、操作中の対象物についての距離情報を除外するという方式であるため、タスクを実行する際に自己位置推定方法を切り替える必要がなく、自己位置推定のロバスト性を向上させることができる。また、自己位置推定方法の切り替えが不要であるため、ロボット１の制御にかかるソフトウェアの汎用性を保つことができる。 Further, in the robot 1 according to the present embodiment, the distance information about the target object currently being operated by the hand 13 is excluded by selecting the distance information acquired by the distance information acquisition unit 151. For this reason, for example, work such as registering model data such as furniture in advance does not occur. Furthermore, since the distance information about the target object is excluded from the distance information, there is no need to switch the self-position estimation method when executing the task, and the robustness of the self-position estimation is improved. be able to. In addition, since it is not necessary to switch the self-position estimation method, the software versatility for controlling the robot 1 can be maintained.

次に、本実施の形態に係る制御装置１５の動作の一例を図７に示すフローチャートを参照して説明する。 Next, an example of operation | movement of the control apparatus 15 which concerns on this Embodiment is demonstrated with reference to the flowchart shown in FIG.

ステップ１００（Ｓ１００）において、ハンド位置姿勢情報取得部１５０は、距離センサ１４の座標系を基準としたハンド１３の位置姿勢についての情報を取得する。また、ハンド位置姿勢情報取得部１５０は、取得したハンド１３の位置姿勢についての情報（データ）を距離情報選択部１５２に出力する。 In step 100 (S100), the hand position / orientation information acquisition unit 150 acquires information about the position and orientation of the hand 13 with reference to the coordinate system of the distance sensor 14. Further, the hand position / orientation information acquisition unit 150 outputs information (data) about the acquired position and orientation of the hand 13 to the distance information selection unit 152.

ステップ１０１（Ｓ１０１）において、距離情報取得部１５１は、距離センサ１４の座標系を基準とした距離情報を距離センサ１４から取得する。距離情報取得部１５１は、例えば、タイミングｔの距離情報として、Ｎ個の距離情報を取得する。また、距離情報取得部１５１は、取得した距離情報のデータを距離情報選択部１５２に出力する。 In step 101 (S101), the distance information acquisition unit 151 acquires distance information based on the coordinate system of the distance sensor 14 from the distance sensor 14. For example, the distance information acquisition unit 151 acquires N pieces of distance information as the distance information at the timing t. The distance information acquisition unit 151 outputs the acquired distance information data to the distance information selection unit 152.

ステップ１０２（Ｓ１０２）において、距離情報選択部１５２は、ステップ１０１で取得した、距離センサ１４の座標系を基準とした各距離情報について、ハンド１３の座標系を基準とした距離情報へと変換する。 In step 102 (S102), the distance information selection unit 152 converts each distance information acquired in step 101 based on the coordinate system of the distance sensor 14 into distance information based on the coordinate system of the hand 13. .

ステップ１０３（Ｓ１０３）において、距離情報選択部１５２は、ステップ１０２で変換された距離情報の変化量と、予め定められた閾値とを比較する。具体的には、距離情報選択部１５２は、タイミングｔの距離情報とタイミングｔ−１の距離情報との差分の絶対値で表される変動量ｄｉｆｆと、閾値ｔｈとを比較する。そして、変動量が予め定められた閾値を超える場合にはステップ１０４へ移行し、変動量が予め定められた閾値以下の場合には、ステップ１０５へ移行する。 In step 103 (S103), the distance information selection unit 152 compares the amount of change in the distance information converted in step 102 with a predetermined threshold value. Specifically, the distance information selection unit 152 compares the variation amount diff represented by the absolute value of the difference between the distance information at the timing t and the distance information at the timing t−1 with the threshold th. If the fluctuation amount exceeds a predetermined threshold, the process proceeds to step 104. If the fluctuation amount is equal to or smaller than the predetermined threshold, the process proceeds to step 105.

ステップ１０４（Ｓ１０４）において、距離情報選択部１５２は、変動量が予め定められた閾値を超えると判定された距離情報のデータをリストに保持する。 In step 104 (S104), the distance information selection unit 152 stores the data of distance information determined that the fluctuation amount exceeds a predetermined threshold in a list.

ステップ１０５（Ｓ１０５）において、距離情報選択部１５２は、ステップ１０２で変換された全ての距離情報についてステップ１０３の処理が行われたか否かを判定し、全ての距離情報について処理が行われた場合には、ステップ１０６へ移行し、未だ処理が行われていない距離情報がある場合については、ステップ１０３に戻り、未だ処理が行われていない距離情報に対してステップ１０３以降の処理を行う。例えば、ステップ１０２においてＮ個の距離情報が変換された場合には、変換されたＮ個の距離情報全てについてステップ１０３の処理が行われると、ステップ１０６に進むこととなる。なお、距離情報選択部１５２は、ステップ１０２で変換された全ての距離情報についてステップ１０３の処理が行われた場合には、変動量が予め定められた閾値を超えると判定された距離情報のデータが含まれている上述のリストを自己位置推定部１５３に出力する。 In step 105 (S105), the distance information selection unit 152 determines whether or not the process of step 103 has been performed for all the distance information converted in step 102, and the process has been performed for all the distance information. If there is distance information that has not been processed yet, the process returns to step 103, and the processing after step 103 is performed on the distance information that has not yet been processed. For example, when N pieces of distance information are converted in step 102, if the process of step 103 is performed for all the converted N pieces of distance information, the process proceeds to step 106. It should be noted that the distance information selection unit 152, when the processing of step 103 is performed for all the distance information converted in step 102, data of distance information determined that the variation amount exceeds a predetermined threshold value. Is output to the self-position estimation unit 153.

ステップ１０６（Ｓ１０６）において、自己位置推定部１５３は、ステップ１０５で距離情報選択部１５２から出力されたリストに含まれる距離情報に基づいて、ロボット１の自己位置を推定する。 In step 106 (S106), the self-position estimating unit 153 estimates the self-position of the robot 1 based on the distance information included in the list output from the distance information selecting unit 152 in step 105.

以上、本発明の実施形態について説明したが、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。 While the embodiments of the present invention have been described above, the present invention is not limited to the above-described embodiments, and can be appropriately changed without departing from the spirit of the present invention.

例えば、上述の実施形態では、距離情報選択部１５２は、インデックスがｉであるタイミングｔにおける距離情報と、同じくインデックスがｉであるタイミングｔ−１における距離情報とが対応するものとして、両者の差分を算出しているが、タイミングｔの距離情報のインデックスとは異なるインデックスを有するタイミングｔ−１の距離情報とを対応付けてもよい。例えば、距離情報選択部１５２は、インデックスがｉであるタイミングｔにおける距離情報に対応するタイミングｔ−１の距離情報を最近傍探索により見つけるよう構成してもよい。すなわち、タイミングｔ−１における距離情報のデータ群のうち、タイミングｔの当該距離情報と最も位置が近いデータを当該距離情報に対応するデータとしてもよい。
また、距離情報は、２次元の情報に限らず深度画像のような３次元情報であってもよい。 For example, in the above-described embodiment, the distance information selection unit 152 assumes that the distance information at the timing t at which the index is i corresponds to the distance information at the timing t−1 at which the index is i. However, the distance information at the timing t−1 having an index different from the index of the distance information at the timing t may be associated with each other. For example, the distance information selecting unit 152 may be configured to find the distance information at the timing t−1 corresponding to the distance information at the timing t where the index is i by the nearest neighbor search. That is, in the data group of distance information at the timing t−1, data closest to the distance information at the timing t may be data corresponding to the distance information.
The distance information is not limited to two-dimensional information, but may be three-dimensional information such as a depth image.

１ロボット
１０台車
１１ロボット本体
１２アーム
１３ハンド
１４距離センサ
１５制御装置
１００台車用駆動機構
１０１台車状態検出センサ
１２０アーム用駆動機構
１２１アーム状態検出センサ
１３０ハンド用駆動機構
１３１ハンド状態検出センサ
１５０ハンド位置姿勢情報取得部
１５１距離情報取得部
１５２距離情報選択部
１５３自己位置推定部 DESCRIPTION OF SYMBOLS 1 Robot 10 Cart 11 Robot main body 12 Arm 13 Hand 14 Distance sensor 15 Control device 100 Cart drive mechanism 101 Cart state detection sensor 120 Arm drive mechanism 121 Arm state detection sensor 130 Hand drive mechanism 131 Hand state detection sensor 150 Hand position Posture information acquisition unit 151 Distance information acquisition unit 152 Distance information selection unit 153 Self-position estimation unit

Claims

A gripping means for gripping a gripping object;
Distance detecting means for detecting distance information with an object in the detection region;
Self-position estimating means for estimating the self-position based on the distance information;
The self-position estimating means estimates the self-position by excluding the distance information in which the amount of change from the distance information in the past with reference to the coordinate system of the gripping means is equal to or less than a predetermined threshold. Robot.