JP6780942B2 - 操舵支援システム - Google Patents

Description

本発明は、移動経路に沿って移動しようとする移動体の操舵を支援するシステムに関する。

自動車、船舶、航空機やロボット等の移動体の自律制御、自動運転または半自動運転を実施する際には、移動体が移動可能な道路その他の移動経路の位置を予めデータベース化しておき、移動体による適正な経路に沿った移動を実現することが求められる。

移動体の現在位置の緯度及び経度をＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）を用いて把握し、その現在位置を地図上に示すとともに、その現在位置から一定の距離の範囲内にある通路（道路）や建物等の情報を表示するカーナビゲーションシステムが公知である（例えば、下記特許文献１を参照）。だが、既製のカーナビゲーションシステムはあくまでも、搭乗者に提示する画面表示用の地図画像データとしてのベクターデータ（ベクトル画像データ）を有しているものであるに過ぎず、移動体の自律制御、自動運転や半自動運転に必ずしも役立てられるものではない。

他方、移動体が走行する道路に敷設されている区画線（特に、路面にペイントされている白線）をセンシングして移動体の最適な走行ラインを算出し、移動体の操舵装置を自動的に操作する自動運転技術が既に開発されている（例えば、下記非特許文献１を参照）。

特開２００５−３３９１０１号公報

トヨタ自動車株式会社、"トヨタ自動車、自動運転技術の開発状況について公表｜ＴＯＹＯＴＡ Ｇｌｏｂａｌ Ｎｅｗｓｒｏｏｍ"、［ｏｎｌｉｎｅ］、平成２６年、トヨタ自動車株式会社、［平成２８年１月２５日検索］、インターネット＜ＵＲＬ：http://newsroom.toyota.co.jp/en/detail/3932729＞

移動体の自律制御、自動運転または半自動運転の用途に供するべく、空間内に存在する移動経路や障害物の位置や形状を精密にデータ化して蓄積しようとすると、データ量が膨大となる上、そのデータを利用して移動体からの距離や方向を把握するための処理が徒に複雑化し、計算量が増大して処理の遅延を招く懸念がある。

本発明は、移動体が移動しようとする移動経路を軽量かつ利用容易なデータの形で表現し、移動体の周囲に存在する経路の検出ひいては移動体の制御に寄与することを所期の目的とする。

上述した課題を解決するべく、移動体が移動しようとする経路を複数の部分に区分しその各部分について移動経路の延伸方向と交差する幅方向の所定位置の緯度及び経度を求めたデータである経路点データを格納する経路点データ格納部と、移動体の現在位置の緯度及び経路を反復的に取得する現在位置取得部と、前記経路点データ格納部に格納している、移動体がこれから一定の時間内に進入することとなる連続した複数の経路の部分についての複数の経路点データを読み出し、それら複数の経路点データの緯度及び経路を結ぶような直線または曲線の当てはめを行い当該直線または曲線の延伸方向を求め、その延伸方向と前記現在位置取得部で取得した移動体の現在位置の緯度及び経度の時系列から判明する当該移動体の移動方向とが略平行になるように移動体が備える操舵装置を操作し、またはその延伸方向と移動体の移動方向とが略平行となるために必要な操舵量若しくは操舵方向を移動体の搭乗者の視覚または聴覚に訴えかける態様で出力する制御部とを具備し、移動体の移動経路が道路であり、前記経路点データ格納部が格納する経路の各部分についての経路点データが道路の区画線の緯度及び経度を含んでおり、かつその各経路点データが移動体の移動経路上における標準的な一秒あたり若しくは数分の一秒あたりの移動距離毎に等間隔に移動経路を区分した場合の各部分についての経路点データであり、前記制御部が、今後数秒間のうちに進入することとなる複数の経路の部分についての複数の経路点データを経路点データ格納部から読み出して制御に用いる操舵支援システムを構成した。

並びに、移動体が移動しようとする経路を複数の部分に区分しその各部分について移動経路の延伸方向と交差する幅方向の所定位置の緯度及び経度を求めたデータである経路点データを格納する経路点データ格納部と、移動体の現在位置の緯度及び経路を反復的に取得する現在位置取得部と、前記経路点データ格納部に格納している、移動体がこれから一定の時間内に進入することとなる連続した複数の経路の部分についての複数の経路点データを読み出し、それら複数の経路点データの緯度及び経路を結ぶような直線または曲線の当てはめを行い当該直線または曲線の延伸方向を求め、その延伸方向と前記現在位置取得部で取得した移動体の現在位置の緯度及び経度の時系列から判明する当該移動体の移動方向とが略平行になるように移動体が備える操舵装置を操作し、またはその延伸方向と移動体の移動方向とが略平行となるために必要な操舵量若しくは操舵方向を移動体の搭乗者の視覚または聴覚に訴えかける態様で出力する制御部とを具備し、移動体の移動経路が道路であり、前記経路点データ格納部が格納する経路の各部分についての経路点データが道路の区画線の緯度及び経度を含んでおり、前記制御部が、前記現在位置取得部を介して移動体の現在位置の緯度及び経路を取得し、その現在位置から最も近い緯度及び座標を含む経路点データを前記経路点データ格納部から読み出して移動体が現在所在している道路の部分における区画線の緯度及び経度の値を得る一方、移動体に搭載されているセンサを利用して道路に敷設されている区画線の位置を検出し、移動体と当該区画線との間の距離を計測して、経路点データ格納部から読み出した区画線の緯度及び経度と移動体から当該区画線までの距離とから移動体の現在位置の緯度及び経度を演算して補正する操舵支援システムを構成した。

