特許 7227849 軌道生成装置

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2023-02-14

(45)【発行日】2023-02-22

(54)【発明の名称】軌道生成装置

(51)【国際特許分類】

   G05D 1/02 20200101AFI20230215BHJP

【ＦＩ】

G05D1/02 H

【請求項の数】 8

(21)【出願番号】P 2019094477

(22)【出願日】2019-05-20

(65)【公開番号】P2020190814

(43)【公開日】2020-11-26

【審査請求日】2022-02-21

(73)【特許権者】

【識別番号】000003609

【氏名又は名称】株式会社豊田中央研究所

(73)【特許権者】

【識別番号】000003218

【氏名又は名称】株式会社豊田自動織機

(74)【代理人】

【識別番号】110000110

【氏名又は名称】弁理士法人 快友国際特許事務所

(72)【発明者】

【氏名】藤井 亮暢

(72)【発明者】

【氏名】田中 稔

(72)【発明者】

【氏名】津坂 祐司

(72)【発明者】

【氏名】一瀬 誠

【審査官】牧 初

(56)【参考文献】

【文献】特開２０１２－５３８３７（ＪＰ，Ａ）

【文献】特開２０１８－５７０９（ＪＰ，Ａ）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０５Ｄ １／００－ １／１２

Ｇ０８Ｇ １／００－９９／００

Ｂ６２Ｄ ４１／００－６７／００

Ｇ６５Ｇ ６３／００－６３／０６

Ｂ６４Ｆ １／００－ ５／６０

(57)【特許請求の範囲】

【請求項1】

移動体の、始点位置から搬送装置までの移動のための軌道を生成する軌道生成装置であって、
前記搬送装置は、第１方向に伸びる搬送経路部と、前記搬送経路部の端部に配置されている荷物出し入れ部と、を備えており、
前記第１方向と直交する方向であって水平方向に伸びる第２方向の正方向または負方向の何れに、前記移動体の前記始点位置が位置しているかを判定する判定部と、
移動体が備える環境センサの計測データに基づいて、前記搬送経路部の側面の位置および姿勢を示す計測データを取得する取得部と、
前記搬送装置の外形形状を示す外形データによって表される外形モデルであって予め用意された前記外形モデルと前記計測データとを用いて、前記移動体を基準とした前記搬送装置の位置および姿勢を推定する推定部と、
前記始点位置から前記荷物出し入れ部の前面まで移動するための目標軌道であって、前記荷物出し入れ部の前面において前記移動体の前面が向いている方向である移動体向きが前記第２方向と一致するような前記目標軌道を生成する軌道生成部と、
を備える、軌道生成装置。

【請求項2】

前記推定部は、前記外形モデルを前記計測データに重畳することで前記搬送装置の位置および姿勢を推定する、請求項１に記載の軌道生成装置。

【請求項3】

前記計測データは、前記移動体向きに対して前記搬送経路部の側面が有する所定角度を示すとともに、前記搬送経路部の基準位置を示すデータであり、
前記推定部は、
前記外形モデルの側面が前記移動体向きに対して有する角度が前記所定角度と一致するように前記外形モデルを回転させるとともに、
前記外形モデルの基準位置が前記計測データの基準位置と一致するように前記外形モデルを移動させることで、前記外形モデルを前記計測データに重畳する、
請求項２に記載の軌道生成装置。

【請求項4】

前記環境センサはレーザセンサであり、
前記計測データは前記レーザセンサで計測された点群データである、請求項１～３の何れか１項に記載の軌道生成装置。

【請求項5】

前記搬送装置は、
前記第２方向の正方向側の第１の側面に配置された第１のマーカと、
前記第２方向の負方向側の第２の側面に配置された第２のマーカと、
を備えており、
前記判定部は、
前記移動体が備えるカメラで撮像された画像内で検出されたマーカが前記第１のマーカである場合に、前記始点位置が前記第２方向の正方向に位置していると判定し、
前記画像内で検出されたマーカが前記第２のマーカである場合に、前記始点位置が前記第２方向の負方向に位置していると判定する、請求項１～４の何れか１項に記載の軌道生成装置。

【請求項6】

前記判定部は、前記移動体が備えるカメラで撮像した前記搬送装置の像が、第１テンプレート画像および第２テンプレート画像の何れにマッチングする度合いが高いかを判断し、
前記第１テンプレート画像は、前記第２方向の正方向側から前記搬送装置を予め撮像した画像であり、
前記第２テンプレート画像は、前記第２方向の負方向側から前記搬送装置を予め撮像した画像であり、
前記判定部は、
前記搬送装置の像が前記第１テンプレート画像にマッチングする度合いが高いと判断した場合に、前記始点位置が前記第２方向の正方向に位置していると判定し、
前記搬送装置の像が前記第２テンプレート画像にマッチングする度合いが高いと判断した場合に、前記始点位置が前記第２方向の負方向に位置していると判定する、
請求項１～４の何れか１項に記載の軌道生成装置。

