JP5427662B2

JP5427662B2 - Robot system

Info

Publication number: JP5427662B2
Application number: JP2010065426A
Authority: JP
Inventors: 高斉松本; 俊夫守屋; 修一槙
Original assignee: Hitachi Industrial Equipment Systems Co Ltd
Current assignee: Hitachi Industrial Equipment Systems Co Ltd
Priority date: 2010-03-23
Filing date: 2010-03-23
Publication date: 2014-02-26
Anticipated expiration: 2030-03-23
Also published as: JP2011198173A

Description

本発明は、ロボットシステムに関し、特に、通行可能領域と進入禁止領域を設定する技術に関する。 The present invention relates to a robot system, and more particularly to a technique for setting a passable area and an entry prohibition area.

ロボットが環境内を効率的に移動するためには、環境内の障害物などの位置を表す地図を保持し、その地図上での自身の位置・姿勢を求める必要がある。
これを実現する方法として、2次元平面をレーザで走査することで環境の幾何形状の計測が可能な距離センサをロボットに搭載し、走査面の高さでの環境の地図を作成して自律移動に用いることが一般的となっている。ただし、この地図は、ある高さでの地図であるため、例えば机などは、地図に反映されているのは脚の位置だけであり、地図からは机の天板部分とロボットの干渉の有無が判断できない、つまりは、通行可能な領域・通行不能な領域が地図上のどこであるかが判断できないことが起こりうる。これに対応する方法としては、非特許文献１のように、環境中にある、机や設備のモデルを準備して、別途距離センサで作成しておいた地図に追加することで、ロボットが直接認識できない物体形状を地図に反映させ、物体がない領域を通行可能領域とする方法が挙げられる。環境中にある物体の幾何形状が予めわかっている場合に用いられる。 In order for the robot to move efficiently in the environment, it is necessary to hold a map indicating the position of an obstacle or the like in the environment and obtain its position / posture on the map.
To achieve this, the robot is equipped with a distance sensor capable of measuring the geometric shape of the environment by scanning a two-dimensional plane with a laser, creating an environment map at the height of the scanning plane, and moving autonomously. It has become common to use. However, since this map is a map at a certain height, for example, desks, etc., only the position of the legs is reflected in the map, and there is no interference between the top of the desk and the robot from the map Can not be determined, that is, it is not possible to determine where on the map the area that can be passed or the area that cannot be passed. As a method corresponding to this, as in Non-Patent Document 1, a model of a desk or equipment in the environment is prepared and added to a map created by a separate distance sensor, so that the robot directly An example is a method in which an unrecognizable object shape is reflected on a map so that a region without an object can be passed. Used when the geometry of an object in the environment is known in advance.

Takashi Tsubouchi, Akichika Tanaka, Atsushi Ishioka, Masahiro Tomono, Shin'ichi, ``A SLAM Based Teleoperation and Interface System for Indoor Environment Reconnaissance in Rescue Activities," 2004 IEEE/RSJ International Conference on Intelligent Robots and Systems, pp.1096-1102, 2004.Takashi Tsubouchi, Akichika Tanaka, Atsushi Ishioka, Masahiro Tomono, Shin'ichi, `` A SLAM Based Teleoperation and Interface System for Indoor Environment Reconnaissance in Rescue Activities, '' 2004 IEEE / RSJ International Conference on Intelligent Robots and Systems, pp.1096- 1102, 2004.

ロボットが移動する環境中の物体１つ１つの幾何形状の情報がない場合においても、ロボットが通行可能な領域、あるいは通行不能な領域を容易に設定できるロボットシステムの構築を課題とする。 It is an object of the present invention to construct a robot system that can easily set an area where a robot can pass or cannot pass even if there is no information about the geometric shape of each object in the environment where the robot moves.

レーザ距離センサを搭載したロボットを用い、通行可能な範囲の境、例えば壁面などに沿ってロボットを移動させる。この移動の際、距離センサによるセンサデータを記録しておき、これを環境の幾何形状を表す地図とマッチングすることで、環境中をロボットが移動したときの軌跡を求める。この軌跡の各点において、センサによる走査を行った場合に、レーザが地図上の障害物と交わる位置を求め、障害物との交わりが軌跡の内側にある場合は進入禁止領域、そうでない場合は通行可能領域と設定する。 Using a robot equipped with a laser distance sensor, the robot is moved along the boundary of the passable range, such as a wall surface. At the time of this movement, sensor data from the distance sensor is recorded, and this is matched with a map representing the geometric shape of the environment, thereby obtaining a trajectory when the robot moves in the environment. When scanning with a sensor at each point of this locus, the position where the laser intersects the obstacle on the map is obtained, and if the intersection with the obstacle is inside the locus, the entry prohibition area, otherwise Set as a passable area.

