知財求人 - 知財ポータルサイト「IP Force」
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2024134853
(43)【公開日】2024-10-04
(54)【発明の名称】補正装置および補正方法
(51)【国際特許分類】
B61L 23/00 20060101AFI20240927BHJP
【ＦＩ】
B61L23/00 Z
【審査請求】未請求
【請求項の数】17
【出願形態】ＯＬ
(21)【出願番号】P 2023045263
(22)【出願日】2023-03-22
(71)【出願人】
【識別番号】000005108
【氏名又は名称】株式会社日立製作所
(74)【代理人】
【識別番号】110002365
【氏名又は名称】弁理士法人サンネクスト国際特許事務所
(72)【発明者】
【氏名】工藤 匠
(72)【発明者】
【氏名】清水 拓
(72)【発明者】
【氏名】今本 健二
(72)【発明者】
【氏名】佐藤 究
【テーマコード（参考）】
5H161
【Ｆターム（参考）】
5H161AA01
5H161MM05
5H161MM12
5H161NN10
5H161NN12
(57)【要約】
【課題】キャリブレーション用の設備を設けなくても、より正確なキャリブレーションを行うことができる補正装置、補正方法を提供する。
【解決手段】第一のセンサ１０１ａによって取得される第一のセンサ情報に基づき、レール２０Ｌｔ、２０Ｒｔの位置を表す第一のレール点群３０ａｎを検出するレール検出部１０２と、第一のレール点群３０ａｎに対する直線の度合いを表す第一の直線度に基づき、第一のセンサ１０１ａのキャリブレーションの可否判断を行うキャリブレーション可否判断部１０３と、キャリブレーション可否判断部１０３においてキャリブレーション可能と判断された場合に、第一のレール点群３０ａｎと、レールの３次元位置を表し第一のレール点群とは異なる参照レール点群である点群３０ａｔとを基に、第一のセンサ情報を補正するセンサ情報補正部１０５と、を有する補正装置。
【選択図】図１
【解決手段】第一のセンサ１０１ａによって取得される第一のセンサ情報に基づき、レール２０Ｌｔ、２０Ｒｔの位置を表す第一のレール点群３０ａｎを検出するレール検出部１０２と、第一のレール点群３０ａｎに対する直線の度合いを表す第一の直線度に基づき、第一のセンサ１０１ａのキャリブレーションの可否判断を行うキャリブレーション可否判断部１０３と、キャリブレーション可否判断部１０３においてキャリブレーション可能と判断された場合に、第一のレール点群３０ａｎと、レールの３次元位置を表し第一のレール点群とは異なる参照レール点群である点群３０ａｔとを基に、第一のセンサ情報を補正するセンサ情報補正部１０５と、を有する補正装置。
【選択図】図１
【特許請求の範囲】
【請求項1】
第一のセンサによって取得される第一のセンサ情報に基づき、レールの位置を表す第一のレール点群を検出するレール検出部と、
前記第一のレール点群に対する直線の度合いを表す第一の直線度に基づき、前記第一のセンサのキャリブレーションの可否判断を行うキャリブレーション可否判断部と、
前記キャリブレーション可否判断部においてキャリブレーション可能と判断された場合に、前記第一のレール点群と、レールの３次元位置を表し前記第一のレール点群とは異なる参照レール点群とを基に、前記第一のセンサ情報を補正するセンサ情報補正部と、
を有する補正装置。
前記第一のレール点群に対する直線の度合いを表す第一の直線度に基づき、前記第一のセンサのキャリブレーションの可否判断を行うキャリブレーション可否判断部と、
前記キャリブレーション可否判断部においてキャリブレーション可能と判断された場合に、前記第一のレール点群と、レールの３次元位置を表し前記第一のレール点群とは異なる参照レール点群とを基に、前記第一のセンサ情報を補正するセンサ情報補正部と、
を有する補正装置。
【請求項2】
前記第一の直線度は、前記直線の度合いに加え、前記第一のレール点群に対するばらつきの程度を表す請求項１に記載の補正装置。
【請求項3】
前記キャリブレーション可否判断部は、前記第一のレール点群から直線抽出を行うことで推定されるレール直線モデルと前記第一のレール点群に含まれる点との距離の平均値に基づき、前記第一の直線度を求める請求項２に記載の補正装置。
【請求項4】
前記キャリブレーション可否判断部は、一対のレールのそれぞれについての前記平均値として、第一の距離平均値および第二の距離平均値を求め、前記第一の距離平均値と前記第二の距離平均値との平均値の逆数を、前記第一の直線度とする請求項３に記載の補正装置。
【請求項5】
前記キャリブレーション可否判断部は、前記第一のレール点群から分岐の有無を判定し、分岐がある場合はキャリブレーション不可と判断する請求項１に記載の補正装置。
【請求項6】
前記キャリブレーション可否判断部は、前記第一のレール点群について、車両の幅方向の距離と軌間とを比較することにより分岐の有無を判定する請求項５に記載の補正装置。
【請求項7】
前記参照レール点群は、第二のセンサによって取得される第二のセンサ情報に基づき検出され、レールの位置を表す第二のレール点群である請求項１に記載の補正装置。
【請求項8】
前記キャリブレーション可否判断部は、前記第一の直線度と、前記第二のレール点群に対する直線の度合いを表す第二の直線度との差に基づきキャリブレーションの可否判断を行う請求項７に記載の補正装置。
【請求項9】
前記キャリブレーション可否判断部においてキャリブレーション可能と判断された場合に、前記第一のレール点群と前記参照レール点群とを用い、前記第一のセンサの三軸回転及び併進を求めるキャリブレーション部をさらに備え、
前記センサ情報補正部は、前記キャリブレーション部で求められた前記三軸回転及び前記併進を用い、前記第一のセンサ情報を補正する請求項１に記載の補正装置。
前記センサ情報補正部は、前記キャリブレーション部で求められた前記三軸回転及び前記併進を用い、前記第一のセンサ情報を補正する請求項１に記載の補正装置。
【請求項10】
前記キャリブレーション部は、前記第一のセンサの前記三軸回転及び前記併進を複数時刻記録し、複数時刻における前記第一のセンサの前記三軸回転及び前記併進の中央値を出力し、
前記センサ情報補正部は、出力された前記中央値を用い、前記第一のセンサ情報を補正する請求項９に記載の補正装置。
前記センサ情報補正部は、出力された前記中央値を用い、前記第一のセンサ情報を補正する請求項９に記載の補正装置。
【請求項11】
前記第一のレール点群は、レールの二次元位置を表し、
前記キャリブレーション部は、前記参照レール点群を、前記第一のレール点群を含む画像座標系へ変換して２次元の前記参照レール点群を取得し、前記第一のレール点群と２次元の前記参照レール点群とを一致させるための、前記三軸回転及び前記併進を算出する請求項９に記載の補正装置。
前記キャリブレーション部は、前記参照レール点群を、前記第一のレール点群を含む画像座標系へ変換して２次元の前記参照レール点群を取得し、前記第一のレール点群と２次元の前記参照レール点群とを一致させるための、前記三軸回転及び前記併進を算出する請求項９に記載の補正装置。
【請求項12】
前記キャリブレーション部において求められる前記三軸回転及び前記併進が、予め定められた閾値を上回る場合に、センサ取付角異常と判定するセンサ取付異常検出部をさらに有する請求項９に記載の補正装置。
【請求項13】
前記センサ取付異常検出部は、前記第一のレール点群と、第二のセンサによって取得される第二のセンサ情報に基づき検出され、レールの位置を表す第二のレール点群としての参照レール点群と、から求められる前記三軸回転及び前記併進、および前記第一のレール点群と、第三のセンサによって取得される第三のセンサ情報に基づき検出され、レールの位置を表す第三のレール点群と、から求められる前記三軸回転及び前記併進が、ともに予め定められた閾値を上回る場合に、センサ取付角異常と判定する請求項１２に記載の補正装置。
【請求項14】
前記キャリブレーション可否判断部は、車両の在線位置とキャリブレーション可能区間との関係を含むマップに基づき、前記第一のレール点群が、キャリブレーション可能区間に含まれる場合に、キャリブレーション可能と判断する請求項１に記載の補正装置。
【請求項15】
前記キャリブレーション可否判断部は、前記第一の直線度がキャリブレーション不可の値であり、かつ前記第一のレール点群が、キャリブレーション可能区間に含まれる場合に、センサ異常信号を発出する請求項１４に記載の補正装置。
【請求項16】
前記レール検出部は、前記第一のレール点群の代わりに第一の架線柱点群を検出する請求項１に記載の補正装置。
【請求項17】
プロセッサがメモリに記録されたソフトウェアを実行することにより、
第一のセンサによって取得される第一のセンサ情報に基づき、レールの位置を表す第一のレール点群を検出し、
前記第一のレール点群に対する直線の度合いを表す第一の直線度に基づき、前記第一のセンサのキャリブレーションの可否判断を行い、
キャリブレーション可能と判断された場合に、前記第一のレール点群と、レールの３次元位置を表し前記第一のレール点群とは異なる参照レール点群とを基に、前記第一のセンサ情報を補正する、
補正方法。
第一のセンサによって取得される第一のセンサ情報に基づき、レールの位置を表す第一のレール点群を検出し、
前記第一のレール点群に対する直線の度合いを表す第一の直線度に基づき、前記第一のセンサのキャリブレーションの可否判断を行い、
キャリブレーション可能と判断された場合に、前記第一のレール点群と、レールの３次元位置を表し前記第一のレール点群とは異なる参照レール点群とを基に、前記第一のセンサ情報を補正する、
補正方法。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、補正装置、補正方法に関する。特に、列車に搭載されたセンサから出力されるセンサ情報を補正するのに適した補正装置、補正方法に関する。
【背景技術】
【0002】
例えば、列車前方監視システムは、監視領域や周波数特性の異なる複数センサを搭載することで、前方監視領域の拡大や物体検知率の向上が期待される。このようなシステムでは、各センサの外部パラメータ（取付位置、角度情報）使用することで、複数のセンサ情報を統合する。各センサの外部パラメータは車両によって個体差があり、車体振動等により経年変化するため、実際の外部パラメータは設計値に対して誤差を有する。この誤差が拡大すると各センサに含まれる物体の位置誤差が拡大し、センサ情報の統合が困難となる。従って、複数のセンサ情報を統合する際にセンサ外部パラメータ誤差に起因する失敗を防ぐため、各センサの位置、姿勢といった外部パラメータを算出し補正をする必要がある。即ち、キャリブレーションを行い、センサ情報の補正を行う補正装置等が必要である。
【0003】
特許文献１には、自車の形状に対応した２点と交わる線を含み、カメラの撮像範囲内に少なくとも２本配置される参照線と、この参照線の間に予め定められたパターン形状を有する校正用パターンとを有する測定領域と、画像処理部とを備えるカメラキャリブレーション装置が記載されている。そして、この装置は、画像処理部が、カメラで校正用パターン及び参照線が配置される測定領域を撮影することで測定用画像を生成する撮像処理と、２本の参照線に基づいて自車の停車位置とパターン形状との座標関係を停車誤差として算出する停車誤差算出処理と、当該停車誤差をキャンセルした状態でパターン形状に基づいてカメラの取付姿勢に対応する外部パラメータを算出する外部パラメータ算出処理とを備える。
【0004】
特許文献２には、車両の屋根上の枕木方向に沿って配置されたカメラによってそれぞれ車両の前方または後方のレールを撮像した画像に基づいて傾き推定装置に、カメラによって撮像した画像からそれぞれレールの位置を求めるレール検出部と、カメラによって撮像した画像からそれぞれ求めたレールの位置に基づきレールの三次元形状を算出する三次元形状算出部と、ＩＣＰアルゴリズムによりレールの三次元形状と予め用意したレール形状モデルとの位置合わせを行う際にＭ推定またはＲＡＮＳＡＣを用いてノイズの影響を抑制する傾き推定部とを設けた車体の傾き推定装置が記載されている。
【先行技術文献】
【特許文献】
【0005】
【特許文献1】特開２０１３－２８２０号公報
【特許文献2】特開２０１９－２３０１７号公報
【発明の概要】
【発明が解決しようとする課題】
【0006】
しかしながら、測定領域を設置することは、設置コストのほか、汚れや欠損といった劣化を防ぐための保守が必要となることが見込まれるため、このようなものは設置しない方が望ましい。
本発明は、キャリブレーション用の設備を設けなくても、より正確なキャリブレーションを行うことができる補正装置、補正方法を提供することを目的とする。
本発明は、キャリブレーション用の設備を設けなくても、より正確なキャリブレーションを行うことができる補正装置、補正方法を提供することを目的とする。
【課題を解決するための手段】
【0007】
上記の課題を解決するため本発明は、第一のセンサによって取得される第一のセンサ情報に基づき、レールの位置を表す第一のレール点群を検出するレール検出部と、第一のレール点群に対する直線の度合いを表す第一の直線度に基づき、第一のセンサのキャリブレーションの可否判断を行うキャリブレーション可否判断部と、キャリブレーション可否判断部においてキャリブレーション可能と判断された場合に、第一のレール点群と、レールの３次元位置を表し第一のレール点群とは異なる参照レール点群とを基に、第一のセンサ情報を補正するセンサ情報補正部と、を有する補正装置である。この場合、キャリブレーション用の設備を設けなくても、より正確なキャリブレーションを行うことができる補正装置を提供することができる。
【0008】
ここで、第一の直線度は、直線の度合いに加え、第一のレール点群に対するばらつきの程度を表すようにできる。この場合、直線度の評価の精度を向上させることができる。
また、キャリブレーション可否判断部は、第一のレール点群から直線抽出を行うことで推定されるレール直線モデルと第一のレール点群に含まれる点との距離の平均値に基づき、第一の直線度を求めるようにできる。この場合、直線度をより容易に求めることができる。
さらに、キャリブレーション可否判断部は、一対のレールのそれぞれについての平均値として、第一の距離平均値および第二の距離平均値を求め、第一の距離平均値と第二の距離平均値との平均値の逆数を、第一の直線度とすることができる。この場合、直線度としてより直感的な値を求めることができる。
また、キャリブレーション可否判断部は、第一のレール点群から直線抽出を行うことで推定されるレール直線モデルと第一のレール点群に含まれる点との距離の平均値に基づき、第一の直線度を求めるようにできる。この場合、直線度をより容易に求めることができる。
さらに、キャリブレーション可否判断部は、一対のレールのそれぞれについての平均値として、第一の距離平均値および第二の距離平均値を求め、第一の距離平均値と第二の距離平均値との平均値の逆数を、第一の直線度とすることができる。この場合、直線度としてより直感的な値を求めることができる。
【0009】
またさらに、キャリブレーション可否判断部は、第一のレール点群から分岐の有無を判定し、分岐がある場合はキャリブレーション不可と判断することができる。この場合、外部パラメータの推定がより正確になる。
そして、キャリブレーション可否判断部は、第一のレール点群について、車両の幅方向の距離とレールの軌間とを比較することにより分岐の有無を判定するようにできる。この場合、分岐の判定をより容易に行うことができる。
そして、キャリブレーション可否判断部は、第一のレール点群について、車両の幅方向の距離とレールの軌間とを比較することにより分岐の有無を判定するようにできる。この場合、分岐の判定をより容易に行うことができる。
【0010】
また、参照レール点群は、第二のセンサによって取得される第二のセンサ情報に基づき検出され、レールの位置を表す第二のレール点群であるようにできる。この場合、第一のセンサと第二のセンサの各センサ情報を統合することができる。
さらに、キャリブレーション可否判断部は、第一の直線度と、第二のレール点群に対する直線の度合いを表す第二の直線度との差に基づきキャリブレーションの可否判断を行うようにできる。この場合、キャリブレーションの可否判断がより正確になる。
さらに、キャリブレーション可否判断部は、第一の直線度と、第二のレール点群に対する直線の度合いを表す第二の直線度との差に基づきキャリブレーションの可否判断を行うようにできる。この場合、キャリブレーションの可否判断がより正確になる。
【0011】
そして、キャリブレーション可否判断部においてキャリブレーション可能と判断された場合に、第一のレール点群と参照レール点群とを用い、第一のセンサの三軸回転及び併進を求めるキャリブレーション部をさらに備え、センサ情報補正部は、キャリブレーション部で求められた三軸回転及び併進を用い、第一のセンサ情報を補正することができる。この場合、第一のセンサ情報の補正の精度が向上する。
また、キャリブレーション部は、第一のセンサの三軸回転及び併進を複数時刻記録し、複数時刻における第一のセンサの三軸回転及び併進の中央値を出力し、センサ情報補正部は、出力された中央値を用い、第一のセンサ情報を補正することができる。この場合、より正確な外部パラメータの推定が可能となる。
さらに、第一のレール点群は、レールの二次元位置を表し、キャリブレーション部は、参照レール点群を、第一のレール点群を含む画像座標系へ変換して２次元の参照レール点群を取得し、第一のレール点群と２次元の参照レール点群とを一致させるための、三軸回転及び併進を算出することができる。この場合、二次元画像上の点群を三次元化する際に発生する、レール点群の角度誤差などが発生せず、より正確な外部パラメータの推定結果を得られやすい。
またさらに、キャリブレーション部において求められる三軸回転及び併進が、予め定められた閾値を上回る場合に、センサ取付角異常と判定するセンサ取付異常検出部をさらに有するようにできる。この場合、センサ取付異常を検出することができる。
そして、センサ取付異常検出部は、第一のレール点群と、第二のセンサによって取得される第二のセンサ情報に基づき検出され、レールの位置を表す第二のレール点群としての参照レール点群と、から求められる三軸回転及び併進、および第一のレール点群と、第三のセンサによって取得される第三のセンサ情報に基づき検出され、レールの位置を表す第三のレール点群と、から求められる三軸回転及び併進が、ともに予め定められた閾値を上回る場合に、センサ取付角異常と判定するようにできる。この場合、第一のセンサと第二のセンサを用い、センサ取付異常を判定することができる。
また、キャリブレーション部は、第一のセンサの三軸回転及び併進を複数時刻記録し、複数時刻における第一のセンサの三軸回転及び併進の中央値を出力し、センサ情報補正部は、出力された中央値を用い、第一のセンサ情報を補正することができる。この場合、より正確な外部パラメータの推定が可能となる。
さらに、第一のレール点群は、レールの二次元位置を表し、キャリブレーション部は、参照レール点群を、第一のレール点群を含む画像座標系へ変換して２次元の参照レール点群を取得し、第一のレール点群と２次元の参照レール点群とを一致させるための、三軸回転及び併進を算出することができる。この場合、二次元画像上の点群を三次元化する際に発生する、レール点群の角度誤差などが発生せず、より正確な外部パラメータの推定結果を得られやすい。
またさらに、キャリブレーション部において求められる三軸回転及び併進が、予め定められた閾値を上回る場合に、センサ取付角異常と判定するセンサ取付異常検出部をさらに有するようにできる。この場合、センサ取付異常を検出することができる。
そして、センサ取付異常検出部は、第一のレール点群と、第二のセンサによって取得される第二のセンサ情報に基づき検出され、レールの位置を表す第二のレール点群としての参照レール点群と、から求められる三軸回転及び併進、および第一のレール点群と、第三のセンサによって取得される第三のセンサ情報に基づき検出され、レールの位置を表す第三のレール点群と、から求められる三軸回転及び併進が、ともに予め定められた閾値を上回る場合に、センサ取付角異常と判定するようにできる。この場合、第一のセンサと第二のセンサを用い、センサ取付異常を判定することができる。
【0012】
さらに、キャリブレーション可否判断部は、車両の在線位置とキャリブレーション可能区間との関係を含むマップに基づき、第一のレール点群が、キャリブレーション可能区間に含まれる場合に、キャリブレーション可能と判断するようにできる。この場合、カーブ区間を含み外部パラメータの推定が難しい領域を、事前に把握し、さらに確実に外部パラメータの推定を行うことができる。
そして、キャリブレーション可否判断部は、第一の直線度がキャリブレーション不可の値であり、かつ第一のレール点群が、キャリブレーション可能区間に含まれる場合に、センサ異常信号を発出するようにできる。この場合、マップを利用してセンサ取付異常を検出することができる。
さらに、レール検出部は、第一のレール点群の代わりに互いに平行な物体の位置を表す点群を検出するようにしてもよい。例えばレール検出部は、第一のレール点群の代わりに第一の架線柱点群を検出する。この場合、架線柱に例示される互いに平行な物体を利用してキャリブレーションに必要な点群を取得することができる。
そして、キャリブレーション可否判断部は、第一の直線度がキャリブレーション不可の値であり、かつ第一のレール点群が、キャリブレーション可能区間に含まれる場合に、センサ異常信号を発出するようにできる。この場合、マップを利用してセンサ取付異常を検出することができる。
さらに、レール検出部は、第一のレール点群の代わりに互いに平行な物体の位置を表す点群を検出するようにしてもよい。例えばレール検出部は、第一のレール点群の代わりに第一の架線柱点群を検出する。この場合、架線柱に例示される互いに平行な物体を利用してキャリブレーションに必要な点群を取得することができる。
【0013】
また、本発明は、プロセッサがメモリに記録されたソフトウェアを実行することにより、第一のセンサによって取得される第一のセンサ情報に基づき、レールの位置を表す第一のレール点群を検出し、第一のレール点群に対する直線の度合いを表す第一の直線度に基づき、第一のセンサのキャリブレーションの可否判断を行い、キャリブレーション可能と判断された場合に、第一のレール点群と、レールの３次元位置を表し第一のレール点群とは異なる参照レール点群とを基に、第一のセンサ情報を補正する、補正方法である。この場合、キャリブレーション用の設備を設けなくても、より正確なキャリブレーションを行うことができる補正方法を提供することができる。
【発明の効果】
【0014】
本発明によれば、キャリブレーション用の設備を設けなくても、より正確なキャリブレーションを行うことができる補正装置、補正方法を提供することができる。
【図面の簡単な説明】
【0015】
【図1】第１の実施形態におけるセンサ補正システムの全体構成を示した図である。
【図2】第１の実施形態において、第一のセンサ情報を取得してから第一のセンサの情報を補正するまでの動作を表すフローチャート図である。
【図3】（ａ）～（ｂ）は、直線度の算出方法を示した図である。（ｃ）～（ｅ）は、直線度を算出する際の具体例を表す図である。
【図4】（ａ）～（ｂ）は、キャリブレーション可否判断部に分岐判定機能を追加したときの、分岐判定を行う方法の具体例を表す図である。
【図5】（ａ）～（ｄ）は、第１の実施形態で使用する座標系と、第一のレール点群を用いて外部パラメータを推定する方法を表す図である。
【図6】横軸に外部パラメータの推定結果の蓄積フレーム数、縦軸に外部パラメータの推定結果を示した図である。
【図7】第２の実施形態におけるセンサ補正システムの全体構成を示した図である。
【図8】（ａ）～（ｅ）は、第２の実施形態における自動キャリブレーション部の動作を示した図である。
【図9】単眼カメラ情報を、第一のセンサ情報として使用した場合における自動キャリブレーション装置の動作を表すフローチャート図である。
【図11】第４の実施形態におけるセンサ補正システムの全体構成を示した図である。
【図12】（ａ）～（ｂ）は、鉄道デジタルマップ、及び鉄道デジタルマップを用いて外部パラメータの推定の可否を判断する方法を表す図である。
【図13】第５の実施形態におけるセンサ補正システムの全体構成を示した図である。
【発明を実施するための形態】
【0016】
以下、添付図面を参照し、本発明の実施の形態について、詳細に説明する。ここでは、本発明を第１の実施形態～第５の実施形態により説明する。
〔第１の実施形態〕
＜センサ補正システム１ａの全体構成の説明＞
図１は、第１の実施形態におけるセンサ補正システム１ａの全体構成を示した図である。
図示するように、センサ補正システム１ａは、車両１０に搭載される。車両１０は、鉄軌道を走行する車両筐体であり、例えば鉄道やトラムなどが挙げられる。車両１０は、一対のレール２０Ｌｔ、２０Ｒｔ上を走行する。なおここでは、車両１０の進行方向に対し、左側になるレールをレール２０Ｌｔとし、右側になるレールを２０Ｒｔとしている。
〔第１の実施形態〕
＜センサ補正システム１ａの全体構成の説明＞
図１は、第１の実施形態におけるセンサ補正システム１ａの全体構成を示した図である。
図示するように、センサ補正システム１ａは、車両１０に搭載される。車両１０は、鉄軌道を走行する車両筐体であり、例えば鉄道やトラムなどが挙げられる。車両１０は、一対のレール２０Ｌｔ、２０Ｒｔ上を走行する。なおここでは、車両１０の進行方向に対し、左側になるレールをレール２０Ｌｔとし、右側になるレールを２０Ｒｔとしている。
【0017】
またセンサ補正システム１ａは、第一のセンサ１０１ａと、自動キャリブレーション装置１１ａとを備える。
自動キャリブレーション装置１１ａは、補正装置の一例であり、レール検出部１０２、キャリブレーション可否判断部１０３、自動キャリブレーション部１０４、センサ情報補正部１０５を備え、第一のセンサ１０１ａが取得する情報を入力とする。なお、本実施の形態では、自動キャリブレーション部１０４を車両１０に搭載されているものとしているが、センサ１０１ａの情報を地上に送信する手段を有する場合、地上に設置した設備に搭載しても良い。
自動キャリブレーション装置１１ａは、補正装置の一例であり、レール検出部１０２、キャリブレーション可否判断部１０３、自動キャリブレーション部１０４、センサ情報補正部１０５を備え、第一のセンサ１０１ａが取得する情報を入力とする。なお、本実施の形態では、自動キャリブレーション部１０４を車両１０に搭載されているものとしているが、センサ１０１ａの情報を地上に送信する手段を有する場合、地上に設置した設備に搭載しても良い。
【0018】
自動キャリブレーション装置１１ａは、第一のセンサ１０１ａが取得したデータを用い、レール検出部１０２が、車両１０の前方にある左右のレール２０Ｌｔ、２０Ｒｔの位置を推定し、第一のレール点群３０ａｎとして取得する。さらに、キャリブレーション可否判断部１０３は、第一のレール点群３０ａｎの直線度を基にキャリブレーション可否を判断する。そして、キャリブレーションが可能であると判断された場合に、自動キャリブレーション部１０４において、外部パラメータの推定を行う。さらに、センサ情報補正部１０５は、外部パラメータの推定結果を用い、第一のセンサ１０１ａが取得したデータを補正する。
【0019】
第一のセンサ１０１ａは、レール検出部１０２によって第一のレール点群３０ａｎを推定する元となる第一のセンサ情報を出力するセンサである。第一のセンサ１０１ａとしては、例えば、レーザーレーダやステレオカメラ、単眼カメラなどが挙げられる。第一のセンサ情報は、第一のセンサ１０１ａがレーザーレーダの場合は、三次元点群である。また、第一のセンサ１０１ａがステレオカメラの場合は、視差画像及び左右カメラ画像である。さらに、第一のセンサ１０１ａが単眼カメラの場合は、二次元画像である。
【0020】
レール検出部１０２は、第一のセンサ情報を用いて三次元点群を推定する機能を有する。また、レール検出部１０２は、第一のセンサ１０１ａによって取得される第一のセンサ情報に基づき、レール２０Ｌｔ、２０Ｒｔの位置を表す第一のレール点群３０ａｎを検出する、と言うこともできる。
第一のセンサ情報がレーザーレーダの出力する三次元点群の場合は、レール検出部１０２は、三次元点群の中から左右のレール２０Ｌｔ、２０Ｒｔに対応する点群を抽出し、第一のレール点群３０ａｎとする。第一のセンサ情報がステレオカメラの視差画像及び左右カメラ画像の場合は、レール検出部１０２は、画像上のパターンマッチングなどを利用した二次元レール抽出と、視差画像による奥行き推定結果等を用いて第一のレール点群３０ａｎを推定する。入力画像が単眼カメラ画像の場合は、レール検出部１０２は、例えば画像上のパターンマッチングなどを利用した二次元レール抽出を行い、実際の左右レール２０Ｌｔ、２０Ｒｔの幅（＝軌間）と観測された軌間との比較に基づく奥行き推定結果を使用し、第一のレール点群３０ａｎを推定する。
第一のセンサ情報がレーザーレーダの出力する三次元点群の場合は、レール検出部１０２は、三次元点群の中から左右のレール２０Ｌｔ、２０Ｒｔに対応する点群を抽出し、第一のレール点群３０ａｎとする。第一のセンサ情報がステレオカメラの視差画像及び左右カメラ画像の場合は、レール検出部１０２は、画像上のパターンマッチングなどを利用した二次元レール抽出と、視差画像による奥行き推定結果等を用いて第一のレール点群３０ａｎを推定する。入力画像が単眼カメラ画像の場合は、レール検出部１０２は、例えば画像上のパターンマッチングなどを利用した二次元レール抽出を行い、実際の左右レール２０Ｌｔ、２０Ｒｔの幅（＝軌間）と観測された軌間との比較に基づく奥行き推定結果を使用し、第一のレール点群３０ａｎを推定する。
【0021】
第一のレール点群３０ａｎは、左右のレール２０Ｌｔ、２０Ｒｔの三次元位置に対応する点群であり、例えば左右のレール２０Ｌｔ、２０Ｒｔの最上面の内側に対応する点群とすることができる。また、本実施の形態において、第一のレール点群３０ａｎは、第一のセンサ座標系から車両１０に固定された車両固定座標系４０ｔに変換するための回転設計値、併進設計値を用いて座標変換を行い、車両固定座標系’４０ｎで表現されているものとする。
【0022】
車両固定座標系４０ｔは、車両１０に対して原点や向きを固定された座標系である。本実施の形態では、車両固定座標系４０ｔのｘ方向をレール２０Ｌｔ、２０Ｒｔに沿った直進方向、ｙ方向を枕木に沿った方向、ｚ方向をｘ、ｙと垂直な方向とし、ｘ原点を車体前面、ｙ原点を左右のレール２０Ｌｔ、２０Ｒｔの中線上、ｚ原点を左右のレール２０Ｌｔ、２０Ｒｔ上面が張る平面上と定義するが、他の方法でもよい。
【0023】
車両固定座標系’４０ｎは、第一のセンサ座標系のセンサ情報を回転設計値、併進設計値を用いて座標変換した結果遷移する座標系である。回転設計値、併進設計値が実際の回転、併進とは異なるため、車両固定座標系’４０ｎの原点や軸の向きは、実際の車両固定座標系４０ｔとは異なる。そのため、第一のレール点群３０ａｎは、車両固定座標系４０ｔにおける実際のレール２０Ｌｔ、２０Ｒｔの位置に対応する点群３０ａｔとは異なる位置に存在する。
【0024】
キャリブレーション可否判断部１０３は、第一のレール点群３０ａｎを直線度によって評価する。そして、キャリブレーション可否判断部１０３は、直線度に閾値を設定し、この閾値に基づきキャリブレーションの可否を判断する。キャリブレーション可否判断部１０３は、例えば、直線度が所定の閾値以上である場合に、キャリブレーション可能であると判断し、直線度が所定の閾値未満である場合に、キャリブレーション不可であると判断する。よって、キャリブレーション可否判断部１０３は、第一のレール点群３０ａｎに対する直線の度合いを表す直線度に基づき、第一のセンサ１０１ａのキャリブレーションの可否判断を行う、と言うこともできる。なお、この直線度は、後述する第二の直線度と区別する意味で第一の直線度であると言うこともできる。
そして、キャリブレーション可否判断部１０３は、キャリブレーション実施可否を判断するためのキャリブレーション可否信号と、第一のレール点群３０ａｎとを、自動キャリブレーション部１０４へ出力する機能を有する。
そして、キャリブレーション可否判断部１０３は、キャリブレーション実施可否を判断するためのキャリブレーション可否信号と、第一のレール点群３０ａｎとを、自動キャリブレーション部１０４へ出力する機能を有する。
【0025】
直線度とは、推定された第一のレール点群３０ａｎが直線か否か、及び第一のレール点群３０ａｎのデータのばらつきの程度を評価するための指標であり、詳細は後述する。直線度の閾値は、自動キャリブレーション部１０４における外部パラメーのタ推定精度が要求を満たすことのできるよう、事前に調整する値である。
【0026】
キャリブレーション可否信号とは、自動キャリブレーション部１０４が第一のレール点群３０ａｎを用いた外部パラメータの推定処理を実行してよいかを判断するための情報である。キャリブレーション可否信号は、例えば、０または１の信号とし、０がキャリブレーション不可であること、１をキャリブレーション可能であることを意味する。
【0027】
自動キャリブレーション部１０４は、キャリブレーション可否信号によってキャリブレーション可能と判断された場合に、第一のレール点群３０ａｎと、実際のレール２０Ｌｔ、２０Ｒｔの位置に対応する点群３０ａｔを用い、第一のセンサ１０１ａの外部パラメータの推定を行う機能を有する。よって、自動キャリブレーション部１０４は、キャリブレーション可否判断部１０３においてキャリブレーション可能と判断された場合に、第一のレール点群３０ａｎと参照レール点群（第１の実施形態では、実際のレール２０Ｌｔ、２０Ｒｔの位置に対応する点群３０ａｔ）とを用い、第一のセンサ１０１ａの三軸回転及び併進を求めるキャリブレーション部として機能する。第一のレール点群３０ａｎを用いた外部パラメータの推定方法の詳細は後述する。
【0028】
センサ情報補正部１０５は、第一のセンサ１０１ａの外部パラメータの推定結果を用い、第一のセンサ１０１ａのデータを車両固定座標系４０ｔに変換する機能を有する。センサ情報補正部１０５は、キャリブレーション可否判断部１０３においてキャリブレーション可能と判断された場合に、第一のレール点群３０ａｎと、レール２０Ｌｔ、２０Ｒｔの３次元位置を表し第一のレール点群ａｎとは異なる参照レール点群（第１の実施形態では、実際のレール２０Ｌｔ、２０Ｒｔの位置に対応する点群３０ａｔ）とを基に、第一のセンサ情報を補正する、と言うこともできる。この場合、センサ情報補正部１０５は、自動キャリブレーション部１０４で求められた三軸回転及び併進を用い、第一のセンサ情報を補正する。
【0029】
図２は、第１の実施形態において、第一のセンサ情報を取得してから第一のセンサ１０１ａの情報を補正するまでの動作を表すフローチャート図である。
まず、レール検出部１０２において、ある時刻における第一のセンサ情報を取得する（Ｓ１０１）。
次に、レール検出部１０２は、第一のセンサ情報のデータ次元が三次元であるか否かを判断する（Ｓ１０２）。
その結果、データ次元が三次元である場合（Ｓ１０２でＹｅｓ）、レール検出部１０２は、三次元点群に対するレール２０Ｌｔ、２０Ｒｔの検出を実施する（Ｓ１０３）。データ次元が三次元である場合は、例えばレーザレーダにより取得した点群の場合である。
対して、データ次元が二次元である場合（Ｓ１０２でＮｏ）、レール検出部１０２は、まず二次元画像上でレール２０Ｌｔ、２０Ｒｔを検出し（Ｓ１０４）、視差画像や軌間を用いてレール検知結果の三次元化を行い（Ｓ１０５）、キャリブレーション可否判断部１０３へ出力する。データ次元が二次元である場合は、例えば、ステレオカメラ画像や単眼カメラ画像の場合である。
まず、レール検出部１０２において、ある時刻における第一のセンサ情報を取得する（Ｓ１０１）。
次に、レール検出部１０２は、第一のセンサ情報のデータ次元が三次元であるか否かを判断する（Ｓ１０２）。
その結果、データ次元が三次元である場合（Ｓ１０２でＹｅｓ）、レール検出部１０２は、三次元点群に対するレール２０Ｌｔ、２０Ｒｔの検出を実施する（Ｓ１０３）。データ次元が三次元である場合は、例えばレーザレーダにより取得した点群の場合である。
対して、データ次元が二次元である場合（Ｓ１０２でＮｏ）、レール検出部１０２は、まず二次元画像上でレール２０Ｌｔ、２０Ｒｔを検出し（Ｓ１０４）、視差画像や軌間を用いてレール検知結果の三次元化を行い（Ｓ１０５）、キャリブレーション可否判断部１０３へ出力する。データ次元が二次元である場合は、例えば、ステレオカメラ画像や単眼カメラ画像の場合である。
【0030】
次に、キャリブレーション可否判断部１０３が、第一のレール点群３０ａｎによる直線度の算出を実施する（Ｓ１０６）。
そして、キャリブレーション可否判断部１０３は、直線度が所定の閾値以上である場合はキャリブレーション可否信号を１とし、直線度が所定の閾値未満である場合はキャリブレーション可否信号を０とする（Ｓ１０７）。
そして、キャリブレーション可否判断部１０３は、直線度が所定の閾値以上である場合はキャリブレーション可否信号を１とし、直線度が所定の閾値未満である場合はキャリブレーション可否信号を０とする（Ｓ１０７）。
【0031】
次に、自動キャリブレーション部１０４が、キャリブレーション可否信号が１であるか否かを判断する（Ｓ１０８）。
その結果、キャリブレーション可否信号が１の場合（Ｓ１０８でＹｅｓ）、自動キャリブレーション部１０４は、実際のレール２０Ｌｔ、２０Ｒｔの位置に対応する点群３０ａｔを第二のレール点群（参照レール点群）として取り込む（Ｓ１０９）。
そして、自動キャリブレーション部１０４は、第一のレール点群３０ａｎと第二のレール点群との間の回転及び併進を算出することにより、第一のセンサ１０１ａの外部パラメータを推定する（Ｓ１１０）。
さらに、センサ情報補正部１０５が、第一のセンサ情報の補正を行う（Ｓ１１１）。
一方、キャリブレーション可否信号が０の場合（Ｓ１０８でＮｏ）、第一のセンサ情報の補正は行わず、一連の処理を終了する。
その結果、キャリブレーション可否信号が１の場合（Ｓ１０８でＹｅｓ）、自動キャリブレーション部１０４は、実際のレール２０Ｌｔ、２０Ｒｔの位置に対応する点群３０ａｔを第二のレール点群（参照レール点群）として取り込む（Ｓ１０９）。
そして、自動キャリブレーション部１０４は、第一のレール点群３０ａｎと第二のレール点群との間の回転及び併進を算出することにより、第一のセンサ１０１ａの外部パラメータを推定する（Ｓ１１０）。
さらに、センサ情報補正部１０５が、第一のセンサ情報の補正を行う（Ｓ１１１）。
一方、キャリブレーション可否信号が０の場合（Ｓ１０８でＮｏ）、第一のセンサ情報の補正は行わず、一連の処理を終了する。
【0032】
＜直線度の説明＞
次に直線度について詳細に説明する。
図３（ａ）～（ｂ）は、直線度の算出方法を示した図である。また、図３（ｃ）～（ｅ）は、直線度を算出する際の具体例を表す図である。
まず、図３（ａ）に示すように、キャリブレーション可否判断部１０３が、レール検出部１０２において得られた第一のレール点群３０ａｎに対し、左側のレール２０Ｌｔに対応する点群に沿った左レール直線モデル２０１Ｌを推定する。また、キャリブレーション可否判断部１０３は、右側のレール２０Ｒｔに対応する点群に沿った右レール直線モデル２０１Ｒを推定する。これらの直線モデルの推定方法としては、例えば第一のレール点群３０ａｎに対してＲＡＮＳＡＣ（RANdom SAmple Consensus）による直線推定を実施して左右何れか一方の直線モデルを第一の直線モデルとして推定したのち、第一の直線モデルに対して所定の距離範囲内である点群を除去して再度ＲＡＮＳＡＣによる直線推定を実施することで、他方の直線モデルを第二の直線モデルとして推定する。さらに、第一の直線モデルと第二の直線モデルのうち、ｙ切片が大きい方を左レール直線モデル２０１Ｌ、ｙ切片が小さい方を右レール直線モデル２０１Ｒとすることができる。
次に直線度について詳細に説明する。
図３（ａ）～（ｂ）は、直線度の算出方法を示した図である。また、図３（ｃ）～（ｅ）は、直線度を算出する際の具体例を表す図である。
まず、図３（ａ）に示すように、キャリブレーション可否判断部１０３が、レール検出部１０２において得られた第一のレール点群３０ａｎに対し、左側のレール２０Ｌｔに対応する点群に沿った左レール直線モデル２０１Ｌを推定する。また、キャリブレーション可否判断部１０３は、右側のレール２０Ｒｔに対応する点群に沿った右レール直線モデル２０１Ｒを推定する。これらの直線モデルの推定方法としては、例えば第一のレール点群３０ａｎに対してＲＡＮＳＡＣ（RANdom SAmple Consensus）による直線推定を実施して左右何れか一方の直線モデルを第一の直線モデルとして推定したのち、第一の直線モデルに対して所定の距離範囲内である点群を除去して再度ＲＡＮＳＡＣによる直線推定を実施することで、他方の直線モデルを第二の直線モデルとして推定する。さらに、第一の直線モデルと第二の直線モデルのうち、ｙ切片が大きい方を左レール直線モデル２０１Ｌ、ｙ切片が小さい方を右レール直線モデル２０１Ｒとすることができる。
【0033】
次に、キャリブレーション可否判断部１０３は、左レール直線モデル２０１Ｌに対して所定の距離２０２以内である範囲２０３Ｌに存在する点群２０４Ｌ、及び右レール直線モデル２０１Ｒに対して所定の距離２０２以内である範囲２０３Ｒに存在する点群２０４Ｒを抽出する。距離２０２については、範囲２０３Ｌ、２０３Ｒに曲線のレール点群を含むことのできるような値とする。
【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】
第一の代表位置ｐｂｎ１の算出方法としては、例えば、第一のレール点群３０ａｎが存在する任意のｘ方向位置を第一の代表位置のｘ座標とし、第一の代表位置のｘ座標を基準に所定の範囲内の第一のレール点群３０ａｎを抽出し、抽出後の第一のレール点群とする。抽出後の第一のレール点群に対して左レールモデル４０１Ｌｂｎ、及び右レールモデル４０１Ｒｂｎを推定し、左レールモデル４０１Ｌｂｎと右レールモデル４０１Ｒｂの中線上を第一の代表位置のｙ座標とする。さらに、左レールモデル４０１Ｌｂｎ、及び右レールモデル４０１Ｒｂが張る三次元平面の第一の代表位置のｘ座標と第一の代表位置のｙ座標に対応するｚ座標を、第一の代表位置のｚ座標とする。
【0072】
第一の三軸方向ベクトル（ｅｘｂｎ１，ｅｙｂｎ１，ｅｚｂｎ１）の算出方法としては、例えば左レールモデル４０１Ｌｂｎ方向の単位ベクトルをｅｘｂｎ１、左レールモデル４０１Ｌｂｎ及び右レールモデル４０１Ｒｂが張る平面の法線ベクトルをｅｚｂｎ１とし、ｅｘｂｎ１とｅｚｂｎ１に垂直な単位ベクトルｅｙｂｎ１を、ｅｘｂｎ１とｅｚｂｎ１の外積により求めることができる。
【0073】
第二の代表位置ｐｂｎ２の算出方法としては、ｘ座標は第一の車両固定座標系と第二の車両固定座標系とのｘ方向誤差が小さいとして、第一の代表位置のｘ座標を第二の代表位置のｘ座標と定義する。さらに、第二の代表位置のｘ座標を基準に所定の範囲内の第二のレール点群３０ｂｎを抽出し、抽出後の第二のレール点群とする。抽出後の第二のレール点群に対して左レールモデル４０２Ｌｂｎ、及び右レールモデル４０２Ｒｂｎを推定し、左レールモデル４０２Ｌｂｎと右レールモデル４０２Ｒｂの中線上を第二の代表位置のｙ座標とする。さらに、左レールモデル４０２Ｌｂｎ、及び右レールモデル４０２Ｒｂが張る三次元平面の第二の代表位置のｘ座標と第二の代表位置のｙ座標に対応するｚ座標を、第二の代表位置のｚ座標とする。
【0074】
第二の三軸方向ベクトル（ｅｘｂｎ２，ｅｙｂｎ２，ｅｚｂｎ２）の算出方法としては、例えば左レールモデル４０２Ｌｂｎ方向の単位ベクトルをｅｘｂｎ２、左レールモデル４０２Ｌｂｎ及び右レールモデル４０２Ｒｂが張る平面の法線ベクトルをｅｚｂｎ２とし、ｅｘｂｎ２とｅｚｂｎ２に垂直な単位ベクトルｅｙｂｎ２を、ｅｘｂｎ２とｅｚｂｎ２の外積により求めることができる。
【0075】
上記方法によって求められた第一の代表位置ｐｂｎ１、第一の三軸方向ベクトル（ｅｘｂｎ１，ｅｙｂｎ１，ｅｚｂｎ１）、第二の代表位置ｐｂｎ２、第二の三軸方向ベクトル（ｅｘｂｎ２，ｅｙｂｎ２，ｅｚｂｎ２）を用いて、図８（ｅ）に記載した式を解くと、第一の車両固定座標系’から第二の車両固定座標系に変換するための回転Ｒｂｎ１２ｂｎ２及び併進ｔｂｎ１２ｂｎ２を求めることができる。
【0076】
以上が第一のレール点群３０ａｎ、及び第二のレール点群３０ｂｎを用いた外部パラメータの算出方法である。上記方法によれば、レール検出によって第一のセンサ１０１ａの外部パラメータを算出することができる為、ターゲットを新設することなく高精度の自動キャリブレーションを行うことができる。よって、前方監視装置を用いたシステムにおけるキャリブレーションコストの低減、及びより安全な車両走行が可能となる。
【0077】
本実施の形態では、直線のレールの検出結果に基づくキャリブレーションを基本としているが、第一のレール点群３０ａｎと第二のレール点群３０ｂｎを用いた第２の実施形態の場合、第一のレール点群３０ａｎと第二のレール点群３０ｂｎが曲線のレールであっても良い。実際のレール２０Ｌｔ、２０Ｒｔが曲線の場合、例えば第一のレール点群３０ａｎと第二のレール点群３０ｂｎとの間のＩＣＰ（Iterative Closest Point）マッチングによって回転及び併進を算出する方法がある。
【0078】
〔第３の実施形態〕
第１の実施形態や第２の実施形態では、レール検出部１０２において第一のセンサ１０１ａが二次元画像の場合は、二次元レール検出結果を三次元レール点群に変換し、自動キャリブレーション部１０４における外部パラメータの推定を行う例を説明した。単眼カメラにより検出した二次元レール点群を三次元化する場合、奥行き方向を推定するために、例えば勾配一定やｘ軸周りのロール回転が無い、といった仮定を置かないと三次元化することが難しく、正確な三次元点群を取得することが容易ではない。単眼カメラによる二次元レール検知結果を三次元化する際の誤差によって外部パラメータの推定結果の目標性能を満たすことができない場合、三次元化を行わない方法を検討する必要がある。
第３の実施形態は、上記課題を鑑みてなされたものであり、正確な三次元化が難しい単眼カメラ情報については、二次元画像上で外部パラメータの推定を行う。
第１の実施形態や第２の実施形態では、レール検出部１０２において第一のセンサ１０１ａが二次元画像の場合は、二次元レール検出結果を三次元レール点群に変換し、自動キャリブレーション部１０４における外部パラメータの推定を行う例を説明した。単眼カメラにより検出した二次元レール点群を三次元化する場合、奥行き方向を推定するために、例えば勾配一定やｘ軸周りのロール回転が無い、といった仮定を置かないと三次元化することが難しく、正確な三次元点群を取得することが容易ではない。単眼カメラによる二次元レール検知結果を三次元化する際の誤差によって外部パラメータの推定結果の目標性能を満たすことができない場合、三次元化を行わない方法を検討する必要がある。
第３の実施形態は、上記課題を鑑みてなされたものであり、正確な三次元化が難しい単眼カメラ情報については、二次元画像上で外部パラメータの推定を行う。
【0079】
図９は、単眼カメラ情報を、第一のセンサ情報として使用した場合における自動キャリブレーション装置１１ａの動作を表すフローチャート図である。
まず、レール検出部１０２において、単眼カメラ情報を第一のセンサ情報として取得する（Ｓ２０１）。
次に、レール検出部１０２は、二次元画像上でレール検出を行う（Ｓ２０２）。
次に、レール検出部１０２は、二次元画像上でのレール検出結果を用いて三次元レール点群を算出する一方で、二次元画像上でのレール検出結果を用いて二次元レール点群を取得し、自動キャリブレーション部１０４で使用するために保持する（Ｓ２０３）。
まず、レール検出部１０２において、単眼カメラ情報を第一のセンサ情報として取得する（Ｓ２０１）。
次に、レール検出部１０２は、二次元画像上でレール検出を行う（Ｓ２０２）。
次に、レール検出部１０２は、二次元画像上でのレール検出結果を用いて三次元レール点群を算出する一方で、二次元画像上でのレール検出結果を用いて二次元レール点群を取得し、自動キャリブレーション部１０４で使用するために保持する（Ｓ２０３）。
【0080】
次に、キャリブレーション可否判断部１０３が、三次元化したレール点群情報に基づき直線度を算出し（Ｓ２０４）、キャリブレーション可否信号の算出（Ｓ２０５）を行う。キャリブレーション可否判断部１０３の動作については、第１の実施形態と同様である。
【0081】
二次元レール点群を三次元化した場合に不正確になることを上述したが、二次元点群から変換した三次元点群が、何らかの手段で正確に三次元化することのできた正しい三次元点群に対してロール角等の角度が一様に異なるという現象が発生するため、点群内の点同士の相対的な位置関係は保持されると考えることができる。従って、二次元点群から変換した三次元点群が曲線を描く場合は、正しい三次元点群も曲線であり、二次元点群から変換した三次元点群が高ノイズである場合、正しい三次元点群も高ノイズであり、直線度も同じ値を取ると考えられる。よって、第１の実施形態に示す方法によって直線度を用いた閾値判定をしても問題ない。
【0082】
次に、自動キャリブレーション部１０４が、キャリブレーション可否信号が１であるか否かを判断する（Ｓ２０６）。
その結果、キャリブレーション可否信号が１の場合（Ｓ２０６でＹｅｓ）、自動キャリブレーション部１０４は、実際のレール２０Ｌｔ、２０Ｒｔの位置に対応する点群３０ａｔを第二のレール点群３０ｂｎとして取り込む（Ｓ２０７）。
そして、自動キャリブレーション部１０４は、二次元画像上において第一のレール点群３０ａｎと第二のレール点群３０ｂｎのマッチングを行うことで外部パラメータを推定する（Ｓ２０８）。
さらに、センサ情報補正部１０５が、第一のセンサ情報の補正を行う（Ｓ２０９）。
一方、キャリブレーション可否信号が０の場合（Ｓ２０６でＮｏ）、第一のセンサ情報の補正は行わず、一連の処理を終了する。
その結果、キャリブレーション可否信号が１の場合（Ｓ２０６でＹｅｓ）、自動キャリブレーション部１０４は、実際のレール２０Ｌｔ、２０Ｒｔの位置に対応する点群３０ａｔを第二のレール点群３０ｂｎとして取り込む（Ｓ２０７）。
そして、自動キャリブレーション部１０４は、二次元画像上において第一のレール点群３０ａｎと第二のレール点群３０ｂｎのマッチングを行うことで外部パラメータを推定する（Ｓ２０８）。
さらに、センサ情報補正部１０５が、第一のセンサ情報の補正を行う（Ｓ２０９）。
一方、キャリブレーション可否信号が０の場合（Ｓ２０６でＮｏ）、第一のセンサ情報の補正は行わず、一連の処理を終了する。
【0083】
第二のレール点群３０ｂｎは、第１の実施形態のように理想状態の三次元レール点群でも、第２の実施形態のように第二のセンサ１０１ｂによって推定したものであってもよい。第３の実施形態では、理想レール点群を本実施の形態における第二のレール点群３０ｂｎとして採用した場合における具体例を示す。
【0084】
図１０（ａ）～（ｃ）は、図９のＳ２０８の動作の詳細と、使用する座標系定義を表す図である。
画像座標系（図１０（ｃ））は、画像の、例えば左上を原点とした二次元直交座標系であり、カメラ取付位置、姿勢の設計値やカメラ行列などを使用することで、画像座標系上の点は、車両固定座標系とは若干異なる車両固定座標系’（図１０（ｂ））に変換される。本実施の形態における外部パラメータの推定対象は、車両固定座標系’（図１０（ｂ））から車両固定座標系（図１０（ａ））への回転Ｒｃｂｎ２ｂ及び併進ｔｃｂｎ２ｂである。
画像座標系（図１０（ｃ））は、画像の、例えば左上を原点とした二次元直交座標系であり、カメラ取付位置、姿勢の設計値やカメラ行列などを使用することで、画像座標系上の点は、車両固定座標系とは若干異なる車両固定座標系’（図１０（ｂ））に変換される。本実施の形態における外部パラメータの推定対象は、車両固定座標系’（図１０（ｂ））から車両固定座標系（図１０（ａ））への回転Ｒｃｂｎ２ｂ及び併進ｔｃｂｎ２ｂである。
【0085】
本実施の形態では、二次元画像座標系においてマッチングにより回転と併進を推定するため、自動キャリブレーション部１０４は、まず第１の実施形態と同様に、車両固定座標系における理想レール点群６０１Ｌｂ及び６０１Ｒｂを設定する。そして、自動キャリブレーション部１０４は、回転Ｒｃｂｎ２ｂ及び併進ｔｃｂｎ２ｂの逆変換である回転Ｒｂ２ｃｂｎ及び併進ｔｂ２ｃｂｎを用いて車両固定座標系’に変換することでレール点群６０１Ｌｂｎ、６０１Ｒｂｎを算出する。さらに、自動キャリブレーション部１０４は、第一のセンサ１０１ａの外部パラメータ設計情報、及びカメラ行列などを用い、レール点群６０１Ｌｂｎ、６０１Ｒｂｎを画像座標系へ変換し、６０１Ｌｃ及び６０１Ｒｃを得る。回転Ｒｂ２ｃｂｎ及び併進ｔｂ２ｃｂｎが不定であることから、６０１Ｌｃ及び６０１Ｒｃは回転Ｒｂ２ｃｂｎ及び併進ｔｂ２ｃｂｎの関数である。
【0086】
自動キャリブレーション部１０４は、上記６０１Ｌｃ及び６０１Ｒｃが画像座標系において検出した二次元レール点群６０２Ｌｃ、６０２Ｒｃと一致するような回転Ｒｂ２ｃｂｎ及び併進ｔｂ２ｃｂｎを、数値計算により算出する。数値計算手法は、例えば最急降下法や焼きなまし法等、一般に使用されている最適化アルゴリズムで良い。
【0087】
第３の実施形態では、第一のレール点群３０ａｎは、レール２０Ｌｔ、２０Ｒｔの二次元位置を表し、自動キャリブレーション部１０４は、参照レール点群（この場合、第二のレール点群３０ｂｎ）を、第一のレール点群３０ａｎを含む画像座標系へ変換して２次元の参照レール点群（第二のレール点群３０ｂｎ）を取得し、第一のレール点群３０ａｎと２次元の参照レール点群とを一致させるための、三軸回転及び併進を算出する、と言うこともできる。
【0088】
本実施の形態では二次元画像上の点群を三次元化する際に発生する、レール点群の角度誤差などが発生しない。従って、より正確な外部パラメータの推定結果を得られることが期待され、より安全な車両走行が可能となる。
【0089】
〔第４の実施形態〕
第１の実施形態～第３の実施形態では、レール点群の直線度を算出することで、カーブ区間を含むレール点群が自動キャリブレーション部１０４で使用されることを回避した。第４の実施形態では上記に加え、鉄道の在線位置に付随する情報を記載した鉄道デジタルマップを使用することで、レール点群がカーブ区間を含み外部パラメータの推定が難しい領域を、事前に把握し、さらに確実に外部パラメータの推定を行う方法を示す。
第１の実施形態～第３の実施形態では、レール点群の直線度を算出することで、カーブ区間を含むレール点群が自動キャリブレーション部１０４で使用されることを回避した。第４の実施形態では上記に加え、鉄道の在線位置に付随する情報を記載した鉄道デジタルマップを使用することで、レール点群がカーブ区間を含み外部パラメータの推定が難しい領域を、事前に把握し、さらに確実に外部パラメータの推定を行う方法を示す。
【0090】
図１１は、第４の実施形態におけるセンサ補正システム１ｃの全体構成を示した図である。
図示するセンサ補正システム１ｃは、図１に示した第１の実施形態のセンサ補正システム１ａに対して、自動キャリブレーション装置１１ｃに鉄道デジタルマップ１０６、在線位置推定用センサ１０７、及び在線位置推定部１０８が追加されている点が異なるが、他は同様である。
図示するセンサ補正システム１ｃは、図１に示した第１の実施形態のセンサ補正システム１ａに対して、自動キャリブレーション装置１１ｃに鉄道デジタルマップ１０６、在線位置推定用センサ１０７、及び在線位置推定部１０８が追加されている点が異なるが、他は同様である。
【0091】
鉄道デジタルマップ１０６は、車両１０が走行する区間における、在線位置と対応する地点における曲率や分岐の有無など、地点における外部パラメータの推定可否を判断するための情報を少なくとも含むデータベースである。
在線位置推定用センサ１０７は、車両１０の在線位置を推定するために必要なセンサであり、例えば鉄道地上子の受信機や車輪の回転数を計測するエンコーダ、ＧＮＳＳ（Global Navigation Satellite System）受信機などが挙げられる。
在線位置推定部１０８は、在線位置推定用センサ１０７が取得した情報に応じて、車両の在線位置を算出する機能を有する。
在線位置推定用センサ１０７は、車両１０の在線位置を推定するために必要なセンサであり、例えば鉄道地上子の受信機や車輪の回転数を計測するエンコーダ、ＧＮＳＳ（Global Navigation Satellite System）受信機などが挙げられる。
在線位置推定部１０８は、在線位置推定用センサ１０７が取得した情報に応じて、車両の在線位置を算出する機能を有する。
【0092】
図１２（ａ）～（ｂ）は、鉄道デジタルマップ１０６、及び鉄道デジタルマップ１０６を用いて外部パラメータの推定の可否を判断する方法を表す図である。
図１２（ａ）は、鉄道デジタルマップ１０６の具体例を表す図である。
鉄道デジタルマップ１０６には、左右レール２０Ｌｔ、２０Ｒｔの中線７０１に沿って在線位置（キロ程［ｋｍ］）毎にレールの曲率や分岐の有無が記録されている。例えば位置７０３ａは、キロ程が１．０ｋｍであり、曲率０、分岐無し（０は分岐無し、１は分岐有りを意味する）の直線軌道である。領域７０２ａの斜線で示す区間は、曲率、分岐情報が位置７０３ａと同一であることを示す。位置７０３ｂは、分岐が発生し、かつ曲線となっている地点であり、曲率が０．００１６、分岐有という情報が格納されている。さらに、位置７０３ｃは、キロ程が２．５ｋｍであり、曲率０、分岐無しの直線軌道である。そして、領域７０２ｂの斜線で示す区間は、曲率、分岐情報が位置７０３ｃと同一であることを示す。
図１２（ａ）は、鉄道デジタルマップ１０６の具体例を表す図である。
鉄道デジタルマップ１０６には、左右レール２０Ｌｔ、２０Ｒｔの中線７０１に沿って在線位置（キロ程［ｋｍ］）毎にレールの曲率や分岐の有無が記録されている。例えば位置７０３ａは、キロ程が１．０ｋｍであり、曲率０、分岐無し（０は分岐無し、１は分岐有りを意味する）の直線軌道である。領域７０２ａの斜線で示す区間は、曲率、分岐情報が位置７０３ａと同一であることを示す。位置７０３ｂは、分岐が発生し、かつ曲線となっている地点であり、曲率が０．００１６、分岐有という情報が格納されている。さらに、位置７０３ｃは、キロ程が２．５ｋｍであり、曲率０、分岐無しの直線軌道である。そして、領域７０２ｂの斜線で示す区間は、曲率、分岐情報が位置７０３ｃと同一であることを示す。
【0093】
図１２（ｂ）は、鉄道デジタルマップ１０６を用いて外部パラメータの推定の可否を判断する方法を表す図である。
例えば、車両１０が位置７０５ａに存在し、かつ、レール検出が可能である領域７０６の区間が領域７０２ａに含まれる場合、レール検出が可能である領域は全て直線軌道であることが保証される。これは、キャリブレーション可否判断部１０３は、車両１０の在線位置とキャリブレーション可能区間との関係を含むマップ（この場合、鉄道デジタルマップ１０６）に基づき、第一のレール点群３０ａｎが、キャリブレーション可能区間に含まれる場合に、キャリブレーション可能と判断する、と言うことができる。
例えば、車両１０が位置７０５ａに存在し、かつ、レール検出が可能である領域７０６の区間が領域７０２ａに含まれる場合、レール検出が可能である領域は全て直線軌道であることが保証される。これは、キャリブレーション可否判断部１０３は、車両１０の在線位置とキャリブレーション可能区間との関係を含むマップ（この場合、鉄道デジタルマップ１０６）に基づき、第一のレール点群３０ａｎが、キャリブレーション可能区間に含まれる場合に、キャリブレーション可能と判断する、と言うことができる。
【0094】
一方、レール検出が可能である領域７０６の区間が領域７０２ａに含まれず、例えば位置７０３ｂのように、曲率が所定の閾値を上回る区間を含む、或いは分岐が存在する区間を含む場合は、キャリブレーション可否判断部１０３は、キャリブレーション可否信号を０とする。これにより、カーブや分岐を含むレール点群が自動キャリブレーション部１０４で使用されることを防ぐことが可能である。従って、より正確な外部パラメータの推定結果を得られることが期待され、より安全な車両走行が可能となる。
【0095】
また、キャリブレーション可否判断部１０３において、直線区間でかつ分岐ではないなど、車両１０の前方がキャリブレーション実施可能な直線であるにも関わらず直線度が所定の閾値を下回る場合、キャリブレーション可否判断部１０３は、キャリブレーション可否信号を０としつつ、センサ異常があることを示すアラートを発出しても良い。この場合、キャリブレーション可否判断部１０３は、第一の直線度がキャリブレーション不可の値であり、かつ第一のレール点群３０ａｎが、キャリブレーション可能区間に含まれる場合に、センサ異常信号を発出する、と言うこともできる。
キャリブレーション可否判断部１０３にアラートを発出する機能を追加することで、センサ異常を検知して車両１０の運用に反映し、より安全な車両走行が可能となる。
キャリブレーション可否判断部１０３にアラートを発出する機能を追加することで、センサ異常を検知して車両１０の運用に反映し、より安全な車両走行が可能となる。
【0096】
〔第５の実施形態〕
第１の実施形態に記載の方法によって車両固定座標系に対する各センサの回転及び併進を算出することができれば、各センサの回転及び併進の設計値と現在の値を比較し、センサ取付異常を検出することが可能である。第５の実施形態では、これを鑑みたものであり、設計値と第１の実施形態のキャリブレーション結果を使用し、センサ取付異常を検出する。
第１の実施形態に記載の方法によって車両固定座標系に対する各センサの回転及び併進を算出することができれば、各センサの回転及び併進の設計値と現在の値を比較し、センサ取付異常を検出することが可能である。第５の実施形態では、これを鑑みたものであり、設計値と第１の実施形態のキャリブレーション結果を使用し、センサ取付異常を検出する。
【0097】
図１３は、第５の実施形態におけるセンサ補正システム１ｄの全体構成を示した図である。
図示するセンサ補正システム１ｄは、図１に示した第１の実施形態のセンサ補正システム１ａに対して、自動キャリブレーション装置１１ｄにセンサ取付異常検出部１０９が追加されている点が異なるが、他は同様である。
図示するセンサ補正システム１ｄは、図１に示した第１の実施形態のセンサ補正システム１ａに対して、自動キャリブレーション装置１１ｄにセンサ取付異常検出部１０９が追加されている点が異なるが、他は同様である。
【0098】
センサ取付異常検出部１０９は、自動キャリブレーション部１０４において求められる三軸回転及び併進が、予め定められた閾値を上回る場合に、センサ取付角異常と判定する。
自動キャリブレーション部１０４が出力する第一のセンサ１０１ａの外部パラメータが、センサ取付異常検出部１０９へ入力される。センサ取付異常検出部１０９は、自動キャリブレーション部１０４が出力する第一のセンサ１０１ａの外部パラメータと、第一のセンサ１０１ａの外部パラメータ設計値との差分を、各座標軸（併進２軸又は３軸、及び回転３軸）でそれぞれ異常度として算出し、異常度が所定の閾値を上回る場合はセンサ取付異常と判定する機能を有する。
自動キャリブレーション部１０４が出力する第一のセンサ１０１ａの外部パラメータが、センサ取付異常検出部１０９へ入力される。センサ取付異常検出部１０９は、自動キャリブレーション部１０４が出力する第一のセンサ１０１ａの外部パラメータと、第一のセンサ１０１ａの外部パラメータ設計値との差分を、各座標軸（併進２軸又は３軸、及び回転３軸）でそれぞれ異常度として算出し、異常度が所定の閾値を上回る場合はセンサ取付異常と判定する機能を有する。
【0099】
また、第２の実施形態に記載の通り、第一のセンサ１０１ａと第二のセンサ１０１ｂを用い、第一のセンサ１０１ａと第二のセンサ１０１ｂ間の第一二間外部パラメータの推定結果を用い、センサ取付異常を判定することも可能である。
この場合は、第一のセンサ１０１ａ、第二のセンサ１０１ｂとは異なる第三のセンサを用い、自動キャリブレーション部１０４によって第一のセンサ１０１ａと第三のセンサ間の第一四間外部パラメータの推定結果をさらに取得する。次に、第一二間外部パラメータの推定結果の要素が所定の閾値を上回り、かつ、第一四間外部パラメータの推定結果の要素が所定の閾値を上回る場合、第一のセンサ１０１ａの取付異常であると推定することが可能である。
この場合は、第一のセンサ１０１ａ、第二のセンサ１０１ｂとは異なる第三のセンサを用い、自動キャリブレーション部１０４によって第一のセンサ１０１ａと第三のセンサ間の第一四間外部パラメータの推定結果をさらに取得する。次に、第一二間外部パラメータの推定結果の要素が所定の閾値を上回り、かつ、第一四間外部パラメータの推定結果の要素が所定の閾値を上回る場合、第一のセンサ１０１ａの取付異常であると推定することが可能である。
【0100】
この場合、センサ取付異常検出部１０９は、第一のレール点群３０ａｎと、第二のセンサ１０１ｂによって取得される第二のセンサ情報に基づき検出され、レール２０Ｌｔ、２０Ｒｔの位置を表す第二のレール点群３０ｂｎとしての参照レール点群と、から求められる三軸回転及び併進、および第一のレール点群３０ａｎと、第三のセンサによって取得される第三のセンサ情報に基づき検出され、レール２０Ｌｔ、２０Ｒｔの位置を表す第三のレール点群と、から求められる三軸回転及び併進が、ともに予め定められた閾値を上回る場合に、センサ取付角異常と判定する、と言うこともできる。
【0101】
第５の実施形態によれば、所定の直線度以上でレール検出が可能である区間において、各センサの取付角異常を検出することが可能である。従って、取付角異常に伴う前方物体検知失敗を防ぎ、より安全な走行が可能となる。
【0102】
なお以上詳述した形態では、自動キャリブレーションとし、自動的にキャリブレーションを行う場合について説明したが、これに限られるものではなく、手動にてキャリブレーションを実施してもよい。
また以上詳述した形態では、直線度とは、レール点群が直線か否か、及びレール点群のデータのばらつきの程度を評価するための指標であったが、レール点群が直線であるかを判断するだけで直線のレールを判断するのに十分な精度が確保できれば、レール点群のデータのばらつきの程度は必ずしも考慮に入れなくてよい。
また以上詳述した形態では、直線度とは、レール点群が直線か否か、及びレール点群のデータのばらつきの程度を評価するための指標であったが、レール点群が直線であるかを判断するだけで直線のレールを判断するのに十分な精度が確保できれば、レール点群のデータのばらつきの程度は必ずしも考慮に入れなくてよい。
【0103】
＜補正方法の説明＞
以上説明を行った自動キャリブレーション装置１１ａ～１１ｄが行う処理は、ソフトウェアとハードウェア資源とが協働することにより実現される。即ち、自動キャリブレーション装置１１ａ～１１ｄに設けられたコンピュータ内部のプロセッサが、上述した各機能を実現するソフトウェアをメモリにロードして実行し、これらの各機能を実現させる。
以上説明を行った自動キャリブレーション装置１１ａ～１１ｄが行う処理は、ソフトウェアとハードウェア資源とが協働することにより実現される。即ち、自動キャリブレーション装置１１ａ～１１ｄに設けられたコンピュータ内部のプロセッサが、上述した各機能を実現するソフトウェアをメモリにロードして実行し、これらの各機能を実現させる。
【0104】
よって、自動キャリブレーション装置１１ａ～１１ｄが行う処理は、プロセッサがメモリに記録されたソフトウェアを実行することにより、第一のセンサ１０１ａによって取得される第一のセンサ情報に基づき、レール２０Ｌｔ、２０Ｒｔの位置を表す第一のレール点群３０ａｎを検出し、第一のレール点群３０ａｎに対する直線の度合いを表す第一の直線度に基づき、第一のセンサ１０１ａのキャリブレーションの可否判断を行い、キャリブレーション可能と判断された場合に、第一のレール点群３０ａｎと、レール２０Ｌｔ、２０Ｒｔの３次元位置を表し第一のレール点群３０ａｎとは異なる参照レール点群とを基に、第一のセンサ情報を補正する、補正方法であると捉えることができる。
【0105】
以上、本実施の形態について説明したが、本発明の技術的範囲は上記実施の形態に記載の範囲には限定されない。上記実施の形態に、種々の変更または改良を加えたものも、本発明の技術的範囲に含まれることは、特許請求の範囲の記載から明らかである。
【符号の説明】
【0106】
１ａ～１ｄ…センサ補正システム、１１ａ～１１ｃ…自動キャリブレーション装置、２０Ｌｔ、２０Ｒｔ…レール、３０ａｎ…第一のレール点群、３０ａｔ…点群、１０１ａ…第一のセンサ、１０１ｂ…第二のセンサ、１０２…レール検出部、１０３…キャリブレーション可否判断部、１０４…自動キャリブレーション部、１０５…センサ情報補正部、１０６…鉄道デジタルマップ、１０７…在線位置推定用センサ、１０８…在線位置推定部、１０９…センサ取付異常検出部、レール…２０Ｌｔ、２０Ｒｔ
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】