【請求項7】

前記判定部は、
前記搬送装置から前記移動体に荷物を降ろす作業計画である場合に、前記始点位置が前記第２方向の正方向に位置していると判定し、
前記移動体から前記搬送装置へ荷物を積み込む作業計画である場合に、前記始点位置が前記第２方向の負方向に位置していると判定する、
請求項１～４の何れか１項に記載の軌道生成装置。

【請求項8】

請求項１～７の何れか１項に記載の軌道生成装置を備えた移動体。

【発明の詳細な説明】

【技術分野】

【0001】

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

【背景技術】

【0002】

荷物を自走により運搬する移動体（例：無人牽引車）が開発されている。荷物の出し入れ位置までの軌跡を生成するために、移動体を基準として、荷物の出し入れ位置を推定する必要がある。例えば特許文献１には、目的地に配置された標識をカメラを用いて認識する技術が開示されている。

【先行技術文献】

【特許文献】

【0003】

【文献】特開２００３－２８５２８８号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

カメラの認識精度は、レーザを用いる場合などに比して低い。よって、カメラによる標識認識では、目的地の位置・姿勢を高精度に推定することは困難である。

【課題を解決するための手段】

【0005】

本明細書に開示する軌道生成装置の一実施形態は、移動体の、始点位置から搬送装置までの移動のための軌道を生成する軌道生成装置である。搬送装置は、第１方向に伸びる搬送経路部と、搬送経路部の端部に配置されている荷物出し入れ部と、を備えている。軌道生成装置は、第１方向と直交する方向であって水平方向に伸びる第２方向の正方向または負方向の何れに、移動体の始点位置が位置しているかを判定する判定部を備える。軌道生成装置は、移動体が備える環境センサの計測データに基づいて、搬送経路部の側面の位置および姿勢を示す計測データを取得する取得部を備える。軌道生成装置は、搬送装置の外形形状を示す外形データによって表される外形モデルであって予め用意された外形モデルと計測データとを用いて、移動体を基準とした搬送装置の位置および姿勢を推定する推定部を備える。軌道生成装置は、始点位置から荷物出し入れ部の前面まで移動するための目標軌道であって、荷物出し入れ部の前面において移動体の前面が向いている方向である移動体向きが第２方向と一致するような目標軌道を生成する軌道生成部を備える。

【0006】

搬送装置は、第１方向に伸びる搬送経路部を備えている。また、移動体の始点位置が、第１方向と直交する第２方向の正方向または負方向に位置している。これにより取得部は、移動体が備える環境センサによって、搬送経路部の側面の位置および姿勢を計測することができる。そして取得した計測データと外形モデルとを用いて、搬送装置の位置および姿勢を推定することができる。カメラを用いる場合に比して、搬送装置の位置および姿勢を高精度に推定することが可能となる。

【0007】

推定部は、外形モデルを計測データに重畳することで搬送装置の位置および姿勢を推定してもよい。効果の詳細は実施例で説明する。

【0008】

計測データは、移動体向きに対して搬送経路部の側面が有する所定角度を示すとともに、搬送経路部の基準位置を示すデータであってもよい。推定部は、外形モデルの側面が移動体向きに対して有する角度が所定角度と一致するように外形モデルを回転させるとともに、外形モデルの基準位置が計測データの基準位置と一致するように外形モデルを移動させることで、外形モデルを計測データに重畳してもよい。効果の詳細は実施例で説明する。

【0009】

環境センサはレーザセンサであってもよい。計測データはレーザセンサで計測された点群データであってもよい。

【0010】

搬送装置は、第２方向の正方向側の第１の側面に配置された第１のマーカと、第２方向の負方向側の第２の側面に配置された第２のマーカと、を備えていてもよい。判定部は、移動体が備えるカメラで撮像された画像内で検出されたマーカが第１のマーカである場合に、始点位置が第２方向の正方向に位置していると判定してもよい。判定部は、画像内で検出されたマーカが第２のマーカである場合に、始点位置が第２方向の負方向に位置していると判定してもよい。効果の詳細は実施例で説明する。

【0011】

