JP5837902B2

JP5837902B2 - 自律走行車両、自律走行車両の制御システム及び自律走行車両の制御方法

Info

Publication number: JP5837902B2
Application number: JP2013167530A
Authority: JP
Inventors: 隆則深尾; 石山　健二; 健二石山; 浩久今井
Original assignee: Yamaha Motor Co Ltd
Current assignee: Yamaha Motor Co Ltd
Priority date: 2013-08-12
Filing date: 2013-08-12
Publication date: 2015-12-24
Anticipated expiration: 2033-08-12
Also published as: JP2015036840A

Description

本発明は、自律走行車両、自律走行車両の制御システム及び自律走行車両の制御方法に関する。

省力化・省人化や、人体にとって過酷な環境下での作業を可能とするべく、無人にて自律的に走行する車両が研究されている。車両の位置を走行経路に追従させるように自律走行させる際には、通常、車両の現在位置及び姿勢を知る必要がある。自律走行車両の現在位置及び姿勢を知る方法には種々のものがあるが、ＬＲＦ（ＬａｓｅｒＲａｎｇｅＦｉｎｄｅｒ）等のセンサにより車両周辺の地形を測定して、あらかじめ用意した地図と測定結果を照合する方法が知られている。

特許文献１には、自律移動装置において、レーザレンジファインダの検出結果に基づいて生成したローカルマップ及び、自機の移動量からベイズフィルタを用いて確率的に自己位置を推定することが記載されている。

特許文献２には、ロボットにおいて、予め求めた自己位置推定容易性パラメータの値に応じて、外界センサ部により得られた情報及び内界センサ部により取得された位置情報に重み付けし、重み付けした情報に基づいて自己位置を推定することが記載されている。

特開２０１０−７９８６９号公報特開２０１２−１４１６６２号公報

センサによる地形測定に基づく自己位置推定では、推定結果にノイズが含まれる。このような場合に自律走行車両を走行経路に追従させようとすると、自己位置推定の誤差に起因して、その挙動が不安定となりがちとなる。かといって、ロバスト安定性を重視して追従制御を保守的に設計すると、追従性能が低下する。

本発明はかかる観点に鑑みてなされたものであって、その目的は、走行経路に追従して走行する自律走行車両において、自己位置推定に誤差が含まれる場合にその走行安定性と、追従性能を両立させることである。

本出願において開示される発明は種々の側面を有しており、それら側面の代表的なものの概要は以下のとおりである。

（１）設定された走行経路に追従して走行するよう車両を制御する車両制御部と、車両の周囲の地形を計測する地形計測部と、前記地形計測部により計測された計測地形と地図とを照合して前記車両の現在位置及び姿勢を推定する位置姿勢推定部と、前記位置姿勢推定部による推定結果の信頼性を評価する信頼性評価部と、信頼性評価に基づいて前記車両制御部の制御パラメータ及び制御入力の少なくともいずれかを変更する制御変更部と、を有し、前記地形計測部は、前記車両の周囲に存在するランドマークの車両に対する位置を計測し、前記地図は、前記ランドマークの位置を記録したものであり、前記信頼性評価部は、前記地図に記録された前記ランドマーク間を結ぶ直線と、計測された前記ランドマークを前記地図上にプロットした際に、当該ランドマーク間を結ぶ直線との傾きの差に基づいて信頼性を評価する、自律走行車両。

（２）（１）において、前記位置姿勢推定部は、前記車両と前記ランドマーク間の距離及び、前記地図に基づいて前記車両の現在位置及び姿勢を推定する、自律走行車両。

（３）（１）又は（２）において、前記制御変更部は、車両の目標速度を変更するもの、操舵角ゲインを変更するもの、操舵角目標値に時間フィルタを適用するもの、及び、前記走行経路に接続する修正経路を生成するものの少なくともいずれかである自律走行車両。

（４）（３）において、前記修正経路は、前記位置姿勢推定部により推定された前記車両の現在位置及び姿勢から前記走行経路になだらかに接続するクロソイド曲線である自律走行車両。