加えて、前記経路点データ格納部が格納する経路の各部分についての経路点データが当該部分の高度のデータを含んでおり、前記制御部が、前記経路点データ格納部に格納している、移動体がこれから一定の時間内に進入することとなる連続した複数の経路の部分についての複数の経路点データを読み出し、それら複数の経路点データから判明する移動経路の上り下りに応じて移動体の駆動源であるエンジン若しくはモータの出力を操作し、または移動体の移動速度の変動を抑制するために必要な駆動源の出力の操作量若しくは操作方向を移動体の搭乗者の視覚または聴覚に訴えかける態様で出力するものとしても構わない。

データ量及び計算量の一層の軽量化を図るためには、前記経路点データ格納部が、移動体の移動経路上における標準的な一秒あたり若しくは数分の一秒あたりの移動距離毎に移動経路を区分した場合の各部分についての経路点データを格納しており、前記制御部が、今後数秒間のうちに進入することとなる複数の経路の部分についての経路点データを経路点データ格納部から読み出して制御に用いるシステムとすることが好ましい。

本発明によれば、移動体が移動しようとする移動経路を軽量かつ利用容易なデータの形で表現でき、移動体の周囲に存在する経路の検出ひいては移動体の制御に寄与し得る。

本発明の一実施形態における操舵支援システムが有するハードウェア資源を示す図。 同実施形態の操舵支援システムの機能ブロック図。 同実施形態における経路点データ格納部が格納している経路点データを例示する図。 同実施形態における移動体、移動経路及びその部分、並びに区画線の位置の関係を例示する図。 同実施形態においてコンピュータがプログラムに従い実行する処理の手順例を示すフロー図。

本発明の一実施形態を、図面を参照して説明する。本実施形態の操舵支援システムは、移動体１、典型的には自動車またはロボットに搭載されて運用される。本操舵支援システムは、移動体１に実装されている操舵装置１１や移動体１の駆動源１２であるエンジン若しくはモータを制御するコンピュータ（電子制御装置（Ｅｌｅｃｔｒｏｎｉｃ Ｃｏｎｔｒｏｌ Ｕｎｉｔ）と呼称されることがある）を主体として構成される。

本実施形態における移動体１は、走行用の車輪の向きを変化させることのできる既知の操舵装置１１、車輪を回転駆動させる駆動源１２となる既知のエンジン若しくはモータの他、移動体１の移動経路の路面上に敷設されている区画線や移動体１の周囲の物体を検出するためのセンサ１３としてのカメラセンサ（パノラマカメラであることがある）、レーダセンサ若しくは対物距離（測距）センサ（レーザレーダ、超音波レーダ、赤外線レーダ等）、パッシブ赤外線センサ（人感センサ）等、並びに、ＧＰＳやＱＺＳＳ（Ｑｕａｓｉ−Ｚｅｎｉｔｈ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ）といった航法衛星システム（衛星測位システム）の航法信号受信装置１４を備える。加えて、移動体１に搭乗している搭乗者の視覚に訴えかける態様で情報を表示出力するディスプレイ１５や、搭乗者の聴覚に訴えかける態様で情報を音声出力する音声出力デバイス１６等を備えていることもある。

図１に示すように、移動体１の運転制御を司るコンピュータは、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）１ａ、メインメモリ１ｂ、補助記憶デバイス１ｃ、ビデオコーデック１ｄ、オーディオコーデック１ｅ、通信インタフェース１ｆ等のハードウェア資源を備え、これらがコントローラ（システムコントローラ、Ｉ／Ｏコントローラ等）１ｇにより制御されて連携動作するものである。

補助記憶デバイス１ｃは、フラッシュメモリ、ハードディスクドライブ、光学ディスクドライブ、その他である。ビデオコーデック１ｄは、ＣＰＵ１ａより受けた描画指示をもとに表示させるべき画面を生成しその画面信号をディスプレイ１５に向けて送出するＧＰＵ（Ｇｒａｐｈｉｃｓ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）、画面や画像のデータを一時的に格納しておくビデオメモリ等を要素とする。オーディオコーデック１ｅは、符号化されている音声データを復号化して音声出力デバイス１６から音声出力する。ビデオコーデック１ｄ及びオーディオコーデック１ｅはそれぞれ、ハードウェアでなくソフトウェアとして実装することも可能である。通信インタフェース１ｆは、当該コンピュータが外部の装置と情報の授受を行うためのデバイスである。

ＣＰＵ１ａによって実行されるべきプログラムは補助記憶デバイス１ｃに格納されており、プログラムの実行の際には補助記憶デバイス１ｃからメインメモリ１ｂに読み込まれ、ＣＰＵ１ａによって解読される。コンピュータは、プログラムに従って上記ハードウェア資源を作動させ、図２に示す経路点データ格納部１０１、現在位置取得部１０２及び制御部１０３としての機能を発揮する。