判定部は、移動体が備えるカメラで撮像した搬送装置の像が、第１テンプレート画像および第２テンプレート画像の何れにマッチングする度合いが高いかを判断してもよい。第１テンプレート画像は、第２方向の正方向側から搬送装置を予め撮像した画像であってもよい。第２テンプレート画像は、第２方向の負方向側から搬送装置を予め撮像した画像であってもよい。判定部は、搬送装置の像が第１テンプレート画像にマッチングする度合いが高いと判断した場合に、始点位置が第２方向の正方向に位置していると判定してもよい。判定部は、搬送装置の像が第２テンプレート画像にマッチングする度合いが高いと判断した場合に、始点位置が第２方向の負方向に位置していると判定してもよい。効果の詳細は実施例で説明する。

【0012】

判定部は、搬送装置から移動体に荷物を降ろす作業計画である場合に、始点位置が第２方向の正方向に位置していると判定してもよい。判定部は、移動体から搬送装置へ荷物を積み込む作業計画である場合に、始点位置が第２方向の負方向に位置していると判定してもよい。効果の詳細は実施例で説明する。

【0013】

上記の軌道生成装置を備えた移動体も、新規で有用である。

【図面の簡単な説明】

【0014】

【図1】牽引車の構成を示すブロック図。

【図2】軌道生成の具体例を示す図。

【図3】軌道生成の具体例を示す図。

【図4】外形モデルデータの一例を示す図。

【図5】目標軌道の生成方法を示すフローチャート。

【図6】搭降載器材の位置および姿勢の計測の様子を示す上面図。

【図7】搭降載器材の測定データ例。

【図8】外形モデルデータおよび環境センサによる計測データを重ね合わせた例を示す図。

【発明を実施するための形態】

【実施例1】

【0015】

（牽引車１０の構成）
図１に、本実施例に係る牽引車１０のブロック図を示す。牽引車１０は、無人牽引車である。牽引車１０は、車体２０、演算装置３０、環境センサ４０、カメラ４１を備えている。車体２０は、操舵制御部２１および駆動制御部２２を備えている。操舵制御部２１および駆動制御部２２は、ＣＰＵ等を備えたマイクロプロセッサによって構成されている。操舵制御部２１は、不図示の操舵装置と通信可能に接続されており、牽引車１０の進行方向を制御する。駆動制御部２２は、不図示の駆動輪モータと通信可能に接続されており、牽引車１０の走行速度を制御する。

【0016】

環境センサ４０は、車体２０に設置されている。本実施例では、環境センサ４０は、２Ｄ－ＬｉＤＡＲである。２Ｄ－ＬｉＤＡＲは、水平方向にレーザ光を走査する測域センサである。環境センサ４０は、車体２０の前方に設定された設定空間内の物体と車体２０との距離を計測する。これにより、牽引車１０の前方の距離データを取得する。環境センサ４０の高さは、搭降載器材１５の側面が計測できる高さに設置する。環境センサ４０で取得された距離データは、演算装置３０の計測データ取得部３２および位置姿勢推定部３５に入力される。カメラ４１は、車体２０の前方を撮像する。カメラ４１で撮像された画像データは、判定部３１に入力される。

【0017】

演算装置３０は、ＣＰＵ等を備えたマイクロプロセッサによって構成されている。演算装置３０は、判定部３１、計測データ取得部３２、外形モデル記憶部３３、座標変換部３４、位置姿勢推定部３５、軌道生成部３６、速度パターン生成部３７、制御指令生成部３８、を備える。判定部３１～制御指令生成部３８の内容については後述する。

【0018】

（概略説明）
本明細書で説明する技術は、航空機１へコンテナ（貨物）を搭降載するためのガイドレス無人牽引車の軌道生成に関する技術である。具体例を図２および図３に示す。図２は、航空機１の前方から牽引車１０が接近する場合（すなわち航空機１に貨物を積み込む場合）を示す図である。

【0019】

牽引車１０は、ドリー（被牽引車）１１～１４を牽引している。ドリー１１～１４の各々には、コンテナＣ１～Ｃ４が載置されている。航空機１の貨物室ドアの近傍には、搭降載器材（ハイリフトローダー）１５が配置してある。搭降載器材１５は、コンテナを航空機１に搭降載する器材である。

【0020】

搭降載器材１５は、荷物出し入れ部１５ａ、第１側面１５ｂ、第２側面１５ｃ、搬送経路部１５ｄを備えている。搬送経路部１５ｄは、方向ｙ０に伸びている。搬送経路部１５ｄは、コンテナを滑らせて移動させる経路である。搬送経路部１５ｄの方向ｙ０の正方向の端部には、荷物出し入れ部１５ａが配置されている。荷物出し入れ部１５ａは、ドリーとの間でコンテナを受け渡しする部位である。

