JP5886502B2 - MOBILE BODY CONTROL DEVICE, MOBILE BODY CONTROL METHOD, AND CONTROL PROGRAM - Google Patents

MOBILE BODY CONTROL DEVICE, MOBILE BODY CONTROL METHOD, AND CONTROL PROGRAM Download PDF

Info

Publication number
JP5886502B2
JP5886502B2 JP2012278099A JP2012278099A JP5886502B2 JP 5886502 B2 JP5886502 B2 JP 5886502B2 JP 2012278099 A JP2012278099 A JP 2012278099A JP 2012278099 A JP2012278099 A JP 2012278099A JP 5886502 B2 JP5886502 B2 JP 5886502B2
Authority
JP
Japan
Prior art keywords
route
distance point
moving body
obstacle
moving
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.)
Expired - Fee Related
Application number
JP2012278099A
Other languages
Japanese (ja)
Other versions
JP2014123200A (en
Inventor
豊 高岡
豊 高岡
亮暢 藤井
亮暢 藤井
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Toyota Motor Corp
Toyota Central R&D Labs Inc
Original Assignee
Toyota Motor Corp
Toyota Central R&D Labs Inc
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Toyota Motor Corp, Toyota Central R&D Labs Inc filed Critical Toyota Motor Corp
Priority to JP2012278099A priority Critical patent/JP5886502B2/en
Publication of JP2014123200A publication Critical patent/JP2014123200A/en
Application granted granted Critical
Publication of JP5886502B2 publication Critical patent/JP5886502B2/en
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Landscapes

  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Description

本発明は、移動体の移動経路を算出し、その移動経路に従って移動体の移動を制御する移動体制御装置、移動体制御方法及び制御プログラムに関するものである。   The present invention relates to a moving body control device, a moving body control method, and a control program for calculating a moving path of a moving body and controlling the movement of the moving body according to the moving path.

近年、計測センサなどを用いて移動体周囲の障害物を検出して移動経路を設定し、その移動経路に従って自律的に移動する様々な自律移動体が提案されている。例えば、3次元地図情報を生成し、生成した3次元地図情報を用いて障害物を回避しつつ移動を行う自律移動体が知られている(特許文献1参照)。   In recent years, various autonomous mobile bodies have been proposed in which obstacles around a moving body are detected using a measurement sensor or the like to set a movement path and move autonomously according to the movement path. For example, an autonomous moving body that generates 3D map information and moves while avoiding an obstacle using the generated 3D map information is known (see Patent Document 1).

特開2006−236098号公報JP 2006-236098 A

しかしながら、上記特許文献1に示す自律移動体においては、移動経路上の障害物を検出しグリッドマップに設定された場合、次回、同一の経路を移動する場合もその障害物を回避する移動経路を設定することとなる。一方で、障害物には、壁などの定常的に固定された静的障害物と、椅子などの移動させることが可能な準動的障害物と、人など移動する動的障害物が存在する。このため、例えば、一定時間経過後には、障害物の種類によってはその障害物が移動し回避の必要が無くなる可能性もある。したがって、上記のように、障害物を常に回避する移動経路を設定した場合に、無駄に迂回する経路が設定されるため、最適に移動経路が設定できない可能が生じる。   However, in the autonomous mobile body shown in Patent Document 1, when an obstacle on the moving route is detected and set in the grid map, a moving route that avoids the obstacle next time when moving on the same route is set. It will be set. On the other hand, obstacles include static obstacles that are constantly fixed such as walls, semi-dynamic obstacles that can be moved such as chairs, and dynamic obstacles that move such as people. . For this reason, for example, after a certain period of time, depending on the type of obstacle, the obstacle may move and there is no need to avoid it. Therefore, as described above, when a movement route that always avoids an obstacle is set, a route that is detoured wastefully is set, so that it may not be possible to set the movement route optimally.

本発明は、このような問題点を解決するためになされたものであり、最適な移動経路を設定することができる移動体制御装置、移動体制御方法及び制御プログラムを提供することを主たる目的とする。   The present invention has been made to solve such problems, and has as its main object to provide a mobile control device, a mobile control method and a control program capable of setting an optimal travel path. To do.

上記目的を達成するための本発明の一態様は、移動体周囲の障害物に対する距離点データを取得する距離点取得手段と、前記距離点取得手段により取得された前記障害物の距離点データに基づいて、前記障害物の外形を示すポリゴンマップを生成するポリゴンマップ生成手段と、前記移動体の移動経路を前記移動体の移動環境を示す格子状のグリッドマップ上で算出する経路算出手段と、前記経路算出手段により算出された移動経路を修正する経路修正手段と、を備え、前記経路修正手段は、前記経路算出手段により算出された移動経路上で、前記ポリゴンマップ生成手段により生成された障害物のポリゴンマップに基づいて通過不能と判断した前記移動経路の経路部分に対して、前記距離点取得手段により距離点データを取得し、該取得した距離点データを記録する、ことを特徴とする移動体制御装置である。
この一態様において、前記経路修正手段は、前記経路算出手段により算出された移動経路で通過不能と判断した経路部分があるとき、該経路部分に対して記録されている前記距離点データと、前記距離点取得手段により再度取得された前記経路部分に対する距離点データと、を比較してもよい。
この一態様において、前記経路修正手段は、前記経路部分に記録されている距離点データと、前記距離点取得手段により再度取得された経路部分に対する距離点データと、が一致するとき、前記経路部分を通過不能な禁止領域として確定してもよい。
この一態様において、前記経路修正手段が、前記経路部分に記録されている距離点データと、前記距離点取得手段により再度取得された経路部分に対する距離点データと、が一致しないと判断したとき、前記経路算出手段は、前記移動体の移動経路を再計算してもよい。
この一態様において、前記経路修正手段は、前記距離点取得手段により取得された前記障害物の距離点データに基づいて、前記通過不能と判断した経路部分において動的障害物が存在すると判断したとき、所定時間経過後、前記移動経路の再修正を行ってもよい。
この一態様において、前記経路修正手段は、前記距離点取得手段により取得された前記障害物の距離点データに基づいて、前記通過不能と判断した経路部分において静的障害物のみが存在すると判断したとき、該経路部分に対して通過不能を示す通過不可ポテンシャルを設定してもよい。
この一態様において、前記経路修正手段は、前記経路部分に対する通過不可ポテンシャルの重みを時間経過と共に減少させ、前記経路部分を移動経路として設定可能にしてもよい。
この一態様において、前記経路修正手段は、前記移動経路の再修正を行った結果、前記通過不能と判断した経路部分を再度、通過不能と判断したとき、該経路部分に対して、前記距離点取得手段により距離点データを取得し、該取得した距離点データを記録してもよい。
この一態様において、前記経路修正手段は、前記移動経路の再修正を行った結果、前記通過不能と判断した経路部分を、通過可能と判断したとき、該再修正後の移動経路を最終的な移動経路として設定してもよい。
この一態様において、前記経路修正手段により修正された移動経路に従って、前記移動体の移動を制御する移動制御手段を更に備えていてもよい。
他方、上記目的を達成するための本発明の一態様は、移動体周囲の障害物に対する距離点データを取得するステップと、前記取得された前記障害物の距離点データに基づいて、前記障害物の外形を示すポリゴンマップを生成するステップと、前記移動体の移動経路を前記移動体の移動環境を示す格子状のグリッドマップ上で算出するステップと、前記算出された移動経路を修正するステップと、前記算出された移動経路上で、前記生成された障害物のポリゴンマップに基づいて通過不能と判断した前記移動経路の経路部分に対して、前記距離点データを取得し、該取得した距離点データを記録するステップと、を含むことを特徴とする移動体制御方法であってもよい。
さらに、上記目的を達成するための本発明の一態様は、移動体周囲の障害物に対する距離点データを取得する処理と、前記取得された前記障害物の距離点データに基づいて、前記障害物の外形を示すポリゴンマップを生成する処理と、前記移動体の移動経路を前記移動体の移動環境を示す格子状のグリッドマップ上で算出する処理と、前記算出された移動経路上で、前記生成された障害物のポリゴンマップに基づいて通過不能と判断した前記移動経路の経路部分に対して、前記距離点データを取得し、該取得した距離点データを記録する処理と、をコンピュータに実行させることを特徴とする制御プログラムであってもよい。
One aspect of the present invention for achieving the above object is that distance point acquisition means for acquiring distance point data for obstacles around a moving body, and distance point data of the obstacle acquired by the distance point acquisition means. A polygon map generating means for generating a polygon map indicating the outline of the obstacle, a path calculating means for calculating a moving path of the moving object on a grid-like grid map indicating a moving environment of the moving object, Route correction means for correcting the movement route calculated by the route calculation means, wherein the route correction means is a fault generated by the polygon map generation means on the movement route calculated by the route calculation means. The distance point data is acquired by the distance point acquisition unit for the path portion of the movement path that is determined to be impossible to pass based on the polygon map of the object. Distance point data is recorded, it is a mobile control device according to claim.
In this one aspect, when there is a route portion that is determined to be unpassable in the travel route calculated by the route calculation means, the route correction means, the distance point data recorded for the route portion, You may compare the distance point data with respect to the said route part acquired again by the distance point acquisition means.
In this one aspect, the route correction means, when the distance point data recorded in the route portion matches the distance point data for the route portion acquired again by the distance point acquisition means, the route portion May be determined as a prohibited area that cannot pass through.
In this aspect, when the route correction unit determines that the distance point data recorded in the route portion and the distance point data for the route portion acquired again by the distance point acquisition unit do not match, The route calculating means may recalculate the moving route of the moving body.
In this one aspect, when the route correcting means determines that there is a dynamic obstacle in the route portion determined not to pass based on the distance point data of the obstacle acquired by the distance point acquiring means. The travel route may be re-corrected after a predetermined time has elapsed.
In this aspect, the route correcting unit determines that only a static obstacle exists in the route portion determined to be unpassable based on the distance point data of the obstacle acquired by the distance point acquiring unit. At this time, a non-passable potential indicating non-passability may be set for the route portion.
In this aspect, the route correcting unit may reduce the weight of the impassable potential for the route portion with the passage of time so that the route portion can be set as a moving route.
In this one aspect, when the route correcting means determines that the route portion determined to be unpassable again as a result of the re-correction of the moving route, the distance point is determined with respect to the route portion. The distance point data may be acquired by the acquisition means, and the acquired distance point data may be recorded.
In this aspect, when the route correction means determines that the route portion determined to be unpassable as a result of re-correction of the travel route is determined to be passable, the route correction means finally determines the travel route after the re-correction. It may be set as a movement route.
In this aspect, the apparatus may further include movement control means for controlling movement of the moving body in accordance with the movement path corrected by the path correction means.
On the other hand, according to one aspect of the present invention for achieving the above object, the obstacle is obtained based on the step of obtaining distance point data for the obstacle around the moving body, and the obtained distance point data of the obstacle. Generating a polygon map indicating the outer shape of the moving object, calculating a moving path of the moving object on a grid-like grid map indicating a moving environment of the moving object, and correcting the calculated moving path; The distance point data is acquired for the route portion of the movement route that is determined to be unpassable on the calculated movement route based on the polygon map of the generated obstacle, and the obtained distance point And a step of recording data.
Further, according to one aspect of the present invention for achieving the above object, the obstacle is based on a process of acquiring distance point data for an obstacle around a moving body and the acquired distance point data of the obstacle. A process for generating a polygon map indicating the outer shape of the mobile object, a process for calculating a moving path of the moving object on a grid-like grid map indicating a moving environment of the moving object, and the generation on the calculated moving path The computer executes the process of acquiring the distance point data and recording the acquired distance point data for the path portion of the moving path that is determined to be unpassable based on the polygon map of the obstacle It may be a control program characterized by this.

本発明によれば、最適な移動経路を設定することができる移動体制御装置、移動体制御方法及び制御プログラムを提供することができる。   ADVANTAGE OF THE INVENTION According to this invention, the mobile body control apparatus which can set an optimal movement path | route, a mobile body control method, and a control program can be provided.

本発明の一実施の形態に係る移動体制御装置の概略的なシステム構成を示すブロック図である。It is a block diagram which shows the schematic system configuration | structure of the moving body control apparatus which concerns on one embodiment of this invention. (a)障害物のポリゴンマップの一例を示す図である。(b)図2(a)に示すポリゴンマップに対応するグリッドマップを示す図である。(A) It is a figure which shows an example of the polygon map of an obstruction. (B) It is a figure which shows the grid map corresponding to the polygon map shown to Fig.2 (a). 障害物及び移動経路が設定されたグリッドマップの一例を示す図である。It is a figure which shows an example of the grid map in which the obstruction and the movement path | route were set. (a)移動経路が滑らかな曲率変化となるように修正する方法を示す図である。(b)移動経路が滑らかな曲率変化となるように修正する方法を示す図である。(c)移動経路が滑らかな曲率変化となるように修正する方法を示す図である。(A) It is a figure which shows the method of correct | amending so that a movement path | route may become a smooth curvature change. (B) It is a figure which shows the method of correct | amending so that a movement path | route may become a smooth curvature change. (C) It is a figure which shows the method of correct | amending so that a movement path | route may become a smooth curvature change. 本発明の一実施の形態に係る移動体制御方法のフローを示すフローチャートである。It is a flowchart which shows the flow of the moving body control method which concerns on one embodiment of this invention. 袋小路の問題を説明するための図である。It is a figure for demonstrating the problem of a dead end. (a)動的障害物の一例を示す図である。(b)準動的障害物の一例を示す図である。(c)静的障害物の一例を示す図である。(A) It is a figure which shows an example of a dynamic obstruction. (B) It is a figure which shows an example of a semi-dynamic obstacle. (C) It is a figure which shows an example of a static obstruction. 本発明の一実施の形態に係る移動体制御装置による経路計画方法のフローを示すフローチャートである。It is a flowchart which shows the flow of the route planning method by the mobile body control apparatus which concerns on one embodiment of this invention. 本発明の一実施の形態に係る移動体制御装置による経路計画方法のフローを示すフローチャートである。It is a flowchart which shows the flow of the route planning method by the mobile body control apparatus which concerns on one embodiment of this invention.

以下、図面を参照して本発明の実施の形態について説明する。
本発明の一実施の形態に係る移動体制御装置は、自律的に移動する自律移動体に搭載されており、自律移動体の移動経路を算出し、算出した移動経路に従って自律移動体の移動を制御する。なお、自律移動体としては、例えば、二足歩行ロボット、車輪駆動型ロボットなどの自律移動が可能な任意の自律型移動体に適用可能である。
Embodiments of the present invention will be described below with reference to the drawings.
A mobile body control device according to an embodiment of the present invention is mounted on an autonomous mobile body that moves autonomously, calculates a travel path of the autonomous mobile body, and moves the autonomous mobile body according to the calculated travel path. Control. In addition, as an autonomous mobile body, it can apply to arbitrary autonomous mobile bodies which can autonomously move, such as a bipedal walking robot and a wheel drive type robot, for example.

図1は、本発明の一実施の形態に係る移動体制御装置の概略的なシステム構成を示すブロック図である。本実施の形態に係る移動体制御装置1は、距離センサ2と、ポリゴンマップ生成部3と、経路算出部4と、経路修正部5と、移動制御部6と、自己位置推定部7と、を備えている。   FIG. 1 is a block diagram showing a schematic system configuration of a mobile control device according to an embodiment of the present invention. The mobile body control device 1 according to the present embodiment includes a distance sensor 2, a polygon map generation unit 3, a route calculation unit 4, a route correction unit 5, a movement control unit 6, a self-position estimation unit 7, It has.

なお、移動体制御装置1は、例えば、演算処理、制御処理等と行うCPU(Central Processing Unit)、CPUによって実行される演算プログラム、制御プログラム等が記憶されたROM(Read Only Memory)やRAM(Random Access Memory)からなるメモリ、外部と信号の入出力を行うインターフェイス部(I/F)、などからなるマイクロコンピュータを中心にして、ハードウェア構成されている。CPU、メモリ、及びインターフェイス部は、データバスなどを介して相互に接続されている。   The mobile control device 1 includes, for example, a CPU (Central Processing Unit) that performs arithmetic processing, control processing, and the like, a ROM (Read Only Memory) or a RAM (RAM) that stores arithmetic programs executed by the CPU, control programs, and the like. Random Access Memory), and a microcomputer composed of an external interface unit (I / F) for inputting / outputting signals to / from the outside, and the like, are configured as hardware. The CPU, memory, and interface unit are connected to each other via a data bus or the like.