経路点データ格納部１０１は、メインメモリ１ｂ若しくは補助記憶デバイス１ｃの所要の記憶領域を利用して、経路点データを格納する。図３に、経路点データ格納部１０１が格納する経路点データの一例を示す。

経路点データは、少なくとも、移動体が移動しようとする経路を複数の部分αに区分したときの各部分αについて、移動経路の延伸方向と交差（特に、直交）する幅方向の所定点、換言すれば移動経路と平行に延伸する仮想的な線上の位置の緯度及び経度のデータを含む。本実施形態では、道路を走行する自動車またはロボットを移動体として想定している。従って、経路点データが、移動体の移動経路たる道路の経路の各部分αにおける区画線の位置を示す緯度及び経度のデータを含んでいる。区画線とは、道路の路面にペイントされた線や、路面に設置された道路鋲（チャッタバー）、石等によって描かれる線のことであり、下記を含む：
「道路外側線」車道の最も外側の境界を示す、路端寄りに引かれている区画線。道路外側線よりも外側は、原則として自動車の走行が禁止される路側帯となる。そして、道路外側線よりも内側が、自動車の走行が許される車両通行帯となる。
「車道中央線」自動車の通行を方向別に区分するために引かれている区画線。実際には、ペイントされた線だけでなく、道路上に設置されたランブルストリップス、道路鋲、車線分離標（センターポール、ポストコーン）、中央分離帯等の構造物によって表されることも多い。
「車線境界線」同一方向に走行するための車両通行帯が二車線以上存在している場合の、隣り合う車線の間を仕切る境界線。

図３及び図４に示しているように、本実施形態では、移動体１が道路を走行するときの標準的な速度、即ち一秒あたり若しくは数分の一秒（例えば、二分の一秒、四分の一秒等）あたりの移動距離毎に、移動体１の移動経路を区分し、当該距離の長さを持つ各部分αの起点（または、終点、中間点等。各部分αの、移動経路の延伸方向に沿った所定位置）における区画線の位置を示す緯度及び経度の値を経路点データとして、当該移動経路を識別する識別子である経路（道路）の種別に関連づけて経路点データ格納部１０１に格納することとしている。移動経路が高速道路であり、当該高速道路上を走行する移動体１の標準的な移動速度が時速１００ｋｍであるとすると、移動体１の標準的な秒速即ち一秒あたりの移動距離は２７．７８ｍ、二分の一秒あたりの移動距離は１３．８９ｍ、四分の一秒あたりの移動距離は６．９４５ｍとなる。従って、移動経路が高速道路である場合には、当該高速道路を２７．７８ｍ、１３．８９ｍまたは６．９４５ｍの長さ（道程）を持つ複数の部分αに区分し、それら各部分αを単位として経路点データを求め、経路点データ格納部１０１に格納する。

図示例では、移動経路の各部分αの起点における、左側の車道外側線Ｌ及び右側の車道外側線（車道中央線に等しいことがある）Ｒの各々の緯度及び経度をデータベース化している。なお、これに加えて、移動経路の各部分αの起点における車線境界線の緯度及び経度を経路点データに含めてもよい。

また、移動経路の各部分αについての経路点データは、上記の区画線の緯度及び経路の他に、当該部分αの起点（または、終点）における左側の道路側壁ＬＬの位置を示す緯度及び経度の値や、当該部分αの起点における右側の道路側壁ＲＲの位置を示す緯度及び経度の値を含むことがある。道路側壁ＬＬ、ＲＲとは、道路外側線や車道中央線の外側に存在して移動体１の移動、通行または通過の障害となる、ランブルストリップス、道路鋲、車線分離標、中央分離帯、防音壁、ガードレール、縁石、ポール、側溝、建築構造物等をいう。

さらに、移動経路の各部分αについての経路点データは、当該部分αの起点（または、終点）の高度である標高の値や、当該部分αが包有している車線数の情報を含むことがある。ここで、車線数の情報は、同一方向に向かう道路の車線数のみを１、２、３、……といった数値で表すものの他、Ｔ２（登坂車線を含む、二車線）、Ｉ２（インターチェンジ車線を含む、二車線）、Ｐ２（パーキングエリア車線を含む、二車線）、Ｏ２（追い越し禁止の、二車線）、Ｇ１（対向車線一車線。この場合、移動方向が相反する往路と復路とを一つの道路として取り扱うことがある）等のように、車線数に加えて道路に関する付加的な情報を表現するものであっても構わない。

経路点データの要素となる緯度及び経度はそれぞれ、所定桁数の値とする。緯度及び経度の有効桁数を小数点以下６桁までとする場合、日本国内におけるそれら緯度及び経度の解像度は緯度方向に約０．１１ｍ、経度方向に約０．０９ｍとなる。

因みに、経路点データは、移動体１に搭乗している搭乗者が移動体１の移動先の目的地を指定したときにはじめて生成されることがある。例えば、搭乗者が移動体１に実装されているカーナビゲーションシステムを操作して目的地を入力し、カーナビゲーションシステムが移動体１の現在位置から目的地に至るための移動経路を検索して決定した後、本操舵支援システムのコンピュータが、当該移動経路を複数の部分αに区分して各部分αについての経路点データを生成、経路点データ格納部１０１に格納する処理を実行する。

