JP7027206B2 - 軌道生成装置 - Google Patents

Description

本明細書に開示の技術は、軌道生成装置に関する。

無人フォークリフトなど、移動体を自走させる技術が開発されている。移動体を自走させるには、軌道を生成する必要である。例えば特許文献１には、軌道データを生成する技術が開示されている。

特開平３－２５２７０７号公報

移動体を自走させる場合には、一定の制御周期（例：１０ミリ秒毎）で軌道データを生成し、走行状態を微調整する必要がある。ここで例えば、軌道データの生成にクロソイド曲線を用いる場合には、クロソイド点の座標は初等関数で表すことは不可能であるため、数値積分が必要となる。すると制御周期内で軌道データを生成することが困難な場合がある。本明細書は、簡易に軌道データを生成することが可能な軌道生成装置を開示する。

本明細書に開示する軌道生成装置の一実施形態は、移動体が移動する軌道を生成する軌道生成装置である。移動体のスタート位置、スタート位置における移動体の進行方向であるスタート位置進行方向、移動体のゴール位置、ゴール位置における移動体の進行方向であるゴール位置進行方向、に基づいて、スタート位置とゴール位置とを接続する第１の二等辺三角形および第２の二等辺三角形を生成することで、移動体がスタート位置からゴール位置まで移動する際の操舵角および操舵角速度を計画する計画部を、軌道生成装置は備える。第１の二等辺三角形の等辺に沿って第１の曲線軌道を当てはめるとともに、第２の二等辺三角形の等辺に沿って第２の曲線軌道を当てはめる曲線軌道あてはめ部を、軌道生成装置は備える。計画部は、第１の二等辺三角形の等辺と第２の二等辺三角形の等辺の長さを等しくする。計画部は、スタート位置に第１の二等辺三角形の底辺側の第１頂点を配置する。計画部は、ゴール位置に第２の二等辺三角形の底辺側の第１頂点を配置する。計画部は、第１の二等辺三角形の底辺側の第２頂点と第２の二等辺三角形の底辺側の第２頂点とを接続する。

スタート位置とゴール位置とを接続する第１の二等辺三角形および第２の二等辺三角形によって、操舵角および操舵角速度を計画ことができる。そして、第１および第２の二等辺三角形の等辺に沿って曲線軌道を当てはめることで、第１および第２の曲線軌道を生成することができる。曲線軌道を生成するに際し、数値積分などの複雑な計算を用いる必要がない。制御周期内に曲線軌道を生成することが可能となる。

計画部は、第１の二等辺三角形と第２の二等辺三角形との接続部において第１および第２の二等辺三角形の等辺が一直線になる条件下で、等辺の長さを算出してもよい。計画部は、スタート位置を通りスタート位置進行方向に伸びる第１延長線上の位置であって、スタート位置から等辺の長さの距離を進んだ位置に、第１の二等辺三角形の等辺に挟まれた第３頂点を決定してもよい。計画部は、ゴール位置を通りゴール位置進行方向の反対方向に伸びる第２延長線上の位置であって、ゴール位置から等辺の長さの距離を進んだ位置に、第２の二等辺三角形の等辺に挟まれた第３頂点を決定してもよい。計画部は、第１の二等辺三角形の第３頂点と第２の二等辺三角形の第３頂点との中点を、第１の二等辺三角形の第２頂点および第２の二等辺三角形の第２頂点として決定してもよい。効果の詳細は実施例で説明する。

計画部は、第１の二等辺三角形の底辺に対して第１の二等辺三角形のスタート位置から伸びる等辺がなす角度である第１角度の正負を判断してもよい。計画部は、第２の二等辺三角形の底辺に対して第２の二等辺三角形の第２頂点から伸びる等辺がなす角度である第２角度の正負を判断してもよい。計画部は、第１角度と第２角度との両方が正または両方が負である場合に、ゴール位置と第２の二等辺三角形の第３頂点とを結んだ第３延長線と第１延長線との交点を決定してもよい。計画部は、交点とスタート位置との第１距離が交点とゴール位置との第２距離よりも小さい場合に、第３延長線上の位置であって交点を始点としてゴール位置へ向かって第１距離を進んだ位置を、第１の二等辺三角形の第２頂点および第２の二等辺三角形の第２頂点として再決定してもよい。計画部は、第１距離が第２距離よりも大きい場合に、第１延長線上の位置であって交点を始点としてスタート位置へ向かって第２距離を進んだ位置を、第１の二等辺三角形の第２頂点および第２の二等辺三角形の第２頂点として再決定してもよい。効果の詳細は実施例で説明する。

曲線軌道あてはめ部は、パラメータを用いて曲線軌道を示すパラメトリック方程式を微分することで、第１の二等辺三角形の底辺の垂直二等分線と曲線軌道との交点における曲線軌道の傾きが、第１の二等辺三角形の底辺の傾きと等しくなるような特定パラメータの値を決定してもよい。曲線軌道あてはめ部は、特定パラメータによって得られた軌道を、第１の二等辺三角形の底辺の垂直二等分線に対して線対称に組み合わせることで、サイズが正規化された曲線軌道を生成してもよい。曲線軌道あてはめ部は、サイズが正規化された曲線軌道の両端部が第１の二等辺三角形の底辺側の第１頂点および第２頂点と一致するように、サイズが正規化された曲線軌道を拡大または縮小することで、第１の曲線軌道を第１の二等辺三角形の等辺にそって当てはめてもよい。効果の詳細は実施例で説明する。