【0021】

搭降載器材１５は、搭降載器材座標系ＣＳ０を備えている。搭降載器材座標系ＣＳ０は、基準位置Ｐ０を基準とした、搭降載器材１５の姿勢を表している。基準位置Ｐ０は任意の位置でよい。本実施例では、荷物出し入れ部１５ａの右隅部を基準位置Ｐ０としている。搭降載器材１５の姿勢は、搬送経路部１５ｄが伸びている方向である方向ｙ０と、方向ｙ０に垂直な方向ｘ０とによって定められる。

【0022】

第１側面１５ｂは、方向ｘ０の正方向側における搭降載器材１５の側面である。第１側面１５ｂには、第１マーカＭ１が配置されている。第２側面１５ｃは、方向ｘ０の負方向側における搭降載器材１５の側面である。第２側面１５ｃには、第２マーカＭ２が配置されている。第１マーカＭ１および第２マーカＭ２は、固有ＩＤを検出可能な視覚マーカ（例えばＡＲマーカ）である。第１マーカＭ１および第２マーカＭ２は、マグネットシートにより貼付されている。

【0023】

牽引車１０は、始点位置Ｐ１から停車位置Ｐ３まで移動する。始点位置座標系ＣＳ１は、始点位置Ｐ１における牽引車１０の姿勢を表す。牽引車１０の姿勢は、進行方向ｘ１と、進行方向ｘ１に垂直な垂直方向ｙ１によって定められる。停車位置Ｐ３は、ドリー１１が搭降載器材１５の荷物出し入れ部１５ａに横付けされた状態になる位置である。このようにドリーを整列させた状態で停車位置Ｐ３に牽引車１０を停車させるためには、始点位置Ｐ１から停車位置Ｐ３へ至る目標軌道ＴＴ（図２点線）を設定しなければならない。しかし、航空機１は決まった位置および姿勢で停車するとは限らない。従って、荷物の搭降載のたびに目標軌道ＴＴを設定する必要がある。そのため、搭降載器材１５の位置および姿勢を始点位置Ｐ１から正確に推定する必要が生じる。

【0024】

また図３は、航空機１の後方から牽引車１０が接近する場合（すなわち航空機１から貨物を降ろす場合）を示す図である。なお、図３と図２の差異点は、接近方向のみである。よって図３の各部の内容は図２と同様であるため、説明を省略する。

【0025】

（搭降載器材１５の外形モデルデータの作成）
事前準備として、搭降載器材１５の外形形状を示す外形モデルデータを用意し、外形モデル記憶部３３に記憶させておく必要がある。第１のステップとして、搭降載器材１５の３次元点群モデルを取得する。具体的には、３次元測量器を用いて搭降載器材１５の形状を測量する。

【0026】

第２のステップとして、３次元点群モデルのダウンサンプリングを行う。第１のステップで得た３次元点群は点数が非常に多い（例：約４３万点）ためである。ダウンサンプリングの一例を説明する。まず、所定間隔にダウンサンプリングする。本実施例では、４ｃｍ間隔でダウンサンプリングすることで、点数を約２万点まで減少させることができた。次に、後述するマッチング処理（ステップＳ７０）に必要な箇所を残し、不要な部分を除去する。本実施例では、運転台と荷台部分（コンテナが滑って移動する面）を除去した。これにより、約１０００点まで減少させることができた。

【0027】

第３のステップとして、３次元点群を２次元に圧縮する。具体的には、環境センサ４０（２Ｄ－ＬｉＤＡＲ）でレーザ光を走査する水平面で、３次元点群を輪切り形状に抜き出す。これにより、図４に示す、外形モデルデータＭＤが得られる。図４では、搭降載器材１５を上方から見たときの外形モデルデータＭＤを示している。外形モデルデータＭＤは、任意座標系での位置座標値を有する。図４の上側の点群が第１側面１５ｂに対応しており、下側の点群が第２側面１５ｃに対応している。図４の左側が、荷物出し入れ部１５ａに対応している。外形モデルデータＭＤはデータサイズが圧縮されているため、後述するマッチング処理（ステップＳ７０）での計算時間を短縮することができる。

【0028】

（目標軌道の生成方法）
図５のフローチャートを用いて、目標軌道ＴＴの生成方法を説明する。図５のフローは、始点位置Ｐ１から停車位置Ｐ３まで牽引車１０を移動させる指示が入力されたときに、開始される。

【0029】