距離センサ2は、距離点取得手段の一具体例であり、自律移動体に設けられており、自律移動体の周囲に存在する障害物の3次元点群データを取得する。   The distance sensor 2 is a specific example of a distance point acquisition unit, is provided in the autonomous mobile body, and acquires 3D point cloud data of an obstacle existing around the autonomous mobile body.

ここで、距離センサ2は、平面検出処理を行うことで、取得した3次元点群データのうち、自律移動体が移動する平面(床面)を除去する。これにより、距離センサ2は障害物のみの3次元点群データを取得することができる。なお、距離センサ2の視野が狭い場合は、過去の数フレームを重畳させて視野を拡大してもよい。   Here, the distance sensor 2 removes a plane (floor surface) on which the autonomous mobile body moves from the acquired three-dimensional point group data by performing a plane detection process. Thereby, the distance sensor 2 can acquire three-dimensional point cloud data of only an obstacle. In addition, when the visual field of the distance sensor 2 is narrow, the visual field may be enlarged by superimposing several past frames.

距離センサ2は、例えば、超音波センサ、レーザセンサ、3次元画像カメラなどで構成されている。距離センサ2は取得した3次元点群データをポリゴンマップ生成部3、経路算出部4及び経路修正部5に対して出力する。   The distance sensor 2 includes, for example, an ultrasonic sensor, a laser sensor, a three-dimensional image camera, and the like. The distance sensor 2 outputs the acquired three-dimensional point cloud data to the polygon map generation unit 3, the route calculation unit 4, and the route correction unit 5.