パラメトリック方程式は、パラメータの５次式であってもよい。パラメトリック方程式を一階微分した式には、２乗または４乗された形でのみパラメータが含まれていてもよい。

実施例に係るフォークリフトの構成を示すブロック図。 スタート位置およびゴール位置を示す図。 曲線軌道の生成方法を示すフローチャート。 Ｓ３０の処理内容を説明するフローチャート。 第１および第２の二等辺三角形の形成例を示す図。 第１および第２の二等辺三角形の形成例を示す図。 Ｓ４０の処理内容を説明するフローチャート。 第１および第２の二等辺三角形の形成例を示す図。 第１および第２の二等辺三角形の形成例を示す図。 第１および第２の二等辺三角形の形成例を示す図。 Ｓ８０の処理内容を説明するフローチャート。 曲線軌道の当てはめ例を示す図。 曲線形状の形成例を示す図。 曲線軌道の形成例を示す図。 軌道上での位置姿勢を計算する方法を示すフローチャート。

（フォークリフト１０の構成）
以下、図面を参照して、本実施例のフォークリフト１０について説明する。図１に示すフォークリフト１０は、無人フォークリフトである。フォークリフト１０は、車体１２、測域センサ２６、演算装置３０を備えている。

車体１２は、前輪２８及び後輪２９を備えている。左右の前輪２８には、エンコーダ９２が接続されており、前輪２８の回転量を検出可能となっている。エンコーダ９２は、左右の前輪２８の動作に基づいて、車体１２の移動量及び移動方向を検出する。前輪２８は、図示しない駆動機構を介して駆動輪モータ９０が接続されており、駆動輪モータ９０によって回転駆動されるようになっている。

測域センサ２６は、車体１２に設置され、一方向（本実施例では、水平方向）にレーザ光を走査する１次元走査型の測域センサである。測域センサ２６は、車体１２の前方に設定された設定空間内の物体と車体１２との距離を計測する。これにより、フォークリフト１０の前方の距離データを取得する。測域センサ２６で取得された距離データは、演算装置３０の自己位置推定部３８に入力される。

制御装置３２は、ＣＰＵ等を備えたマイクロプロセッサによって構成されている。制御装置３２は、車体１２に搭載されている。図１に示すように、制御装置３２は、駆動輪モータ９０、操舵装置９４等と通信可能に接続されており、これらの動作を制御する。すなわち、制御装置３２は、駆動輪モータ９０及び操舵装置９４を駆動することで、フォークリフト１０の進行方向及び走行速度を制御する。

演算装置３０は、ＣＰＵ等を備えたマイクロプロセッサによって構成されている。演算装置３０は、スタート位置／ゴール位置設定部３５、軌道生成装置４０、地図データ５８、自己位置推定部３８、軌道追従制御部４８、を備える。地図データ５８は、フォークリフト１０が移動する空間内に存在する物体（例えば、柱、壁等）の位置及び大きさを示すデータである。地図データ５８は、例えば、フォークリフト１０が移動する領域内の物体を測域センサ２６によって予め測定し、この測定結果に基づいて作成しておくことができる。

自己位置推定部３８には、地図データ５８と、測域センサ２６により計測される周辺物体との距離データと、エンコーダ９２により検出された車体１２の移動量及び移動方向が入力される。自己位置推定部３８は、これらの入力データに基づいて、フォークリフト１０の自己位置を推定する。スタート位置／ゴール位置設定部３５は、フォークリフト１０のスタート位置とゴール位置を設定する。

軌道生成装置４０は、操舵計画部４１、曲線軌道生成部４２、曲線軌道当てはめ部４３を備えている。操舵計画部４１は、スタート位置とゴール位置とを接続する第１の二等辺三角形および第２の二等辺三角形を生成することで、フォークリフト１０がスタート位置からゴール位置まで移動する際の操舵方向を計画する。曲線軌道生成部４２は、サイズが正規化された曲線軌道を生成する。曲線軌道当てはめ部４３は、第１および第２の二等辺三角形に曲線軌道を当てはめることで、曲線軌道データを生成する。これにより、操舵角および操舵角速度が求まる。

軌道追従制御部４８は、曲線軌道当てはめ部４３で生成された曲線軌道データと、フォークリフト１０の現在位置（オドメトリにより算出される自己位置）とに基づいて、ゴール位置まで車体１２を移動するための制御指令値を算出する。生成された制御指令値は、制御装置３２に入力され、制御装置３２は、車体１２をゴール位置に向かって移動させる。

（前提条件）
本明細書における、記載方法の前提条件を説明する。フォークリフト１０の位置と向きを合わせて、「ポーズ」と呼ぶ場合がある。車輛の中心点(代表点または原点ともいう)は、操舵しない輪の左右の中心とする。本実施例のフォークリフト１０の場合、前輪２８の左右の中心である。この中心点は、タイヤの滑りが無視できる程度に小さいと仮定する。これにより、中心点は車輛の左右には動かず前後だけに動くため、代表点として都合が良い。一般性を失わないため、図２に示すように、スタート位置Ｏを地面座標系の原点とする。また、スタート位置におけるフォークリフト１０の向きであるスタート向きＡ１の方向を、地面座標系のｘ軸方向とする。スタート向きＡ１は、スタート位置Ｏにおけるフォークリフト１０の進行方向である。本実施例では、スタート位置姿勢(ポーズ)から、任意のゴール向きＡ２を有する任意のゴール位置Ｐまでの軌道の生成を考える。ここでゴール向きＡ２は、ゴール位置Ｐに到達した時点におけるフォークリフト１０の進行方向である。また本実施例では、ゴール位置Ｐのｘ座標が正(ｘ＞０)の場合のみ考えることとする。必ずしもゴールでｘ＞０でなくても全く同じプログラムで妥当な軌道が生成出来るケースも存在するが、ｘ＜０の場合はバックする方が妥当なケースが多いためである。

