JP5923973B2 - Route correction device - Google Patents
Route correction device Download PDFInfo
- Publication number
- JP5923973B2 JP5923973B2 JP2011284391A JP2011284391A JP5923973B2 JP 5923973 B2 JP5923973 B2 JP 5923973B2 JP 2011284391 A JP2011284391 A JP 2011284391A JP 2011284391 A JP2011284391 A JP 2011284391A JP 5923973 B2 JP5923973 B2 JP 5923973B2
- Authority
- JP
- Japan
- Prior art keywords
- route
- evaluation value
- description data
- moving body
- path
- 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
Links
- 238000012937 correction Methods 0.000 title claims description 92
- 238000011156 evaluation Methods 0.000 claims description 233
- 238000000926 separation method Methods 0.000 claims description 84
- 238000000034 method Methods 0.000 claims description 65
- 238000004364 calculation method Methods 0.000 claims description 32
- 230000008569 process Effects 0.000 claims description 25
- 230000004069 differentiation Effects 0.000 claims description 13
- 230000007423 decrease Effects 0.000 description 10
- 238000013459 approach Methods 0.000 description 9
- 238000005516 engineering process Methods 0.000 description 9
- 230000008859 change Effects 0.000 description 8
- 238000012545 processing Methods 0.000 description 6
- 238000004422 calculation algorithm Methods 0.000 description 4
- 238000012067 mathematical method Methods 0.000 description 4
- 238000002939 conjugate gradient method Methods 0.000 description 3
- 238000009958 sewing Methods 0.000 description 3
- 238000002945 steepest descent method Methods 0.000 description 3
- 230000007246 mechanism Effects 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 238000013473 artificial intelligence Methods 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000009499 grossing Methods 0.000 description 1
Images
Landscapes
- Navigation (AREA)
- Traffic Control Systems (AREA)
Description
本明細書では、移動体が移動するべき経路の初期値が与えられたときに、より好ましい経路に修正する技術を開示する。 The present specification discloses a technique for correcting to a more preferable route when given an initial value of the route on which the moving body should move.
出発点と目標点が与えられたときに、出発点から目標点に至る経路を探索する技術が開発されている。経路に必要な正確性は周囲の状況によって変化し、移動体から障害物までの距離が長い状況下では、粗い精度で経路を探索すればよい。粗い精度で探索した経路では不十分な状況も存在する。例えば、移動体から障害物までの距離が短くなって移動体が障害物の間を縫って移動するような状況下では、細かな精度で経路を決定する必要がある。
移動経路の全部を細かな精度で探索しようとすると、計算に長時間を要する他、探索不能となる場合も生じる。粗い制度で大局的な経路を探査する手法が有効である場合も多い。
A technique for searching for a route from a starting point to a target point when a starting point and a target point are given has been developed. The accuracy required for the route varies depending on the surrounding conditions, and in a situation where the distance from the moving body to the obstacle is long, the route may be searched with rough accuracy. There are situations where a route searched with coarse accuracy is insufficient. For example, in a situation where the distance from the moving body to the obstacle becomes short and the moving body moves by sewing between the obstacles, it is necessary to determine the route with fine accuracy.
If an attempt is made to search the entire movement path with fine accuracy, the calculation takes a long time and the search may become impossible. In many cases, a rough route system is used to explore global routes.
本明細書では、現場の情況に必ずしも適していない可能性がある初期経路が与えられたときに、現場の情況に適した経路に修正する技術を開示する。例えば、粗い精度で探索された経路を細かな精度で検証された経路に修正する技術を開示する。 The present specification discloses a technique for correcting a path suitable for the situation of the site when given an initial path that may not necessarily be suitable for the situation of the scene. For example, a technique for correcting a route searched with coarse accuracy to a route verified with fine accuracy is disclosed.
経路を修正する技術も開発されている。非特許文献１には、経路を記述するパラメータからその経路の好ましさ（あるいは現場への適応度）の程度を示す評価値を計算する評価関数を求め、その評価値を極小化するパラメータを求める。非特許文献１では、好ましい経路ほど小さな評価値となる評価関数を用いるので、評価値を極小化する経路記述パラメータを求める。好ましい経路ほど大きな評価値をとる評価関数を用いる場合には、評価値を極大化する経路記述パラメータを求めることになる。本明細書では、両者を包含するために、評価値の極値化条件を探索するという。好ましい経路ほど小さな値となる評価関数を用いる場合には評価値を最小とする経路記述パラメータを求めることに相当し、好ましい経路ほど大きな値となる評価関数を用いる場合には評価値を最大とする経路記述パラメータを求めることに相当する。
Technology to correct the route has also been developed.
非特許文献１では、移動体と障害物の間の離反距離が短くなるほど、大きな評価値となる評価関数を採用している。離反距離がゼロになると（移動体と障害物が接触する場合）、評価値が無限大となる評価関数を用いる。その評価関数を用いて評価値を最小化する経路を探索すれば、移動体と障害物が接触しない経路が探索される。
In
しかしながら、経路の初期値（例えば粗い精度で探索された経路）によると、移動体と障害物が重なりあってしまう場合も生じる。その場合、非特許文献１の技術では、計算不能となってしまい、移動体と障害物が接触しない経路に修正することができない。
However, according to the initial value of the route (for example, the route searched with rough accuracy), the moving body and the obstacle may overlap each other. In that case, with the technique of
本明細書では、与えられた初期経路によると移動体と障害物が重なりあってしまう場合であっても、計算不能に陥らず、移動体と障害物が接触しない経路に修正する技術を開示する。 In this specification, even if a moving body and an obstacle are overlapped according to a given initial route, a technique for correcting to a route in which the moving body and the obstacle do not come into contact with each other is disclosed. .
本明細書では、経路記述データの修正装置を開示する。その修正装置は、経路記述データを入力して記憶する装置を備えており、入力して記憶した経路記述データで記述される経路よりも好ましい経路を記述する経路記述データに修正する。その修正処理のために、本修正装置では、経路のトータル評価値を用いる。そのトータル評価値は経路記述データから計算することができる。 The present specification discloses a device for correcting route description data. The correction device includes a device that inputs and stores route description data, and corrects the route description data to describe a route that is preferable to the route described by the input and stored route description data. For the correction process, the total correction value of the route is used in the correction device. The total evaluation value can be calculated from the route description data.
本修正装置は、経路記述データからトータル評価値を計算するトータル評価関数が極値をとるときの経路記述データを探索する探索装置を備えている。その探索装置は、経路記述データを用いて、下記を計算する。
・経路長に関する経路長評価値を計算する：
・移動体と障害物の離反距離に関する離反距離評価値を計算する：
・少なくとも経路長評価値と離反距離評価値からトータル評価値を計算する。
本修正装置では、移動体と障害物が重なりあう経路に対して、有限の離反距離評価値を計算する。経路を修正することで、最終的には移動体と障害物が重なりあわない経路に修正されるが、修正計算の途上では移動体と障害物が重なりあう経路に関する計算をすることがある。本修正装置では、修正計算の途上で計算不能に陥ることがなく、移動体と障害物が重なりあわない経路に修正することができる。
The correction device includes a search device that searches for route description data when the total evaluation function for calculating the total evaluation value from the route description data takes an extreme value. The search device calculates the following using the route description data.
Calculate the path length evaluation value for the path length:
Calculate the separation distance evaluation value for the separation distance between the moving object and the obstacle:
・ A total evaluation value is calculated from at least the path length evaluation value and the separation distance evaluation value.
In the present correction device, a finite separation distance evaluation value is calculated for a path in which the moving body and the obstacle overlap. By correcting the route, the route is finally corrected so that the moving body and the obstacle do not overlap. However, in the course of the correction calculation, a calculation may be performed regarding the route where the moving body and the obstacle overlap. In this correction apparatus, it is possible to correct the path so that the moving body and the obstacle do not overlap without causing the calculation to be impossible during the correction calculation.
実際の移動体と実際の障害物は点でなく、大きさを備えていることから、移動体から障害物までの最短距離を、移動体と障害物の間の離反距離とすることが好ましい。すなわち移動体の輪郭上にあって障害物に最近接している点と、障害物の輪郭上にあって移動体に最近接している点の間の距離を離反距離とすることが好ましい。その場合の離反距離は、移動体方向（移動体が向いている方向、あるいは移動体の姿勢方向といってもよい）によって変化する。 Since the actual moving object and the actual obstacle are not points but sizes, it is preferable that the shortest distance from the moving object to the obstacle is a separation distance between the moving object and the obstacle. That is, it is preferable that a distance between a point on the contour of the moving object and closest to the obstacle and a point on the contour of the obstacle and closest to the moving object is a separation distance. In this case, the separation distance varies depending on the direction of the moving body (which may be referred to as the direction in which the moving body is facing or the posture direction of the moving body).
クレーン車のような特殊移動体の中には、走行方向（移動方向といってもよい。本明細書では経路方向という。）と、車体方向（姿勢方向といってもよい。本明細書では移動体方向という。）が独立しているものが存在する。戦車の砲塔方向も、戦車の走行方向から独立に調整することができる。しかしながら、自動車や車椅子などの通常の移動体は、経路方向が決まれば移動体方向も決まり、経路方向のほかに移動体方向を決める必要がない。 In a special mobile body such as a crane truck, a traveling direction (may be referred to as a moving direction, referred to as a route direction in the present specification) and a vehicle body direction (referred to as a posture direction. In this specification, There is something that is independent of the moving body direction). The turret direction of the tank can also be adjusted independently from the traveling direction of the tank. However, in the case of a normal mobile body such as an automobile or a wheelchair, if the route direction is determined, the mobile body direction is also determined, and it is not necessary to determine the mobile body direction in addition to the route direction.
本明細書では、移動体が障害物の間を縫って移動するような経路に修正できるように、経路方向のほかに移動体方向をも記述している経路記述データを用いる技術も開示する。通常の移動体の場合は、経路方向が決まれば移動体方向も決まるので、経路方向のほかに移動体方向を決める必要がないにもかかわらず、経路方向のほかに移動体方向をも記述している経路記述データを用いる
この場合は、探索装置が、下記の計算をする。
・経路長に関する経路長評価値を計算する：
・経路方向と移動体方向の間の偏差に関する偏差評価値を計算する：
・移動体と障害物の離反距離に関する離反距離評価値を計算する：
・経路長評価値と偏差評価値と離反距離評価値からトータル評価値を計算する。
The present specification also discloses a technique using route description data that describes the direction of the moving body in addition to the direction of the route so that the moving body can be corrected to a route that moves by sewing between obstacles. In the case of a normal moving body, the direction of the moving body is determined once the path direction is determined.Therefore, it is not necessary to determine the direction of the moving body in addition to the direction of the path. In this case, the search device performs the following calculation.
Calculate the path length evaluation value for the path length:
Calculate the deviation evaluation value for the deviation between the path direction and the moving body direction:
Calculate the separation distance evaluation value for the separation distance between the moving object and the obstacle:
・ A total evaluation value is calculated from the path length evaluation value, the deviation evaluation value, and the separation distance evaluation value.
この技術によると、離反距離評価値を最適化する要素が作用することから、好ましい離反距離が確保される移動体方向に修正される。同時に、経路方向と移動体方向の間の偏差を最小化する経路方向と移動体方向が算出されることから、経路方向と移動体方向がよく一致する経路が計算される。自動車や車椅子などの通常の移動体が追従可能な経路に修正される。移動体が移動方向と姿勢方向を変えながら障害物の間を縫って移動する経路に修正される。 According to this technique, since an element for optimizing the separation distance evaluation value acts, the moving body direction is corrected to ensure a preferable separation distance. At the same time, since the path direction and the moving body direction that minimize the deviation between the path direction and the moving body direction are calculated, a path in which the path direction and the moving body direction coincide well is calculated. The route is corrected so that a normal moving body such as a car or a wheelchair can follow. The path is corrected so that the moving body moves by sewing between obstacles while changing the moving direction and the posture direction.
上記の２つの技術、すなわち、移動体と障害物が重なりあう経路に対して有限の離反距離評価値を計算する技術と、経由点位置のほかに移動体方向をも加味して障害物までの離反距離を計算して離反距離評価値を計算する技術は、一体不可分でなく、必要に応じてどちらか一方のみを採用することができる。例えば、与えられる初期経路が障害物と重なりあわない場合は、前者の技術を採用しなくてもよい。あるいは、移動体の輪郭が円に近く、移動体方向によって離反距離が変化しない場合は、後者の技術を採用しなくてもよい。 The above two technologies, that is, a technology for calculating a finite separation distance evaluation value for a path where a moving object and an obstacle overlap, and a moving object direction in addition to the waypoint position, The technique for calculating the separation distance and calculating the separation distance evaluation value is not integral, and only one of them can be adopted as necessary. For example, when the given initial route does not overlap an obstacle, the former technique may not be adopted. Alternatively, when the contour of the moving body is close to a circle and the separation distance does not change depending on the moving body direction, the latter technique may not be adopted.
両者を同時に用いてもよい。その場合は、移動体方向を記述するデータを含んでいる経路記述データを用いる。
またトータル評価関数が極値をとるときの経路記述データを探索する装置は、経路記述データを用いて下記を計算する。
・経路長に関する経路長評価値を計算する：
・経路方向と移動体方向の間の偏差に関する偏差評価値を計算する：
・移動体と障害物の離反距離に関する離反距離評価値を計算する：
・経路長評価値と偏差評価値と離反距離評価値からトータル評価値を計算する。
離反距離評価値を算出する際には、移動体と障害物が重なりあう計算途上の経路に対して、有限の離反距離評価値を計算する。
Both may be used simultaneously. In that case, route description data including data describing the moving body direction is used.
An apparatus for searching for route description data when the total evaluation function has an extreme value calculates the following using the route description data.
Calculate the path length evaluation value for the path length:
Calculate the deviation evaluation value for the deviation between the path direction and the moving body direction:
Calculate the separation distance evaluation value for the separation distance between the moving object and the obstacle:
・ A total evaluation value is calculated from the path length evaluation value, the deviation evaluation value, and the separation distance evaluation value.
When calculating the separation distance evaluation value, a finite separation distance evaluation value is calculated for a path in the course of calculation where the moving body and the obstacle overlap.
上記修正装置によると、修正計算の途上で移動体と障害物が重なりあう経路に関する計算をする際に計算不能に陥ることがなく、移動体と障害物が重なりあわない経路に修正することができる。
また、移動体が姿勢方向を変えることで障害物からの離反距離を確保しながら障害物の間を縫って移動する経路に修正される。
According to the above correction device, it is possible to correct the path so that the moving body and the obstacle do not overlap each other without causing the calculation to be impossible when calculating the path where the moving body and the obstacle overlap in the course of the correction calculation. .
Further, the moving body changes the posture direction so that the distance between the moving object and the obstacle is secured while the distance between the moving object is secured and the path is moved.
前記したように、移動体と障害物は有限の大きさを備えており、移動体から障害物までの最短距離によって離反距離評価値を計算することが好ましい。
そのためには、探索装置によって、移動体を包含する凸包と障害物を包含する凸包の間の最短距離を計算し、その最短距離から離反距離評価値を計算することが好ましい。
As described above, the moving object and the obstacle have a finite size, and it is preferable to calculate the separation distance evaluation value based on the shortest distance from the moving object to the obstacle.
For this purpose, it is preferable to calculate the shortest distance between the convex hull including the moving object and the convex hull including the obstacle by the search device, and to calculate the separation distance evaluation value from the shortest distance.
移動体を包含する凸包を導入すると、既知の数学的手法（例えばＧＪＫアルゴリズム）で障害物から移動体までの最短距離を計算することができ、現実に即した離反距離評価値を計算することができる。また、移動体が姿勢方向を変えることで離反距離を確保しながら障害物の間を縫って移動する経路が得られる。 When a convex hull that includes a moving object is introduced, the shortest distance from the obstacle to the moving object can be calculated using a known mathematical method (for example, the GJK algorithm), and the separation distance evaluation value that is realistic can be calculated. Can do. In addition, a path through which the moving body sews between obstacles while securing a separation distance by changing the posture direction is obtained.
探索装置は、トータル評価関数を経路記述データで偏微分した偏微分関数を用いて、トータル評価関数が極値をとるときの経路記述データを探索する処理を実行する装置で構成することができる。
関数が極値をとるときの変数の値を探査する多くの数学技術が開発されている。例えば、最急降下法、ニュートン法、準ニュートン法、共役勾配法などが知られている。トータル評価関数を経路記述データで偏微分した偏微分関数を用いてトータル評価関数が極値をとるときの経路記述データを探索する段階には、既知の技術を採用することができる。
The search device can be configured by a device that executes processing for searching for path description data when the total evaluation function takes an extreme value using a partial differential function obtained by partial differentiation of the total evaluation function with the path description data.
Many mathematical techniques have been developed to explore the value of variables when a function takes extreme values. For example, the steepest descent method, Newton method, quasi-Newton method, conjugate gradient method and the like are known. A known technique can be used for searching the path description data when the total evaluation function takes an extreme value using the partial differentiation function obtained by partial differentiation of the total evaluation function with the path description data.
探索装置は、経路長評価値と偏差評価値と離反距離評価値を先に計算し、それからトータル評価値を計算するものであってもよいし、トータル評価値を一挙に計算するものであってもよい。 The search device may calculate the path length evaluation value, the deviation evaluation value, and the separation distance evaluation value first, and then calculate the total evaluation value, or may calculate the total evaluation value all at once. Also good.
経路方向と移動体方向の偏差という場合、直前の経路方向と移動体方向の偏差と、直後の経路方向と移動体方向の偏差が存在する。そこで、直前の経路方向と移動体方向の偏差に関する評価値と、直後の経路方向と移動体方向の偏差に関する評価値を加算して、偏差評価値を計算することが好ましい。 In the case of the deviation between the route direction and the moving body direction, there is a deviation between the immediately preceding route direction and the moving body direction, and a deviation between the immediately following route direction and the moving body direction. Therefore, it is preferable to calculate the deviation evaluation value by adding the evaluation value regarding the deviation between the immediately preceding path direction and the moving body direction and the evaluation value regarding the deviation between the immediately following path direction and the moving body direction.
この場合、直前の経路方向と移動体方向の偏差が小さくなり、直後の経路方向と移動体方向の偏差も小さくなる経路が探索される。上記の２項目における移動体方向は同一である。その結果、直前の経路方向と直後の経路方向の偏差が小さくなる経路が探索される。すなわち、経路方向が急に変化する経路から、経路方向が緩やかに変化する経路に修正され、経路の平滑化が行われる。 In this case, a route is searched for that has a smaller deviation between the immediately preceding route direction and the moving body direction, and a smaller deviation between the immediately following route direction and the moving body direction. The moving body directions in the above two items are the same. As a result, a route with a small deviation between the immediately preceding route direction and the immediately following route direction is searched. That is, the route is changed from a route in which the route direction changes suddenly to a route in which the route direction changes slowly, and the route is smoothed.
経路記述データが経由点の順列を含んでいる場合、前記探索装置が、経由点間の距離が全て等しいという制約下で、トータル評価関数が極値をとるときの経路記述データを探索するようにしてもよい。 When the route description data includes a sequence of via points, the search device searches for the route description data when the total evaluation function takes an extreme value under the constraint that the distances between the via points are all equal. May be.
経由点間の距離に制約を設けないで経路修正処理を実行した場合、障害物から遠い範囲では経由点間の距離が短く、障害物から近い範囲では経由点間の距離が長い経路記述データに修正されることがある。障害物から近い範囲では経由点間の距離が長くなるのは好ましくない。そこで、経由点間の距離が全て等しいという制約を課しておくのが好ましい。 When route correction processing is executed without restricting the distance between via points, the route description data has a short distance between via points in the range far from the obstacle, and a long distance between via points in the range close to the obstacle. May be corrected. In the range close to the obstacle, it is not preferable that the distance between the waypoints is long. Therefore, it is preferable to impose a constraint that the distances between the via points are all equal.
探索装置に、経路方向と移動体方向の間の偏差が許容偏差角以下であるという制約を課して置くことも可能である。この場合、自動車などのように旋回可能な半径に下限を有する移動体でも追従可能な経路に修正される。 It is also possible to place a restriction on the search device such that the deviation between the route direction and the moving body direction is less than the allowable deviation angle. In this case, it is corrected to a path that can be followed even by a mobile body having a lower limit radius that can be turned, such as an automobile.
探索装置に、移動体と障害物の離反距離が許容距離以上であるという制約を課して置くことも可能である。この場合、障害物から少なくとも許容距離以上離反した経路に修正される。 It is also possible to place a restriction on the search device that the distance between the moving object and the obstacle is greater than the allowable distance. In this case, the route is corrected to a route separated from the obstacle by at least an allowable distance.
移動体と障害物が重なりあう経路に対して有限の離反距離評価値が計算されるようにしておいてトータル評価値を最適化する経路記述データに修正する技術によると、移動体と障害物と重なりあう初期経路が与えられても計算不能となることがなく、障害物と重なりあわない経路に修正される。経路修正技術の適用範囲が拡大される。
また、移動体方向をも加味して障害物までの離反距離を計算する技術によると、移動体が姿勢方向（移動体方向）を変えることで離反距離を確保しながら障害物の間を縫って移動する経路に修正される。
両技術を併用すれば、両方の効果が得られる。
According to the technology that corrects the route description data to optimize the total evaluation value by calculating the finite separation distance evaluation value for the route where the moving object and the obstacle overlap, Even if an initial route that overlaps is given, calculation is not disabled and the route is corrected so as not to overlap with an obstacle. The scope of application of route correction technology is expanded.
In addition, according to the technology that calculates the separation distance to the obstacle in consideration of the moving body direction, the movable body can sew between the obstacles while ensuring the separation distance by changing the posture direction (moving body direction). It is corrected to the moving route.
If both technologies are used together, both effects can be obtained.
下記に説明する実施例の主要な特徴を列記する。
特徴１：経路長評価値と偏差評価値と離反距離評価値の各々に重み係数を乗算した値の総和によってトータル評価値を計算する。
特徴２：離反距離評価値は、離反距離が長ければ小さな値をとり、離反距離が短くなると増大する。移動体と障害物が重なりあう場合には、深く重なりあうほど増大する離反距離評価値を用いる。
特徴３：離反距離がポテンシャル増大開始距離以上であれば、離反距離評価値はゼロである。
The main features of the embodiments described below are listed.
Feature 1: A total evaluation value is calculated by a sum of values obtained by multiplying each of the path length evaluation value, the deviation evaluation value, and the separation distance evaluation value by a weighting factor.
Characteristic 2: The separation distance evaluation value takes a small value when the separation distance is long, and increases when the separation distance is short. When the moving body and the obstacle are overlapped, the separation distance evaluation value that increases as the overlapping overlaps is used.
Feature 3: If the separation distance is equal to or greater than the potential increase start distance, the separation distance evaluation value is zero.
経路記述データの修正装置の実施例を説明する。最初に用語の意味を明らかにしておく。
経路記述データ：経由点（移動体の基準点が通過する点）のｘ座標と、経由点のｙ座標と、経由点における移動体方向（ｘ軸の正方向から移動体に固定されている基準線が指向している方向までの反時計回転方向の角θ）が、経由点の順序に従って並んでいるデータをいう。
経路記述データは、式（１）で示され、Ｐｉは式（２）で示される。ｘi，ｙi，θiを、本明細書では、経路記述パラメータという。ｉ＝１は出発点であり、ｉ＝Ｎは目標点である。ｘ１、ｙ１，θ１，ｘＮ、ｙＮ，θＮは与えられている。
移動体：有限の大きさを備えている。
障害物：有限の大きさを備えている。
凸包：物体を包含する凸多角形。
最短距離計算法：離れて存在する２個の凸包のそれぞれにおける最近接点を求め、その距離（凸包間の最短距離）を計算する手法。ＧＪＫアルゴリズムなどの既知の数学手法を利用することができる。
ポテンシャル増大開始距離：離反距離がそれ以下となると、離反距離評価値を計算するポテンシャル関数がゼロでなくなる距離。
許容距離：移動体と障害物の間に確保する最小離反距離。ポテンシャル増大開始距離に等しくしてもよいし、異なってしてもよい。
許容偏差：先行経由点から後続経由点を指す方向と、ｘ軸の正方向がなす角をδとしたときに、先行経由点における移動体方向とδの偏差、ならびに、後続経由点における移動体方向とδの偏差に関して許容される最大偏差をいう。
An embodiment of a device for correcting route description data will be described. First, clarify the meaning of the terms.
Path description data: x-coordinate of via point (point through which the reference point of the moving body passes), y coordinate of the via point, and moving body direction at the via point (reference fixed from the positive x-axis direction to the moving body) This is data in which the angle θ) in the counterclockwise direction to the direction in which the line is directed is arranged according to the order of the via points.
The route description data is represented by the formula (1), and Pi is represented by the formula (2). In this specification, xi, yi, and θi are referred to as route description parameters. i = 1 is the starting point and i = N is the target point. x1, y1, θ1, xN, yN, and θN are given.
Mobile body: It has a finite size.
Obstacle: It has a finite size.
Convex hull: A convex polygon that encompasses an object.
Shortest distance calculation method: A method of calculating the distance (the shortest distance between convex hulls) by obtaining the closest point in each of two convex hulls that are present apart. Known mathematical techniques such as the GJK algorithm can be used.
Potential increase start distance: The distance at which the potential function for calculating the separation distance evaluation value is not zero when the separation distance is less than that.
Allowable distance: The minimum separation distance secured between the moving object and the obstacle. It may be equal to the potential increase start distance or may be different.
Allowable deviation: When the angle between the direction from the preceding via point to the subsequent via point and the positive direction of the x axis is δ, the deviation of the moving body direction and δ at the preceding via point, and the moving body at the subsequent via point The maximum deviation allowed for the deviation of direction and δ.
（修正装置の全体構成）
図１は、入力装置２から初期経路を入力し、その初期経路を好ましい経路に修正し、修正後経路を出力装置２２から出力する経路修正装置のシステム構成を示している。
入力装置２から入力した初期経路は、修正途上の経路記述パラメータ記憶装置４に記憶される。後記するように、経路記述パラメータ記憶装置４の記憶内容は、計算の進行に伴って更新されていく。経路記述パラメータ記憶装置４は、修正途上の経路記述パラメータを記憶している装置ということができる。
(Overall configuration of correction device)
FIG. 1 shows a system configuration of a route correction device that inputs an initial route from the
The initial route input from the
修正装置は、経路長に関する評価関数を記憶している装置６と、経路方向と移動体方向の偏差に関する評価関数を記憶している装置８と、移動体から障害物までの離反距離に関する評価関数を記憶している装置１０と、経路長評価値と偏差評価値と離反距離評価値を合計したトータル評価値を計算するトータル評価関数記憶装置１２を備えている。経路記述パラメータ記憶装置４に記憶されている経路記述パラメータをトータル評価関数に代入することで、トータル評価値を計算することができる。図１は、システム構成を理解しやすくするために、トータル評価関数記憶装置１２とは別に、経路長評価関数記憶装置６と偏差評価関数記憶装置８と離反距離評価関数記憶装置１０が図示されているが、経路長評価関数記憶装置６と偏差評価関数記憶装置８と離反距離評価関数記憶装置１０は、トータル評価関数記憶装置１２に内蔵されていてもよい。
The correction device includes a
修正装置は、トータル評価関数を経路記述パラメータで偏微分した偏微分関数記憶装置１４を備えている。すなわち、トータル評価関数をｘiで偏微分した偏微分関数と、トータル評価関数をｙiで偏微分した偏微分関数と、トータル評価関数をθiで偏微分した偏微分関数を記憶している。ここで、i＝２，３，・・Ｎ―１である。ｉ＝１は出発点であり、ｉ＝Ｎは目標点であるから、ｘ１、ｙ１，θ１，ｘＮ、ｙＮ，θＮは固定されている。中間の経路点に関して偏微分する。経路記述パラメータ記憶装置４に記憶されている経路記述パラメータを、トータル評価関数をｘiで偏微分した偏微分関数と、トータル評価関数をｙiで偏微分した偏微分関数と、トータル評価関数をθiで偏微分した偏微分関数に代入することで、３種類の偏微分係数を計算することができる。図１では、微分関数が一つのブロック１４に図示されているが、ｘiの偏微分関数と、ｙiの偏微分関数と、θiの偏微分関数に分けて図示してもよい。
The correction device includes a partial differentiation
経路記述パラメータを変数とするトータル評価関数が与えられ、経路記述パラメータを変数とする偏微分関数（トータル評価関数を経路記述パラメータで偏微分した関数）が与えられれば、既知の数学的手法で、トータル評価関数が極値をとる際の経路記述パラメータの値を決定することができる。本実施例では、好ましい経路ほど小さな評価値をとるトータル評価関数を採用する。本実施例では、トータル評価関数が最小値をとる経路記述パラメータの値を決定することによって、最も好ましい経路に修正する。本実施例では、好ましい経路に修正する速度と安定性にすぐれている準ニュートン法を採用する。準ニュートン法に限られず、最急降下法、ニュートン法、共役勾配法などの勾配法を用いることもできる。 Given a total evaluation function with a path description parameter as a variable and a partial differential function with a path description parameter as a variable (a function in which the total evaluation function is partially differentiated with a path description parameter), a known mathematical method The value of the path description parameter when the total evaluation function takes an extreme value can be determined. In this embodiment, a total evaluation function that takes a smaller evaluation value for a preferable route is adopted. In the present embodiment, the route evaluation parameter value at which the total evaluation function takes the minimum value is determined, thereby correcting to the most preferable route. In the present embodiment, a quasi-Newton method is adopted which is excellent in speed and stability for correcting to a preferable route. The method is not limited to the quasi-Newton method, and gradient methods such as the steepest descent method, Newton method, and conjugate gradient method can also be used.
多変数関数が最小値をとる条件を探査する勾配法では、偏微微分関数を利用して、関数値を最小化させる変数の修正方向を決定する。多変数関数の場合には、各変数の修正量の比率を計算する。ここでは、それを探索方向ベクトルという。修正装置は、探索方向ベクトルを計算する装置１６を備えている。
In the gradient method that searches for a condition in which a multivariable function takes a minimum value, a modification direction of a variable that minimizes the function value is determined using a partial differential function. In the case of a multivariable function, the ratio of the correction amount of each variable is calculated. Here, it is called a search direction vector. The correction device comprises a
多変数関数が最小値をとる条件を探査する勾配法では、修正量の大きさが問題となる。修正量が過大であれば、修正方向は正しくても、最小化点を超えた範囲で関数値（トータル評価値）を計算することになり、トータル評価値が増大するという計算結果になってしまう。修正量が過小であれば、修正によって得られる関数値の減少量が微小となり、探索速度が遅くなる。本実施例の修正装置では、複数種類のステップ幅（修正量を決めるパラメータ）を順に用意しておき、関数値が減少する制約下で最大の修正量を採用するステップ幅設定手段１８を備えている。 In the gradient method that searches for the condition under which the multivariable function takes the minimum value, the magnitude of the correction amount becomes a problem. If the correction amount is excessive, even if the correction direction is correct, the function value (total evaluation value) is calculated within the range beyond the minimization point, resulting in a calculation result that the total evaluation value increases. . If the correction amount is too small, the decrease amount of the function value obtained by the correction becomes minute and the search speed becomes slow. The correction apparatus according to the present embodiment includes step width setting means 18 that prepares a plurality of types of step widths (parameters for determining a correction amount) in order and adopts the maximum correction amount under the constraint that the function value decreases. Yes.
探索が進行して最小化位置に接近すると、探索方向ベクトル計算装置１６で計算された方向に、ステップ幅設定手段１８で設定された修正量だけ修正しても、評価値が変化しなくなる。最小化点では、偏微分係数がゼロであることに対応する。
本実施例では、探索方向ベクトル計算装置１６で計算された方向にステップ幅設定手段１８で設定された修正量だけ修正することで関数値が変化（減少）するうちは、修正後の経路記述パラメータによって、経路記述パラメータ記憶装置４の記憶内容を更新する。更新した経路記述パラメータを用いて最小化条件探索処理を繰り返す。経路記述パラメータ記憶装置４の記憶内容は、計算途上の経路記述パラメータに更新されていく。
探索方向ベクトル計算装置１６で計算された方向にステップ幅設定手段１８で設定された修正量だけ修正しても評価値が変化しなくなると、最小化点が検索されたとして処理を終了する。その段階の経路記述データが、出力装置２２から出力される。
As the search proceeds and approaches the minimization position, the evaluation value does not change even if the correction amount set by the step width setting means 18 is corrected in the direction calculated by the search direction
In the present embodiment, while the function value changes (decreases) by correcting only the correction amount set by the step width setting means 18 in the direction calculated by the search direction
If the evaluation value does not change even if the correction amount set by the step width setting means 18 is corrected in the direction calculated by the search direction
（経路記述データ）
図２において、参照番号２６は移動体を示し、出力装置２２から出力された修正後経路に沿って移動する。参照番号２４は移動体２６を包含する凸包であり、図１の修正装置では、凸包２４と障害物間の最短距離を求めて離反距離を計算する。参照番号２８は移動体２６に固定されている基準点であり、参照番号３０は移動体２６に固定されている基準線である。
(Route description data)
In FIG. 2,
経路は、データＰｉ＝（ｘi，ｙi，θi）（ここでｉ＝１，２，・・Ｎ）で定義される。移動体２６は、基準点２８が（ｘi，ｙi）の座標点を辿るように移動する。また、移動体２６は、基準線３０がθiの方向を向く車体姿勢をとる。本明細書では、角度をｘ軸の正方向から反時計回転方向に計測した角度で示す。
参照記号Ｏjは、移動体２６の走行を妨げる障害物である。本明細書では、出発点（ｘ1，ｙ1）から目標点（ｘN，ｙN）までの経路を決定する際に考慮する必要がある障害物がＭ個存在するものとする。
The route is defined by data Pi = (xi, yi, θi) (where i = 1, 2,... N). The moving
The reference symbol Oj is an obstacle that prevents the moving
（初期経路）
図１の修正装置に入力される初期経路は、ＲＲＴのような確率的手法あるいはＡ＊探索のようなグリッド探索法で求めた大局的経路である。データＰｉ＝（ｘi，ｙi，θi）のＮ個の並びで記述されている。Ａ＊探索のようなグリッド探索法で経路を探索すると、移動体方向θiは計算されない。この場合、経路方向と移動体方向が等しいとすることができる。あるいは別の手法で、移動体方向を決定してもよい。
初期経路のうち、出発点のデータＰ1＝（ｘ1，ｙ1，θ1）と目的点のデータＰN＝（ｘN，ｙN，θN）は、確定的なものであり、図１の修正装置で修正することはない。ただし、経由点のデータＰｉ（i＝２，・・，Ｎ−1）は最善なものでない可能性があり、図１の修正装置で修正する。初期経路には下記の問題が内在されていることがある。
（１）移動体と障害物が干渉する（重なりあう）経路である可能性がある。
（２）移動体と障害物が重なり合わないまでも非常に接近する経路である可能性がある。
（３）経路方向が急激に変化する経路である可能性がある。
（４）経路方向と移動体方向が不一致であって、移動体が追従できない経路情報である可能性がある。図１の修正装置は、上記の問題を解消する側に修正した経路を計算する。その修正計算のために、修正装置は下記の３種類の評価関数を利用する。
(Initial route)
The initial route input to the correction device in FIG. 1 is a global route obtained by a probabilistic method such as RRT or a grid search method such as A * search. It is described in N rows of data Pi = (xi, yi, θi). When a route is searched by a grid search method such as A * search, the moving body direction θi is not calculated. In this case, the path direction and the moving body direction can be made equal. Alternatively, the moving body direction may be determined by another method.
Of the initial route, the starting point data P1 = (x1, y1, θ1) and the destination point data PN = (xN, yN, θN) are deterministic and should be corrected by the correcting device of FIG. There is no. However, there is a possibility that the data Pi (i = 2,..., N−1) at the waypoints is not the best and is corrected by the correction device shown in FIG. The following problems may be inherent in the initial route.
(1) There is a possibility that the moving object and the obstacle interfere (overlap).
(2) There is a possibility that the path is very close even if the moving body and the obstacle do not overlap.
(3) The route direction may change rapidly.
(4) There is a possibility that the path direction and the moving body direction do not match and the moving body cannot follow the path information. The correction device in FIG. 1 calculates a corrected route on the side that solves the above problem. For the correction calculation, the correction device uses the following three types of evaluation functions.
（経路長評価関数）
出発点から目標点に至る経路の全長が短いことが好ましい。全長が短い経路に修正されるように、修正装置は経路長評価関数を利用する。経路長評価関数を、図２の（２）式に示す。係数Ｋａは、トータル評価値に占める経路長評価値の重みを示し、距離ｌiは、座標点（ｘi，ｙi）から座標点（ｘi+1，ｙi+1）までの距離である。ｉ＝１からＮ−１までの総和を取れば、経路全長に関する評価値が求められる。全長が短いほど小さな評価値が計算される。修正装置は、評価値を最小化する経路に修正する。全長が短いほど小さな評価値が計算される経路長評価関数を導入するために、修正装置によって全長が短い経路に修正される。
図２の（２）式に示す経路長評価関数を最小化すると、
（１）全長が短くなる経路に修正され、
（２）Ｐ2，・・，ＰN-1というＮ−２個の経由点を、等間隔に配置した経路記述データに修正される（全長が等しければ、経由点が等間隔である場合の評価値の方が、経由点が等間隔でない場合の評価値よりも小さくなる）。
もっとも他の評価関数をも利用するために、全長のみを考慮して修正されるものでないが、全長をも考慮した修正計算が実行されるということはできる。
(Path length evaluation function)
The total length of the route from the starting point to the target point is preferably short. The correction device uses a path length evaluation function so that the total length is corrected to a short path. The path length evaluation function is shown in equation (2) in FIG. The coefficient Ka indicates the weight of the path length evaluation value in the total evaluation value, and the distance l i is the distance from the coordinate point (xi, y i) to the coordinate point (xi + 1, y i + 1). If the sum from i = 1 to N−1 is taken, an evaluation value related to the total length of the route is obtained. The smaller the total length, the smaller the evaluation value is calculated. The correction device corrects the path so as to minimize the evaluation value. In order to introduce a path length evaluation function in which a smaller evaluation value is calculated as the total length is shorter, the correction apparatus corrects the path to a shorter length.
When the path length evaluation function shown in equation (2) in FIG. 2 is minimized,
(1) It has been corrected to a route that shortens the overall length,
(2) The route description data in which N-2 via points P2,..., PN-1 are arranged at equal intervals is modified (if the total length is equal, the evaluation value when the via points are equally spaced) Is smaller than the evaluation value when the waypoints are not equally spaced).
Of course, in order to use other evaluation functions, correction is not performed in consideration of only the total length, but correction calculation in consideration of the total length can be executed.
（偏差評価関数）
移動体２６は、移動方向（経路方向）と独立して車体方向（姿勢方向、移動体方向）を変える機構を備えていないから、経由点Ｐi＝（ｘi，ｙi）を辿って移動する際の移動体方向θiが経路方向δiに一致することが保証されていない。その問題を解消するために、修正装置には経路方向δiと移動体方向θiを一致させるように修正する評価関数が導入されており、移動体がＰi＝（ｘi，ｙi）を辿って移動すると、移動体方向θiが経路方向δiによく一致する関係が得られるようにしている。移動方向から独立して車体方向を変える機構を備えていない移動体２６の経路を修正する際に、経路方向δiと独立して移動体方向θiを定義する経路記述データを採用しても、問題が生じないようにしている。
(Deviation evaluation function)
Since the moving
図３の（ｂ）は、経由点Ｐiから経由点Ｐi+1に至る経路方向（δi）と、移動体方向の偏差を示している。実際には、経由点Ｐiにおける移動体方向θiと、経由点Ｐi+1における移動体方向θi+1は相違している。そこで、２種類の偏差評価関数を導入している。
第１偏差表関数は、図３の（３）に示すように、経路方向δiと移動体方向θiの偏差φiを自乗した値を計算する。係数Ｋｂは、トータル評価値に占める第１偏差評価値の重みを示している。図３の（４）に示すように、i＝１からＮ−１までの総和を取れば、第１偏差評価値を経路全長に亘って累積した第１偏差評価値Ｕｂを計算することができる。偏差φiが小さいほど評価値が小さくなる。評価値を最小化する方向に修正すると、偏差φiが小さくなる経路に修正される。
第２偏差表関数は、図３の（５）に示すように、経路方向δiと移動体方向θi+1の偏差ηiを自乗した値を計算する。係数Ｋｃは、トータル評価値に占める第２偏差評価値の重みを示している。図３の（６）に示すように、i＝１からＮ−１までの総和を取れば、第２偏差評価値を経路全長に亘って累積した第２偏差評価値Ｕｃを計算することができる。偏差ηiが小さいほど評価値が小さくなる。評価値を最小化する方向に修正すると、偏差ηiが小さくなる経路に修正される。
(B) of FIG. 3 shows the deviation between the route direction (δi) from the route point Pi to the route point Pi + 1 and the moving body direction. Actually, the moving body direction θi at the waypoint Pi is different from the moving body direction θi + 1 at the waypoint Pi + 1. Therefore, two types of deviation evaluation functions are introduced.
As shown in (3) of FIG. 3, the first deviation table function calculates a value obtained by squaring the deviation φi between the path direction δi and the moving body direction θi. The coefficient Kb indicates the weight of the first deviation evaluation value in the total evaluation value. As shown in (4) of FIG. 3, if the sum from i = 1 to N−1 is taken, the first deviation evaluation value Ub obtained by accumulating the first deviation evaluation value over the entire path length can be calculated. . The smaller the deviation φi, the smaller the evaluation value. When the evaluation value is corrected in the direction of minimizing, the path is corrected so that the deviation φi becomes smaller.
As shown in (5) of FIG. 3, the second deviation table function calculates a value obtained by squaring the deviation ηi between the path direction δi and the moving body
移動体方向θを考慮すると、障害物までの離反距離を増大させられることがある。移動体方向θを加味して経路を修正すると、移動体方向θを振りながら移動することで移動体が障害物の間を縫って移動する経路に修正することができる。この場合には、移動体方向θによく一致する経路方向δとなる経由点位置に修正されることになる。 Considering the moving body direction θ, the separation distance to the obstacle may be increased. When the path is corrected in consideration of the moving body direction θ, the path can be corrected so that the moving body sews between obstacles by moving while moving the moving body direction θ. In this case, the position is corrected to a via point position having a route direction δ that closely matches the moving body direction θ.
図３の(ａ)と（ｂ）比較すると明らかに、経由点Ｐｉで第１評価関数と第２評価関数を計算することになる。その際には、共通して移動体方向θｉを利用する。第１評価関数は、移動体方向θｉと経路方向δiの偏差φiが小さくなる方向に修正する。第２評価関数は、移動体方向θｉと経路方向δi-1の偏差ηi-1が小さくなる方向に修正する。共通の移動体方向θｉに対する偏差が小さくなる側に修正すると、経路方向δi-1と経路方向δiの差も小さくなっていく。経路方向δi-1と経路方向δiの差が大きいことは、経路が急激に方向を変えることを意味する。経路方向δi-1と経路方向δiの差を小さくすることは、緩やかに方向を変える経路に修正することに相当する。１つの経由点Ｐiに対して、その経由点Ｐiにおける移動体方向θiと直前の経路方向δi-1の偏差ηi-1に対応する第２評価関数と、その経由点Ｐiにおける移動体方向θiと直後の経路方向δiの偏差φiに対応する第１評価関数を用いて評価値を計算するようにすると、急激に方向を変える経路から緩やかに方向を変える経路に修正される。経路の平滑化処理が実施される。 Obviously, when comparing (a) and (b) of FIG. 3, the first evaluation function and the second evaluation function are calculated at the via point Pi. In that case, the moving body direction θi is used in common. The first evaluation function is corrected so that the deviation φi between the moving body direction θi and the path direction δi becomes smaller. The second evaluation function is corrected so that the deviation ηi-1 between the moving body direction θi and the path direction δi-1 becomes smaller. When the deviation with respect to the common moving body direction θi is corrected to be smaller, the difference between the route direction δi−1 and the route direction δi also becomes smaller. A large difference between the route direction δi-1 and the route direction δi means that the route suddenly changes direction. Reducing the difference between the route direction δi-1 and the route direction δi is equivalent to correcting to a route that gradually changes direction. For one waypoint Pi, a second evaluation function corresponding to the deviation ηi-1 of the moving body direction θi at the waypoint Pi and the immediately preceding route direction δi-1, and the moving body direction θi at the waypoint Pi When the evaluation value is calculated using the first evaluation function corresponding to the deviation φi of the immediately following route direction δi, the route is changed from a route that changes direction suddenly to a route that changes direction slowly. A route smoothing process is performed.
（離反距離）
図４は、移動体が経由点Ｐiに移動した状態を示し、近傍にｊ番目の障害物が位置している状態を示している。移動体２６の凸包２４の存在範囲は、座標値（ｘi，ｙi）と移動体方向θiから計算することができる。障害物は、動かない凸包３２の存在範囲を記述するデータで記述されている。凸包２４と凸包３２の位置と姿勢が与えられれば、ＧＪＫアルゴリズムなどの既知の数学手法によって、凸包２４の輪郭上にあって凸包３２に最も近い点Ｒi,jと、凸包３２の輪郭上にあって凸包２４に最も近い点Ｏi,jを計算し、その距離ｌｅi,jを計算することができる。図示の場合は、最近接点が頂点である場合を例示しているが、辺上に最近接点がある場合にも対応することができる。図４の（７）から（９）は、そうして求められる近接点Ｏi,jの座標値と、近接点Ｒi,jの座標値と、最短距離（離反距離）ｌｅi,jの値を示している。
(Separation distance)
FIG. 4 shows a state in which the moving body has moved to the waypoint Pi, and shows a state in which the jth obstacle is located in the vicinity. The existence range of the
（離反距離評価関数）
図５（ａ）は、離反距離評価関数Ｕｅi,jを示している。離反距離ｌｅi,jがポテンシャル増大開始距離ｌe0以上であれば、離反距離評価値はゼロである。離反距離ｌｅi,jがポテンシャル増大開始距離ｌe0以下になるほど接近すると、離反距離評価値は増加し始める。移動体と障害物が接近して離反距離ｌｅi,jがゼロに近づくほど、離反距離評価値は大きくなる。移動体と障害物が接近して離反距離ｌｅi,jがゼロとなっても、離反距離評価値は無限大に発散しない。有限の値が計算される関数形を採用している。
移動体と障害物が接近して重なりあう場合、ＧＪＫアルゴリズムでは正の値を持つｌｅi,jが計算される。この場合のｌｅi,jは、移動体と障害物が重なりあっている距離であり、両者を離反させるのに必要な移動距離に等しい。本実施例では、移動体と障害物が接近して重なりあう場合には、ｌｅi,jが負の値を持つ距離だと扱う。
図５の（ｂ）は、離反距離評価関数を示している。上記の離反距離評価関数を用いると、移動体が障害物に接近し、接触し、深く重なりあうのに伴って、離反距離評価値が一様に増加する関係が得られる。係数Ｋｅは、離反距離評価関数の重み係数である。
実際には、移動体と障害物が重なりあう現象は生じない。しかしながら、修正計算の途上では、移動体と障害物が重なりあう経路に関する計算が必要とされることがある。例えば、移動体と障害物が重なりあう初期経路を、重なり合わない経路に修正する場合には、移動体と障害物が重なりあう経路に関する計算が必要とされる。本実施例では、移動体と障害物が重なりあう場合も有限の離反距離評価値が計算される離反距離評価関数を用いるので、修正計算の途上で計算不能に陥ることがない。
障害物はＭ個存在する。そこで、移動体が経由点Ｐiにあるときの離反距離評価値は、図５の（１０）式で求められる。また、経路の全長に亘って累積した離反距離評価値は、図５の（１１）式で求められる。出発点と目標点は、修正作業の対象でないので、i＝２，・・Ｎ−１まで加算する。
(Separation distance evaluation function)
FIG. 5A shows the separation distance evaluation function Uei, j. If the separation distance lei, j is equal to or greater than the potential increase start distance le0, the separation distance evaluation value is zero. When the separation distance lei, j approaches the potential increase start distance le0 or less, the separation distance evaluation value starts to increase. As the moving object approaches the obstacle and the separation distance lei, j approaches zero, the separation distance evaluation value increases. Even if the moving object approaches the obstacle and the separation distance lei, j becomes zero, the separation distance evaluation value does not diverge infinitely. A function form in which a finite value is calculated is adopted.
When the moving object and the obstacle are close to each other and overlap, the GJK algorithm calculates lei, j having a positive value. In this case, lei, j is a distance where the moving body and the obstacle overlap each other, and is equal to the movement distance necessary to separate them. In the present embodiment, when the moving object and the obstacle approach and overlap each other, it is treated that lei, j is a distance having a negative value.
FIG. 5B shows a separation distance evaluation function. When the above separation distance evaluation function is used, a relationship in which the separation distance evaluation value increases uniformly as the moving body approaches, touches, and overlaps the obstacle is obtained. The coefficient Ke is a weighting coefficient of the separation distance evaluation function.
Actually, a phenomenon in which a moving object and an obstacle overlap does not occur. However, in the course of the correction calculation, a calculation related to a route where the moving body and the obstacle overlap may be required. For example, when an initial route where a moving body and an obstacle overlap is corrected to a route which does not overlap, calculation regarding a route where the moving body and the obstacle overlap is required. In the present embodiment, since the separation distance evaluation function for calculating a finite separation distance evaluation value is used even when the moving object and the obstacle overlap, calculation does not become impossible during the correction calculation.
There are M obstacles. Therefore, the separation distance evaluation value when the moving body is at the via point Pi is obtained by the equation (10) in FIG. Further, the separation distance evaluation value accumulated over the entire length of the route is obtained by the equation (11) in FIG. Since the starting point and the target point are not subject to correction work, i = 2,.
（トータル評価関数）
本実施例では、下記式で示されるトータル評価関数を利用する。
トータル評価関数
＝経路長評価関数＋偏差評価関数＋ 離反距離評価関数
＝経路長評価関数＋第１偏差評価関数＋第２偏差評価関数＋離反距離評価関数
すなわち、下記の式３を用いて、経路の修正処理を実行する。
In this embodiment, a total evaluation function represented by the following formula is used.
Total evaluation function = path length evaluation function + deviation evaluation function + separation distance evaluation function = path length evaluation function + first deviation evaluation function + second deviation evaluation function + separation distance evaluation function Execute the correction process.
本実施例では、トータル評価関数を経路記述パラメータｘi，ｙi，θiの各々で偏微分した偏微分関数を利用して、トータル評価値を最小化する経路記述パラメータｘi，ｙi，θiを探索する。
トータル評価関数＝経路長評価関数＋偏差評価関数＋離反距離評価関数であるために、
トータル評価関数の偏微分関数＝経路長評価関数の偏微分関数＋偏差評価関数の偏微分関数＋離反距離評価関数の偏微分関数の関係が成立する。
In the present embodiment, a path description parameter xi, yi, θi that minimizes the total evaluation value is searched using a partial differential function obtained by partial differentiation of the total evaluation function with each of the path description parameters xi, yi, θi.
Since the total evaluation function = path length evaluation function + deviation evaluation function + separation distance evaluation function,
The relationship of partial differential function of total evaluation function = partial differential function of path length evaluation function + partial differential function of deviation evaluation function + partial differential function of separation distance evaluation function is established.
（経路長評価関数の偏微分関数）
最初に図２の（２）に示す経路長評価関数Ｕａの偏微分関数を求めると下記となる。
図３の（４）（６）に示す第１偏差評価関数Ｕｂと第２偏差評価関数Ｕｃの偏微分関数から偏差評価関数Ｕｂ＋Ｕｃの偏微分関数を求めると下記となる。
First, the partial differential function of the path length evaluation function Ua shown in (2) of FIG. 2 is obtained as follows.
The partial differential function of the deviation evaluation function Ub + Uc is obtained from the partial differential functions of the first deviation evaluation function Ub and the second deviation evaluation function Uc shown in (4) and (6) of FIG.
（離反距離評価関数の偏微分関数）
離反距離評価関数の偏微分関数の導出過程を、図６に示す。その結果、図５の（ｂ）に示す離反距離評価関数Ｕｅi,j、すなわち移動体が経由点Ｐiにあるときのｊ番目の障害物までの離反距離に対する評価関数の偏微分関数を求めると、下記となる。
FIG. 6 shows the process of deriving the partial differential function of the separation distance evaluation function. As a result, when the separation distance evaluation function Uei, j shown in FIG. 5B, that is, the partial differential function of the evaluation function with respect to the separation distance to the jth obstacle when the moving body is at the via point Pi, is obtained. It becomes as follows.
（評価関数の最小化過程）
関数とその関数の偏微分関数が判明していれば、既知の数学的手法で、関数を最小化する変数の値を決定することができる。その問題は、最急降下法、ニュートン法、準ニュートン法、共役勾配法などの勾配法で解くことができる。本実施例では、計算の収束速度と安定性のバランスが優れている準ニュートン法を用いる。
準ニュートン法などの勾配法では、計算途上の変数値（複数個の変数を扱うからベクトルとなる）における関数値から、関数値を減少させる変数の修正方向を特定する。偏微分関数から変数の変化に起因して関数値が変化する量を計算することができるから、関数値を減少させる変数の修正方向を特定することができる。次に、修正量を仮定し、変数群を修正方向へ修正量だけ修正した場合の関数値を計算する。関数値を減少させる修正方向を用いることから、関数値は減少するはずである。しかしながら、修正量が大きすぎれば、関数が最小値を取る位置を越えて修正することになり、関数値が増大してしまう。そこで、関数値が減少するという制約を満たす修正量の範囲内で修正する。勾配法では、
（１）現在の変数値の並び（ベクトル）よりも関数値が減少する修正方向を計算し、
（２）関数値が減少するという制約を満たす修正量で変数値の並びを修正方向に修正し、
（２）で修正された変数値の並びを（１）の変数値の並びとして(１)と（２）を繰り返す。
この結果、変数値の並びは、関数値を最小化するものに近づいていく。
関数値を最小化する変数値に近づくと、偏微分係数がゼロなって、変数値を修正方向に修正量だけ修正しても関数値は変化しなくなる。このことから、関数値を最小化する変数の並びに修正されたことがわかる。
(Evaluation function minimization process)
If the function and the partial differential function of the function are known, the value of the variable that minimizes the function can be determined by a known mathematical method. The problem can be solved by gradient methods such as the steepest descent method, Newton method, quasi-Newton method, and conjugate gradient method. In this embodiment, a quasi-Newton method is used, which is excellent in the balance between calculation convergence speed and stability.
In the gradient method such as the quasi-Newton method, the correction direction of the variable that decreases the function value is specified from the function value in the variable value in the course of calculation (because it handles a plurality of variables, it becomes a vector). Since the amount by which the function value changes due to the change of the variable can be calculated from the partial differential function, the correction direction of the variable that decreases the function value can be specified. Next, assuming a correction amount, a function value is calculated when the variable group is corrected by the correction amount in the correction direction. Since a correction direction that reduces the function value is used, the function value should decrease. However, if the correction amount is too large, the function is corrected beyond the position where the function takes the minimum value, and the function value increases. Therefore, the correction is performed within the range of the correction amount that satisfies the constraint that the function value decreases. In the gradient method,
(1) Calculate the correction direction in which the function value decreases from the current variable value sequence (vector),
(2) The variable value sequence is corrected in the correction direction with the correction amount that satisfies the constraint that the function value decreases.
(1) and (2) are repeated with the variable value sequence corrected in (2) as the variable value sequence in (1).
As a result, the sequence of variable values approaches that which minimizes the function value.
When approaching the variable value that minimizes the function value, the partial differential coefficient becomes zero, and the function value does not change even if the variable value is corrected by the correction amount in the correction direction. From this it can be seen that the sequence of variables that minimizes the function value has been modified.
準ニュートン法では、修正方向ｄkが下記の式で計算される。なお添え字ｋは、前記（１）（２）の修正処理をすでにｋ-１回繰り返しており、これからｋ回目の修正処理を実施することを示している。
本実施例では、修正量を下記の表に従って変えていく。
最小値に近づくにつれ、修正前のトータル評価関数の値と修正後のトータル評価関数の値の差の絶対値は減少していく。本実施例では、その差の絶対値が、閾値（本実施例では１０−６）を下回ったときに、トータル評価値を最小化する経路記述パラメータに並びに修正されたとし、修正処理を終了する。そうして修正された経路記述パラメータの並びを出力装置２２から出力する。
As the value approaches the minimum value, the absolute value of the difference between the value of the total evaluation function before correction and the value of the total evaluation function after correction decreases. In this embodiment, when the absolute value of the difference falls below a threshold value (10 −6 in this embodiment), it is assumed that the route description parameter has been corrected to minimize the total evaluation value, and the correction processing is terminated. . The list of route description parameters thus corrected is output from the
上記の修正計算を実行した結果、障害物に近い範囲で経由点が疎に分布し、障害物に遠い範囲で経由点が密に分布する経路記述データに修正されることがある。経由点が疎に分布すると、移動体と障害物の干渉の有無を判定する処理の精度が低下するので好ましくない。経由点が密に分布すると、曲率半径が小さな急カーブに修正される可能性があり、それも好ましくない。
それを避けるためには、全部の経由点が等間隔で配置されているという条件を課して修正処理を進めることが好ましい。下記式を加えて修正処理を進めれば、全部の経由点が等間隔で配置されている経路記述データに修正される
In order to avoid this, it is preferable to proceed with the correction process under the condition that all the waypoints are arranged at equal intervals. If the correction process is advanced by adding the following formula, all the waypoints are corrected to the route description data arranged at equal intervals.
また、計算された移動体方向θと実際の移動体方向が一致するようにするためには、経路方向δと移動体方向θの偏差に関して許容偏差角を設定しておいてもよい。下記式を加えて修正処理を進めれば、経路方向δと移動体方向θの偏差が許容偏差角η0，φ0以下であることが保証されている経路記述データに修正される。経路方向δと移動体方向θの偏差が小さい経路が計算されていれば、計算された移動体方向θと実際の移動体方向はよく一致する。
In order to make the calculated moving body direction θ coincide with the actual moving body direction, an allowable deviation angle may be set for the deviation between the path direction δ and the moving body direction θ. If correction processing is advanced by adding the following formula, the correction is made to route description data in which the deviation between the route direction δ and the moving body direction θ is guaranteed to be equal to or smaller than the allowable deviation angles η0 and φ0. If a route having a small deviation between the route direction δ and the moving body direction θ is calculated, the calculated moving body direction θ and the actual moving body direction are in good agreement.
図７は、移動体が移動可能な範囲と、出発点Ｐ1と、移動体が出発点Ｐ1にあるときの凸包２４と、目標点ＰNと、移動体が目標点ＰNにあるときの凸包２４を例示している。
図８は、初期経路を例示している。初期経路によると、移動体と障害物が重なりあってしまう。図９は、トータル評価値を低下させる経路記述データの探索処理を３０回実施した時点での経路を示している。図１０は、探索処理を６０回実施した時点での経路を示している。図１１は、探索処理を１３８回実施した時点での経路を示している。１３８回の探索処理の結果、経路記述データはそれ以上に修正されなくなった。移動体は、障害物を避けながら、目標点に目標姿勢で到達する。
FIG. 7 shows a movable range of the moving body, a starting point P1, a
FIG. 8 illustrates the initial path. According to the initial route, the moving body and the obstacle overlap. FIG. 9 shows a route when the route description data search process for reducing the total evaluation value is performed 30 times. FIG. 10 shows a route when the search process is performed 60 times. FIG. 11 shows a route when the search process is performed 138 times. As a result of the search process of 138 times, the route description data is no longer corrected. The moving body reaches the target point with the target posture while avoiding the obstacle.
図１２は、修正計算の進行によって評価値（ポテンシャル）が変化していく様子を示している。１３８回修正計算を繰り返した時点で、トータル評価値（全ポテンシャル）が最小化されることが確認される。 FIG. 12 shows how the evaluation value (potential) changes as the correction calculation progresses. It is confirmed that the total evaluation value (total potential) is minimized when the correction calculation is repeated 138 times.
以上、本発明の具体例を詳細に説明したが、これらは例示にすぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。
また、本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。
Specific examples of the present invention have been described in detail above, but these are merely examples and do not limit the scope of the claims. The technology described in the claims includes various modifications and changes of the specific examples illustrated above.
The technical elements described in this specification or the drawings exhibit technical usefulness alone or in various combinations, and are not limited to the combinations described in the claims at the time of filing. In addition, the technology illustrated in the present specification or the drawings achieves a plurality of objects at the same time, and has technical utility by achieving one of the objects.
２：初期経路入力装置
４：修正途上の経路記述パラメータの記憶装置
６：経路長に関する評価関数の記憶装置
８：経路方向と移動体方向の偏差に関する評価関数の記憶装置
１０：障害物までの離反距離に関する評価関数の記憶装置
１２：トータル評価関数の記憶装置
１４：経路記述パラメータについてのトータル評価関数の微分関数の記憶装置
１６：探索方向ベクトルの計算装置
１８：ステップ幅の設定装置
２０：トータル評価値の変化量の判定装置
２２：修正後経路出力装置
2: Initial route input device 4: Storage device of route description parameter under correction 6: Storage device of evaluation function related to route length 8: Storage device of evaluation function related to deviation between route direction and moving body direction 10: Separation to obstacle Distance evaluation function storage device 12: Total evaluation function storage device 14: Total evaluation function differential function storage device for path description parameters 16: Search direction vector calculation device 18: Step width setting device 20: Total evaluation Value change amount determination device 22: corrected route output device
Claims (9)
経路記述データから経路記述データで記述される経路を評価したトータル評価値を計算するトータル評価関数が、極値をとるときの経路記述データを探索する探索装置を備えており、
その探索装置が、経路記述データを用いて、
・経路長に関する経路長評価値を計算する処理と、
・移動体と障害物の離反距離に関する離反距離評価値を計算する処理と、
・少なくとも経路長評価値と離反距離評価値からトータル評価値を計算する処理を実行し、
移動体と障害物が重なりあう計算途上の経路に対して、有限の離反距離評価値を計算することを特徴とする経路記述データの修正装置。 A device for inputting and storing route description data;
The total evaluation function for calculating the total evaluation value that evaluates the route described by the route description data from the route description data has a search device that searches for the route description data when the extreme value is taken,
The search device uses the route description data,
・ Process to calculate the path length evaluation value for the path length;
・ A process of calculating a separation distance evaluation value regarding a separation distance between the moving object and the obstacle,
-At least execute the process of calculating the total evaluation value from the path length evaluation value and the separation distance evaluation value,
An apparatus for correcting path description data, characterized in that a finite separation distance evaluation value is calculated for a path under calculation in which a moving body and an obstacle overlap.
前記探索装置が、経路記述データを用いて、さらに、
・経路方向と移動体方向の間の偏差に関する偏差評価値を計算する処理と、
・経路長評価値と偏差評価値と離反距離評価値からトータル評価値を計算する処理を実行することを特徴とする請求項１に記載の経路記述データの修正装置。 The route description data includes data describing a moving body direction;
The search device uses the route description data , and
A process of calculating a deviation evaluation value regarding a deviation between the route direction and the moving body direction;
And correction device of the path description data according to claim 1, characterized in that to perform the process of calculating the total evaluation value pathlength evaluation value and the difference evaluation value from separating distance evaluation value.
前記探索装置が、経由点間の距離が等しいという制約下で、トータル評価関数が極値をとるときの経路記述データを探索することを特徴とする請求項１から５のいずれかの１項に記載の経路記述データの修正装置。 The route description data includes permutations of waypoints,
The search device, under the constraint that the distance between the route point is equal to the one of any one of claims 1 to 5, characterized in that searches for a route description data when the total evaluation function takes the extreme value Device for correcting the described route description data.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2011284391A JP5923973B2 (en) | 2011-12-26 | 2011-12-26 | Route correction device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2011284391A JP5923973B2 (en) | 2011-12-26 | 2011-12-26 | Route correction device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2013134615A JP2013134615A (en) | 2013-07-08 |
| JP5923973B2 true JP5923973B2 (en) | 2016-05-25 |
Family
ID=48911265
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2011284391A Expired - Fee Related JP5923973B2 (en) | 2011-12-26 | 2011-12-26 | Route correction device |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP5923973B2 (en) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP6131587B2 (en) * | 2012-12-12 | 2017-05-24 | 株式会社豊田中央研究所 | Route correction device |
| KR101906952B1 (en) * | 2013-11-08 | 2018-10-11 | 한화지상방산 주식회사 | Method for generating optimized parking path of manless driving vehicle, and manless driving vehicle adopting the method |
| CN105203120B (en) * | 2014-06-30 | 2018-12-07 | 腾讯科技（深圳）有限公司 | Navigation routine evaluating method and device |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH10230862A (en) * | 1997-02-21 | 1998-09-02 | Nissan Motor Co Ltd | Vehicle parking assist device |
| JP4759878B2 (en) * | 2001-08-10 | 2011-08-31 | 日産自動車株式会社 | Parking assistance device for vehicles |
| JP4169065B2 (en) * | 2006-02-13 | 2008-10-22 | 株式会社デンソー | Vehicle control device |
| JP5130638B2 (en) * | 2006-03-22 | 2013-01-30 | 日産自動車株式会社 | Avoidance operation calculation device, avoidance control device, vehicle including each device, avoidance operation calculation method, and avoidance control method |
| JP2008242859A (en) * | 2007-03-27 | 2008-10-09 | Sony Corp | Object motion control apparatus, motion control method, and computer program |
| JP2008257002A (en) * | 2007-04-06 | 2008-10-23 | National Institute Of Advanced Industrial & Technology | Parking evaluation method and system for parking driving operation |
-
2011
- 2011-12-26 JP JP2011284391A patent/JP5923973B2/en not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| JP2013134615A (en) | 2013-07-08 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP5993784B2 (en) | Route correction device | |
| CN113433936B (en) | Mobile equipment obstacle detouring method and device, mobile equipment and storage medium | |
| KR101585504B1 (en) | Method and apparatus for generating pathe of autonomous vehicle | |
| CN108445750B (en) | Method and system for vehicle motion planning | |
| Gu et al. | Tunable and stable real-time trajectory planning for urban autonomous driving | |
| JP2017016645A (en) | Semi-autonomous vehicle and method for controlling semi-autonomous vehicle | |
| EP3486612B1 (en) | Method for generating a trajectory | |
| CN109227533A (en) | Mobile device for planning, mobile robot and mobile planning procedure | |
| JP6131587B2 (en) | Route correction device | |
| WO2018176593A1 (en) | Local obstacle avoidance path planning method for unmanned bicycle | |
| CN114355923A (en) | MPC-based trajectory planning and tracking method under guidance of A | |
| CN110162029A (en) | A kind of motion control method and device, robot based on planning path | |
| CN107289938A (en) | A kind of unmanned ground vehicle local paths planning method | |
| JP5923973B2 (en) | Route correction device | |
| CN117870708A (en) | A path planning method, terminal device and storage medium | |
| CN111912411B (en) | Robot navigation positioning method, system and storage medium | |
| JP5986533B2 (en) | Route correction device | |
| JP6941039B2 (en) | Orbit planning equipment, orbit planning methods, and programs | |
| Vailland et al. | Cubic bézier local path planner for non-holonomic feasible and comfortable path generation | |
| CN107990906A (en) | The method for determining path | |
| Xi et al. | A safe and efficient timed-elastic-band planner for unstructured environments | |
| CN115639817B (en) | A path trajectory correction method, device, equipment and medium | |
| CN113759892A (en) | Unmanned vehicle obstacle detour method, device, unmanned vehicle and storage medium | |
| Nagasaka et al. | Towards safe, smooth, and stable path planning for on-road autonomous driving under uncertainty | |
| JP5074153B2 (en) | Route generation device and method, and mobile device including route generation device |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20141121 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20150723 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20150804 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20151005 |
|
| 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: 20160322 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20160404 |
|
| R150 | Certificate of patent or registration of utility model |
Ref document number: 5923973 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| LAPS | Cancellation because of no payment of annual fees |