（５）設定された走行経路に追従して走行するよう車両を制御する車両制御部と、車両の周囲の地形を計測する地形計測部と、前記地形計測部により計測された計測地形と地図とを照合して前記車両の現在位置及び姿勢を推定する位置姿勢推定部と、前記位置姿勢推定部による推定結果の信頼性を評価する信頼性評価部と、信頼性評価に基づいて前記車両制御部の制御パラメータ及び制御入力の少なくともいずれかを変更する制御変更部と、を有し、前記地形計測部は、前記車両の周囲に存在するランドマークの車両に対する位置を計測し、前記地図は、前記ランドマークの位置を記録したものであり、前記信頼性評価部は、前記地図に記録された前記ランドマーク間を結ぶ直線と、計測された前記ランドマークを前記地図上にプロットした際に、当該ランドマーク間を結ぶ直線との傾きの差に基づいて信頼性を評価する、自律走行車両の制御システム。

（６）設定された走行経路に追従して走行するよう車両を制御するステップと、車両の周囲の地形を計測するステップと、計測された地形と地図とを照合して前記車両の現在位置及び姿勢を推定するステップと、前記車両の現在位置及び姿勢の推定結果の信頼性を評価するステップと、信頼性評価に基づいて車両制御の制御パラメータ及び制御入力の少なくともいずれかを変更するステップと、を有し、前記地形を計測するステップでは、前記車両の周囲に存在するランドマークの車両に対する位置を計測し、前記地図は、前記ランドマークの位置を記録したものであり、前記信頼性を評価するステップでは、前記地図に記録された前記ランドマーク間を結ぶ直線と、計測された前記ランドマークを前記地図上にプロットした際に、当該ランドマーク間を結ぶ直線との傾きの差に基づいて信頼性を評価する、自律走行車両の制御方法。

上記本発明によれば、走行経路に追従して走行する自律走行車両において、自己位置推定に誤差が含まれる場合にその走行安定性と、追従性能を両立させることができる。

本発明の一実施形態に係る自律走行車両の外観図である。自律走行車両の構成を示す模式図である。自律走行車両の機能的構成を示す機能ブロック図である。位置姿勢推定部が現在位置を推定するアルゴリズムを示すフローチャートである。地図の例を示す平面図である。クロソイド曲線による修正経路の例を示す図である。

以下に、本発明の一実施形態を図面を参照しつつ説明する。

図１は、本発明の一実施形態に係る自律走行車両１の外観図である。本発明における自律走行車両とは、自動運転として、あらかじめ設定した経路又は自動生成した経路上をあらかじめ与えられ、又は自動生成された速度プロファイルに追従するように自律走行を行う車両を指している。したがって、自律走行車両１がそれ以外の走行時、例えば自動運転の状態にない場合の有人／無人運転の別は問わない。また、図１に示した自律走行車両１は４輪自動車であるが、装輪数は３輪であっても、４輪であっても、或いは６輪以上の多装輪であっても、無限軌道車であってもよい。さらに、以降説明する本実施形態における自律走行車両１の制御は不整地走行に特に適したものであるため、自律走行車両１は不整地走行に適したＡＴＶ（ＡｌｌＴｅｒｒａｉｎＶｅｈｉｃｌｅ）を改修したものとして示しているが、自律走行車両１が舗装面等の整地を走行しても差し支えない。

本実施形態では、自律走行車両１は、果樹園等の農園を走行しつつ、果樹等の作物に対して農薬等の薬液を散布する農業用車両として示されており、車両本体２の後部に農薬散布用のスプレーノズル３と、薬液を貯蔵するタンクが装備されている。しかしながら、これは単に自律走行車両１の実用上の役割の一例を示すものであり、自律走行車両１は農業用車両に限定されない。車両本体２は、作業者による有人運転と、後述するコントローラ６による無人運転の両方が可能であり、作業者による操作または指示により有人／無人運転が切り替えられる。

また、自律走行車両１には、車両の周囲の地形、特に、車両の水平方向についての障害物の有無とその障害物までの距離を計測する地形計測部として、ＬＲＦ（レーザレンジファインダ）５が設けられている。ＬＲＦ５の設置数は任意であるが、ここでは、水平方向に２７０°の測定範囲をもつＬＲＦ５を車体の左右側面に設け、自律走行車両１の周囲３６０°にわたって障害物までの距離を測定できるようにしている。なお、ＬＲＦ５の設置個数及び設置位置は任意であり、また、地形計測部として用いるセンサは、ＬＲＦに限られず、車両の水平方向について障害物の車両に対する位置が測定できるものであればどのようなものであってもよい。例えば、ステレオカメラを用いた画像認識により障害物の存在及び、障害物までの距離を測定するものであってもよい。