現在位置取得部１０２は、移動体１が備えている航法信号受信装置１４の機能を利用し、移動体１の現在位置の緯度及び経度を所定時間毎に反復的に取得する。

制御部１０３は、経路点データ格納部１０１に格納している、移動体１の移動経路上の複数の部分αに関する経路点データと、現在位置取得部１０２を介して知得した移動体１の現在位置の推移とに基づき、移動体１の操舵や移動速度の制御を行う。

図５に示すように、制御部１０３はまず、今現在の移動体１の位置を示す緯度及び経度から最も距離が近い緯度及び経度の値を有する経路点データを、経路点データ格納部１０１の中から検索する（ステップＳ１）。ここで、移動体１の位置と対象となる位置との距離である測地線長、及び移動体１の位置から見た対象の位置の方位角の計算方法の一例を示す。以下に述べる計算方法は、国土地理院ウェブサイト（http://vldb.gsi.go.jp/sokuchi/surveycalc/surveycalc/bl2stf.html）で紹介されているものである。

移動体１の位置の緯度をφ₁、経度をＬ₁とおき、対象の位置の緯度をφ₂、経度をＬ₂とおく。但し、北緯を正、南緯を負、東経を正、西経を（３６０°−経度）とする。また、Ｌ＝Ｌ₂−Ｌ₁が負値となる場合には、移動体１の位置の緯度をφ₂、経度をＬ₂とし、対象の位置の緯度をφ₁、経度をＬ₁として、Ｌを正値化する。並びに、地球楕円体（準拠楕円体）の長半径をａ、扁平率をｆとおく。
Ｌ＝Ｌ₂−Ｌ₁； Ｌ’＝１８０°−Ｌ （但し、０°≦Ｌ≦１８０°）
Δ＝φ₂−φ₁； Σ＝φ₁＋φ₂
ｕ₁＝ｔａｎ^-1［（１−ｆ）ｔａｎφ₁］； ｕ₂＝ｔａｎ^-1［（１−ｆ）ｔａｎφ₂］
Σ’＝ｕ₁＋ｕ₂； Δ’＝ｕ₂−ｕ₁ （但し、−１８０°≦Σ’≦１８０°、−１８０°≦Δ’≦１８０°）
ξ＝ｃｏｓ（Σ’／２）； ξ’＝ｓｉｎ（Σ’／２）
η＝ｓｉｎ（Δ’／２）； η’＝ｃｏｓ（Δ’／２）
ｘ＝ｓｉｎｕ₁ｓｉｎｕ₂； ｙ＝ｃｏｓｕ₁ｃｏｓｕ₂
ｃ＝ｙｃｏｓＬ＋ｘ
ε＝［ｆ（２−ｆ）］／［（１−ｆ）²］
以降、ｃの値に応じて一部計算式が異なり、ｃ≧０が成立する場合をゾーン１、０＞ｃ≧−ｃｏｓ（３°ｃｏｓｕ₁）が成立する場合をゾーン２、ｃ＜−ｃｏｓ（３°ｃｏｓｕ₁）が成立する場合をゾーン３とする。
θの初期値θ₍₀₎について、ゾーン１ではθ₍₀₎＝Ｌ（１＋ｆｙ）とし、ゾーン２ではθ₍₀₎＝Ｌ’とする。
ゾーン３では、Σ＝０であるゾーン３（ａ）と、Σ≠０であるゾーン３（ｂ）とで計算方法が分かれる。
Ｒ＝ｆπｃｏｓ²ｕ₁［１−（１／４）ｆ（１＋ｆ）ｓｉｎ²ｕ₁＋（３／１６）ｆ²ｓｉｎ⁴ｕ₁］
ｄ₁＝Ｌ’ｃｏｓｕ₁−Ｒ； ｄ₂＝｜Σ’｜＋Ｒ
ｑ＝Ｌ’／（ｆπ）； ｆ₁＝（１／４）ｆ［１＋（１／２）ｆ］； γ₀＝ｑ＋ｆ₁ｑ−ｆ₁ｑ³
ゾーン３（ａ）でのθ₍₀₎の計算は、
Ａ₀＝ｔａｎ^-1（ｄ₁／ｄ₂） （但し、−９０°≦Ａ₀≦９０°）
Ｂ₀＝ｓｉｎ^-1［Ｒ／√（ｄ₁ ²＋ｄ₂ ²）］ （但し、０°≦Ｂ₀≦９０°）
ψ＝Ａ₀＋Ｂ₀； ｊ＝γ₀／ｃｏｓｕ₁
ｋ＝（１＋ｆ₁）｜Σ’｜（１−ｆｙ）／（ｆπｙ）； ｊ₁＝ｊ／（１＋ｋｓｅｃψ）
ψ’＝ｓｉｎ^-1ｊ₁ （但し、０°≦ψ’≦９０°）
ψ’’＝ｓｉｎ^-1（ｊ₁ｃｏｓｕ₁／ｃｏｓｕ₂） （但し、０°≦ψ’’≦９０°）
θ₍₀₎＝２ｔａｎ^-1｛ｔａｎ［（ψ’＋ψ’’）／２］ｓｉｎ（｜Σ’｜／２）／ｃｏｓ（Δ’／２）｝
他方、ゾーン３（ｂ）では、ｄ₁＞０の場合にθ₍₀₎＝Ｌ’とする。ゾーン３（ｂ）でｄ₁≦０である場合の測地線長及び方位角の計算に関しては、別途述べる。

