(57)【特許請求の範囲】
【請求項１】
車両の自動運転システムに用いられる走行計画生成装置であって、
前記車両の状態量に関する制約を生成する制約生成部と、
収束演算を伴わない状態推定演算としてベイズフィルタを用いて前記制約を満たすように前記車両の目標軌道および目標車速を走行計画として生成する計画生成部と、を有する、走行計画生成装置。
【請求項２】
前記制約は、前記車両のタイヤ発生力が摩擦円限界を超えないという拘束条件を課した上で、前記拘束条件を満たすために前記車両の状態量が取り得る値の範囲として生成される、請求項１に記載の走行計画生成装置。
【請求項３】
前記拘束条件は、前記走行計画生成装置の外部から取得される路面の状態に基づいて決定される、請求項２記載の走行計画生成装置。
【請求項４】
前記状態推定演算は、前記ベイズフィルタとして複数のパーティクルによって状態の確率密度分布を近似するパーティクルフィルタを用いる、請求項１記載の走行計画生成装置。
【請求項５】
前記計画生成部は、
前記パーティクルフィルタで用いられるシステムモデルへの入力値を前記制約に基づいて決定する、請求項４記載の走行計画生成装置。
【請求項６】
前記計画生成部は、
前記走行計画生成装置の外部から取得される障害物の予測軌道に基づいて、前記パーティクルフィルタにおける前記複数のパーティクルのそれぞれにおいて、前記車両が存在する領域に前記障害物が存在するか否かを判定し、前記障害物が存在すると判定した場合には、前記障害物が存在すると判定されたパーティクルの重みを０にする、請求項４記載の走行計画生成装置。
【請求項７】
前記制約は、
車体の横滑り角、タイヤの横滑り角、舵角、舵角速度、速度、加速度およびヨーレートのうちの少なくとも１つが取り得る値の範囲で規定される、請求項１記載の走行計画生成装置。
【請求項８】
前記状態推定演算は、
状態変数に、前記車両の位置、方位、速度、加速度、舵角、舵角速度、ヨーレートおよび車体の横滑り角のうちの少なくとも１つを含む、請求項１記載の走行計画生成装置。
【請求項９】
前記状態推定演算は、
観測変数に、前記車両の速度、前記車両から目標車線への横偏差、前記車両の周囲に設定した領域への前記障害物の侵入距離のうちの少なくとも１つを含む請求項６記載の走行計画生成装置。
【請求項１０】
請求項１記載の走行計画生成装置と、
前記車両の周辺の障害物の情報を取得する情報取得装置と、
前記情報取得装置から前記障害物の情報を取得して、前記障害物の予測軌道を取得する環境状態推定装置と、を備え、
前記走行計画生成装置は、
前記環境状態推定装置から取得した前記障害物の予測軌道に基づいて、前記車両と前記障害物との接触を回避した前記走行計画を生成する、自動運転システム。
【請求項１１】
前記走行計画生成装置で生成された前記走行計画に基づいて、前記車両の目標操舵角、目標駆動力および目標制動力を演算して、前記車両の軌道、前記車両の加速および減速を制御する車両制御装置をさらに備える、請求項１０記載の自動運転システム。
【請求項１２】
請求項１から請求項９のいずれか１項に記載の走行計画生成装置と、
前記走行計画生成装置で生成された前記走行計画に基づいて、前記車両の目標操舵角、目標駆動力および目標制動力を演算して、前記車両の軌道、前記車両の加速および減速を制御する車両制御装置と、を備える、自動運転システム。
【請求項１３】
前記車両の周辺環境の情報を取得する情報取得装置と、
前記情報取得装置で取得された前記周辺環境の情報に基づいて、前記車両が走行する路面の路面摩擦係数を推定する環境状態推定装置と、をさらに備え、
前記走行計画生成装置は、
前記環境状態推定装置で推定された路面摩擦係数を用いて前記制約を生成する、請求項１１または請求項１２記載の自動運転システム。
【請求項１４】
前記車両の外部との通信を行って前記車両が走行する路面の路面摩擦係数を取得する情報取得装置をさらに備え、
前記走行計画生成装置は、
前記情報取得装置で取得された前記路面摩擦係数を用いて前記制約を生成する、請求項１１または請求項１２記載の自動運転システム。
【請求項１５】
車両の自動運転システムに用いられる走行計画生成装置であって、
前記車両の状態量に関する制約を生成する制約生成部と、
収束演算を伴わない状態推定演算により、前記制約を満たすように前記車両の目標軌道および目標車速を走行計画として生成する計画生成部と、を有し、
前記制約は、前記車両のタイヤ発生力が摩擦円限界を超えないという拘束条件を課した上で、前記拘束条件を満たすために前記車両の状態量が取り得る値の範囲として生成される、走行計画生成装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は走行計画生成装置に関し、特に、車両等の自動運転を実現する走行計画生成装置に関する。
【背景技術】
【０００２】
従来、車両の自動運転システムにおいて、車両の走行すべき軌道と車速とから走行計画を生成する手法として、収束演算を用いるものが知られている。例えば、特許文献１には、車両が走行する路面の状態に応じて走行計画の拘束条件を設定し、走行計画がその拘束条件を満たすまで収束演算を行う。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特開２００９−１６６６２３号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
特許文献１の手法では、拘束条件として、道路形状情報および路面摩擦係数などの道路環境上の要求、道路上を走行するなどの交通上の要求、摩擦円限界、加減速限界、操舵限界等の車両性能から生じる要求に基づき拘束条件を設定する。特許文献１のように収束演算を用いる手法では評価関数、制約条件によっては解として局所最適解を出力してしまうこととなり、走行計画が望ましい解に収束しない場合、および収束演算に時間を要する場合があり、適切な走行計画を安定した時間で出力できない可能性がある。【０００５】
この発明は、上記のような問題点を解決するためになされたものであり、適切な走行計画を安定した時間で演算する走行計画生成装置を提供することを目的とする。
【課題を解決するための手段】
【０００６】
本発明に係る走行計画生成装置は、車両の自動運転システムに用いられる走行計画生成装置であって、前記車両の状態量に関する制約を生成する制約生成部と、収束演算を伴わない状態推定演算としてベイズフィルタを用いて前記制約を満たすように前記車両の目標軌道および目標車速を走行計画として生成する計画生成部と、を有する。
【発明の効果】
【０００７】
本発明に係る走行計画生成装置によれば、収束演算を伴わない状態推定演算としてベイズフィルタを用いて走行計画を生成するので、安定した計算時間で安全な走行計画の生成が可能となる。また、収束演算を伴わない状態推定演算を用いることにより、収束演算を伴わない状態推定問題として解くことで、全体の確率分布を推定することが可能となるため、局所最適解を出力する頻度を小さくすることができる。
【図面の簡単な説明】
【０００８】
【図１】本発明に係る実施の形態１の走行計画生成装置を搭載した車両の概略構成を示す図である。
【図２】本発明に係る実施の形態１で用いる座標系を模式的に表した図である。
【図３】本発明に係る実施の形態１の走行計画生成装置が適用された自動運転システムの機能ブロック図である。
【図４】制約生成演算のフローを示したフローチャートである。
【図５】計画生成演算のフローを示したフローチャートである。
【図６】観測変数を模式的に示した図である。
【図７】計画生成演算結果を模式的に示した図である。
【図８】計画生成演算結果を模式的に示した図である。
【図９】本発明に係る実施の形態１の走行計画生成装置が適用された自動運転システムの機能ブロック図である。
【図１０】本発明に係る実施の形態１および２の走行計画生成装置を実現するハードウェア構成を示す図である。
【図１１】本発明の実施の形態１および２の走行計画生成装置を実現するハードウェア構成を示す図である。
【発明を実施するための形態】
【０００９】
＜実施の形態１＞
図１は、実施の形態１の走行計画生成装置を搭載した車両１の概略構成の一例を示すシステム構成図である。図１に示すように車両１は、駆動システムとして、ステアリングホイール２、ステアリング軸３、操舵ユニット４、ＥＰＳモータ５、パワートレインユニット６およびブレーキユニット７を備えている。
【００１０】
また、センサシステムとして、前方カメラ１１、レーダセンサ１２およびＧＮＳＳ（Global Navigation Satellite System）センサ１３、ヨーレートセンサ１６、速度センサ１７、加速度センサ１８、操舵角センサ２０および操舵トルクセンサ２１を備えている。
【００１１】
これらの他に、ナビゲーション装置１４、Ｖ２Ｘ（Vehicle-to-Everything）受信機１５、車両制御ユニット３０と、ＥＰＳ(Electric Power Steering)コントローラ４０、パワートレインコントローラ４１およびブレーキコントローラ４２を備えている。
【００１２】
実施の形態１の走行計画装置は、車両制御ユニット３０の一部として実現される。なお、ここでは、実施の形態１の走行計画生成装置が車両の自動運転システムに適用される場合を例示しているが、実施の形態１および実施の形態２の走行計画生成装置は、自律走行ロボットなど、自律制御によって走行する車両以外の移動体にも適用可能である。
【００１３】
ドライバが車両１を運転するために設置されているステアリングホイール２は、ステアリング軸３に結合されている。ステアリング軸３には操舵ユニット４が連接されている。操舵ユニット４は、操舵輪としての前輪の２つのタイヤを回動自在に支持すると共に、車体フレームに転舵自在に支持されている。従って、ドライバのステアリングホイール２の操作によって発生したトルクは、ステアリング軸３を回転させ、操舵ユニット４によって前輪を左右方向へ転舵する。これによって、ドライバは車両１が前進および後進する際の車両の横移動量を操作することができる。なお、ステアリング軸３はＥＰＳモータ５によって回転させることも可能であり、ＥＰＳコントローラ４０でＥＰＳモータ５に流れる電流を制御することで、ドライバのステアリングホイール２の操作と独立して、前輪を自在に転舵させることができる。
【００１４】
車両制御ユニット３０は、ＡＤＡＳ−ＥＣＵ（Advanced Driving Assistance Systems-Electronic Control Unit）とも呼称され、マイクロプロセッサ等の集積回路であり、Ａ／Ｄ（Analog/Digital）変換回路、Ｄ／Ａ（Digital/Analog）変換回路、ＣＰＵ（Central Processing Unit）、ＲＯＭ（Read Only Memory）、ＲＡＭ（Random Access Memory）等を備えている。
【００１５】
車両制御ユニット３０には、前方カメラ１１、レーダセンサ１２、ＧＮＳＳセンサ１３、ナビゲーション装置１４、Ｖ２Ｘ受信機１５、操舵角を検出する操舵角センサ２０、操舵トルクを検出する操舵トルクセンサ２１、ヨーレートを検出するヨーレートセンサ１６、自車の速度を検出する速度センサ１７、自車の加速度を検出する加速度センサ１８、ＥＰＳコントローラ４０、パワートレインコントローラ４１およびブレーキコントローラ４２が接続されている。
【００１６】
車両制御ユニット３０は、接続されているセンサから入力された情報を、ＲＯＭに格納されたプログラムに従って処理し、ＥＰＳコントローラ４０に目標操舵角を送信し、パワートレインコントローラ４１に目標駆動力を送信し、ブレーキコントローラ４２に目標制動力を送信する。
【００１７】
前方カメラ１１は、車両前方の区画線が画像として検出できる位置に設置され、画像情報を基に、車線情報および障害物の位置などの自車両の前方環境を検出する。なお、本実施の形態では、車両１の前方環境を検出するカメラのみを例に挙げたが、後方および側方の環境を検出するカメラも設置しても良い。また、前方カメラ１１は、車両１が走行する路面の状態を推定するために使用することもできる。
【００１８】
レーダセンサ１２は、レーダを照射し、その反射波を検出することで、自車両と障害物との相対距離と相対速度を出力する。このレーダセンサとしては、ミリ波レーダ、ＬｉＤＡＲ（Light Detection and Ranging）、レーザーレンジファインダ、超音波レーダなど周知の方式の測距センサを用いることができる。
【００１９】
ＧＮＳＳセンサ１３は測位衛星からの電波をアンテナで受信し、測位演算することによって車両１の絶対位置、絶対方位を出力する。
【００２０】
ナビゲーション装置１４は、ドライバが設定した行き先に対する最適な走行ルートを演算する機能を有し、走行ルート上の道路情報を記憶している。道路情報は道路線形を表現する地図ノードデータであり、各地図ノードデータは各ノードでの絶対位置を示す緯度、経度および標高の情報、車線幅、カント角、傾斜角情報などが組み込まれている。
【００２１】
Ｖ２Ｘ受信機１５は他車両および路側機との通信によって情報を取得し、出力する機能を有している。取得する情報は、他車両および歩行者を含む車両１にとっての障害物の位置、速度などの障害物情報および路面の摩擦係数などの道路情報を含んでいる。
【００２２】
ＥＰＳコントローラ４０は、車両制御ユニット３０から送信された目標操舵角を実現するように、ＥＰＳモータ５を制御することで、車両１の軌道を制御する。
【００２３】
パワートレインコントローラ４１は、車両制御ユニット３０から送信された目標駆動力を実現するように、パワートレインユニット６を制御することで、車両１の加速を制御する。
【００２４】
なお、本実施の形態では、エンジンのみを駆動力源とする車両を例に挙げたが、電動モータのみを駆動力源とする車両、エンジンと電動モータの両方を駆動力源とする車両等に本実施の形態を適用しても良い。
【００２５】
ブレーキコントローラ４２は、車両制御ユニット３０から送信された目標制動力を実現するように、ブレーキユニット７を制御することで、車両１の減速を制御する。
【００２６】
図２は本実施の形態で用いる座標系を模式的に表した図である。すなわち図２のＸ軸、Ｙ軸は慣性座標系を表し、Ｘｃ、Ｙｃ、Θｃは慣性座標系での自車両の位置と方位を示す。図２のｘｂ、ｙｂは自車の重心を原点とし、自車前方にｘｂ軸、自車前方に向かって左手方向にｙｂ軸を取った自車座標系である。
【００２７】
図３は実施の形態１に係る走行計画生成装置が適用された自動運転システム１００の機能ブロック図である。自動運転システム１００は、車両制御ユニット３０を備えており、車両制御ユニット３０には、情報取得部３００、ＥＰＳコントローラ４０、パワートレインコントローラ４１およびブレーキコントローラ４２が接続される。
【００２８】
情報取得部３００は、車両１の情報および車両１の周辺環境の情報を取得する機能を有しており、車両情報取得部３０１、障害物情報取得部３０２および道路情報取得部３０３を有している。情報取得部３００は、情報取得装置と呼称することもできる。
【００２９】
車両情報取得部３０１は、車両１の情報である車両情報を取得する。車両情報には、車両１の状態を表す車両１の状態量が含まれる。ＧＮＳＳセンサ１３、ヨーレートセンサ１６、速度センサ１７、加速度センサ１８、操舵角センサ２０および操舵トルクセンサ２１が、車両情報取得部３０１に含まれる。
【００３０】
障害物情報取得部３０２は、車両１の周辺の障害物の情報である障害物情報を取得する。前方カメラ１１、レーダセンサ１２およびＶ２Ｘ受信機１５が、障害物情報取得部３０２に含まれる。
【００３１】
道路情報取得部３０３は、車両１が走行する道路の情報である道路情報を取得する。前方カメラ１１、ナビゲーション装置１４およびＶ２Ｘ受信機１５が、道路情報取得部３０３に含まれる。なお、障害物情報および道路情報が、車両１の周辺環境の情報となる。
【００３２】
車両制御ユニット３０は、環境状態推定部３１０、走行計画生成部３２０および車両制御部３３０有している。
【００３３】
環境状態推定部３１０は、情報取得部３００から得られる車両１の周辺環境の情報に基づき、情報取得部３００から直接的に得られない情報を推定する機能を有し、推定した情報を走行計画生成部３２０に出力する機能を有している。なお、環境状態推定部３１０は、必要に応じて車両１の情報も用いても良い。
【００３４】
環境状態推定部３１０は、路面推定部３１１および障害物予測部３１２を有している。路面推定部３１１は、例えば特開平３−２４９３４０号公報（路面摩擦係数推定装置）に開示されている手法を用いて車両１が走行する路面の摩擦係数を推定する機能を有している。
【００３５】
障害物予測部３１２は、例えば特開昭６１−１５５８１３号公報（移動体位置推定装置）に開示されている手法を用いて車両１の周辺の障害物の予測軌道を予測する機能を有している。環境状態推定部３１０は、環境状態推定装置と呼称することもできる。
【００３６】
走行計画生成部３２０は、自動運転システムで制御された車両１が追従するための目標軌道および目標車速を含む走行計画を演算する機能を有すると共に走行計画の情報を車両制御部３３０に出力する機能を有している。走行計画生成部３２０は走行計画生成装置と呼称することもできる。走行計画生成部３２０の詳細については後述する。
【００３７】
車両制御部３３０は、走行計画生成部３２０から得られた目標軌道情報と、情報取得部３００から車両情報として得られる車両１の状態量を用いて、ＥＰＳコントローラ４０に出力するための目標操舵角を演算し、出力する機能を有している。
【００３８】
また、車両制御部３３０は、走行計画生成部３２０から得られた目標車速情報と、情報取得部３００から得られる車両１の状態量を用いて、パワートレインコントローラ４１に転送するための目標駆動力と、ブレーキコントローラ４２に転送するための目標制動力を演算し、出力する機能を有している。車両制御部３３０は、車両制御装置と呼称することもできる。
【００３９】
以下、走行計画生成部３２０の詳細を説明する。走行計画生成部３２０は、制約生成部３２１と計画生成部３２２とを有している。
【００４０】
制約生成部３２１は、走行計画の生成に用いるために、車両１の状態量に関する制約を生成し、計画生成部３２２に出力する機能を有している。制約生成部３２１は、環境状態推定部３１０の路面推定部３１１で推定した路面の摩擦係数に基づき、車両１のタイヤが発生する水平面内の力であるタイヤ発生力が路面の摩擦円限界を超えないという拘束条件を課した上で、当該拘束条件を満たすために車両１の状態量が取り得る値の範囲を演算し、車両１の状態量に関する制約とする。
【００４１】
このような拘束条件を設定することで、車両１のスリップを防ぐことができる走行計画の生成が可能となる。
【００４２】
なお、自動運転システムが路面推定部３１１を備えない場合には、制約生成部３２１は、予め設定された仮想的な路面の摩擦係数を用いて、車両１の状態量に関する制約を求めることもできる。
【００４３】
制約とする車両１の状態量としては、例えば、車体の横滑り角、タイヤの横滑り角、舵角、舵角速度、車体の速度、車体の加速度およびヨーレートのうちの少なくとも１つ、およびこれらの組み合わせが挙げられ、これらの状態量が取り得る値の範囲として制約が規定される。これにより、車両１の状態量を考慮した走行計画を作成することができる。なお、本実施の形態では、舵角を例として取り上げる。
【００４４】
タイヤと路面との摩擦によって発生するコーナリングフォースは、タイヤの横滑り角が小さい場合、横滑り角の増加に伴って線形に増加する。しかし、横滑り角がある程度以上に大きくなり、コーナリングフォースが摩擦円限界に近づくと、その増加はゆるやかになり、最終的に摩擦円限界に達し、飽和する。コーナリングフォースが飽和するときのタイヤの横滑り角を、ここでは飽和横滑り角βstと呼称する。タイヤの横滑り角が飽和横滑り角βstに達するとき、コーナリングフォースが摩擦円限界に到達し、スリップが発生する。従ってタイヤの縦方向に発生する力が十分小さいと仮定すれば、タイヤ発生力が路面の摩擦円限界を超えないという拘束条件は、タイヤの横滑り角が飽和横滑り角βstを超えないという条件に置き換えることができる。
【００４５】
また、車両運動を等価二輪モデルの定常円旋回で近似すれば、タイヤの横滑り角は車速Ｖと舵角δと、その他の車両のパラメータによって表すことができる。これにより、タイヤの横滑り角に関する条件を、舵角に関する条件に置き換えることが可能となる。
【００４６】
以上のように、タイヤ発生力が摩擦円限界を超えないという条件から、計画生成部３２２での計画生成演算に利用可能な、舵角に関する制約を導くことが可能である。
【００４７】
図４は、制約対象を舵角とした場合の制約生成部３２１における制約生成演算のフローを示したフローチャートである。なお、舵角は、角度の絶対値で表されるものとする。
【００４８】
制約生成部３２１は、まず、ステップＳ１００において、路面推定部３１１から路面の摩擦係数μを取得する。
【００４９】
次に、ステップＳ１０１において、摩擦係数から飽和横滑り角βstを求める。飽和横滑り角βstは、予め計測しておいたタイヤの垂直荷重Ｗｖと、コーナリングスティフネスＫと、ステップＳ１００で取得した摩擦係数μを用いて、以下の数式（１）のように表される。
【００５０】
【数１】
0006594589.tif 000002
【００５１】
ここでコーナリングスティフネスとは、横滑り角が小さいときの、単位横滑り角あたりのコーナリングフォースのことを言う。
【００５２】
次に、ステップＳ１０２において、横滑り角上限値を決定する。ステップＳ１０１で求めたタイヤの飽和横滑り角βstは、タイヤの垂直荷重ＷｖおよびコーナリングスティフネスＫを定数として求めたものであるが、実際にこれらのパラメータは乗車人数および車両の縦方向（車長方向）の加減速、車両の経年数等の状況によって変化するため、正確なβstを求めることは難しい。従って横滑り角がβst未満であっても、これと近い値を取るとき、摩擦円限界を超え、スリップが発生する可能性が高い。そこで、予め設定しておいた安全率Ｓβを用いて、計画生成における横滑り角上限値βｍｘを以下の数式（２）のように定める。なお、安全率Ｓβは、１以下の正の値であり、通常は１未満の値とする。
【００５３】
【数２】
0006594589.tif 000003
【００５４】
次に、ステップＳ１０３において舵角上限値を求める。以下に、タイヤの横滑り角βtを横滑り角上限値βｍｘ以下とする、舵角上限値δｍｘの求め方を説明する。
【００５５】
等価二輪モデルでは、前後輪の横滑り角βｆおよびβｒは、それぞれ次以下の数式（３）および（４）で与えられる。
【００５６】
【数３】
0006594589.tif 000004
【００５７】
【数４】
0006594589.tif 000005
【００５８】
ここで、「β」は車体の横滑り角、「ｒ」はヨーレート、「Ｖ」は車速、「δ」は舵角、「ｌｆ」および「ｌｒ」は車両重心点および前後車輪間の距離である。さらに定常円旋回を仮定すると、車体の横滑り角およびヨーレートは、それぞれ次以下の数式（５）および（６）で与えられる。
【００５９】
【数５】
0006594589.tif 000006
【００６０】
【数６】
0006594589.tif 000007
【００６１】
ここで、「Ｋｆ」および「Ｋｒ」は、それぞれ前輪および後輪のコーナリングスティフネス、Ｌはホイールベースである。
【００６２】
この場合の前後輪の横滑り角は、それぞれ次以下の数式（７）および（８）で与えられる。
【００６３】
【数７】
0006594589.tif 000008
【００６４】
【数８】
0006594589.tif 000009
【００６５】
さらにオーバーステアでないと仮定すると、ｌｆＫｆ−ｌｒＫｒ≦０より｜βｒ｜≦｜βｆ｜となる。
【００６６】
従って車両モデルのタイヤ横滑り角を上限値βｍｘ以下に制限するには、以下の数式（９）を満たせば良いことになる。
【００６７】
【数９】
0006594589.tif 000010
【００６８】
数式（７）で示した前輪の横滑り角より、舵角δの絶対値は、以下の数式（１０）で表される。
【００６９】
【数１０】
0006594589.tif 000011
【００７０】
このため、舵角上限値δｍｘを以下の数式（１１）を満たすようにすれば、横滑り角に関する条件を満たす。
【００７１】
【数１１】
0006594589.tif 000012
【００７２】
数式（１１）から分かるように、横滑り角上限値βｍｘが小さくなるほど、舵角上限値δｍｘは小さくなる。従って、路面の摩擦係数が小さくなるほど、舵角上限値δｍｘは小さくなる。
【００７３】
以上の方法で求めた舵角上限値δｍｘを、制約として計画生成部３２２に出力し、制約生成演算を終了する。
【００７４】
なお、以上の説明では、制約対象を舵角とした場合について述べたが、制約対象を例えば加速度または減速度とした場合にも、路面の摩擦係数が小さくなるほど、加速度または減速度の上限値は小さくなる。
【００７５】
計画生成部３２２は、情報取得部３００から得られた情報に基づき、自動運転システムが追従するための目標軌道および目標車速を含む走行計画を、収束演算を伴わない状態推定演算により生成し、車両制御部３３０に出力する機能を有している。本実施の形態では、状態推定演算としてパーティクルフィルタを用いる。
【００７６】
図５は、計画生成部３２２における計画生成演算のフローを示したフローチャートである。
【００７７】
計画生成部３２２は、まず、ステップＳ１２０において、情報取得部３００から得た情報から車両近傍の車線情報を抽出する。情報取得部３００において車線情報を取得する方法としては、例えば、前方カメラ１１で撮影された画像に対してエッジ検出処理を行い、区画線を構成する線を近似的に求め、得られた近似線に基づいて、車線情報を求める方法が挙げられる。また、Ｖ２Ｘ受信機１５を用いてネットワークと接続し、車両位置を送信し、ネットワークを介して車両近傍の地図を受信する方法が挙げられるが、これらに限定されるものではなく、本実施の形態ではどのような方法を用いて車線情報を取得しても効果には影響しない。本実施の形態では、ナビゲーション装置１４とＧＮＳＳセンサ１３から得られる情報を組み合わせる方法を説明する。以下にその方法を説明する。
【００７８】
まず、ＧＮＳＳセンサ１３から車両１の位置と方位を取得する。取得した車両１の位置とナビゲーション装置１４が記憶している道路情報とをマッチングすることにより、車両１の近傍の地図ノードデータが得られる。この地図ノードデータをＮｇ点分、地図点群として抽出する。Ｎｇは２以上の整数である。計画生成部３２２は、抽出した地図点群を、車両の方位を用いて自車座標系に変換する。
【００７９】
次に、ステップＳ１２１において、車両の現在の状態に基づきＮｐ個のパーティクルを定義する。パーティクルとは、決められた複数の状態変数、例えば車両の位置、方位、速度、加速度、舵角、舵角速度、ヨーレートおよび車体の横滑り角等に対し、各状態変数の値（状態量）で構成されるデータ群のことを指し、各パーティクルはそれぞれに状態量を有している。
【００８０】
これらの状態変数を用いて状態推定演算を行うことで、適切な走行計画を作成することができる。
【００８１】
パーティクルフィルタとは、確率密度分布による時系列データの予測手法であり、逐次モンテカルロ法と呼称される場合もある。また、状態推定演算としてのパーティクルフィルタは、複数のパーティクルによって状態の確率密度分布を近似するものであり、例えば、ある状態量を持つパーティクルが多ければ、その状態の確率密度が高いことになる。
【００８２】
状態推定演算としてパーティクルフィルタを用いることで、全体の確率密度分布を推定することが可能となるため、局所最適解を出力する頻度を小さくすることができる。
【００８３】
Ｎｐ個のパーティクルは、それぞれ異なる状態量を有する。Ｎｐは２以上の整数である。本実施の形態では、パーティクルの状態量Ｐは、車両の二次元位置ｘｂ、ｙｂ、方位θｂ、車速Ｖ、舵角δ、加速度ａおよび舵角速度ｕを有するものとし、以下の数式（１２）で表されるものとする。
【００８４】
【数１２】
0006594589.tif 000013
【００８５】
ここで、二次元位置ｘｂ、ｙｂおよび方位θｂは、車両の現在位置における自車座標系で表すものとする。また、ｎ個目のパーティクルの状態量をＰｎと表記することとする。全てのパーティクルについて状態変数の初期値は同じ値とし、「ｘｂ」、「ｙｂ」および「θｂ」は０、「Ｖ」は現在の車速、「δ」は現在の舵角、「ａ」および「ｓ」は０とする。また、各パーティクルに対し重みＷを定義し、初期値は全パーティクルで等しく、重みＷは、以下の数式（１３）で表されるものとする。また、時刻Ｔを定義し、初期値０を設定する。
【００８６】
【数１３】
0006594589.tif 000014
【００８７】
次に、ステップＳ１２２において、計画生成部３２２は、各パーティクルに対して乱数を用い、制約に基づいて離散時間幅Ｔｄ後の状態を予測する。以下に、パーティクルの状態予測の方法を説明する。
【００８８】
パーティクルの状態予測は、システムモデルを用いて行うが、本実施の形態で用いるモデルを以下に説明する。システムモデルの状態変数はパーティクルの二次元位置ｘｂ、ｙｂ、方位θｂ、車速Ｖおよび舵角δとし、状態量Ｐｘは、以下の数式（１４）で表されるものとする。
【００８９】
【数１４】
0006594589.tif 000015
【００９０】
また、システムモデルへの入力値Ｐｕは、車両の加速度ａと舵角速度ｕとで構成され、以下の数式（１５）で表されるものとする。
【００９１】
【数１５】
0006594589.tif 000016
【００９２】
また、車体の横滑り角βは、以下の数式（１６）で表されるものとする。
【００９３】
【数１６】
0006594589.tif 000017
【００９４】
この場合、システムモデルは車両のホイールベースＬを用いて、微分方程式として以下の数式（１７）で表される。
【００９５】
【数１７】
0006594589.tif 000018
【００９６】
なお、上述したシステムモデルは、四輪を二輪に近似し、力学を考慮しない運動学モデルと言えるが、四輪を二輪に近似した動力学モデルである二輪モデル等、他の車両運動モデルを用いても良い。
【００９７】
システムモデルへの入力変数のうち、加速度ａは予め設定しておいた上限値ａｍｘと下限値ａｍｎについて、以下の数式（１８）を満たす値を、パーティクルごとに乱数を用いて決定する。
【００９８】
【数１８】
0006594589.tif 000019
【００９９】
また、システムモデルへの入力変数のうち、舵角速度ｕに関しては、予め設定しておいた上限値ｕｍｘ（＞０）について、以下の数式（１９）を満たすことを舵角速度ｕの第１の拘束条件とする。
【０１００】
【数１９】
0006594589.tif 000020
【０１０１】
また、車両１の状態量に関する制約として制約生成部３２１から得られる舵角の上限値δｍｘ（＞０）について、離散時間幅Ｔｄ後の舵角δ’が以下の数式（２０）を満たすことを舵角速度ｕの第２の拘束条件とする。
【０１０２】
【数２０】
0006594589.tif 000021
【０１０３】
離散時間幅Ｔｄ後の舵角δ’は、以下の数式（２１）で表される。
【０１０４】
【数２１】
0006594589.tif 000022
【０１０５】
このため、第２の拘束条件は、以下の数式（２２）で表される。
【０１０６】
【数２２】
0006594589.tif 000023
【０１０７】
システムモデルへの入力値Ｐｕのうち舵角速度ｕは、第１の拘束条件および第２の拘束条件を満たす値を、パーティクルごとに乱数を用いて決定する。
【０１０８】
以上のように、制約である舵角上限値δｍｘに基づいて決定した入力値Ｐｕを用いて、上述したシステムモデルにより離散時間幅Ｔｄ後の状態量Ｐｘ’を予測する。これにより、制約を考慮したパーティクルの状態予測が可能となる。
【０１０９】
パーティクルの状態量は予測状態量Ｐｘ’と入力値Ｐｕを用いて更新し、以下の数式（２３）で表されるものとする。
【０１１０】
【数２３】
0006594589.tif 000024
【０１１１】
なお、パーティクルの状態量、予測状態量および入力値は全て縦ベクトルであり、簡略化のため上記数式（２３）のように転置を用いて表している。
【０１１２】
また、時刻Ｔに離散時間幅Ｔｄを足した値を更新後の時刻とする。この際、障害物予測部３１２から得られる周辺障害物の予測軌道を用いて、各パーティクルにおいて車両が存在する領域に、同時刻に障害物が存在するかを判断し、障害物が存在する場合はそのパーティクルの重みＷを０とする。これにより、車両１と障害物との接触を回避した走行計画を生成することが可能となる。
【０１１３】
次に、ステップＳ１２３において、各パーティクルの更新後の状態量から観測値を求める。観測変数は走行計画の目標に基づき定義する。走行計画の目標は、車両１の走行状況またはユーザの設定により、計画生成部３２２の外部から与えられる。本実施の形態では、目標車速の維持と、車線中央の走行と、周辺の障害物からの安全な距離の保持とを目標とする。これらの目標に基づき、車速Ｖ、車両から目標車線への横偏差ｙ０、危険領域への障害物の侵入距離ｄを観測変数として観測値Ｐｙを以下の数式（２４）で表すものとする。
【０１１４】
【数２４】
0006594589.tif 000025
【０１１５】
なお、観測変数としては、車速Ｖ、車両から目標車線への横偏差ｙ０および危険領域への障害物の侵入距離ｄの少なくとも１つを含めば良い。これにより、車両１と障害物との接触を回避した走行計画を生成することが可能となる。
【０１１６】
図６は、上述した各観測変数を模式的に示した図である。図６において、自車両ＯＶにはハッチングを付し、障害物となる他車両ＡＶにはハッチングを付していない。目標車線ＯＪＬへの横偏差ｙ０は、パーティクルの位置と方位における自車座標系で車線を表した場合に、目標車線ＯＪＬとｙｂ軸とが交差する点のｙｂの値である。ただし、目標車線の位置は、ステップＳ１２０で得られた車線情報を用いる。
【０１１７】
また、危険領域ＤＲへの侵入距離ｄは、パーティクルの位置に基づき設定した危険領域ＤＲに、各障害物が侵入した縦距離、すなわちｘｂ軸に沿った方向の距離のうち最大値とする。ここで危険領域ＤＲとは、パーティクルの向き、すなわち方位θｂに長辺の向きを傾けた長方形の領域で示される。
【０１１８】
危険領域ＤＲは、パーティクルから前方に長さＬｘｆ、後方に長さＬｘｒ、左右に長さＬｙだけの幅を持つものとして規定され、自車両ＯＶの周囲に設定される。
【０１１９】
ここで、長さＬｘｆはパーティクルの車速Ｖと予め設定しておいた安全見込時間Ｔｓを用いて、以下の数式（２５）で表すものとする。
【０１２０】
【数２５】
0006594589.tif 000026
【０１２１】
また、長さＬｘｒおよびＬｙは、それぞれ、予め設定しておいたパラメータＴｓｒおよびＴｓｙを用いて、以下の数式（２６）および（２７）で表すものとする。
【０１２２】
【数２６】
0006594589.tif 000027
【０１２３】
【数２７】
0006594589.tif 000028
【０１２４】
ここで、図５のフローチャートの説明に戻り、次に、ステップＳ１２４において、各パーティクルの観測値Ｐｙと、理想観測値Ｐｙｉとの差から、各パーティクルの重みＷを更新する。ここで、理想観測値Ｐｙｉとは、仮想的に設定した理想状態にある車両１に対する観測値であり、走行計画の目標から決定される。車両１が走行計画の目標を満たしている場合に、車両１は理想状態となる。本実施の形態において理想観測値Ｐｙｉは目標車速Ｖｎｏｍと、目標横偏差ｙ０ｎｏｍと、目標侵入領域ｄｎｏｍで構成され、以下の数式（２８）で表される。
【０１２５】
【数２８】
0006594589.tif 000029
【０１２６】
各パーティクルの更新前の重みＷをＷｐとして再定義する。重みＷは、更新前の重みＷｐと尤度αとに比例し、全パーティクルの重みの積算値が１となるよう更新する。
【０１２７】
ｎ個目のパーティクルの重みＷｎは、以下の数式（２９）で表される。
【０１２８】
【数２９】
0006594589.tif 000030
【０１２９】
ここで、尤度αは、予め設定しておいた、パーティクルの状態量Ｐｘに関する共分散行列Ｑと観測値Ｐｙに関する共分散行列Ｒを用いて、以下の数式（３０）で求められる。
【０１３０】
【数３０】
0006594589.tif 000031
【０１３１】
ここで、行列Πは以下の数式（３１）で表される。
【０１３２】
【数３１】
0006594589.tif 000032
【０１３３】
ただし、ｎ個目のパーティクルにおける測定行列Ｈの値Ｈｎは、Ｐｘ＝Ｐｘｎとした場合、測定関数ｈを状態量Ｐｘで微分した値とし、以下の数式（３２）で表される。
【０１３４】
【数３２】
0006594589.tif 000033
【０１３５】
また、測定関数ｈは状態量Ｐｘから観測値Ｐｙを求める関数であり、以下の数式（３３）で表される。
【０１３６】
【数３３】
0006594589.tif 000034
【０１３７】
次に、ステップＳ１２５において、各パーティクルの重みに基づき、パーティクルのリサンプリングを行う。ただし、パーティクル数の大幅な減少を防ぐため、有効パーティクル数Ｎｅｆｆが閾値Ｎｔｈ以上となる場合にのみリサンプリングを行い、それ以外の場合はこのステップでは何も行わない。
【０１３８】
ここで、有効パーティクル数Ｎｅｆｆは、以下の数式（３４）で表される。
【０１３９】
【数３４】
0006594589.tif 000035
【０１４０】
リサンプリングでは、通常のパーティクルフィルタ同様、経験分布関数から等間隔にサンプリングを行う。リサンプリングを行った場合、以下の数式（３５）に基づいて、各パーティクルの重みは同等として重みのリセットを行う。
【０１４１】
【数３５】
0006594589.tif 000036
【０１４２】
次に、ステップＳ１２６において、パーティクルの位置と車速について加重平均値を演算し、走行計画として位置データおよび車速データを計画生成部３２２内に記憶する。
【０１４３】
次に、ステップＳ１２７において、時刻Ｔが計画生成の計画対象期間である計画ホライズン（planning horizon）Ｔｈｒに達したかどうかを判断する。時刻Ｔが計画ホライズンＴｈｒ未満と判断される場合、ステップＳ１２２に戻る。時刻Ｔが計画ホライズンＴｈｒ以上であると判断される場合、走行計画として記憶しておいた位置データを目標軌道、車速データを目標車速として出力し、計画生成の演算を終了する。
【０１４４】
図７は、以上説明した計画生成演算結果を模式的に示した図である。ここでは簡単のためパーティクルの個数Ｎｐを４個として表しているが、実際には最低でも５０個程度とする。
【０１４５】
図７において、全パーティクルの初期値７００は、車両の現在の状態に基づいて設定されるので、同じ値として出発する。これらのパーティクルに対して数式（１２）〜（２２）を用いて説明した処理を経てそれぞれの状態遷移を予測し、数式（２３）を用いてパーティクルの状態量を更新してパーティクル７０１〜７０４の更新値を得る。これらは制約として得られる舵角の上限値と、予め設定しておいた入力変数に関する上下限値に基づき、図７中の破線で囲まれた領域７１０で示すような分散を持つ。
【０１４６】
そして、更新された各角パーティクルに対して数式（２４）〜（３５）を用いて説明した処理を経て、目標車線７２０および目標車速、障害物との位置関係に応じて各パーティクルの重みが求められ、重みに応じてリサンプリングが行われる。ここでは仮に、パーティクル７０１の重みが大きく、パーティクル７０４の重みが小さいため、パーティクル７０１が増加し、７０４が消滅したとする。その後、パーティクル７０１からパーティクル７０３の状態の加重平均値７０５が、この時刻における走行計画として記憶される。同様の処理を各パーティクルに対して繰り返し、走行計画７０５〜７０８を得る。
【０１４７】
図８は、図７の場合に比べ、制約である舵角の上限値が小さくなった場合の、計画生成演算を模式的に示した図である。すなわち、図８は、図７の場合と比較して、路面の摩擦係数が小さくなった場合の計画生成演算結果を模式的に示した図である。
【０１４８】
図８では、全パーティクルの初期値８００が車両の現在の状態に基づいて設定され、まず、状態遷移の予測によりパーティクルの更新値８０１〜８０４を得る。この際、制約として得られる舵角の上限値が小さいことから、図８中の破線で囲まれた領域８１０のように、パーティクルの横方向への分散が小さくなる。その結果、得られる走行計画８０５〜８０８は、図７に示した走行計画７０５〜７０８に比べ、同じ時間で横方向に移動する量が小さくなる。
【０１４９】
以上説明した走行計画生成部３２０の構成によれば、パーティクルフィルタに代表される状態推定演算を用いることで、収束演算を用いずに車両と障害物との接触を回避した走行計画を生成することが可能となる。これにより、局所最適解を出力してしまうことなく、安定した計算時間で走行計画が得られるため、高い安全性を持つ自動運転システムの実現が可能となる。
【０１５０】
ここで、収束演算とは、評価値が予め決められた条件を満たすまで繰り返される演算であり、上限回数は設定できるが、繰り返される回数が条件によって変化するので、演算時間が安定しない。収束演算を行う場合は、評価関数および制約条件によっては局所最適解を出力する可能性があるが、局所最適解は、望ましい解ではない場合が多い。
【０１５１】
一方、収束演算を伴わない演算とは、予め決められた回数だけ繰り返される演算であるので、演算時間が安定する。
【０１５２】
収束演算を伴わない状態推定演算を用いることにより、収束演算を伴わない状態推定問題として解くことで、全体の確率分布を推定することが可能となるため、局所最適解を出力する頻度を小さくすることができる。
【０１５３】
なお、本実施の形態においては、状態推定演算としてパーティクルフィルタを例示して説明したが、状態推定演算としてはカルマンフィルタ、拡張カルマンフィルタ等、他の演算を用いることもできる。例えば、システムモデルおよび測定関数ｈが線形と見做せる場合には、カルマンフィルタを使用することもできる。また、システムモデルおよび測定関数ｈが線形と見做せない場合には、拡張カルマンフィルタを用いることもできる。パーティクルフィルタを用いれば、カルマンフィルタまたは拡張カルマンフィルタを用いる場合と比較して、より高精度な状態推定が可能となる。パーティクルフィルタ、カルマンフィルタおよび拡張カルマンフィルタは、総称としてベイズフィルタと呼ばれることもある。
【０１５４】
＜実施の形態２＞
以上説明した実施の形態１の制約生成演算は、図３に示した環境状態推定部３１０の路面推定部３１１において推定された路面摩擦係数を用いて行われるが、情報取得部３００の道路情報取得部３０３において、例えばＶ２Ｘ受信機１５（図１）のような、路面の摩擦係数を取得する機能を有するものを用い、走行計画生成部３２０の制約生成部３２１は、道路情報取得部３０３で車両外部から取得された路面摩擦係数に基づいて制約を生成しても良い。
【０１５５】
図９は実施の形態２に係る走行計画生成装置が適用された自動運転システム２００の機能ブロック図である。なお、図９においては、図３を用いて説明した自動運転システム１００と同一の構成については同一の符号を付し、重複する説明は省略する。
【０１５６】
図９に示す自動運転システム２００の車両制御ユニット３１においては、環境状態推定部３１０には路面推定部３１１を有さず、道路情報取得部３０３で車両外部から取得された路面摩擦係数に基づいて制約を生成する構成となっている。
【０１５７】
この構成によれば、路面推定部３１１が不要となり計算負荷が減ることに加え、路面の摩擦係数を前もって取得可能であり、より確実にスリップを防ぐことができる。
【０１５８】
ここで、Ｖ２Ｘ受信機１５としては、例えば、路肩インフラとして、路面の画像を取得するカメラと、カメラから得られた情報に基づいて路面状態を推定するインフラと通信するＶ２Ｉ（Vehicle to Infrastructure）受信機を用いることで路面の摩擦係数の情報を取得することができる。路面の画像を取得するカメラと、カメラから得られた情報に基づいて路面状態を推定する方法としては、例えば特開２００２−１２７８８２号公報に開示される、路面が雪路であるか、アスファルトウェット路面であるか、アスファルトドライ路面であるかを判定し、判定した路面状況に基づいて路面摩擦係数を推定する方法が挙げられ、このような方法で路肩インフラが路面状態を推定して送信することで、Ｖ２Ｉ受信機を介して路面摩擦係数を取得できる。
【０１５９】
また、Ｖ２Ｘ受信機１５として、車車間通信を行うＶ２Ｖ（Vehicle to Infrastructure）受信機により、先行車両が推定した路面摩擦係数を取得する方法も考えられる。
【０１６０】
なお、以上説明した実施の形態１および２に係る走行計画生成装置である走行計画生成部３２０の各構成要素は、コンピュータを用いて構成することができ、コンピュータがプログラムを実行することで実現される。すなわち、走行計画生成部３２０は、例えば図１０に示す処理回路５０により実現される。処理回路５０には、ＣＰＵ、ＤＳＰ（Digital Signal Processor）などのプロセッサが適用され、記憶装置に格納されるプログラムを実行することで各部の機能が実現される。
【０１６１】
なお、処理回路５０には、専用のハードウェアが適用されても良い。処理回路５０が専用のハードウェアである場合、処理回路５０は、例えば、単一回路、複合回路、プログラム化したプロセッサ、並列プログラム化したプロセッサ、ＡＳＩＣ（Application Specific Integrated Circuit）、ＦＰＧＡ（Field-Programmable Gate Array）、またはこれらを組み合わせたもの等が該当する。
【０１６２】
走行計画生成部３２０は、構成要素の各々の機能が個別の処理回路で実現されても良いし、それらの機能がまとめて１つの処理回路で実現されても良い。
【０１６３】
また、図１１には、処理回路５０がプロセッサを用いて構成されている場合におけるハードウェア構成を示している。この場合、走行計画生成部３２０の各部の機能は、ソフトウェア等（ソフトウェア、ファームウェア、またはソフトウェアとファームウェア）との組み合わせにより実現される。ソフトウェア等はプログラムとして記述され、メモリ５２に格納される。処理回路５０として機能するプロセッサ５１は、メモリ５２（記憶装置）に記憶されたプログラムを読み出して実行することにより、各部の機能を実現する。すなわち、このプログラムは、走行計画生成部３２０の構成要素の動作の手順および方法をコンピュータに実行させるものであると言える。
【０１６４】
ここで、メモリ５２は、例えば、ＲＡＭ、ＲＯＭ、フラッシュメモリー、ＥＰＲＯＭ（Erasable Programmable Read Only Memory）、ＥＥＰＲＯＭ（Electrically Erasable Programmable Read Only Memory）等の、不揮発性または揮発性の半導体メモリ、ＨＤＤ（Hard Disk Drive）、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ミニディスク、ＤＶＤ（Digital Versatile Disc）およびそのドライブ装置等、または、今後使用されるあらゆる記憶媒体であっても良い。
【０１６５】
以上、走行計画生成部３２０の各構成要素の機能が、ハードウェアおよびソフトウェア等のいずれか一方で実現される構成について説明した。しかしこれに限ったものではなく、走行計画生成部３２０の一部の構成要素を専用のハードウェアで実現し、別の一部の構成要素をソフトウェア等で実現する構成であっても良い。例えば、一部の構成要素については専用のハードウェアとしての処理回路５０でその機能を実現し、他の一部の構成要素についてはプロセッサ５１としての処理回路５０がメモリ５２に格納されたプログラムを読み出して実行することによってその機能を実現することが可能である。
【０１６６】
以上のように、走行計画生成部３２０は、ハードウェア、ソフトウェア等、またはこれらの組み合わせによって、上述の各機能を実現することができる。
【０１６７】
この発明は詳細に説明されたが、上記した説明は、全ての局面において、例示であって、この発明がそれに限定されるものではない。例示されていない無数の変形例が、この発明の範囲から外れることなく想定され得るものと解される。
【０１６８】
なお、本発明は、その発明の範囲内において、各実施の形態を自由に組み合わせたり、各実施の形態を適宜、変形、省略することが可能である。
【要約】
車両の自動運転システムに用いられる走行計画生成装置であって、車両の状態量に関する制約を生成する制約生成部と、収束演算を伴わない状態推定演算により、制約を満たすように車両の目標軌道および目標車速を走行計画として生成する計画生成部と、を有している。【図１】
0006594589.tif 000037
【図２】
0006594589.tif 000038
【図３】
0006594589.tif 000039
【図４】
0006594589.tif 000040
【図５】
0006594589.tif 000041
【図６】
0006594589.tif 000042
【図７】
0006594589.tif 000043
【図８】
0006594589.tif 000044
【図９】
0006594589.tif 000045
【図１０】
0006594589.tif 000046
【図１１】
0006594589.tif 000047