なお、本実施形態では、コントローラ６は車両本体２に搭載されているものとして以降の説明を行うが、コントローラ６を車両本体の外部に設置し、無線通信により車両本体２の制御を行うものとしてもよい。かかる場合には、コントローラ６と、車両本体２を含む車両の制御システムとして本発明が把握され得ることとなる。無線通信は、例えば、車両本体２に設けられたアンテナ４を用いて行われる。

図２は、自律走行車両１の構成を示す模式図である。自律走行車両１の車両本体２は、前輪である操舵輪２１と、操舵輪２１の操舵角を変更するためのステアリングアクチュエータ２２、後輪である駆動輪２３に回転駆動力を与えるエンジン又は電動機である駆動力源２４を備えており、ステアリングアクチュエータ２２と駆動力源２４を情報処理装置であるコントローラ６により電子的に制御するようになっている。なお、図示のものでは駆動力源２４は単一のものとして示しているが、駆動力源２４として内燃機関と電動機を併設するいわゆるハイブリッドシステムを用いてもよいし、電動機を駆動輪２３内に設置するいわゆるインホイールモータとしてもよい。

操作部２５は、自律走行車両１を有人運転する際の各種操作用部材であり、具体的には、ハンドルや、アクセル／ブレーキ等のペダル、各種スイッチ類である。本実施形態では、自律走行車両１は、搭乗員による有人運転を行う場合には、この操作部２５に入力された操作量がコントローラ６に伝えられ、その操作量に応じてステアリングアクチュエータ２２や駆動力源２４が駆動されるようになっている。これに対し、自律走行車両１が無人運転となっている場合には、操作部２５からの入力は自律走行車両１の走行には反映されない。自律走行車両１の有人運転／無人運転の切り替えは、この操作部２５に設けられた適宜のスイッチ、例えば、キースイッチのポジションを変更することにより行われてよい。

インタフェース２６は、コントローラ６に対し、自律走行車両１を無人運転する際に必要となる各種情報を入力するためのマンマシンインタフェースである。このインタフェース２６の形式は特に限定されず、どのようなものであってもよい。例えば、無線又は有線により通信可能に接続された任意のコンピュータ又はティーチングペンダント等の操作ボックスをインタフェース２６として用いてよい。あるいは、自律走行車両１の適宜の場所、例えば運転席にタッチパネル式ディスプレイ等の入力デバイスを設け、かかる入力デバイスをインタフェース２６としてもよい。いずれにせよ、オペレータは、インタフェース２６を用いて、自律走行車両１を自動走行させる際に必要となるパラメータや地図等の必要な情報をコントローラ６に入力する。

記憶部２７は、情報記憶であり、好ましくは不揮発性のメモリである。そして、記憶部２７には、自律走行車両１が自律走行を行う際に必要となるパラメータや、地図等の情報が記憶される。

さらに、コントローラ６には、ＬＲＦ５により計測された自律走行車両１の周囲の地形を示す情報である計測地形が入力される。また、コントローラ６からは、適宜、スプレーノズル３に対し、噴霧の有無を制御する信号が送られ、スプレーノズル３は、コントローラ６からの指示に従って噴霧を行う。

以上の構成により、自律走行車両１はコントローラ６により制御され、自律走行を行う。ここで、コントローラ６は、物理的には、ＣＰＵ（ＣｅｎｔｒａｌＰｒｏｃｅｓｓｉｎｇＵｎｉｔ）とメモリ等からなるコンピュータであっても、いわゆるマイクロコントローラや、ＤＳＰ（ＤｉｇｉｔａｌＳｉｇｎａｌＰｒｏｃｅｓｓｏｒ）、ＦＰＧＡ（ＦｉｅｌｄＰｒｏｇｒａｍｍａｂｌｅＧａｔｅＡｒｒａｙ）等であってもよく、任意の情報処理装置を使用してよい。また、コントローラ６は単一の部品により構成しても、複数の部品を結合させて構成してもよい。自律走行車両１におけるステアリングアクチュエータ２２に対する制御、すなわち、操舵角制御と、駆動力源２４に対する制御、すなわち、速度制御の２つの制御は、コントローラ６による情報処理により行われる。