ステップＳ１０において、判定部３１は、カメラ４１によって搭降載器材１５を撮像する。そして撮像した画像内から、第１マーカＭ１を示す第１のＩＤ情報、または、第２マーカＭ２を示す第２のＩＤ情報が検出されたか否かを判断する。検出されなかった場合（Ｓ１０：ＮＯ）にはＳ１０へ戻り、搭降載器材１５の撮像からやり直される。検出された場合（Ｓ１０：ＹＥＳ）には、Ｓ２０へ進む。

【0030】

ステップＳ２０において、判定部３１は、牽引車１０の始点位置Ｐ１の位置を判定する。ステップＳ１０において第２のＩＤ情報が検出された場合には、図２に示すように、始点位置Ｐ１が搭降載器材１５に対して方向ｘ０の負方向（矢印Ｙ２が示す航空機１の前方側）に位置していると判定する。一方、第１のＩＤ情報が検出された場合には、図３に示すように、始点位置Ｐ１が搭降載器材１５に対して方向ｘ０の正方向（矢印Ｙ１が示す航空機１の後方側）に位置していると判定する。

【0031】

ステップＳ２５において、計測データ取得部３２は、搭降載器材１５の位置および姿勢を、環境センサ４０（２Ｄ－ＬｉＤＡＲ）を用いて計測する。図６に、計測の様子を示す上面図を示す。図７に、測定データ例を示す。図７は、２Ｄ－ＬｉＤＡＲの測定結果を上面図として示している。分りやすさのため、図７には、牽引車１０を点線で記載している。図７に示すように、環境センサ４０の計測データは、点群データである。点群データは、牽引車１０を基準とした座標系である、始点位置座標系ＣＳ１での位置座標値を有している。

【0032】

ステップＳ３０において、計測データ取得部３２は、セグメンテーション処理を実行する。セグメンテーション処理は、ステップＳ２５で取得した測定データにおいて、一定距離内に存在する計測点群を同一物体としてクラスタリングする処理である。図７の測定データ例では、領域Ｒ１および領域Ｒ２内の点群がクラスタリングされる。領域Ｒ１内の点群は、第２側面１５ｃの測定データである。領域Ｒ２内の点群は、荷物出し入れ部１５ａの測定データである。

【0033】

ステップＳ４０において計測データ取得部３２は、得られたクラスタの中に、搬送経路部１５ｄの側面の位置および姿勢を示す特定クラスタが存在するか否かを判断する。特定クラスタは、長さが所定サイズ以上であるクラスタである。また特定クラスタは、特定クラスタが伸びる方向と牽引車１０の進行方向ｘ１とが成す角度が、所定角度以上であるクラスタである。所定サイズは、搬送経路部１５ｄの長さＬＥ（図６参照）に基づいて定めればよい。例えば所定サイズは、長さＬＥの７０％の長さとしてもよい。また所定角度は、例えば３０度以上としてもよい。特定クラスタが存在しない場合（Ｓ４０：ＮＯ）にはＳ１０へ戻り、各処理がやり直される。一方、特定クラスタが存在する場合（Ｓ４０：ＹＥＳ）には、Ｓ５０へ進む。図７の測定データ例では、領域Ｒ１内のクラスタが、特定クラスタとして決定される。

【0034】

ステップＳ５０において、計測データ取得部３２は、特定クラスタの傾き角度および基準位置を求める。まず、特定クラスタに含まれる点群にフィッティングする直線ＳＬを求める。直線ＳＬを求める方法の一例としては、RANSAC（Random Sample Consensus）法が挙げられる。直線ＳＬは、搭降載器材１５の側面の直線形状を表している。直線ＳＬが進行方向ｘ１に対して有する傾き角度Ａ１（図７参照）が、クラスタの傾き角度である。次に、特定クラスタの基準位置ＰＥを決定する。基準位置ＰＥは、領域Ｒ１内のクラスタの牽引車１０側（図７の左側）の端点とされる。

【0035】

ステップＳ６０において、座標変換部３４は、外形モデル記憶部３３に記憶されている外形モデルデータＭＤ（図４）を、環境センサ４０による計測データ（図７）と重ね合わせるように、座標変換する。すなわちステップＳ６０では、ステップＳ７０でマッチング処理を行う前処理として、両データを大まかに重ね合わせる処理が行われる。外形モデルデータＭＤで表される搭降載器材１５の寸法と、環境センサ４０による計測データで表される搭降載器材１５の寸法とは、どちらも実測値であるため、等しい。よって、外形モデルデータＭＤの回転および平行移動により、両データの重ね合わせが可能である。

【0036】