本発明の一態様においては、以下の構成を備える。即ち、環境中の障害物までの距離が計測可能な距離センサ部と距離センサデータと地図とを用いて位置・姿勢の推定を行う位置姿勢推定部と、与えられた経路に従って車体制御を行う経路追従制御部と、経路追従制御部の指令に従って動作する移動機構部と、操作者からの指示を受け取るためのリモコン入力取得部からなるロボットであって、位置推定部によって求められたロボット移動時の推定位置・姿勢データをもとに移動した経路の履歴を求める経路履歴生成部と、経路履歴による閉領域と環境地図を用い、閉領域内の障害物の有無を確認することで、閉領域が通行可能領域か進入禁止領域かを判定する通行可能領域・進入禁止領域生成部と、求められた各通行可能領域と進入禁止領域を統合して、地図全体での通行可能領域と進入禁止領域とを地図として求める通行可能領域・進入禁止領域統合部を備える。 One embodiment of the present invention includes the following configuration. In other words, a distance sensor unit that can measure the distance to an obstacle in the environment, a position / orientation estimation unit that estimates position / orientation using distance sensor data and a map, and a route that performs vehicle body control according to a given route A robot including a tracking control unit, a moving mechanism unit that operates according to a command from the path tracking control unit, and a remote control input acquisition unit for receiving an instruction from an operator. By using the route history generator that obtains the history of the route traveled based on the estimated position / posture data and the closed region and environment map based on the route history, the closed region Integrate the obtained passable area / entry prohibited area with the passable area / prohibited area generation part that determines whether it is a passable area or an entry-prohibited area. Comprising a passable area, no entry region integrating unit for obtaining the region as a map.

環境内にある設備などについて、通行可能領域・進入禁止領域の設定の時間や労力を削減する効果がある。 It has the effect of reducing the time and labor required to set the passable / prohibited areas for facilities in the environment.

通行可能領域と進入禁止領域の設定機能の構成を示す図である。It is a figure which shows the structure of the setting function of a passable area | region and an entry prohibition area | region. ロボットの構成を示す図である。It is a figure which shows the structure of a robot. ロボット内の全体の処理の流れを示す図である。It is a figure which shows the flow of the whole process in a robot. 通行可能領域と進入禁止領域の設定処理の流れを示す図である。It is a figure which shows the flow of a setting process of a passable area | region and an entry prohibition area | region. 環境とロボットの経路の例を示す図である。It is a figure which shows the example of the path | route of an environment and a robot. 通行可能領域と進入禁止領域の判定例を示す図である。It is a figure which shows the example of determination of a passable area | region and an entry prohibition area | region. 設定された通行可能領域と進入禁止領域を示す図である。It is a figure which shows the set passable area and the entry prohibition area. 位置推定の異常を検出して警告する処理の流れを示す図である。It is a figure which shows the flow of the process which detects and warns of the abnormality of a position estimation.

以下、図面を用いて本発明の実施形態を説明する。 Hereinafter, embodiments of the present invention will be described with reference to the drawings.