操舵角制御は、前述したように、あらかじめ設定した経路又は自動生成した経路を追従するよう操舵輪２１を駆動する制御であり、オペレータによりあらかじめ与えられた地図上における自律走行車両１の現在位置及び姿勢（向き）を推定し、追従すべき経路に基いた制御入力に自律走行車両１の現在位置及び姿勢が合致するようにするというものである。この制御は、古典的なＰＩＤ制御等の制御手法によってもよいし、その他の制御手法、例えば、Ｈ∞制御等の制御手法によってもよい。操舵角制御における制御の鋭敏性、すなわち、自律走行車両１の推定された現在位置及び姿勢と追従すべき車両位置及び姿勢との誤差に対する制御出力の大きさは、パラメータである操舵角ゲインに反映される。操舵角ゲインは、記憶部２７に記憶される。

速度制御は、あらかじめ与えられ、又は自動生成された速度プロファイルに車両速度を追従させるよう駆動力源２４を駆動する制御であり、自律走行車両１の現在速度が追従すべき速度に合致するようにするというものである。この制御もまた古典的な制御手法によっても、Ｈ∞制御等の他の制御手法によってもよい。速度制御における制御の鋭敏性はパラメータである速度ゲインに反映される。速度ゲインもまた、記憶部２７に記憶される。

図３は、自律走行車両１の機能的構成を示す機能ブロック図である。

コントローラ６内には、自律走行車両１を設定された走行経路に追従して走行するよう制御する車両制御部６１が含まれる。本実施形態では、車両制御部６１には、操舵角制御を行う操舵角制御部６２と速度制御を行う速度制御部６３の２つの制御部が含まれており、操舵角制御部６２は、後述するＥＫＦ（拡張カルマンフィルタ）６５を通して、位置姿勢推定部６４により推定された車両の現在位置及び姿勢、現在の操舵角θ及び制御入力ｕの入力を受けてステアリングアクチュエータ２２への制御出力を行う部分である。また、速度制御部６３は、現在の車両速度ｖ及び制御入力ｕの入力を受けて駆動力源２４への制御出力を行う部分である。操舵角制御部６２及び速度制御部６３のいずれか又は両方は、後述する制御変更部６７からの入力により、使用するパラメータ及び制御入力のいずれかまたは両方が変更される。

位置姿勢推定部６４は、記憶部２７に記憶された地図と、ＬＲＦ５により計測された計測地形とを照合して、自律走行車両１の現在位置及び姿勢を推定する部分である。ＥＫＦ６５は、ＬＲＦ５による測定にはノイズが含まれることから、推定結果を平滑化し、誤差を最小にするために設けられている。ＥＫＦ６５は、不要であればこれを省略してもよく、また、ＥＫＦ６５に換えて、ノイズを低減するために用いられる他の形式のフィルタを用いてもよい。位置姿勢推定部６４は、１サイクル前のＥＫＦ６５を通過した自律走行車両１の現在位置及び姿勢を前回位置及び姿勢として用いている。

信頼性評価部６６は、位置姿勢推定部６４による推定結果の信頼性を評価する部分であり、その評価の結果を制御変更部６７へと出力する。制御変更部６７は、信頼性評価部６６による評価の結果が信頼性が低いことを示すものである場合に、車両制御部６１にて用いられる制御パラメータ及び、制御入力の少なくともいずれかを変更するものである。信頼性評価部６６及び制御変更部６７の動作の詳細及びその技術的意味については後述する。

以上説明したコントローラ６内部の機能ブロックは、必ずしもこれらブロックが物理的に存在している必要はなく、コントローラ６上で実行されるソフトウェアにより機能的に実現されるものである。したがって、コントローラ６上で実行されるソフトウェアを記述するプログラムにおいても、これらブロックが必ずしも明確に分離できるとは限らない。

続いて、コントローラ６の位置姿勢推定部６４が実行する現在位置推定方法について説明する。図４は、本実施形態の自律走行車両１において、位置姿勢推定部６４が現在位置を推定するアルゴリズムを示すフローチャートである。

まず、ステップＳＴ１で、位置姿勢推定部６４は、前回推定された車両位置及び姿勢である前回位置及び姿勢を読み込む。前回位置及び姿勢が求められていない場合、すなわち、初回の実行の場合には、あらかじめ与えられた初期位置及び姿勢を前回位置及び姿勢とする。続いて、ステップＳＴ２で、ＬＲＦ５による計測を行い、計測された地形である計測地形を読み込む。