まず、外形モデルデータＭＤの基準位置ＰＤを決定する。基準位置ＰＤは、特定クラスタの基準位置ＰＥ（図７）と対応する位置である。基準位置ＰＤは、外形モデルデータＭＤにおける第２側面１５ｃの、牽引車１０側（図４の左側）の端点とされる。次に、外形モデルデータＭＤの第２側面１５ｃが進行方向ｘ１に対して有する傾きが、直線ＳＬの傾き角度Ａ１（図７）と同じになるように、外形モデルデータＭＤを回転させる。最後に、外形モデルデータＭＤの基準位置ＰＤが、特定クラスタの基準位置ＰＥ（図７）に一致するように、外形モデルデータＭＤを平行移動させる。これにより、図８に示すように、外形モデルデータＭＤを、環境センサ４０による計測データに重ねることができる。

【0037】

ステップＳ７０において、位置姿勢推定部３５は、外形モデルデータＭＤと環境センサ４０による計測点群とのマッチング処理を行う。マッチング処理には、例えば、ICP（Iterative Closest Points）を用いることができるが、これ以外の手法を用いてもよい。マッチング処理により、図８に示す外形モデルデータＭＤの位置および姿勢が微調整される。これにより、環境センサ４０による計測データと外形モデルデータＭＤとを、完全に一致するように重ね合わせることができる。換言すると、外形モデルデータＭＤの座標系を、任意座標系から始点位置座標系ＣＳ１に変換することができる。マッチング処理の終了後における、始点位置座標系ＣＳ１上の外形モデルデータＭＤの位置および姿勢が、搭降載器材１５の位置および姿勢の推定結果となる。

【0038】

ステップＳ８０において、軌道生成部３６は、目標軌道ＴＴを生成する。図２および図３に示すように、目標軌道ＴＴは、始点位置Ｐ１を始点位置座標系ＣＳ１で示す姿勢で出発し、停車位置Ｐ３へ停車位置座標系ＣＳ３で示す停車姿勢で停止可能な軌道である。停車姿勢は、図２および図３に示すように、停車位置Ｐ３における牽引車１０の進行方向ｘ３が、荷物出し入れ部１５ａに平行な方向ｘ０と一致するような姿勢である。

【0039】

ステップＳ９０において、速度パターン生成部３７は、目標軌道ＴＴに基づいて速度パターンの設計を行う。ステップＳ１００において、制御指令生成部３８は、目標軌道ＴＴと速度パターンに基づいて、停車位置Ｐ３まで牽引車１０を移動するための制御指令値を算出する。生成された制御指令値は、操舵制御部２１および駆動制御部２２に入力され、牽引車１０を停車位置Ｐ３に向かって移動させる。

【0040】

なお、牽引車１０の走行中に図５のフローを実行することも可能である。例えば、停車位置Ｐ３へ向かって一定距離走行する度に、新たな始点位置Ｐ１を設定するとともに図５のフローを実行すればよい。これにより、牽引車１０の走行中に、搭降載器材１５の位置および姿勢の推定値を更新することができる。

【0041】

なお本実施例では、図２に示すように、始点位置Ｐ１が航空機１の前方に位置している場合における、目標軌道ＴＴの生成フロー（図５）を説明した。しかし図３に示すように、始点位置Ｐ１が航空機１の後方に位置している場合においても、同様にして目標軌道ＴＴの生成フローを実行することが可能である。

【0042】

（効果）
航空機１へコンテナを搭降載する牽引車１０は、十数ｍ先などの遠方に位置する搭降載器材１５まで自走する必要がある。そのため、搭降載器材１５の位置および姿勢を高精度に推定する必要がある。例えば、搭降載器材１５に配置されたＡＲマーカをカメラを用いて認識する方法では、搭降載器材１５が遠方に位置するほど撮像されるマーカの像が小さくなるため、搭降載器材１５の位置および姿勢を高精度に推定することが困難とある。本明細書に記載の技術では、レーザセンサ（２Ｄ－ＬｉＤＡＲ）による計測データと、外形モデルデータＭＤとをマッチングすることができる（ステップＳ７０）。レーザセンサを用いることにより、カメラを用いる場合に比して、搭降載器材１５までの距離を正確に測定することができる。また外形モデルデータＭＤは、３次元測量をもとにして生成したデータであり、非常に精度が高いデータである。よって外形モデルデータＭＤを用いることにより、搭降載器材１５の位置および姿勢を、カメラを用いる場合に比して高精度に推定することができる。

【0043】