本実施例で想定するシステムの基本的な構成について述べる。通行可能領域・進入禁止領域を設定するロボットシステムの構成は図１のようになる。ロボットは、距離センサ部０００１、位置姿勢推定部０００２、移動機構制御部０００３、移動機構部０００４、リモコン入力取得部０００５、経路履歴クロージング部０００６、通行可能領域・進入禁止領域生成部０００７、通行可能領域・進入禁止領域統合部０００８、地図データ０００９、推定位置・姿勢データ００１０、そして生成した通行可能領域・進入禁止領域を保存するデータ領域００１１から構成される。この図１においては、すべての機能をロボットに搭載しているが、機能の一部を遠隔の端末など移した構成としてもよい。この図１の一例として図２のロボットを想定し、以下ではこのロボットの構成に沿った説明を行う。まず、図２のロボット２００１の構成について述べる。ロボット２００１は、通信線２００２、プロセッサ２００３、メモリ２００４、リモコン２００５、レーザ距離センサ２００６、移動機構２００７、記憶装置２００８から構成される。本実施例では、移動機構として、車輪による、全方向移動が可能な移動機構を想定しているが、環境内を移動する効果が得られるのであれば、他の車輪移動機構、船舶、航空機、飛行船、無限軌道を備える車両、脚を備えるロボットなどの移動機構であってもよい。また、本実施例では、環境の幾何形状を計測するセンサとして、水平方向にレーザを走査しながら、照射したレーザ光が障害物から反射して返ってくるまでの時間の計測によってセンサから障害物までの距離を算出することで、2次元水平面における物体の幾何形状を計測するレーザ距離センサを想定する。しかし、この他のセンサとして、例えば、水平方向以外も走査することで物体の3次元の幾何形状を計測するレーザ距離センサ、物体をカメラで撮影した際の画像特徴量を複数野カメラで三角測量することで物体の3次元の幾何形状を計測するステレオカメラなど、幾何形状が計測可能であればそのセンサの方式は問わない。 The basic configuration of the system assumed in this embodiment will be described. The configuration of the robot system for setting the passable area and the entry prohibition area is as shown in FIG. The robot includes a distance sensor unit 0001, a position / orientation estimation unit 0002, a movement mechanism control unit 0003, a movement mechanism unit 0004, a remote control input acquisition unit 0005, a route history closing unit 0006, a passable area / intrusion prohibition area generation unit 0007, and passable. An area / entry prohibition area integration unit 0008, map data 0009, estimated position / posture data 0010, and a data area 0011 for storing the generated passable area / entry prohibition area. In FIG. 1, all functions are mounted on the robot. However, a part of the functions may be transferred to a remote terminal or the like. As an example of this FIG. 1, the robot of FIG. 2 is assumed, and the following description will be given along the configuration of this robot. First, the configuration of the robot 2001 in FIG. 2 will be described. The robot 2001 includes a communication line 2002, a processor 2003, a memory 2004, a remote controller 2005, a laser distance sensor 2006, a moving mechanism 2007, and a storage device 2008. In this embodiment, a moving mechanism capable of omnidirectional movement by wheels is assumed as the moving mechanism. However, if the effect of moving in the environment is obtained, other wheel moving mechanisms, ships, aircraft, It may be a moving mechanism such as an airship, a vehicle with an endless track, or a robot with legs. Further, in this embodiment, as a sensor for measuring the geometric shape of the environment, the obstacle is detected from the sensor by measuring the time until the irradiated laser light is reflected and returned from the obstacle while scanning the laser in the horizontal direction. A laser distance sensor that measures the geometrical shape of an object in a two-dimensional horizontal plane is calculated. However, as other sensors, for example, a laser distance sensor that measures the three-dimensional geometric shape of an object by scanning other than in the horizontal direction, and triangulation using a multi-field camera for image feature values when an object is photographed with a camera. As long as the geometric shape can be measured, such as a stereo camera that measures the three-dimensional geometric shape of the object, the sensor method is not limited.

ロボット２００１の記憶装置２００８には、プログラムとして、ＯＳ２００９、全体制御プログラム２０１０、レーザ距離センサ２００６を用いて幾何形状データを取得するレーザ距離センサデータ取得プログラム２０１１、ユーザによるロボットの操縦のための入力と通行可能領域・進入禁止領域の教示開始・終了の入力を取得するリモコン入力取得プログラム２０１２、地図データ２０１８とレーザ距離センサ２００６から得られる幾何形状データのマッチングによってロボットの位置と姿勢を求める位置姿勢推定プログラム２０１３、得られたリモコン入力に従って車体の制御を行うための移動機構制御プログラム２０１４、教示時に記録された経路の履歴から閉じた領域を生成する経路履歴クロージングプログラム２０１５、経路の履歴によって閉じられた領域について通行可能か進入禁止かを判定する通行可能領域・進入禁止領域生成プログラム２０１６、得られた通行可能領域と進入禁止領域を統合して最終的に有効な通行可能領域・進入禁止領域を求める通行可能領域・進入禁止領域統合プログラム２０１７が格納されている。また、データとして、ロボットが自律移動する環境内の障害物の幾何形状を記録した地図データ２０１８、位置姿勢推定プログラム２０１３で推定した位置・姿勢を記録した推定位置・姿勢データ２０１９、推定位置・姿勢データをスプライン等で補間することで得られる経路履歴データ２０２０、ロボットと環境の干渉の有無の確認などに用いるロボットの形状データ２０２１が格納されている。 The storage device 2008 of the robot 2001 includes, as a program, an OS 2009, an overall control program 2010, a laser distance sensor data acquisition program 2011 that acquires geometric shape data using the laser distance sensor 2006, and an input for operating the robot by the user. Position / posture estimation for obtaining the position and posture of the robot by matching the geometric data obtained from the remote control input acquisition program 2012, the map data 2018, and the laser distance sensor 2006 for acquiring teaching start / end input of the passable area / prohibited area Program 2013, moving mechanism control program 2014 for controlling the vehicle body according to the obtained remote control input, route history closing program 2015 for generating a closed region from the history of the route recorded at the time of teaching, A passable area / entry prohibited area generation program 2016 for determining whether the area closed by history is allowed to pass or not allowed to enter, and finally integrating the obtained passable area and the entry prohibited area to obtain an effective passable area / A passable area / entry prohibited area integrated program 2017 for obtaining an entry prohibited area is stored. Further, as data, map data 2018 in which the geometric shape of the obstacle in the environment where the robot autonomously moves is recorded, estimated position / posture data 2019 in which the position / posture estimated by the position / posture estimation program 2013 is recorded, estimated position / posture Stored are path history data 2020 obtained by interpolating the data with a spline or the like, and robot shape data 2021 used for confirming the presence or absence of interference between the robot and the environment.