そして、続くステップＳＴ３で、地図に含まれるランドマーク（以下、「地図ランドマーク」という。）の一つに対して、計測地形に含まれるランドマーク（以下、「計測ランドマーク」という。）のうち、対応するものを抽出する。ここで、地図は、自律走行車両１が走行することを予定している領域の平面図のデータであり、地形として、当該領域内に含まれるランドマークの位置情報を含むものである。そして、ランドマークとは、自律走行車両１が走行する際のスポット状の障害物を指しており、個々のランドマークの位置は地図上の点座標として表現される。本実施形態では、ランドマークは、果樹園内に植栽された果樹を示している。

図５は、地図の例を示す平面図である。ここでは、地図は、ＸＹ平面上にプロットされた６つの黒丸として示され、それぞれの黒丸は地図ランドマーク、すなわち、果樹の位置を示している。ここでは理解を容易にするために、各地図ランドマークに通し番号を付けた。また、同図中には、自律走行車両１の前回位置及び姿勢Ｐを示した。ここで、自律走行車両１が前回位置及び姿勢Ｐにあると仮定してＬＲＦ５により計測された計測地形に含まれる計測ランドマークの位置の例を白丸で示す。このとき、参照している地図ランドマークの位置に最も近い計測ランドマークを、当該参照している地図ランドマークに対応するものとみなしてよい。なぜなら、ＬＲＦ５による計測及び、位置姿勢推定部６４による自律走行車両１の自己位置推定の実行周期が自律走行車両１の走行速度に比して十分に短い場合には、地図ランドマークの位置と計測ランドマークの位置とは大きくずれることはないと考えられるからである。同図中では、計測ランドマークに対応する地図ランドマークと同番号を付した。

なお、計測ランドマークが狭い領域に複数存在する場合には、測定時のノイズにより、単一のランドマークが複数の点として検出されたものと考えられるので、計測ランドマークのうち、互いの距離が所定の距離以下であるものについてはこれらをひとまとまりとして、その重心位置に単一の計測ランドマークがあるものとして取り扱ってよい。

図４に戻り、続くステップＳＴ４では、対応づけられた計測ランドマークの信頼性を評価する。この信頼性は、対応する地図ランドマークと計測ランドマーク間の距離、計測ランドマークの自律走行車両１からの距離等に基いて設定してよい。ｎ番目の計測ランドマークについて、その信頼性をｃ_ｎ、対応する地図ランドマークからの距離をｄ_ｎ、自律走行車両１からの距離をＬ_ｎとすると、信頼性は任意の関数ｆにより次のようにあらわされる。

本実施形態では、信頼性ｃ_ｎは次の式により求めている。

この式は、その値が大きいほど信頼性が高いことを示しており、対応する地図ランドマークからの距離ｄ_ｎがより小さく、自律走行車両１からの距離Ｌ_ｎがより大きい方が信頼性が高くなるように設定されたものである。なお、この式の具体的な形は使用するＬＲＦ５や他の形式のセンサの特性等に合わせ適宜設計してよい。

続くステップＳＴ５で、全ての地図ランドマークに対して対応付け及び信頼性評価が終了したか否かを判定し、終了していなければステップＳＴ３へと戻る。終了していればステップＳＴ６へと進む。

ステップＳＴ６では、計測ランドマークの信頼性を用いて、自律走行車両１の現在位置を推定する。この推定には、種々の方法が考えられるが、本実施形態では、三角測量による方法を用いている。以下この方法を説明する。

番号ｎの地図ランドマークの地図上の座標を（ｘ_ｎ，ｙ_ｎ）とし、自律走行車両１の現在位置の地図上の座標を（ｘ_Ｒ，ｙ_Ｒ）とすると、任意の異なる番号ａ，ｂについて地図ランドマークに対応する計測ランドマークから自律走行車両１までの距離Ｌ_ａ，Ｌ_ｂを用いて、三角測量の手法により、（ｘ_Ｒ，ｙ_Ｒ）を求めることができる。なお、厳密にはこの手法では（ｘ_Ｒ，ｙ_Ｒ）は２点求められるが、これは自律走行車両１の前回位置（ｘ_Ｐ，ｙ_Ｐ）により近い方を選択するとよい。すなわち、番号ａ，ｂについての現在位置（ｘ_Ｒ，ｙ_Ｒ）_ａ，ｂは三角測量関数をｇとすると、次の式であらわされる。

