▶ 三菱重工業株式会社の特許一覧 ▶ 国立大学法人 筑波大学の特許一覧
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2023140976
(43)【公開日】2023-10-05
(54)【発明の名称】演算装置、演算方法及びプログラム
(51)【国際特許分類】
G01B 11/00 20060101AFI20230928BHJP
G05D 1/02 20200101ALI20230928BHJP
【ＦＩ】
G01B11/00 H
G01B11/00 Z
G05D1/02 J
【審査請求】未請求
【請求項の数】10
【出願形態】ＯＬ
(21)【出願番号】P 2022047072
(22)【出願日】2022-03-23
(71)【出願人】
【識別番号】000006208
【氏名又は名称】三菱重工業株式会社
(71)【出願人】
【識別番号】504171134
【氏名又は名称】国立大学法人 筑波大学
(74)【代理人】
【識別番号】110002147
【氏名又は名称】弁理士法人酒井国際特許事務所
(72)【発明者】
【氏名】木内 裕介
(72)【発明者】
【氏名】北島 克将
(72)【発明者】
【氏名】坪内 孝司
【テーマコード（参考）】
2F065
5H301
【Ｆターム（参考）】
2F065AA03
2F065AA04
2F065AA09
2F065AA31
2F065AA37
2F065CC11
2F065DD03
2F065FF04
2F065FF09
2F065FF11
2F065FF64
2F065FF65
2F065FF67
2F065GG04
2F065MM16
2F065QQ13
2F065QQ29
2F065QQ31
2F065QQ38
5H301AA01
5H301AA10
5H301BB05
5H301BB07
5H301CC03
5H301CC06
5H301CC10
5H301DD01
5H301DD06
5H301DD15
5H301GG08
5H301GG09
5H301GG12
5H301GG14
5H301GG16
5H301HH01
5H301HH03
(57)【要約】
【課題】移動体の移動量を推定するためのパラメータを高精度に算出する。
【解決手段】演算装置は、移動体に搭載されて移動体の周囲を検出する外界センサの検出結果に基づいて、移動体の第１推定位置を算出する第１位置算出部と、移動体に搭載されて移動体の制御出力を示す出力パラメータを検出する内界センサの検出結果に基づいて、移動体の第２推定位置を算出する第２位置算出部と、第１推定位置及び第２推定位置に基づいて、移動体の移動量を算出するための移動パラメータの推定値を算出するパラメータ算出部と、移動体の停止時における外界センサの検出結果に基づいて、推定値の妥当性を判定する判定部と、妥当であると判断された推定値を移動体の移動パラメータとして設定するパラメータ設定部と、を含む。
【選択図】図２
【解決手段】演算装置は、移動体に搭載されて移動体の周囲を検出する外界センサの検出結果に基づいて、移動体の第１推定位置を算出する第１位置算出部と、移動体に搭載されて移動体の制御出力を示す出力パラメータを検出する内界センサの検出結果に基づいて、移動体の第２推定位置を算出する第２位置算出部と、第１推定位置及び第２推定位置に基づいて、移動体の移動量を算出するための移動パラメータの推定値を算出するパラメータ算出部と、移動体の停止時における外界センサの検出結果に基づいて、推定値の妥当性を判定する判定部と、妥当であると判断された推定値を移動体の移動パラメータとして設定するパラメータ設定部と、を含む。
【選択図】図２
【特許請求の範囲】
【請求項1】
移動体に搭載されて前記移動体の周囲を検出する外界センサの検出結果に基づいて、前記移動体の第１推定位置を算出する第１位置算出部と、
前記移動体に搭載されて前記移動体の制御出力を示す出力パラメータを検出する内界センサの検出結果に基づいて、前記移動体の第２推定位置を算出する第２位置算出部と、
第１推定位置及び前記第２推定位置に基づいて、前記移動体の移動量を算出するための移動パラメータの推定値を算出するパラメータ算出部と、
前記移動体の停止時における前記外界センサの検出結果に基づいて、前記推定値の妥当性を判定する判定部と、
妥当であると判断された前記推定値を前記移動体の移動パラメータとして設定するパラメータ設定部と、
を含む、
演算装置。
前記移動体に搭載されて前記移動体の制御出力を示す出力パラメータを検出する内界センサの検出結果に基づいて、前記移動体の第２推定位置を算出する第２位置算出部と、
第１推定位置及び前記第２推定位置に基づいて、前記移動体の移動量を算出するための移動パラメータの推定値を算出するパラメータ算出部と、
前記移動体の停止時における前記外界センサの検出結果に基づいて、前記推定値の妥当性を判定する判定部と、
妥当であると判断された前記推定値を前記移動体の移動パラメータとして設定するパラメータ設定部と、
を含む、
演算装置。
【請求項2】
前記第１位置算出部は、所定時間毎に検出された前記外界センサの検出結果から、所定時間毎の前記第１推定位置を算出し、
前記第２位置算出部は、所定時間毎に検出された前記内界センサの検出結果から、所定時間毎の前記第２推定位置を算出し、
前記パラメータ算出部は、所定時間毎の前記第１推定位置と前記第２推定位置との差分に基づいて、前記移動パラメータの推定値を算出する、請求項１に記載の演算装置。
前記第２位置算出部は、所定時間毎に検出された前記内界センサの検出結果から、所定時間毎の前記第２推定位置を算出し、
前記パラメータ算出部は、所定時間毎の前記第１推定位置と前記第２推定位置との差分に基づいて、前記移動パラメータの推定値を算出する、請求項１に記載の演算装置。
【請求項3】
前記パラメータ算出部は、所定時間毎の前記第１推定位置と前記第２推定位置との差分の合計値が、最小となるように、前記移動パラメータの推定値を算出する、請求項２に記載の演算装置。
【請求項4】
前記判定部は、
前記移動体の停止時において第１外界センサにより検出された、前記移動体の周囲の検出体の検出結果と、前記移動パラメータの推定値とに基づいて、前記移動体の基準座標系における前記検出体の位置を算出し、
前記移動体の停止時において第２外界センサにより検出された、前記検出体の検出結果と、前記移動パラメータの推定値とに基づいて、前記移動体の基準座標系における前記検出体の位置を算出し、
前記第１外界センサの検出結果に基づいて算出された前記検出体の位置と、前記第２外界センサの検出結果に基づいて算出された前記検出体の位置とのずれ量が、所定値以下である場合に、前記移動パラメータの推定値が妥当であると判断する、請求項１から請求項３のいずれか１項に記載の演算装置。
前記移動体の停止時において第１外界センサにより検出された、前記移動体の周囲の検出体の検出結果と、前記移動パラメータの推定値とに基づいて、前記移動体の基準座標系における前記検出体の位置を算出し、
前記移動体の停止時において第２外界センサにより検出された、前記検出体の検出結果と、前記移動パラメータの推定値とに基づいて、前記移動体の基準座標系における前記検出体の位置を算出し、
前記第１外界センサの検出結果に基づいて算出された前記検出体の位置と、前記第２外界センサの検出結果に基づいて算出された前記検出体の位置とのずれ量が、所定値以下である場合に、前記移動パラメータの推定値が妥当であると判断する、請求項１から請求項３のいずれか１項に記載の演算装置。
【請求項5】
前記判定部は、
前記移動体の停止時において第１外界センサにより検出された、前記移動体の周囲の複数の検出体の検出結果から、前記第１外界センサの位置を算出し、
前記移動体の停止時において第２外界センサにより検出された、前記複数の検出体の検出結果から、前記第２外界センサの位置を算出し、
前記第１外界センサの位置と前記第２外界センサの位置とから算出された、前記第１外界センサに対する前記第２外界センサの相対位置と、前記移動パラメータの推定値から算出された、前記第１外界センサに対する前記第２外界センサの相対位置とのずれ量が、所定値以下である場合に、前記移動パラメータの推定値が妥当であると判断する、請求項１から請求項３のいずれか１項に記載の演算装置。
前記移動体の停止時において第１外界センサにより検出された、前記移動体の周囲の複数の検出体の検出結果から、前記第１外界センサの位置を算出し、
前記移動体の停止時において第２外界センサにより検出された、前記複数の検出体の検出結果から、前記第２外界センサの位置を算出し、
前記第１外界センサの位置と前記第２外界センサの位置とから算出された、前記第１外界センサに対する前記第２外界センサの相対位置と、前記移動パラメータの推定値から算出された、前記第１外界センサに対する前記第２外界センサの相対位置とのずれ量が、所定値以下である場合に、前記移動パラメータの推定値が妥当であると判断する、請求項１から請求項３のいずれか１項に記載の演算装置。
【請求項6】
前記パラメータ算出部は、前記移動体の停止時における前記内界センサの検出結果に基づいて、前記内界センサの検出結果の正常値からのずれ量を示すバイアス値を算出し、前記バイアス値にも基づき、前記移動パラメータの推定値を算出する、請求項１から請求項５のいずれか１項に記載の演算装置。
【請求項7】
前記パラメータ算出部は、
第１時間帯での前記外界センサ及び前記内界センサの検出結果に基づいて算出された前記第１推定位置と前記第２推定位置とに基づいて、前記第１時間帯における前記移動パラメータの推定値を算出し、
第１時間帯より後の第２時間帯での前記外界センサ及び前記内界センサの検出結果に基づいて算出された前記第１推定位置と前記第２推定位置とに基づいて、前記第２時間帯における前記移動パラメータの推定値を算出し、
前記判定部は、前記第２時間帯における前記移動パラメータの推定値が、前記第１時間帯における前記移動パラメータの推定値に対して所定条件を満たす場合に、前記第２時間帯における前記移動パラメータの推定値が妥当であると判断する、請求項１から請求項６のいずれか１項に記載の演算装置。
第１時間帯での前記外界センサ及び前記内界センサの検出結果に基づいて算出された前記第１推定位置と前記第２推定位置とに基づいて、前記第１時間帯における前記移動パラメータの推定値を算出し、
第１時間帯より後の第２時間帯での前記外界センサ及び前記内界センサの検出結果に基づいて算出された前記第１推定位置と前記第２推定位置とに基づいて、前記第２時間帯における前記移動パラメータの推定値を算出し、
前記判定部は、前記第２時間帯における前記移動パラメータの推定値が、前記第１時間帯における前記移動パラメータの推定値に対して所定条件を満たす場合に、前記第２時間帯における前記移動パラメータの推定値が妥当であると判断する、請求項１から請求項６のいずれか１項に記載の演算装置。
【請求項8】
前記移動パラメータは、前記移動体の基準座標系における前記外界センサの位置と、前記移動体の車輪径と、前記内界センサの検出結果の正常値からのずれ量を示すバイアス値との、少なくとも１つである、請求項１から請求項７のいずれか１項に記載の演算装置。
【請求項9】
移動体に搭載されて前記移動体の周囲を検出する外界センサの検出結果に基づいて、前記移動体の第１推定位置を算出するステップと、
前記移動体に搭載されて前記移動体の制御出力を示す出力パラメータを検出する内界センサの検出結果に基づいて、前記移動体の第２推定位置を算出するステップと、
第１推定位置及び前記第２推定位置に基づいて、前記移動体の移動量を算出するための移動パラメータの推定値を算出するステップと、
前記移動体の停止時における複数の前記外界センサの検出結果に基づいて、前記推定値の妥当性を判定するステップと、
妥当であると判断された前記推定値を前記移動体の移動パラメータとして設定するステップと、
を含む、
演算方法。
前記移動体に搭載されて前記移動体の制御出力を示す出力パラメータを検出する内界センサの検出結果に基づいて、前記移動体の第２推定位置を算出するステップと、
第１推定位置及び前記第２推定位置に基づいて、前記移動体の移動量を算出するための移動パラメータの推定値を算出するステップと、
前記移動体の停止時における複数の前記外界センサの検出結果に基づいて、前記推定値の妥当性を判定するステップと、
妥当であると判断された前記推定値を前記移動体の移動パラメータとして設定するステップと、
を含む、
演算方法。
【請求項10】
移動体に搭載されて前記移動体の周囲を検出する外界センサの検出結果に基づいて、前記移動体の第１推定位置を算出するステップと、
前記移動体に搭載されて前記移動体の制御出力を示す出力パラメータを検出する内界センサの検出結果に基づいて、前記移動体の第２推定位置を算出するステップと、
第１推定位置及び前記第２推定位置に基づいて、前記移動体の移動量を算出するための移動パラメータの推定値を算出するステップと、
前記移動体の停止時における複数の前記外界センサの検出結果に基づいて、前記推定値の妥当性を判定するステップと、
妥当であると判断された前記推定値を前記移動体の移動パラメータとして設定するステップと、
を、コンピュータに実行させる、
プログラム。
前記移動体に搭載されて前記移動体の制御出力を示す出力パラメータを検出する内界センサの検出結果に基づいて、前記移動体の第２推定位置を算出するステップと、
第１推定位置及び前記第２推定位置に基づいて、前記移動体の移動量を算出するための移動パラメータの推定値を算出するステップと、
前記移動体の停止時における複数の前記外界センサの検出結果に基づいて、前記推定値の妥当性を判定するステップと、
妥当であると判断された前記推定値を前記移動体の移動パラメータとして設定するステップと、
を、コンピュータに実行させる、
プログラム。
【発明の詳細な説明】
【技術分野】
【0001】
本開示は、演算装置、演算方法及びプログラムに関する。
【背景技術】
【0002】
移動体には、周囲を検出するセンサが備えられる場合がある。特許文献１には、車両に設けられた複数のセンサ同士の位置ずれ量を検出するキャリブレーション装置が記載されている。
【先行技術文献】
【特許文献】
【0003】
【特許文献1】特開２０１６－２２３９６３号公報
【発明の概要】
【発明が解決しようとする課題】
【0004】
このような移動体においては、例えば移動体の基準座標系におけるセンサの位置などの、移動体の移動量を推定するためのパラメータを高精度に算出することで、移動体のキャリブレーションを適切に実行することが求められている。
【0005】
本開示は、上述した課題を解決するものであり、移動体の移動量を推定するためのパラメータを高精度に算出可能な演算装置、演算方法及びプログラムを提供することを目的とする。
【課題を解決するための手段】
【0006】
上述した課題を解決し、目的を達成するために、本開示に係る演算装置は、移動体に搭載されて前記移動体の周囲を検出する外界センサの検出結果に基づいて、前記移動体の第１推定位置を算出する第１位置算出部と、前記移動体に搭載されて前記移動体の制御出力を示す出力パラメータを検出する内界センサの検出結果に基づいて、前記移動体の第２推定位置を算出する第２位置算出部と、第１推定位置及び前記第２推定位置に基づいて、前記移動体の移動量を算出するための移動パラメータの推定値を算出するパラメータ算出部と、前記移動体の停止時における前記外界センサの検出結果に基づいて、前記推定値の妥当性を判定する判定部と、妥当であると判断された前記推定値を前記移動体の移動パラメータとして設定するパラメータ設定部と、を含む。
【0007】
上述した課題を解決し、目的を達成するために、本開示に係る演算方法は、移動体に搭載されて前記移動体の周囲を検出する外界センサの検出結果に基づいて、前記移動体の第１推定位置を算出するステップと、前記移動体に搭載されて前記移動体の制御出力を示す出力パラメータを検出する内界センサの検出結果に基づいて、前記移動体の第２推定位置を算出するステップと、第１推定位置及び前記第２推定位置に基づいて、前記移動体の移動量を算出するための移動パラメータの推定値を算出するステップと、前記移動体の停止時における複数の前記外界センサの検出結果に基づいて、前記推定値の妥当性を判定するステップと、妥当であると判断された前記推定値を前記移動体の移動パラメータとして設定するステップと、を含む。
【0008】
上述した課題を解決し、目的を達成するために、本開示に係るプログラムは、移動体に搭載されて前記移動体の周囲を検出する外界センサの検出結果に基づいて、前記移動体の第１推定位置を算出するステップと、前記移動体に搭載されて前記移動体の制御出力を示す出力パラメータを検出する内界センサの検出結果に基づいて、前記移動体の第２推定位置を算出するステップと、第１推定位置及び前記第２推定位置に基づいて、前記移動体の移動量を算出するための移動パラメータの推定値を算出するステップと、前記移動体の停止時における複数の前記外界センサの検出結果に基づいて、前記推定値の妥当性を判定するステップと、妥当であると判断された前記推定値を前記移動体の移動パラメータとして設定するステップと、を、コンピュータに実行させる。
【発明の効果】
【0009】
本開示によれば、移動体の移動量を推定するためのパラメータを高精度に算出することができる。
【図面の簡単な説明】
【0010】
【発明を実施するための形態】
【0011】
以下に添付図面を参照して、本開示の好適な実施形態を詳細に説明する。なお、この実施形態により本開示が限定されるものではなく、また、実施形態が複数ある場合には、各実施形態を組み合わせて構成するものも含むものである。
【0012】
（第１実施形態）
（移動体）
図１は、本実施形態に係る移動体の模式図である。本実施形態においては、移動体１０は、自動で移動可能な装置である。より詳しくは、移動体１０は、フォークリフトであり、より詳しくはいわゆるＡＧＶ（Ａｕｔｏｍａｔｅｄ Ｇｕｉｄｅｄ Ｖｅｈｉｃｌｅ）やＡＧＦ（Ａｕｔｏｍａｔｅｄ Ｇｕｉｄｅｄ Ｆｏｒｋｌｉｆｔ）である。ただし、移動体１０は、移動可能な任意の装置であってよく、フォークリフトであることに限られない。また、移動体１０は、自動で移動する装置に限られず、ユーザの運転によって移動する装置であってもよい。
（移動体）
図１は、本実施形態に係る移動体の模式図である。本実施形態においては、移動体１０は、自動で移動可能な装置である。より詳しくは、移動体１０は、フォークリフトであり、より詳しくはいわゆるＡＧＶ（Ａｕｔｏｍａｔｅｄ Ｇｕｉｄｅｄ Ｖｅｈｉｃｌｅ）やＡＧＦ（Ａｕｔｏｍａｔｅｄ Ｇｕｉｄｅｄ Ｆｏｒｋｌｉｆｔ）である。ただし、移動体１０は、移動可能な任意の装置であってよく、フォークリフトであることに限られない。また、移動体１０は、自動で移動する装置に限られず、ユーザの運転によって移動する装置であってもよい。
【0013】
（移動体の構成）
図１に示すように、移動体１０は、車体２０と、車輪２０Ａと、ストラドルレッグ２１と、マスト２２と、フォーク２４と、外界センサ２６Ａと、内界センサ２６Ｂと、制御装置２８とを備えている。ストラドルレッグ２１は、車体２０の前後方向における一方の端部に設けられて、車体２０から突出する一対の軸状の部材である。車輪２０Ａは、それぞれのストラドルレッグ２１の先端と、車体２０とに設けられている。すなわち、車輪２０Ａは、合計３個設けられているが、車輪２０Ａの設けられる位置や個数は任意であってよい。マスト２２は、ストラドルレッグ２１に移動可能に取り付けられ、車体２０の前後方向に移動する。マスト２２は、前後方向に直交する上下方向に沿って延在する。フォーク２４は、マスト２２に方向Ｚに移動可能に取付けられている。フォーク２４は、マスト２２に対して、車体２０の横方向（上下方向及び前後方向に交差する方向）にも移動可能であってよい。フォーク２４は、一対のツメ２４Ａ、２４Ｂを有している。ツメ２４Ａ、２４Ｂは、マスト２２から車体２０の前方向に向けて延在している。ツメ２４Ａとツメ２４Ｂとは、マスト２２の横方向に、互いに離れて配置されている。以下、前後方向のうち、移動体１０においてフォーク２４が設けられている側の方向を、前方向とし、フォーク２４が設けられていない側の方向を、後方向とする。
図１に示すように、移動体１０は、車体２０と、車輪２０Ａと、ストラドルレッグ２１と、マスト２２と、フォーク２４と、外界センサ２６Ａと、内界センサ２６Ｂと、制御装置２８とを備えている。ストラドルレッグ２１は、車体２０の前後方向における一方の端部に設けられて、車体２０から突出する一対の軸状の部材である。車輪２０Ａは、それぞれのストラドルレッグ２１の先端と、車体２０とに設けられている。すなわち、車輪２０Ａは、合計３個設けられているが、車輪２０Ａの設けられる位置や個数は任意であってよい。マスト２２は、ストラドルレッグ２１に移動可能に取り付けられ、車体２０の前後方向に移動する。マスト２２は、前後方向に直交する上下方向に沿って延在する。フォーク２４は、マスト２２に方向Ｚに移動可能に取付けられている。フォーク２４は、マスト２２に対して、車体２０の横方向（上下方向及び前後方向に交差する方向）にも移動可能であってよい。フォーク２４は、一対のツメ２４Ａ、２４Ｂを有している。ツメ２４Ａ、２４Ｂは、マスト２２から車体２０の前方向に向けて延在している。ツメ２４Ａとツメ２４Ｂとは、マスト２２の横方向に、互いに離れて配置されている。以下、前後方向のうち、移動体１０においてフォーク２４が設けられている側の方向を、前方向とし、フォーク２４が設けられていない側の方向を、後方向とする。
【0014】
（外界センサ）
外界センサ２６Ａは、移動体１０に搭載されて、移動体１０の周囲を検出するセンサである。外界センサ２６Ａは、移動体１０の周辺に存在する対象物の位置を検出する。本実施形態では、外界センサ２６Ａは、移動体１０に対する対象物の位置及び姿勢を検出することが好ましい。外界センサ２６Ａは、例えばレーザ光を照射するセンサである。外界センサ２６Ａは、一方向（ここでは横方向）に走査しつつレーザ光を照射し、照射したレーザ光の反射光から、対象物の位置及び向きを検出する。すなわち、外界センサ２６Ａは、いわゆる２次元（２Ｄ）－ＬｉＤＡＲ（Ｌｉｇｈｔ Ｄｅｔｅｃｔｉｏｎ Ａｎｄ Ｒａｎｇｉｎｇ）であるともいえる。ただし、外界センサ２６Ａは、以上のものに限られず任意の方法で対象物を検出するセンサであってよく、例えば、複数の方向に走査されるいわゆる３次元（３Ｄ）－ＬｉＤＡＲであってもよいし、走査されない、いわゆる１次元（１Ｄ）－ＬｉＤＡＲであってもよいし、周囲を撮像するカメラであってもよい。
外界センサ２６Ａは、移動体１０に搭載されて、移動体１０の周囲を検出するセンサである。外界センサ２６Ａは、移動体１０の周辺に存在する対象物の位置を検出する。本実施形態では、外界センサ２６Ａは、移動体１０に対する対象物の位置及び姿勢を検出することが好ましい。外界センサ２６Ａは、例えばレーザ光を照射するセンサである。外界センサ２６Ａは、一方向（ここでは横方向）に走査しつつレーザ光を照射し、照射したレーザ光の反射光から、対象物の位置及び向きを検出する。すなわち、外界センサ２６Ａは、いわゆる２次元（２Ｄ）－ＬｉＤＡＲ（Ｌｉｇｈｔ Ｄｅｔｅｃｔｉｏｎ Ａｎｄ Ｒａｎｇｉｎｇ）であるともいえる。ただし、外界センサ２６Ａは、以上のものに限られず任意の方法で対象物を検出するセンサであってよく、例えば、複数の方向に走査されるいわゆる３次元（３Ｄ）－ＬｉＤＡＲであってもよいし、走査されない、いわゆる１次元（１Ｄ）－ＬｉＤＡＲであってもよいし、周囲を撮像するカメラであってもよい。
【0015】
図１の例では、外界センサ２６Ａは、車体２０の後方向側（フォーク２４と反対側）において、横方向に並んで２つ設けられている。ただし、外界センサ２６Ａの設けられる位置はこれに限られず、任意の位置に設けられてもよいし、設けられる数も任意であってよい。ただし、外界センサ２６Ａは、複数設けられることが好ましい。
【0016】
（内界センサ）
内界センサ２６Ｂは、移動体１０に搭載されて、移動体１０の出力パラメータを検出するセンサである。出力パラメータとは、移動体の制御出力を示すパラメータである。さらに言えば、出力パラメータとは、移動体１０が制御によって動作した度合いを指しており、移動体１０に制御入力がなされた際の、移動体１０からの出力値であるといえる。すなわち、移動体１０の駆動機構（アクチュエータなど）には、制御装置２８により、所望の進行方向及び移動速度となるように、制御入力がなされる。移動体１０の駆動機構は、制御入力に応じて作動して、制御入力に応じた出力パラメータが、内界センサ２６Ｂにより検出される。出力パラメータは、移動体の制御出力を示す任意のパラメータであってよいが、本実施形態では、移動体１０の車輪２０Ａの回転数と移動体１０の操舵角である。この場合、内界センサ２６Ｂは、車輪２０Ａの回転数（単位時間当たりの回転数）と、移動体１０の操舵角（車輪２０Ａの向きの変化量）とを、検出する。内界センサ２６として、回転数を検出するセンサと操舵角を検出するセンサとが設けられていてもよいし、１つのセンサで回転数と操舵角とを検出してもよい。内界センサ２６Ｂは、例えば、ＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）であってもよい。
内界センサ２６Ｂは、移動体１０に搭載されて、移動体１０の出力パラメータを検出するセンサである。出力パラメータとは、移動体の制御出力を示すパラメータである。さらに言えば、出力パラメータとは、移動体１０が制御によって動作した度合いを指しており、移動体１０に制御入力がなされた際の、移動体１０からの出力値であるといえる。すなわち、移動体１０の駆動機構（アクチュエータなど）には、制御装置２８により、所望の進行方向及び移動速度となるように、制御入力がなされる。移動体１０の駆動機構は、制御入力に応じて作動して、制御入力に応じた出力パラメータが、内界センサ２６Ｂにより検出される。出力パラメータは、移動体の制御出力を示す任意のパラメータであってよいが、本実施形態では、移動体１０の車輪２０Ａの回転数と移動体１０の操舵角である。この場合、内界センサ２６Ｂは、車輪２０Ａの回転数（単位時間当たりの回転数）と、移動体１０の操舵角（車輪２０Ａの向きの変化量）とを、検出する。内界センサ２６として、回転数を検出するセンサと操舵角を検出するセンサとが設けられていてもよいし、１つのセンサで回転数と操舵角とを検出してもよい。内界センサ２６Ｂは、例えば、ＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）であってもよい。
【0017】
（制御装置）
図２は、制御装置の模式的なブロック図である。本実施形態では、演算装置としての制御装置２８は、移動体１０の移動パラメータを設定するキャリブレーション処理を実行し、移動体１０の移動を制御する装置である。すなわち、本実施形態では、制御装置２８が、移動パラメータを設定する演算装置と、移動体１０の移動を制御する制御装置との両方の機能を兼ね備えている。ただし、後述のように、移動パラメータを設定する演算装置と、移動体１０の移動を制御する制御装置とが別体であってよい。この場合、移動パラメータを設定する演算装置は、移動体１０に搭載されず、移動体１０とは別の場所に配置されていてよい。なお、移動パラメータについては後述する。
図２は、制御装置の模式的なブロック図である。本実施形態では、演算装置としての制御装置２８は、移動体１０の移動パラメータを設定するキャリブレーション処理を実行し、移動体１０の移動を制御する装置である。すなわち、本実施形態では、制御装置２８が、移動パラメータを設定する演算装置と、移動体１０の移動を制御する制御装置との両方の機能を兼ね備えている。ただし、後述のように、移動パラメータを設定する演算装置と、移動体１０の移動を制御する制御装置とが別体であってよい。この場合、移動パラメータを設定する演算装置は、移動体１０に搭載されず、移動体１０とは別の場所に配置されていてよい。なお、移動パラメータについては後述する。
【0018】
制御装置２８は、コンピュータであり、図２に示すように、通信部３０と記憶部３２と制御部３４とを含む。通信部３０は、制御部３４に用いられて、外部の装置と通信するモジュールであり、例えばＷｉＦｉ（登録商標）モジュールやアンテナなどを含んでよい。通信部３０による通信方式は、本実施形態では無線通信であるが、通信方式は任意であってよい。記憶部３２は、制御部３４の演算内容やプログラムなどの各種情報を記憶するメモリであり、例えば、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）と、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）のような主記憶装置と、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）などの外部記憶装置とのうち、少なくとも１つ含む。
【0019】
制御部３４は、演算装置であり、例えばＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）などの演算回路を含む。制御部３４は、移動制御部４０と、第１位置算出部４２と、第２位置算出部４４と、パラメータ算出部４６と、判定部４８と、パラメータ設定部５０とを含む。制御部３４は、記憶部３２からプログラム（ソフトウェア）を読み出して実行することで、移動制御部４０と第１位置算出部４２と第２位置算出部４４とパラメータ算出部４６と判定部４８とパラメータ設定部５０とを実現して、それらの処理を実行する。なお、制御部３４は、１つのＣＰＵによってこれらの処理を実行してもよいし、複数のＣＰＵを備えて、それらの複数のＣＰＵで、処理を実行してもよい。また、移動制御部４０と第１位置算出部４２と第２位置算出部４４とパラメータ算出部４６と判定部４８とパラメータ設定部５０との少なくとも一部を、ハードウェア回路で実現してもよい。また、記憶部３２が保存する制御部３４用のプログラムは、制御装置２８（演算装置）が読み取り可能な記録媒体に記憶されていてもよい。
【0020】
（移動制御部）
移動制御部４０は、移動体１０の移動を制御する。移動制御部４０は、移動体１０が移動する経路を設定する。移動制御部４０は、移動体１０の移動経路を任意の方法で設定してよく、例えば、移動体１０の移動先などを設定するシステム（装置）から、移動経路の情報を取得してもよいし、そのシステムから取得した移動先の情報に基づき、自身で移動経路を設定してもよい。移動制御部４０は、移動体１０の駆動機構に対して制御入力を行うことで駆動機構を駆動させ、設定した移動経路に沿って移動体１０を移動させる。また、移動制御部４０は、外界センサ２６Ａ及び内界センサ２６Ｂを制御して、外界センサ２６Ａに周囲を検出させつつ、内界センサ２６Ｂに出力パラメータを検出させる。移動制御部４０は、外界センサ２６Ａに所定時間毎に周囲を検出させて、内界センサ２６Ｂに所定時間毎に出力パラメータを検出させる。
移動制御部４０は、移動体１０の移動を制御する。移動制御部４０は、移動体１０が移動する経路を設定する。移動制御部４０は、移動体１０の移動経路を任意の方法で設定してよく、例えば、移動体１０の移動先などを設定するシステム（装置）から、移動経路の情報を取得してもよいし、そのシステムから取得した移動先の情報に基づき、自身で移動経路を設定してもよい。移動制御部４０は、移動体１０の駆動機構に対して制御入力を行うことで駆動機構を駆動させ、設定した移動経路に沿って移動体１０を移動させる。また、移動制御部４０は、外界センサ２６Ａ及び内界センサ２６Ｂを制御して、外界センサ２６Ａに周囲を検出させつつ、内界センサ２６Ｂに出力パラメータを検出させる。移動制御部４０は、外界センサ２６Ａに所定時間毎に周囲を検出させて、内界センサ２６Ｂに所定時間毎に出力パラメータを検出させる。
【0021】
（第１位置算出部）
図３は、推定位置の算出の一例を説明する模式図である。第１位置算出部４２は、外界センサ２６Ａの検出結果に基づいて、移動体１０の第１推定位置を算出する。さらに言えば、第１位置算出部４２は、外界センサ２６Ａの検出結果と、後述の移動パラメータΦの設定値（本例では移動体１０の基準座標系における外界センサ２６Ａの位置）とに基づいて、移動体１０の第１推定位置を算出する。第１推定位置とは、外界センサ２６Ａの検出結果により推定された、移動体１０の位置を指す。第１位置算出部４２は、移動体１０の移動中における外界センサ２６Ａの所定時間毎の検出結果を取得して、所定時間毎の検出結果から、所定時間毎の第１推定位置を算出する。以下、図３に基づいてより具体的に説明する。
図３は、推定位置の算出の一例を説明する模式図である。第１位置算出部４２は、外界センサ２６Ａの検出結果に基づいて、移動体１０の第１推定位置を算出する。さらに言えば、第１位置算出部４２は、外界センサ２６Ａの検出結果と、後述の移動パラメータΦの設定値（本例では移動体１０の基準座標系における外界センサ２６Ａの位置）とに基づいて、移動体１０の第１推定位置を算出する。第１推定位置とは、外界センサ２６Ａの検出結果により推定された、移動体１０の位置を指す。第１位置算出部４２は、移動体１０の移動中における外界センサ２６Ａの所定時間毎の検出結果を取得して、所定時間毎の検出結果から、所定時間毎の第１推定位置を算出する。以下、図３に基づいてより具体的に説明する。
【0022】
ここで、ある時刻（図３の例では時刻ｉ－１）からその後の時刻（図３の例では時刻ｉ）までの外界センサ２６Ａの移動方向及び移動量（図３の例では、移動ベクトルｆ（ｚｉ））は、時刻ｉにおいて外界センサ２６Ａに検出された対象物の位置と、時刻ｉ－１において外界センサ２６Ａに検出された同じ対象物の位置との差分に相当する。従って、第１位置算出部４２は、時刻ｉにおいて外界センサ２６Ａに検出された対象物の位置と、時刻ｉ－１において外界センサ２６Ａに検出された同じ対象物の位置とに基づいて、時刻ｉ－１から時刻ｉまでの外界センサ２６Ａの移動方向及び移動量を算出できる。例えば、第１位置算出部４２は、時刻ｉ－１での対象物の位置から、時刻ｉでの同じ対象物の位置までの変位量及び変位方向を、時刻ｉ－１から時刻ｉまでの外界センサ２６Ａの移動方向及び移動量として算出する。なお、同じ対象物であるかは任意の方法で判定できるが、例えば外界センサ２６ＡがＬｉＤＡＲの場合、第１位置算出部４２は、スキャンマッチングを行うことで、時刻ｉ－１における点群と時刻ｉにおける点群のうちから、同一対象物を検出した点群（検出点）を抽出する。そして、第１位置算出部４２は、抽出した時刻ｉ－１における点群の位置と、抽出した時刻ｉにおける点群の位置とに基づいて、時刻ｉ－１から時刻ｉまでの外界センサ２６Ａの移動方向及び移動量を算出する。また例えば、外界センサ２６Ａがカメラの場合、第１位置算出部４２は、画像認識によって、時刻ｉ－１に撮像された画像と時刻ｉに撮像された画像のうちから、同じ対象物が写っている領域（画素群）を特定する。そして、第１位置算出部４２は、時刻ｉ－１において特定した領域の位置と、時刻ｉにおいて特定した領域の位置とに基づいて、時刻ｉ－１から時刻ｉまでの外界センサ２６Ａの移動方向及び移動量を算出する。
【0023】
第１位置算出部４２は、このようにして算出した時刻ｉ－１から時刻ｉまでの外界センサ２６Ａの移動方向及び移動量（移動ベクトルｆ（ｚｉ））と、移動体１０の基準座標系における外界センサ２６Ａの位置（図３の例では外界センサ２６Ａから基準点ＰまでのベクトルΦ’）に基づいて、時刻ｉ－１から時刻ｉまでの移動体１０の基準点Ｐの移動方向及び移動量（図３の例では、移動ベクトルｆΦ’（ｚｉ））を算出する。言い換えれば、第１位置算出部４２は、移動ベクトルｆ（ｚｉ）から、ベクトルΦ’だけずらしたベクトルを、時刻ｉ－１から時刻ｉまでの基準点Ｐの移動方向及び移動量（移動ベクトルｆΦ’（ｚｉ））として算出する。なお、移動体１０の基準座標系とは、移動体１０の基準点Ｐを基準とした座標系であり、基準点Ｐは任意の位置に設定されてよい。移動体１０の基準座標系における外界センサ２６Ａの位置は、移動体１０の基準点Ｐに対する外界センサ２６Ａの位置ともいえ、例えば設計情報などから、予め設定されてよい。
【0024】
第１位置算出部４２は、このようにして算出した、時刻ｉ－１から時刻ｉまでの基準点Ｐの移動方向及び移動量（移動ベクトルｆΦ’（ｚｉ））を、時刻ｉにおける移動体１０の第１推定位置とする。ただしそれに限られず、第１位置算出部４２は、所定位置（図３の例では位置Ｐｉ－１）から移動ベクトルｆΦ’（ｚｉ）だけ離れた座標（図３の例では位置ＰＡｉ）そのものを、第１推定位置としてもよい。
【0025】
第１位置算出部４２は、所定時間毎に検出された外界センサ２６Ａの検出結果（対象物の位置）に基づき、所定時間毎の、第１推定位置を算出する。すなわち例えば、第１位置算出部４２は、時刻ｉにおける外界センサ２６Ａの検出結果と、時刻ｉ＋１における外界センサ２６Ａの検出結果とから、上記と同様の方法で、時刻ｉ＋１における第１推定位置を算出する。このように、本実施形態では、移動体１０を移動させつつ、移動中の時刻を一定の規則に従い離散的にサンプリングして、各サンプル時刻における、外界センサ２６Ａの検出により求めた移動体１０の位置及び姿勢（第１推定位置）をノードとし、ノード間をエッジで結んだグラフ（ポーズグラフ）を構築するといえる。
【0026】
（第２位置算出部）
第２位置算出部４４は、内界センサ２６Ｂの検出結果に基づいて、移動体１０の第２推定位置を算出する。第２推定位置とは、内界センサ２６Ｂの検出結果により推定された、移動体１０の位置を指す。第２位置算出部４４は、移動体１０の移動中における内界センサ２６Ｂの所定時間毎の検出結果を取得する。第２位置算出部４４は、移動体１０の移動中における内界センサ２６Ｂの所定時間毎の検出結果から、所定時間毎の第２推定位置を算出する。以下、図３に基づいてより具体的に説明する。
第２位置算出部４４は、内界センサ２６Ｂの検出結果に基づいて、移動体１０の第２推定位置を算出する。第２推定位置とは、内界センサ２６Ｂの検出結果により推定された、移動体１０の位置を指す。第２位置算出部４４は、移動体１０の移動中における内界センサ２６Ｂの所定時間毎の検出結果を取得する。第２位置算出部４４は、移動体１０の移動中における内界センサ２６Ｂの所定時間毎の検出結果から、所定時間毎の第２推定位置を算出する。以下、図３に基づいてより具体的に説明する。
【0027】
第２位置算出部４４は、内界センサ２６Ｂが検出した出力パラメータに基づいて、移動体１０の移動量及び移動方向を算出する。例えば、第２位置算出部４４は、時刻ｉ－１において内界センサ２６Ｂに検出された出力パラメータに基づき、時刻ｉ－１から時刻ｉまでの移動体１０の基準点Ｐの移動方向及び移動量（図３の例では、移動ベクトルαｕｉ）を算出する。第２位置算出部４４は、出力パラメータを積分することにより、オドメトリの手法で、基準点Ｐの移動方向及び移動量を算出する。すなわち例えば、第２位置算出部４４は、時刻ｉ－１で検出された車輪２０Ａの回転数を積分して、時刻ｉ－１から時刻ｉまでの基準点Ｐの移動量を算出し、時刻ｉ－１で検出された操舵角（車輪２０Ａの向きの変化量）から、基準点Ｐの移動方向を算出して、時刻ｉ－１から時刻ｉまでの移動体１０の基準点Ｐの移動方向及び移動量とする。第２位置算出部４４は、このようにして算出した、時刻ｉ－１から時刻ｉまでの基準点Ｐの移動方向及び移動量（移動ベクトルαｕｉ）を、時刻ｉにおける移動体１０の第２推定位置とする。ただしそれに限られず、第２位置算出部４４は、所定位置（図３の例では位置Ｐｉ－１）から移動ベクトルαｕｉだけ離れた座標（図３の例では位置ＰＢｉ）そのものを、第２推定位置としてもよい。
【0028】
なお、第２位置算出部４４は、後述の移動パラメータΦの設定値にも基づいて、第２推定位置を算出してよい。例えば、第２位置算出部４４は、移動体１０の車輪２０Ａの径の設定値と、内界センサ２６Ｂのバイアス値（正常値からのずれ量）の設定値との少なくとも１つ（好ましくは両方）も用いて、第２推定位置を算出してよい。
【0029】
第２位置算出部４４は、所定時間毎に検出された内界センサ２６Ｂの検出結果（出力パラメータ）に基づき、所定時間毎の、第２推定位置を算出する。すなわち例えば、第２位置算出部４４は、時刻ｉにおける内界センサ２６Ｂの検出結果から、上記と同様の方法で、時刻ｉ＋１における第２推定位置を算出する。このように、本実施形態では、移動体１０を移動させつつ、移動中の時刻を一定の規則に従い離散的にサンプリングして、各サンプル時刻における、内界センサ２６Ｂの検出により求めた移動体１０の位置及び姿勢（第１推定位置）をノードとし、ノード間をエッジで結んだグラフ（ポーズグラフ）を構築するといえる。
【0030】
（パラメータ算出部）
パラメータ算出部４６は、第１推定位置及び第２推定位置に基づいて、移動体１０の移動量を算出するための移動パラメータΦの、推定値を算出する。移動パラメータΦとは、移動体１０の移動量に影響を及ぼすパラメータであるが、上述の出力パラメータとは異なり制御出力を示すものではなく、制御入力によって変動させることができないパラメータといえる。また、移動パラメータΦは、経時により変化する可能性があるパラメータともいえる。本実施形態の例では、移動パラメータΦとは、移動体１０の基準座標系における外界センサ２６Ａの位置（基準点Ｐに対する外界センサ２６Ａの位置）と、移動体１０の車輪径（車輪２０Ａの径）と、内界センサ２６Ｂの検出結果の正常値からのずれ量を示すバイアス値との、少なくとも１つであってよく、これら全てであってもよい。ただしそれに限られず、移動パラメータΦは、経時により変動する可能性があり、移動体１０の移動量に影響を及ぼすが制御入力によっては変動されない、任意のパラメータであってよい。
パラメータ算出部４６は、第１推定位置及び第２推定位置に基づいて、移動体１０の移動量を算出するための移動パラメータΦの、推定値を算出する。移動パラメータΦとは、移動体１０の移動量に影響を及ぼすパラメータであるが、上述の出力パラメータとは異なり制御出力を示すものではなく、制御入力によって変動させることができないパラメータといえる。また、移動パラメータΦは、経時により変化する可能性があるパラメータともいえる。本実施形態の例では、移動パラメータΦとは、移動体１０の基準座標系における外界センサ２６Ａの位置（基準点Ｐに対する外界センサ２６Ａの位置）と、移動体１０の車輪径（車輪２０Ａの径）と、内界センサ２６Ｂの検出結果の正常値からのずれ量を示すバイアス値との、少なくとも１つであってよく、これら全てであってもよい。ただしそれに限られず、移動パラメータΦは、経時により変動する可能性があり、移動体１０の移動量に影響を及ぼすが制御入力によっては変動されない、任意のパラメータであってよい。
【0031】
ここで、同じ時刻における第１推定位置と第２推定位置との差分（誤差）は、理想的な条件ではゼロである。しかしながら、第１推定位置や第２推定位置の算出時に用いたパラメータの誤差などにより、第１推定位置と第２推定位置との差分がゼロにならない場合がある。それに対して、本実施形態では、その差分が移動パラメータΦの誤差により生じていると仮定し、パラメータ算出部４６が、第１推定位置と第２推定位置との差分に基づいて、例えばその差分ができるだけ小さくなるような移動パラメータΦの値を、移動パラメータΦの推定値として算出する。すなわち、第１推定位置と第２推定位置とは、移動パラメータΦに基づいて算出されるため、言い換えれば移動パラメータΦの関数であるため、第１推定位置と第２推定位置との差分が小さくなるような移動パラメータΦの値を、移動パラメータΦの推定値として算出するといえる。
【0032】
具体的には、パラメータ算出部４６は、所定時間毎の第１推定位置と第２推定位置との差分に基づいて、移動パラメータΦの推定値を算出する。さらに言えば、パラメータ算出部４６は、所定時間毎の第１推定位置と第２推定位置との差分の合計値が最小となるように、言い換えれば、所定時間毎の第１推定位置と第２推定位置との差分の合計値が最小となるような最適化問題を解くことで、移動パラメータΦの推定値を算出する。具体的には、時刻ｉにおける第１推定位置と第２推定位置との差分をｅｉ、時刻ｉにおける定数をΩｉとすると、パラメータ算出部４６は、次の式（１）に示す移動パラメータΦの評価関数をＪＯ（Φ）が最小となるような移動パラメータΦを、移動パラメータΦの推定値として算出する。
【0033】
【数1】
【0034】
なお、Ｔは転置を指す。また、Ωｉは、任意に設定される重み係数であり、例えば、差分ｅｉの分散共分散行列の逆行列である。
【0035】
（判定部）
判定部４８は、パラメータ算出部４６によって算出された移動パラメータΦの推定値が妥当であるかを判定する。移動パラメータΦの値は、例えば自己位置検出などの移動体１０の移動制御において用いられるため、高精度に推定されることが好ましい。それに対して、本実施形態においては、判定部４８により、移動パラメータΦの推定値が妥当であるかを判定することで、移動パラメータΦを高精度に推定することを可能としている。
判定部４８は、パラメータ算出部４６によって算出された移動パラメータΦの推定値が妥当であるかを判定する。移動パラメータΦの値は、例えば自己位置検出などの移動体１０の移動制御において用いられるため、高精度に推定されることが好ましい。それに対して、本実施形態においては、判定部４８により、移動パラメータΦの推定値が妥当であるかを判定することで、移動パラメータΦを高精度に推定することを可能としている。
【0036】
判定部４８は、移動体１０の停止時における外界センサ２６Ａの検出結果に基づいて、移動パラメータΦの推定値が妥当であるかを判定する。すなわち、判定部４８は、移動体１０の停止時において検出された外界センサ２６Ａの検出結果を取得して、その検出結果に基づき、移動パラメータΦの推定値が妥当であるかを判定する。判定部４８は、停止時における外界センサ２６Ａの検出結果に基づいた任意の方法で、移動パラメータΦの推定値の妥当性を判定してよいが、以下に判定方法の具体例を説明する。
【0037】
判定部４８は、移動体１０の停止時における複数の外界センサ２６Ａの検出結果から、移動パラメータΦの推定値の妥当性を判定することが好ましい。すなわち、判定部４８は、移動体１０の停止時において、複数の外界センサ２６Ａに、移動体１０の周囲の検出体Ａを検出させて、それらの検出体Ａの検出結果に基づいて、推定値の妥当性を判断する。以降では、複数の外界センサ２６Ａとして、第１外界センサ２６Ａ１及び第２外界センサ２６Ａ２を用いる場合を例として説明するが、用いる外界センサ２６Ａの数は２つに限られず３つ以上であってもよい。検出体Ａは、外界センサ２６Ａが検出可能な任意の物体であってよく、例えばレーザ光を反射可能な反射体であってもよいし、停止している移動体１０の周囲に位置している構造物であってもよい。外界センサ２６Ａは、推定値の妥当性を判断するために設けられた検出体Ａを検出してもよいし、停止している移動体１０の周囲に位置している任意の物体を検出体Ａとして利用してもよい。
【0038】
図４は、移動パラメータの推定値の妥当性の判定方法の例を説明する模式図である。図４に示すように、判定部４８は、移動体１０の停止時において第１外界センサ２６Ａ１により検出された検出体Ａの検出結果を取得して、検出体Ａの検出結果と、移動パラメータΦの推定値とに基づいて、移動体１０の基準座標系における検出体Ａの位置を算出する。すなわち、判定部４８は、検出体Ａの検出結果が示す、第１外界センサ２６Ａ１の座標系における検出体Ａの位置（第１外界センサ２６Ａ１に対する検出体Ａの位置）と、移動パラメータΦの推定値が示す、移動体１０の基準座標系における第１外界センサ２６Ａ１の位置（移動体１０の基準点Ｐに対する第１外界センサ２６Ａ１の位置）とから、移動体１０の基準座標系における検出体Ａの位置（移動体１０の基準点Ｐに対する検出体Ａの位置）を算出する。このようにして第１外界センサ２６Ａ１の検出結果から算出された移動体１０の基準座標系における検出体Ａの位置を、以下適宜、第１検出位置とする。同様に、判定部４８は、移動体１０の停止時において第２外界センサ２６Ａ２により検出された同じ検出体Ａの検出結果と、移動パラメータΦの推定値とに基づいて、移動体１０の基準座標系における検出体Ａの位置を算出する。すなわち、判定部４８は、検出体Ａの検出結果が示す、第２外界センサ２６Ａ２の座標系における検出体Ａの位置（第２外界センサ２６Ａ２に対する検出体Ａの位置）と、移動パラメータΦの推定値が示す、移動体１０の基準座標系における第２外界センサ２６Ａ２の位置（移動体１０の基準点Ｐに対する第２外界センサ２６Ａ２の位置）とから、移動体１０の基準座標系における検出体Ａの位置を算出する。このようにして第２外界センサ２６Ａ２の検出結果から算出された移動体１０の基準座標系における検出体Ａの位置を、以下適宜、第２検出位置とする。
【0039】
このように、第１検出位置と第２検出位置とは、異なる外界センサ２６Ａにより検出された同じ検出体Ａの位置を指す。同じ検出体Ａであるかは任意の方法で判定できるが、例えば外界センサ２６ＡがＬｉＤＡＲの場合、判定部４８は、スキャンマッチングを行うことで、第１外界センサ２６Ａ１が検出した点群と第２外界センサ２６Ａ２が検出した点群のうちから、同一の対象を検出した点群（検出点）を抽出する。そして、判定部４８は、抽出した第１外界センサ２６Ａ１の点群の位置を、第１外界センサ２６Ａ１の座標系における検出体Ａの位置とし、抽出した第２外界センサ２６Ａ２の点群の位置を、第２外界センサ２６Ａ２の座標系における検出体Ａの位置とする。また例えば、外界センサ２６Ａがカメラの場合、判定部４８は、画像認識によって、第１外界センサ２６Ａ１が撮像した画像と第２外界センサ２６Ａ２が撮像した画像のうちから、同じ対象物が写っている領域（画素群）を特定する。そして、判定部４８は、第１外界センサ２６Ａ１の撮像画像から特定した領域の位置を、第１外界センサ２６Ａ１の座標系における検出体Ａの位置とし、第２外界センサ２６Ａ２の撮像画像から特定した領域の位置を、第２外界センサ２６Ａ２の座標系における検出体Ａの位置とする。
【0040】
判定部４８は、第１検出位置と第２検出位置とに基づいて、移動パラメータΦの推定値の妥当性を判断する。すなわち、理想的には、第１検出位置と第２検出位置との差分（誤差）はゼロであるが、移動パラメータΦの推定値の精度が低い場合には、第１検出位置と第２検出位置との差分が大きくなる。従って、判定部４８は、第１検出位置と第２検出位置とのずれ量が所定値以下である場合には、言い換えれば、第１検出位置と第２検出位置との間の距離が所定距離以下である場合には、移動パラメータΦの推定値が妥当であると判断する。一方、判定部４８は、第１検出位置と第２検出位置とのずれ量が所定値より大きい場合には、移動パラメータΦの推定値が妥当でないと判断する。
【0041】
次に、妥当性の判定方法の他の例を説明する図５は、移動パラメータの推定値の妥当性の判定方法の例を説明する模式図である。本例の場合、図５に示すように、判定部４８は、移動体１０の停止時において、第１外界センサ２６Ａ１に、異なる位置にある複数の検出体Ａを検出させる。なお、検出体Ａの数は複数であれば任意であってよい。判定部４８は、第１外界センサ２６Ａ１による、複数の検出体Ａの検出結果を取得して、複数の検出体Ａの検出結果に基づいて、第１外界センサ２６Ａ１の位置を算出する。複数の検出体Ａの検出結果からの第１外界センサ２６Ａ１の位置の算出方法は任意であるが、例えば、判定部４８は、複数の検出体Ａの位置から、三角測量の手法を用いて、第１外界センサ２６Ａ１の位置を算出してよい。同様に、判定部４８は、移動体１０の停止時において、第２外界センサ２６Ａ２に、異なる位置にある複数の検出体Ａを検出させる。判定部４８は、第２外界センサ２６Ａ２による、複数の検出体Ａの検出結果を取得して、複数の検出体Ａの検出結果に基づいて、第２外界センサ２６Ａ２の位置を算出する。なお、本例では、第１外界センサ２６Ａ１と第２外界センサ２６Ａ２とは、同じ検出体Ａを検出することが好ましいが、それに限られず、異なる検出体Ａを検出してもよい。
【0042】
判定部４８は、複数の検出体Ａの検出結果から算出した第１外界センサ２６Ａ１の位置と、複数の検出体Ａの検出結果から算出した第２外界センサ２６Ａ２の位置とに基づき、第１外界センサ２６Ａ１に対する第２外界センサ２６Ａ２の位置を算出する。以下、このようにして算出した第１外界センサ２６Ａ１に対する第２外界センサ２６Ａ２の位置を、適宜、第１センサ位置と記載する。また、判定部４８は、移動パラメータΦの推定値から、第１外界センサ２６Ａ１に対する第２外界センサ２６Ａ２の位置を算出する。具体的には、判定部４８は、移動パラメータΦの推定値で示される移動体１０の基準座標系における第１外界センサ２６Ａ１の位置と、移動パラメータΦの推定値で示される移動体１０の基準座標系における第２外界センサ２６Ａ２の位置とから、第１外界センサ２６Ａ１に対する第２外界センサ２６Ａ２の位置を算出する。以下、このようにして算出した第１外界センサ２６Ａ１に対する第２外界センサ２６Ａ２の位置を、適宜、第２センサ位置と記載する。
【0043】
判定部４８は、第１センサ位置と第２センサ位置とに基づいて、移動パラメータΦの推定値の妥当性を判断する。すなわち、理想的には、第１センサ位置と第２センサ位置の差分（誤差）はゼロであるが、移動パラメータΦの推定値の精度が低い場合には、第１センサ位置と第２センサ位置との差分が大きくなる。従って、判定部４８は、第１センサ位置と第２センサ位置とのずれ量が所定値以下である場合には、言い換えれば、第１センサ位置と第２センサ位置との間の距離が所定距離以下である場合には、移動パラメータΦの推定値が妥当であると判断する。一方、判定部４８は、第１センサ位置と第２センサ位置とのずれ量が所定値より大きい場合には、移動パラメータΦの推定値が妥当でないと判断する。
【0044】
（パラメータ設定部）
パラメータ設定部５０は、判定部４８によって妥当であると判断された移動パラメータΦの推定値を、移動パラメータΦの値として設定する。すなわち、パラメータ設定部５０は、判定部４８によって妥当であると判断された移動パラメータΦの推定値を、今後の移動体１０の制御に用いる正式な移動パラメータΦとして設定する。言い換えれば、パラメータ設定部５０は、移動パラメータΦを更新するキャリブレーション処理を行うといえる。移動制御部４０は、パラメータ設定部５０によって設定された移動パラメータΦ（妥当とされた移動パラメータΦの推定値）に基づいて、移動体１０の移動を制御する。すなわち、移動制御部４０は、パラメータ設定部５０によって設定された移動パラメータΦを入力値として演算を実行することで、移動体１０の移動を制御する。
パラメータ設定部５０は、判定部４８によって妥当であると判断された移動パラメータΦの推定値を、移動パラメータΦの値として設定する。すなわち、パラメータ設定部５０は、判定部４８によって妥当であると判断された移動パラメータΦの推定値を、今後の移動体１０の制御に用いる正式な移動パラメータΦとして設定する。言い換えれば、パラメータ設定部５０は、移動パラメータΦを更新するキャリブレーション処理を行うといえる。移動制御部４０は、パラメータ設定部５０によって設定された移動パラメータΦ（妥当とされた移動パラメータΦの推定値）に基づいて、移動体１０の移動を制御する。すなわち、移動制御部４０は、パラメータ設定部５０によって設定された移動パラメータΦを入力値として演算を実行することで、移動体１０の移動を制御する。
【0045】
（処理フロー）
次に、以上説明した移動パラメータΦの設定の処理フローを説明する。図６は、本実施形態に係る移動パラメータの設定の処理フローを説明するフローチャートである。図６に示すように、制御装置２８は、第１位置算出部４２により、所定時間毎の外界センサ２６Ａの検出結果に基づき、所定時間毎の移動体１０の第１推定位置を算出し（ステップＳ１０）、第２位置算出部４４により、所定時間毎の内界センサ２６Ｂの検出結果に基づき、所定時間毎の移動体１０の第２推定位置を算出する（ステップＳ１２）。制御装置２８は、パラメータ算出部４６により、第１推定位置及び第２推定位置に基づいて、移動パラメータΦの推定値を算出する（ステップＳ１４）。本実施形態では、パラメータ算出部４６は、所定時間毎の第１推定位置と第２推定位置との差分が最小になるように、移動パラメータΦの推定値を算出する。制御装置２８は、判定部４８により、移動体１０の停止時における外界センサ２６Ａの検出結果に基づいて、移動パラメータΦの推定値が妥当であるかを判断し（ステップＳ１６）、妥当である場合（ステップＳ１６；Ｙｅｓ）、パラメータ設定部５０により、その移動パラメータΦの推定値を、正式な移動パラメータΦとして設定する（ステップＳ１８）。一方、移動パラメータΦの推定値が妥当でない場合（ステップＳ１６；Ｎｏ）、その移動パラメータΦの推定値を正式な移動パラメータΦとして設定しない（ステップＳ２０）。なお、移動パラメータΦの推定値を採用しない場合には、以降の移動においては、これまでに設定されていた移動パラメータΦの値を用いてよい。その後、外界センサ２６Ａや内界センサ２６Ｂから、以降の移動における検出結果を取得したら、同様の処理を実行して、移動パラメータΦの推定値の算出と妥当性判断とを行ってよい。
次に、以上説明した移動パラメータΦの設定の処理フローを説明する。図６は、本実施形態に係る移動パラメータの設定の処理フローを説明するフローチャートである。図６に示すように、制御装置２８は、第１位置算出部４２により、所定時間毎の外界センサ２６Ａの検出結果に基づき、所定時間毎の移動体１０の第１推定位置を算出し（ステップＳ１０）、第２位置算出部４４により、所定時間毎の内界センサ２６Ｂの検出結果に基づき、所定時間毎の移動体１０の第２推定位置を算出する（ステップＳ１２）。制御装置２８は、パラメータ算出部４６により、第１推定位置及び第２推定位置に基づいて、移動パラメータΦの推定値を算出する（ステップＳ１４）。本実施形態では、パラメータ算出部４６は、所定時間毎の第１推定位置と第２推定位置との差分が最小になるように、移動パラメータΦの推定値を算出する。制御装置２８は、判定部４８により、移動体１０の停止時における外界センサ２６Ａの検出結果に基づいて、移動パラメータΦの推定値が妥当であるかを判断し（ステップＳ１６）、妥当である場合（ステップＳ１６；Ｙｅｓ）、パラメータ設定部５０により、その移動パラメータΦの推定値を、正式な移動パラメータΦとして設定する（ステップＳ１８）。一方、移動パラメータΦの推定値が妥当でない場合（ステップＳ１６；Ｎｏ）、その移動パラメータΦの推定値を正式な移動パラメータΦとして設定しない（ステップＳ２０）。なお、移動パラメータΦの推定値を採用しない場合には、以降の移動においては、これまでに設定されていた移動パラメータΦの値を用いてよい。その後、外界センサ２６Ａや内界センサ２６Ｂから、以降の移動における検出結果を取得したら、同様の処理を実行して、移動パラメータΦの推定値の算出と妥当性判断とを行ってよい。
【0046】
（効果）
ここで、移動体１０を適切に制御するために、移動体１０の移動量を推定するための移動パラメータΦを高精度に算出することが求められている。移動パラメータΦは、定期メンテナンス時に計測されて更新される場合もあるが、経時的に値が変化する場合もあるため、定期メンテナンスまでの間に値が変わってしまう場合がある。それに対して、本実施形態においては、移動体１０が移動中の外界センサ２６Ａと内界センサ２６Ｂとの検出結果から、移動パラメータΦの推定値を算出するため、定期メンテナンスを待たずに移動パラメータΦを更新することができ、移動パラメータΦを高精度に算出できるといえる。さらに言えば、このようにして算出した移動パラメータΦは、局所最適解に陥ったり、検出タイミング（タイムスタンプ）の誤差が生じたりすることなどにより、精度が低下することも懸念される。それに対して、本実施形態においては、停止時における外界センサ２６Ａの検出結果から、移動パラメータΦの妥当性を判断して、妥当であるものを採用する。従って、局所最適解への収束を検知し、かつ、タイムスタンプの誤差を抑制できるので、移動パラメータΦをより高精度に算出でき、キャリブレーションを適切に実行することが可能となる。
ここで、移動体１０を適切に制御するために、移動体１０の移動量を推定するための移動パラメータΦを高精度に算出することが求められている。移動パラメータΦは、定期メンテナンス時に計測されて更新される場合もあるが、経時的に値が変化する場合もあるため、定期メンテナンスまでの間に値が変わってしまう場合がある。それに対して、本実施形態においては、移動体１０が移動中の外界センサ２６Ａと内界センサ２６Ｂとの検出結果から、移動パラメータΦの推定値を算出するため、定期メンテナンスを待たずに移動パラメータΦを更新することができ、移動パラメータΦを高精度に算出できるといえる。さらに言えば、このようにして算出した移動パラメータΦは、局所最適解に陥ったり、検出タイミング（タイムスタンプ）の誤差が生じたりすることなどにより、精度が低下することも懸念される。それに対して、本実施形態においては、停止時における外界センサ２６Ａの検出結果から、移動パラメータΦの妥当性を判断して、妥当であるものを採用する。従って、局所最適解への収束を検知し、かつ、タイムスタンプの誤差を抑制できるので、移動パラメータΦをより高精度に算出でき、キャリブレーションを適切に実行することが可能となる。
【0047】
（他の例）
図７は、第１実施形態の他の例に係る演算装置の模式的なブロック図である。以上の説明では、制御装置２８が、移動パラメータを設定する演算装置と、移動体１０の移動を制御する制御装置との両方の機能を兼ね備えていた。ただし、移動パラメータを設定する演算装置と、移動体１０の移動を制御する制御装置とが別体であってよい。この場合、図７に示すように、移動体１０に搭載されない演算装置２８Ａが、第１位置算出部４２と第２位置算出部４４とパラメータ算出部４６と判定部４８とパラメータ設定部５０とを含み、それらの処理を実行する。この場合、演算装置２８Ａは、通信部３０を介して、移動体１０から、外界センサ２６Ａ及び内界センサ２６Ｂの検出結果を取得して、それらの検出結果から、移動パラメータΦの推定値の設定と妥当性の判断を行う。そして、演算装置２８Ａは、妥当であると判断された移動パラメータΦの推定値を、正式な移動パラメータΦとして移動体１０に送信する。
図７は、第１実施形態の他の例に係る演算装置の模式的なブロック図である。以上の説明では、制御装置２８が、移動パラメータを設定する演算装置と、移動体１０の移動を制御する制御装置との両方の機能を兼ね備えていた。ただし、移動パラメータを設定する演算装置と、移動体１０の移動を制御する制御装置とが別体であってよい。この場合、図７に示すように、移動体１０に搭載されない演算装置２８Ａが、第１位置算出部４２と第２位置算出部４４とパラメータ算出部４６と判定部４８とパラメータ設定部５０とを含み、それらの処理を実行する。この場合、演算装置２８Ａは、通信部３０を介して、移動体１０から、外界センサ２６Ａ及び内界センサ２６Ｂの検出結果を取得して、それらの検出結果から、移動パラメータΦの推定値の設定と妥当性の判断を行う。そして、演算装置２８Ａは、妥当であると判断された移動パラメータΦの推定値を、正式な移動パラメータΦとして移動体１０に送信する。
【0048】
（第２実施形態）
次に、第２実施形態について説明する。第２実施形態においては、内界センサ２６Ｂのバイアス値を算出して、それに基づいて移動パラメータΦを算出する点で、第１実施形態と異なる。第２実施形態において、第１実施形態と構成が共通する箇所は、説明を省略する。
次に、第２実施形態について説明する。第２実施形態においては、内界センサ２６Ｂのバイアス値を算出して、それに基づいて移動パラメータΦを算出する点で、第１実施形態と異なる。第２実施形態において、第１実施形態と構成が共通する箇所は、説明を省略する。
【0049】
図８は、バイアス値の一例を示すグラフである。第２実施形態においては、パラメータ算出部４６は、移動体１０の停止時に、内界センサ２６Ｂに出力パラメータを検出させる。そして、パラメータ算出部４６は、移動体１０の停止時における内界センサ２６Ｂの検出結果（出力パラメータ）に基づいて、内界センサ２６Ｂの検出結果のバイアス値を算出する。バイアス値とは、内界センサ２６Ｂの検出結果の、正常値からのずれ量を示す。パラメータ算出部４６は、移動体１０の停止時における内界センサ２６Ｂの検出結果に基づいた任意の方法でバイアス値を算出してよいが、例えば、移動体１０の停止時における正常値を予め設定しておき、停止時における内界センサ２６Ｂの検出結果（出力パラメータ）と正常値との差分から、バイアス値を算出してよい。この場合例えば、図８の例では、時間帯Δｔが、移動体１０が停止している時間帯を示している。この場合、パラメータ算出部４６は、時間帯Δｔにおける内界センサ２６Ｂによる出力パラメータの検出値と、時間帯Δｔにおける出力パラメータの正常値（例えばゼロ）との差分ΔＢを算出し、その差分ΔＢを、バイアス値としてよい。
【0050】
パラメータ算出部４６は、第１推定位置及び第２推定位置に加えて、このようにして算出したバイアス値にも基づいて、移動パラメータΦの推定値を算出する。例えば第１実施形態においては、パラメータ算出部４６は、内界センサ２６Ｂのバイアス値を、算出対象となる移動パラメータΦに含めて、第１推定位置及び第２推定位置により、内界センサ２６Ｂのバイアス値も、移動パラメータΦとして算出することが可能である。それに対して、第２実施形態においては、移動パラメータΦの推定値の算出の前に、予め内界センサ２６Ｂのバイアス値を固定値として設定しておき、第１推定位置及び第２推定位置と、固定値として設定した内界センサ２６Ｂのバイアス値とに基づいて、第１実施形態と同様の方法で、内界センサ２６Ｂのバイアス値以外の移動パラメータΦについての、推定値を算出する。これにより、算出対象となる変数を減らすことができるので、移動パラメータΦの推定値の演算負荷を低減できる。
【0051】
なお、内界センサ２６Ｂのバイアス値は、例えば、ＩＭＵの測定結果のバイアス値を指してよい。
【0052】
（第３実施形態）
次に、第３実施形態について説明する。第３実施形態においては、停止時における外界センサ２６Ａの検出結果から妥当であると判断され、かつ、過去における移動パラメータΦの推定値に対して所定条件を満たす場合に、移動パラメータΦの推定値が妥当であるかを判断する点で、第１実施形態とは異なる。第２実施形態において、第１実施形態と構成が共通する箇所は、説明を省略する。なお、第３実施形態は、第２実施形態にも適用可能である。
次に、第３実施形態について説明する。第３実施形態においては、停止時における外界センサ２６Ａの検出結果から妥当であると判断され、かつ、過去における移動パラメータΦの推定値に対して所定条件を満たす場合に、移動パラメータΦの推定値が妥当であるかを判断する点で、第１実施形態とは異なる。第２実施形態において、第１実施形態と構成が共通する箇所は、説明を省略する。なお、第３実施形態は、第２実施形態にも適用可能である。
【0053】
第３実施形態においては、第１実施形態などで説明した移動パラメータΦの推定値の算出処理と、推定値の妥当性の判断とを、繰り返す。以降においては、第１時間帯におけるセンサの検出結果から、第１時間帯における移動パラメータΦの推定値を算出し、第１時間帯よりも後の第２時間帯におけるセンサの検出結果から、第２時間帯における移動パラメータΦの推定値を算出する場合を例に説明する。
【0054】
第３実施形態においては、第１位置算出部４２は、移動体１０が移動している第１時間帯において検出された、外界センサ２６Ａの検出結果を取得して、第１時間帯での外界センサ２６Ａの検出結果に基づいて、第１時間帯での第１推定位置を算出する。また、第２位置算出部４４は、第１時間帯において検出された内界センサ２６Ｂの検出結果を取得して、第１時間帯での内界センサ２６Ｂの検出結果に基づいて、第１時間帯での第２推定位置を算出する。そして、パラメータ算出部４６は、第１時間帯での第１推定位置と、第１時間帯での第２推定位置とに基づき、第１時間帯での移動パラメータΦの推定値を算出する。判定部４８は、停止時の外界センサ２６Ａの検出結果に基づき、第１時間帯での移動パラメータΦの推定値の仮の妥当性を判断する。仮の妥当性の判定方法は、第１実施形態の妥当性の判断方法と同様である。
【0055】
その後、第３実施形態においては、第１位置算出部４２は、第２時間帯において検出された外界センサ２６Ａの検出結果を取得して、第２時間帯での外界センサ２６Ａの検出結果に基づいて、第２時間帯での第１推定位置を算出する。なお、第２時間帯は、第１時間帯より後であって移動体１０が移動した時間帯である。第２位置算出部４４は、第２時間帯において検出された内界センサ２６Ｂの検出結果を取得して、第２時間帯での内界センサ２６Ｂの検出結果に基づいて、第２時間帯での第２推定位置を算出する。そして、パラメータ算出部４６は、第２時間帯での第１推定位置と、第２時間帯での第２推定位置とに基づき、第２時間帯での移動パラメータΦの推定値を算出する。判定部４８は、停止時の外界センサ２６Ａの検出結果に基づき、第２時間帯での移動パラメータΦの推定値の仮の妥当性を判断する。
【0056】
さらに、判定部４８は、複数の外界センサ２６Ａの検出結果に基づき、第２時間帯での移動パラメータΦの推定値が仮の妥当ありと判断された場合には、その第２時間帯での移動パラメータΦの推定値と、仮の妥当ありと判断された第１時間帯での移動パラメータΦの推定値とを比較する。具体的には、判定部４８は、第２時間帯での移動パラメータΦの推定値が、第１時間帯での移動パラメータΦの推定値に対して所定条件を満たすかを判断する。例えば、移動パラメータΦは経時により変化するため、判定部４８は、移動パラメータΦの単位時間当たりの許容される変化量の範囲を予め設定しておく。そして、判定部４８は、単位時間当たりの許容される変化量の範囲から、第１時間帯から第２時間帯における許容変化量の範囲を算出して、第１時間帯での移動パラメータΦの推定値に対する、第２時間帯での移動パラメータΦの推定値の変化量が、許容変化量の範囲内にあるかを判断する。判定部４８は、第２時間帯での移動パラメータΦの推定値の変化量が、許容変化量の範囲内にある場合に、所定条件を満たすと判断し、第２時間帯での移動パラメータΦの推定値の変化量が、許容変化量の範囲内にない場合に、所定条件を満たさないと判断する。
【0057】
判定部４８は、第２時間帯での移動パラメータΦの推定値が、第１時間帯での移動パラメータΦの推定値に対して所定条件を満たすと判断した場合に、第２時間帯での移動パラメータΦの推定値が妥当であると最終判定する。パラメータ設定部５０は、妥当であると判断された第２時間帯での移動パラメータΦの推定値を、移動パラメータΦとして正式に設定する。一方、判定部４８は、第２時間帯での移動パラメータΦの推定値が、第１時間帯での移動パラメータΦの推定値に対して所定条件を満たさないと判断した場合には、第２時間帯での移動パラメータΦの推定値が妥当ではないと判定する。この場合、パラメータ設定部５０は、第２時間帯での移動パラメータΦを採用しない。
【0058】
以上説明した第３実施形態における移動パラメータΦの設定の処理フローを説明する。図９は、第３実施形態における移動パラメータの設定の処理フローを説明するフローチャートである。なお、図９では、第１時間帯での移動パラメータΦの推定値の算出及び仮の妥当性の処理については省略している。図９に示すように、第３実施形態においては、第１位置算出部４２及び第２位置算出部４４により、第２時間帯における第１推定位置及び第２推定位置が算出され、パラメータ算出部４６は、第２時間帯における第１推定位置及び第２推定位置に基づき、第２時間帯での移動パラメータΦの推定値を算出する（ステップＳ１４Ａ）。そして、判定部４８は、移動体１０の停止時における外界センサ２６Ａの検出結果に基づいて、第２時間帯での移動パラメータΦの推定値の仮の妥当性を判断し（ステップＳ１６Ａ）、仮に妥当である場合（ステップＳ１６Ａ；Ｙｅｓ）、第２時間帯での移動パラメータΦの推定値が、第１時間帯での移動パラメータΦの推定値に対して所定条件を満たすかを判定する（ステップＳ１７Ａ）。所定条件を満たす場合（ステップＳ１７Ａ；Ｙｅｓ）、判定部４８は、第２時間帯での移動パラメータΦの推定値が妥当と判断して、パラメータ設定部５０により、第２時間帯での移動パラメータΦの推定値を、正式な移動パラメータΦとして設定する（ステップＳ１８Ａ）。一方、第２時間帯での移動パラメータΦの推定値に仮妥当性がない場合（ステップＳ１６Ａ；Ｎｏ）、及び第２時間帯での移動パラメータΦの推定値が所定条件を満たさない場合（ステップＳ１７Ａ；Ｎｏ）、第２時間帯での移動パラメータΦの推定値を正式な移動パラメータΦとして設定しない（ステップＳ２０Ａ）。
【0059】
このように、第３実施形態においては、過去の移動パラメータΦの推定値と比較して妥当でないとされた場合には、移動パラメータΦの推定値を正式な値として採用しない。そのため、移動パラメータΦの算出精度をより好適に向上させることができる。
【0060】
なお、以上の説明では、第３実施形態においては、停止時の外界センサ２６Ａの検出結果による仮の妥当性の判定と、過去の移動パラメータΦの推定値との比較による判定との、２段階の判定を行っていた。ただしそれに限られず、第３実施形態においては、停止時の外界センサ２６Ａの検出結果による仮の妥当性の判定を行わなくてよい。すなわちこの場合、判定部４８は、停止時の外界センサ２６Ａの検出結果による仮の妥当性を行わずに、第２時間帯での移動パラメータΦの推定値と第１時間帯での移動パラメータΦの推定値とを比較して、第２時間帯での移動パラメータΦの妥当性を判定してよい。
【0061】
（効果）
以上説明したように、本開示に係る演算装置は、第１位置算出部４２と、第２位置算出部４４と、パラメータ算出部４６と、判定部４８と、パラメータ設定部５０とを含む。第１位置算出部４２は、移動体１０に搭載されて移動体１０の周囲を検出する外界センサ２６Ａの検出結果に基づいて、移動体の第１推定位置を算出する。第２位置算出部４４は、移動体１０に搭載されて移動体１０の制御出力を示す出力パラメータを検出する内界センサ２６Ｂの検出結果に基づいて、移動体１０の第２推定位置を算出する。パラメータ算出部４６は、第１推定位置及び第２推定位置に基づいて、移動体１０の移動量を算出するための移動パラメータΦの推定値を算出する。判定部４８は、移動体１０の停止時における外界センサ２６Ａの検出結果に基づいて、移動パラメータΦの推定値の妥当性を判定する。パラメータ設定部５０は、妥当であると判断された移動パラメータΦの推定値を、移動体１０の移動パラメータとして設定する。
以上説明したように、本開示に係る演算装置は、第１位置算出部４２と、第２位置算出部４４と、パラメータ算出部４６と、判定部４８と、パラメータ設定部５０とを含む。第１位置算出部４２は、移動体１０に搭載されて移動体１０の周囲を検出する外界センサ２６Ａの検出結果に基づいて、移動体の第１推定位置を算出する。第２位置算出部４４は、移動体１０に搭載されて移動体１０の制御出力を示す出力パラメータを検出する内界センサ２６Ｂの検出結果に基づいて、移動体１０の第２推定位置を算出する。パラメータ算出部４６は、第１推定位置及び第２推定位置に基づいて、移動体１０の移動量を算出するための移動パラメータΦの推定値を算出する。判定部４８は、移動体１０の停止時における外界センサ２６Ａの検出結果に基づいて、移動パラメータΦの推定値の妥当性を判定する。パラメータ設定部５０は、妥当であると判断された移動パラメータΦの推定値を、移動体１０の移動パラメータとして設定する。
【0062】
本開示に係る演算装置は、外界センサ２６Ａと内界センサ２６Ｂとの検出結果から、移動パラメータΦの推定値を算出するため、移動パラメータΦを高精度に算出できるといえる。また、停止時における外界センサ２６Ａの検出結果から、移動パラメータΦの妥当性を判断するため、局所最適解への収束を検知し、かつ、タイムスタンプの誤差を抑制できるので、移動パラメータΦをより高精度に算出できる。
【0063】
第１位置算出部４２は、所定時間毎に検出された外界センサ２６Ａの検出結果から、所定時間毎の第１推定位置を算出し、第２位置算出部４４は、所定時間毎に検出された内界センサ２６Ｂの検出結果から、所定時間毎の第２推定位置を算出する。パラメータ算出部４６は、所定時間毎の第１推定位置と第２推定位置との差分に基づいて、移動パラメータΦの推定値を算出する。本開示に係る演算装置は、このようにして移動パラメータΦの推定値を算出することで、移動パラメータΦをより高精度に算出できる。
【0064】
パラメータ算出部４６は、所定時間毎の第１推定位置と第２推定位置との差分の合計値が、最小となるように、移動パラメータΦの推定値を算出する。本開示に係る演算装置は、このようにして移動パラメータΦの推定値を算出することで、移動パラメータΦをより高精度に算出できる。
【0065】
判定部４８は、移動体１０の停止時において第１外界センサ２６Ａ１により検出された、移動体１０の周囲の検出体Ａの検出結果と、移動パラメータΦの推定値とに基づいて、移動体１０の基準座標系における検出体Ａの位置を算出する。判定部４８は、移動体１０の停止時において第２外界センサ２６Ａ２により検出された、移動体１０の周囲の検出体Ａの検出結果と、移動パラメータΦの推定値とに基づいて、移動体１０の基準座標系における検出体Ａの位置を算出する。そして、判定部４８は、第１外界センサ２６Ａ１の検出結果に基づいて算出された検出体Ａの位置と、第２外界センサ２６Ａ２の検出結果に基づいて算出された検出体Ａの位置とのずれ量が、所定値以下である場合に、移動パラメータΦの推定値が妥当であると判断する。本開示に係る演算装置は、このようにして移動パラメータΦの妥当性を判断することで、移動パラメータΦをより高精度に算出できる。
【0066】
判定部４８は、移動体１０の停止時において第１外界センサ２６Ａ１により検出された、移動体１０の周囲の複数の検出体Ａの検出結果から、第１外界センサ２６Ａ１の位置を算出する。判定部４８は、移動体１０の停止時において第２外界センサ２６Ａ２により検出された、移動体１０の周囲の複数の検出体Ａの検出結果から、第２外界センサ２６Ａ２の位置を算出する。そして、判定部４８は、第１外界センサ２６Ａ１の位置と第２外界センサ２６Ａ２の位置とから算出された、第１外界センサ２６Ａ１に対する第２外界センサ２６Ａ２の相対位置と、移動パラメータΦの推定値から算出された、第１外界センサ２６Ａ１に対する第２外界センサ２６Ａ２の相対位置とのずれ量が、所定値以下である場合に、移動パラメータΦの推定値が妥当であると判断する。本開示に係る演算装置は、このようにして移動パラメータΦの妥当性を判断することで、移動パラメータΦをより高精度に算出できる。
【0067】
パラメータ算出部４６は、移動体１０の停止時における内界センサ２６Ｂの検出結果に基づいて、内界センサ２６Ｂの検出結果の正常値からのずれ量を示すバイアス値を算出し、バイアス値にも基づき、移動パラメータΦの推定値を算出する。これにより、算出対象となる変数を減らすことができるので、移動パラメータΦの推定値の演算負荷を低減できる。
【0068】
パラメータ算出部４６は、第１時間帯での外界センサ２６Ａ及び内界センサ２６Ｂの検出結果に基づいて算出された第１推定位置と第２推定位置とに基づいて、第１時間帯における移動パラメータΦの推定値を算出し、第１時間帯より後の第２時間帯での外界センサ２６Ａ及び内界センサ２６Ｂの検出結果に基づいて算出された第１推定位置と第２推定位置とに基づいて、第２時間帯における移動パラメータΦの推定値を算出する。判定部４８は、第２時間帯における移動パラメータΦの推定値が、第１時間帯における移動パラメータΦの推定値に対して所定条件を満たす場合に、第２時間帯における移動パラメータΦの推定値が妥当であると判断する。これにより、移動パラメータΦをより高精度に算出できる。
【0069】
移動パラメータΦは、移動体１０の基準座標系における外界センサ２６Ａの位置と、移動体１０の車輪径と、内界センサ２６Ｂの検出結果の正常値からのずれ量を示すバイアス値との、少なくとも１つである。本開示によれば、このような移動パラメータΦを高精度に算出できる。
【0070】
以上、本開示の実施形態を説明したが、この実施形態の内容により実施形態が限定されるものではない。また、前述した構成要素には、当業者が容易に想定できるもの、実質的に同一のもの、いわゆる均等の範囲のものが含まれる。さらに、前述した構成要素は適宜組み合わせることが可能である。さらに、前述した実施形態の要旨を逸脱しない範囲で構成要素の種々の省略、置換又は変更を行うことができる。
【符号の説明】
【0071】
１０ 移動体
２６Ａ 外界センサ
２６Ｂ 内界センサ
４０ 移動制御部
４２ 第１位置算出部
４４ 第２位置算出部
４６ パラメータ算出部
４８ 判定部
５０ パラメータ設定部
２６Ａ 外界センサ
２６Ｂ 内界センサ
４０ 移動制御部
４２ 第１位置算出部
４４ 第２位置算出部
４６ パラメータ算出部
４８ 判定部
５０ パラメータ設定部
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】