なお、図２では、プロセッサが、記憶装置のプログラムをメモリにロードし、プログラムを実行することで所定の処理を実現することを想定しているが、これと同等の働きをするのであれば、FPGA（Field Programmable Grid Array）やCPLD（Complex Programmable Logic Device）などのプログラマブルなハードウェアで実現してもよい。また、プログラムやデータは、CD-ROM等の記憶媒体から移してもよいし、ネットワーク経由で他の装置からダウンロードしてもよい。また、以上のハードウェアやソフトウェアは、実施形態に応じて、取捨選択してもよい。以上のハードウェア、ソフトウェアは以下で説明するロボットシステムの処理動作の動作主体として働くものとする。 In FIG. 2, it is assumed that the processor implements a predetermined process by loading a program of the storage device into the memory and executing the program. You may implement | achieve by programmable hardware, such as FPGA (Field Programmable Grid Array) and CPLD (Complex Programmable Logic Device). Further, the program and data may be transferred from a storage medium such as a CD-ROM, or may be downloaded from another device via a network. The above hardware and software may be selected according to the embodiment. It is assumed that the above hardware and software work as the operation subject of the processing operation of the robot system described below.

加えて、ロボットの構成要素については、各要素が有線の通信線にて接続されていることを想定しているが、通信が可能であれば無線であってもよい。また、通信が可能であれば、構成要素のうち、特定の要素のみが物理的に遠隔にあってもよい。以上のロボット２００１の運用例として、図５のように、５００２で表される斜線部分に設備や家具、あるいは壁面やドアが配置された環境で、水平に180度の範囲を走査するレーザ距離センサ５００４を備えるロボット５００３を、５００６の矢印で表される各経路に沿って、ユーザがリモコン操縦によってロボットを走らせた際に得られるロボットの推定位置・姿勢より通行可能領域・進入禁止領域を設定する例を想定して処理の流れを説明する。なお、ここでは、ユーザが、図５の各矢印で表される経路毎に、経路の始点にて、リモコンの教示開始ボタンを押し、終点で教示終了ボタンを押すことで、教示のためのロボットの移動中の位置・姿勢が、レーザ距離センサ２００６より得られる幾何形状データと地図データ２０１８とのマッチングによって求められ、２０１９に記録されているものとする。通行可能領域・進入禁止領域を求めるための全体の処理の流れを図３に示す。まず、地図データ２０１８、ロボットの推定位置・姿勢データ２０１９、ロボットの形状データ２０２１が読み込まれる。（３００１）。また、このとき、ロボットの位置・姿勢データ２０１９に含まれる座標データは、線分で補間され、経路履歴データ２０２０として記録される。続いて、この経路履歴毎にクロージング処理を行う（３００２）。ここでのクロージング処理とは、各経路履歴毎に、始点と終点をつなぐ処理である。ロボットの形状データ２０２１をもとに、始点と終点の距離がロボットのサイズよりも小さいと判明した場合は、ロボットの通過が想定されない１つの領域とみなして、始点と終点とをつなぎ、閉じた領域とする。経路履歴の始点と終点がロボットのサイズより大きい場合は、クロージングが行われず、閉じてない経路履歴が残ることとなる。このような閉じてない経路履歴がある場合は（３００３）、閉じられてない経路履歴を地図上で強調するようにディスプレイ２０２２に表示し、ユーザに警告する（３０１１）。ユーザは警告に従い、キーボードやマウスなどの入力機器２０２３を用い、経路履歴が閉じていない部分について、閉じてよいならば、始点や終点位置を移動させて、距離がロボットサイズ以下となるようにして、クロージングを行う。また、閉じられた経路履歴について、経路履歴同士で交わる部分があると判定された場合においても（３００４）、ユーザへの警告を出す（３０１１）。この場合も先と同様に警告に従い、交差している部分について、交差をなくすように、入力機器２０２３を用いて修正する。 In addition, the components of the robot are assumed to be connected by wired communication lines, but may be wireless as long as communication is possible. Further, if communication is possible, only a specific element among the constituent elements may be physically remote. As an example of the operation of the robot 2001 described above, as shown in FIG. 5, a laser distance sensor that scans a range of 180 degrees horizontally in an environment where equipment, furniture, wall surfaces, and doors are arranged in the hatched portion indicated by 5002. The accessible area / prohibited area is set based on the estimated position / posture of the robot 5003 provided by the user by running the robot by remote control along the route indicated by the arrow 5006. The flow of processing will be described assuming an example. Here, for each route represented by each arrow in FIG. 5, the user presses the teaching start button of the remote controller at the starting point of the route and presses the teaching end button at the end point, thereby teaching robot It is assumed that the position / orientation during movement is obtained by matching the geometric shape data obtained from the laser distance sensor 2006 with the map data 2018 and recorded in 2019. FIG. 3 shows the flow of the entire process for obtaining the passable area / prohibited area. First, map data 2018, estimated robot position / posture data 2019, and robot shape data 2021 are read. (3001). At this time, coordinate data included in the position / posture data 2019 of the robot is interpolated with line segments and recorded as route history data 2020. Subsequently, a closing process is performed for each route history (3002). The closing process here is a process for connecting the start point and the end point for each route history. If the distance between the start point and the end point is found to be smaller than the robot size based on the robot shape data 2021, it is regarded as one area where the robot is not supposed to pass, and the start point and the end point are connected and closed. This is an area. If the starting point and the ending point of the route history are larger than the size of the robot, closing is not performed and a route history that is not closed remains. If there is such an unclosed route history (3003), the unclosed route history is displayed on the display 2022 so as to be emphasized on the map, and the user is warned (3011). If the user follows the warning and uses an input device 2023 such as a keyboard or a mouse and can close the part where the route history is not closed, the start point and end point position are moved so that the distance is less than the robot size. , Closing. Further, even when it is determined that there is a portion where the route histories intersect with each other for the closed route history (3004), a warning is issued to the user (3011). In this case as well, the warning is followed and the intersection is corrected using the input device 2023 so that the intersection is eliminated.

