JP2014211760A - 経路修正装置 - Google Patents
経路修正装置 Download PDFInfo
- Publication number
- JP2014211760A JP2014211760A JP2013087749A JP2013087749A JP2014211760A JP 2014211760 A JP2014211760 A JP 2014211760A JP 2013087749 A JP2013087749 A JP 2013087749A JP 2013087749 A JP2013087749 A JP 2013087749A JP 2014211760 A JP2014211760 A JP 2014211760A
- Authority
- JP
- Japan
- Prior art keywords
- route
- evaluation value
- point interval
- path
- distance
- 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.)
- Granted
Links
- 230000004048 modification Effects 0.000 title abstract description 5
- 238000012986 modification Methods 0.000 title abstract description 5
- 238000011156 evaluation Methods 0.000 claims abstract description 259
- 238000000926 separation method Methods 0.000 claims abstract description 75
- 238000012937 correction Methods 0.000 claims description 82
- 238000000034 method Methods 0.000 claims description 58
- 230000008569 process Effects 0.000 claims description 33
- 238000004364 calculation method Methods 0.000 claims description 20
- 238000012545 processing Methods 0.000 abstract description 7
- 230000007423 decrease Effects 0.000 description 12
- 230000004069 differentiation Effects 0.000 description 11
- 230000008859 change Effects 0.000 description 10
- 238000013459 approach Methods 0.000 description 9
- 238000012067 mathematical method Methods 0.000 description 4
- 238000004422 calculation algorithm Methods 0.000 description 3
- 230000000052 comparative effect Effects 0.000 description 2
- 238000002939 conjugate gradient method Methods 0.000 description 2
- 238000009499 grossing Methods 0.000 description 2
- 230000007246 mechanism Effects 0.000 description 2
- 238000002945 steepest descent method Methods 0.000 description 2
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
-
- G—PHYSICS
- G08—SIGNALLING
- G08G—TRAFFIC CONTROL SYSTEMS
- G08G1/00—Traffic control systems for road vehicles
- G08G1/16—Anti-collision systems
Landscapes
- Navigation (AREA)
- Traffic Control Systems (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
Abstract
Description
図1は、入力装置2から初期経路を入力し、その初期経路を好ましい経路に修正し、修正後経路を出力装置22から出力する経路修正装置のシステム構成を示している。入力装置2から入力した初期経路は、修正途上の経路記述パラメータ記憶装置4に記憶される。後記するように、経路記述パラメータ記憶装置4の記憶内容は、計算の進行に伴って更新されていく。経路記述パラメータ記憶装置4は、修正途上の経路記述パラメータを記憶している装置ということができる。
図2において、参照番号26は移動体を示し、出力装置22から出力された修正後経路に沿って移動する。参照番号24は移動体26を包含する凸包であり、図1の修正装置では、凸包24と障害物間の最短距離を求めて離反距離を計算する。参照番号28は移動体26に固定されている基準点であり、参照番号30は移動体26に固定されている基準線である。
図1の経路修正装置に入力される初期経路は、RRTのような確率的手法あるいはA*探索のようなグリッド探索法で求めた大局的経路である。データPi=(xi,yi,θi)のN個の並びで記述されている。A*探索のようなグリッド探索法で経路を探索すると、移動体方向θiは計算されない。この場合、経路方向と移動体方向が等しいとすることができる。あるいは別の手法で、移動体方向を決定してもよい。
(1)移動体と障害物が干渉する(重なりあう)経路である可能性がある。
(2)移動体と障害物が重なり合わないまでも非常に接近する経路である可能性がある。
(3)経路方向が急激に変化する経路である可能性がある。
(4)経路方向と移動体方向が不一致であって、移動体が追従できない経路である可能性がある。図1の修正装置は、上記の問題を解消する側に修正した経路を計算する。その修正計算のために、修正装置は下記の3種類の評価関数を利用する。
出発点から目標点に至る経路の全長(各経路点の間隔(先行経路点と後続経路点の距離)の総和)は短いことが好ましい。ただし、出発点から目標点に至る経路に、経路点の間隔が長い領域が含まれると、その領域において障害物との干渉判定の精度が低下する。その結果、障害物との干渉(衝突)が生じる経路が最終的な経路として出力される可能性が生じる。したがって、障害物との干渉が問題となる領域においては、経路点の間隔が長くなり過ぎることを抑制する必要がある。本実施例では、移動体と障害物との最短距離(離反距離)に応じて、2種類の経路点間隔評価関数を利用する。すなわち、修正途上の経路において、移動体と障害物との離反距離が設定値(正の値)以上であると判定される場合は、経路点間隔が短い経路に修正される第1経路点間隔評価関数を利用する。一方、修正途上の経路において、移動体と障害物との離反距離が設定値未満であると判定される場合は、経路点間隔が予め設定された範囲内となるように修正される第2経路点間隔評価関数を利用する。なお、移動体と障害物との距離判定については後述する。
ここで、移動体と障害物との最短距離(離反距離)について図4を参照して説明する。図4は、移動体が経路点Piに移動した状態を示し、近傍にj番目の障害物が位置している状態を示している。移動体26の凸包24の存在範囲は、座標値(xi,yi)と移動体方向θiから計算することができる。障害物は、動かない凸包32の存在範囲を記述するデータで記述されている。凸包24と凸包32の位置と姿勢が与えられれば、GJKアルゴリズムなどの既知の数学手法によって、凸包24の輪郭上にあって凸包32に最も近い点Ri,jと、凸包32の輪郭上にあって凸包24に最も近い点Oi,jを計算し、その距離lei,jを計算することができる。図示の場合は、最近接点が頂点である場合を例示しているが、辺上に最近接点がある場合にも対応することができる。図4に示す式には、近接点Oi,jの座標値と、近接点Ri,jの座標値と、最短距離(離反距離)lei,jの値を示している。したがって、最短距離(離反距離)lei,jが設定値(例えば、距離l0)以上となるか否かによって、選択する経路点間隔評価関数が変化することとなる。
上述した最短距離(離反距離)lei,jは、移動体と障害物とが干渉する確率を評価するための離反距離評価関数としても利用される。すなわち、離反距離lei,jが大きいほど移動体と障害物とが干渉する確率は低くなり、離反距離lei,jが小さいほど移動体と障害物とが干渉する確率は高くなる。したがって、離反距離を評価する離反距離評価関数によって、移動体と障害物とが干渉する確率を評価することができる。
本実施例では、移動体26は、移動方向(経路方向)と独立して車体方向(姿勢方向、移動体方向)を変える機構を備えていない。このため、経路点Pi=(xi,yi)を辿って移動する際の移動体方向θiが経路方向δiに一致することは保証されていない。その問題を解消するために、経路修正装置には経路方向δiと移動体方向θiを一致させるように修正する評価関数が導入されており、移動体がPi(xi,yi)を辿って移動すると、移動体方向θiが経路方向δiによく一致する関係が得られるようにしている。移動方向から独立して車体方向を変える機構を備えていない移動体26の経路を修正する際に、経路方向δiと独立して移動体方向θiを定義する経路記述データを採用しても、問題が生じないようにしている。
本実施例では、下記式で示されるトータル評価関数を利用する。
トータル評価関数
=経路点間隔評価関数+偏差評価関数+離反距離評価関数
=経路点間隔評価関数+第1偏差評価関数+第2偏差評価関数+離反距離評価関数
すなわち、下記の式3を用いて、経路の修正処理を実行する。
経路点間隔評価関数Uaの偏微分関数を求めると下記となる。上述したように、経路点間隔評価関数は、移動体と障害物との最短距離が設定値以上となるか否かで2種類の経路点間隔評価関数を用いる。具体的には、移動体と障害物との最短距離が設定値以上となるときの第1経路点間隔評価関数の偏微分関数は式4となり、移動体と障害物との最短距離が設定値未満となるときの第2経路点間隔評価関数の偏微分関数は式5となる。
関数とその関数の偏微分関数が判明していれば、既知の数学的手法で、関数を最小化する変数の値を決定することができる。その問題は、最急降下法、ニュートン法、準ニュートン法、共役勾配法などの勾配法で解くことができる。本実施例では、計算の収束速度と安定性のバランスが優れている準ニュートン法を用いる。
(1)現在の変数値の並び(ベクトル)よりも関数値が減少する修正方向を計算し、
(2)関数値が減少するという制約を満たす修正量で変数値の並びを修正方向に修正し、
(2)で修正された変数値の並びを(1)の変数値の並びとして(1)と(2)を繰り返す。
この結果、変数値の並びは、関数値を最小化するものに近づいていく。
関数値を最小化する変数値に近づくと、偏微分係数がゼロなって、変数値を修正方向に修正量だけ修正しても関数値は変化しなくなる。このことから、関数値を最小化する変数の並びに修正されたことがわかる。
4:修正途上の経路記述パラメータの記憶装置
5:移動体と障害物との距離を判定する判定装置
6:経路点間隔に関する評価関数の記憶装置
8:障害物までの離反距離に関する評価関数の記憶装置
10:経路方向と移動体方向の偏差に関する評価関数の記憶装置
12:トータル評価関数の記憶装置
14:経路記述パラメータについてのトータル評価関数の微分関数の記憶装置
16:探索方向ベクトルの計算装置
18:ステップ幅の設定装置
20:トータル評価値の変化量の判定装置
22:修正後経路出力装置
Claims (3)
- 経路データを入力して記憶する装置と、
入力された経路データから、経路データで記述される経路を評価したトータル評価値を計算するトータル評価関数が極値をとるときの経路データを探索する探索装置を備えており、
経路データは、経路を複数の離散した経路点で記述するデータを含んでおり、
探索装置は、
隣接する経路点の経路点間隔に関する経路点間隔評価値を計算する処理と、
移動体と障害物の離反距離に関する離反距離評価値を計算する処理と、
少なくとも経路点間隔評価値と離反距離評価値とからトータル評価値を計算する処理と、を実行し、
探索装置は、経路点間隔が予め設定された設定範囲内から外れると、無限となる経路点間隔評価値を算出することを特徴とする経路データの修正装置。 - 経路データを入力して記憶する装置と、
入力された経路データから、経路データで記述される経路を評価したトータル評価値を計算するトータル評価関数が極値をとるときの経路データを探索する探索装置を備えており、
経路データは、経路を複数の離散した経路点で記述するデータを含んでおり、
探索装置は、
隣接する経路点の経路点間隔に関する経路点間隔評価値を計算する処理と、
移動体と障害物の離反距離に関する離反距離評価値を計算する処理と、
少なくとも経路点間隔評価値と離反距離評価値とからトータル評価値を計算する処理と、を実行し、
探索装置は、さらに計算途上の経路に対して、移動体と障害物との距離を算出し、
その算出した距離が予め設定された設定長さを越えるときは、経路点間隔を短くするように評価値を算出する第1経路点間隔評価関数により経路点間隔評価値を算出し、
その算出した距離が予め設定された設定長さ以下となるときは、経路点間隔が予め設定された設定範囲内から外れると、無限となる評価値を算出する第2経路点間隔評価関数により経路点間隔評価値を算出することを特徴とする経路データの修正装置。 - 経路データは、移動体の方向を記述するデータを含んでおり、
探索装置が、さらに、経路方向と移動体方向の間の偏差に関する偏差評価値を計算する処理を実行し、
トータル評価値を計算する処理では、経路点間隔評価値と離反距離評価値と偏差評価値とからトータル評価値を計算することを特徴とする請求項1又は2に記載の経路データの修正装置。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2013087749A JP5986533B2 (ja) | 2013-04-18 | 2013-04-18 | 経路修正装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
JP2013087749A JP5986533B2 (ja) | 2013-04-18 | 2013-04-18 | 経路修正装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
JP2014211760A true JP2014211760A (ja) | 2014-11-13 |
JP5986533B2 JP5986533B2 (ja) | 2016-09-06 |
Family
ID=51931475
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
JP2013087749A Active JP5986533B2 (ja) | 2013-04-18 | 2013-04-18 | 経路修正装置 |
Country Status (1)
Country | Link |
---|---|
JP (1) | JP5986533B2 (ja) |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2017151687A (ja) * | 2016-02-24 | 2017-08-31 | 本田技研工業株式会社 | 移動体の経路計画生成装置 |
JP2019108124A (ja) * | 2016-03-15 | 2019-07-04 | 本田技研工業株式会社 | 車両制御装置、車両制御方法、および車両制御プログラム |
CN112381425A (zh) * | 2020-11-18 | 2021-02-19 | 天津市城市规划设计研究总院有限公司 | 适用于农业用地生产潜力评价的用地连片度指标计算方法 |
CN112712733A (zh) * | 2020-12-23 | 2021-04-27 | 交通运输部公路科学研究所 | 基于车路协同的碰撞预警方法、系统和路侧单元 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2004325083A (ja) * | 2003-04-21 | 2004-11-18 | I Transport Lab Co Ltd | 経路決定装置、経路決定方法および経路決定プログラム |
JP2006195969A (ja) * | 2004-12-14 | 2006-07-27 | Honda Motor Co Ltd | 自律移動ロボットの移動経路生成装置 |
JP2010127646A (ja) * | 2008-11-25 | 2010-06-10 | Toshiba Corp | 経路を選択する装置、方法およびプログラム |
JP2012190405A (ja) * | 2011-03-14 | 2012-10-04 | Toyota Motor Corp | 経路情報修正装置、軌道計画装置、及びロボット |
JP2013019683A (ja) * | 2011-07-07 | 2013-01-31 | Hitachi Ltd | 経路作成システム、経路作成方法及びプログラム |
-
2013
- 2013-04-18 JP JP2013087749A patent/JP5986533B2/ja active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2004325083A (ja) * | 2003-04-21 | 2004-11-18 | I Transport Lab Co Ltd | 経路決定装置、経路決定方法および経路決定プログラム |
JP2006195969A (ja) * | 2004-12-14 | 2006-07-27 | Honda Motor Co Ltd | 自律移動ロボットの移動経路生成装置 |
JP2010127646A (ja) * | 2008-11-25 | 2010-06-10 | Toshiba Corp | 経路を選択する装置、方法およびプログラム |
JP2012190405A (ja) * | 2011-03-14 | 2012-10-04 | Toyota Motor Corp | 経路情報修正装置、軌道計画装置、及びロボット |
JP2013019683A (ja) * | 2011-07-07 | 2013-01-31 | Hitachi Ltd | 経路作成システム、経路作成方法及びプログラム |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2017151687A (ja) * | 2016-02-24 | 2017-08-31 | 本田技研工業株式会社 | 移動体の経路計画生成装置 |
JP2019108124A (ja) * | 2016-03-15 | 2019-07-04 | 本田技研工業株式会社 | 車両制御装置、車両制御方法、および車両制御プログラム |
CN112381425A (zh) * | 2020-11-18 | 2021-02-19 | 天津市城市规划设计研究总院有限公司 | 适用于农业用地生产潜力评价的用地连片度指标计算方法 |
CN112712733A (zh) * | 2020-12-23 | 2021-04-27 | 交通运输部公路科学研究所 | 基于车路协同的碰撞预警方法、系统和路侧单元 |
Also Published As
Publication number | Publication date |
---|---|
JP5986533B2 (ja) | 2016-09-06 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP5986533B2 (ja) | 経路修正装置 | |
WO2017173990A1 (zh) | 一种机器人避障中的最短路径规划方法 | |
JP5993784B2 (ja) | 経路修正装置 | |
Yang | Anytime synchronized-biased-greedy rapidly-exploring random tree path planning in two dimensional complex environments | |
KR101585504B1 (ko) | 자율 이동 차량의 경로 생성 방법 및 경로 생성 장치 | |
CN112683278B (zh) | 一种基于改进a*算法和贝塞尔曲线的全局路径规划方法 | |
CN109491389A (zh) | 一种具有速度约束的机器人轨迹跟踪方法 | |
KR101764653B1 (ko) | 이동체의 경로계획 장치 및 그 계획 방법 | |
CN112327876B (zh) | 一种基于终距指数的机器人路径规划方法 | |
Song et al. | T $^\star $: Time-Optimal Risk-Aware Motion Planning for Curvature-Constrained Vehicles | |
US20220203534A1 (en) | Path planning method and biped robot using the same | |
EP3316065A1 (en) | Method for determining a path | |
US20210269030A1 (en) | Vehicle Control Device, Vehicle Control Method, and Vehicle Control System | |
CN108803592B (zh) | 用于避免机器人碰撞的方法、装置和系统 | |
JP5923973B2 (ja) | 経路修正装置 | |
JP2008040702A (ja) | 近似点群データからの近似曲線生成プログラム及び方法 | |
Do et al. | Narrow passage path planning using fast marching method and support vector machine | |
JP6131587B2 (ja) | 経路修正装置 | |
JP2017146893A (ja) | 自己位置推定方法 | |
Lin et al. | Humanoid navigation planning in large unstructured environments using traversability-based segmentation | |
US11579587B2 (en) | Automatic program-correction device, automatic program-correction method, and automatic path-generation device | |
CN110749332B (zh) | Rs曲线的曲率优化方法、装置、计算机设备及存储介质 | |
CN114939872B (zh) | 基于MIRRT*-Connect算法的智能仓储冗余机械臂动态避障运动规划方法 | |
JP6941039B2 (ja) | 軌道計画装置、軌道計画方法、およびプログラム | |
JP2006155559A (ja) | 経路計画装置 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20150611 |
|
A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20160413 |
|
A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20160426 |
|
A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20160607 |
|
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: 20160802 |
|
A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20160805 |
|
R150 | Certificate of patent or registration of utility model |
Ref document number: 5986533 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |
|
R250 | Receipt of annual fees |
Free format text: JAPANESE INTERMEDIATE CODE: R250 |