（ゾーン３（ｂ）でｄ₁≦０である場合を除く）下記θの算出において、一回目の演算ではθ_(n)≡θ₍₀₎とし、二回目以降の演算ではθ_(n+1)≡θ_(n)とする。
ゾーン１では、ｇ＝√［η²ｃｏｓ²（θ_(n)／２）＋ξ²ｓｉｎ²（θ_(n)／２）］
ゾーン１以外では、ｇ＝√［η²ｓｉｎ²（θ_(n)／２）＋ξ²ｃｏｓ²（θ_(n)／２）］
ゾーン１では、ｈ＝√［η’²ｃｏｓ²（θ_(n)／２）＋ξ’²ｓｉｎ²（θ_(n)／２）］
ゾーン１以外では、ｈ＝√［η’²ｓｉｎ²（θ_(n)／２）＋ξ’²ｃｏｓ²（θ_(n)／２）］
σ＝２ｔａｎ^-1（ｇ／ｈ）； Ｊ＝２ｇｈ； Ｋ＝ｈ²−ｇ²
γ＝（ｙ／Ｊ）ｓｉｎθ_(n)； Γ＝１−γ²； ζ＝ΓＫ−２ｘ； ζ’＝ζ＋ｘ
Ｄ＝（１／４）ｆ（１＋ｆ）−（３／１６）ｆ²Γ
Ｅ＝（１−ＤΓ）ｆγ｛σ＋ＤＪ［ζ＋ＤＫ（２ζ²−Γ²）］｝
ゾーン１では、Ｆ＝θ_(n)−Ｌ−Ｅ
ゾーン１以外では、Ｆ＝θ_(n)−Ｌ’＋Ｅ
Ｇ＝ｆγ²（１−２ＤΓ）＋ｆζ’（σ／Ｊ）［１−ＤΓ＋（１／２）ｆγ²］＋（１／４）ｆ²ζζ’
θ_(n+1)＝θ_(n)−Ｆ／（１−Ｇ）
上記を所要回数反復することで、θを求める。その反復的な演算は、｜Ｆ｜＜１０^-15となるまで続行することが好ましい。

（ゾーン３（ｂ）でｄ₁≦０である場合を除く）測地線長ｓは、
ｎ₀＝εΓ／［√（１＋εΓ）＋１］²； Ａ＝（１＋ｎ₀）［１＋（５／４）ｎ₀ ²］
Ｂ＝ε［１−（３／８）ｎ₀ ²］／［√（１＋εΓ）＋１］²
ｓ＝（１−ｆ）ａＡ（σ−ＢＪ｛ζ−（１／４）Ｂ［Ｋ（Γ²−２ζ²）−（１／６）Ｂζ（１−４Ｋ²）（３Γ²−４ζ²）］｝）
（ゾーン３（ｂ）でｄ₁≦０である場合を除く）移動体１の位置から見た対象の位置の方位角αは、
ゾーン１では、α＝ｔａｎ^-1［ξｔａｎ（θ／２）／η］−ｔａｎ^-1［ξ’ｔａｎ（θ／２）／η’］
ゾーン１以外では、α＝ｔａｎ^-1［η’ｔａｎ（θ／２）／ξ’］−ｔａｎ^-1［ηｔａｎ（θ／２）／ξ］
但し、移動体１の位置の緯度及び経度を（φ₂，Ｌ₂）とし、対象の位置の緯度及び経度を（φ₁，Ｌ₁）としているのであれば、
ゾーン１では、α＝１８０°＋ｔａｎ^-1［ξｔａｎ（θ／２）／η］＋ｔａｎ^-1［ξ’ｔａｎ（θ／２）／η’］
ゾーン１以外では、α＝３６０°−ｔａｎ^-1［η’ｔａｎ（θ／２）／ξ’］−ｔａｎ^-1［ηｔａｎ（θ／２）／ξ］
なお、ゾーン３（ｂ）でｄ₁＝０である場合の測地線長ｓ及び方位角αは、
Γ＝ｓｉｎ²ｕ₁
ｎ₀＝εΓ／［√（１＋εΓ）＋１］²； Ａ＝（１＋ｎ₀）［１＋（５／４）ｎ₀ ²］
ｓ＝（１−ｆ）ａＡπ
α＝９０°
但し、移動体１の位置の緯度及び経度を（φ₂，Ｌ₂）とし、対象の位置の緯度及び経度を（φ₁，Ｌ₁）としているのであれば、α＝２７０°
また、ゾーン３（ｂ）でｄ₁＜０である場合の測地線長ｓ及び方位角αは、下記に則って算出する。一回目の演算ではγ_(n)≡γ₍₀₎とし、二回目以降の演算ではγ_(n+1)≡γ_(n)として、
Γ＝１−γ_(n) ²； Ｄ＝（１／４）ｆ（１＋ｆ）−（３／１６）ｆ²Γ
γ_(n+1)＝ｑ／（１−ＤΓ）
上記を所要回数反復することで、Γ及びＤを求める。その反復的な演算は、｜γ_(n)−γ_(n-1)｜＜１０^-15となるまで続行することが好ましい。そして、
ｎ₀＝εΓ／［√（１＋εΓ）＋１］²； Ａ＝（１＋ｎ₀）［１＋（５／４）ｎ₀ ²］
ｓ＝（１−ｆ）ａＡπ
ｍ＝１−ｑｓｅｃｕ₁； ｎ＝ＤΓ／（１−ＤΓ）； ｗ＝ｍ−ｎ＋ｍｎ
ｗ≦０のとき、α＝９０°
ｗ＞０のとき、α＝９０°−２ｓｉｎ^-1√（ｗ／２）
但し、移動体１の位置の緯度及び経度を（φ₂，Ｌ₂）とし、対象の位置の緯度及び経度を（φ₁，Ｌ₁）としているのであれば、
ｗ≦０のとき、α＝２７０°
ｗ＞０のとき、α＝２７０°＋２ｓｉｎ^-1√（ｗ／２）
今現在の移動体１の位置から最も近い経路点データを経路点データ格納部１０１の中から検索した制御部１０３は、次に、その検索した経路点データと同一の移動経路に属しており、移動体１がこれから進入することになる、連続した複数の移動経路の部分αについての経路点データを経路点データ格納部１０１から読み出す（ステップＳ２）。より具体的には、移動体１が今後一定の時間内に（例えば、今から十秒間のうちに）進入するであろう複数の経路の部分αの（経路点データの生成に際して移動経路を一秒あたりの移動距離の長さで切り分けているならば、十個の）経路点データ［左側の道路側壁ＬＬの位置，左側の車道外側線Ｌの位置，右側の車道外側線Ｒの位置，右側の道路側壁ＲＲの位置，標高，車線数，……］を読み出す。