続いて、各経路履歴毎に通行可能領域・進入禁止領域の判定を行う（３００５）。この判定処理の詳細を図４に示す。また、判定処理の例を図６に示す。処理３００５の処理開始後、まず、経路履歴に沿ってロボットを走らせながらセンサで計測を行った場合に得られるセンサデータのシミュレーションを行う（４００１）。ここでのシミュレーションとは、経路履歴上の各位置・姿勢でセンサによる計測を行うことを想定して、センサより照射されるレーザを模した線分を、センサの計測角度範囲分だけ、センサの光学中心から地図の端までを伸ばしていき、このとき各方向に伸ばされる線分が地図上の障害物と交わる箇所、各経路履歴と交わる箇所を記録することであるとする。 Subsequently, a passable area / entry prohibited area is determined for each route history (3005). Details of this determination processing are shown in FIG. An example of the determination process is shown in FIG. After the processing 3005 starts, first, simulation of sensor data obtained when measurement is performed by the sensor while running the robot along the route history is performed (4001). In this simulation, assuming that measurement is performed by the sensor at each position / posture on the route history, the line segment simulating the laser emitted from the sensor is equivalent to the measurement angle range of the sensor. It is assumed that the point extending from the optical center to the end of the map is recorded where the line segment extending in each direction intersects with an obstacle on the map and where each line history intersects.

続いて、各経路履歴において、内側の判定を行う（４００２）。これは、ある経路履歴について処理４００１で得られる線分が、当該経路履歴と交差する回数を用いて判定を行う。ロボット位置も交点とみなして交差回数に含めるとしたときに、ロボット位置から地図の端まで伸ばした線分が経路履歴と交差する回数が奇数回か偶数回かを判定し、偶数回ならば、レーザをなす線分のうち、交わる回数が奇数番目から偶数番目となるまでの範囲を経路履歴の内側と判定し、また交差回数が奇数回ならば、レーザをなす線分のうち、交わる回数が偶数番目から奇数番目となるまでの範囲を経路履歴の内側と判定する。例えば、経路履歴６０１１についてシミュレーション４００１を行った際、センサの位置・姿勢６００７でセンサによる一回の走査に含まれるデータとして、線分６００５と線分６００９が得られたとする。線分６００５と経路履歴６０１１は、点６００５と点６００４で交差回数２回となる。交差回数が偶数のため、線分の始点６００６から交点６００４までが内側と判定される。同様に、線分６００９と経路履歴６０１１は、交差回数３回で点６００８と６０１０とで交わる。交差回数が奇数のため、線分の始点６００６から交点６００８までが外側、６００８から６０１０までが内側と判定される。同様の処理を経路履歴上の各位置姿勢において、センサの計測範囲分、経路履歴一周分行う。 Subsequently, inside determination is performed in each route history (4002). This is determined using the number of times that the line segment obtained in the process 4001 for a certain route history intersects the route history. When the robot position is also considered as an intersection and included in the number of intersections, it is determined whether the number of times the line segment extending from the robot position to the edge of the map intersects the route history is an odd number or an even number. Of the line segments forming the laser, the range from the odd number to the even number of intersections is determined as the inside of the path history, and if the number of intersections is an odd number, the number of intersections among the line segments forming the laser is The range from the even number to the odd number is determined to be inside the route history. For example, when the simulation 4001 is performed on the route history 6011, it is assumed that a line segment 6005 and a line segment 6009 are obtained as data included in one scan by the sensor at the position / orientation 6007 of the sensor. In the line segment 6005 and the route history 6011, the number of intersections is two at points 6005 and 6004. Since the number of intersections is an even number, the line segment starting point 6006 to the intersection point 6004 is determined to be inside. Similarly, the line segment 6009 and the route history 6011 intersect at points 6008 and 6010 when the number of intersections is three. Since the number of intersections is an odd number, it is determined that the start point 6006 to the intersection point 6008 of the line segment is the outside, and the range from 6008 to 6010 is the inside. The same process is performed for each measurement range of the sensor and for one round of the route history at each position and orientation on the route history.