（最初の曲線軌道の生成方法）
図３のフローチャートを用いて、最初の曲線軌道の生成方法を説明する。図３は、ゴール位置の指示が入力されたときに、曲線軌道を生成する場合のフローである。

Ｓ１０において、ゴール位置Ｐのポーズが設定される。Ｓ１０は、スタート位置／ゴール位置設定部３５によって実行される処理である。図２の例に示すように、ゴール位置Ｐおよびゴール向きＡ２が設定される。

Ｓ２０において、ゴール向きＡ２が、スタート向きＡ１と略等しいか否かが判断される。すなわち、ゴール向きＡ２とスタート向きＡ１とが略平行であるか否かが判断される。ここで「略平行」とは、例えば、ゴール向きＡ２とスタート向きＡ１との差が３°以下である場合である。ゴール向きＡ２がスタート向きＡ１と略等しくない場合（Ｓ２０：ＮＯ）には、Ｓ３０へ進む。

Ｓ３０において、フォークリフト１０がスタート位置Ｏからゴール位置Ｐまで移動する際の操舵角および操舵角速度を計画する。Ｓ３０は、操舵計画部４１によって実行される処理である。具体的には、スタート位置Ｏとゴール位置Ｐとを接続する第１の二等辺三角形および第２の二等辺三角形を生成する。

図４のフローチャートを用いて、Ｓ３０の処理内容を説明する。Ｓ１１０において、第１および第２の二等辺三角形の等辺の長さＬを計算する。第１および第２の二等辺三角形の等辺の長さＬは、等しい。これは、解きやすさと解の妥当性を考慮したものである。等辺の長さＬを変数とおく。また図５に示すように、第１の二等辺三角形ＩＴ１と第２の二等辺三角形ＩＴ２との接続部において等辺ＥＳ１２と等辺ＥＳ２１が一直線になる条件を定める。この条件下で方程式を立てると２次方程式となるため、その２次方程式を解く。これにより、等辺の長さＬが求められる。

Ｓ１２０において、第１および第２の二等辺三角形を配置する。図５を用いて具体例を説明する。スタート位置Ｏに、第１の二等辺三角形ＩＴ１の底辺側の第１頂点ＶＥ１１を配置する。ゴール位置Ｐに、第２の二等辺三角形ＩＴ２の底辺側の第１頂点ＶＥ２１を配置する。スタート位置Ｏを通りスタート向きＡ１方向に伸びる延長線であるｘ軸上の位置であって、スタート位置Ｏから等辺の長さＬの距離を進んだ位置に、第１の二等辺三角形ＩＴ１の等辺に挟まれた第３頂点ＶＥ１３（Ｓ点ともいう）を決定する。ゴール位置Ｐを通りゴール向きＡ２の反対方向に伸びる延長線上の位置であって、ゴール位置Ｐから等辺の長さＬの距離を進んだ位置に、第２の二等辺三角形ＩＴ２の等辺に挟まれた第３頂点ＶＥ２３（Ｑ点ともいう）を決定する。

Ｓ１３０において、第１の二等辺三角形ＩＴ１の第３頂点ＶＥ１３（Ｓ点）と、第２の二等辺三角形ＩＴ２の第３頂点ＶＥ２３（Ｑ点）との中点であるＲ点を求める。そしてＲ点を、第１の二等辺三角形ＩＴ１の第２頂点ＶＥ１２および第２の二等辺三角形ＩＴ２の第２頂点ＶＥ２２として決定する。換言すると、第１の二等辺三角形および第２の二等辺三角形を生成するとは、Ｒ点の位置、および、Ｒ点を通る等辺ＥＳ１２およびＥＳ２１の傾きを求めることでもある。

Ｓ１４０において、第１の二等辺三角形ＩＴ１の等辺および第２の二等辺三角形ＩＴ２の等辺に曲線軌道を当てはめた場合に、その曲線軌道の最大曲率が、予め定められた閾値より大きいか否かを判断する。当てはめる曲線のタイプが決まっていれば、二等辺三角形の頂角とサイズから最大曲率を一意に求めることができる。最大曲率が閾値より大きい場合（Ｓ１４０：ＹＥＳ）には、カーブが急峻過ぎると判断され、Ｓ１５０へ進み、エラーとされる。一方、最大曲率が閾値以下の場合（Ｓ１４０：ＮＯ）には、第１の二等辺三角形および第２の二等辺三角形の生成が完了したと判断され、図３のＳ４０へ進む。

最大曲率の閾値は、予め定められた最小角度（例：６０度から９０度の範囲）よりも小さい頂角に当てはめられた曲線軌道を、エラーとして除外するように定めてもよい。これにより、例えば図６の例のように、第２の二等辺三角形ＩＴ２の第３頂点ＶＥ２３の頂角が９０°よりも小さい場合に、曲線軌道の生成を中止することができる。