ポリゴンマップ生成部3は、ポリゴンマップ生成手段の一具体例であり、距離センサ2により取得された3次元点群データに基づいて、自律移動体周囲の障害物のポリゴンマップを生成する(図2(a))。このポリゴンマップは、格子状のグリッドマップ(図2(b))と比較して障害物の外形をポリゴンで高精度に表現できる。なお、ポリゴンマップ生成部3は、例えば、距離センサ2により取得された3次元点群を平面に投影して2次元点群を生成し、その2次元点群の外側の点を直線で結ぶことで、障害物の外形を示すポリゴンマップを生成する。ポリゴンマップ生成部3は、生成した障害物のポリゴンマップを経路修正部5に対して出力する。   The polygon map generation unit 3 is a specific example of polygon map generation means, and generates a polygon map of obstacles around the autonomous mobile body based on the three-dimensional point cloud data acquired by the distance sensor 2 (FIG. 2). (A)). This polygon map can express the outline of an obstacle with polygons with higher accuracy than a grid-like grid map (FIG. 2B). The polygon map generator 3 generates a two-dimensional point group by projecting, for example, a three-dimensional point group acquired by the distance sensor 2 and connects the points outside the two-dimensional point group with straight lines. Then, a polygon map showing the outline of the obstacle is generated. The polygon map generation unit 3 outputs the generated polygon map of the obstacle to the path correction unit 5.

経路算出部4は、経路算出手段の一具体例であり、自律移動体100の移動環境を示すグリッドマップを生成し、始点から終点までの最短の移動経路をグリッドマップ上で算出する。経路算出部4は、予め設定された地図情報に基づいて、位置が定常的に不動な静的障害物(壁、テーブルなど)の領域をグリッドマップ上に設定する(図3)。さらに、経路算出部4は、距離センサ2により取得された動的な障害物である動的障害物(人、動物、ロボットなど)の3次元点群を2次元平面に投影して2次元点群を生成し、その2次元点群を埋めるグリッドをその動的障害物の領域としてグリッドマップ上に逐次設定する。   The route calculation unit 4 is a specific example of route calculation means, generates a grid map indicating the movement environment of the autonomous mobile body 100, and calculates the shortest movement route from the start point to the end point on the grid map. The route calculation unit 4 sets an area of a static obstacle (wall, table, etc.) whose position is stationary constantly on the grid map based on preset map information (FIG. 3). Further, the route calculation unit 4 projects a three-dimensional point group of dynamic obstacles (people, animals, robots, etc.) that are dynamic obstacles acquired by the distance sensor 2 onto a two-dimensional plane, thereby producing a two-dimensional point. A group is generated, and a grid that fills the two-dimensional point group is sequentially set on the grid map as the area of the dynamic obstacle.

経路算出部4は、例えば、距離センサ2により取得された障害物の3次元点群の時系列変化に基づいて、その障害物が動的障害物および静的障害物のいずれかを判断することができる。経路算出部4は、距離センサ2により取得された障害物の3次元点群に時系列変化がある場合、その障害物を動的障害物と判断し、障害物の3次元点群に時系列変化がない場合、その障害物を静的障害物と判断する。さらに、経路算出4部は、距離センサ2により取得された障害物の3次元点群データに基づいてパターン認識を行い、その障害物が何であるかを判断し、その障害物が動的障害物および静的障害物のいずれに属するかを判断してよい。   For example, the route calculation unit 4 determines whether the obstacle is a dynamic obstacle or a static obstacle based on the time-series change of the three-dimensional point cloud of the obstacle acquired by the distance sensor 2. Can do. When there is a time-series change in the 3D point cloud of the obstacle acquired by the distance sensor 2, the route calculation unit 4 determines that the obstacle is a dynamic obstacle and time-series the 3D point cloud of the obstacle. If there is no change, the obstacle is determined as a static obstacle. Further, the route calculation unit 4 performs pattern recognition based on the three-dimensional point cloud data of the obstacle acquired by the distance sensor 2, determines what the obstacle is, and the obstacle is a dynamic obstacle. And whether it belongs to a static obstacle.

経路算出部4は、静的障害物及び動的障害物の領域の周りに自律移動体100の外形分だけ自律移動体100の移動を禁止する禁止領域を設定してもよい。これにより、自律移動体100を1グリッドの点で表現し移動経路を容易に設定できる。経路算出部4は、算出したグリッドマップを経路修正部5に対して出力する。経路算出部4は、上記のような禁止領域を含む静的及び動的障害物を回避しつつ、最短の移動経路を算出する。   The route calculation unit 4 may set a prohibited area in which movement of the autonomous mobile body 100 is prohibited by the outer shape of the autonomous mobile body 100 around the areas of the static obstacle and the dynamic obstacle. Thereby, the autonomous mobile body 100 can be expressed by a point of one grid, and the movement route can be easily set. The route calculation unit 4 outputs the calculated grid map to the route correction unit 5. The route calculation unit 4 calculates the shortest movement route while avoiding the static and dynamic obstacles including the prohibited area as described above.