そして、それら経路点データを基に、将来的な移動体１のあるべき移動の軌跡、即ち移動体１が道路を走行するときの適正な走行ラインを得る。例えば、移動体１がこれから進入する複数の移動経路の部分αについての左側の車道外側線Ｌの位置（右側の車道外側線Ｒの位置や車線境界線の位置等でもよいことは言うまでもない）の緯度及び経度を読み出し、それら複数の車道外側線Ｌの位置を結ぶような直線または曲線（図４において一点鎖線で描画している）の当てはめを行い、その直線または曲線の延伸方向を求める（ステップＳ３）。

さらに、制御部１０３は、現在位置取得部１０２を介して反復的に取得している移動体１の現在位置の緯度及び経度を基に、現在の移動体１の移動方向を求める（ステップＳ４）。移動体１がどの方角に向かって移動しているのか（図４において白抜き矢印で描画している）は、たった今取得した最新の移動体１の位置の緯度及び経度と、直近の過去に知得した移動体１の位置の緯度及び経度との差分から明らかとなる。

その上で、制御部１０３は、移動体１の移動方向のベクトルと、移動体１が走行しようとする移動経路の延伸方向のベクトルとの偏差を縮小させるように操舵方向及び／または操舵量を決定し、操舵装置１１を制御する（ステップＳ５）。

なお、制御部１０３が、当該偏差を縮小させるために必要な操舵方向及び／または操舵量の情報を、移動体１の搭乗者の視覚または聴覚に訴えかける態様で出力、即ちディスプレイ１５の画面に表示させたり音声出力デバイス１６を介して音声出力させたりすることも好ましい。これにより、移動体１の自律制御、自動運転または半自動運転中に搭乗者が制御部１０３による操舵の内容の予告を受けることができ、あるいは、搭乗者自身が移動体１を操舵する場合に適切な操舵量を搭乗者に通知することができる。

加えて、制御部１０３は、経路点データ格納部１０１から読み出した複数の経路の部分αの経路点データに含まれる、各部分αの標高の値に基づき、移動体１の駆動源１２であるエンジン若しくはモータの出力を制御する。移動体１がこれから進入することになる複数の移動経路の部分αの標高を参照すれば、移動体１がこれから上り坂に差し掛かるのか、または下り坂に差し掛かるのかが判明する。また、連続する複数の部分αの高低差が、路面の傾斜を表すこととなる。そこで、制御部１０３は、移動体１がこれから進入する移動経路の部分αが上り坂であるか下り坂であるか、またその路面の傾斜を求め（ステップＳ６）、それに応じて移動体１に搭載されたエンジン若しくはモータの出力を増減させる（ステップＳ７）ことにより、移動経路の上り下りに起因する移動体１の移動速度の変動を抑制する。

なお、制御部１０３が、移動速度の変動を抑制するためにエンジン若しくはモータの出力を増量するのか減量するのか、及び／または、移動速度の変動を抑制するために必要な出力の増減量の情報を、移動体１の搭乗者の視覚または聴覚に訴えかける態様で出力、即ちディスプレイ１５の画面に表示させたり音声出力デバイス１６を介して音声出力させたりすることも好ましい。これにより、移動体１の自律制御、自動運転または半自動運転中に搭乗者が制御部１０３による出力制御の内容の予告を受けることができ、あるいは、搭乗者自身がアクセルペダルを操作する場合に適切な操作量を搭乗者に通知することができる。