続いて、経路履歴の内側にある他の経路履歴の検出を行う（４００４）。これは、ある経路履歴について処理４００１で得られる線分が、他の経路履歴と交差する回数を用いて判定を行う。交差する回数が奇数回か偶数回かを判定し、奇数回ならば、レーザをなす線分のうち、交わる回数が偶数番目から奇数番目となるまでの範囲を他の経路履歴の内側と判定し、また交差回数が偶数回ならば、レーザをなす線分のうち、交わる回数が奇数番目から偶数番目となるまでの範囲を他の経路履歴の内側と判定する。ここまでで、各経路履歴毎に他の経路履歴を除く内側の領域が求められる。 Subsequently, another route history inside the route history is detected (4004). This is determined using the number of times that the line segment obtained in the process 4001 for a certain route history intersects with another route history. Determine whether the number of crossings is odd or even, and if it is odd, determine that the range from the even number to the odd number of lines that make up the laser is inside the other path history. If the number of intersections is an even number, the range from the odd number to the even number among the line segments forming the laser is determined to be inside the other route history. Up to this point, an inner region excluding other route histories is obtained for each route history.

次に、ここまでで求められた経路履歴の内側の領域が、通行可能領域か進入禁止領域かを判定する処理を行う（４００３）。この処理は、ある経路履歴について処理４００１で得られる線分のうち、他の経路履歴に含まれない範囲の線分と地図上の障害物と交点を検出する。この交点が1つでもあれば、処理４００４に進み、経路履歴の内側の領域かつ他の経路履歴を除く範囲が進入禁止領域と設定され（４００４）、そうでなければ同領域は通行可能領域として設定される（４００５）。例えば、経路履歴６０１３におけるセンサシミュレーション時に得られる線分６００３が得られたとすると、この線分が経路履歴の内側となるのは、点６０１３から６０１４までの範囲となる。この範囲の線分は点６００１にて地図上の障害物と交わっていることが検出され、経路履歴６０１３の内側は進入禁止領域として設定される。ここまでで、経路履歴内の他の経路履歴による領域を除く領域が通行可能領域か、進入禁止領域なのかが求められる。続いて、図６で表される地図全体が一旦進入禁止領域として設定され（３００６）、また、経路履歴による領域のうち、通行可能領域と判定された領域と進入禁止領域と判定された領域とが地図上に上書きされる形で順にそれぞれ設定される（３００７、３００８）。以上により、図６で表された地図の通行可能領域は図７の斜線部、進入禁止領域は図７で斜線部以外の領域のように求められる。なお、本実施例においては、経路履歴の内側を求めるための前処理として、仮想的なセンサより発した線分と地図上との障害物の交点を求め、これを用いて内側を求めているが、地図上を一方向に走査していく線分と障害物との交点を求め、これを用いて同様の方法により内側を判定してもよい。 Next, a process of determining whether the area inside the route history obtained so far is a passable area or an entry prohibited area is performed (4003). This process detects a line segment in a range not included in the other route history, an obstacle on the map, and an intersection among the line segments obtained in the process 4001 for a certain route history. If there is even one intersection, the process proceeds to process 4004, where the area inside the route history and the range excluding other route histories are set as the entry prohibition area (4004), otherwise the area is set as a passable area. It is set (4005). For example, if a line segment 6003 obtained at the time of sensor simulation in the route history 6013 is obtained, this line segment is inside the route history in a range from points 6013 to 6014. It is detected that a line segment in this range intersects with an obstacle on the map at a point 6001, and the inside of the route history 6013 is set as an entry prohibition area. Up to this point, it is determined whether the area excluding the area due to the other route history in the route history is the passable region or the entry prohibited region. Subsequently, the entire map shown in FIG. 6 is once set as an entry prohibition area (3006), and among the areas based on the route history, an area determined to be a passable area and an area determined to be an entry prohibition area; Are sequentially set in the form of being overwritten on the map (3007, 3008). As described above, the passable area of the map shown in FIG. 6 is obtained as a shaded area in FIG. 7, and the entry prohibition area is obtained as an area other than the shaded area in FIG. In this embodiment, as a pre-process for obtaining the inside of the route history, the intersection of the line segment emitted from the virtual sensor and the obstacle on the map is obtained, and the inside is obtained using this. However, the intersection between the line segment that scans in one direction on the map and the obstacle may be obtained, and the inside may be determined by using the same method.