経路修正部5は、ポリゴンマップ生成部3により生成された障害物のポリゴンマップに基づいて、自律移動体100が障害物に干渉せず、自律移動体100の姿勢変化が連続的(移動経路が滑らかな曲率変化)となるように、経路算出部4に算出された移動経路の修正を行う。   Based on the polygon map of the obstacle generated by the polygon map generator 3, the route correction unit 5 does not interfere with the obstacle, and the posture change of the autonomous mobile body 100 is continuous (the movement route is The travel route calculated by the route calculation unit 4 is corrected so as to achieve a smooth curvature change.

例えば、図4(a)に示すように経路算出部4により算出された初期の移動経路が障害物を回避しつつ、移動経路が不連続な曲率変化となっている場合を想定する。この場合、経路修正部5は、図4(b)に示すような仮想バネによる修正を行い、図4(c)に示すような滑らかな曲率変化となる移動経路に修正を行う。ここで、仮想バネとして、例えば、隣接する経路点間に伸長バネ(第1バネ)及び捻りバネ(第2バネ)を設定し、自律移動体100と障害物との間に圧縮バネ(第3バネ)を設定することで、上記修正を行う。経路修正部5は、修正した移動経路を移動体制御部6に対して出力する。   For example, as shown in FIG. 4A, a case is assumed where the initial movement route calculated by the route calculation unit 4 avoids an obstacle and the movement route has a discontinuous curvature change. In this case, the path correction unit 5 performs correction by a virtual spring as shown in FIG. 4B, and corrects the moving path that has a smooth curvature change as shown in FIG. 4C. Here, as the virtual spring, for example, an extension spring (first spring) and a torsion spring (second spring) are set between adjacent path points, and a compression spring (third) is set between the autonomous mobile body 100 and the obstacle. The above correction is performed by setting a spring. The route correcting unit 5 outputs the corrected moving route to the moving body control unit 6.

移動制御部6は、移動制御手段の一具体例であり、自律移動体100が経路修正部5により修正された移動経路に従って移動するように、自律移動体100を制御する。
自己位置推定部7は、自律移動体100の自己位置を推定する。自己位置推定部7は、例えば、オドメトリあるいはレーザセンサによる検出結果と地図情報とのマッチングなどの周知の手法を用いて自立移動体100の自己位置を推定することができる。
The movement control unit 6 is a specific example of a movement control unit, and controls the autonomous moving body 100 so that the autonomous moving body 100 moves according to the movement route corrected by the route correction unit 5.
The self position estimation unit 7 estimates the self position of the autonomous mobile body 100. The self-position estimation unit 7 can estimate the self-position of the self-supporting moving body 100 by using a known method such as matching between a detection result by odometry or a laser sensor and map information.

図5は、本実施の形態に係る移動体制御方法のフローを示すフローチャートである。距離センサ2は、自律移動体100の周囲に存在する障害物の3次元点群データを取得し(ステップS101)、ポリゴンマップ生成部3、及び経路修正部5に対して出力する。   FIG. 5 is a flowchart showing a flow of the mobile control method according to the present embodiment. The distance sensor 2 acquires three-dimensional point cloud data of an obstacle existing around the autonomous mobile body 100 (step S101), and outputs it to the polygon map generation unit 3 and the route correction unit 5.

ポリゴンマップ生成部3は、距離センサ2により取得された3次元点群データに基づいて、障害物のポリゴンマップを生成し(ステップS102)、経路修正部5に対して出力する。   The polygon map generation unit 3 generates a polygon map of the obstacle based on the three-dimensional point cloud data acquired by the distance sensor 2 (step S102), and outputs it to the route correction unit 5.

経路算出部4は、距離センサ2により取得された動的障害物の3次元点群に基づいて、グリッドマップを生成し(ステップS103)、設定された始点から終点までの最短の移動経路をグリッドマップ上で算出し(ステップS104)、経路修正部5に出力する。   The route calculation unit 4 generates a grid map based on the three-dimensional point cloud of the dynamic obstacle acquired by the distance sensor 2 (step S103), and grids the shortest movement route from the set start point to the end point. It is calculated on the map (step S104) and output to the route correction unit 5.

経路修正部5は、ポリゴンマップ生成部3により生成された障害物のポリゴンマップに基づいて、経路算出部4に算出された移動経路において、自律移動体100が障害物に干渉しないように、移動経路の修正を行い(ステップS105)、移動体制御部6に対して出力する。このように、グリッドマップによる移動経路探索と、狭路などの詳細移動に対するポリゴンマップ修正と、を行うことで、高速な広域移動と高精度な詳細移動を両立することができる。   The route correction unit 5 moves on the movement route calculated by the route calculation unit 4 based on the obstacle polygon map generated by the polygon map generation unit 3 so that the autonomous mobile body 100 does not interfere with the obstacle. The route is corrected (step S105) and output to the moving body control unit 6. As described above, by performing the movement route search by the grid map and the polygon map correction for the detailed movement such as a narrow road, it is possible to achieve both high-speed wide area movement and high-precision detailed movement.

移動制御部6は、経路修正部5により修正された移動経路に従って、自律移動体100の移動を制御する(ステップS106)。   The movement control unit 6 controls the movement of the autonomous mobile body 100 according to the movement route corrected by the route correction unit 5 (step S106).

ところで、自律移動体100がグリッドマップ上では狭路などを通過できると判断し、その後、より高精度なポリゴンマップを用いて同一の狭路について干渉判定を行ったところ、通過不能と判断される所謂袋小路の問題が生じている(図6)。ここで、例えば、人などの移動する動的障害物(図7(a))や、椅子などの静的ではあるが移動可能な準動的障害物(図7(b))により、その挟路が通過不能と判断される場合がある。この場合、後に、その障害物の移動によりその狭路が通過可能となる可能性がある。一方で、壁などの定常的に固定された静的障害物(図7(c))によりその挟路が通過不能と判断された場合、後に、その狭路が通過可能となる可能性は略あり得ない。したがって、通過不能なる狭路に対して、グリッドマップ上でその障害物のより詳細な情報を設定し、その後の移動経路の計算にその情報を用いるのが望ましい。   By the way, it is determined that the autonomous mobile body 100 can pass a narrow road on the grid map, and then interference determination is performed on the same narrow road using a higher-accuracy polygon map. A so-called bag path problem has occurred (FIG. 6). Here, for example, a moving obstacle such as a person (FIG. 7A) or a static but movable semi-dynamic obstacle such as a chair (FIG. 7B) sandwiches the object. It may be determined that the road cannot pass. In this case, there is a possibility that the narrow path can be passed later due to the movement of the obstacle. On the other hand, if it is determined that the narrow path cannot pass due to a stationary fixed obstacle such as a wall (FIG. 7C), the possibility that the narrow path can be passed later is almost impossible. Therefore, it is desirable to set more detailed information of the obstacle on the grid map for the narrow road that cannot pass, and use the information for the calculation of the subsequent movement route.

そこで、本実施の形態に係る移動体制御装置1において、経路修正部5は、経路算出部4により算出された移動経路上で、ポリゴンマップ生成部3により生成された障害物のポリゴンマップに基づいて通過不能と判断した経路部分に対して、距離センサ2により3次元点群データを取得し、取得した3次元点群データを記録する。   Therefore, in the moving body control device 1 according to the present embodiment, the route correction unit 5 is based on the obstacle polygon map generated by the polygon map generation unit 3 on the movement route calculated by the route calculation unit 4. The three-dimensional point cloud data is acquired by the distance sensor 2 for the path portion determined to be unpassable, and the acquired three-dimensional point cloud data is recorded.

これにより、自律移動体100が再度その通過不能と判断された経路部分に来たときに、記録したその経路部分の3次元点群データを用いて、経路部分に存在する動的障害物、準動的障害物、或いは静的障害物に応じたより最適な移動経路を設定できる。さらに、通過不能と判断した経路部分に対してのみ、3次元点群データを記録することから、そのデータ処理量を低減でき、処理速度の高速化を図ることができる。   As a result, when the autonomous mobile body 100 comes again to the route portion that is determined to be unpassable, the recorded dynamic 3D point cloud data of the route portion is used. It is possible to set a more optimal movement route according to a dynamic obstacle or a static obstacle. Furthermore, since the three-dimensional point cloud data is recorded only for the path portion that is determined not to pass, the data processing amount can be reduced, and the processing speed can be increased.

図8及び図9は、上述した本実施の形態に係る移動体制御装置による経路計画方法のフローを示すフローチャートである。まず、図8に示すフローチャートを用いて、自律移動体が最初に挟路などの通過不能の経路部分を通過する際の処理フローについて詳細に説明する。   8 and 9 are flowcharts showing a flow of the route planning method by the mobile control device according to the present embodiment described above. First, with reference to the flowchart shown in FIG. 8, a processing flow when the autonomous mobile body first passes through a path portion that cannot pass, such as a narrow path, will be described in detail.

経路算出部4は、距離センサ2により取得された動的障害物の3次元点群に基づいて、グリッドマップを生成し、始点から終点までの最短の移動経路をグリッドマップ上で算出する(ステップS201)。   The route calculation unit 4 generates a grid map based on the three-dimensional point cloud of the dynamic obstacle acquired by the distance sensor 2, and calculates the shortest movement route from the start point to the end point on the grid map (step) S201).

経路修正部5は、ポリゴンマップ生成部3により生成された障害物のポリゴンマップに基づいて、経路算出部4により算出された移動経路において、自律移動体100が障害物に干渉せず、自律移動体100の姿勢変化が連続的となるように、移動経路の修正を行う(ステップS202)。   The route correcting unit 5 performs autonomous movement on the moving route calculated by the route calculating unit 4 based on the polygon map of the obstacle generated by the polygon map generating unit 3 without interfering with the obstacle. The movement path is corrected so that the posture change of the body 100 is continuous (step S202).

経路修正部5は、上記修正において、経路算出部4により算出された移動経路で通過不能と判断した経路部分があるか否かを判断する(ステップS203)。   The route correcting unit 5 determines whether or not there is a route portion that is determined to be unpassable in the travel route calculated by the route calculating unit 4 in the above correction (step S203).

経路修正部5は、経路算出部4により算出された移動経路で通過不能と判断した経路部分があるとき(ステップS203のYES)、距離センサ2により取得された障害物の3次元点群データに基づいて、その通過不能と判断した経路部分に静的障害物のみが存在するか否かを判断する(ステップS204)。一方、経路修正部5は、経路算出部4に算出された移動経路で通過不能と判断した経路部分がないとき(ステップS203のNO)、後述の(ステップS210)に移行する。   When there is a route portion that is determined to be unpassable on the travel route calculated by the route calculation unit 4 (YES in step S203), the route correction unit 5 uses the three-dimensional point cloud data of the obstacle acquired by the distance sensor 2 as the path portion. Based on this, it is determined whether or not only a static obstacle exists in the route portion determined to be unpassable (step S204). On the other hand, the route correcting unit 5 proceeds to (step S210), which will be described later, when there is no route portion determined to be impossible to pass in the travel route calculated by the route calculating unit 4 (NO in step S203).

