JP2014115924A - Route correction device - Google Patents
Route correction device Download PDFInfo
- Publication number
- JP2014115924A JP2014115924A JP2012270948A JP2012270948A JP2014115924A JP 2014115924 A JP2014115924 A JP 2014115924A JP 2012270948 A JP2012270948 A JP 2012270948A JP 2012270948 A JP2012270948 A JP 2012270948A JP 2014115924 A JP2014115924 A JP 2014115924A
- Authority
- JP
- Japan
- Prior art keywords
- target
- route
- value
- evaluation value
- evaluation
- 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
- 238000012937 correction Methods 0.000 title claims abstract description 184
- 238000011156 evaluation Methods 0.000 claims abstract description 293
- 238000000926 separation method Methods 0.000 claims abstract description 66
- 238000004364 calculation method Methods 0.000 claims abstract description 39
- 238000000034 method Methods 0.000 claims description 75
- 230000008569 process Effects 0.000 claims description 38
- 230000007423 decrease Effects 0.000 description 10
- 238000013459 approach Methods 0.000 description 9
- 230000008859 change Effects 0.000 description 9
- 238000005457 optimization Methods 0.000 description 8
- 230000004069 differentiation Effects 0.000 description 6
- 238000012545 processing Methods 0.000 description 5
- 238000004422 calculation algorithm Methods 0.000 description 3
- 238000012067 mathematical method Methods 0.000 description 3
- 238000002939 conjugate gradient method Methods 0.000 description 2
- 230000007246 mechanism Effects 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 238000002945 steepest descent method Methods 0.000 description 2
- 238000013473 artificial intelligence Methods 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 230000002349 favourable effect Effects 0.000 description 1
- 238000009499 grossing Methods 0.000 description 1
Images
Landscapes
- Manipulator (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
Description
本明細書では、移動体が移動するべき経路の初期値が与えられた後に、あるいは経路の初期値を計算した後に、より好ましい経路に修正する技術を開示する。 The present specification discloses a technique for correcting to a more preferable route after the initial value of the route on which the moving body should move is given or after the initial value of the route is calculated.
出発位置と目標位置が与えられたときに、出発位置から目標位置に至る経路を計算する技術が開発されている。例えば、出発位置と目標位置と障害物の存在範囲を示す地図情報に基づいて経路を計算する技術が開発されている。特許文献１には、移動体と障害物の間に斥力が作用するモデルを利用して、出発位置から目標位置に至る経路を計算する技術が開示されている。
移動体に障害物の存在範囲を検出するセンサを搭載し、センサで検出された障害物の存在範囲から、現在位置から目標位置に至る経路を計算する技術も開発されている。
A technique for calculating a route from a starting position to a target position when a starting position and a target position are given has been developed. For example, a technique for calculating a route based on map information indicating a starting position, a target position, and an obstacle existing range has been developed.
A technology has been developed in which a sensor that detects an obstacle existing range is mounted on a moving body, and a route from the current position to the target position is calculated from the obstacle existing range detected by the sensor.
地図情報に基づいて計算した経路が、現場の情況に適していないことがある。例えば、経路計算に用いた地図情報が不正確であり、経路計算に用いた目標位置では移動体と障害物が干渉してしまうことが後になって判明することもある。あるいは、移動開始時に検出した障害物の存在範囲を示すデータが不正確であり、障害物に接近して障害物の存在範囲が正確に検出できるようになった時点で、移動開始時に設定した目標位置または目標姿勢が好ましいものでなかったことが判明することがある。移動体に搭載されているセンサには死角が存在することが多いことから、実際に移動している途中で、目標位置の周辺における障害物の存在範囲等が判明することもある。経路の初期値には、修正を要するデータが含まれていることがある。 The route calculated based on the map information may not be suitable for the situation at the site. For example, the map information used for the route calculation may be inaccurate, and it may later be found that the moving object and the obstacle interfere with each other at the target position used for the route calculation. Alternatively, the target set at the start of movement when the data indicating the obstacle's presence range detected at the start of movement is inaccurate and the obstacle's existence range can be accurately detected by approaching the obstacle. It may be found that the position or the target posture was not favorable. Since there are many blind spots in the sensors mounted on the moving body, the existence range of obstacles around the target position may be found during actual movement. The initial value of the route may include data that needs to be corrected.
経路の初期値から現場の情況に適した経路に修正する技術も開発されている。非特許文献１では、経路を記述するパラメータからその経路の好ましさ（あるいは現場への適応度）の程度を示す評価値を計算するための評価関数を利用し、評価値を極値化させる経路記述パラメータに修正する。非特許文献１では、好ましい経路ほど小さな評価値を与える評価関数を用いるので、評価値を極小化させる経路記述パラメータに修正する。好ましい経路ほど大きな評価値を与える評価関数を用いる場合には、評価値を極大化させる経路記述パラメータに修正することになる。本明細書では、両者を包含するために、評価値を極値化させる経路記述パラメータを探索するという。好ましい経路ほど小さな値を与える評価関数を用いる場合には評価値を極小とする経路記述パラメータに修正することに相当し、好ましい経路ほど大きな値を与える評価関数を用いる場合には評価値を極大とする経路記述パラメータに修正することに相当する。
A technique for correcting the initial route value to a route suitable for the situation in the field has also been developed. In
特許文献１等の経路計算技術と、非特許文献１等の経路修正技術を併用すると、現場の情況に適した経路が得られるように思われる。しかしながら、実行可能な移動経路に修正できないために、移動体が移動不能となって立ち往生する事態が発生する。
本発明者らがその原因を追究したところ、目標位置および／または目標姿勢（目標位置と目標姿勢の一方または双方）が不適当であるために、実行可能な移動経路に修正できなくなっている事態が発生していることを発見した。非特許文献１では、目標位置に至る経由点について最適化する。しかしながら、目標位置自体は与えられたものであり、最適化の対象にしない。移動体でどこへ行くかは、移動目的によって決まる問題であり、数学的に最適化するものでないのに対応している。
When the route calculation technology such as
When the present inventors investigated the cause, the target position and / or the target posture (one or both of the target position and the target posture) is inappropriate, so that the movement path cannot be corrected. Found that has occurred. In
一般的にいうと、経路計算装置あるいは経路修正装置では、目標位置および／または目標姿勢を与えられたものと扱い、最適化の対象にしないとするのが正しいと思われる。どこへ行くか・いかなる姿勢で停止するかは、移動目的によって決まる問題であり、数学的に最適化にできるものでないはずだからである。実際に、経路計算に用いる地図情報等が正確であるか、あるいは目標位置周辺における障害物の存在範囲が正確には検出されていれば、正確なデータに基づいて設定された目標位置または目標姿勢をそれ以上に最適化する必要はないし、それ以上には最適化しようがないということもできる。 Generally speaking, it seems that it is correct that a route calculation device or a route correction device treats a target position and / or target posture as being given and does not optimize it. This is because where to go and how to stop is a problem that depends on the purpose of movement and cannot be optimized mathematically. Actually, if the map information used for route calculation is accurate, or if the existence range of obstacles around the target position is detected correctly, the target position or target posture set based on accurate data There is no need for further optimization, and there is no further optimization.
しかしながら、経路計算装置あるいは経路修正装置に与えられた目標位置および／または目標姿勢が、正確なデータに基づいて最適に設定された目標位置および／または目標姿勢であるとは限られない。経路計算に用いた地図情報が不正確であり、実際に移動してみると、目標位置および／または目標姿勢が不適切に設定されていたことが判明することがある。あるいは、移動開始時に検出した障害物の存在範囲を示すデータが不正確であり、障害物に接近してみて始めて、目標位置および／または目標姿勢が不適切に設定されていたことが判明することもある。移動体に搭載されているセンサで障害物の存在位置を検出する場合、障害物の存在位置を検出した時のセンサ位置とセンサ姿勢に誤差が含まれており、それに起因して、障害物の存在範囲を示すデータが不正確であることもある。 However, the target position and / or target posture given to the route calculation device or the route correction device is not necessarily the target position and / or target posture set optimally based on accurate data. The map information used for route calculation is inaccurate, and when it is actually moved, it may be found that the target position and / or target posture is set inappropriately. Or, the data indicating the range of obstacles detected at the start of movement is inaccurate, and it becomes clear that the target position and / or target posture was set inappropriately only after approaching the obstacles. There is also. When detecting the position of an obstacle with a sensor mounted on a moving object, an error is included in the sensor position and sensor orientation when the position of the obstacle is detected. The data indicating the existence range may be inaccurate.
本来は最適化の対象に馴染まないにも係らず、目標位置および／または目標姿勢を最適化するという着想を導入すると、実行可能な移動経路に修正できないために移動体が立ち往生してしまうという事態の発生を抑制できることが確認されたことから、本明細書で開示する技術が創作された。 When the idea of optimizing the target position and / or target posture is introduced even though it is not familiar with the optimization target, the moving body may be stuck because it cannot be corrected to an executable movement path Therefore, the technology disclosed in this specification has been created.
本明細書で開示する経路記述データの修正装置は、経路記述データを入力して記憶する装置と、トータル評価関数が極値をとるときの経路記述データを探索する探索装置を備えている。
その探索装置は、経路記述データを用いて、
・目標位置と目標姿勢のうちの少なくとも一方の修正量に対する目標修正量評価値を計算する処理と、
・移動体と障害物の離反距離に対する離反距離評価値を計算する処理と、
・少なくとも目標修正量評価値と離反距離評価値を含むトータル評価値を計算する処理を実行する。
上記の評価値計算では、移動体と障害物が重なりあう計算途上の経路に対して、有限の離反距離評価値を計算する。
The route description data correcting device disclosed in the present specification includes a device for inputting and storing route description data, and a search device for searching for route description data when the total evaluation function takes an extreme value.
The search device uses the route description data,
A process of calculating a target correction amount evaluation value for a correction amount of at least one of the target position and the target posture;
A process of calculating a separation distance evaluation value for a separation distance between the moving object and the obstacle;
A process for calculating a total evaluation value including at least a target correction amount evaluation value and a separation distance evaluation value is executed.
In the above evaluation value calculation, 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 correction device described above, a finite separation distance evaluation value is calculated for a path that is in the process of calculation where a moving object and an obstacle overlap, so a route where the moving object and an obstacle overlap during the correction calculation is processed. However, the calculation can be continued without being disabled. If for some reason (the reason will be described later), a path in which the moving body and the obstacle overlap is formed, the path is corrected so that the moving body and the obstacle do not overlap.
When the target position and / or target posture (hereinafter referred to as target value) is inappropriate for some reason, the correction is not made to a route in which the moving body and the obstacle do not overlap only by correcting the waypoint. In this apparatus, even the target value is a correction target. When the target value is inappropriate for some reason and the moving object and the obstacle overlap, the target value is corrected, and the route search without overlapping the moving object and the obstacle is continued.
When correcting the target value, the target correction amount evaluation value is incorporated in the calculation. As the evaluation function for calculating the target correction amount evaluation value from the target correction amount, a function reflecting the degree to which the correction of the target value is allowed can be used. For example, an evaluation function that gives a higher evaluation value as the correction amount of the target value is smaller is used.
When the target value is inappropriate, it is possible to adopt a route that secures the separation distance to the obstacle by correcting the target value. By correcting the target value, the separation distance evaluation value is expected to change to the high evaluation value side.
By correcting the target value itself, the evaluation value for the correction changes to the low evaluation value side. On the other hand, the separation distance evaluation value is improved to the high evaluation value side, and the total evaluation value including both is used as an index. If the search process is continued, the target value is corrected so that a path in which the moving body and the obstacle do not overlap is corrected, and the path is corrected so that the moving body and the obstacle do not overlap. If the evaluation function for the correction amount is a function that gives a higher evaluation value as the correction amount of the target value is smaller, the target value can be obtained by correcting the target value to a smaller value so that a path where the moving object and the obstacle do not overlap is obtained. Will be corrected.
修正処理を実際に実行すると、何らかの理由で目標値が不適切であって移動体と障害物が重なりあってしまう場合には、目標値を少しずつ修正し、修正された目標値に至る経路によると移動体と障害物が重なりあわない経路に修正される。経路計算に用いた地図情報が不正確であって実際に移動してみると障害物と重なりあってしまう目標値が設定されていたような場合、あるいは、移動開始時に検出した障害物の存在範囲を示すデータが不正確であって障害物に接近してみると障害物と重なりあってしまう目標値が設定されていたことが判明するような場合には、目標値が少しずつ修正され、修正された目標値に至る経路によると移動体と障害物が重なりあわない経路に修正される。修正された目標値に至る経路によると移動体と障害物が重なりあわない経路が得られるに至るまで、修正量を少しずつ増加させながら、経路の最適化処理が繰り返される。
本来は最適化の対象に馴染まない目標値を最適化することによって、実行可能な経路へ修正する計算が実行可能となることが多い。
When the correction process is actually executed, if the target value is inappropriate for some reason and the moving object and the obstacle overlap, the target value is corrected little by little, and the path to the corrected target value is used. And the moving body and the obstacle are corrected so that they do not overlap. If the map information used for route calculation is inaccurate and a target value that overlaps with the obstacle is set when actually moving, or the existence range of the obstacle detected at the start of movement If the target value that is inaccurate and the target value that overlaps the obstacle is found when approaching the obstacle, the target value is corrected little by little. According to the route reaching the target value, the route is corrected so that the moving object and the obstacle do not overlap. According to the route to the corrected target value, the route optimization process is repeated while increasing the correction amount little by little until a route in which the moving object and the obstacle do not overlap is obtained.
By optimizing target values that are not originally familiar with the optimization target, it is often possible to perform a calculation to correct to an executable path.
目標位置と目標姿勢は、目標位置の第１座標成分と、目標位置の第２座標成分と、目標姿勢（角度）で記述される。例えば、地図情報がｘ−ｙ直交座標系で記述されていれば、移動体の基準点のｘ座標値（第１座標成分）と、ｙ座標値（第２座標成分）と、移動体に固定されている基準線とｘ軸のなす角（目標姿勢）で、目標位置と目標姿勢が記述される。
目標値の修正に伴う評価値を計算する際には、
・目標位置の第１座標成分の修正量に対する目標修正量第１評価値を計算する処理と、
・目標位置の第２座標成分の修正量に対する目標修正量第２評価値を計算する処理と、
・目標姿勢の修正量に対する目標修正量第３評価値を計算する処理と、
のうちの少なくとも１種を実施することが好ましい。
The target position and the target posture are described by a first coordinate component of the target position, a second coordinate component of the target position, and a target posture (angle). For example, if the map information is described in an xy orthogonal coordinate system, the x coordinate value (first coordinate component) and the y coordinate value (second coordinate component) of the reference point of the moving object are fixed to the moving object. The target position and the target posture are described by an angle (target posture) formed by the reference line and the x axis.
When calculating the evaluation value associated with the correction of the target value,
A process of calculating a target correction amount first evaluation value for a correction amount of the first coordinate component of the target position;
A process of calculating a target correction amount second evaluation value for a correction amount of the second coordinate component of the target position;
A process of calculating a target correction amount third evaluation value with respect to a target posture correction amount;
It is preferable to implement at least one of them.
目標値の修正に関する拘束条件（あるいは許容条件）は必ずしも一様でない。例えば、ｘ軸に沿って進行して駐車するような場合、目標姿勢の修正に関しては強く拘束し、目標ｘ座標値の修正に関しては弱く拘束するのが適当なことがある。その場合、移動体はｘ軸に沿った姿勢を維持し、障害物までの実際距離が予定距離となる位置に停止させやすい。目標ｘ座標値の修正を強く拘束すると、障害物までの実際距離が予定距離から離れていても目標ｘ座標値の修正が拘束されるのに対し、目標ｘ座標値の修正に対する拘束を緩めれば、障害物までの実際距離を基準にして目標ｘ座標値が修正されていく。 The constraint condition (or allowable condition) regarding the correction of the target value is not necessarily uniform. For example, in the case where the vehicle travels along the x-axis and parks, it may be appropriate to restrict the target posture strongly and weakly restrict the target x-coordinate value. In that case, the moving body maintains a posture along the x-axis, and is easy to stop at a position where the actual distance to the obstacle is the planned distance. If the correction of the target x coordinate value is strongly constrained, the correction of the target x coordinate value is constrained even if the actual distance to the obstacle is away from the planned distance, whereas the constraint on the correction of the target x coordinate value can be relaxed. For example, the target x-coordinate value is corrected based on the actual distance to the obstacle.
ｘ―ｙ座標系に代えて、移動体の進行方向を第１座標軸とし、それに直交する方向を第２座標軸とすることもできる。例えば、移動体の進行方向の目標位置については強く拘束し、それに直交する方向には弱く拘束するのが適当なことがある。その場合、例えば狭い通路内に侵入して停止させる経路に修正しやすい。 Instead of the xy coordinate system, the traveling direction of the moving body may be the first coordinate axis, and the direction orthogonal thereto may be the second coordinate axis. For example, it may be appropriate to strongly restrain the target position in the traveling direction of the moving body and weakly restrain in the direction orthogonal to the target position. In that case, for example, it is easy to correct the route to enter and stop in a narrow passage.
目標位置の第１座標成分と、目標位置の第２座標成分と、目標姿勢の全部を修正対象に含めてもよい。あるいは、そのうちの１種または２種のみを修正対象としてもよい。修正量に対して敏感に増大する評価関数を設定すれば、その値の修正を強く拘束することができ、極端に敏感に変化する評価関数を設定することによって、修正対象から外すこともできる。
目標姿勢を拘束して目標位置のみを修正する形態、目標位置を拘束して目標姿勢のみを修正する形態、目標位置と目標姿勢の修正を同程度に許容する形態等に調整することができる。
The first coordinate component of the target position, the second coordinate component of the target position, and the target posture may all be included in the correction target. Alternatively, only one or two of them may be targeted for correction. If an evaluation function that increases sensitively with respect to the correction amount is set, the correction of the value can be strongly constrained, and it can be excluded from the correction target by setting an evaluation function that changes extremely sensitively.
It is possible to adjust to a mode in which only the target position is corrected by constraining the target posture, a mode in which only the target posture is corrected by constraining the target position, and a mode in which correction of the target position and the target posture is allowed to the same extent.
本明細書に記載の技術によると、目標値が不適切であって移動体と障害物が重なりあう経路となる場合には、移動体と障害物が重なりあわない経路に修正されるまで目標値が修正され、移動体と障害物が重なりあわない経路に修正される。本来的には最適化の対象でない目標値までをも修正対象に含めることで、実行可能な経路が探索されないために（実行可能な経路に修正できないことに対応する）、移動不能となる事態の発生を抑制することができる。 According to the technique described in this specification, when the target value is inappropriate and the path is such that the moving body and the obstacle overlap, the target value is corrected until the path is corrected so that the moving body and the obstacle do not overlap. Is corrected so that the moving object and the obstacle do not overlap. By including even target values that are not originally targeted for optimization, the feasible route is not searched (corresponding to the fact that it cannot be corrected to the feasible route), and it becomes impossible to move. Occurrence can be suppressed.
特に、目標値の種類毎に修正量に対する評価値の変化量に大小関係を設定すると、大きく修正してもかまわない種類の目標値と大きく修正してはいけない種類の目標値があるのに対応して目標値を修正することができ、実情に適合した修正形態に調整することができる。 In particular, if a magnitude relationship is set for the amount of change in the evaluation value relative to the correction amount for each type of target value, there is a type of target value that may be corrected greatly and a type of target value that should not be corrected greatly. Thus, the target value can be corrected and can be adjusted to a correction form adapted to the actual situation.
下記に説明する実施例の主要な特徴を列記する。
特徴１：離反距離評価値は、離反距離が長ければ小さな値をとり、離反距離が短くなると増大する。移動体と障害物が重なりあう場合には、深く重なりあうほど増大する離反距離評価値を用いる。
特徴２：離反距離がポテンシャル増大開始距離以上であれば、離反距離評価値はゼロである。
特徴３：第１座標成分の修正量から計算される第１評価値と、第２座標成分の修正量から計算される第２評価値と、目標姿勢の修正量から計算される第３評価値と、離反距離から計算される離反距離評価値を加味してトータル評価値を計算する。
特徴４：第１座標成分の修正量から計算される第１評価値と、第２座標成分の修正量から計算される第２評価値と、目標姿勢の修正量から計算される第３評価値と、離反距離から計算される離反距離評価値と、経路長から計算される経路長評価値を加味してトータル評価値を計算する。
特徴５：第１座標成分の修正量から計算される第１評価値と、第２座標成分の修正量から計算される第２評価値と、目標姿勢の修正量から計算される第３評価値と、離反距離から計算される離反距離評価値と、経路長から計算される経路長評価値と、移動体の向きと移動体の進行方向の偏差から計算される偏差評価値を加味してトータル評価値を計算する。
The main features of the embodiments described below are listed.
Feature 1: 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 2: If the separation distance is equal to or greater than the potential increase start distance, the separation distance evaluation value is zero.
Feature 3: a first evaluation value calculated from the correction amount of the first coordinate component, a second evaluation value calculated from the correction amount of the second coordinate component, and a third evaluation value calculated from the correction amount of the target posture Then, the total evaluation value is calculated in consideration of the separation distance evaluation value calculated from the separation distance.
Feature 4: a first evaluation value calculated from the correction amount of the first coordinate component, a second evaluation value calculated from the correction amount of the second coordinate component, and a third evaluation value calculated from the correction amount of the target posture The total evaluation value is calculated by taking into account the separation distance evaluation value calculated from the separation distance and the route length evaluation value calculated from the route length.
Feature 5: a first evaluation value calculated from the correction amount of the first coordinate component, a second evaluation value calculated from the correction amount of the second coordinate component, and a third evaluation value calculated from the correction amount of the target posture And the distance evaluation value calculated from the distance, the path length evaluation value calculated from the path length, and the deviation evaluation value calculated from the deviation of the moving body direction and the moving direction of the moving body. Calculate the evaluation value.
経路記述データの修正装置の実施例を説明する。最初に用語の意味を明らかにしておく。
経路記述データ：経由点（移動体の基準点が通過する点）のｘ座標と、経由点のｙ座標と、経由点における移動体方向（ｘ軸の正方向から移動体に固定されている基準線が指向している方向までの反時計回転方向の角θ）が、経由点の順序に従って並んでいるデータをいう。
経路記述データは、式（１）で示され、Ｐｉは式（２）で示される。ｘi，ｙi，θiを、本明細書では、経路記述パラメータという。ｉ＝１は出発点であり、ｉ＝Ｎは目標点である。ｘ１，ｙ１，θ１は与えられている。目標位置と目標姿勢を記述するｘＮ，ｙＮ，θＮは、経路記述データの初期値を計算する段階では与えられた値であるが、修正した方がよい場合がある。実施例の修正装置では、目標位置と目標姿勢を記述するｘＮ，ｙＮ，θＮを変数として扱い、修正処理の対象とする。
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, and θ1 are given. XN, yN, and θN that describe the target position and the target posture are values given at the stage of calculating the initial value of the path description data, but it may be better to correct them. In the correction apparatus of the embodiment, xN, yN, and θN that describe the target position and target posture are treated as variables and are subjected to correction processing.
初期経路：修正装置に入力される、修正処理前の経路記述データで記述される経路をいう。初期経路記述データは、ＲＲＴのような確率的手法で求めることもできるし、Ａ＊探索のようなグリッド探索法で求めることができる。実施例の修正装置は、初期経路記述データの生成プロセスに限定されないで適用することができる。
初期経路は、出発点の出発前に、地図情報に基づいて計算したものであってもよい。あるいは、移動体が移動しながら計算したものであってもよい。地図情報に基づいて初期経路を計算した場合、地図情報が不正確であり、地図情報に基づいて設定した目標値が不適切な場合がある。あるいは、地図情報と移動体の現在位置の間にずれがあり、そのずれの分だけ、移動体から見た目標値が地図上の目標値からずれていることがある。移動体が障害物の存在範囲を検出するセンサを備えている場合、移動開始直後には目標位置までの距離が遠く、目標位置の近傍に位置している障害物の存在範囲が不正確に検出されていることがある。あるいは、センサの死角になっていたために目標位置の近傍に位置している障害物が検出されていなかったこともある。あるいは、障害物の存在範囲を検出したときのセンサの位置と姿勢の計算値に誤差が含まれており、それによって障害物の存在範囲が不正確に検出されていることがある。
初期経路に含まれている目標値は、必ずしも適切なものでなく、障害物の実際の存在範囲が判明した際に、修正したほうがよい場合が生じる。
初期経路には各種の不都合が含まれている可能性があり、例えば移動体がその初期経路に沿って移動すると移動体が障害物に接触することがある。あるいは、経路方向が急激に変化することがある。あるいは、移動体の経路長が不必要に長い場合もある。ＲＲＴのような確率的手法やＡ＊探索のようなグリッド探索法で求めた経路が、移動体と障害物の干渉をもたらすにはいくつかの理由がある。例えば、初期経路の探索過程を簡単化するために、移動体の形状を小さめに設定することがある。あるいは移動体の形状を単純化することがある。移動体の形状を簡単化したために、実際には移動体と障害物が干渉することがある。またグリッド探索手法では移動体方向を無視することがある。移動体が、計算された経路に沿って移動体の方向を変えながら移動すると、移動体と障害物が干渉することがある。種々の理由によって、初期経路には各種の不都合が含まれている可能性がある。経路の修正装置には、障害物と干渉する経路を処理して障害物と干渉しない経路に修正する能力が必要とされる。
Initial route: A route described by route description data before correction processing, which is input to the correction device. The initial path description data can be obtained by a probabilistic method such as RRT, or by a grid search method such as A * search. The correction apparatus according to the embodiment can be applied without being limited to the process of generating the initial path description data.
The initial route may be calculated based on map information before departure from the departure point. Alternatively, it may be calculated while the moving body is moving. When the initial route is calculated based on the map information, the map information may be inaccurate and the target value set based on the map information may be inappropriate. Alternatively, there is a difference between the map information and the current position of the moving object, and the target value viewed from the moving object may be different from the target value on the map by the amount of the difference. If the moving object is equipped with a sensor that detects the range of obstacles, the distance to the target position is long immediately after the start of movement, and the range of obstacles located near the target position is detected incorrectly. Have been. Or the obstacle located in the vicinity of the target position may not be detected due to the blind spot of the sensor. Alternatively, an error may be included in the calculated values of the position and orientation of the sensor when the presence range of the obstacle is detected, and thereby the presence range of the obstacle may be detected inaccurately.
The target value included in the initial route is not necessarily appropriate, and there are cases where it is better to correct the target value when the actual range of the obstacle is found.
The initial path may include various inconveniences. For example, when the moving body moves along the initial path, the moving body may contact an obstacle. Alternatively, the route direction may change abruptly. Alternatively, the path length of the moving body may be unnecessarily long. There are several reasons why a path obtained by a stochastic method such as RRT or a grid search method such as A * search causes interference between a moving object and an obstacle. For example, in order to simplify the search process of the initial route, the shape of the moving body may be set smaller. Or the shape of a moving body may be simplified. Since the shape of the moving object is simplified, the moving object and the obstacle may actually interfere with each other. The grid search method may ignore the moving body direction. When the moving body moves along the calculated route while changing the direction of the moving body, the moving body and the obstacle may interfere with each other. For various reasons, the initial path may contain various inconveniences. The path correction device needs the ability to process a path that interferes with an obstacle and correct it to a path that does not interfere with the obstacle.
移動体：有限の大きさを備えている。
障害物：有限の大きさを備えている。
凸包：物体を包含する最小の凸多角形をいう。
最短距離計算法：離れて存在する２個の凸包のそれぞれにおける最近接点を求め、その距離（凸包間の最短距離）を計算する手法。ＧＪＫアルゴリズムなどの既知の数学手法を利用することができる。
ポテンシャル増大開始距離：離反距離がそれ以下となると、離反距離評価値を計算するポテンシャル関数がゼロでなくなる距離。
許容距離：移動体と障害物の間に確保する最小離反距離。ポテンシャル増大開始距離に等しくしてもよいし、異なっていてもよい。
許容偏差：先行経由点から後続経由点を指す方向と、ｘ軸の正方向がなす角をδとしたときに、先行経由点における移動体の方向とδの偏差、ならびに、後続経由点における移動体の方向とδの偏差に関して許容される最大偏差をいう。
Mobile body: It has a finite size.
Obstacle: It has a finite size.
Convex hull: The smallest 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 movement at the subsequent via point The maximum deviation allowed for the body direction and δ deviation.
（修正装置の全体構成）
図１（ａ）は、初期経路（修正前の経路記述パラメータ）２を入力装置４から入力し、その初期経路を好ましい経路に修正し、出力装置８から修正後経路（修正後の経路記述パラメータ）１０を出力する、経路修正装置のシステム構成を示している。
入力装置４から入力した経路記述パラメータは、経路記述パラメータ記憶装置６に記憶される。後記するように、経路記述パラメータ記憶装置６の記憶内容は、計算の進行に伴って更新されていく。経路記述パラメータ記憶装置６は、修正途上の経路記述パラメータを記憶している装置ということができる。
(Overall configuration of correction device)
In FIG. 1A, an initial route (route description parameter before correction) 2 is input from the
The route description parameter input from the
修正装置は、トータル評価値を計算するトータル評価関数記憶装置２０を備えている。経路記述パラメータ記憶装置６に記憶されている経路記述パラメータを、トータル評価関数に代入することで、トータル評価値を計算することができる。
The correction device includes a total evaluation
トータル評価値は、下記の評価値の総和である。
第１評価値：目標位置のｘ座標値（目標ｘ座標値）の修正量に依存する評価値である。本実施例では、高評価であるほど値が低い評価値を用いる。図１（ｂ）に例示されているように、目標ｘ座標値の修正量がゼロであれば評価値がゼロであり、目標ｘ座標値の修正量が増大すれば増大する評価値を用いる。目標ｘ座標値の修正量がゼロに近いほど、高く評価される。目標ｘ座標値を示すｘＮの修正量ΔｘＮから第１評価値が特定される。
第２評価値：目標位置のｙ座標値（目標ｙ座標値）の修正量に依存する評価値である。図１（ｃ）に例示されているように、目標ｙ座標値の修正量がゼロであれば評価値がゼロであり、目標ｙ座標値の修正量が増大すれば増大する評価値を用いる。目標ｙ座標値の修正量がゼロに近いほど、高く評価される。目標ｙ座標値を示すｙＮの修正量ΔｙＮから第２評価値が特定される。
第３評価値：目標姿勢の修正量に依存する評価値である。本実施例では、図２を参照して後記するように、ｘ軸の正方向と移動体２６に固定されている基準線３０がなす角度θiで目標姿勢を記述する。図１（ｄ）に例示されているように、目標姿勢の修正量がゼロであれば評価値がゼロであり、目標姿勢の修正量が増大すれば増大する評価値を用いる。目標姿勢の修正量がゼロに近いほど、高く評価される。目標姿勢を示すθＮの修正量ΔθＮから第３評価値が特定される。
離反距離評価値：障害物から移動体までの最短距離に依存する評価価である。経路を示すｘ２，ｙ２，θ２・・・ｘＮ，ｙＮ，θＮから、障害物から移動体までの最短距離が計算でき、離反距離評価値が計算できる。離反距離評価値の一例が図１（ｅ）に例示されている。詳しくは図４、図５を参照して後記するが、移動体と障害物間の離反距離が接近するほど、低く評価される評価値（大きな評価値）を与える。
The total evaluation value is the sum of the following evaluation values.
First evaluation value: An evaluation value that depends on the correction amount of the x-coordinate value (target x-coordinate value) of the target position. In this embodiment, an evaluation value having a lower value is used as the evaluation is higher. As illustrated in FIG. 1B, the evaluation value is zero if the correction amount of the target x coordinate value is zero, and an evaluation value that increases if the correction amount of the target x coordinate value increases. The closer the correction amount of the target x coordinate value is to zero, the higher the evaluation. The first evaluation value is specified from the correction amount ΔxN of xN indicating the target x coordinate value.
Second evaluation value: An evaluation value that depends on the correction amount of the y-coordinate value (target y-coordinate value) of the target position. As illustrated in FIG. 1C, the evaluation value is zero if the correction amount of the target y coordinate value is zero, and an evaluation value that increases if the correction amount of the target y coordinate value increases. The closer the correction amount of the target y coordinate value is to zero, the higher the evaluation. The second evaluation value is specified from the correction amount ΔyN of yN indicating the target y coordinate value.
Third evaluation value: an evaluation value depending on the correction amount of the target posture. In this embodiment, as will be described later with reference to FIG. 2, the target posture is described by an angle θi formed by the positive direction of the x axis and the
Separation distance evaluation value: An evaluation value depending on the shortest distance from an obstacle to a moving object. The shortest distance from the obstacle to the moving body can be calculated from x2, y2, θ2,... XN, yN, θN indicating the route, and the separation distance evaluation value can be calculated. An example of the separation distance evaluation value is illustrated in FIG. Although details will be described later with reference to FIGS. 4 and 5, an evaluation value (large evaluation value) that is evaluated to be lower is given as the separation distance between the moving body and the obstacle approaches.
技術的には、上記によって、目標値を修正して移動体と障害物が重なり合わない経路に修正することができるが、本実施例では、さらに下記の評価値を利用する。
経路長評価値：出発点から目標点に至る経路の全長が短いほど高評価を与える評価値である。経由点を示すｘ２，ｙ２，θ２・・・ｘＮ，ｙＮ，θＮから、出発点から目標点に至る経路の全長に対応する量が計算でき、経路長評価値が計算できる。
偏差評価値：移動体の向き（車体方向、姿勢方向、移動体方向といってもよい）と移動方向（経路方向）の偏差が小さいほど高評価を与える評価値である。移動方向がスムースに変化する経路が高評価されるように作用する。経路を示すｘ２，ｙ２，θ２・・・ｘＮ，ｙＮ，θＮから、偏差に対応する量が計算でき、偏差評価値が計算できる。
Technically, according to the above, the target value can be corrected to correct the path so that the moving body and the obstacle do not overlap. In this embodiment, the following evaluation value is further used.
Route length evaluation value: An evaluation value that gives higher evaluation as the total length of the route from the starting point to the target point is shorter. From x2, y2, θ2,..., XN, yN, θN indicating the waypoints, an amount corresponding to the total length of the route from the starting point to the target point can be calculated, and a route length evaluation value can be calculated.
Deviation evaluation value: An evaluation value that gives a higher evaluation as the deviation between the direction of the moving body (which may be referred to as the vehicle body direction, the posture direction, or the moving body direction) and the moving direction (path direction) is smaller. It acts so that a path in which the moving direction changes smoothly is highly evaluated. An amount corresponding to the deviation can be calculated from x2, y2, θ2,..., XN, yN, θN indicating the route, and a deviation evaluation value can be calculated.
修正装置は、トータル評価関数を経路記述パラメータ（ｘ２，ｙ２，θ２・・・ｘＮ，ｙＮ，θＮ）で偏微分した偏微分関数記憶装置１２を備えている。すなわち、中間経路点（ｘ２，ｙ２，θ２・・・ｘＮ−１，ｙＮ−１，θＮ−１）で偏微分した関数と、目標位置と目標姿勢（ｘＮ，ｙＮ，θＮ）で偏微分した関数を記憶している。トータル評価関数をｘiで偏微分した偏微分関数と、トータル評価関数をｙiで偏微分した偏微分関数と、トータル評価関数をθiで偏微分した偏微分関数に代入することで、３種類の偏微分係数を計算することができる。図１では、偏微分関数が一つのブロック１２にまとめて図示されているが、ｘiの偏微分関数と、ｙiの偏微分関数と、θiの偏微分関数に分けて図示してもよい。
The correction device includes a partial differential
経路記述パラメータを変数とするトータル評価関数が与えられ、経路記述パラメータを変数とする偏微分関数（トータル評価関数を経路記述パラメータで偏微分した関数）が与えられれば、既知の数学的手法で、トータル評価関数が極値をとる際の経路記述パラメータの値を決定することができる。本実施例では、好ましい経路ほど小さな評価値を与えるトータル評価関数を採用する。本実施例では、トータル評価関数が極小値をとる経路記述パラメータの値を決定することによって、好ましい経路に修正する。本実施例では、好ましい経路に修正する計算速度と安定性にすぐれている準ニュートン法を採用する。準ニュートン法に限られず、最急降下法、ニュートン法、共役勾配法などの勾配法を用いることもできる。 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 gives a smaller evaluation value for a preferable route is adopted. In this embodiment, the route is corrected to a preferable route by determining a route description parameter value at which the total evaluation function takes a minimum value. In the present embodiment, a quasi-Newton method is used which is excellent in calculation 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 where a multivariable function takes a minimum value, a partial differential function is used to determine the correction direction of the variable that minimizes the function value. 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 where 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 vector is correct, the function value (total evaluation value) is calculated within the range exceeding the minimum 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 a step
探索が進行して極小位置に接近すると、探索方向ベクトル計算装置１４で計算された方向にステップ幅設定手段１６で設定された修正量だけ修正しても、評価値が変化しなくなる。極小点では、偏微分係数がゼロであることに対応する。
本実施例では、探索方向ベクトル計算装置１４で計算された方向にステップ幅設定手段１６で設定された修正量だけ修正することで関数値が変化（減少）するうちは、修正後の経路記述パラメータによって、経路記述パラメータ記憶装置６の記憶内容を更新する。更新した経路記述パラメータを用いて極小化条件探索処理を繰り返す。経路記述パラメータ記憶装置６の記憶内容は、計算途上の経路記述パラメータに更新されていく。
探索方向ベクトル計算装置１４で計算された方向にステップ幅設定手段１６で設定された修正量だけ修正しても評価値が変化しなくなると、極小点が検索されたとして処理を終了する。その段階の経路記述データ１０が、出力装置８から出力される。
When the search progresses and approaches the minimum position, the evaluation value does not change even if the correction amount set by the step width setting means 16 is corrected in the direction calculated by the search direction
In this embodiment, while the function value changes (decreases) by correcting only the correction amount set by the step width setting means 16 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 16 is corrected in the direction calculated by the search direction
（経路記述データの詳細）
図２において、参照番号２６は移動体を示し、出力装置８から出力された修正後経路に沿って移動する。参照番号２４は移動体２６を包含する凸包であり、図１の修正装置では、凸包２４と障害物間の最短距離を求めて離反距離を計算する。参照番号２８は移動体２６に固定されている基準点であり、参照番号３０は移動体２６に固定されている基準線である。
経路は、データＰｉ＝（ｘi，ｙi，θi）（ここでｉ＝１，２，・・Ｎ）で定義される。移動体２６は、基準点２８が（ｘi，ｙi）の座標点を辿るように移動する。また、移動体２６は、基準線３０がθiの方向を向く車体姿勢をとる。本明細書では、角度をｘ軸の正方向から反時計回転方向に計測した角度で示す。
参照記号Ｏjは、移動体２６の走行を妨げる障害物である。本明細書では、出発点（ｘ１，ｙ１）から目標点（ｘＮ，ｙＮ）までの経路を決定する際に考慮する必要がある障害物がＭ個存在するものとする。
(Details of route description data)
In FIG. 2,
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は計算されない。この場合、経路方向と移動体方向が等しいとすることができる。あるいは別の手法で、移動体方向を決定してもよい。
確率的手法あるいはグリッド探索法で狭い空間を通過する経路を求める場合、探索に失敗することがある。確率的手法による場合には、実用的な時間内に通過経路が計算できないことがある。グリッド探索による場合は、離散化されたグリッドが粗すぎて通過経路を発見できないことがある。探索失敗を避けるために、初期経路の探索過程で制約条件を緩和し、実用的な時間内に初期経路が探索されるようにしている。
上記の事情等によって、初期経路には下記の問題が内在されていることがある。
（１）目標位置および／または目標姿勢が適切でなく、目標位置および／または目標姿勢をわずかに修正すれば移動体と障害物と重なり合わないのに、初期データの目標位置と目標姿勢によると、移動体と障害物と重なりあって走行不能となってしまう。
（２）移動体と障害物が干渉する（重なりあう）経路である。
（３）移動体と障害物が重なり合わないまでも非常に接近する経路である。
（４）経路方向が急激に変化する経路である。
（５）経路方向と移動体方向が不一致であって、移動体が追従できない経路情報である。
図１の修正装置は、上記の問題を解消する側に修正した経路を計算する。初期経路のうち、出発点のデータＰ１＝（ｘ１，ｙ１，θ１）は、確定的なものであり、図１の修正装置で修正することはない。経由点のデータＰｉ（i＝２，・・，Ｎ−1）は最善なものでない可能性があり、図１の修正装置で修正する。目標点のデータＰＮ（i＝Ｎ）も最善なものでない可能性があり、図１の修正装置で修正する。その修正計算のために、修正装置は下記の６種類の評価関数を利用する。
(Details of 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.
When a route that passes through a narrow space is obtained by a probabilistic method or a grid search method, the search may fail. In the case of the probabilistic method, the passage route may not be calculated within a practical time. In the case of grid search, the discretized grid may be too coarse to find a passage route. In order to avoid a search failure, the constraints are relaxed in the initial route search process so that the initial route is searched within a practical time.
Due to the above circumstances, the following problems may be inherent in the initial route.
(1) According to the target position and target posture of the initial data, the target position and / or target posture is not appropriate, and the moving body and the obstacle do not overlap if the target position and / or target posture is slightly corrected. The moving body and obstacles overlap each other, making it impossible to travel.
(2) A path where a moving body and an obstacle interfere (overlap).
(3) The route is very close even if the moving body and the obstacle do not overlap.
(4) A route whose route direction changes abruptly.
(5) The route information and the moving body direction are mismatched and the moving body cannot follow the route information.
The correction device in FIG. 1 calculates a corrected route on the side that solves the above problem. Of the initial route, the starting point data P1 = (x1, y1, θ1) is deterministic and is not corrected by the correction device of FIG. The via point data Pi (i = 2,..., N−1) may not be the best, and is corrected by the correction device of FIG. The target point data PN (i = N) may not be optimal, and is corrected by the correction device of FIG. For the correction calculation, the correction device uses the following six types of evaluation functions.
（第１評価値）目標ｘ座標値を示すｘＮの修正量ΔｘＮから第１評価値が特定される。目標ｘ座標値の初期値が好ましいものでなく、実際には障害物と移動体が重なり合ってしまう位置であることがある。目標ｘ座標値の初期値が好ましいものでない場合、それを与えられた値として固定的に扱うと、走行可能な経路に修正できず、移動体が立ち往生することがある。本実施例では、目標ｘ座標値を修正対象に含めるために、目標ｘ座標値ｘＮの修正量ΔｘＮに対して有限の評価値を与える。本実施例では、図２の（２）に示すように、目標ｘ座標値の修正量ΔｘＮの二乗に比例する第１評価値を与える。Ｋ１は重み係数である。
目標ｘ座標値の初期値によると移動体と障害物が重なりあう場合、目標ｘ座標値を修正することで第１評価値が増大する（低評価に変わる）が、反面、離反距離評価値が高評価値に変わる。トータル評価値を極小化するプロセスを実行することで、移動体と障害物が重なりあわない位置に目標ｘ座標値を修正する結果が得られる。目標値ｘ座標値を修正することで移動体が障害物と重なりあわないで移動できる経路に修正される。
(First evaluation value) The first evaluation value is specified from the correction amount ΔxN of xN indicating the target x-coordinate value. The initial value of the target x-coordinate value is not preferable and may actually be a position where the obstacle and the moving body overlap. When the initial value of the target x-coordinate value is not preferable, if it is fixedly handled as a given value, it cannot be corrected to a travelable route and the moving body may get stuck. In this embodiment, in order to include the target x-coordinate value in the correction target, a finite evaluation value is given to the correction amount ΔxN of the target x-coordinate value xN. In the present embodiment, as shown in (2) of FIG. 2, a first evaluation value proportional to the square of the correction amount ΔxN of the target x coordinate value is given. K1 is a weighting factor.
According to the initial value of the target x-coordinate value, when the moving object and the obstacle overlap, the first evaluation value increases (changes to low evaluation) by correcting the target x-coordinate value. It changes to a high evaluation value. By executing the process of minimizing the total evaluation value, a result of correcting the target x-coordinate value at a position where the moving body and the obstacle do not overlap can be obtained. By correcting the target value x-coordinate value, the moving body is corrected so as to move without overlapping the obstacle.
（第２評価値）目標ｙ座標値を示すｙＮの修正量ΔｙＮから第２評価値が特定される。本実施例では、図２の（２）に示すように、目標ｙ座標値の修正量ΔｙＮの二乗に比例する第２評価値を利用する。Ｋ２は重み係数である。第２評価値を導入すると、目標ｙ座標値を修正することによって移動体が障害物と重なりあわないで移動できる経路に修正される。 (Second Evaluation Value) A second evaluation value is specified from the correction amount ΔyN of yN indicating the target y coordinate value. In the present embodiment, as shown in (2) of FIG. 2, a second evaluation value proportional to the square of the correction amount ΔyN of the target y coordinate value is used. K2 is a weighting factor. When the second evaluation value is introduced, the target y coordinate value is corrected so that the moving body can be corrected so as to move without overlapping the obstacle.
（第３評価値）目標姿勢を示すθＮの修正量ΔθＮからから第３評価値が特定される。本実施例では、図２の（２）に示すように、目標姿勢の修正量ΔθＮの二乗に比例する第３評価値を利用する。Ｋ３は重み係数である。第３評価値を導入すると、目標姿勢を修正することによって移動体が障害物と重なりあわないで移動できる経路に修正される。 (Third evaluation value) The third evaluation value is specified from the correction amount ΔθN of θN indicating the target posture. In this embodiment, as shown in (2) of FIG. 2, a third evaluation value proportional to the square of the correction amount ΔθN of the target posture is used. K3 is a weighting factor. When the third evaluation value is introduced, the target posture is corrected so that the moving body can be corrected so as to move without overlapping the obstacle.
本実施例では、第１〜第３評価関数に、目標値の修正量ΔｘＮ，ΔｙＮ，ΔθＮの二乗に比例する評価関数を用いる。その重み係数Ｋ１，Ｋ２，Ｋ３は、それぞれ固有に決めることができる。例えば、ｘ軸に沿って進行して駐車するような場合、目標姿勢の修正に関しては強く拘束し、目標ｘ座標値の修正に関しては弱く拘束するのが適当なことがある。そのためには、目標姿勢の修正量ΔθＮに対して敏感に変化する関数を第３評価関数とし（重み係数Ｋ３に大きな値を用いる）、目標ｘ座標値の修正量ΔｘＮに対して鈍感に変化する関数を第１評価関数とする（重み係数Ｋ１に小さな値を用いる）。その場合、姿勢に関しては初期値に近い値を維持するためにｘ軸に沿った姿勢を維持する一方において、ｘ方向の位置に関しては柔軟に修正して障害物までの実際距離を優先して停止させるという経路に修正することができる。
また第１〜第３評価関数の重み係数を変えるのに代えて、それぞれに固有の関数形を利用してもよい。
In the present embodiment, evaluation functions that are proportional to the squares of the correction amounts ΔxN, ΔyN, ΔθN of the target values are used as the first to third evaluation functions. The weighting factors K1, K2, and K3 can be uniquely determined. For example, in the case where the vehicle travels along the x-axis and parks, it may be appropriate to restrict the target posture strongly and weakly restrict the target x-coordinate value. For this purpose, a function that changes sensitively with respect to the correction amount ΔθN of the target posture is set as a third evaluation function (a large value is used for the weighting coefficient K3), and changes insensitive to the correction amount ΔxN of the target x-coordinate value. Let the function be the first evaluation function (use a small value for the weighting factor K1). In that case, while maintaining the posture along the x-axis in order to maintain a value close to the initial value for the posture, the position in the x direction is flexibly corrected to give priority to the actual distance to the obstacle and stop. It can be corrected to the path of letting.
Further, instead of changing the weighting coefficients of the first to third evaluation functions, a unique function form may be used for each.
（経路長評価値）
出発点から目標点に至る経路の全長が短いことが好ましい。全長が短い経路に修正されるように、修正装置は経路長評価関数を利用する。経路長評価関数を、図２の（３）式に示す。係数Ｋａは、トータル評価値に占める経路長評価値の重みを示している。距離ｌiは、座標点（ｘｉ，ｙｉ）から座標点（ｘｉ＋１，ｙｉ＋１）までの距離である。ｉ＝１からＮ−１までの総和を取れば、経路全長に関する評価値が求められる。全長が短いほど小さな評価値が計算される。修正装置は、トータル評価値を極小化する経路に修正する。全長が短いほど小さな評価値が計算される経路長評価関数を導入するために、修正装置によって全長が短い経路に修正される。
図２の（３）式に示す経路長評価関数を極小化すると、
（１）全長が短くなる経路に修正され、
（２）Ｐ２，・・，ＰＮ−１というＮ−２個の経由点を、等間隔に配置した経路記述データに修正される（全長が等しければ、経由点が等間隔である場合の評価値の方が、経由点が等間隔でない場合の評価値よりも小さくなる）。
経路長評価関数以外の評価関数をも利用するために、全長のみを考慮して修正されるものでない。目標位置を出発位置に近づける修正をすれば、経路長評価値は高評価に修正されるが、目標位置を大きく修正すれば第１〜第３評価値が低評価に修正されるので、目標位置が出発位置にむやみに近づけられることはない。
(Path length evaluation value)
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 (3) in FIG. The coefficient Ka indicates the weight of the path length evaluation value in the total evaluation value. The distance l i is a distance from the coordinate point (xi, yi) to the coordinate point (xi + 1, yi + 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 total evaluation value to a path that minimizes the total 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 (3) in FIG. 2 is minimized,
(1) It has been corrected to a route that shortens the overall length,
(2) N-2 via points P2,..., PN-1 are corrected to route description data arranged at equal intervals (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).
Since an evaluation function other than the path length evaluation function is also used, it is not corrected in consideration of only the total length. If the target position is corrected to be close to the departure position, the path length evaluation value is corrected to a high evaluation. However, if the target position is largely corrected, the first to third evaluation values are corrected to a low evaluation. Will not be brought close to the starting position.
（偏差評価関数）
図２に示すように、移動体２６は、移動方向（経路方向）と独立して車体方向（姿勢方向、移動体方向）を変える機構を備えていないから、経由点Ｐi＝（ｘi，ｙi）を辿って移動する際の移動体方向θiが経路方向δiに一致することが保証されていない。その問題を解消するために、修正装置には経路方向δiと移動体方向θiを一致させるように修正する評価関数が導入されており、移動体がＰi＝（ｘi，ｙi）を辿って移動すると、移動体方向θiが経路方向δiによく一致する関係が得られるようにしている。移動方向から独立して車体方向を変える機構を備えていない移動体２６の経路を修正する際に、経路方向δiと独立して移動体方向θiを定義する経路記述データを採用しても、問題が生じないようにしている。
(Deviation evaluation function)
As shown in FIG. 2, the moving
図３の（ｂ）は、経由点Ｐiから経由点Ｐi＋１に至る経路方向（δi）と、移動体方向の偏差を示している。実際には、経由点Ｐiにおける移動体方向θiと、経由点Ｐi＋１における移動体方向θi+1は相違している。そこで、２種類の偏差評価関数を導入している。
第１偏差評価関数は、図３の（３）に示すように、経路方向δiと移動体方向θiの偏差φiを自乗した値を計算する。係数Ｋｂは、トータル評価値に占める第１偏差評価値の重みを示している。図３の（４）に示すように、i＝１からＮ−１までの総和を取れば、第１偏差評価値を経路全長に亘って累積した第１偏差評価値Ｕｂを計算することができる。偏差φiが小さいほど評価値が小さくなる。評価値を極小化する方向に修正すると、偏差φiが小さくなる経路に修正される。
第２偏差評価関数は、図３の（５）に示すように、経路方向δiと移動体方向θｉ＋１の偏差ηｉを自乗した値を計算する。係数Ｋｃは、トータル評価値に占める第２偏差評価値の重みを示している。図３の（６）に示すように、i＝１からＮ−１までの総和を取れば、第２偏差評価値を経路全長に亘って累積した第２偏差評価値Ｕｃを計算することができる。偏差ηｉが小さいほど評価値が小さくなる。評価値を極小化する方向に修正すると、偏差ηｉが小さくなる経路に修正される。
(B) of FIG. 3 shows the deviation of the path direction (δi) from the transit point Pi to the transit 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 evaluation function calculates a value obtained by squaring the deviation φi between the route 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 minimization, the path is corrected so as to reduce the deviation φi.
As shown in (5) of FIG. 3, the second deviation evaluation function calculates a value obtained by squaring the deviation ηi between the route direction δi and the moving body
移動体方向θを考慮すると、障害物までの離反距離を増大させられることがある。移動体方向θを加味して経路を修正すると、移動体方向θを振りながら移動することで移動体が障害物の間を縫って移動する経路に修正することができる。この場合には、移動体方向θによく一致する経路方向δとなる経由点位置に修正されることになる。
図３の(ａ)と（ｂ）比較すると明らかに、経由点Ｐｉで第１評価関数と第２評価関数を計算することになる。その際には、共通して移動体方向θｉを利用する。第１評価関数は、移動体方向θｉと経路方向δｉの偏差φｉが小さくなる方向に修正する。第２評価関数は、移動体方向θｉと経路方向δｉ−１の偏差ηｉ−１が小さくなる方向に修正する。共通の移動体方向θｉに対する偏差が小さくなる側に修正すると、経路方向δｉ−１と経路方向δｉの差も小さくなっていく。経路方向δｉ−１と経路方向δｉの差が大きいことは、経路が急激に方向を変えることを意味する。経路方向δｉ−１と経路方向δｉの差を小さくすることは、緩やかに方向を変える経路に修正することに相当する。１つの経由点Ｐiに対して、その経由点Ｐiにおける移動体方向θiと直前の経路方向δi-1の偏差ηi-1に対応する第２評価関数と、その経由点Ｐiにおける移動体方向θiと直後の経路方向δiの偏差φiに対応する第１評価関数を用いて評価値を計算するようにすると、急激に方向を変える経路から緩やかに方向を変える経路に修正される。経路の平滑化処理が実施される。
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 θ.
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 transit point 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 is not the object of correction work, i = 2,.
（トータル評価関数）
本実施例では、下記の式３で示されるトータル評価関数を利用する。式３において、Ｕ１は第１評価関数であり、Ｕ２は第２評価関数であり、Ｕ３は第３評価関数であり、Ｕａは経路長評価関数であり、Ｕｂは第１偏差評価関数であり、Ｕｃは第２偏差評価関数であり、Ｕｅは離反距離評価関数である。
In this embodiment, a total evaluation function expressed by the following
本実施例では、トータル評価関数を経路記述パラメータｘi，ｙi，θiの各々で偏微分した偏微分関数を利用して、トータル評価値を極小化する経路記述パラメータｘi，ｙi，θiを探索する。
トータル評価関数＝第１評価関数＋第２評価関数＋第３評価関数＋経路長評価関数＋偏差評価関数＋離反距離評価関数であるために、
トータル評価関数の偏微分関数＝第１評価関数の偏微分関数＋第２評価関数の偏微分関数＋第３評価関数の偏微分関数＋経路長評価関数の偏微分関数＋偏差評価関数の偏微分関数＋離反距離評価関数の偏微分関数の関係が成立する。
In this 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 total evaluation function = first evaluation function + second evaluation function + third evaluation function + path length evaluation function + deviation evaluation function + separation distance evaluation function,
Partial differential function of total evaluation function = partial differential function of first evaluation function + partial differential function of second evaluation function + partial differential function of third evaluation function + partial differential function of path length evaluation function + partial differential function of deviation evaluation function The relation of partial differential function of function + separation distance evaluation function is established.
（第１〜第３評価関数の偏微分関数）
図２の（２）に示す第１評価関数Ｕ１をΔｘＮで偏微分した偏微分関数を求めると下記の式（ａ）に示すものとなる。ここで、ｘＮは修正後の目標ｘ座標値であり、ｘＮＯは目標ｘ座標値の初期値である。第２評価関数の偏微分関数は下記の式（ｂ）に示すものとなる。ここで、ｙＮは修正後の目標ｙ座標値であり、ｙＮＯは目標ｙ座標値の初期値である。第３評価関数の偏微分関数下記の式（ｃ）に示すものとなる。θＮは修正後の目標姿勢であり、θＮＯは目標姿勢の初期値である。
(Partial differential function of the first to third evaluation functions)
When a partial differential function obtained by partial differentiation of the first evaluation function U1 shown in (2) of FIG. 2 by ΔxN is obtained, the following equation (a) is obtained. Here, xN is a corrected target x-coordinate value, and xNO is an initial value of the target x-coordinate value. The partial differential function of the second evaluation function is shown in the following formula (b). Here, yN is the corrected target y coordinate value, and yNO is the initial value of the target y coordinate value. The partial differential function of the third evaluation function is shown in the following formula (c). θN is a corrected target posture, and θNO is an initial value of the target posture.
（経路長評価関数の偏微分関数）
図２の（３）に示した経路長評価関数Ｕａの偏微分関数を求めると下記の式（ｄ），（ｅ），（ｆ）で示すものとなる。
The partial differential function of the path length evaluation function Ua shown in (3) of FIG. 2 is obtained by the following equations (d), (e), and (f).
（偏差評価関数の偏微分関数）
図３の（４）（６）に示した第１偏差評価関数Ｕｂと第２偏差評価関数Ｕｃの偏微分関数から偏差評価関数Ｕｂ＋Ｕｃの偏微分関数を求めると下記となる。
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 (10) of FIG. 5, 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 the following.
（評価関数の極小化過程）
多変数関数とその関数の偏微分関数が判明していれば、既知の数学的手法で、関数を極小化する変数の値を決定することができる。その問題は、最急降下法、ニュートン法、準ニュートン法、共役勾配法などの勾配法で解くことができる。本実施例では、計算の収束速度と安定性のバランスが優れている準ニュートン法を用いる。
(Minimization process of evaluation function)
If the multivariable 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 sensitively 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 the correction direction is used to reduce the function value sensitively, the function value should decrease. However, if the correction amount is too large, the function is corrected beyond the position where the minimum value is taken, 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 a correction direction in which the function value decreases more sensitively than 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 for minimizing the function value.
When approaching a 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 arrangement of variables for minimizing the function value has been corrected.
準ニュートン法では、修正方向ｄkが下記の式で計算される。なお添え字ｋは、前記（１）（２）の修正処理をすでにｋ-１回繰り返しており、これからｋ回目の修正処理を実施することを示している。
本実施例では、修正量を下記の表に従って変えていく。
極小値に近づくにつれ、修正前のトータル評価関数の値と修正後のトータル評価関数の値の差の絶対値は減少していく。本実施例では、その差の絶対値が、閾値（本実施例では１０−６）を下回ったときに、トータル評価値を極小化する経路記述パラメータに並びに修正されたとして修正処理を終了する。そうして修正された経路記述パラメータの並びを出力装置８から出力する。
As the local minimum value is approached, 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 the present embodiment, when the absolute value of the difference falls below a threshold value (10 −6 in the present embodiment), the correction process is terminated because it is corrected along with the path description parameter that minimizes the total evaluation value. 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以下であることが保証されている経路記述データに修正される。経路方向δと移動体方向θの偏差が小さい経路が計算されていれば、計算された移動体方向θと実際の移動体方向はよく一致する。
移動体と障害物の離反距離に関して最短値を設定して修正作業を進めることもできる。下記式を加えて修正処理を進めれば、移動体と障害物の最短離反距離が許容距離minlen以上であることが保証されている経路記述データに修正される。許容距離minlenは、図５に示した増加開始距離le0に等しくてもよいし、異なっていてもよい。
また、数式１０から１２のうちの２種または３種を用いて経路修正処理を実施してもよい。
Further, the route correction process may be performed using two or three of the
図７は、移動体が移動可能な範囲と、出発点Ｐ1と、移動体が出発点Ｐ1にあるときの凸包２４と、目標点ＰＮと、移動体が目標点ＰＮにあるときの凸包２４を例示している。
図８は、初期経路を例示している。初期経路によると、移動体と障害物が重なりあってしまう。図９は、トータル評価値を低下させる経路記述データの探索処理を３０回実施した時点での経路を示している。図１０は、探索処理を６０回実施した時点での経路を示している。図１１は、探索処理を１３８回実施した時点での経路を示している。１３８回の探索処理の結果、経路記述データはそれ以上に修正されなくなった。移動体は、障害物を避けながら、目標点に目標姿勢で到達する。
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.
（第２実施例）
図１３は、初期経路記述データ１３２における目標位置（ｘＮＯ、ｙＮＯ）１３２ｅが不適切に設定されており、実際には障害物と干渉する範囲１３６内に設定されていた場合を例示する。また、目標姿勢に沿った座標軸における目標座標値の修正量に対して第１評価関数が設定されており、目標姿勢に直交する座標軸における目標座標値の修正量に対して第２評価関数が設定されている場合を例示している。さらに、目標姿勢に沿った座標軸での目標位置の修正量に対して第１評価関数が敏感に変化し、目標姿勢に直交する座標軸での目標位置の修正量に対して第２評価関数が鈍感に変化し、目標姿勢の修正量に対して第３評価関数が敏感に変化する場合を例示している。この場合、目標姿勢に直交する座標軸上での目標座標が柔軟に修正される。その結果、干渉範囲１３６外に目標位置を修正する際に、主として目標姿勢に直交する方向に修正する探索が行われ、１３４ｅに示す修正後目標位置が探索され、目標値を修正することによってトータル評価値が極小化された場合を例示している。
(Second embodiment)
FIG. 13 illustrates a case where the target position (xNO, yNO) 132e in the initial
第１・第３評価値が敏感に変化し、第２評価値が鈍感に変化する場合、目標姿勢に直交する方向の収集量αに対して、目標ｘ座標の修正量ΔｘＮと目標ｙ座標の修正量ΔｙＮは図１４に示すものになる。この関係を利用して、目標姿勢に直交する方向の修正量αに関するトータル評価値の偏微分関数を得ることができ、それによって、トータル評価値を極小化するαの値を探索することができる。第２実施例によると、目標位置の初期値が不適切な場合に、進行方向と直交方向に目標位置をずらす経路に修正されることになる。 When the first and third evaluation values change sensitively and the second evaluation value changes insensitively, the correction amount ΔxN of the target x coordinate and the target y coordinate of the collection amount α in the direction orthogonal to the target posture The correction amount ΔyN is as shown in FIG. Using this relationship, it is possible to obtain a partial differential function of the total evaluation value regarding the correction amount α in the direction orthogonal to the target posture, thereby searching for the value of α that minimizes the total evaluation value. . According to the second embodiment, when the initial value of the target position is inappropriate, the route is corrected to shift the target position in the direction orthogonal to the traveling direction.
以上、本発明の具体例を詳細に説明したが、これらは例示にすぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。
例えば、実施例ではトータル評価値に経路長評価値と偏差評価値を組み込んでいるが、これらの一方または双方を外してもよい。また実施例では、トータル評価値に目標修正量第１評価値と目標修正量第２評価値と目標修正量第３評価値の全部を組み込んでいるが、これらのうちの1種または2種のみを採用してもよい。
また、本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。
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.
For example, in the embodiment, the path length evaluation value and the deviation evaluation value are incorporated in the total evaluation value, but one or both of them may be removed. Further, in the embodiment, the target correction amount first evaluation value, the target correction amount second evaluation value, and the target correction amount third evaluation value are all incorporated in the total evaluation value, but only one or two of them are included. May be adopted.
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: Route description parameter before correction 4: Input device 6: Storage device of route description parameter under correction 8: Output device 10: Route description parameter after correction 12: Partial differential function of total evaluation function for route description parameter Storage device 14: Search direction vector calculation device 16: Step width setting device 18: Total evaluation value change determination device 20: Total evaluation function storage device
Claims (2)
トータル評価関数が極値をとるときの経路記述データを探索する探索装置を備えており、
その探索装置が、経路記述データを用いて、
・目標位置と目標姿勢のうちの少なくとも一方の修正量に対する目標修正量評価値を計算する処理と、
・移動体と障害物の離反距離に対する離反距離評価値を計算する処理と、
・少なくとも目標修正量評価値と離反距離評価値を含むトータル評価値を計算する処理を実行し、
移動体と障害物が重なりあう計算途上の経路に対して、有限の離反距離評価値を計算することを特徴とする経路記述データの修正装置。 A device for inputting and storing route description data;
It has a search device that searches for route description data when the total evaluation function takes an extreme value,
The search device uses the route description data,
A process of calculating a target correction amount evaluation value for a correction amount of at least one of the target position and the target posture;
A process of calculating a separation distance evaluation value for a separation distance between the moving object and the obstacle;
・ Execute the process of calculating the total evaluation value including at least the target correction amount 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 search device
A process of calculating a target correction amount first evaluation value related to a correction amount of the first coordinate component of the target position;
A process of calculating a target correction amount second evaluation value related to the correction amount of the second coordinate component of the target position;
A process of calculating a target correction amount third evaluation value related to the correction amount of the target posture;
The apparatus for correcting path description data according to claim 1, wherein at least one of the above is implemented.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2012270948A JP6131587B2 (en) | 2012-12-12 | 2012-12-12 | Route correction device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2012270948A JP6131587B2 (en) | 2012-12-12 | 2012-12-12 | Route correction device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JP2014115924A true JP2014115924A (en) | 2014-06-26 |
| JP6131587B2 JP6131587B2 (en) | 2017-05-24 |
Family
ID=51171826
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2012270948A Expired - Fee Related JP6131587B2 (en) | 2012-12-12 | 2012-12-12 | Route correction device |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP6131587B2 (en) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2016198855A (en) * | 2015-04-10 | 2016-12-01 | セイコーエプソン株式会社 | Robot, control device, and control method |
| JP2019214084A (en) * | 2018-06-11 | 2019-12-19 | オムロン株式会社 | Route planning device, route planning method, and route planning program |
| WO2024084560A1 (en) * | 2022-10-18 | 2024-04-25 | ファナック株式会社 | Adjustment device |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH05297937A (en) * | 1992-02-10 | 1993-11-12 | Honda Motor Co Ltd | Obstacle evading route searching method for mobile object |
| JP2004287672A (en) * | 2003-03-20 | 2004-10-14 | Nissan Motor Co Ltd | Recommended manipulation variable generating device for vehicle |
| JP2010073080A (en) * | 2008-09-22 | 2010-04-02 | Komatsu Ltd | Method of generating traveling path of unmanned vehicle |
| JP2010152684A (en) * | 2008-12-25 | 2010-07-08 | Toshiba Corp | Method and apparatus for generating trajectory of mobile body |
| JP2010202030A (en) * | 2009-03-03 | 2010-09-16 | Nissan Motor Co Ltd | Driving support device and driving support method for vehicle |
| JP2011031309A (en) * | 2009-07-29 | 2011-02-17 | Toshiba Corp | Method of generating moving body posture and device of generating moving body posture |
| JP2012148861A (en) * | 2011-01-20 | 2012-08-09 | Univ Of Tokyo | Movable body system |
| JP2013134615A (en) * | 2011-12-26 | 2013-07-08 | Toyota Central R&D Labs Inc | Route correction device |
-
2012
- 2012-12-12 JP JP2012270948A patent/JP6131587B2/en not_active Expired - Fee Related
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPH05297937A (en) * | 1992-02-10 | 1993-11-12 | Honda Motor Co Ltd | Obstacle evading route searching method for mobile object |
| JP2004287672A (en) * | 2003-03-20 | 2004-10-14 | Nissan Motor Co Ltd | Recommended manipulation variable generating device for vehicle |
| JP2010073080A (en) * | 2008-09-22 | 2010-04-02 | Komatsu Ltd | Method of generating traveling path of unmanned vehicle |
| JP2010152684A (en) * | 2008-12-25 | 2010-07-08 | Toshiba Corp | Method and apparatus for generating trajectory of mobile body |
| JP2010202030A (en) * | 2009-03-03 | 2010-09-16 | Nissan Motor Co Ltd | Driving support device and driving support method for vehicle |
| JP2011031309A (en) * | 2009-07-29 | 2011-02-17 | Toshiba Corp | Method of generating moving body posture and device of generating moving body posture |
| JP2012148861A (en) * | 2011-01-20 | 2012-08-09 | Univ Of Tokyo | Movable body system |
| JP2013134615A (en) * | 2011-12-26 | 2013-07-08 | Toyota Central R&D Labs Inc | Route correction device |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2016198855A (en) * | 2015-04-10 | 2016-12-01 | セイコーエプソン株式会社 | Robot, control device, and control method |
| JP2019214084A (en) * | 2018-06-11 | 2019-12-19 | オムロン株式会社 | Route planning device, route planning method, and route planning program |
| US11207780B2 (en) | 2018-06-11 | 2021-12-28 | Omron Corporation | Path planning apparatus, path planning method, and path planning program |
| JP7077800B2 (en) | 2018-06-11 | 2022-05-31 | オムロン株式会社 | Route planning device, route planning method, and route planning program |
| WO2024084560A1 (en) * | 2022-10-18 | 2024-04-25 | ファナック株式会社 | Adjustment device |
Also Published As
| Publication number | Publication date |
|---|---|
| JP6131587B2 (en) | 2017-05-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| KR101585504B1 (en) | Method and apparatus for generating pathe of autonomous vehicle | |
| CN110361013B (en) | Path planning system and method for vehicle model | |
| CN110260867B (en) | Method, equipment and device for determining and correcting neutral position in robot navigation | |
| CN112577491A (en) | Robot path planning method based on improved artificial potential field method | |
| KR101679482B1 (en) | A multi robot system for avoding obstacle and a method using switching formation strategy for obstable avoidandce | |
| US8234032B2 (en) | Method and apparatus for generating safe path of mobile robot | |
| CN110887484A (en) | Mobile robot path planning method based on improved genetic algorithm and storage medium | |
| WO2022000961A1 (en) | Edgewise path selection method for robot obstacle crossing, chip, and robot | |
| CN109491389A (en) | A kind of robot trace tracking method with constraint of velocity | |
| CN103324196A (en) | Multi-robot path planning and coordination collision prevention method based on fuzzy logic | |
| WO2022000960A1 (en) | Obstacle-crossing termination determination method, obstacle-crossing control method, chip, and robot | |
| EP3316065B1 (en) | Method for determining a path | |
| CN111679678A (en) | Track planning method and system for transverse and longitudinal separation and computer equipment | |
| JP6131587B2 (en) | Route correction device | |
| CN113467476B (en) | Collision-free detection rapid random tree global path planning method considering corner constraint | |
| US20220203534A1 (en) | Path planning method and biped robot using the same | |
| JP5993784B2 (en) | Route correction device | |
| CN113433936A (en) | Mobile equipment obstacle-avoiding method and device, mobile equipment and storage medium | |
| JP5986533B2 (en) | Route correction device | |
| JP5923973B2 (en) | Route correction device | |
| CN114355923B (en) | MPC-based track planning and tracking method under A-guidance | |
| JP2007328486A (en) | Moving robot | |
| JP2007257200A (en) | Mobile body and control method for it | |
| JP5446765B2 (en) | Route search system, route search method, route search program, and moving body | |
| CN113341999A (en) | Forklift path planning method and device based on optimized D-x algorithm |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A621 | Written request for application examination |
Free format text: JAPANESE INTERMEDIATE CODE: A621 Effective date: 20151023 |
|
| A977 | Report on retrieval |
Free format text: JAPANESE INTERMEDIATE CODE: A971007 Effective date: 20160831 |
|
| A131 | Notification of reasons for refusal |
Free format text: JAPANESE INTERMEDIATE CODE: A131 Effective date: 20160906 |
|
| A521 | Request for written amendment filed |
Free format text: JAPANESE INTERMEDIATE CODE: A523 Effective date: 20161020 |
|
| 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: 20170321 |
|
| A61 | First payment of annual fees (during grant procedure) |
Free format text: JAPANESE INTERMEDIATE CODE: A61 Effective date: 20170403 |
|
| R150 | Certificate of patent or registration of utility model |
Ref document number: 6131587 Country of ref document: JP Free format text: JAPANESE INTERMEDIATE CODE: R150 |
|
| LAPS | Cancellation because of no payment of annual fees |