図３のフローに説明を戻す。Ｓ４０において、一回操舵への変更を試行する。図７のフローチャートおよび図５を用いて、Ｓ４０の処理内容を説明する。

Ｓ２１０において、第１の二等辺三角形によって計画される１回目の操舵方向と、第２の二等辺三角形によって計画される２回目の操舵方向が同方向であるか否かを判断する。具体的には、第１の二等辺三角形ＩＴ１において、底辺（線ＯＲ）に対して、スタート位置Ｏから伸びる等辺ＥＳ１１（線ＯＳ）がなす底角である第１角度ＡＧ１の正負を判断する。また、第２の二等辺三角形ＩＴ２において、底辺（線ＲＰ）に対して、第２頂点ＶＥ２２（Ｒ点）から伸びる等辺ＥＳ２１（線ＲＱ）がなす底角である第２角度ＡＧ２の正負を判断する。

第１角度ＡＧ１と第２角度ＡＧ２の両方が正、または、両方が負である場合には、１回目の操舵方向と２回目の操舵方向が同方向であると判断される（Ｓ２１０：ＹＥＳ）。例えば、図８に示すように、第１角度ＡＧ１および第２角度ＡＧ２の両方が正である場合である。この場合、１回操舵へ変更するためにＳ２２０へ進む。

Ｓ２２０において、図８に示すように、ゴール位置Ｐと第２の二等辺三角形ＩＴ２の第３頂点ＶＥ２３（Ｑ点）とを結んだ線ＱＰの延長線ＥＸ３を生成する。そして延長線ＥＸ３とｘ軸との交点Ｃを決定する。

Ｓ２３０において、交点Ｃの位置が適切であるか否かが判断される。具体的には、交点Ｃが、ｘ＞０かつゴール位置Ｐのゴール向きＡ２のベクトルの後ろ側にあるか（すなわち線ＣＰのベクトルとゴール向きＡ２のベクトルの内積が負であるか）が判断される。同方向に二回操舵していることから、通常は、交点Ｃの位置は必ず適切になる。Ｓ２３０において否定判断される場合（Ｓ２３０：ＮＯ）には、エラー判断され、フローが終了する。一方、Ｓ２３０において肯定判断される場合（Ｓ２３０：ＹＥＳ）には、交点Ｃを中心に新たな二等辺三角形を作成するために、Ｓ２４０へ進む。

Ｓ２４０において、交点Ｃが、スタート位置Ｏとゴール位置Ｐの何れに近いかを判断する。具体的には、スタート位置Ｏと交点Ｃとの第１距離Ｄ１と、交点Ｃとゴール位置Ｐとの第２距離Ｄ２とを比較する。

第１距離Ｄ１の方が第２距離Ｄ２よりも小さい場合には、交点Ｃがスタート位置Ｏに近い場合であると判断され、Ｓ２５０へ進む。Ｓ２５０において、延長線ＥＸ３上の位置であって交点Ｃを始点としてゴール位置Ｐへ向かって第１距離Ｄ１を進んだ位置を、第１の二等辺三角形ＩＴ１の第２頂点ＶＥ１２（Ｒ点）および第２の二等辺三角形ＩＴ２の第２頂点ＶＥ２２（Ｒ点）として再決定する。これにより、図９の例に示すように、新たな第１の二等辺三角形ＩＴ１および第２の二等辺三角形ＩＴ２が形成される。そしてＳ２８０へ進む。

図９において、第２の二等辺三角形ＩＴ２は一直線に潰れている。すなわち、Ｒ点とゴール位置Ｐとを直線で結ぶことができる。これにより、第１の二等辺三角形ＩＴ１の１つだけで操舵計画を立てるように修正することができる。従って、１つの二等辺三角形に後述する方法で曲線をはめ込むことで、曲線軌道を完成させることが可能となる。曲率がほぼ一定に変化する曲線長が長くとれるため、全体として曲率の変化を小さくすることができる。従って、同じ線速度で移動するとした場合の操舵速度の最大値を低く抑えることが可能となる。

また、第２距離Ｄ２の方が第１距離Ｄ１よりも小さい場合には、交点Ｃがゴール位置Ｐに近い場合であると判断され、Ｓ２６０へ進む。Ｓ２６０において、ｘ軸上の位置であって交点Ｃを始点としてスタート位置Ｏへ向かって第２距離Ｄ２を進んだ位置を、第１の二等辺三角形ＩＴ１の第２頂点ＶＥ１２（Ｒ点）および第２の二等辺三角形ＩＴ２の第２頂点ＶＥ２２（Ｒ点）として再決定する。これにより、新たな第１の二等辺三角形ＩＴ１および第２の二等辺三角形ＩＴ２が形成される。第１の二等辺三角形ＩＴ１は一直線に潰れている。これにより、第２の二等辺三角形ＩＴ２の１つだけで操舵計画を立てるように、計画を修正することができる。

一方、Ｓ２１０において、第１角度ＡＧ１と第２角度ＡＧ２の一方が正であり他方が負である場合には、１回目の操舵方向と２回目の操舵方向が異なる方向であると判断される（Ｓ２１０：ＮＯ）。この場合、２回操舵を１回操舵へ変更することは行わない。従ってＳ２７０へ進む。図５に示す例では、第１角度ＡＧ１が負であり第２角度ＡＧ２が正であるため、Ｓ２１０で否定判断される。