ここで、番号ａ，ｂについての現在位置（ｘ_Ｒ，ｙ_Ｒ）_ａ，ｂの信頼性ｅ_ａ，ｂを次式のように設定する。

なお、信頼性ｅ_ａ，ｂはここでは三角測量に用いた計測ランドマークの信頼性ｃ_ａ、ｃ_ｂのうち信頼性の低いものを二乗したものとしているが、これに限定されるものでなく、これ以外にも、単により信頼性の低いものを使用したり、計測ランドマークの信頼性ｃ_ａ、ｃ_ｂを積算したものを使用したりしてもよい。

最終的に求めるべき自律走行車両１の現在位置（ｘ_Ｒ，ｙ_Ｒ）は、異なるランドマークの番号ｎ，ｍの全ての組み合わせについて信頼性ｅ_ａ，ｂによる重みづけを行い、次式により求められる。

さらに、ステップＳＴ７にて自律走行車両１の現在姿勢を求める。この手法には種々のものが考えられるが、本実施形態では次のようにして行う。

任意の異なる番号ａ，ｂについて、ａ番目の地図ランドマークとｂ番目の地図ランドマークを結ぶ直線と、それぞれ対応する計測ランドマークを結ぶ直線とがなす角をθ_ａ，ｂとしたときに、その平均値θ_ａｖｒを前回姿勢に対する現在姿勢のヨー角差として用いる。すなわち、ランドマークの個数をｋとすると、

ここで、重みづけとして前述の信頼性ｅ_ａ，ｂを考慮するものとしてもよい。その場合には、θ_ａｖｒは次式であらわされる。

前回姿勢をθ_Ｐとすると、現在姿勢θ_Ｒは、次の通りである。

なお、以上で説明したステップＳＴ６、ＳＴ７における現在位置及び姿勢の推定にあたっては三角測量の手法を用いるものとしたが、必ずしもこの方法に限定はされない。この他にも、例えば、いわゆる最小二乗法によってもよい。すなわち、対応する地形ランドマークと計測ランドマークとの距離ｄ_ｎを誤差と考えると、この二乗和Ｊは、計測ランドマークの信頼性ｃ_ｎを二乗したもので重みづけを行うと次式であらわされる。

このＪを最小化するような計測地形から地図への写像を求めることにより、その写像の原点位置及び向きとして、自律走行車両１の現在位置及び姿勢を求めることができる。もちろん、ここで重みづけをどのようにするかは任意であり、また、現在位置及び姿勢を求める方法として、さらに他の方法を用いてもよい。

ところで、他のランドマークの背後となるなどしてＬＲＦ５によりその位置を計測することができないランドマークがある場合には、当該ランドマークを除外して上記アルゴリズムを実行すればよい。

以上説明した位置姿勢推定部６４にて算出される信頼性ｃ_ｎは、個々の計測ランドマークの信頼性を示すものであって、ＬＲＦ５による計測全体の信頼性や、現在位置及び姿勢の推定結果に対する信頼性を示すものではない。

そこで、図３に示す信頼性評価部６６は、位置姿勢推定部６４による現在位置及び姿勢の推定結果に対する信頼性を評価する。この評価は、単純に個々の計測ランドマークの信頼性の総和或いは二乗和等としてもよいが、本実施形態では、より実用上の利用価値が高い評価として、図４のステップＳＴ７にて使用した、任意の２つの地図ランドマーク間を結ぶ直線と、それぞれ対応する計測ランドマークを結ぶ直線とがなす角θ_ａ，ｂを用いる。すなわち、信頼性評価部６６は、地図ランドマーク間を結ぶ直線と、地図上にプロットされた計測ランドマーク間を結ぶ直線との傾きの差に基づいて信頼性を評価する。図５中に、ランドマーク番号１番と２番について、角θ_１，２を示した。

ここでは、位置姿勢推定部６４による現在位置及び姿勢の推定結果に対する信頼性ｃ_ｅｓｔを次式により求めている。