搭降載器材１５は、方向ｙ０に伸びる搬送経路部１５ｄを備えている。また、牽引車１０の始点位置Ｐ１が、航空機１の前方または後方側に位置する、という特徴がある（図２および図３参照）。これにより、牽引車１０が備える環境センサ４０によって、搬送経路部１５ｄの第１側面１５ｂまたは第２側面１５ｃの何れかを計測することができる。第１側面１５ｂまたは第２側面１５ｃの計測データは、図７の例に示すように、略直線の単純な形状である。よって単純な形状を用いてマッチング処理（ステップＳ７０）を実行することができるため、マッチングの計算負担を低減することができる。牽引車１０の走行中に、マッチング処理を行うことが可能となる。

【0044】

外形モデルデータＭＤ（図４）と環境センサ４０による計測データ（図７）との位置や姿勢が大きく異なる状態で、両データにマッチング処理（ステップＳ７０）を実行すると、部分的に重なるだけなどの誤マッチングが発生する場合がある。本明細書に記載の技術では、外形モデルデータＭＤと環境センサ４０による計測データとを大まかに重ね合わせる処理（ステップＳ６０）を実行してから、両データにマッチング処理（ステップＳ７０）を実行する。両データの位置や姿勢が近い状態でマッチング処理を実行できるため、誤マッチングを抑制することができる。

【0045】

航空機１に牽引車１０が接近する場合には、前方から接近するパターン（図２）と、後方から接近するパターン（図３）とが存在する。本明細書に記載の技術では、前方から接近する場合に見える第２側面１５ｃに第２マーカＭ２を配置し、後方から接近する場合に見える第１側面１５ｂに第１マーカＭ１を配置することで、何れの方向から近接しているかを判定することができる（ステップＳ２０）。搭降載器材１５の位置および姿勢を正確に推定することが可能となる。

【実施例2】

【0046】

実施例２では、判定部３１が、第１マーカＭ１および第２マーカＭ２を用いずに始点位置Ｐ１の位置を判定する場合を説明する。実施例１の内容と異なる部位についてのみ説明する。

【0047】

判定部３１は、第１テンプレート画像および第２テンプレート画像を予め記憶している。第１テンプレート画像は、方向ｘ０の正方向側（図３の矢印Ｙ１が示す、航空機１の後方側）から搭降載器材１５を予め撮像した画像である。第１テンプレート画像では、航空機１が左側、搭降載器材１５が右側に写っている。また第１側面１５ｂが写っている。第２テンプレート画像は、方向ｘ０の負方向側（図２の矢印Ｙ２が示す、航空機１の前方側）から搬送装置を予め撮像した画像である。第２テンプレート画像では、航空機１が右側、搭降載器材１５が左側に写っている。また第２側面１５ｃが写っている。

【0048】

ステップＳ１０において、判定部３１は、カメラ４１によって搭降載器材１５を撮像する。ステップＳ２０において、判定部３１は、撮像した搭降載器材１５の像が、第１テンプレート画像および第２テンプレート画像の何れにマッチングする度合いが高いかを判断する。例えば画像認識技術を用いて、撮像した搭降載器材１５の像に、第１側面１５ｂと第２側面１５ｃの何れが含まれているかを判断してもよい。また例えば、搭降載器材１５と航空機１との左右の位置関係を判断してもよい。第１テンプレート画像にマッチングする度合いが高いと判断された場合には、始点位置Ｐ１が方向ｘ０の正方向（図３の矢印Ｙ１の方向）に位置していると判定される。 一方、第２テンプレート画像にマッチングする度合いが高いと判断された場合には、始点位置Ｐ１が方向ｘ０の負方向（図２の矢印Ｙ２の方向）に位置していると判定される。ステップＳ３０以降の処理は、実施例１と同様である。

【0049】

なお、ステップＳ２０における画像マッチング判定は、機械学習モデル（例：ニューラルネットワーク）を用いてもよい。この場合、搭降載器材１５を様々な状況で撮影した教師画像データを多数用意しておけばよい。そして、正確な画像マッチング判定が行えるように、機械学習モデルを予め学習させておけばよい。

【0050】

（効果）
第１マーカＭ１や第２マーカＭ２を搭降載器材１５に配置しておく必要がない。より簡易な構成で、搭降載器材１５の位置および姿勢を正確に推定することが可能となる。

【実施例3】

【0051】

実施例３では、判定部３１が、作業計画に関する情報に基づいて始点位置Ｐ１の位置を判定する場合を説明する。実施例１の内容と異なる部位についてのみ説明する。

【0052】