本実施形態では、移動体１が移動しようとする経路を複数の部分αに区分しその各部分αについて移動経路の延伸方向と交差する幅方向の所定位置の緯度及び経度を求めたデータである経路点データを格納する経路点データ格納部１０１と、移動体１の現在位置の緯度及び経路を反復的に取得する現在位置取得部１０２と、前記経路点データ格納部１０１に格納している移動体１がこれから進入することとなる複数の経路の部分αについての経路点データから判明する移動経路の延伸方向と、前記現在位置取得部１０２で取得した移動体１の現在位置の緯度及び経度の時系列から判明する当該移動体１の移動方向とが略平行になるように移動体１が備える操舵装置１１を操作し、または移動経路の延伸方向と移動体１の移動方向とが略平行となるために必要な操舵量若しくは操舵方向を移動体１の搭乗者の視覚または聴覚に訴えかける態様で出力する制御部１０３とを具備する操舵支援システムを構成した。本実施形態によれば、移動体１が移動しようとする移動経路を軽量かつ利用容易なデータの形で表現できる。本実施形態のシステムは、移動体１の周囲に存在する経路の検出ひいては移動体１の制御に有効である。

移動体１の移動経路が道路である場合に、前記経路点データ格納部１０１が格納する経路の各部分αについての経路点データが道路の区画線の緯度及び経度を含んでいるものであるならば、道路に敷設されている区画線に沿って移動体１を適正に操舵することが可能となる。

加えて、前記経路点データ格納部１０１が格納する経路の各部分αについての経路点データが当該部分αの高度のデータを含んでおり、前記制御部１０３が、前記経路点データ格納部１０１に格納している移動体１がこれから進入することとなる複数の経路の部分αについての経路点データから判明する移動経路の上り下りに応じて移動体１の駆動源１２であるエンジン若しくはモータの出力を操作し、または移動体１の移動速度の変動を抑制するために必要な駆動源１２の出力の操作量若しくは操作方向を移動体１の搭乗者の視覚または聴覚に訴えかける態様で出力するものであるならば、移動体１の操舵とともに移動速度の制御にも寄与し得る。

前記経路点データ格納部１０１が、移動体１の移動経路上における標準的な一秒あたり若しくは数分の一秒あたりの移動距離毎に移動経路を区分した場合の各部分αについての経路点データを格納しており、前記制御部１０３が、今後数秒間のうちに進入することとなる複数の経路の部分αについての経路点データを経路点データ格納部１０１から読み出して制御に用いるものであるならば、経路点データ格納部１０１に格納するべき経路点データの量を著しく削減でき、メインメモリ１ｂ若しくは補助記憶デバイス１ｃの記憶領域の占有量を縮小することができる上、計算負荷も軽減する。

なお、本発明は以上に詳述した実施形態に限られるものではない。例えば、上記実施形態のシステムの応用として、航法信号受信装置１４を介して取得される移動体１の現在位置の緯度及び経度の値の補正を実行することが考えられる。制御部１０３が、現在位置取得部１０２の要素である航法信号受信装置１４の機能を利用して移動体１の現在位置の緯度及び経度の値を得、その現在位置から最も近い緯度及び座標を含む経路点データを経路点データ格納部１０１の中から検索し、抽出された経路点データに記述されている、移動体１が現在所在している道路の部分αにおける区画線等（道路側壁ＬＬ、ＲＲ、道路外側線Ｌ、Ｒ、または車線境界線のうちの少なくとも一つ）の緯度及び経度の値を得る。一方で、制御部１０３は、移動体１に搭載されているセンサ１３の機能を利用し、道路に敷設されている区画線等の位置を検出、移動体１と当該区画線等との間の距離を計測する。そして、経路点データ格納部１０１から読み出した区画線等の緯度及び経度と、移動体１から当該区画線等までの距離とから、移動体１の現在位置の緯度及び経度を演算し、これを航法信号受信装置１４を介して得られる移動体１の現在位置の緯度及び経度と比較することで、両者の誤差を求めることができる。

本発明に係るシステムが対象とする移動体１は自動車やロボットには限定されず、船舶や航空機等であることがある。

その他、各部の具体的な構成や処理の手順等は、本発明の趣旨を逸脱しない範囲で種々変形が可能である。

１…移動体
１１…操舵装置
１２…駆動源（エンジン若しくはモータ）
１５…ディスプレイ
１６…音声出力デバイス
１０１…経路点データ格納部
１０２…現在位置取得部
１０３…制御部
α…移動経路の部分
ＬＬ、Ｌ、Ｒ、ＲＲ…移動経路の幅方向の所定位置（道路側壁、道路外側線）

Claims (5)