この場合の信頼性ｃ_ｅｓｔは、その値が小さいほど信頼性が高いことを示している。なお、上式は信頼性ｃ_ｅｓｔの一例である。より一般的な表現とすると、任意の関数ｈを用い、次の通りである。

（全てのｎ及びｎ，ｍの組み合わせについて）

ところで、以上の説明では、現在位置及び姿勢の推定の際、及びその信頼性の評価の際に全てのランドマーク番号ｎ，ｍの組み合わせについて計算を行ったが、計算の負荷が大きい場合には、計測ランドマークの信頼性ｃ_ｎが一定以上のランドマーク、あるいは計測ランドマークの信頼性ｃ_ｎが大きいものから一定数のランドマークについてのみ組み合わせについての計算を行うようにしてもよい。

信頼性評価部６６により評価された信頼性ｃ_ｅｓｔは図３に示す制御変更部６７へと受け渡され、制御変更部６７は必要に応じて車両制御部６１の制御パラメータ及び制御入力のいずれか又は両方を変更する。

この物理的意味合いについて考察する。信頼性ｃ_ｅｓｔ、すなわち、現在位置及び姿勢の推定値の信頼性が低い場合というのは、自律走行車両１全体がＬＲＦ５による計測に不利な（すなわち、誤差を生じやすい）条件となっている場合であると考えられる。そのような場合としては、例えば、路面の凹凸や、ステアリング時の遠心力により自律走行車両１の車体がロール方向或いはピッチ方向に傾き、路面に対する車体の水平が損なわれている場合が考えられる。このような場合に、所定の経路及び速度に対する追従性を重視した制御を継続すると、車体の揺れや傾きが大きいままとなり、現在位置及び姿勢の推定値に含まれる誤差が大きいため、制御が不安定となりがちである。

そこで、制御変更部６７は、現在位置及び姿勢の推定値の信頼性が低い、この場合は、信頼性ｃ_ｅｓｔの値が所定の閾値より大きい場合に、車両制御部６１の制御パラメータ及び制御入力のいずれか又は両方を変更し、車体の揺れや傾きを緩和する。こうすることにより、現在位置及び姿勢の推定値の信頼性が十分に得られている間は追従性を重視した制御を実行し、現在位置及び姿勢の推定値の信頼性が損なわれた場合に車体の姿勢回復を重視した制御を行うことができ、その結果、自律走行車両１の走行安定性と、追従性能を両立させることが可能となるのである。

より具体的には、制御変更部６７は、次のいずれかの処理又はこれらの処理の複数を行う。
・制御パラメータである操舵角ゲインを低減し、急峻なステアリング操作が行われないようにする。
・制御入力である速度目標値を低減し、車両速度を低下させる。
・制御入力である操舵角目標値に対し時間フィルタ、例えば一次遅れフィルタを適用し、急峻なステアリング操作が行われないようにする。
・規定の走行経路に接続する修正経路を生成し、急峻なステアリング操作が行われないようにする。

最後のものについてより詳しく説明する。上述の議論から明らかなように、急なステアリング操作により発生するロールは現在位置及び姿勢の推定値の信頼性を悪化させる要因であると考えられる。ここで、自律走行車両１の現在位置及び姿勢が規定の走行経路から大きく外れている場合には、操舵角制御部６２は、速やかに自律走行車両１を走行経路上に復帰させるべく、ステアリングアクチュエータ２２に対し、大きな制御指令を発するものと考えられる。しかしながら、このような制御指令は、自律走行車両１の車体の姿勢を損なうものであり、現在位置及び姿勢の推定値の信頼性を悪化させると考えられる。

そこで、制御変更部６７は、自律走行車両１の現在位置及び姿勢から、規定の走行経路へとなだらかに接続する修正経路を生成する。操舵角制御部６２がかかる修正経路を目標の経路とすることにより、自律走行車両１は規定の走行経路へと滑らかに復帰し、車体の姿勢の悪化を小さく留めることができると考えられる。

修正経路は特に限定されないが、自律走行車両１が追従しやすいものとして、クロソイド曲線により修正経路が生成されることが望ましい。図６は、クロソイド曲線による修正経路の例を示す図である。規定の経路Ｓに対し、クロソイド曲線により生成された修正経路Ｃを破線で示した。修正経路Ｃの起点における接線方向は現時点における自律走行車両１の操舵角と一致し、修正経路Ｃの終点における接線方向は規定の経路Ｓの接線方向と一致するようになっている。クロソイド曲線を生成する上での操舵角速度ωはあらかじめ指定しておくとよい。

