ホーム > 特許ランキング > アルプスアルパイン株式会社
知財求人 - 知財ポータルサイト「IP Force」
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2023120956
(43)【公開日】2023-08-30
(54)【発明の名称】ＬｉＤＡＲシステム
(51)【国際特許分類】
G01S 7/497 20060101AFI20230823BHJP
G08G 1/16 20060101ALI20230823BHJP
G06T 7/521 20170101ALI20230823BHJP
G06T 7/70 20170101ALI20230823BHJP
【ＦＩ】
G01S7/497
G08G1/16 C
G06T7/521
G06T7/70 A
【審査請求】未請求
【請求項の数】7
【出願形態】ＯＬ
(21)【出願番号】P 2022024121
(22)【出願日】2022-02-18
(71)【出願人】
【識別番号】000010098
【氏名又は名称】アルプスアルパイン株式会社
(74)【代理人】
【識別番号】100099748
【弁理士】
【氏名又は名称】佐藤 克志
(74)【代理人】
【識別番号】100103171
【弁理士】
【氏名又は名称】雨貝 正彦
(74)【代理人】
【識別番号】100105784
【弁理士】
【氏名又は名称】橘 和之
(74)【代理人】
【識別番号】100098497
【弁理士】
【氏名又は名称】片寄 恭三
(72)【発明者】
【氏名】橋本 真也
【テーマコード（参考）】
5H181
5J084
5L096
【Ｆターム（参考）】
5H181AA01
5H181AA27
5H181CC03
5H181CC14
5H181FF04
5H181FF07
5H181FF22
5H181FF27
5H181LL01
5H181LL09
5J084AA04
5J084AA10
5J084AC02
5J084AC07
5J084BA48
5J084CA65
5J084CA68
5J084EA04
5J084EA08
5L096AA06
5L096AA09
5L096BA04
5L096EA15
5L096EA16
5L096EA26
5L096FA67
5L096FA69
5L096GA32
5L096HA05
(57)【要約】
【課題】移動体に対する幾何パラメータを検出する「LiDARシステム」を提供する。
【解決手段】第２LiDAR２で検出した三次元点群(ａ１）を第１LiDAR１で検出した三次元点群（ａ２）に合成した三次元点群（ｂ）中から支持面の三次元点群２０１を抽出する（ｃ）。抽出した支持面の三次元点群から高さ０ｍの仮想支持面の三次元点群２０１を生成し（ｄ）、支持面の三次元点群２０１を仮想支持面の三次元点群２０２にフィッティングし、支持面の三次元点群を仮想支持面の三次元点群に最もよく座標変換する座標変換行列を算出し、座標変換行列の高さ、ロール、ピッチのパラメータを、第１LiDAR１の高さ、ロール、ピッチの幾何パラメータとする。
【選択図】図４
【解決手段】第２LiDAR２で検出した三次元点群(ａ１）を第１LiDAR１で検出した三次元点群（ａ２）に合成した三次元点群（ｂ）中から支持面の三次元点群２０１を抽出する（ｃ）。抽出した支持面の三次元点群から高さ０ｍの仮想支持面の三次元点群２０１を生成し（ｄ）、支持面の三次元点群２０１を仮想支持面の三次元点群２０２にフィッティングし、支持面の三次元点群を仮想支持面の三次元点群に最もよく座標変換する座標変換行列を算出し、座標変換行列の高さ、ロール、ピッチのパラメータを、第１LiDAR１の高さ、ロール、ピッチの幾何パラメータとする。
【選択図】図４
【特許請求の範囲】
【請求項1】
移動体に固定されたLiDAR（light detection and ranging）を備えた、前記移動体に搭載されたLiDARシステムであって、
前記LiDARが出力する三次元点群中から、移動体を支持する支持面の三次元点群を抽出する支持面抽出手段と、
前記LiDARの、前記移動体に対する幾何学的関係を表す幾何パラメータを算定する幾何パラメータ算定手段とを有し、
前記幾何パラメータ算定手段は、前記支持面抽出手段が抽出した三次元点群が表す支持面を、高さ０の水平な平面である仮想の支持面に座標変換する座標変換行列を算出し、座標変換行列の高さ、ロール、ピッチのパラメータを、LiDARの前記移動体に対する高さ、ロール角、ピッチ角の幾何パラメータとすることを特徴とするLiDARシステム。
前記LiDARが出力する三次元点群中から、移動体を支持する支持面の三次元点群を抽出する支持面抽出手段と、
前記LiDARの、前記移動体に対する幾何学的関係を表す幾何パラメータを算定する幾何パラメータ算定手段とを有し、
前記幾何パラメータ算定手段は、前記支持面抽出手段が抽出した三次元点群が表す支持面を、高さ０の水平な平面である仮想の支持面に座標変換する座標変換行列を算出し、座標変換行列の高さ、ロール、ピッチのパラメータを、LiDARの前記移動体に対する高さ、ロール角、ピッチ角の幾何パラメータとすることを特徴とするLiDARシステム。
【請求項2】
請求項１記載のLiDARシステムであって、
前記幾何パラメータ算定手段は、前記支持面抽出手段が抽出した支持面の三次元点群の高さを０とすることにより、前記仮想の支持面の三次元点群を生成し、前記支持面抽出手段が抽出した支持面の三次元点群を、前記仮想の支持面の三次元点群に最も良く近似する三次元点群に座標変換する座標変換行列を算出し、座標変換行列の高さ、ロール、ピッチのパラメータを、LiDARの前記移動体に対する高さ、ロール角、ピッチ角の幾何パラメータとすることを特徴とするLiDARシステム。
前記幾何パラメータ算定手段は、前記支持面抽出手段が抽出した支持面の三次元点群の高さを０とすることにより、前記仮想の支持面の三次元点群を生成し、前記支持面抽出手段が抽出した支持面の三次元点群を、前記仮想の支持面の三次元点群に最も良く近似する三次元点群に座標変換する座標変換行列を算出し、座標変換行列の高さ、ロール、ピッチのパラメータを、LiDARの前記移動体に対する高さ、ロール角、ピッチ角の幾何パラメータとすることを特徴とするLiDARシステム。
【請求項3】
移動体に固定されたLiDAR（light detection and ranging）を備えた、前記移動体に搭載されたLiDARシステムであって、
前記LiDARの出力から前記移動体の実空間上の座標を第１座標として算定するSLAM(Simultaneous Localization and Mapping)手段と、
前記LiDARの出力を用いずに、自律航法もしくは慣性航法もしくは衛星測位により、前記移動体の実空間上の座標を、第２座標として計測する位置計測手段と、
前記LiDARの、前記移動体に対する幾何学的関係を表す幾何パラメータを算定する幾何パラメータ算定手段とを有し、
前記幾何パラメータ算定手段は、前記移動体を移動させながら、前記第１座標の軌跡と前記第２座標の軌跡を記録し、記録した前記第１座標の軌跡と前記LiDARの前記移動体に対するヨウ角とから求まる前記移動体の実空間上の座標の軌跡が、前記第２座標の軌跡と最も近似することとなるヨウ角を算定し、算定したヨウ角をLiDARの前記移動体に対するヨウ角の幾何パラメータとすることを特徴とするLiDARシステム。
前記LiDARの出力から前記移動体の実空間上の座標を第１座標として算定するSLAM(Simultaneous Localization and Mapping)手段と、
前記LiDARの出力を用いずに、自律航法もしくは慣性航法もしくは衛星測位により、前記移動体の実空間上の座標を、第２座標として計測する位置計測手段と、
前記LiDARの、前記移動体に対する幾何学的関係を表す幾何パラメータを算定する幾何パラメータ算定手段とを有し、
前記幾何パラメータ算定手段は、前記移動体を移動させながら、前記第１座標の軌跡と前記第２座標の軌跡を記録し、記録した前記第１座標の軌跡と前記LiDARの前記移動体に対するヨウ角とから求まる前記移動体の実空間上の座標の軌跡が、前記第２座標の軌跡と最も近似することとなるヨウ角を算定し、算定したヨウ角をLiDARの前記移動体に対するヨウ角の幾何パラメータとすることを特徴とするLiDARシステム。
【請求項4】
請求項１または２記載のLiDARシステムであって、
前記LiDARの出力から前記移動体の実空間上の座標を第１座標として算定するSLAM(Simultaneous Localization and Mapping)手段と、
前記LiDARの出力を用いずに、自律航法もしくは慣性航法もしくは衛星測位により、前記移動体の実空間上の座標を、第２座標として計測する位置計測手段とを有し、
前記幾何パラメータ算定手段は、前記移動体を移動させながら、前記第１座標の軌跡と前記第２座標の軌跡を記録し、記録した前記第１座標の軌跡と前記LiDARの前記移動体に対するヨウ角とから求まる前記移動体の実空間上の座標の軌跡が、前記第２座標の軌跡と最も近似することとなるヨウ角を算定し、算定したヨウ角をLiDARの前記移動体に対するヨウ角の幾何パラメータとすることを特徴とするLiDARシステム。
前記LiDARの出力から前記移動体の実空間上の座標を第１座標として算定するSLAM(Simultaneous Localization and Mapping)手段と、
前記LiDARの出力を用いずに、自律航法もしくは慣性航法もしくは衛星測位により、前記移動体の実空間上の座標を、第２座標として計測する位置計測手段とを有し、
前記幾何パラメータ算定手段は、前記移動体を移動させながら、前記第１座標の軌跡と前記第２座標の軌跡を記録し、記録した前記第１座標の軌跡と前記LiDARの前記移動体に対するヨウ角とから求まる前記移動体の実空間上の座標の軌跡が、前記第２座標の軌跡と最も近似することとなるヨウ角を算定し、算定したヨウ角をLiDARの前記移動体に対するヨウ角の幾何パラメータとすることを特徴とするLiDARシステム。
【請求項5】
請求項１、２、３、または４記載のLiDARシステムであって、
前記SLAM手段は、前記LiDARの出力から当該記LiDARの実空間上の位置の第１の座標系上の座標を第１座標として算定し、
前記位置計測手段は、前記移動体上の所定の基準点の実空間上の位置の第２の座標系上の座標を第２座標として計測し、
前記幾何パラメータ算定手段は、前記SLAM手段が、前記LiDARの前記移動体に対するヨウ角を０と見なして前記第１座標を算定する状態で、前記移動体を移動させながら、前記第１座標の軌跡と前記第２座標の軌跡を記録し、
（Gx1、Gy1)を第１座標、（Gx2、Gy2)を第２座標、Rθをθ回転させる回転行列、Tを第１座標系から第２座標系への座標変換行列、（dx、dy)を前記第２座標系上の前記LiDARの位置の前記基準点に対する位置の差、モデル式を
として、
各時点の（Gx1、Gy1)、（Gx2、Gy2)、（dx、dy)の組に対して当該モデル式を最も良く成立させるθを、LiDARの前記移動体に対するヨウ角の幾何パラメータとすることを特徴とするLiDARシステム。
前記SLAM手段は、前記LiDARの出力から当該記LiDARの実空間上の位置の第１の座標系上の座標を第１座標として算定し、
前記位置計測手段は、前記移動体上の所定の基準点の実空間上の位置の第２の座標系上の座標を第２座標として計測し、
前記幾何パラメータ算定手段は、前記SLAM手段が、前記LiDARの前記移動体に対するヨウ角を０と見なして前記第１座標を算定する状態で、前記移動体を移動させながら、前記第１座標の軌跡と前記第２座標の軌跡を記録し、
（Gx1、Gy1)を第１座標、（Gx2、Gy2)を第２座標、Rθをθ回転させる回転行列、Tを第１座標系から第２座標系への座標変換行列、（dx、dy)を前記第２座標系上の前記LiDARの位置の前記基準点に対する位置の差、モデル式を
【数1】
各時点の（Gx1、Gy1)、（Gx2、Gy2)、（dx、dy)の組に対して当該モデル式を最も良く成立させるθを、LiDARの前記移動体に対するヨウ角の幾何パラメータとすることを特徴とするLiDARシステム。
【請求項6】
請求項１または２記載のLiDARシステムであって、
前記幾何パラメータ算定手段が設定した幾何パラメータを用いて、前記LiDARの出力から前記移動体の実空間上の座標の算定と三次元マップの生成を行うSLAM(Simultaneous Localization and Mapping)手段を有することを特徴とするLiDARシステム
前記幾何パラメータ算定手段が設定した幾何パラメータを用いて、前記LiDARの出力から前記移動体の実空間上の座標の算定と三次元マップの生成を行うSLAM(Simultaneous Localization and Mapping)手段を有することを特徴とするLiDARシステム
【請求項7】
請求項３、４、５または６記載ののLiDARシステムであって、
前記SLAM手段は、前記幾何パラメータ算定手段が設定した幾何パラメータを用いて、前記LiDARの出力から前記移動体の実空間上の座標の算定と三次元マップの生成を行うことを特徴とするLiDARシステム。
前記SLAM手段は、前記幾何パラメータ算定手段が設定した幾何パラメータを用いて、前記LiDARの出力から前記移動体の実空間上の座標の算定と三次元マップの生成を行うことを特徴とするLiDARシステム。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、LiDAR（light detection and ranging）をキャリブレーションする技術に関するものである。
【背景技術】
【0002】
LiDAR（light detection and ranging）をキャリブレーションする技術としては、LiDARの検出座標からグローバル座標への変換パラメータを変化させながら、移動体に搭載したLiDARで取得した障害物の三次元点群を比較し、障害物のグローバル座標の差異が最小となった変換パラメータを採用する技術が知られている（たとえば、特許文献１）。
【0003】
また、LiDARのキャリブレーションする技術としては、移動体に搭載されたLiDARの当該移動体に対する位置や姿勢などの幾何パラメータを外部の計測装置で計測し、計測した幾何パラメータからLiDARの視野を表す画像を提示することにより、LiDARの移動体への取付けを支援する技術も知られている（たとえば、特許文献２）。
【0004】
また、本発明に関する技術としては、移動体に搭載されたLiDARで三次元点群を取得しながら、三次元地図の生成と移動体の位置の検出を行うLiDAR SLAM(Simultaneous Localization and Mapping)の技術が知られている（たとえば、特許文献３）。
【先行技術文献】
【特許文献】
【0005】
【特許文献1】特開２０２０-６０５５３号公報
【特許文献2】国際公開第２０２１/１９９７３０号
【特許文献3】特開２０１９-２０７２２０号公報
【発明の概要】
【発明が解決しようとする課題】
【0006】
移動体に搭載したLiDAR SLAMで、三次元地図の生成と移動体の位置の検出を正しく行うためには、LiDARの移動体に対する位置や姿勢などの幾何パラメータを精度良く検出し設定するキャリブレーションを行う必要がある。
一方、上述した幾何パラメータを外部の計測装置で計測する技術によれば、計測の作業が繁雑であるし、実稼働中などの計測装置を利用できない状況においては幾何パラメータを検出することができない。
そこで、本発明は、外部の計測装置による計測を必要とすることなく、LiDARの移動体に対する位置や姿勢などの幾何パラメータを精度良く検出することを課題とする。
一方、上述した幾何パラメータを外部の計測装置で計測する技術によれば、計測の作業が繁雑であるし、実稼働中などの計測装置を利用できない状況においては幾何パラメータを検出することができない。
そこで、本発明は、外部の計測装置による計測を必要とすることなく、LiDARの移動体に対する位置や姿勢などの幾何パラメータを精度良く検出することを課題とする。
【課題を解決するための手段】
【0007】
前記課題達成のために、本発明は、移動体に固定されたLiDAR（light detection and ranging）を備えた、前記移動体に搭載されたLiDARシステムに、前記LiDARが出力する三次元点群中から、移動体を支持する支持面の三次元点群を抽出する支持面抽出手段と、前記LiDARの、前記移動体に対する幾何学的関係を表す幾何パラメータを算定する幾何パラメータ算定手段とを備えたものである。前記幾何パラメータ算定手段は、前記支持面抽出手段が抽出した三次元点群が表す支持面を、高さ０の水平な平面である仮想の支持面に座標変換する座標変換行列を算出し、座標変換行列の高さ、ロール、ピッチのパラメータを、LiDARの前記移動体に対する高さ、ロール角、ピッチ角の幾何パラメータとする。
【0008】
ここで、このLiDARシステムは、前記幾何パラメータ算定手段において、前記支持面抽出手段が抽出した支持面の三次元点群の高さを０とすることにより、仮想の支持面の三次元点群を生成し、前記支持面抽出手段が抽出した支持面の三次元点群を、前記仮想の支持面の三次元点群に最も良く近似する三次元点群に座標変換する座標変換行列を算出し、座標変換行列の高さ、ロール、ピッチのパラメータを、LiDARの前記移動体に対する高さ、ロール角、ピッチ角の幾何パラメータとするように構成してもよい。
【0009】
また、本発明は、前記課題達成のために、移動体に固定されたLiDAR（light detection and ranging）を備えた、前記移動体に搭載されたLiDARシステムに、前記LiDARの出力から前記移動体の実空間上の座標を第１座標として算定するSLAM(Simultaneous Localization and Mapping)手段と、前記LiDARの出力を用いずに、自律航法もしくは慣性航法もしくは衛星測位により、前記移動体の実空間上の座標を、第２座標として計測する位置計測手段と、前記LiDARの、前記移動体に対する幾何学的関係を表す幾何パラメータを算定する幾何パラメータ算定手段とを備えたものである。ここで、前記幾何パラメータ算定手段は、前記移動体を移動させながら、前記第１座標の軌跡と前記第２座標の軌跡を記録し、記録した前記第１座標の軌跡と前記LiDARの前記移動体に対するヨウ角とから求まる前記移動体の実空間上の座標の軌跡が、前記第２座標の軌跡と最も近似することとなるヨウ角を算定し、算定したヨウ角をLiDARの前記移動体に対するヨウ角の幾何パラメータとする。
【0010】
ここで、このLiDARシステムは、前記SLAM手段において、前記LiDARの出力から当該記LiDARの実空間上の位置の第１の座標系上の座標を第１座標として算定し、前記位置計測手段において、前記移動体上の所定の基準点の実空間上の位置の第２の座標系上の座標を第２座標として計測し、前記幾何パラメータ算定手段において、前記SLAM手段が、前記LiDARの前記移動体に対するヨウ角を０と見なして前記第１座標を算定する状態で、前記移動体を移動させながら、前記第１座標の軌跡と前記第２座標の軌跡を記録し、
（Gx1、Gy1)を第１座標、（Gx2、Gy2)を第２座標、Rθをθ回転させる回転行列、Tを第１座標系から第２座標系への座標変換行列、（dx、dy)を前記第２座標系上の前記LiDARの位置の前記基準点に対する位置の差、モデル式を
（Gx1、Gy1)を第１座標、（Gx2、Gy2)を第２座標、Rθをθ回転させる回転行列、Tを第１座標系から第２座標系への座標変換行列、（dx、dy)を前記第２座標系上の前記LiDARの位置の前記基準点に対する位置の差、モデル式を
【0011】
【数1】
【0012】
として、
各時点の（Gx1、Gy1)、（Gx2、Gy2)、（dx、dy)の組に対して当該モデル式を最も良く成立させるθを、LiDARの前記移動体に対するヨウ角の幾何パラメータとするように構成してもよい。
各時点の（Gx1、Gy1)、（Gx2、Gy2)、（dx、dy)の組に対して当該モデル式を最も良く成立させるθを、LiDARの前記移動体に対するヨウ角の幾何パラメータとするように構成してもよい。
【0013】
また、以上の各LiDARシステムは、前記幾何パラメータ算定手段が設定した幾何パラメータを用いて、前記LiDARの出力から前記移動体の実空間上の座標の算定と三次元マップの生成を行うSLAM(Simultaneous Localization and Mapping)手段を備えていてもよい。
以上のようなによれば、外部の計測装置による計測を必要とすることなく、LiDARで検出した三次元点群自身を用いて、LiDARの移動体に対する高さやロールやピッチの幾何パラメータを精度良く検出することができる。また、移動体が、自律航法や衛星測位や慣性航法によって現在位置を算出する位置計測手段を備えている場合には、当該位置計測手段とLiDAR SLAMのみを用いて、LiDARの移動体に対するヨウの幾何パラメータも精度良く検出することができる。
以上のようなによれば、外部の計測装置による計測を必要とすることなく、LiDARで検出した三次元点群自身を用いて、LiDARの移動体に対する高さやロールやピッチの幾何パラメータを精度良く検出することができる。また、移動体が、自律航法や衛星測位や慣性航法によって現在位置を算出する位置計測手段を備えている場合には、当該位置計測手段とLiDAR SLAMのみを用いて、LiDARの移動体に対するヨウの幾何パラメータも精度良く検出することができる。
【発明の効果】
【0014】
以上のように、本発明によれば、外部の計測装置による計測を必要とすることなく、LiDARの移動体に対する位置や姿勢などの幾何パラメータを精度良く検出することができる。
【図面の簡単な説明】
【0015】
【図1】本発明の実施形態に係る移動体の機能構成を示す図である。
【図2】本発明の実施形態に係るパーソナルモビリティを示す図である。
【図3】本発明の実施形態に係る第１キャリブレーション処理を示すフローチャートである。
【図4】本発明の実施形態に係る第１キャリブレーション処理の処理例を示す図である。
【図5】本発明の実施形態に係る第２キャリブレーション処理を示すフローチャートである。
【図6】本発明の実施形態に係る第２キャリブレーション処理に用いるパラメータを示す図である。
【発明を実施するための形態】
【0016】
以下、本発明の実施形態について説明する。
図１に、本実施形態に係る移動体の機能構成を示す。
移動体は、パーソナルモビリティ、自走式車椅子、自動車、自走式ロボット、その他の車両などの自走式の移動体であり、自動運転もしくは手動運転により走行する。
移動体は、図示するように、物体の三次元点群を検出する第１LiDAR１と、物体の三次元点群を検出する第２LiDAR２との２つのLiDAR（light detection and ranging）、衛星測位により現在位置を検出するGNSSモジュール３、自律航法(Dead Reckoning)により高精度に現在位置を算出する自己位置計測モジュール４、モニタ５、ユーザの操作を受け付ける操作装置６、移動体を自走させる走行機構７、移動体の移動速度や、操舵角などの進行方向などの各種状態を検出する状態センサ８、制御装置９を備えている。
図１に、本実施形態に係る移動体の機能構成を示す。
移動体は、パーソナルモビリティ、自走式車椅子、自動車、自走式ロボット、その他の車両などの自走式の移動体であり、自動運転もしくは手動運転により走行する。
移動体は、図示するように、物体の三次元点群を検出する第１LiDAR１と、物体の三次元点群を検出する第２LiDAR２との２つのLiDAR（light detection and ranging）、衛星測位により現在位置を検出するGNSSモジュール３、自律航法(Dead Reckoning)により高精度に現在位置を算出する自己位置計測モジュール４、モニタ５、ユーザの操作を受け付ける操作装置６、移動体を自走させる走行機構７、移動体の移動速度や、操舵角などの進行方向などの各種状態を検出する状態センサ８、制御装置９を備えている。
【0017】
また、制御装置９は、第１LiDAR１と第２LiDAR２が検出した三次元点群から三次元地図の生成と現在位置の算定を行うSLAM(Simultaneous Localization and Mapping)９１、第１LiDAR１と第２LiDAR２の移動体に対する位置や姿勢などの幾何パラメータをキャリブレーションしてSLAM９１に設定するキャリブレーション処理部９２を備えている。
【0018】
ここで、SLAM９１は、設定された第１LiDAR１と第２LiDAR２の幾何パラメータを用いて、第１LiDAR１が検出した三次元点群と第２LiDAR２が検出した三次元点群を、移動体の基準位置からみた三次元点群に変換し、変換した三次元点群を用いて実世界の座標系であるグローバル座標系上の三次元地図の生成とグローバル座標系上の現在位置の算定を行う。
【0019】
第１LiDAR１の移動体に対する幾何パラメータしては、移動体の左右方向をｘ方向、移動体の前後方向をｙ方向、移動体の上下方向をｚ方向、移動体の前後方向の軸回りの回転方向をRoll(ロール）方向、移動体の左右方向の軸回りの回転方向をPitch（ピッチ）方向、移動体の上下の方向の軸回りの回転方向をYaw（ヨウ）方向として、第１LiDAR１の移動体に対するｘ方向位置x1、ｙ方向位置y1、ｚ方向位置z1、Roll方向回転角（ロール角）Roll1、Pitch方向回転角（ピッチ角）Pitch1、Yaw方向回転角（ヨウ角）Yaw1を用いる。
【0020】
同様に、第２LiDAR２の移動体に対する幾何パラメータとしては、第２LiDAR２の移動体に対するｘ方向位置x2、ｙ方向位置y2、ｚ方向位置z2、Roll方向回転角Roll2、Pitch方向回転角Pitch2、Yaw方向回転角Yaw2を用いる。
また、制御装置９は、予め取得しておいた道路地図や施設地図や、GNSSモジュール３や自己位置計測モジュール４やSLAM９１で検出した現在位置や、SLAM９１で生成した三次元地図等を参照して、自動運転により走行機構７を制御して移動体を走行させる運転制御を行う運転制御部９３を備えている。ここで、運転制御部９３は、操作装置６で受け付けた運転操作に応じて走行機構７を制御して移動体を走行させる運転制御も行うこともできる。
また、制御装置９は、予め取得しておいた道路地図や施設地図や、GNSSモジュール３や自己位置計測モジュール４やSLAM９１で検出した現在位置や、SLAM９１で生成した三次元地図等を参照して、自動運転により走行機構７を制御して移動体を走行させる運転制御を行う運転制御部９３を備えている。ここで、運転制御部９３は、操作装置６で受け付けた運転操作に応じて走行機構７を制御して移動体を走行させる運転制御も行うこともできる。
【0021】
状態センサ８で検出した各種状態は、運転制御部９３における運転制御に用いられる。また、状態センサ８で検出した各種状態を、自己位置計測モジュール４における現在位置の算出に用いるようにしてもよい。
以下、移動体がパーソナルモビリティである場合を例にとり、本実施形態の説明を行う。
図２ａにパーソナルモビリティの側面を、図２ｂにパーソナルモビリティの上面を示す。
図示するように、第１LiDAR１は、パーソナルモビリティの前部の右側に配置され、パーソナルモビリティの前方から右前方の範囲を走査範囲とする走査を行い走査範囲内ある物体の三次元点群を検出する。第２LiDAR２は、パーソナルモビリティの前部の左側に配置され、パーソナルモビリティの前方から左前方の範囲を走査範囲とする走査を行い走査範囲内ある物体の三次元点群を検出する。
以下、移動体がパーソナルモビリティである場合を例にとり、本実施形態の説明を行う。
図２ａにパーソナルモビリティの側面を、図２ｂにパーソナルモビリティの上面を示す。
図示するように、第１LiDAR１は、パーソナルモビリティの前部の右側に配置され、パーソナルモビリティの前方から右前方の範囲を走査範囲とする走査を行い走査範囲内ある物体の三次元点群を検出する。第２LiDAR２は、パーソナルモビリティの前部の左側に配置され、パーソナルモビリティの前方から左前方の範囲を走査範囲とする走査を行い走査範囲内ある物体の三次元点群を検出する。
【0022】
また、第１LiDAR１と第２LiDAR２は、走査範囲が、パーソナルモビリティの前方の左右方向についての中央において重複するように配置されている。
次に、自己位置計測モジュール４は、パーソナルモビリティの内部に配置され、パーソナルモビリティの基準位置の、グローバル座標系上の座標を検出する。
次に、制御装置９のキャリブレーション処理部９２が、第１LiDAR１と第２LiDAR２の移動体に対する幾何パラメータをキャリブレーションする動作について説明する。
キャリブレーション処理部９２は、幾何パラメータのキャリブレーションのために、第１キャリブレーション処理と第２キャリブレーション処理を当該順序で行う。
まず、第１キャリブレーション処理について説明する。
図３に、第１キャリブレーション処理の手順を示す。
図示するように第１キャリブレーション処理において、キャリブレーション処理部９２は、まず、第１LiDAR１の幾何パラメータ（x1、y1、z1、Roll1、Pitch1、Yaw1)と第２LiDAR２の幾何パラメータ（x2、y2、z2、Roll2、Pitch2、Yaw2)の双方を、(0、0、0、0、0、0）に初期化する（ステップ３０２）。
次に、自己位置計測モジュール４は、パーソナルモビリティの内部に配置され、パーソナルモビリティの基準位置の、グローバル座標系上の座標を検出する。
次に、制御装置９のキャリブレーション処理部９２が、第１LiDAR１と第２LiDAR２の移動体に対する幾何パラメータをキャリブレーションする動作について説明する。
キャリブレーション処理部９２は、幾何パラメータのキャリブレーションのために、第１キャリブレーション処理と第２キャリブレーション処理を当該順序で行う。
まず、第１キャリブレーション処理について説明する。
図３に、第１キャリブレーション処理の手順を示す。
図示するように第１キャリブレーション処理において、キャリブレーション処理部９２は、まず、第１LiDAR１の幾何パラメータ（x1、y1、z1、Roll1、Pitch1、Yaw1)と第２LiDAR２の幾何パラメータ（x2、y2、z2、Roll2、Pitch2、Yaw2)の双方を、(0、0、0、0、0、0）に初期化する（ステップ３０２）。
【0023】
そして、パーソナルモビリティを静止させた状態において、第１LiDAR１が検出した三次元点群と第２LiDAR２が検出した三次元点群を取得する（ステップ３０４）。第１LiDAR１が検出する三次元点群の三次元座標は１LiDARの位置を原点とする（x、y、z）であり、幾何パラメータ（x1、y1、z1、Roll1、Pitch1、Yaw1)が真に(0、0、0、0、0、0）であったときに、xはｘ方向の座標を表し、ｙはｙ方向の座標を表し、ｚはｚ方向の座標を表す。同様に、第２LiDAR２が検出する三次元点群の三次元座標は２LiDARの位置を原点とする（x、y、z）であり、幾何パラメータ（x2、y2、z2、Roll2、Pitch2、Yaw2)が真に(0、0、0、0、0、0）であったときに、xはｘ方向の座標を表し、ｙはｙ方向の座標を表し、ｚはｚ方向の座標を表す。
【0024】
そして、２つの三次元点群をマッチングし、第１LiDAR１と第２LiDAR２の走査範囲の重複範囲内の同一物の三次元点群の座標を等しくする、第２LiDAR２の三次元点群の座標の座標変換行列を推定する（ステップ３０６）。
そして、第２LiDAR２の三次元点群に座標変換行列を施した三次元点群を、第１LiDAR１の三次元点群に合成する（ステップ３０８）。
この結果、図４ａ１のように第２LiDAR２で三次元点群が検出され、図４ａ２のように第１LiDAR１で三次元点群が検出された場合、図４ｂに示すように両三次元点群が、第１LiDAR１の三次元点群の座標系上に、第１LiDAR１と第２LiDAR２の走査範囲を合わせた走査範囲内の三次元点群を示すように合成される。
そして、第２LiDAR２の三次元点群に座標変換行列を施した三次元点群を、第１LiDAR１の三次元点群に合成する（ステップ３０８）。
この結果、図４ａ１のように第２LiDAR２で三次元点群が検出され、図４ａ２のように第１LiDAR１で三次元点群が検出された場合、図４ｂに示すように両三次元点群が、第１LiDAR１の三次元点群の座標系上に、第１LiDAR１と第２LiDAR２の走査範囲を合わせた走査範囲内の三次元点群を示すように合成される。
【0025】
図３に戻り、次に、合成した三次元点群中から、床面や路面などのパーソナルモビリティを支持している面である支持面の三次元点群を抽出する（ステップ３１０）。支持面の三次元点群は、たとえば、第１LiDAR１より下方にある、広くおよそ平面を形成する三次元点群として抽出できる。
【0026】
そして、抽出した支持面の三次元点群の高さを０ｍとすることにより、高さ０ｍの水平な平面である仮想支持面の三次元点群を生成する（ステップ３１２）。
また、支持面の三次元点群を仮想支持面の三次元点群にフィッティングし、支持面の三次元点群を仮想支持面の三次元点群に最もよく座標変換する座標変換行列E(xe、ye、ze、Rolle、Pitche、Yawe)を算出する（ステップ３１４）。
また、支持面の三次元点群を仮想支持面の三次元点群にフィッティングし、支持面の三次元点群を仮想支持面の三次元点群に最もよく座標変換する座標変換行列E(xe、ye、ze、Rolle、Pitche、Yawe)を算出する（ステップ３１４）。
【0027】
ここで、xeはｘ方向の移動量を表すパラメータであり、yeはｙ方向の移動量を表すパラメータであり、zeはｚ方向の移動量を表すパラメータであり、RolleはRoll方向の回転角を表すパラメータであり、PitcheはPitch方向の回転角を表すパラメータであり、YaweはYaw方向の回転角を表すパラメータである。
【0028】
この結果、図４ｂの合成した三次元点群から、図４ｃに示す水平方向にみた支持面の三次元点群２０１が抽出された場合には、図４ｃの三次元点群の高さを０ｍとした図４ｄに示す仮想支持面の三次元点群２０２が生成され、図４ｃに示す支持面の三次元点群２０１を、図４ｄに示す仮想支持面の三次元点群２０２に最も良く近似するように座標変換する座標変換行列E(xe、ye、ze、Rolle、Pitche、Yawe)が算出される。
【0029】
図３に戻り、このようにして座標変換行列E(xe、ye、ze、Rolle、Pitche、Yawe)を算出したならば、座標変換行列E(xe、ye、ze、Rolle、Pitche、Yawe)のパラメータ（ze、Rolle、Pitche）を、第１LiDAR１の幾何パラメータ（z1、Roll1、Pitch1）としてSLAM９１に設定する（ステップ３１６）。
【0030】
また、第１LiDAR１の幾何パラメータ（z1、Roll1、Pitch1）と、ステップ３０６で求めた第２LiDAR２の三次元点群の座標の座標変換行列から、第２LiDAR２の幾何パラメータ（z2、Roll2、Pitch2）を算定し、SLAM９１に設定する（ステップ３１８）
そして、第１キャリブレーション処理を終了する。
そして、第１キャリブレーション処理を終了する。
【0031】
次に、第２キャリブレーション処理について説明する。
なお、第２キャリブレーション処理の開始時点において、第１LiDAR１の幾何パラメータ（x1、y1、Yaw1)は(0、0、0）に維持されており、第２LiDAR２の幾何パラメータ（x2、y2、Yaw2)は(0、0、0）に維持されている。
図５に、第２キャリブレーション処理の手順を示す。
図示するように、第２キャリブレーション処理においてキャリブレーション処理部９２は、まず、運転制御部９３にパーソナルモビリティを所定のルートに沿って走行させながら、SLAM９１に、平面直角座標系のように実空間の水平な平面上の座標を表す座標系であるグローバル座標系の第１LiDAR１の現在位置（SGx1、SGy1)を算定させ、走行中に自己位置計測モジュール４が検出したグローバル座標系上の現在位置（Gx、Gy)の履歴と、走行中にSLAM９１が算定した第１LiDAR１の現在位置（SGx1、SGy1)の履歴を取得する（ステップ５０２）。
なお、第２キャリブレーション処理の開始時点において、第１LiDAR１の幾何パラメータ（x1、y1、Yaw1)は(0、0、0）に維持されており、第２LiDAR２の幾何パラメータ（x2、y2、Yaw2)は(0、0、0）に維持されている。
図５に、第２キャリブレーション処理の手順を示す。
図示するように、第２キャリブレーション処理においてキャリブレーション処理部９２は、まず、運転制御部９３にパーソナルモビリティを所定のルートに沿って走行させながら、SLAM９１に、平面直角座標系のように実空間の水平な平面上の座標を表す座標系であるグローバル座標系の第１LiDAR１の現在位置（SGx1、SGy1)を算定させ、走行中に自己位置計測モジュール４が検出したグローバル座標系上の現在位置（Gx、Gy)の履歴と、走行中にSLAM９１が算定した第１LiDAR１の現在位置（SGx1、SGy1)の履歴を取得する（ステップ５０２）。
【0032】
ここで、自己位置計測モジュール４は、GX軸とGY軸とより規定されるグローバル座標系GX-GYで現在位置（Gx、Gy)を検出し、SLAM９１はSGX軸とSGY軸とより規定されるグローバル座標系SGX-SGYで現在位置（SGx1、SGy1)を算定するものとする。
次に、自己位置計測モジュール４がグローバル座標系GX-GY上の現在位置（Gx、Gy)を検出するパーソナルモビリティ上の位置を基準位置BPとして、取得した履歴を用い、（SGx1、SGy1)の履歴から幾何学的に定まる基準位置BPの軌跡と、履歴が示す（Gx、Gy)の軌跡が最も近似するように、第１LiDAR１の幾何パラメータYaw１を算定する（ステップ５０４）。
次に、自己位置計測モジュール４がグローバル座標系GX-GY上の現在位置（Gx、Gy)を検出するパーソナルモビリティ上の位置を基準位置BPとして、取得した履歴を用い、（SGx1、SGy1)の履歴から幾何学的に定まる基準位置BPの軌跡と、履歴が示す（Gx、Gy)の軌跡が最も近似するように、第１LiDAR１の幾何パラメータYaw１を算定する（ステップ５０４）。
【0033】
すなわち、図６ａに示すように、第１LiDAR１の実際のYaw方向回転角がθ１である場合、T()をグローバル座標系SGX-SGYからグローバル座標系GX-GYへの座標変換行列とすると、幾何パラメータYaw1を０としてSLAM９１が算定した現在位置（SGx1、SGy1)のグローバル座標系GX-GY上の座標T(SGx1、SGy1)の軌跡は、図６ｂに示すように、自己位置計測モジュール４が検出したグローバル座標系GX-GY上の現在位置（Gx1、Gy1)の軌跡に対して、θ１に応じた角度傾いた軌跡となる。そこで、この傾きから、逆に、第１LiDAR１の実際のYaw方向回転角θ１を幾何パラメータYaw１として算定する。
【0034】
具体的には、図６ｂに示すように、θ0をグローバル座標系GX-GY上の、パーソナルモビリティの前方方向の角度として、各時刻の（Gx、GY)、（SGx1、SGy1)、θ0の組に対して下記のモデル式を最も良く成立させるθ1を、最小二乗法などを用いた最適化アルゴリズムによって求め、求まったθ1をYaw１とする。
【0035】
ここで、θ0は、自己位置計測モジュール４において現在位置（Gx、Gy)と共にグローバル座標系GX-GY上のパーソナルモビリティの前方方向を計測することにより求めてもよいし、グローバル座標系GX-GY上の現在位置（Gx1、Gy1)の軌跡からパーソナルモビリティの前方方向を算定することにより求めてもよい。
【0036】
モデル式：
【0037】
【数2】
【0038】
ここで、モデル式中において、Rθ1はθ1回転させる回転行列、Rθ0はθ0回転させる回転行列である。
（Lx、Ly)は、図６ｃに示すように、自己位置計測モジュール４が現在位置を検出するパーソナルモビリティ上の基準位置BPを原点とし、LX軸、LY軸より規定されるローカル座標系LX-LY上の、第１LiDAR１の座標である。ここで、LX軸の方向はパーソナルモビリティの左方向であり、LY軸の方向はパーソナルモビリティの前方向である。
（Lx、Ly)は、図６ｃに示すように、自己位置計測モジュール４が現在位置を検出するパーソナルモビリティ上の基準位置BPを原点とし、LX軸、LY軸より規定されるローカル座標系LX-LY上の、第１LiDAR１の座標である。ここで、LX軸の方向はパーソナルモビリティの左方向であり、LY軸の方向はパーソナルモビリティの前方向である。
【0039】
モデル式中において、Rθ1（）の項は、グローバル座標系SGX-SGY上でSLAM９１が算定した第１LiDAR１の座標（SGx1、SGy1)をθ1回転させるものである。また、Rθ0（）の項は、各時点における、基準位置BPと第１LiDAR１のグローバル座標系GX-GY上での座標の差を表し、T()の項が示すθ1回転させた第１LiDAR１のグローバル座標系GX-GY上の座標を基準位置BPの座標に移動するために設けられている。
ここで、グローバル座標系SGX-SGYからグローバル座標系GX-GYへの座標変換行列T()に未知のパラメータがある場合、当該パラメータもθ1と共に、最適化アルゴリズムによって求められる。
ここで、グローバル座標系SGX-SGYからグローバル座標系GX-GYへの座標変換行列T()に未知のパラメータがある場合、当該パラメータもθ1と共に、最適化アルゴリズムによって求められる。
【0040】
たとえば、図６ｄに示すように、GX軸とSGX軸の方向が等しく、GY軸とSGY軸の方向が等しいことが既知である場合、グローバル座標系SGX-SGY原点のグローバル座標系GX-GY上の未知の座標を（Gx0、Gy0）とすると、
【0041】
【数3】
【0042】
となり、（Gx0、Gy0）が、T()の未知のパラメータとなる。
また、モデル式は、
また、モデル式は、
【0043】
【数4】
【0044】
によって表すことができ、当該モデル式を最も良く成立させるθ1、（Gx0、Gy0）を最適化アルゴリズムによって求め、求まったθ1をYaw１とする。
図５に戻り、以上のように、第１LiDAR１の幾何パラメータYaw１を算定したならば、設計値から定まる第１LiDAR１の移動体に対するｘ方向位置とｙ方向位置と、算定したYaw1を、第１LiDAR１の幾何パラメータ（x1、y1、Yaw1）としてSLAM９１に設定する（ステップ５０６）。第１LiDAR１の移動体に対するｘ方向位置とｙ方向位置としては、上述したLx1、Ly1を用いてもよい。
図５に戻り、以上のように、第１LiDAR１の幾何パラメータYaw１を算定したならば、設計値から定まる第１LiDAR１の移動体に対するｘ方向位置とｙ方向位置と、算定したYaw1を、第１LiDAR１の幾何パラメータ（x1、y1、Yaw1）としてSLAM９１に設定する（ステップ５０６）。第１LiDAR１の移動体に対するｘ方向位置とｙ方向位置としては、上述したLx1、Ly1を用いてもよい。
【0045】
また、第１LiDAR１の幾何パラメータと、第１キャリブレーション処理のステップ３０６で求めた第２LiDAR２の三次元点群の座標の座標変換行列から、第２LiDAR２の幾何パラメータ（x2、y2、Yaw2）を算定し、SLAM９１に設定する（ステップ５０８）。
【0046】
そして、第２キャリブレーション処理を終了する。
以上、本発明の実施形態について説明した。
なお、以上の説明では、第２LiDAR２の幾何パラメータを、第１LiDAR１の幾何パラメータと第１LiDAR１と第２LiDAR２との間の座標変換行列から求めたが、第２LiDAR２の幾何パラメータも第１LiDAR１の幾何パラメータと同様の算定法により求めるようにしてもよい。
以上、本発明の実施形態について説明した。
なお、以上の説明では、第２LiDAR２の幾何パラメータを、第１LiDAR１の幾何パラメータと第１LiDAR１と第２LiDAR２との間の座標変換行列から求めたが、第２LiDAR２の幾何パラメータも第１LiDAR１の幾何パラメータと同様の算定法により求めるようにしてもよい。
【0047】
また、以上の第２キャリブレーション処理では、自己位置計測モジュール４が検出した現在位置（Gx、Gy)に代えて、GNSSモジュール３で測位した現在位置や、別途、慣性航法で現在位置を検出する慣性航法装置で検出した現在位置を用いてもよい。
以上のように、本実施形態によれば、外部の計測装置による計測を必要とすることなく、LiDARで検出した三次元点群自身を用いて、LiDARの移動体に対する高さやロールやピッチの幾何パラメータを精度良く検出することができる。また、移動体が、自律航法や衛星測位や慣性航法によって現在位置を算出する機能を備えている場合には、当該機能とLiDAR SLAMのみを用いて、LiDARの移動体に対するヨウの幾何パラメータを精度良く検出することができる。
以上のように、本実施形態によれば、外部の計測装置による計測を必要とすることなく、LiDARで検出した三次元点群自身を用いて、LiDARの移動体に対する高さやロールやピッチの幾何パラメータを精度良く検出することができる。また、移動体が、自律航法や衛星測位や慣性航法によって現在位置を算出する機能を備えている場合には、当該機能とLiDAR SLAMのみを用いて、LiDARの移動体に対するヨウの幾何パラメータを精度良く検出することができる。
【符号の説明】
【0048】
１…第１LiDAR、２…第２LiDAR、３…GNSSモジュール、４…自己位置計測モジュール、５…モニタ、６…操作装置、７…走行機構、８…状態センサ、９…制御装置、９１…SLAM、９２…キャリブレーション処理部、９３…運転制御部。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