1. 移動体が移動しようとする経路を複数の部分に区分しその各部分について移動経路の延伸方向と交差する幅方向の所定位置の緯度及び経度を求めたデータである経路点データを格納する経路点データ格納部と、
   移動体の現在位置の緯度及び経路を反復的に取得する現在位置取得部と、
   前記経路点データ格納部に格納している、移動体がこれから一定の時間内に進入することとなる連続した複数の経路の部分についての複数の経路点データを読み出し、それら複数の経路点データの緯度及び経路を結ぶような直線または曲線の当てはめを行い当該直線または曲線の延伸方向を求め、その延伸方向と前記現在位置取得部で取得した移動体の現在位置の緯度及び経度の時系列から判明する当該移動体の移動方向とが略平行になるように移動体が備える操舵装置を操作し、またはその延伸方向と移動体の移動方向とが略平行となるために必要な操舵量若しくは操舵方向を移動体の搭乗者の視覚または聴覚に訴えかける態様で出力する制御部とを具備し、
   移動体の移動経路が道路であり、前記経路点データ格納部が格納する経路の各部分についての経路点データが道路の区画線の緯度及び経度を含んでおり、かつその各経路点データが移動体の移動経路上における標準的な一秒あたり若しくは数分の一秒あたりの移動距離毎に等間隔に移動経路を区分した場合の各部分についての経路点データであり、
   前記制御部が、今後数秒間のうちに進入することとなる複数の経路の部分についての複数の経路点データを経路点データ格納部から読み出して制御に用いる操舵支援システム。
2. 移動体が移動しようとする経路を複数の部分に区分しその各部分について移動経路の延伸方向と交差する幅方向の所定位置の緯度及び経度を求めたデータである経路点データを格納する経路点データ格納部と、
   移動体の現在位置の緯度及び経路を反復的に取得する現在位置取得部と、
   前記経路点データ格納部に格納している、移動体がこれから一定の時間内に進入することとなる連続した複数の経路の部分についての複数の経路点データを読み出し、それら複数の経路点データの緯度及び経路を結ぶような直線または曲線の当てはめを行い当該直線または曲線の延伸方向を求め、その延伸方向と前記現在位置取得部で取得した移動体の現在位置の緯度及び経度の時系列から判明する当該移動体の移動方向とが略平行になるように移動体が備える操舵装置を操作し、またはその延伸方向と移動体の移動方向とが略平行となるために必要な操舵量若しくは操舵方向を移動体の搭乗者の視覚または聴覚に訴えかける態様で出力する制御部とを具備し、
   移動体の移動経路が道路であり、前記経路点データ格納部が格納する経路の各部分についての経路点データが道路の区画線の緯度及び経度を含んでおり、
   前記制御部が、前記現在位置取得部を介して移動体の現在位置の緯度及び経路を取得し、その現在位置から最も近い緯度及び座標を含む経路点データを前記経路点データ格納部から読み出して移動体が現在所在している道路の部分における区画線の緯度及び経度の値を得る一方、移動体に搭載されているセンサを利用して道路に敷設されている区画線の位置を検出し、移動体と当該区画線との間の距離を計測して、経路点データ格納部から読み出した区画線の緯度及び経度と移動体から当該区画線までの距離とから移動体の現在位置の緯度及び経度を演算して補正する操舵支援システム。
3. 前記経路点データ格納部が格納する経路の各部分についての経路点データが当該部分の高度のデータを含んでおり、
   前記制御部が、前記経路点データ格納部に格納している、移動体がこれから一定の時間内に進入することとなる連続した複数の経路の部分についての複数の経路点データを読み出し、それら複数の経路点データから判明する移動経路の上り下りに応じて移動体の駆動源であるエンジン若しくはモータの出力を操作し、または移動体の移動速度の変動を抑制するために必要な駆動源の出力の操作量若しくは操作方向を移動体の搭乗者の視覚または聴覚に訴えかける態様で出力する請求項１または２記載の操舵支援システム。
4. 前記経路点データ格納部が、移動体の移動経路上における標準的な一秒あたり若しくは数分の一秒あたりの移動距離毎に等間隔に移動経路を区分した場合の各部分についての経路点データを格納しており、
   前記制御部が、今後数秒間のうちに進入することとなる複数の経路の部分についての複数の経路点データを経路点データ格納部から読み出して制御に用いる請求項２記載の操舵支援システム。
5. 請求項１、２、３または４記載の操舵支援システムを構築するために用いられるものであって、コンピュータを、
   移動体が移動しようとする経路を複数の部分に区分しその各部分について移動経路の延伸方向と交差する幅方向の所定位置の緯度及び経度を求めたデータである経路点データを格納する経路点データ格納部、
   移動体の現在位置の緯度及び経路を反復的に取得する現在位置取得部、並びに、
   前記経路点データ格納部に格納している、移動体がこれから一定の時間内に進入することとなる連続した複数の経路の部分についての複数の経路点データを読み出し、それら複数の経路点データの緯度及び経路を結ぶような直線または曲線の当てはめを行い当該直線または曲線の延伸方向を求め、その延伸方向と前記現在位置取得部で取得した移動体の現在位置の緯度及び経度の時系列から判明する当該移動体の移動方向とが略平行になるように移動体が備える操舵装置を操作し、またはその延伸方向と移動体の移動方向とが略平行となるために必要な操舵量若しくは操舵方向を移動体の搭乗者の視覚または聴覚に訴えかける態様で出力する制御部として機能させ、
   移動体の移動経路が道路であり、前記経路点データ格納部が格納する経路の各部分についての経路点データが道路の区画線の緯度及び経度を含んでおり、かつその各経路点データが移動体の移動経路上における標準的な一秒あたり若しくは数分の一秒あたりの移動距離毎に等間隔に移動経路を区分した場合の各部分についての経路点データであり、
   前記制御部が、今後数秒間のうちに進入することとなる複数の経路の部分についての複数の経路点データを経路点データ格納部から読み出して制御に用いるプログラム。

Images