なお、以上説明した制御変更部６７における処理は、車体の姿勢を回復させるための処理の例であり、車体の姿勢回復に資すると考えられる他の処理を行ってもよい。

１自律走行車両、２車両本体、３スプレーノズル、４アンテナ、５ＬＲＦ、
６コントローラ、２１操舵輪、２２ステアリングアクチュエータ、２３駆動輪、２４駆動力源、２５操作部、２６インタフェース、２７記憶部、６１車両制御部、６２操舵角制御部、６３速度制御部、６４位置姿勢推定部、６５ＥＫＦ、６６信頼性評価部、６７制御変更部。

Claims

設定された走行経路に追従して走行するよう車両を制御する車両制御部と、
車両の周囲の地形を計測する地形計測部と、
前記地形計測部により計測された計測地形と地図とを照合して前記車両の現在位置及び姿勢を推定する位置姿勢推定部と、
前記位置姿勢推定部による推定結果の信頼性を評価する信頼性評価部と、
信頼性評価に基づいて前記車両制御部の制御パラメータ及び制御入力の少なくともいずれかを変更する制御変更部と、
を有し、
前記地形計測部は、前記車両の周囲に存在するランドマークの車両に対する位置を計測し、
前記地図は、前記ランドマークの位置を記録したものであり、
前記信頼性評価部は、前記地図に記録された前記ランドマーク間を結ぶ直線と、計測された前記ランドマークを前記地図上にプロットした際に、当該ランドマーク間を結ぶ直線との傾きの差に基づいて信頼性を評価する、
自律走行車両。
前記位置姿勢推定部は、前記車両と前記ランドマーク間の距離及び、前記地図に基づいて前記車両の現在位置及び姿勢を推定する、
請求項１に記載の自律走行車両。
前記制御変更部は、車両の目標速度を変更するもの、操舵角ゲインを変更するもの、操舵角目標値に時間フィルタを適用するもの、及び、前記走行経路に接続する修正経路を生成するものの少なくともいずれかである請求項１又は２に記載の自律走行車両。
前記修正経路は、前記位置姿勢推定部により推定された前記車両の現在位置及び姿勢から前記走行経路になだらかに接続するクロソイド曲線である請求項３に記載の自律走行車両。
設定された走行経路に追従して走行するよう車両を制御する車両制御部と、
車両の周囲の地形を計測する地形計測部と、
前記地形計測部により計測された計測地形と地図とを照合して前記車両の現在位置及び姿勢を推定する位置姿勢推定部と、
前記位置姿勢推定部による推定結果の信頼性を評価する信頼性評価部と、
信頼性評価に基づいて前記車両制御部の制御パラメータ及び制御入力の少なくともいずれかを変更する制御変更部と、
を有し、
前記地形計測部は、前記車両の周囲に存在するランドマークの車両に対する位置を計測し、
前記地図は、前記ランドマークの位置を記録したものであり、
前記信頼性評価部は、前記地図に記録された前記ランドマーク間を結ぶ直線と、計測された前記ランドマークを前記地図上にプロットした際に、当該ランドマーク間を結ぶ直線との傾きの差に基づいて信頼性を評価する、
自律走行車両の制御システム。
設定された走行経路に追従して走行するよう車両を制御するステップと、
車両の周囲の地形を計測するステップと、
計測された地形と地図とを照合して前記車両の現在位置及び姿勢を推定するステップと、
前記車両の現在位置及び姿勢の推定結果の信頼性を評価するステップと、
信頼性評価に基づいて車両制御の制御パラメータ及び制御入力の少なくともいずれかを変更するステップと、
を有し、
前記地形を計測するステップでは、前記車両の周囲に存在するランドマークの車両に対する位置を計測し、
前記地図は、前記ランドマークの位置を記録したものであり、
前記信頼性を評価するステップでは、前記地図に記録された前記ランドマーク間を結ぶ直線と、計測された前記ランドマークを前記地図上にプロットした際に、当該ランドマーク間を結ぶ直線との傾きの差に基づいて信頼性を評価する、
自律走行車両の制御方法。