実施例１では、ロボットを環境内で走行させ、運用する前の準備として、通行可能領域・進入禁止領域を設定する方法について述べた。以下では、得られた通行可能領域を用い、ロボット運用時における位置推定の異常を検出し、ユーザに警告するシステムの構成について述べる。 In the first embodiment, the method for setting the passable area and the entry prohibition area is described as preparation before the robot travels in the environment and is operated. In the following, the configuration of a system that uses the obtained passable area to detect an abnormality in position estimation during robot operation and warn the user will be described.

プログラムが開始されると、まず、通行可能領域内でのセンサデータシミュレーションが行われる（８００１）。ここでのセンサデータシミュレーションとは、地図上のある位置・姿勢でセンサによる計測を行うと仮定して、レーザを模した線分を、センサの計測角度範囲・計測距離範囲分だけ、センサの光学中心から伸ばしていき、線分と地図上の障害物がはじめに交わる位置を求める処理である。この処理を、ロボットが取りうる位置・姿勢のすべてについて行う。 When the program is started, first, sensor data simulation is performed in the passable area (8001). The sensor data simulation here assumes that the sensor measures at a certain position / orientation on the map, and the sensor's optical line is simulated by the sensor's measurement angle range and measurement distance range. It is a process of finding the position where the line segment and the obstacle on the map first meet, extending from the center. This process is performed for all positions and postures that the robot can take.

続いて、予想マッチング割合の算出を行う（８００２）。ここでの予想マッチング割合とは、処理８００１で求めた、障害物と線分との交点の数が全線分数に占める割合であり、ロボットがある位置姿勢で計測を行いってセンサデータと地図とのマッチングにもとづく位置姿勢推定を行った場合に地図と重なるセンサデータの数がセンサデータ全体の数に占める割合に相当する。この処理を、ロボットが取りうる位置・姿勢毎に求め、テーブルに記録する。 Subsequently, an expected matching ratio is calculated (8002). The predicted matching ratio here is the ratio of the number of intersections between the obstacle and the line segment obtained in the process 8001 to the total line segment. When the position / orientation estimation based on the matching is performed, the number of sensor data overlapping the map corresponds to the ratio of the total number of sensor data. This processing is obtained for each position / posture that can be taken by the robot, and recorded in a table.

次に、ロボットを目的地に自律移動させるための準備として、経路計画を行う（８００３）。 Next, as a preparation for autonomously moving the robot to the destination, route planning is performed (8003).

続いて、ロボットは、レーザ距離センサを用いて環境の計測を行う（８００４）。 Subsequently, the robot measures the environment using the laser distance sensor (8004).

得られたレーザ距離センサによるセンサデータと予め準備しておいた環境の地図とのマッチングによるロボットの位置推定が行われる（８００５）。マッチング手法としては、SSD（Sum of Squared Difference）やICP（Iterative Closest Point）など、手法を問わないが、ここではICPの利用を想定し、マッチングにより地図上でのロボットの位置姿勢が求められるものとする。この位置姿勢を合せて推定位置と呼ぶものとする。 The position of the robot is estimated by matching the sensor data obtained by the obtained laser distance sensor with an environment map prepared in advance (8005). As a matching method, any method such as SSD (Sum of Squared Difference) or ICP (Iterative Closest Point) can be used. And The position and orientation are collectively referred to as an estimated position.

次に、得られた推定位置に合せて、センサデータを地図上に配置し、地図に重なるセンサデータの数が、センサデータ数全体に占める割合を実マッチング割合として算出する（８００６）。 Next, sensor data is arranged on the map in accordance with the estimated position obtained, and the ratio of the number of sensor data overlapping the map to the total number of sensor data is calculated as an actual matching ratio (8006).

次に、テーブルより、推定位置に最も距離が短く、角度の差が小さい位置姿勢のときの予想マッチング割合を参照し（８００７）、実マッチング割合との差の絶対値が、予め定義した許容値より大きいかどうかの判定を行う（８００８）。 Next, referring to the table, the expected matching ratio when the estimated position is the shortest distance and the angle difference is the smallest is shown (8007), and the absolute value of the difference from the actual matching ratio is a predefined allowable value. It is determined whether it is larger (8008).