Ｓ２７０において、第１および第２の二等辺三角形の頂角の比率に応じて、二等辺三角形の等辺の長さＬを第１および第２の二等辺三角形で定数倍に変化させる。具体的には、第１および第２の二等辺三角形のうち、頂角が小さい方（すなわちカーブが急である方）の二等辺三角形の等辺の長さを長くし、頂角が大きい方（すなわちカーブが緩やかである方）の二等辺三角形の等辺の長さを短くする。例えば図６に示すように、第２の二等辺三角形ＩＴ２の頂角が小さい場合には、図１０に示すように、第２の二等辺三角形ＩＴ２の等辺の長さをＬからＬ２へ長くするとともに、第１の二等辺三角形ＩＴ１の等辺の長さをＬからＬ１へ短くする。これにより、第１の二等辺三角形ＩＴ１を小さく、第２の二等辺三角形ＩＴ２を大きくすることができる。よって、第２の二等辺三角形ＩＴ２の面積を大きくすることができるため、第２の二等辺三角形ＩＴ２で計画される曲線軌道の最大曲率を小さくする（すなわちカーブを緩やかにする）ことが可能となる。

Ｓ２８０において、Ｓ３０で作成した二等辺三角形と、Ｓ２５０、Ｓ２６０、Ｓ２７０で再作成した二等辺三角形のうち、最大曲率が小さい方の二等辺三角形を選択する。そして図３のＳ８０へ進む。

図３のフローに説明を戻す。Ｓ２０において、ゴール向きＡ２が、スタート向きＡ１と略等しい場合（Ｓ２０：ＹＥＳ）にはＳ５０へ進む。Ｓ５０において、ゴール位置Ｐのｘ座標値が０に近いか否かを判断する。肯定判断された場合（Ｓ５０：ＹＥＳ）には、スタート向きＡ１とゴール向きＡ２とが略等しく、かつ、スタート位置Ｏとゴール位置Ｐとが真横に位置する場合である。このようなケースでは、大回りする曲線軌道を用いてゴールに至るよりも、途中で切り返すことが適切であると考えられる。そのため、Ｓ６０へ進み、エラー(軌道生成不可能)とする。

一方、Ｓ５０において否定判断された場合（Ｓ５０：ＮＯ）は、Ｓ７０へ進む。この場合は、ゴール向きＡ２がスタート向きＡ１と略等しく、スタート位置Ｏとゴール位置Ｐとがある程度離れている場合である。このような場合、Ｓ１１０で説明した２次方程式の二つの解の和と差が無限大に発散するため、計算が困難となる。すなわち、この条件を満たす点は、二次方程式の２個の解ではなく１個の解となる。そこで、Ｓ７０へ進み、二次方程式の解の公式ではなく、級数展開で解を求める。級数展開は、周知の方法を用いればよいため、ここでは詳細な説明を省略する。これにより、ゴール向きＡ２がスタート向きＡ１と略等しい場合においても、安定して連続的な解を求めることが出来る。

Ｓ８０において、第１および第２の二等辺三角形に曲線軌道を当てはめる。具体的には、第１の二等辺三角形ＩＴ１の等辺に沿った第１の曲線軌道、および、第２の二等辺三角形ＩＴ２の等辺に沿った第２の曲線軌道を生成する。

図１１のフローチャートを用いて、Ｓ８０の処理内容を説明する。一般化のため、第１の二等辺三角形ＩＴ１に対する曲線軌道の当てはめについてのみ説明する。第２の二等辺三角形ＩＴ２に対する曲線軌道の当てはめは、原点移動と回転移動することでＲ点を原点にすれば、第１の二等辺三角形ＩＴ１と全く同じ方法で行うことが出来るためである。

ここで、曲線軌道の当てはめには、以下の法則が成立する。（法則１）二等辺三角形に対称に曲線を当てはめる。（法則２）スタート位置Ｏで、曲率および傾きともにゼロである。（法則３）傾きが連続であると言う条件から、図１２に示すように、二等辺三角形の底辺ＥＳ１３の垂直二等分線ＰＢ１と曲線軌道との交点ＩＳ１における曲線軌道の傾きは、二等辺三角形の底辺ＥＳ１３（直線ＯＲ）の傾きと等しい。