ステップＳ２０において判定部３１は、作業計画が、搭降載器材１５から牽引車１０に荷物を積み込む作業計画であるか、牽引車１０から搭降載器材１５へ荷物を降ろす作業計画であるかを判断する。この判断は、例えば、判定部３１に入力される作業計画情報に基づいて行われてもよい。また例えば、牽引車１０が牽引するドリーの総重量が重い場合には荷物を積み込む作業計画であり、軽い場合には荷物を降ろす作業計画であると判断してもよい。

【0053】

荷物を降ろす作業計画である場合には、始点位置Ｐ１が方向ｘ０の正方向（図３の矢印Ｙ１の方向）に位置していると判定される。荷物を降ろす場合には、航空機１の後方から牽引車１０を接近させるためである。一方、荷物を積み込む作業計画である場合には、始点位置Ｐ１が方向ｘ０の負方向（図２の矢印Ｙ２の方向）に位置していると判定される。荷物を積み込む場合には、航空機１の前方から牽引車１０を接近させるためである。ステップＳ３０以降の処理は、実施例１と同様である。

【0054】

（効果）
カメラ４１によって搭降載器材１５を撮像することなく、判定部３１で始点位置Ｐ１の位置を判定することができる。ステップＳ１０の処理を省略することが可能となる。より簡易な構成で、搭降載器材１５の位置および姿勢を正確に推定することが可能となる。

【0055】

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

【0056】

（変形例）
判定部３１が始点位置Ｐ１の位置を判定する方法は、様々であってよい。例えば、牽引車１０がターミナルの貨物積み降ろし場をスタートして、図２の始点位置Ｐ１または図３の始点位置Ｐ１まで移動する場合には、途中で経路が分岐する。その分岐方向が、航空機１の前方側であるか後方側であるかによって、始点位置Ｐ１を判定してもよい。また牽引車１０は、地図上における自己位置を認識することで、始点位置Ｐ１を判定してもよい。

【0057】

ステップＳ６０において、外形モデルデータＭＤを回転させる処理と平行移動させる処理との順番は問わない。

【0058】

外形モデルデータＭＤの生成方法は、３次元測量器を用いた方法に限られない。十分な精度が得られる方法であれば、何れの方法を用いてもよい。例えば、「Structure from Motion法」と呼ばれる、視点の違う複数枚の画像から被写体の３次元形状およびカメラの相対位置を復元する技術を用いてもよい。

【0059】

特定クラスタの基準位置ＰＥ（図７）や、外形モデルデータＭＤの基準位置ＰＤ（図４）は、端部に限られない。位置が明確であれば何れの位置であってもよい。

【0060】

第１マーカＭ１および第２マーカＭ２を搭降載器材１５に配置する方法は、マグネットを用いる形態に限られず、様々な態様であってよい。搭降載器材１５にマーカを直接印字してもよい。またマーカの配置数および配置位置は、牽引車１０から確実に視認できる態様であれば、何れの数および位置であってもよい。

【0061】

本実施例では、環境センサ４０として２Ｄ－ＬｉＤＡＲを用いたが、各種のセンサを用いてもよい。例えば、３Ｄ－ＬｉＤＡＲやＴＯＦ（Time of Flight）カメラであってもよい。

【0062】

外形モデルデータＭＤは、２次元に圧縮したデータに限られず、３次元データを用いることも可能である。

【0063】

本実施例では、牽引車１０が空港での貨物運搬システムで用いられる場合を説明したが、この形態に限られない。本明細書に記載の技術は、荷物を牽引し所定の場所へ貨物を搭降載するシステムであれば、何れのシステムにも適用できる。

【0064】

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

【0065】

牽引車１０は、移動体の一例である。搭降載器材１５は、搬送装置の一例である。演算装置３０は、軌道生成装置の一例である。方向ｙ０は、第１方向の一例である。方向ｘ０は、第２方向の一例である。座標変換部３４および位置姿勢推定部３５は、推定部の一例である。進行方向ｘ１は、移動体向きの一例である。傾き角度Ａ１は、所定角度の一例である。基準位置ＰＥは、搬送経路部の基準位置の一例である。基準位置ＰＤは、外形モデルの基準位置の一例である。

【符号の説明】

【0066】

１０：牽引車 １５：搭降載器材 １５ａ：荷物出し入れ部 １５ｂ：第１側面 １５ｃ：第２側面 １５ｄ：搬送経路部 ３１：判定部 ３２：計測データ取得部 ３３：外形モデル記憶部 ３４：座標変換部 ３５：位置姿勢推定部 ３６：軌道生成部 ３７：速度パターン生成部 ３８：制御指令生成部 ４０：環境センサ ４１：カメラ Ｍ１：第１マーカ Ｍ２：第２マーカ Ｐ１：始点位置 Ｐ３：停車位置

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】