処理８００８による判定により、差の絶対値が許容値より大きい場合は、環境変化など、なんらかの異常が生じたとみなし、ユーザに警告を出し（８０１２）、ロボットを停車させる（８０１１）。 If the absolute value of the difference is larger than the allowable value as determined by the processing 8008, it is considered that some abnormality such as an environmental change has occurred, a warning is given to the user (8012), and the robot is stopped (8011).

一方、差の絶対値が許容値より小さい場合は、処理を進め、推定位置と目的地との比較により、目的地に到着したと判定された場合（８０１０）は停車させ（８０１１）、到着していない場合は目的地に向かう経路に追従するように移動機構を制御する（８００９）。 On the other hand, if the absolute value of the difference is smaller than the allowable value, the process proceeds. If it is determined that the destination has arrived by comparison between the estimated position and the destination (8010), the vehicle stops (8011) and arrives. If not, the moving mechanism is controlled so as to follow the route toward the destination (8009).

０００１…距離センサ部、０００２…位置推定部、０００３…移動機構制御部、０００４…移動機構部、０００５…リモコン入力取得部、０００６…経路履歴クロージング部、０００７…通行可能領域・進入禁止領域生成部、０００８…通行可能領域・進入禁止領域統合部、０００９…地図データ、００１０…推定位置・姿勢データ、００１１…通行可能領域・進入禁止領域データ DESCRIPTION OF SYMBOLS 0001 ... Distance sensor part, 0002 ... Position estimation part, 0003 ... Movement mechanism control part, 0004 ... Movement mechanism part, 0005 ... Remote control input acquisition part, 0006 ... Route history closing part, 0007 ... Passable area / entry prohibition area generation part , 0008 ... Passable area / entry prohibited area integration unit, 0009 ... Map data, 0010 ... Estimated position / posture data, 0011 ... Passable area / access prohibited area data

Claims

環境中の障害物までの距離が計測可能な距離センサ部と距離センサデータと地図とを用いて位置・姿勢の推定を行う位置姿勢推定部と、与えられた経路に従って車体制御を行う経路追従制御部と、経路追従制御部の指令に従って動作する移動機構部と、操作者からの指示を受け取るためのリモコン入力取得部からなるロボットシステムであって、
位置推定部によって求められたロボット移動時の推定位置・姿勢データをもとに移動した経路の履歴を求める経路履歴生成部と、経路履歴による閉領域と環境地図を用い、閉領域内の障害物の有無を確認することで、閉領域が通行可能領域か進入禁止領域かを判定する通行可能領域・進入禁止領域生成部と、求められた各通行可能領域と進入禁止領域を統合して、地図全体での通行可能領域と進入禁止領域とを地図として求める通行可能領域・進入禁止領域統合部を備えることを特徴とするロボットシステム。 Distance sensor unit that can measure the distance to obstacles in the environment, position / orientation estimation unit that estimates the position / orientation using distance sensor data and map, and path tracking control that performs vehicle body control according to the given route A robot system comprising: a moving unit that operates according to a command from the path following control unit; and a remote control input obtaining unit for receiving an instruction from the operator,
Obstacles in the closed area using the path history generator that obtains the history of the route traveled based on the estimated position / posture data at the time of robot movement obtained by the position estimator, and the closed area and environment map based on the path history By checking the presence / absence of a closed area, it is possible to determine whether the closed area is a passable area or an entry-prohibited area. A robot system comprising a passable area / prohibited area integration unit that obtains a map of a passable area and an entry-prohibited area as a whole.

請求項１のロボットシステムであって、経路履歴が閉じているかどうかの判定を、経路履歴の始点と終点間の距離とロボットのサイズを比較によって行い、距離がロボットのサイズより短い場合は閉領域と設定することを特徴とするロボットシステム。 2. The robot system according to claim 1, wherein whether or not the route history is closed is determined by comparing the distance between the start point and the end point of the route history and the size of the robot, and when the distance is shorter than the size of the robot, the closed region. A robot system characterized by setting.

請求項１のロボットシステムであって、通行可能領域内でロボットが取りうる位置姿勢で位置推定を行ったときに得られるマッチング割合を予めシミュレーションで求めておき、ロボットを実際の環境で走らせた際に得られるマッチング割合と比較し、それらの差が大きい場合は、環境変化などの異常があったものとして、ユーザに警告することを特徴とするロボットシステム。 The robot system according to claim 1, wherein a matching ratio obtained when position estimation is performed with a position and orientation that can be taken by the robot within a passable area is obtained in advance by simulation, and the robot is run in an actual environment. A robot system that warns the user that there is an abnormality such as an environmental change when the difference between them is large compared to the matching ratio obtained in the above.