経路修正部5は、距離センサ2により取得された障害物の3次元点群データに基づいて、その通過不能と判断した経路部分に動的障害物が存在すると判断したとき(ステップS204のNO)、所定時間経過後、自律移動体100が障害物に干渉しないように、移動経路の再修正を行う(ステップS205)。このように、動的障害物は、通常、一定時間経過後に移動を行うものである。したがって、その経路部分が動的障害物により通過不能の場合でも、一定時間経過後にその動的障害物が移動し、通過可能となる可能性が高い。したがって、上述のように、所定時間だけ待って移動経路の再修正を行い、その経路部分に経路設定が可能かを再確認するのである。これにより、本来通過可能な経路部分を迂回するというような無駄な移動経路の設定を排除することができる。   When the path correction unit 5 determines based on the 3D point cloud data of the obstacle acquired by the distance sensor 2 that a dynamic obstacle exists in the path portion determined to be unpassable (NO in step S204). After a predetermined time has elapsed, the movement route is re-corrected so that the autonomous mobile body 100 does not interfere with the obstacle (step S205). As described above, the dynamic obstacle usually moves after a certain period of time. Therefore, even when the route portion cannot pass due to a dynamic obstacle, there is a high possibility that the dynamic obstacle moves and can pass after a certain period of time. Therefore, as described above, the travel route is re-corrected after waiting for a predetermined time, and it is reconfirmed whether the route can be set in the route portion. As a result, it is possible to eliminate the setting of a useless travel route that bypasses the originally passable route portion.

経路修正部5は、再修正後の移動経路の経路部分が通過可能か否かを判断する(ステップS206)。経路修正部5は、再修正後の移動経路の経路部分が通過不能と判断したとき(ステップS206のNO)、後述の(ステップS207)の処理に移行する。一方、経路修正部5は、再修正後の移動経路の経路部分が通過可能であると判断したとき(ステップS206のYES)、後述の(ステップS210)に移行する。   The route correction unit 5 determines whether or not the route portion of the travel route after recorrection can pass (step S206). When the route correction unit 5 determines that the route portion of the re-corrected movement route cannot pass (NO in step S206), the route correction unit 5 proceeds to a process described later (step S207). On the other hand, when the route correction unit 5 determines that the route portion of the re-corrected movement route can pass (YES in Step S206), the route correction unit 5 proceeds to (Step S210) described later.

経路修正部5は、距離センサ2により取得された障害物の3次元点群データに基づいて、その通過不能と判断した経路部分に静的障害物のみが存在すると判断したとき(ステップS204のYES)、距離センサ2にその経路部分の3次元点群データを取得させ、グリッドマップのその経路部分に記録する(ステップS207)。   When the path correction unit 5 determines that only a static obstacle exists in the path portion determined to be unpassable based on the three-dimensional point cloud data of the obstacle acquired by the distance sensor 2 (YES in step S204). ), The distance sensor 2 acquires the three-dimensional point cloud data of the route portion, and records it in the route portion of the grid map (step S207).

経路修正部5は、グリッドマップにおける移動経路のその経路部分に通過不能を示す通過不可ポテンシャルを設定する(ステップS208)。このように、経路部分に静的障害物が存在し通過不能となっている場合、通常、静的障害物は定常的に固定されているため、その通過不能の状態は時間経過しても変わることは略あり得ない。よって、その経路部分に通過不可ポテンシャルを設定し、原則として、次回からその経路部分を回避した移動経路を設定する。これにより、通過不能な経路部分に移動経路が設定されることが無い為、無駄な移動経路の設定を排除することができる。   The route correction unit 5 sets a non-passable potential indicating that it is impossible to pass through the route portion of the moving route in the grid map (step S208). Thus, when there is a static obstacle in the route portion and it is impossible to pass through, the static obstacle is normally fixed constantly, so that the passage impossible state changes even with time. That is almost impossible. Therefore, a non-passable potential is set for the route portion, and in principle, a movement route that avoids the route portion is set from the next time. As a result, a travel route is not set for a route portion that cannot pass, and therefore, a useless travel route setting can be eliminated.