曲線軌道の当てはめには、曲線長と曲率の関係が線形に近い曲線を生成するためのパラメトリック曲線を使用する。パラメトリック曲線は、曲率傾きが０から始まって、漸増していく曲線であれば、いずれの曲線であってもよい。本実施例では下式（１）（２）で表されるパラメトリック曲線を用いる場合を説明する。
ｘ＝ｍ×（ｔ－ｔ^５／４０） ・・・式（１）
ｙ＝ｍ×ｔ^３／６ ・・・式（２）
ここで、倍率ｍは相似変換のスケーリングの定数であり、ｔはパラメータである。本実施例では、ｍ＝１の場合(すなわち正規化されている場合）を説明する。

なお、実際の計算では、下式（３）に示すようなパラメータｕを用いて計算してもよい。
ｔ＝ｕ／ｍ ・・・式（３）
式（３）を式（１）および（２）に代入すると、下式（４）および（５）が得られる。
ｘ＝ｕ－ｕ^５／（４０ｍ^４） ・・・式（４）
ｙ＝ｕ^３／（６ｍ^２） ・・・式（５）
式（４）および（５）においてｍは明示的には計算せず、「１／ｍ^２」を直接計算すればよい。式（４）および（５）ではｍの２乗および４乗しか出てこないため、「１／ｍ」を計算する必要がないためである。

式（４）および（５）を用いて計算を行う効果を説明する。二等辺三角形がつぶれて直線になる場合、底辺の傾きがゼロになり、ｍは無限大に発散し、ｔの範囲が０から０となるため、式（１）および（２）では計算が不可能になる。しかし、式（４）および（５）では、ｍとｔを明示的に使用していない。式（４）および（５）では、「１／ｍ^２」の計算を用いるとともに、「ｕ」の範囲は０からｕの最大値で計算すればよい。これにより、二等辺三角形が完全につぶれて直線になる場合も含めて、安定して計算をすることができる。実用上は、二等辺三角形がつぶれるケースが多いため、式（４）および（５）を用いた計算方法は有益である。また、パラメータｕは、二等辺三角形にフィットさせた後の曲線長にほぼ一致する。従って、真の曲線長を計算せずに、パラメータｕを擬似曲線長として用いて速度パターンを作成できる。数値積分を不要にすることができるため、速度パターンの設計を簡易に行うことが可能になる。

Ｓ３１０において、サイズを正規化した曲線形状を生成する。具体的に説明する。式（１）および（２）の曲線では、傾きを与えた時、対応するパラメータｔは一意である。よって、二次方程式を解くことでパラメータｔを求めることができる。すなわち、式（１）および（２）を数式的に微分することで、第１の二等辺三角形ＩＴ１の底辺ＥＳ１３の垂直二等分線ＰＢ１（図１２参照）と曲線軌道との交点ＩＳ１における曲線軌道の傾きが、第１の二等辺三角形ＩＴ１の底辺ＥＳ１３の傾きと等しくなるようなパラメータｔの値を決定する。決定したパラメータｔの値を用いて、式（１）および（２）の相似変換の倍率ｍを「１」とした、サイズを正規化した曲線形状が得られる。図１２の第１の二等辺三角形ＩＴ１の例では、図１３に示すような曲線形状ＣＳ１が得られる。

Ｓ３２０において、正規化された曲線形状を、第１の二等辺三角形ＩＴ１の底辺ＥＳ１３の垂直二等分線ＰＢ１に対して線対称に組み合わせる。これにより、サイズが正規化された曲線軌道ＣＴ１が生成される。図１２の第１の二等辺三角形ＩＴ１の例では、図１４に示すように、曲線形状ＣＳ１およびＣＳ２を組み合わせた曲線軌道ＣＴ１が得られる。曲線形状ＣＳ１およびＣＳ２の接続部分ＣＮの傾きＧＲ１（図１４）は、対称性と微分可能の条件より、図１２の第１の二等辺三角形ＩＴ１の底辺ＥＳ１３の傾きと等しい。

Ｓ３３０において、サイズが正規化された曲線軌道ＣＴ１の両端部が、第１の二等辺三角形ＩＴ１の底辺側の第１頂点ＶＥ１１（スタート位置Ｏ）、および第２頂点ＶＥ１２（Ｒ点）と一致するように、曲線軌道ＣＴ１を拡大または縮小する。具体的には、図１４の曲線軌道ＣＴ１の一方の端部Ｅ１を、図１２のスタート位置Ｏに一致させる。このとき、曲線軌道ＣＴ１の他方の端部Ｅ２が到達する位置と、図１２のＲ点との比率を求める。そして、上記で求めた比率に基づいて、曲線軌道ＣＴ１を拡大または縮小する。これにより、図１２に示すように、第１の二等辺三角形ＩＴ１に対する曲線軌道ＣＴ１の当てはめが完了する。

第２の二等辺三角形ＩＴ２についても、上述したＳ３１０～Ｓ３３０の処理を行うことにより、曲線軌道ＣＴ２を当てはめることができる。これにより、様々なスタート位置Ｏおよびゴール位置Ｐの組み合わせに対し、自然な曲線軌道を生成することができる。

Ｓ３４０において、二等辺三角形に曲線を当てはめる場合の係数を保存する。具体的には、Ｓ３３０で求めた拡縮比率と、Ｓ３１０で取得したパラメータｔの値を保存する。なお、拡縮比率の逆数を二乗した値を、拡縮比率として用いてもよい。そしてフローを終了する。

なお、Ｓ３１０およびＳ３２０は、曲線軌道生成部４２によって実行される処理である。Ｓ３３０は、曲線軌道当てはめ部４３によって実行される処理である。

（軌道上での位置姿勢の計算方法）
図１５のフローチャートを用いて、フォークリフト１０の移動中において、軌道上での位置姿勢を計算する方法を説明する。図１５のフローチャートは、例えば、一定の制御周期（例：１０ミリ秒毎）で実行される。図１５のフローチャートの処理は、軌道追従制御部４８によって実行される処理である。

Ｓ４１０において、第１および第２の二等辺三角形の４つの等辺ＥＳ１１、ＥＳ１２、ＥＳ２１、ＥＳ２２の何れの辺に対応する曲線を計算すべきかを判断する。具体的に説明する。４つの等辺に対応する、パラメータｔの最大値(それぞれで０から最大値まで変化する)が求められている。よって、与えられたパラメータｔの値と大小比較することで、何れの等辺に対応する曲線を計算すべきかが容易に判断できる。

Ｓ４２０において、パラメータｔに対応する位置(ｘ，ｙ)、傾き、曲率を計算する。具体的に説明する。パラメータｔの「倍率ｍ」分の１を、曲線軌道の正規化された式に代入することで、位置(ｘ，ｙ)が求まる。曲線軌道を示す式の微分値から、位置(ｘ，ｙ)での傾きを求めることができる。また二階微分によって、位置(ｘ，ｙ)での曲率κを求めることができる。

なお、フォークリフト１０の操舵角φは、下式（６）に示すように、曲線軌道の曲率κと一対一に対応している。
λ(cosφ, sinφ, 0) = (v, 0, 0) + (0,0,κv)×(bx, by, 0) ・・・式（６）
ここで、vは車輛の中心の前進速度である。bxは、操舵輪の位置(ホイールベース）である。byは、車輛の中心点であり、トレッドの半分である。λは適当な定数である。従って、下式（７）によって、操舵角φが求まる。
φ = tan -1 (bx κ / (1 - by κ)) ・・・式（７）

（効果）
ガイドレスで無人走行するフォークリフトを実現するには、仮想的に走行ラインを引くこと、すなわち軌道生成が必要である。代表的な軌道生成の方法として、クロソイド曲線を用いた方法や、Ｂ－スプライン曲線を用いた方法が挙げられる。またフォークリフトの特徴として、操舵速度の上限が低いことが挙げられる。これはパワーステアリングであるため、ハンドルを高速に回せないためである。そして軌道生成にクロソイド曲線を用いる場合には、クロソイド点の座標は初等関数で表すことは不可能であるため、数値積分が必要となる。すると制御周期内で軌道データを生成することが困難な場合がある。また軌道生成にＢ－スプライン曲線を用いる場合には、曲線軌道の始点と終点近くでの曲率変化が大きくなってしまう傾向がある。そのため、曲率変化がフォークリフトの操舵速度の上限を越えてしまい、フォークリフトが曲線軌道を追従することが困難となる場合がある。

本実施形態の軌道生成装置４０は、スタート位置Ｏとゴール位置Ｐとを接続する第１の二等辺三角形および第２の二等辺三角形に曲線軌道を当てはめることで、軌道を生成する。これにより、第１の効果として、二次方程式の求解および座標変換という代数計算のみで軌道の設計が可能になる。クロソイド曲線などの数値積分が必要な曲線を用いる場合に比して、短時間で曲線軌道を計算できる。制御周期内で軌道データを生成することが可能になる。また、第２の効果として、曲線軌道の始点と終点近くでの曲率変化が小さい軌道を生成することができる。すなわち、スタート位置Ｏおよびゴール位置Ｐで、曲率・傾きともにゼロである曲線軌道を生成することができる。これは、図５に示すように、スタート向きＡ１方向に伸びる延長線（ｘ軸）と第１の二等辺三角形ＩＴ１の等辺ＥＳ１１とが重なるとともに、ゴール向きＡ２の反対方向に伸びる延長線上に第２の二等辺三角形ＩＴ２の等辺ＥＳ２２が重なるように、二等辺三角形を配置するためである。これにより、Ｂ－スプライン曲線で作成した軌道に比して、操舵速度の上限が低いフォークリフトに適した軌道を生成することができる。

本実施形態の軌道生成装置４０は、式（１）および（２）で表されるパラメトリック方程式を用いている。式（１）および（２）を用いる効果を説明する。Ｓ３１０では、曲線の傾きを元にしてパラメータｔの値を決定している。このとき、二次方程式を解くことでパラメータｔを求める。そして、式（１）の５次式および式（２）の３次式は、微分するとパラメータｔのｔ⁴とｔ²のみを含んだ式となるため、「ｔ²」を変数としてみることで２次方程式に容易に帰着することができる。すなわち式（１）および（２）を用いることで、代数計算が実用的に可能になる。なお、３次式のパラメトリック方程式は代数計算での計算が可能であるが、式（１）の５次式に比して利用できる範囲が狭く、直角に曲がる時に利用できない。よって式（１）の５次式は、適用範囲が広く、かつ計算が容易であるという特徴を備えた式であることが分かる。

以上、本明細書が開示する技術の実施例について詳細に説明したが、これらは例示に過ぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。

（変形例）
Ｓ６０において、エラーにする代わりに、進行速度を前後に切りかえる切り返し軌道を生成してもよい。すなわち、切り返し点を適当に与え、切り返し点まで本アルゴリズムで前進用の軌道を生成してもよい。さらに切り返し点からゴール位置Ｐまで、本アルゴリズムで後退用の軌道を生成してもよい。

本実施例では、測域センサ２６として１次元走査型の測域センサを用いたが、各種のセンサを用いてもよい。例えば測域センサ２６は、2D LiDAR、3D LiDAR、ステレオカメラ、単眼カメラ、距離画像センサなどでもよい。自己位置推定部３８は、衛星測位を用いる手段によって自己位置を推定してもよい。

式（１）および（２）は、パラメトリック曲線の一例である。本実施例の技術では、様々なタイプのパラメトリック曲線を使用することが可能である。

Ｓ２８０において、二等辺三角形の選択方法は様々な態様であってよい。例えば、曲率変化の最大値が小さい方の二等辺三角形を選択してもよい。

本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。

スタート向きＡ１は、スタート位置進行方向の一例である。ゴール向きＡ２は、ゴール位置進行方向の一例である。操舵計画部４１は、計画部の一例である。Ｓ点は、第１の二等辺三角形の等辺に挟まれた第３頂点の一例である。Ｑ点は、第２の二等辺三角形の等辺に挟まれた第３頂点の一例である。

１０：フォークリフト、４０：軌道生成装置、４１：操舵計画部、４３：曲線軌道当てはめ部、Ａ１：スタート向き、Ａ２：ゴール向き、ＩＴ１：第１の二等辺三角形、ＩＴ２：第２の二等辺三角形、Ｏ：スタート位置、Ｐ：ゴール位置

Claims (5)

1. 移動体が移動する軌道を生成する軌道生成装置であって、
   前記移動体のスタート位置、前記スタート位置における前記移動体の進行方向であるスタート位置進行方向、前記移動体のゴール位置、前記ゴール位置における前記移動体の進行方向であるゴール位置進行方向、に基づいて、前記スタート位置と前記ゴール位置とを接続する第１の二等辺三角形および第２の二等辺三角形を生成することで、前記移動体が前記スタート位置から前記ゴール位置まで移動する際の操舵角および操舵角速度を計画する計画部と、
   前記第１の二等辺三角形の等辺に沿って第１の曲線軌道を当てはめるとともに、前記第２の二等辺三角形の等辺に沿って第２の曲線軌道を当てはめる曲線軌道あてはめ部と、
   を備え、
   前記計画部は、
   前記第１の二等辺三角形の等辺と前記第２の二等辺三角形の等辺の長さを等しくし、
   前記スタート位置に前記第１の二等辺三角形の底辺側の第１頂点を配置し、
   前記ゴール位置に前記第２の二等辺三角形の底辺側の第１頂点を配置し、
   前記第１の二等辺三角形の底辺側の第２頂点と前記第２の二等辺三角形の底辺側の第２頂点とを接続する、軌道生成装置。
2. 前記計画部は、
   前記第１の二等辺三角形と前記第２の二等辺三角形との接続部において前記第１および第２の二等辺三角形の等辺が一直線になる条件下で、前記等辺の長さを算出し、
   前記スタート位置を通り前記スタート位置進行方向に伸びる第１延長線上の位置であって、前記スタート位置から前記等辺の長さの距離を進んだ位置に、前記第１の二等辺三角形の等辺に挟まれた第３頂点を決定し、
   前記ゴール位置を通り前記ゴール位置進行方向の反対方向に伸びる第２延長線上の位置であって、前記ゴール位置から前記等辺の長さの距離を進んだ位置に、前記第２の二等辺三角形の等辺に挟まれた第３頂点を決定し、
   前記第１の二等辺三角形の前記第３頂点と前記第２の二等辺三角形の前記第３頂点との中点を、前記第１の二等辺三角形の前記第２頂点および前記第２の二等辺三角形の前記第２頂点として決定する、
   請求項１に記載の軌道生成装置。
3. 前記計画部は、
   前記第１の二等辺三角形の底辺に対して前記第１の二等辺三角形の前記スタート位置から伸びる等辺がなす角度である第１角度の正負を判断し、
   前記第２の二等辺三角形の底辺に対して前記第２の二等辺三角形の前記第２頂点から伸びる等辺がなす角度である第２角度の正負を判断し、
   前記第１角度と前記第２角度との両方が正または両方が負である場合に、前記ゴール位置と前記第２の二等辺三角形の第３頂点とを結んだ第３延長線と前記第１延長線との交点を決定し、
   前記交点と前記スタート位置との第１距離が前記交点と前記ゴール位置との第２距離よりも小さい場合に、前記第３延長線上の位置であって前記交点を始点として前記ゴール位置へ向かって前記第１距離を進んだ位置を、前記第１の二等辺三角形の前記第２頂点および前記第２の二等辺三角形の前記第２頂点として再決定し、
   前記第１距離が前記第２距離よりも大きい場合に、前記第１延長線上の位置であって前記交点を始点として前記スタート位置へ向かって前記第２距離を進んだ位置を、前記第１の二等辺三角形の前記第２頂点および前記第２の二等辺三角形の前記第２頂点として再決定する、
   請求項２に記載の軌道生成装置。
4. 前記曲線軌道あてはめ部は、
   パラメータを用いて前記曲線軌道を示すパラメトリック方程式を微分することで、前記第１の二等辺三角形の底辺の垂直二等分線と曲線軌道との交点における曲線軌道の傾きが、第１の二等辺三角形の底辺の傾きと等しくなるような特定パラメータの値を決定し、
   前記特定パラメータによって得られた軌道を、前記第１の二等辺三角形の底辺の垂直二等分線に対して線対称に組み合わせることで、サイズが正規化された曲線軌道を生成し、
   前記サイズが正規化された曲線軌道の両端部が前記第１の二等辺三角形の底辺側の前記第１頂点および前記第２頂点と一致するように、前記サイズが正規化された曲線軌道を拡大または縮小することで、前記第１の曲線軌道を前記第１の二等辺三角形の等辺にそって当てはめる、
   請求項１～３の何れか１項に記載の軌道生成装置。
5. 前記パラメトリック方程式は、前記パラメータの５次式であり、
   前記パラメトリック方程式を一階微分した式には、２乗または４乗された形でのみ前記パラメータが含まれている、請求項４に記載の軌道生成装置。
   

Images