ここで、経路修正部5は、グリッドマップに設定する通過不可ポテンシャルの重みを時間経過と共に減少(漸減あるいは段階的に減少)するように設定してもよい。これは、静的障害物の中に準動的障害物が含まれている場合、一定時間経過後に、その準動的障害物が移動し(例えば、椅子など小型軽量の準動的障害物は容易に移動可能である)、一旦、通過不能と判断した経路部分が通過可能となる可能性があるからである。   Here, the route correction unit 5 may set the weight of the impassable potential set in the grid map so as to decrease (gradually decrease or stepwise decrease) with time. This is because, if a static obstacle contains a quasi-dynamic obstacle, the quasi-dynamic obstacle moves after a certain period of time (for example, a small and lightweight quasi-dynamic obstacle such as a chair This is because there is a possibility that a route portion once determined to be unpassable may be allowed to pass through.

この場合、経路算出部4は、他に最適な経路が存在しない場合、時間経過に伴ってその重みが減少した通過不可ポテンシャルの経路部分を再度、移動経路の一部として設定することとなる。これにより、通過可能となり得る経路部分を直ちに迂回するというような無駄な移動経路の設定を排除することができる。   In this case, when there is no other optimum route, the route calculation unit 4 sets the route portion of the impassable potential whose weight has decreased with the passage of time as a part of the moving route again. As a result, it is possible to eliminate a useless travel route setting such as immediately detouring a route portion that can be passed.

経路修正部5は、通過不可ポテンシャルが設定されたグリッドマップ上で移動経路の再計算を行う(ステップS209)。   The route correcting unit 5 recalculates the travel route on the grid map in which the impassable potential is set (step S209).

移動制御部6は経路修正部5により再計算された移動経路に従って移動するように、自律移動体100の移動を制御する(ステップS210)。   The movement control unit 6 controls the movement of the autonomous mobile body 100 so as to move according to the movement route recalculated by the route correction unit 5 (step S210).

続いて、図9に示すフローチャートを用いて、自律移動体100が二回目以降に挟路などの通過不能の経路部分を通過する際の処理フローについて詳細に説明する。   Next, a processing flow when the autonomous mobile body 100 passes through a non-passable route portion such as a narrow path after the second time will be described in detail with reference to a flowchart shown in FIG.

経路算出部4は、距離センサ2により取得された動的障害物の3次元点群に基づいて、グリッドマップを生成し、設定された始点から終点までの最短の移動経路をグリッドマップ上で算出する(ステップS301)。   The route calculation unit 4 generates a grid map based on the three-dimensional point cloud of the dynamic obstacle acquired by the distance sensor 2, and calculates the shortest movement route from the set start point to the end point on the grid map. (Step S301).

経路修正部5は、ポリゴンマップ生成部3により生成された障害物のポリゴンマップに基づいて、経路算出部4に算出された移動経路において、自律移動体100が障害物に干渉しないように、移動経路の修正を行う(ステップS302)。   The route correction unit 5 moves on the movement route calculated by the route calculation unit 4 based on the obstacle polygon map generated by the polygon map generation unit 3 so that the autonomous mobile body 100 does not interfere with the obstacle. The route is corrected (step S302).

経路修正部5は、上記修正において、経路算出部4に算出された移動経路で通過不能と判断した経路部分(通過不能ポテンシャルの設定)があるか否かを判断する(ステップS303)。   The route correcting unit 5 determines whether or not there is a route portion (setting of a non-passable potential) that is determined to be non-passable in the travel route calculated by the route calculating unit 4 in the above correction (step S303).

経路修正部5は、経路算出部4に算出された移動経路で通過不能と判断した経路部分があるとき(ステップS303のYES)、通過不能と判断した経路部分に対して3次元点群データが記録されているか否かを判断する(ステップS304)。一方、経路修正部5は、経路算出部4に算出された移動経路で通過不能と判断した経路部分がないとき(ステップS303のNO)、後述の(ステップS309)の処理に移行する。   When there is a route portion that is determined to be non-passable in the travel route calculated by the route calculation unit 4 (YES in step S303), the route correction unit 5 stores the 3D point cloud data for the route portion determined to be non-passable. It is determined whether or not it is recorded (step S304). On the other hand, when there is no route portion that is determined to be unpassable in the travel route calculated by the route calculation unit 4 (NO in step S303), the route correction unit 5 proceeds to the process described later (step S309).

経路修正部5は、通過不能と判断した経路部分に対して3次元点群データが記録されていると判断したとき(ステップS304のYES)、距離センサ2により再度取得された経路部分に対する3次元点群データと、記録されている3次元点群データとを比較する(ステップS305)。この比較により、一度、通過不能と判断した経路部分に対して、再度、障害物が移動して通過可能となったか否かを判断することができる。   When the path correction unit 5 determines that the three-dimensional point cloud data is recorded for the path part determined not to pass (YES in step S304), the path correction unit 5 performs the three-dimensional process for the path part acquired again by the distance sensor 2. The point cloud data is compared with the recorded three-dimensional point cloud data (step S305). By this comparison, it is possible to determine again whether or not the obstacle has moved to be able to pass through the path portion that has been determined to be unpassable.

一方、経路修正部5は、通過不能と判断した経路部分に対して3次元点群データが記録されていないと判断したとき(ステップS304のNO)、上述の(ステップS204)(図8)の処理に移行する。この場合、自律移動体100がその通過不能と判断した経路部分を、初めて通過することとなるため、上記図8に示す初回通過のフローに戻る。   On the other hand, when the path correction unit 5 determines that the 3D point cloud data is not recorded for the path portion determined to be unpassable (NO in step S304), the above-described (step S204) (FIG. 8). Transition to processing. In this case, since the autonomous mobile body 100 passes for the first time the route portion determined to be unpassable, the flow returns to the first-pass flow shown in FIG.

ここで、経路修正部5は、例えば、ICP(Interative Closest Point)アルゴリズムを用いて、距離センサ2により再度取得された経路部分に対する3次元点群データと、登録されている3次元点群データとを比較して、その一致度を算出する。   Here, the route correcting unit 5 uses, for example, an ICP (Interative Closest Point) algorithm, the 3D point cloud data for the route portion acquired again by the distance sensor 2, and the registered 3D point cloud data. And the degree of coincidence is calculated.

経路修正部5は、上記経路部分の3次元点群データの比較を行った結果、その経路部分の環境が前回と一致していると判断したとき(ステップS306のYES)、その通過不能ポテンシャルの経路部分を通過不能領域(禁止領域)としてグリッドマップ上に確定する(ステップS307)。この場合、自律移動体100が今回通過するときと前回通過するときとで、その経路部分の障害物の位置に変化が無い為、最終的にその経路部分を通過不能領域としてグリッドマップに確定できる。   As a result of comparing the three-dimensional point cloud data of the route portion, the route correcting unit 5 determines that the environment of the route portion matches the previous time (YES in step S306), and the path impossibility potential is determined. The route portion is determined on the grid map as a non-passable area (prohibited area) (step S307). In this case, since there is no change in the position of the obstacle in the route portion between the time when the autonomous mobile body 100 passes this time and the time when it passes the previous time, the route portion can be finally determined as a non-passable area in the grid map. .

一方、経路修正部5は、上記経路部分の3次元点群データの比較を行った結果、その経路部分の環境が前回と一致していないと判断したとき(ステップS306のNO)、後述の(ステップS308)の処理に移行する。この場合、自律移動体100が今回通過するときと前回通過するときとで、その経路部分の障害物が移動しその位置に変化している判断できる。したがって、後述のように、再度、移動経路の再計算を行う。   On the other hand, as a result of comparing the three-dimensional point cloud data of the route portion, the route correcting unit 5 determines that the environment of the route portion does not match the previous time (NO in step S306), which will be described later ( The process proceeds to step S308). In this case, it can be determined that the obstacle of the route portion has moved and changed to the position when the autonomous mobile body 100 passes this time and when it passes the previous time. Therefore, as will be described later, the movement route is recalculated again.

経路修正部5は、グリッドマップ上で移動経路の再計算を行う(ステップS308)。
移動制御部6は経路修正部5により再計算された移動経路に従って、自律移動体100の移動を制御する(ステップS309)。
The route correction unit 5 recalculates the travel route on the grid map (step S308).
The movement control unit 6 controls the movement of the autonomous mobile body 100 in accordance with the movement route recalculated by the route correction unit 5 (step S309).

以上、本実施の形態に係る移動体制御装置1において、経路算出部4により算出された移動経路上で、ポリゴンマップ生成部3により生成された障害物のポリゴンマップに基づいて通過不能と判断した経路部分に対して、距離センサ2により3次元点群データを取得し、取得した3次元点群データを記録する。これにより、その経路部分の3次元点群データを用いて、経路部分に存在する障害物に応じたより最適な移動経路を設定できる。   As described above, in the moving body control device 1 according to the present embodiment, it is determined that the vehicle cannot pass on the moving route calculated by the route calculating unit 4 based on the polygon map of the obstacle generated by the polygon map generating unit 3. For the path portion, the distance sensor 2 acquires 3D point cloud data and records the acquired 3D point cloud data. Thereby, it is possible to set a more optimal movement route according to the obstacle existing in the route portion using the three-dimensional point cloud data of the route portion.

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

また、本発明は、例えば、図5、図8及び図9に示す処理を、CPUにコンピュータプログラムを実行させることにより実現することも可能である。   Further, the present invention can be realized by causing the CPU to execute a computer program, for example, the processes shown in FIGS. 5, 8, and 9.

プログラムは、様々なタイプの非一時的なコンピュータ可読媒体(non-transitory computer readable medium)を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体(tangible storage medium)を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体(例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ)、光磁気記録媒体(例えば光磁気ディスク)、CD−ROM(Read Only Memory)、CD−R、CD−R/W、半導体メモリ(例えば、マスクROM、PROM(Programmable ROM)、EPROM(Erasable PROM)、フラッシュROM、RAM(random access memory))を含む。   The program may be stored using various types of non-transitory computer readable media and supplied to a computer. Non-transitory computer readable media include various types of tangible storage media. Examples of non-transitory computer-readable media include magnetic recording media (for example, flexible disks, magnetic tapes, hard disk drives), magneto-optical recording media (for example, magneto-optical disks), CD-ROMs (Read Only Memory), CD-Rs, CD-R / W and semiconductor memory (for example, mask ROM, PROM (Programmable ROM), EPROM (Erasable PROM), flash ROM, RAM (random access memory)) are included.

また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体(transitory computer readable medium)によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。   The program may also be supplied to the computer by various types of transitory computer readable media. Examples of transitory computer readable media include electrical signals, optical signals, and electromagnetic waves. The temporary computer-readable medium can supply the program to the computer via a wired communication path such as an electric wire and an optical fiber, or a wireless communication path.

1 移動体制御装置
2 距離センサ
3 ポリゴンマップ生成部
4 経路算出部
5 経路修正部
6 移動制御部
7 自己位置推定部
100 自律移動体
DESCRIPTION OF SYMBOLS 1 Mobile body control apparatus 2 Distance sensor 3 Polygon map production | generation part 4 Path | route calculation part 5 Path | route correction part 6 Movement control part 7 Self-position estimation part 100 Autonomous mobile body

Claims (10)

移動体周囲の障害物に対する距離点データを取得する距離点取得手段と、
前記距離点取得手段により取得された前記障害物の距離点データに基づいて、前記障害物の外形を示すポリゴンマップを生成するポリゴンマップ生成手段と、
前記移動体の移動経路を前記移動体の移動環境を示す格子状のグリッドマップ上で算出する経路算出手段と、
前記経路算出手段により算出された移動経路を修正する経路修正手段と、を備え、
経路修正手段は、前記経路算出手段により算出された移動経路上で、前記ポリゴンマップ生成手段により生成された障害物のポリゴンマップに基づいて通過不能と判断した前記移動経路の経路部分に、静的障害物のみが存在すると判断したとき、該経路部分に対して、前記距離点取得手段により距離点データを取得し、該取得した距離点データを記録し、
該経路部分に、通過不能を示す通過不可ポテンシャルを設定し、該通過不可ポテンシャルの重みを時間経過と共に減少させ、該経路部分を移動経路として設定可能にする、
ことを特徴とする移動体制御装置。
Distance point acquisition means for acquiring distance point data for obstacles around the moving body;
A polygon map generating means for generating a polygon map indicating an outline of the obstacle based on the distance point data of the obstacle acquired by the distance point acquiring means;
Path calculating means for calculating a moving path of the moving body on a grid-like grid map indicating a moving environment of the moving body;
Route correction means for correcting the movement route calculated by the route calculation means,
The route correcting means statically applies a route portion of the moving route that is determined to be unpassable on the moving route calculated by the route calculating means based on the polygon map of the obstacle generated by the polygon map generating means. when it is determined that only the obstacle is present, for the said path portion acquires distance point data by the distance point obtaining means records the distance point data the acquired,
A non-passable potential indicating impossibility of passing is set in the route portion, the weight of the non-passable potential is decreased with time, and the route portion can be set as a moving route.
The moving body control apparatus characterized by the above-mentioned.
請求項1記載の移動体制御装置であって、
前記経路修正手段は、前記経路算出手段により算出された移動経路で通過不能と判断した経路部分があるとき、該経路部分に対して記録されている前記距離点データと、前記距離点取得手段により再度取得された前記経路部分に対する距離点データと、を比較する、ことを特徴とする移動体制御装置。
The moving body control device according to claim 1,
When there is a route portion that is determined to be unpassable on the travel route calculated by the route calculation unit, the route correction unit uses the distance point data recorded for the route portion and the distance point acquisition unit. A moving body control device that compares distance point data for the route portion acquired again.
請求項2記載の移動体制御装置であって、
前記経路修正手段は、前記経路部分に記録されている距離点データと、前記距離点取得手段により再度取得された経路部分に対する距離点データと、が一致するとき、前記経路部分を通過不能な禁止領域として確定する、ことを特徴とする移動体制御装置。
The moving body control device according to claim 2,
The route correction means prohibits the passage portion from being unable to pass through when the distance point data recorded in the route portion and the distance point data for the route portion acquired again by the distance point acquisition means match. A moving body control device characterized by being determined as a region.
請求項2又は3記載の移動体制御装置であって、
前記経路修正手段が、前記経路部分に記録されている距離点データと、前記距離点取得手段により再度取得された経路部分に対する距離点データと、が一致しないと判断したとき、前記経路算出手段は、前記移動体の移動経路を再計算する、ことを特徴とする移動体制御装置。
The moving body control device according to claim 2 or 3,
When the route correction means determines that the distance point data recorded in the route portion and the distance point data for the route portion acquired again by the distance point acquisition means do not match, the route calculation means A moving body control device that recalculates a moving path of the moving body.
請求項1乃至4のうちいずれか1項記載の移動体制御装置であって、
前記経路修正手段は、前記距離点取得手段により取得された前記障害物の距離点データに基づいて、前記通過不能と判断した経路部分において動的障害物が存在すると判断したとき、所定時間経過後、前記移動経路の再修正を行う、ことを特徴とする移動体制御装置。
The mobile body control device according to any one of claims 1 to 4,
When the route correcting means determines that a dynamic obstacle exists in the route portion determined to be impossible to pass based on the distance point data of the obstacle acquired by the distance point acquiring means, after a predetermined time has elapsed. The moving body control device, wherein the moving path is re-corrected.
請求項5記載の移動体制御装置であって、
前記経路修正手段は、前記移動経路の再修正を行った結果、前記通過不能と判断した経路部分を再度、通過不能と判断したとき、該経路部分に対して、前記距離点取得手段により距離点データを取得し、該取得した距離点データを記録する、を特徴とする移動体制御装置。
The moving body control device according to claim 5,
When the route correction unit determines that the route portion determined to be unpassable as a result of re-correction of the moving route is determined to be unpassable again, the distance point acquisition unit performs distance points on the route portion. A moving body control device characterized by acquiring data and recording the acquired distance point data.
請求項5記載の移動体制御装置であって、
前記経路修正手段は、前記移動経路の再修正を行った結果、前記通過不能と判断した経路部分を、通過可能と判断したとき、該再修正後の移動経路を最終的な移動経路として設定する、ことを特徴とする移動体制御装置。
The moving body control device according to claim 5,
The route correction means sets the re-corrected movement route as the final movement route when the route portion determined to be unpassable as a result of re-correction of the movement route is determined to be passable. The moving body control apparatus characterized by the above-mentioned.
請求項1乃至のうちいずれか1項記載の移動体制御装置であって、
前記経路修正手段により修正された移動経路に従って、前記移動体の移動を制御する移動制御手段を更に備える、ことを特徴とする移動体制御装置。
The mobile body control device according to any one of claims 1 to 7 ,
A moving body control device further comprising movement control means for controlling movement of the moving body in accordance with the movement path corrected by the path correction means.
移動体周囲の障害物に対する距離点データを取得するステップと、
前記取得された前記障害物の距離点データに基づいて、前記障害物の外形を示すポリゴンマップを生成するステップと、
前記移動体の移動経路を前記移動体の移動環境を示す格子状のグリッドマップ上で算出するステップと、
前記算出された移動経路を修正するステップと、
前記算出された移動経路上で、前記生成された障害物のポリゴンマップに基づいて通過不能と判断した前記移動経路の経路部分に、静的障害物のみが存在すると判断したとき、該経路部分に対して、前記距離点データを取得し、該取得した距離点データを記録するステップと、
該経路部分に、通過不能を示す通過不可ポテンシャルを設定し、該通過不可ポテンシャルの重みを時間経過と共に減少させ、該経路部分を移動経路として設定可能にするステップと、
を含むことを特徴とする移動体制御方法。
Obtaining distance point data for obstacles around the moving body;
Generating a polygon map showing an outline of the obstacle based on the acquired distance point data of the obstacle;
Calculating a moving path of the moving body on a grid-like grid map indicating a moving environment of the moving body;
Modifying the calculated travel route;
When it is determined that only a static obstacle exists in the route part of the movement route that is determined to be impossible to pass on the calculated movement route based on the polygon map of the generated obstacle, the route part includes On the other hand, acquiring the distance point data and recording the acquired distance point data;
Setting a non-passable potential indicating impossibility of passing in the route portion, reducing the weight of the non-passable potential with time, and enabling the route portion to be set as a movement route;
A moving body control method comprising:
移動体周囲の障害物に対する距離点データを取得する処理と、
前記取得された前記障害物の距離点データに基づいて、前記障害物の外形を示すポリゴンマップを生成する処理と、
前記移動体の移動経路を前記移動体の移動環境を示す格子状のグリッドマップ上で算出する処理と、
前記算出された移動経路上で、前記生成された障害物のポリゴンマップに基づいて通過不能と判断した前記移動経路の経路部分に、静的障害物のみが存在すると判断したとき、該経路部分に対して、前記距離点データを取得し、該取得した距離点データを記録する処理と、
該経路部分に、通過不能を示す通過不可ポテンシャルを設定し、該通過不可ポテンシャルの重みを時間経過と共に減少させ、該経路部分を移動経路として設定可能にする処理と、
をコンピュータに実行させることを特徴とする制御プログラム。
Processing to obtain distance point data for obstacles around the moving body;
Based on the acquired distance point data of the obstacle, a process of generating a polygon map indicating the outline of the obstacle;
Processing for calculating a moving path of the moving body on a grid-like grid map indicating a moving environment of the moving body;
When it is determined that only a static obstacle exists in the route part of the movement route that is determined to be impossible to pass on the calculated movement route based on the polygon map of the generated obstacle, the route part includes On the other hand, a process of acquiring the distance point data and recording the acquired distance point data;
A process of setting a non-passable potential indicating impossibility of passing in the path part, reducing a weight of the non-passable potential with time, and setting the path part as a moving path;
A control program for causing a computer to execute.
JP2012278099A 2012-12-20 2012-12-20 MOBILE BODY CONTROL DEVICE, MOBILE BODY CONTROL METHOD, AND CONTROL PROGRAM Expired - Fee Related JP5886502B2 (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
JP2012278099A JP5886502B2 (en) 2012-12-20 2012-12-20 MOBILE BODY CONTROL DEVICE, MOBILE BODY CONTROL METHOD, AND CONTROL PROGRAM

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2012278099A JP5886502B2 (en) 2012-12-20 2012-12-20 MOBILE BODY CONTROL DEVICE, MOBILE BODY CONTROL METHOD, AND CONTROL PROGRAM

Publications (2)

Publication Number Publication Date
JP2014123200A JP2014123200A (en) 2014-07-03
JP5886502B2 true JP5886502B2 (en) 2016-03-16

Family

ID=51403642

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2012278099A Expired - Fee Related JP5886502B2 (en) 2012-12-20 2012-12-20 MOBILE BODY CONTROL DEVICE, MOBILE BODY CONTROL METHOD, AND CONTROL PROGRAM

Country Status (1)

Country Link
JP (1) JP5886502B2 (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20180105984A (en) * 2017-03-16 2018-10-01 엘지전자 주식회사 Method of modifying path using around map and robot implementing thereof
KR20210034895A (en) * 2019-09-23 2021-03-31 포항공과대학교 산학협력단 Method and Underwater Robot for Scan Route Setting of Underwater Object using Acoustic Camera

Families Citing this family (28)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6559556B2 (en) * 2015-12-07 2019-08-14 株式会社データ変換研究所 Object position database construction system
JP6685755B2 (en) * 2016-02-16 2020-04-22 東芝ライフスタイル株式会社 Autonomous vehicle
CN105843225B (en) * 2016-03-31 2022-01-25 纳恩博(北京)科技有限公司 Data processing method and equipment
EP3611589B1 (en) * 2017-04-11 2021-11-17 Amicro Semiconductor Co., Ltd. Method for controlling motion of robot based on map prediction
JP6919307B2 (en) * 2017-04-25 2021-08-18 トヨタ自動車株式会社 Autonomous mobile
JP7052260B2 (en) * 2017-09-01 2022-04-12 富士フイルムビジネスイノベーション株式会社 Camera-equipped mobiles, camera-equipped mobile control systems, and programs
KR102070817B1 (en) * 2018-03-23 2020-01-30 한국과학기술원 Method for determining usable airspace for unmanned aerial vehicle operations
JP7063066B2 (en) 2018-03-30 2022-05-09 トヨタ自動車株式会社 Route planning device, route planning method and program
KR102485554B1 (en) * 2018-06-29 2023-01-05 현대오토에버 주식회사 Apparatus and method for constructing a 3d space map for route search for unmanned aerial vehicle
JP6823015B2 (en) 2018-07-17 2021-01-27 ファナック株式会社 Robot system
EP3633404B1 (en) * 2018-10-02 2022-09-07 Ibeo Automotive Systems GmbH Method and apparatus for optical distance measurements
US11747825B2 (en) * 2018-10-12 2023-09-05 Boston Dynamics, Inc. Autonomous map traversal with waypoint matching
CN109782765A (en) * 2019-01-22 2019-05-21 南京晓庄学院 A kind of trolley driving path correcting system and its correcting method
CN109857112A (en) * 2019-02-21 2019-06-07 广东智吉科技有限公司 Obstacle Avoidance and device
KR102275300B1 (en) 2019-07-05 2021-07-08 엘지전자 주식회사 Moving robot and control method thereof
KR102224637B1 (en) 2019-07-05 2021-03-08 엘지전자 주식회사 Moving robot and control method thereof
KR102297496B1 (en) * 2019-07-11 2021-09-02 엘지전자 주식회사 A ROBOT CLEANER Using artificial intelligence AND CONTROL METHOD THEREOF
KR102361130B1 (en) 2019-07-11 2022-02-09 엘지전자 주식회사 Moving robot and control method thereof
KR102302575B1 (en) 2019-07-16 2021-09-14 엘지전자 주식회사 Moving robot and control method thereof
CN110320919B (en) * 2019-07-31 2022-05-20 河海大学常州校区 Method for optimizing path of mobile robot in unknown geographic environment
JP7299097B2 (en) * 2019-08-02 2023-06-27 国立大学法人信州大学 Trajectory generator
US11268816B2 (en) 2019-08-06 2022-03-08 Boston Dynamics, Inc. Intermediate waypoint generator
US11262759B2 (en) * 2019-10-16 2022-03-01 Huawei Technologies Co., Ltd. Method and system for localization of an autonomous vehicle in real time
CN110879594B (en) * 2019-11-25 2020-11-17 广西科技师范学院 Big data-based robot path planning data management system
CN110956327B (en) * 2019-11-29 2024-04-26 上海有个机器人有限公司 Multi-robot automatic parking method, medium, terminal and device
CN111543908B (en) * 2020-05-15 2021-09-07 汇智机器人科技(深圳)有限公司 Method and device for planning travelling path and intelligent equipment travelling path
CN113934218B (en) * 2021-11-16 2022-03-25 杭州云象商用机器有限公司 Cleaning robot path planning method, device, equipment and storage medium
JP2024042454A (en) * 2022-09-15 2024-03-28 三菱重工業株式会社 Route setting method, program and moving body

Family Cites Families (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPS57108778A (en) * 1980-12-26 1982-07-06 Toshiba Corp Position detector
JP2748281B2 (en) * 1989-08-14 1998-05-06 本田技研工業株式会社 Self-propelled work robot
JP2006236098A (en) * 2005-02-25 2006-09-07 Mitsubishi Heavy Ind Ltd Obstacle avoiding method for mobile body, and the mobile body
JP2007328486A (en) * 2006-06-07 2007-12-20 Yaskawa Electric Corp Moving robot
JP5112666B2 (en) * 2006-09-11 2013-01-09 株式会社日立製作所 Mobile device
KR100988568B1 (en) * 2008-04-30 2010-10-18 삼성전자주식회사 Robot and method for building map of the same
JP5312894B2 (en) * 2008-10-08 2013-10-09 村田機械株式会社 Autonomous mobile body and movement control method for autonomous mobile body
JP5319255B2 (en) * 2008-12-03 2013-10-16 綜合警備保障株式会社 Autonomous mobile body and speed control method
JP2011175393A (en) * 2010-02-24 2011-09-08 Toyota Motor Corp Route planning apparatus, autonomous mobile robot, and method for planning movement path
JP2011204181A (en) * 2010-03-26 2011-10-13 Nec Soft Ltd Route setting device, route setting method and program

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR20180105984A (en) * 2017-03-16 2018-10-01 엘지전자 주식회사 Method of modifying path using around map and robot implementing thereof
KR102286656B1 (en) 2017-03-16 2021-08-04 엘지전자 주식회사 Method of modifying path using around map and robot implementing thereof
KR20210034895A (en) * 2019-09-23 2021-03-31 포항공과대학교 산학협력단 Method and Underwater Robot for Scan Route Setting of Underwater Object using Acoustic Camera
KR102276943B1 (en) * 2019-09-23 2021-07-13 포항공과대학교 산학협력단 Method and Underwater Robot for Scan Route Setting of Underwater Object using Acoustic Camera

Also Published As

Publication number Publication date
JP2014123200A (en) 2014-07-03

Similar Documents

Publication Publication Date Title
JP5886502B2 (en) MOBILE BODY CONTROL DEVICE, MOBILE BODY CONTROL METHOD, AND CONTROL PROGRAM
JP5724919B2 (en) Orbit generation device, moving body, orbit generation method and program
US8849559B2 (en) Apparatus and method for generating and using a grid map path
US9207674B2 (en) Autonomous moving robot and control method thereof
JP4715787B2 (en) Mobile robot and robot movement control method
JP2012190405A (en) Route information correcting device, track planning device, and robot
JP5208649B2 (en) Mobile device
JP5381679B2 (en) Route search system, method, program, and moving object
JP5287051B2 (en) Autonomous mobile device
JP6235216B2 (en) Autonomous mobile body and control method thereof
JP2013132748A (en) Moving device and method for recognizing position of the same
JP5793851B2 (en) POSITION ESTIMATION METHOD, POSITION ESTIMATION DEVICE, AND PROGRAM
WO2016067640A1 (en) Autonomous moving device
CN113405557B (en) Path planning method and related device, electronic equipment and storage medium
JP5063549B2 (en) Mobile device
JP5650021B2 (en) Three-dimensional environment restoration apparatus, processing method thereof, and program
JP7058761B2 (en) Mobile control device, mobile control learning device, and mobile control method
JP7235060B2 (en) Route planning device, route planning method, and program
KR101436858B1 (en) Path generating method for autonomous mobile robot using uncertainty
CN112639648B (en) Method for controlling movement of plurality of vehicles, movement control device, movement control system, program, and recording medium
JP6686411B2 (en) Map making method
JP5435729B2 (en) Walking landing position planning method for legged robot
JP7480975B2 (en) Travel route generation device, travel device, travel route generation method, and program
JPWO2020074860A5 (en)
JP2017037468A (en) Route search system, route search method and program

Legal Events

Date Code Title Description
A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20150107

A977 Report on retrieval

Free format text: JAPANESE INTERMEDIATE CODE: A971007

Effective date: 20151125

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20151201

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20151224

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20160126

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20160211

R151 Written notification of patent or utility model registration

Ref document number: 5886502

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R151

R250 Receipt of annual fees

Free format text: JAPANESE INTERMEDIATE CODE: R250

LAPS Cancellation because of no payment of annual fees