知財求人 - 知財ポータルサイト「IP Force」
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B1)
(11)【特許番号】
(24)【登録日】2024-12-26
(45)【発行日】2025-01-10
(54)【発明の名称】移動体制御装置
(51)【国際特許分類】
B60W 50/06 20060101AFI20241227BHJP
B60W 30/095 20120101ALI20241227BHJP
B60W 40/02 20060101ALI20241227BHJP
G08G 1/16 20060101ALI20241227BHJP
G05D 1/43 20240101ALI20241227BHJP
G05D 1/46 20240101ALI20241227BHJP
G05B 13/04 20060101ALI20241227BHJP
【ＦＩ】
B60W50/06
B60W30/095
B60W40/02
G08G1/16 C
G05D1/43
G05D1/46
G05B13/04
【請求項の数】
7
(21)【出願番号】P 2024521319
(86)(22)【出願日】2023-11-16
(86)【国際出願番号】 JP2023041239
【審査請求日】2024-04-09
【早期審査対象出願】
(73)【特許権者】
【識別番号】000006013
【氏名又は名称】三菱電機株式会社
(74)【代理人】
【識別番号】100088672
【弁理士】
【氏名又は名称】吉竹 英俊
(74)【代理人】
【識別番号】100088845
【弁理士】
【氏名又は名称】有田 貴弘
(72)【発明者】
【氏名】亀岡 翔太
(72)【発明者】
【氏名】細江 陽平
【審査官】藤村 泰智
(56)【参考文献】
【文献】特開２０１８－１８１１６６（ＪＰ，Ａ）
【文献】中国特許第１１０２４４７５９（ＣＮ，Ｂ）
【文献】中国特許第１１３１２８８２６（ＣＮ，Ｂ）
【文献】特開２００９－１８４４９７（ＪＰ，Ａ）
【文献】特開２０２３－１５７５２８（ＪＰ，Ａ）
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｂ６０Ｗ ３０／００ ～ ５０／１６
Ｇ０８Ｇ １／００ ～ １／１６
Ｇ０５Ｄ １／４０ ～ １／４９５
Ｇ０５Ｂ １３／０４
(57)【特許請求の範囲】
【請求項1】
移動体を制御するための移動体制御量を演算する制御演算部を備え、前記制御演算部は、
確率分布をもつ確率的なパラメータに依存する前記移動体の動特性をモデル化した状態方程式を制約条件とし、評価関数に基づく確率計画問題を逐次解くことによって最適系列を演算する確率系最適化演算部と、
前記最適系列を前記移動体制御量に変換する制御決定部と、
を有し、
前記確率系最適化演算部は、
確率的なパラメータの確率過程モデルに基づいて、確率的なパラメータを含む確率的な評価関数を確定的な評価関数に変換する確率系評価関数変換部と、
確定的な最適化問題を解く確定系最適化演算部と、
を有する、移動体制御装置。
【請求項2】
前記確率系評価関数変換部は、前記確率過程モデルに基づいて定義される写像および漸化式を用いて、前記確率的な評価関数を前記確定的な評価関数に変換する、請求項１に記載の移動体制御装置。
【請求項3】
前記移動体周辺の障害物に関する障害物情報から一定期間未来における前記障害物の移動を予測し、障害物予測情報として出力する障害物移動予測部をさらに備え、前記確率系最適化演算部は、参照経路および前記障害物予測情報に基づいて、前記確定系最適化演算部で制約条件として扱われる確定系制約条件を生成し、
前記確定系最適化演算部は、前記確定系評価関数を前記確定系制約条件の下で最適化する、請求項１または２に記載の移動体制御装置。
【請求項4】
前記制御演算部は、前記移動体の前後方向の制御を行うための前後方向系列を出力する前後方向制御部と、
前記移動体の横方向の制御を行うための横方向最適系列を出力する横方向制御部と、
を有し、
前記横方向制御部は、前記確率系最適化演算部を含み、
前記確率系最適化演算部は、前記前後方向制御部が出力した前記前後方向系列を確定的なパラメータとして参照して前記横方向最適系列を演算する、請求項１または２に記載の移動体制御装置。
【請求項5】
前記前後方向制御部は、前記移動体の前後方向の動特性に基づいて前後方向の最適化系列を演算する、請求項４に記載の移動体制御装置。
【請求項6】
最適化問題の計算時間のばらつきを確率分布として表現し、ランダムなサンプリング時間として前記状態方程式に組み込む、請求項１または２に記載の移動体制御装置。
【請求項7】
前記確率系評価関数変換部は、前記確率過程モデルとして独立同分布または隠れマルコフモデルを用いる、請求項１または２に記載の移動体制御装置。
【発明の詳細な説明】
【技術分野】
【0001】
本開示は、移動体を制御する移動体制御装置に関する。
【背景技術】
【0002】
移動体を制御する場合、移動体が走行する際の参照となる経路に追従させるために、移動体の位置および姿勢に関する情報を利用することがある。しかし、移動体の位置および姿勢に関する情報は、ノイズ等の影響を受けて確率的になることがある。
【0003】
従来、特許文献１に示すような技術が開示されている。特許文献１では、対象の移動体の存在確率を示す移動分布と、周辺物体の存在確率を示す周辺物体分布とに基づいて潜在危険度を算出し、潜在危険度マップを生成する。また、潜在危険度マップに基づいて、相対的に潜在危険度が低い経路を走行経路として選択する。
【先行技術文献】
【特許文献】
【0004】
【文献】特許第６９５６９３２号公報
【非特許文献】
【0005】
【文献】Yohei Hosoe and Tomomichi Hagiwara, “Equivalent Stability Notions, Lyapunov Inequality, and Its Application in Discrete-Time Linear Systems With Stochastic Dynamics Determined by an i.i.d. Process”, IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 64, NO. 11, NOVEMBER 2019.
【発明の概要】
【発明が解決しようとする課題】
【0006】
特許文献１では、潜在危険度マップにおける潜在危険度が低い経路を探索し、探索した経路を走行経路として選択している。しかし、特許文献１に開示されている手法では、潜在危険度のみを考慮しているため、例えば車線中央などの参照経路への追従性を考慮することができない。
【0007】
本開示は、このような問題を解決するためになされたものであり、移動体の位置および姿勢が確率的な場合においても、参照経路への追従性を高くすることが可能な移動体制御装置を提供することを目的とする。
【課題を解決するための手段】
【0008】
上記の課題を解決するために、本開示による移動体制御装置は、移動体を制御するための移動体制御量を演算する制御演算部を備え、制御演算部は、確率分布をもつ確率的なパラメータに依存する移動体の動特性をモデル化した状態方程式を制約条件とし、評価関数に基づく確率計画問題を逐次解くことによって最適系列を演算する確率系最適化演算部と、最適系列を移動体制御量に変換する制御決定部とを有し、確率系最適化演算部は、確率的なパラメータの確率過程モデルに基づいて、確率的なパラメータを含む確率的な評価関数を確定的な評価関数に変換する確率系評価関数変換部と、確定的な最適化問題を解く確定系最適化演算部とを有する。
【発明の効果】
【0009】
本開示によれば、移動体の位置および姿勢が確率的な場合においても、参照経路への追従性を高くすることが可能となる。
【0010】
本開示の目的、特徴、態様、および利点は、以下の詳細な説明と添付図面とによって、より明白となる。
【図面の簡単な説明】
【0011】
【図1】実施の形態１に係る移動体制御装置の構成の一例を示すブロック図である。
【図2】実施の形態１に係る移動体の構成の一例を示すブロック図である。
【図3】実施の形態１に係る制御演算部の構成の一例を示すブロック図である。
【図4】実施の形態１に係る移動体の構成の一例を示す図である。
【図5】実施の形態１に係る移動体の状態方程式を説明するための図である。
【図6】実施の形態１に係るＧＮＳＳで計測される位置の確率過程モデルの一例を示す図である。
【図7】実施の形態２に係る移動体制御装置の構成の一例を示すブロック図である。
【図8】実施の形態２に係る確率系最適化演算部の構成の一例を示すブロック図である。
【図9】実施の形態２に係る制約を説明するための図である。
【図10】実施の形態３に係る制御演算部の構成の一例を示すブロック図である。
【図11】実施の形態３に係る速度計画部の出力を説明するための図である。
【図12】実施の形態３に係る制御演算部の構成の一例を示すブロック図である。
【図13】実施の形態１～３に係る移動体制御装置のハードウェア構成の一例を示すブロック図である。
【図14】実施の形態１～３に係る移動体制御装置のハードウェア構成の一例を示すブロック図である。
【発明を実施するための形態】
【0012】
＜実施の形態１＞
＜移動体制御装置１の全体構成＞
図１は、実施の形態１に係る移動体制御装置１の構成の一例を示すブロック図である。図１に示すように、移動体制御装置１は、内界センサ５、位置姿勢推定装置６、地図データベース７、および移動体８に接続されている。
＜移動体制御装置１の全体構成＞
図１は、実施の形態１に係る移動体制御装置１の構成の一例を示すブロック図である。図１に示すように、移動体制御装置１は、内界センサ５、位置姿勢推定装置６、地図データベース７、および移動体８に接続されている。
【0013】
位置姿勢推定装置６は、移動体８の周辺環境に対する移動体８の位置および姿勢を推定し、位置姿勢情報として出力する機能を有する。位置姿勢推定装置６には、例えば、カメラ、ＬｉＤＡＲ（Light Detection and Ranging）、またはレーダなどの外界センサ（図示せず）が接続されている。
【0014】
カメラ（例えば、図４のカメラ８０）は、移動体８の前方、側方、および後方を撮影可能な位置に設置されており、撮影した画像から、例えば移動体８の周辺環境の特徴量を抽出するために使用される。
【0015】
ＬｉＤＡＲは、レーザを周辺に照射し、レーザが周辺の物体に反射して戻ってくるまでの時間差を検出することによって、移動体８の周辺環境の特徴量を抽出する。
【0016】
レーダは、周囲にレーダ照射を行い、その反射波を検出することによって、移動体８の周辺に存在する物体の相対距離および相対速度を測定し、これらを特徴量として抽出する。
【0017】
位置姿勢推定装置６は、図１に示す地図データベース７が設置されている場合は、外界センサの値と地図データベース７とを照合することによって、移動体８の位置および姿勢を推定する。なお、図１に示す地図データベース７が設定されていない場合は、移動体８自身が環境を移動して地図を生成しながら位置推定を行うＳＬＡＭ（Simultaneous Localization and Mapping）などの処理が位置姿勢推定装置６の内部で行われ、移動体８の位置および姿勢が推定される。
【0018】
移動体８が屋外を移動する場合は、位置姿勢推定装置６としてＧＮＳＳ（Global Navigation Satellite System）センサなどのセンサが、移動体８の位置および姿勢を推定するために用いられる。
【0019】
内界センサ５は、移動体８に設置されており、移動体８の内界情報を出力する。内界情報とは、移動体８の加速度、速度、角加速度、角速度などの慣性に関する情報に加え、移動体８の内部の状態を把握するために必要な情報である。例えば移動体８が自動車である場合、内界センサ５は、例えば車速センサ、ＩＭＵ（Inertial Measurement Unit）センサ、操舵角センサ、および操舵トルクセンサなどである。
【0020】
地図データベース７は、移動体８の周囲の地図データを格納しており、地図情報として出力する。図１では、参照経路生成部３が地図データベース７に接続されているが、参照経路生成部３に限らず、移動体制御装置１内の各構成要素が地図データベース７にアクセスすることができる。地図データベース７には、移動体８の位置および姿勢を把握するための移動体環境の特徴量等が含まれている。移動体８が自動車である場合は、道路の中央座標情報、停止線の情報、白線の情報、および走行可能領域などの走行に関するデータが地図データベース７に含まれることが多い。
【0021】
地図データベース７は、位置姿勢推定装置６および参照経路生成部３においてその情報が必要ないのであれば設けなくてもよい。例えば、移動体８が両側区画線の中央を走行するような場合は、カメラで両側区画線を検出し、その中央を参照経路とすることがある。この場合、区画線の検出自体が位置および姿勢の推定と同等であるため、地図データベース７は不要となる。
【0022】
移動体制御装置１は、状態量推定部２、参照経路生成部３、および制御演算部４を備えている。移動体制御装置１は、内界センサ５から取得した内界情報、位置姿勢推定装置６から取得した位置姿勢情報、および地図データベース７から取得した地図情報に基づいて、移動体８を制御するための移動体制御量を出力する機能を有する。
【0023】
移動体制御量は、移動体８を制御するための信号であり、目標経路、目標軌道、および移動体８のアクチュエータへの指令値などを含む。
【0024】
なお、本開示では、移動体８に追従させる位置座標を含む経路を「目標経路」とし、経路の各点に到達時間の指示など位置座標以外の指示が含まれている場合は「目標軌道」とする。目標軌道には、目標速度（到達時間の指示と同等）が含まれている場合もある。なお、目標軌道は、目標速度または目標位置に限らず、移動体８の状態量であれば何でもよい。
【0025】
状態量推定部２は、制御演算部４で使用する、内界センサ５等で直接取得されない状態量を推定する機能を有する。状態量推定部２は、内界情報、位置姿勢情報、および移動体８の動特性を表す状態方程式に基づいて、オブザーバ、カルマンフィルタ、またはパーティクルフィルタなどの広く普及している手法を用いて移動体８の状態量を推定する。なお、制御演算部４で使用する状態量が全てセンサ等で検出される場合、状態量推定部２は不要となる。
【0026】
参照経路生成部３は、地図情報および位置姿勢情報に基づいて、参照となる経路として参照経路を生成する。移動体８が自動車である場合、参照経路は、車線の中央を表す経路などである。ダイクストラ法またはＡ＊法などの一般的な方法によって、移動体８の初期の位置および姿勢から、目標の位置および姿勢に至るまでの経路を計算し、計算した経路を参照経路とすることもできる。
【0027】
制御演算部４については、図３を用いて後に説明する。
【0028】
＜移動体８＞
移動体とは、自身が持つ車輪、プロペラ、脚、またはクローラなどの移動機構によって、基準座標系に対する自身のボディ座標系の位置および姿勢を変化せることができる物体である。移動体としては、例えば、自動車、飛行体、ドローン、脚ロボット、探査機、および農作機などが挙げられる。これら以外にも、移動できる物体は移動体として扱うことができる。
移動体とは、自身が持つ車輪、プロペラ、脚、またはクローラなどの移動機構によって、基準座標系に対する自身のボディ座標系の位置および姿勢を変化せることができる物体である。移動体としては、例えば、自動車、飛行体、ドローン、脚ロボット、探査機、および農作機などが挙げられる。これら以外にも、移動できる物体は移動体として扱うことができる。
【0029】
移動体８の基本的な構成を図２に示す。移動体８は、指令値演算部９およびアクチュエータ１０を備えている。
【0030】
アクチュエータ１０は、移動体８の移動機構を駆動するための駆動源である。移動体８が自動車である場合、アクチュエータ１０は、電動モータ、自動車駆動装置、およびブレーキ制御装置などである。
【0031】
指令値演算部９は、移動体制御装置１からの移動体制御量に基づいて、実際にアクチュエータ１０を駆動するための指令値をアクチュエータ１０に出力する機能を有する。なお、指令値演算部９で指令値を演算するために、位置姿勢推定装置６からの位置姿勢推定情報、および内界センサ５からの内界情報を使用する場合もある。
【0032】
【0033】
確率系最適化演算部１１は、状態量および参照経路に基づいて、予め定められた評価関数を最小化または最大化する最適化演算を行い、最適系列を制御決定部１２に出力する機能を有する。
【0034】
制御決定部１２は、確率系最適化演算部１１からの最適系列を用いて、移動体制御量を決定する機能を有する。
【0035】
＜確率モデル予測制御＞
本開示は、確率モデル予測制御と呼ばれる技術に関する。ここでは、確率モデル予測制御の説明を行う。
本開示は、確率モデル予測制御と呼ばれる技術に関する。ここでは、確率モデル予測制御の説明を行う。
【0036】
なお、以下では、確率変数で表される量を扱う場合は「確率的」とし、確率変数で表されない確定的な量を扱う場合は「確定的」とする。また、確率分布として、確率変数が実数を取る場合に使用される確率密度関数を前提に説明するが、確率変数が離散値を取る離散確率分布なども同様である。また、以下では、ξｋは各要素がそれぞれの確率分布に従って値が変動するＺ次元の確率的なベクトルであるものとする。
【0037】
通常のモデル予測制御は、有限の評価区間であるホライズン間の望ましい挙動を評価関数として表現し、状態方程式で表される制御対象（本開示では移動体に相当）の動特性、および、状態や入力の制約を制約条件とした制約付き最適制御問題を時々刻々と解くことによって、制御入力を逐次決定する制御手法である。
【0038】
確率モデル予測制御は、この考えをベースとして、制御対象の状態変数および外乱を確率変数として扱う制御手法である。以下では、次の離散時間状態方程式（式（１））を考える。
【0039】
【数1】
【0040】
式（１）において、ｘｋは時刻ｋ（ｋは整数）における制御対象の状態を表すｎ次元のベクトル、ｕｋは制御入力を表すｍ次元のベクトル、ｚｋは評価出力を表すｒ次元のベクトルである。以下では、下付きのｋは時刻ｋを表すものとする。Ａ（ξｋ）、Ｂｕ（ξｋ）、Ｂｗ（ξｋ）、Ｃ（ξｋ）、Ｄｕ（ξｋ）、Ｄｗ（ξｋ）をそれぞれ、ξｋによって定まるｎ×ｎ、ｎ×ｍ、ｎ×１、ｒ×ｎ、ｒ×ｍ、ｒ×１の行列であり、これらの行列はその要素がξｋに依存するような確率的な行列となる。式（１）では、Ａｋ：＝Ａ（ξｋ）、Ｂｕ,ｋ：＝Ｂｕ（ξｋ）、Ｂｗ,ｋ：＝Ｂｗ（ξｋ）、Ｃｋ：＝Ｃ（ξｋ）、Ｄｕ,ｋ：＝Ｄｕ（ξｋ）、Ｄｗ,ｋ：＝Ｄｗ（ξｋ）としている。これらの行列が確率的なため、ｘｋ、ｚｋは確率変数となる。
【0041】
確率モデル予測制御は、前述のように時々刻々と制約付き最適制御問題を解く。そのためにまず、長さがＮ（Ｎは１以上の自然数）のホライズンを考え、ホライズンの初期時刻ｋ＝ｋ０から終端時刻ｋ＝ｋ０＋Ｎまでの式（１）の応答に関する評価関数Ｊを下記の式（２）のように表す。
【0042】
【数2】
【0043】
ここで、ＰＮはｎ×ｎ、Ｑはｒ×ｒ、Ｒはｍ×ｍの確定的な正定な対称行列である。正定とは、その固有値が０以上の実数の固有値を持つ行列のことである。ＰＮは、制御対象が安定となるように設定され、ＱとＲはホライズン間でｚｋおよびｕｋをどのような挙動とさせるかの仕様によって設定される。上付きのＴは転置を表す。ホライズンの初期の状態ｘｋ０は状態量推定部により推定される。Ｅ［ｆ］は写像ｆに関する期待値演算を表している。すなわち、ｆが依存している確率変数の同時確率密度関数ｐとｆを掛けた量の多重積分を意味している。より具体的には、ｘｋ０＋Ｎはξｋ０からξｋ０＋Ｎに依存するため、例えば、下記の式（３）で表される。
【0044】
【数3】
【0045】
期待値演算は線形性を持つため、写像ｆとｇの線形和に対する期待値演算は下記の式（４）で表される。
【0046】
【数4】
【0047】
以下では、式（２）のような形の評価関数を「確率系評価関数」と呼称する。
【0048】
以下、ホライズン間での入力系列ＵＮを式（５）のように表し、最適化対象の変数ベクトルとして扱う。
【0049】
【数5】
【0050】
このとき、ＵＮはＮ・ｍ次元のベクトルとなる。また、同様にホライズンＮ間の状態系列Ｘｓ，Ｎを下記の式（６）とする。このとき、Ｘｓ，Ｎはｎ・（Ｎ－１）次元のベクトルとなる。
【0051】
【数6】
【0052】
確率モデル予測制御の場合、状態系列は確率変数となるため、状態系列に対するｎｐ個の制約は下記の式（７）のように確率的となる。
【0053】
【数7】
【0054】
ここで、Ｈｐはｎｐ×ｎ・（Ｎ－１）の確定的な行列である。ｈｐはｎｐ次元のベクトルである。βｐはｎｐ次元のベクトルであり、その要素がそれぞれ０から１の範囲の実数を取る。Ｐｒはその事象が生じる確率を表しており、式（７）はＨｐＸｓ，Ｎの各要素がｈｐ以下となる確率が、βｐ以上であることを示している。本開示では、式（７）のような制約を「機会制約」と呼称する。
【0055】
一方、下記の式（８）のような状態系列の期待値に対するｎｅ個の確定的な制約条件も課すことができる。
【0056】
【数8】
【0057】
ここで、Ｈｅはｎｅ×ｎ・（Ｎ－１）の確定的な行列である。ｈｅはｎｅ次元のベクトルである。本開示では、式（８）のような制約を「期待値制約」と呼称する。
【0058】
また、下記の式（９）のように、入力系列自体にもｎc個の確定的な制約条件を課すことができる。
【0059】
【数9】
【0060】
ここで、Ｈｃはｎｃ×Ｎ・ｍの確定的な行列である。ｈｃはｎｃ次元のベクトルである。本開示では、式（９）のような制約を「入力制約」と呼称する。
【0061】
これらをまとめると、確率モデル予測制御問題は、式（１）の状態方程式、および式（７），（８），（９）の制約条件とし、式（２）の評価関数Ｊを最小化するＵＮを探索する最適化問題となる。すなわち、下記の式（１０）で表される。
【0062】
【数10】
【0063】
ここで、式（１０）はＪの最小化として記載したが、Ｊを－Ｊとすれば最大化問題となるため、以降ではＪの最小化と最大化は区別せず、最小化で統一する。なお、式（２）の確率的な最適化問題は最適化変数ＵＮについて２次形式で書かれているため、確率的な２次計画問題となっている。また、制約条件を式（７），（８），（９）としたが、すべて用いる必要はなく、問題に適切な制約条件を設定する。
【0064】
式（１０）を解く方法については、様々な手法が提案されているが、その解法やアルゴリズムによっては解が近似解となる場合や、最適化演算に時間がかかるなどの課題がある。本開示では、確率変数ξｋの確率分布が既知であるとして、確率的な最適化問題である式（１０）を、写像を用いて確定的な最適化問題に変換する技術について開示する。
【0065】
確定的な制約付き最適化問題は、Ａｃｔｉｖｅ Ｓｅｔ法や内点法など効率的かつ高速で最適解を得ることができるアルゴリズムやソルバが多数存在する。以下、これらを「確定的ソルバ」と呼称する。確率的な最適化問題である式（１０）を解くことによって、一度確定的な最適化問題に変換してから確定的ソルバを用いて解くことで、効率的かつ高速に確率的な最適化問題を解くことができる。
【0066】
＜確率系最適化演算部＞
図３に戻り、確率系最適化演算部１１について説明する。確率系最適化演算部１１は、確率系評価関数変換部１３および確定系最適化演算部１４を備える。
図３に戻り、確率系最適化演算部１１について説明する。確率系最適化演算部１１は、確率系評価関数変換部１３および確定系最適化演算部１４を備える。
【0067】
確率系評価関数変換部１３は、移動体８の状態量および参照経路に基づいて、確率系評価関数を、下記の式（１１）のようなＵＮに関する２次形式の確定的な評価関数に変換する機能を有する。
【0068】
【数11】
【0069】
ここで、ＨＮはＮ・ｍ×Ｎ・ｍの行列、ｃＮはＮ・ｍ次元のベクトルである。ｃｏｎｓｔ．はＵＮに関する最適化に寄与しない定数項であり、以下では無視する。なお、以下では、式（１１）で表せる評価関数を「確定系評価関数」と呼称する。
【0070】
確定系最適化演算部１４は、確率系評価関数変換部１３からの確定系評価関数を、前述した確定的ソルバを用いて、確定系評価関数を最小化するＵＮを求め、最適系列として出力する。
【0071】
以下では、具体的に確率系評価関数変換部１３の機能である、式（２）の確率系評価関数を、式（１１）の確定的な評価関数に変換する具体的な計算方法について説明する。ここでは例として、式（２）の１項目（下記の式（１２）の項）の計算について説明する。
【0072】
【数12】
【0073】
式（１）の離散時間状態方程式より、ｘｋ０＋Ｎは、下記の式（１３）のように表される。
【0074】
【数13】
【0075】
ここで、下記の式（１４）とすると、式（１３）は、下記の式（１５）と表すことができる。
【0076】
【数14】
【0077】
【数15】
【0078】
従って、式（１２）は、下記の式（１６）のようにＵＮに関する２次形式で表すことができる。
【0079】
【数16】
【0080】
ここでは式（２）のうち、式（１２）の項について述べたが、残りの項についても同様にＵＮに関する２次形式で表すことができるため、式（２）は最終的に式（１２）の２次形式に変換できる。
【0081】
式（１６）に含まれる期待値演算Ｅの部分をオフライン処理もしくはオンライン処理で計算することによって、確定的ソルバで解ける形となる。なお、期待値演算Ｅには、例えば下記の式（１７）などの計算が含まれるが、ＡＴ
ｋ０とＡｋ０が独立でないことから下記の式（１８）であることに注意が必要である。
【0082】
【数17】
【0083】
【数18】
【0084】
このことが、確率系評価関数を確定系評価関数に変換することを困難にしている。本開示の方法によれば、式（１７）のような計算を、写像を用いることで厳密にかつ高速に計算することが可能になる。
【0085】
なお、式（１６）に含まれるようなＥの計算については大きく分けて２つの方法がある。１つ目は、確率変数ξｋの従う確率分布を用いてξｋ０からξｋ０＋Ｎまでのサンプルパスを複数生成し、式（１７）の標本平均を求める方法である。これは大数の法則に基づく計算であるが、サンプルパスの数が十分でなければ精度が悪く、また計算効率も非常に悪いため、オンライン処理での計算には不向きである。また、サンプルパスによっては、確率的にほぼ生じない事象が含まれる場合もあり、サンプルパスが少ない場合、これがＥの計算に悪影響を及ぼす可能性もある。
【0086】
ただし、オフライン処理で事前に標本平均により期待値演算Ｅの部分を計算しメモリに記録しておき、オンライン処理で該当部分を読み出して使用することは可能である。
【0087】
２つ目の方法は、ξｋの時刻ｋに関する関係、すなわち確率過程モデルを用いて解析的に計算する方法である。この方法については、確率過程モデルごとに写像を定義することによって、高速に期待値を計算することが可能となる。この写像は確率過程モデルによって異なるが、本開示では例として、ξｋが時刻ｋに対して独立同分布（Independent and identical distribution、以下では「ｉ．ｉ．ｄ．」と略記する）に従う場合と、隠れマルコフモデル（Hidden Markov Model、以下では「ＨＭＭ」と略記する）に従う場合について説明する。その他の確率過程モデルも、モデルごとに同様に写像を定義することが可能である。
【0088】
以下では、写像の説明のため、式（２）の確率系評価関数を、式（１１）の確定系評価関数に変換する際に生じる項（下記の式（１９））の計算について説明をする。
【0089】
【数19】
【0090】
＜ｉ．ｉ．ｄ．の場合の評価関数の計算＞
ξｋがｉ．ｉ．ｄ．に従う場合、ξｋの確率分布の形状は、時間的に不変であり、かつ値の出方に関して時間的依存性を有しない。ｉ．ｉ．ｄ．の場合、非特許文献１に記載の補題２を使用することができる。すなわち、任意のｎ×ｎの対称行列Ｍに対して、下記の式（２０）が成り立つことを利用する。式（２０）において、丸で囲まれたバツはクロネッカ積を表す。
ξｋがｉ．ｉ．ｄ．に従う場合、ξｋの確率分布の形状は、時間的に不変であり、かつ値の出方に関して時間的依存性を有しない。ｉ．ｉ．ｄ．の場合、非特許文献１に記載の補題２を使用することができる。すなわち、任意のｎ×ｎの対称行列Ｍに対して、下記の式（２０）が成り立つことを利用する。式（２０）において、丸で囲まれたバツはクロネッカ積を表す。
【0091】
【数20】
【0092】
ここで、ＧＡは、下記の式（２１）を満たすｎ’×ｎ２の行フルランク行列ＨＡを、下記の式（２２）のようにｎ’×ｎの行列ＨＡｉ（ｉ＝１，…，ｎ）に分解し、さらに下記の式（２３）としたｎ’・ｎ×ｎの行列である。
【0093】
【数21】
【0094】
【数22】
【0095】
【数23】
【0096】
ここで、ｒｏｗ（Ａ０）は、行列Ａ０の各要素を１行目から順番に並べた行ベクトルである。なお、式（２０）でＡ０（すなわちｋ＝０の場合）のみを考慮しているのは、ξｋがｉ．ｉ．ｄ．であり、ｋ＝０の場合を考慮すれば十分なためである。式（２０）を用いて、下記の式（２４）に表される写像ＶＡＡを定義する。
【0097】
【数24】
【0098】
式（２４）の写像を用いて式（１９）を計算する。ｉ．ｉ．ｄ．の場合、異なる時刻ではξｋは独立であるため、下記の式（２５）が成り立つ。
【0099】
【数25】
【0100】
また、式（３）を考慮すれば、下記の式（２６）のように表される。
【0101】
【数26】
【0102】
ここで、式（２４）の写像を用いれば、下記の式（２７）のようになる。
【0103】
【数27】
【0104】
これを繰り返すとＸＮを式（１９）、Ｘ０＝Ｐとした漸化式（下記の式（２８））が得られ、効率的にかつ厳密に式（１９）の計算が可能になる。
【0105】
【数28】
【0106】
ここでは、式（１９）の計算について写像を用いた方法を説明したが、式（２）の確率系評価関数を、式（１１）の確定系評価関数に変換する際に生じる他の項についても同様に写像を定義することで効率的に計算することが可能となる。例えば、下記の式（２９）の項などが生じるが、式（２０）と同様に、下記の式（３０）を満たすＧＢｕを用いて、下記の式（３１）の写像を定義する。
【0107】
【数29】
【0108】
【数30】
【0109】
【数31】
【0110】
この写像を用いると、式（２９）は下記の式（３２）のように計算することが可能である。
【0111】
【数32】
【0112】
なお、ＧＢｕは、Ｂｕ，０を用いて式（２１），（２２），（２３）と同様の手順で求めることができる。
【0113】
ｉ．ｉ．ｄ．の場合は、このようにして写像を定義することによって、確率系評価関数を確定系評価関数に変換することができる。
【0114】
＜ＨＭＭの場合の評価関数の計算＞
次に、ξｋがＨＭＭに従う場合について説明する。ＨＭＭは、離散もしくは連続の確率分布に従う系列を出力するモード（状態）が、各モード間で定められた遷移確率に従って遷移するとして構築された確率モデルである。以下、ＨＭＭにおける各モードに対応した確率分布を「出力分布」と呼称する。
次に、ξｋがＨＭＭに従う場合について説明する。ＨＭＭは、離散もしくは連続の確率分布に従う系列を出力するモード（状態）が、各モード間で定められた遷移確率に従って遷移するとして構築された確率モデルである。以下、ＨＭＭにおける各モードに対応した確率分布を「出力分布」と呼称する。
【0115】
ＨＭＭの出力およびモードの遷移について説明する。例えば、ＨＭＭがある時刻において「モードＡ」であった場合、「モードＡ」の確率分布に従った系列を出力する。一方、モードは他のモードへある遷移確率に従って遷移し、出力の確率分布が変化する場合もある。例えば、「モードＡ」から「モードＢ」に遷移した場合、「モードＢ」である時間区間では、「モードＢ」の確率分布に従った系列が出力される。ＨＭＭ内では、現在どのモードであるのかを直接観測することできず、その出力系列のみ観測されるため、「隠れ」とされている。
【0116】
以下では、ＨＭＭはＬ個のモードで構成されているとし、それぞれのモードをモード１、モード２、・・・、モードＬと呼称する。各モードの出力分布はそれぞれＤ１、Ｄ２、．．．、ＤＬであり、各時刻ｋにおけるモードをσｋ（すなわち、σｋは１，２，．．．，Ｌをとる）とする。また、ηｋは時刻ｋにおけるＨＭＭが出力する出力系列とし、ηｋは各時刻においてＤ１、Ｄ２、．．．、ＤＬのいずれかの出力分布に従うとする。分布Ｄｊに従うηｋは、確率変数η（ｊ）と書くこととする。なお、以下では、同じモードになる時刻でのηｋは各時刻で独立であるものとする。また、確率変数ξｋを下記の式（３３）で表し、σｋによりその出力分布が異なることを表す。
【0117】
【数33】
【0118】
「モードｉ」から「モードｊ」への遷移確率をｐｉｊとし、ＨＭＭの各モード間の遷移は、既約かつ非周期的なマルコフ連鎖に従うとすると、下記の式（３４）で表される。
【0119】
【数34】
【0120】
下記の式（３５），（３６）のように表される遷移行列Πと、時刻ｋにおける確率ベクトルａｋを用いると、モードの時間遷移は、下記の式（３７）のように表される。
【0121】
【数35】
【0122】
【数36】
【0123】
【数37】
【0124】
次にｉ．ｉ．ｄ．の場合の説明と同様に、式（１９）を例にＨＭＭで定義される写像について述べる。ＨＭＭの場合、ξｋが１時刻前のξｋ－１のみに依存することから、条件付き確率を用いて、下記の式（３８）のように表すことができる。
【0125】
【数38】
【0126】
式（３）を考慮すれば、式（１９）は条件付き期待値を用いて、下記の式（３９）のように表すことができる。
【0127】
【数39】
【0128】
なお、式（３９）の条件付き期待値は、ξｋのうちηｋには依存せずσｋのみに依存することから、条件としてσｋのみを記載している。なお、σｋ０－１はｋ０より１時刻前のモードである。
【0129】
式（３９）の計算は、ＨＭＭのモードがホライズン間でどのようなパスを経由するかを考慮する必要がある。その考慮のため、時刻ｋにおけるＬ個のモードに対応したｎ×ｎの行列Ｍｋ
（ｉ）（ｉ＝１，２，．．．，Ｌ）、および、Ｍｋ
（ｉ）を並べた順序列Ｍｔ，ｋ（下記の式（４０））を定義する。
【0130】
【数40】
【0131】
また、下記の式（４１），（４２）に表す写像を定義する。
【0132】
【数41】
【0133】
【数42】
【0134】
なお、ＧＡｉは、「モードｉ」での確率分布Ｄｉおよびその出力η（ｉ）を用いて、下記の式（４３）で表される行列である。ＧＡｉは式（２１），（２２），（２３）と同様の手順で求めることができる。
【0135】
【数43】
【0136】
式（４０），（４１），（４２）を用いて、式（３９）を計算する。まず、下記の式（４４）とすると、式（３９）の最も内部の部分は、下記の式（４５）で表すことができる。
【0137】
【数44】
【0138】
【数45】
【0139】
ここで、下記の式（４６）で表される漸化式を用いると、式（３９）はさらに下記の式（４７）となる。
【0140】
【数46】
【0141】
【数47】
【0142】
これを繰り返すと、下記の式（４８）のように計算することができる。
【0143】
【数48】
【0144】
上式は、σｋ０－１で条件付けられている。この計算のため、時刻ｋ０－１での確率ベクトルａｋ０－１を考える。時刻ｋ０－１でＨＭＭがどのモードかを観測できる場合、例えば時刻ｋ０－１で「モード１」であった場合は、ａｋ０－１＝［１ ０ ．．． ０］Ｔで与えられる。また時刻ｋ０－１でのＨＭＭが観測できない場合は、各モードの初期確率によりａｋ０－１が与えられ、例えば各モードの確率は一様として下記の式（４９）などが利用される。
【0145】
【数49】
【0146】
このようにして与えられる時刻ｋ０－１での確率ベクトルａｋ０－１を用いれば、下記の式（５０）のように計算することができる。
【0147】
【数50】
【0148】
このようにして、効率的かつ厳密に式（１９）を計算することが可能になる。ただし、ａｋ０－１，ｉは確率ベクトルａｋ０－１のｉ番目の要素である。
【0149】
式（３９）のようなその他の項についても同様に、写像と漸化式を定義すれば良い。ＨＭＭの場合は、このようにして写像を定義することで、確率系評価関数を確定系評価関数に変換することができる。
【0150】
また、ホライズン長さＮやＨＭＭの遷移行列Πの要素ｐｉｊは固定値ではなく、移動体周辺の環境に合わせてオンラインで変化させても良い。
【0151】
ここで説明した確率系評価関数変換部１３での処理は、状態ｘｋ０、ＨＭＭの初期確率ａｋ０－１や遷移確率ｐｉｊなどのオンラインで異なる値を取る項に関わる部分を除けば、多くの部分で確定的な行列や数値となる。そのため、オンライン時に変化しない行列や数値は、オフラインで計算後、メモリに記憶しておき、オンライン時に読み出して利用することで、オンライン処理の高速化が可能である。これにより、ホライズン長さＮの増減に対しても高速に処理が可能となる。
【0152】
例えば、ｉ．ｉ．ｄ．の場合、ξｋの確率分布の形状が変わらない限り、式（２４）の写像で使用されるＧＡは、オンラインで得られる情報によって変化しない。そのため、オフライン処理でＧＡを計算しておき、オンライン処理の際にはＧＡを読み出し後、式（２８）の漸化式を用いることによって、確率系評価関数を確定系評価関数に変換することが可能となる。式（２８）の漸化式の計算自体は処理負荷が小さいため、高速に確率系評価関数を確定系評価関数に変換することが可能となる。
【0153】
また、ＨＭＭの場合も同様に、η（ｉ）
ｋの確率分布の形状が変わらない限り、式（４１）の写像で使用されるＧＡｉはオンラインで得られる情報で変化しない。そのため、ｉ．ｉ．ｄ．の場合と同様に、オフラインで計算し記録されたＧＡｉをオンライン時に読み出して、式（４７）の漸化式を計算することによって、高速に確率系評価関数を確定系評価関数に変換することが可能となる。
【0154】
以下で説明する制御決定部１２も同様に、オンラインで得られる情報で変化しない行列や数値については、オフライン計算とメモリへの記録、オンライン処理時に読み出すことによって処理の高速化が可能である。
【0155】
なお、移動体８の周囲環境によっては、移動体８の制御時にξｋやη（ｉ）ｋの確率分布の形状が変化する場合も考えられる。その際は、写像に使用されるＧＡやＧＡｉをオンライン処理で再度計算し直すか、事前に確率分布のパターンが複数あることが分かっている場合は、それぞれのパターンでの確率分布でＧＡやＧＡｉを計算しておき、オンライン処理では確率分布のパターンに合わせたＧＡやＧＡｉを読み出して使用すれば良い。
【0156】
このように、確率系評価関数変換部１３は、ξｋの確率過程モデルを用いることによって、確率系評価関数を確定系評価関数に変換する。
【0157】
＜制御決定部１２＞
図３に戻り、制御決定部１２の具体的な動作について説明する。制御決定部１２は、確率系最適化演算部１１から出力されるＵＮの最適系列を用いて、移動体８を制御するための移動体制御量に変換する。
図３に戻り、制御決定部１２の具体的な動作について説明する。制御決定部１２は、確率系最適化演算部１１から出力されるＵＮの最適系列を用いて、移動体８を制御するための移動体制御量に変換する。
【0158】
前述の通り、移動体制御量は、目標経路、目標軌道、および移動体８のアクチュエータへの指令値を含む。移動体制御量がアクチュエータへの指令値である場合は、ＵＮに含まれるｕｋ０が制御決定部１２から出力されるが、計算の遅れなどを考慮して、ＵＮに含まれる項、例えばｕｋ０＋１などを出力しても良い。
【0159】
移動体制御量が目標経路または目標軌道である場合は、ＵＮと式（１）の移動体８の状態方程式から求める。すなわち、下記の式（５１）を用いる。
【0160】
【数51】
【0161】
ここで、ΨＮ、Θｕ，Ｎ、およびΘｗ，Ｎのそれぞれは、下記の式（５２），（５３），（５４）で表される。
【0162】
【数52】
【0163】
【数53】
【0164】
【数54】
【0165】
制御決定部１２では、Ｘｓ，Ｎから移動体８の制御に必要な量を抽出し、目標経路もしくは目標軌道として移動体制御量を出力する。しかし、Ｘｓ，Ｎはξｋの確率分布に依存するため、目標経路や目標軌道も確率分布を持つ。移動体８がそのような確率分布を持った目標経路や目標軌道に対応して制御可能であればそのままでよいが、通常は確率分布から、確定的な目標経路や目標軌道に変換する必要がある。変換の方法は期待値を取る方法や分布の最大値を取る方法など様々あるが、ここではＸｓ，Ｎの期待値、すなわち下記の式（５５）を取り、Ｅ［Ｘｓ，Ｎ］から目標経路や目標軌道を生成する方法について述べる。
【0166】
【数55】
【0167】
なお、ここでもξｋがｉ．ｉ．ｄ．の場合とＨＭＭの場合について説明し、例としてＥ［ΨＮ］の計算について述べる。
【0168】
＜ｉ．ｉ．ｄ．の場合＞
ξｋがｉ．ｉ．ｄ．の場合、式（２５）に示したように、ξｋは時刻ｋに対して独立であるため、Ｅ［ΨＮ］は下記の式（５６）のような計算が可能である。
ξｋがｉ．ｉ．ｄ．の場合、式（２５）に示したように、ξｋは時刻ｋに対して独立であるため、Ｅ［ΨＮ］は下記の式（５６）のような計算が可能である。
【0169】
【数56】
【0170】
＜Ｈ．Ｍ．Ｍの場合＞
ξｋがＨＭＭの場合、式（３８）に示したように、ξｋはξｋ－１に依存する。そのため、例えば、下記の式（５７）となる。
ξｋがＨＭＭの場合、式（３８）に示したように、ξｋはξｋ－１に依存する。そのため、例えば、下記の式（５７）となる。
【0171】
【数57】
【0172】
確率系評価関数変換部１３の場合と同様に、時刻ｋにおけるＬ個のモードに対応したｎ×ｎの行列Ｎｋ
（ｉ）（ｉ＝１，２，．．．，Ｌ）および、Ｎｋ
（ｉ）を並べた順序列Ｎｔ，ｋを用いる（下記の式（５８）を参照）。
【0173】
【数58】
【0174】
ここで、下記の式（５９）の写像を定義する。
【0175】
【数59】
【0176】
式（５９）の写像により、下記の式（６０）として、下記の式（６１）の漸化式が得られる。
【0177】
【数60】
【0178】
【数61】
【0179】
式（５９）の写像と、式（６１）の漸化式とによって、式（５７）は、各モードの初期確率ａｋ０－１を用いて、下記の式（６２）のように計算することが可能である。
【0180】
【数62】
【0181】
上記では、ｉ．ｉ．ｄ．やＨＭＭのＥ［ΨＮ］の計算方法について説明した。Ｅ［Θｕ，Ｎ］およびＥ［Θｗ，Ｎ］についても同様に計算が可能であるため、Ｅ［Ｘｓ，Ｎ］を計算することができる。
【0182】
制御決定部１２は、このように最適系列ＵＮの一部の抽出や、Ｅ［Ｘｓ，Ｎ］を求めることによって、目標経路や目標軌道を生成する。
【0183】
＜移動体８が自動車である場合＞
これまでに、図１，３を用いて移動体制御装置１の処理について説明した。ここでは、より具体的な例を提示するために、移動体８が自動車である場合について説明する。
これまでに、図１，３を用いて移動体制御装置１の処理について説明した。ここでは、より具体的な例を提示するために、移動体８が自動車である場合について説明する。
【0184】
図４は、移動体８が自動車である場合の構成の一例を示す図である。
【0185】
ドライバー（すなわち運転者）が自動車を操作するために設置されているステアリングホイール５０は、ステアリング軸５１に結合されている。ステアリング軸５１は、ラックアンドピニオン機構５３のピニオン軸６２が連接されている。ラックアンドピニオン機構５３のラック軸６３は、ピニオン軸６２の回転に応じて往復移動自在であり、その左右両端にはタイロッド５４を介してフロントナックル５５が接続されている。フロントナックル５５は、操舵輪としての前輪６４を回転自在に支持すると共に、車体フレームに転舵自在に支持されている。
【0186】
ドライバーがステアリングホイール５０を操作して発生したトルクはステアリング軸５１を回転させ、ラックアンドピニオン機構５３がステアリング軸５１の回転に応じてラック軸６３を左右方向へ移動させる。ラック軸６３の移動により、フロントナックル５５が図示しないキングピン軸を中心に回動し、それにより前輪６４が左右方向へ転蛇する。よって、ドライバーは、自動車が前進または後進する際にステアリングホイール５０を操作することによって、自動車の横移動量を変化させることができる。
【0187】
なお、完全自動運転などの非搭乗型の移動体８の場合は、ステアリングホイール５０のようなドライバー操作のための構成要素は不要となる。
【0188】
自動車には、自動車の内界を認識するための内界センサとして、車速センサ７０、ＩＭＵセンサ７１、操舵角センサ７２、および操舵トルクセンサ７３などが設置されている。
【0189】
また、自動車には、自動車の横方向の運動を実現するための電動モータ５２、自動車の前後方向の運動を制御するための車両駆動装置５６、およびブレーキ制御装置５９などのアクチュエータが設置されている。
【0190】
電動モータ５２は、一般的にはモータとギアとで構成され、操舵制御装置６１からの指示に従って、ステアリング軸５１にトルクを与えることによって、ステアリング軸５１を自在に回転させることができる。つまり、電動モータ５２は、ドライバーのステアリングホイール５０の操作と独立して、前輪６４を自在に転舵させることができる。
【0191】
車両駆動装置５６は、自動車を前後方向に駆動するためのアクチュエータである。車両駆動装置５６は、加減速制御装置５８からの指示に従って、例えばエンジンやモータなどの駆動源で得られた駆動力を、図示しないトランシミッションとシャフト５７とを介して、前輪６４および後輪６５を回転させる。これにより、車両駆動装置５６は、自動車の駆動力を自在に制御することが可能である。
【0192】
一方、ブレーキ制御装置５９は、自動車を制動するためのアクチュエータであり、加減速制御装置５８からの指示に従って、自動車の前輪６４および後輪６５のそれぞれに設置されたブレーキ６０のブレーキ量を制御する。一般的なブレーキ６０は、前輪６４および後輪６５と共に回転するディスクロータに、油圧を用いてパッドを押し付けることによって、制動力を発生させる。
【0193】
内界センサ、および上記で説明した複数の装置は、自動車内のＣＡＮ（Controller Area Network）やＬＡＮ（Local Area Network）などを用いてネットワークを構成しているものとする。各装置は、ネットワークを介してそれぞれの情報を取得することが可能である。また、内界センサは、ネットワークを介して相互にデータの送受信が可能である。
【0194】
移動体８の動特性ごとにその動特性をモデル化した連続時間状態方程式が求まる。本開示では、各種の移動体８へ適用可能な移動体制御装置１を提供するが、ここでは自動車を例として詳細に説明する。なお、移動体８を制御する方法は数多く提案されているが、実施の形態１では参照経路に対する横方向の運動の制御方法ついて説明する。
【0195】
まず、移動体８の横方向の動特性を表す連続時間状態方程式について図５を用いて説明する。図５中のＸ軸、Ｙ軸を持つ座標系は基準座標系であり、ｘｂ軸、ｙｂ軸を持つ座標系はボディ座標系である。基準座標系で表された参照経路に対する移動体の横偏差および偏角をそれぞれ、ｅｙ、ｅθとすると、移動体８の横方向の連続時間状態方程式は下記の式（６３）のようになる。
【0196】
【数63】
【0197】
ここで、ｘｃ、Ａｃ、Ｂｃｕ、Ｂｃｗのそれぞれは、下記の式（６４）のように表される。
【0198】
【数64】
【0199】
上付きの「’」は時間微分を表す。また、各記号は以下を表す。
【0200】
ｖｘ：車速［ｍ／ｓ］
δ：舵角［ｒａｄ］
κ：参照経路の曲率［１/ｍ］
ｍ：質量［ｋｇ］
Ｌｆ：重心－前車輪軸間距離［ｍ］
Ｌｒ：重心－後車輪軸間距離［ｍ］
Ｉｚ：ヨー軸周りの慣性モーメント［ｋｇ・ｍ＾２］
Ｃｆ：前輪コーナリングスティフネス［Ｎ/ｒａｄ］
Ｃｒ：後輪コーナリングスティフネス［Ｎ/ｒａｄ］
ｅｙ：参照経路から自動車の重心までの横偏差［ｍ］
ｅ‘ ｙ：ｅｙの時間微分［ｍ/ｓ］
ｅθ：参照経路から自動車の重心までの偏角［ｒａｄ］
e' θ：ｅθの時間微分［ｒａｄ/ｓ］
コーナリングスティフネスとは、移動体に発生する横力と横滑り角との関係を表す比例係数であり、例えば、乾燥面、湿潤面、凍結面などと移動体との接触面の状態によって変化する値である。
δ：舵角［ｒａｄ］
κ：参照経路の曲率［１/ｍ］
ｍ：質量［ｋｇ］
Ｌｆ：重心－前車輪軸間距離［ｍ］
Ｌｒ：重心－後車輪軸間距離［ｍ］
Ｉｚ：ヨー軸周りの慣性モーメント［ｋｇ・ｍ＾２］
Ｃｆ：前輪コーナリングスティフネス［Ｎ/ｒａｄ］
Ｃｒ：後輪コーナリングスティフネス［Ｎ/ｒａｄ］
ｅｙ：参照経路から自動車の重心までの横偏差［ｍ］
ｅ‘ ｙ：ｅｙの時間微分［ｍ/ｓ］
ｅθ：参照経路から自動車の重心までの偏角［ｒａｄ］
e' θ：ｅθの時間微分［ｒａｄ/ｓ］
コーナリングスティフネスとは、移動体に発生する横力と横滑り角との関係を表す比例係数であり、例えば、乾燥面、湿潤面、凍結面などと移動体との接触面の状態によって変化する値である。
【0201】
なお、参照経路の曲率κについてはホライズンＮ間の予測値が必要となる。その予測の方法としては複数考えられるが、ホライズンＮ間でκは一定値をとると仮定し、ホライズン間で同じκの値を用いるのが最も簡単な方法である。
【0202】
その他の方法として、移動体８は等速で参照経路上を運動するなどと仮定することによって、ホライズンＮ間での移動体８の参照経路上の予測位置を求める。参照経路の情報として、参照経路上の各点の曲率が含まれている場合は、その予測位置での参照経路の曲率を読み出して使用すればよい。また、参照経路がスプラインや多項式などで表現されている場合は、予測位置での曲率を計算することができる。すなわち、例えば、参照経路がＹ＝ｆｒ（Ｘ）等で表現されている場合は、下記の式（６５）によって予測位置での曲率が求まる。
【0203】
【数65】
【0204】
なお、参照経路がＸ点群、Ｙ点群などで表現されている場合は、点群を一度スプラインや多項式の滑らかな関数で近似し、式（６５）を計算することにより曲率が求まる。また、曲率をばらつきのあるパラメータとして扱い、確率分布をもつ確率変数としてξｋに含め、式（１）に組み入れることが可能である。
【0205】
式（６３）の連続時間状態方程式を式（１）の離散時間状態方程式に変換する。このとき、ホライズン間でのサンプリング間隔をｈｋとすると、下記の式（６６），（６７）となる。
【0206】
【数66】
【0207】
【数67】
【0208】
なお、ｈｋは通常一定値とするが、確定系最適化演算部１４は、その処理時間にばらつきが生じるため、ｈｋにそのばらつきを表す確率変数（すなわちξｋ）を加えても良い。
【0209】
次に、式（１）のｚｋを設計する。まず、自動車を参照経路に追従させるために、ｘｋの要素をすべて０に収束させる必要がある。また同時に、自動車の位置および姿勢の確率分布を考えるため、ｅｙとｅθに確率変数ξ１ｋ、ξ２ｋを加法的ノイズとして加える。すなわち、ｚｋを下記の式（６８）のようにとる。
【0210】
【数68】
【0211】
これにより、自車の位置および姿勢の確率分布を考慮することが可能となる。
【0212】
なお、ここではｅｙとｅθの加法的ノイズのみ考慮したが、乗法的ノイズも考慮可能である。その場合、確率変数を新たに考慮し、Ｃｋのｅｙとｅθにかかる要素にその確率変数を設定すれば良い。また、同様にしてｅｙ’やｅθ’の確率分布も考慮することができる。
【0213】
ξ１ｋおよびξ２ｋは、位置姿勢推定装置６からの位置姿勢情報がどのような確率分布で表現できるかで設計するのがよい。多くの場合は、ｉ．ｉ．ｄ．が仮定され、かつその確率分布は正規分布等で表現されることが多いが、本開示の方法では、確率分布の形に制限がないためその他の形状の確率分布でも適用が可能である。
【0214】
ｉ．ｉ．ｄ．かつ正規分布以外の例として、ＧＮＳＳで検出した位置の確率モデルがＨＭＭで表現できることについて説明する。図６は、ＧＮＳＳで計測される位置の確率過程モデルである。ＧＮＳＳで高精度に位置を計測する方法としてＲＴＫ（Real Time Kinematic）がよく利用される。ＲＴＫは、移動体８に設置されたアンテナの位置を、移動体８から数ｋｍの範囲に設置される基準点の情報、および測位衛星からの測位信号を用いて高精度かつ高速に計測する方法である。ＲＴＫには、基本的にＦｉｘ、Ｆｌｏａｔ、ＤＧＮＳＳ（Differential－GNSS）、および誤Ｆｉｘの４つのモードがある。その他に未測位というモードがあるが、この場合は位置情報が出ないので無視する。また、慣性センサを用いる場合はＤＲ（Dead Reckoning）のモードがあるが、ここでは無いものとする。
【0215】
詳細な説明は省略するが、Ｆｉｘは、搬送波位相の情報を用いて、測位衛星からの電波の波数（整数）が精度良く求まったモードであり、最も位置精度が良い。Ｆｌｏａｔは、Ｆｉｘの前段階で計算される暫定的な位置であり、波数の整数解が求まらなかった場合に出力される。ＤＧＰＳは、搬送波位相情報が得られない場合の推定位置である。誤Ｆｉｘは、波数が間違って求められた場合に出力される。
【0216】
図６は、さらにそれぞれのモードごとの確率分布のイメージを示している。図の破線が真の位置であるとすると、Ｆｉｘは、真の位置に近く、またばらつきも小さい。Ｆｌｏａｔは、Ｆｉｘに比べてばらつきが大きくなるが、ＤＧＮＳＳよりは小さい。誤Ｆｉｘは、ばらつきは小さいが、真の位置からオフセットを持った位置が出力される。これらのモード間の遷移は、式（３５）のように遷移行列Πで表現される。
【0217】
なお、ＧＮＳＳの場合、遷移行列Πは移動体８の周辺環境によって、その要素の値であるｐｉｊが変化する場合がある。例えば、衛星からの電波の遮蔽物や、ビルなど電波を反射する構造体が少ない環境では、Ｆｉｘのモードに遷移しやすい。また、都市部などのビルが多い環境では、Ｆｌｏａｔや誤Ｆｉｘに遷移しやすくなる。本開示の移動体制御装置１では、遷移行列Πが移動体８の周辺環境に合わせてｐｉｊを変化させても問題はない。
【0218】
このようにして、移動体８の存在確率がＨＭＭで表せる場合においても、本開示の移動体制御装置１は、精度を維持したまま移動体８を参照経路へ追従させることができる。
【0219】
なお、本開示では、評価関数を式（２）のように最適化変数ＵＮに対して２次計画問題としているが、１次の計画問題、すなわち評価関数をＵＮに対して１次となるようにしても良い。この場合は線形計画問題と呼ばれるが、本開示で示した方法と同様の方法で、確率計画問題から確定的な最適化問題に変換することが可能である。この場合、確定的ソルバとして線形計画法などを用いた効率的なソルバを用いれば良い。
【0220】
また、本開示では、式（６４）のような状態方程式を使用したが、移動体８に合わせて変更する必要がある。
【0221】
なお、式（６４）で使用されている質量ｍや慣性モーメントＩｚ、コーナリングスティフネスＣｆやＣｒのパラメータは、乗員の数や自動車と路面との接触面の状態によって変化する場合がある。これらについて、直接観測することはできないが、確率分布が分かっている場合、その確率分布を持つ確率変数をξｋに含め、式（１）に組み入れることが可能である。これにより、より高精度な移動体制御が可能となる。なお、自動車以外の移動体８についても、移動体８の動特性の不確かな部分を確率変数に加えることよって、より高精度な移動体制御が可能となる。
【0222】
本開示に示す方法により、特許文献１では考慮ができなかった、自車の位置および姿勢の確率分布を考慮しつつ経路追従性を向上させることが可能な移動体制御装置１を提供することができる。
【0223】
＜実施の形態２＞
実施の形態２では、移動体８の周辺に障害物が存在する場合でも障害物を回避することによって安全を確保することが可能な移動体制御装置を提供する。なお、実施の形態１と同内容については説明を省略する。
実施の形態２では、移動体８の周辺に障害物が存在する場合でも障害物を回避することによって安全を確保することが可能な移動体制御装置を提供する。なお、実施の形態１と同内容については説明を省略する。
【0224】
図７は、実施の形態２に係る移動体制御装置１５の機能ブロック図である。実施の形態１に係る移動体制御装置１に対して、障害物センサ１８が接続され、障害物移動予測部１６が追加されている。
【0225】
障害物センサ１８は、１以上のカメラ、ＬｉＤＡＲ、Ｌａｄａｒなどのセンサで構成される。障害物センサ１８は、移動体８と障害物との相対的な位置を障害物情報として出力する。また、障害物センサ１８は、移動体８と障害物との相対速度を出力可能な場合もある。この場合は、移動体８と障害物との相対位置および相対速度を障害物情報として出力する。移動体８が自動車である場合は、他の自動車、自転車、歩行者などの障害物の位置および速度が障害物情報となる。障害物センサ１８は、移動体８自身に設置されることが多いが、環境センサとして移動体８が走行する環境に複数個設置されることもある。この場合、移動体８は環境センサとの通信により、障害物情報を取得する。
【0226】
障害物移動予測部１６は、障害物センサ１８からの障害物情報を用いて、ホライゾンＮの間の障害物の位置を予測し、障害物予測情報として出力する機能を有する。障害物の移動の予測方法として、例えば障害物センサ１８から障害物の速度情報が出力される場合は、障害物が等速直線運動を行うと仮定して予測する方法、機械学習により学習された障害物移動予測モデルを用いて予測する方法など既存の様々な方法が適用できる。障害物移動予測部１６は、ホライゾンＮ間の障害物の位置が予測できれば、どのような方法を用いても良い。
【0227】
＜制約演算部２０＞
図８は、実施の形態２に係る確率系最適化演算部１９の機能ブロック図である。実施の形態１と比較して、制約演算部２０が追加されている。確率系最適化演算部１９および制御決定部１２は、制御演算部１７が有している。
図８は、実施の形態２に係る確率系最適化演算部１９の機能ブロック図である。実施の形態１と比較して、制約演算部２０が追加されている。確率系最適化演算部１９および制御決定部１２は、制御演算部１７が有している。
【0228】
制約演算部２０は、参照経路および障害物情報を用いて、障害物を回避するための確定的な制約条件を生成する機能を有する。ここで、確定的な制約条件とは、下記の式（６９）のように確率を含まないｎｕ個の制約条件であり、本開示では「確定系制約条件」と呼称する。
【0229】
【数69】
【0230】
ここで、Ｈｕはｎｕ×ｍ・Ｎの確定的な行列であり、ｈｕはｎｕ次元のベクトルである。
【0231】
確率モデル予測制御における制約条件として、式（７），（８），（９）のように、機会制約、期待値制約、および入力制約などがあることは既に説明した。このうち、入力制約については、式（７）をそのまま利用すればよい。また、期待値制約については、式（８）におけるＥ［Ｘｓ，Ｎ］を、式（５５）を用いて計算すればよい。このとき、既に説明したようにｉ．ｉ．ｄ．やＨＭＭなどの確率過程モデルを用いれば高速かつ厳密に計算することが可能となる。
【0232】
機会制約を確定的な制約条件に変換する方法は、チェビシェフの不等式、カンテリの不等式などの確率不等式を用いる方法、一般化多項式カオスによる方法、期待値制約へ変換する方法（保守的近似）など、様々提案されている。これらの手法を用いれば、式（７）の機会制約は、確定的な制約条件に変換することができる。
【0233】
制約演算部２０は、機会制約、期待値制約、および入力制約を合わせて、式（６９）のような確定系制約条件を演算し、確定系最適化演算部２１に出力する。
【0234】
確定系最適化演算部２１は、確率系評価関数変換部１３からの確定系評価関数を、制約演算部２０からの確定系制約条件のもとで最適化を行い、最適系列を出力する。
【0235】
＜障害物回避の例＞
制約演算部２０の動作の説明について、図９を用いて説明する。図９は、移動体８（自動車）の参照経路として幅２Ｌｌの区画線の中心を用いており、静止している障害物が参照経路上付近に存在するシーンを示している。この場合、移動体８は静止障害物に近寄らないような制約を守る必要がある。図中の黒点はホライズンＮの間の、参照経路近傍の移動体の予測位置であり、移動体８が参照経路上を等速で運動するなどの仮定により求まる。
制約演算部２０の動作の説明について、図９を用いて説明する。図９は、移動体８（自動車）の参照経路として幅２Ｌｌの区画線の中心を用いており、静止している障害物が参照経路上付近に存在するシーンを示している。この場合、移動体８は静止障害物に近寄らないような制約を守る必要がある。図中の黒点はホライズンＮの間の、参照経路近傍の移動体の予測位置であり、移動体８が参照経路上を等速で運動するなどの仮定により求まる。
【0236】
このような場合、制約演算部２０は、移動体８が静止障害物に必要以上に近寄らないようにするために、静止障害物を含むような大きさの回避領域を設定し、この回避領域に対して制約を設定する。図９の例では、区画線の幅Ｌｌに対して、Ｌｏだけ内側に回避領域を設定している。ホライズンＮ間の時刻、ｋ＝ｋ０＋ｉ１、ｋ＝ｋ０＋ｉ２、ｋ＝ｋ０＋ｉ３に静止障害物の回避領域に近づくような予測がされたとする。これらの時刻での横位置偏差ｅｙ,ｋ０＋ｉ１、ｅｙ,ｋ０＋ｉ２、ｅｙ,ｋ０＋ｉ３について、下記の式（７０）のような機会制約を設定する。
【0237】
【数70】
【0238】
βｃについては、例えば０．９５のように設定すれば９５％以上の確率で、移動体８が回避領域に入らないようにすることができる。また、期待値制約で問題ない場合は、下記の式（７１）とすればよい。
【0239】
【数71】
【0240】
なお、その他の時刻でのｅｙについては、区画線からはみ出さないような制約を設定する。このように、ｅｙ,ｋ０＋ｉ１、ｅｙ,ｋ０＋ｉ２、ｅｙ,ｋ０＋ｉ３に関する制約は、適当なＤｃと、Ｘｓ，Ｎにより表現することができるため、式（７）のように書くことができる。
【0241】
実施の形態２では、静止障害物の回避の例について述べたが、障害物が移動する場合でも同様に障害物を回避することができる。具体的には、ホライズンＮ間の障害物の予測位置に合わせて、制約を設ければ良い。
【0242】
実施の形態２に係る構成により、障害物を回避可能な移動体制御装置１５を提供することができる。
【0243】
＜実施の形態３＞
実施の形態３では、移動体８の横方向の制御と前後方向の制御とを分離することによって、前後方向と横方向の移動の制御を同時に行うことが可能な移動体制御装置を提供する。なお、実施の形態１，２と同内容については説明を省略する。ここで前後方向とは、図５のボディ座標系においてｘｂ軸方向を示し、横方向とは、ｙｂ軸方向を示す。このような移動体制御装置が必要な理由は、一般に移動体の場合はその運動拘束により、前後方向の移動と横方向の移動とが連成する。そのため、前後方向と横方向の運動を同時に制御することが望ましいが、この場合、移動体の状態方程式が状態に対して非線形となり、扱いが複雑になる。実際に、式（６４）のように横方向の状態方程式に速度ｖｘが含まれていることがわかる。そのため、まず前後方向について制御を行い、その情報を用いて横方向の制御を行うことが好適である。
実施の形態３では、移動体８の横方向の制御と前後方向の制御とを分離することによって、前後方向と横方向の移動の制御を同時に行うことが可能な移動体制御装置を提供する。なお、実施の形態１，２と同内容については説明を省略する。ここで前後方向とは、図５のボディ座標系においてｘｂ軸方向を示し、横方向とは、ｙｂ軸方向を示す。このような移動体制御装置が必要な理由は、一般に移動体の場合はその運動拘束により、前後方向の移動と横方向の移動とが連成する。そのため、前後方向と横方向の運動を同時に制御することが望ましいが、この場合、移動体の状態方程式が状態に対して非線形となり、扱いが複雑になる。実際に、式（６４）のように横方向の状態方程式に速度ｖｘが含まれていることがわかる。そのため、まず前後方向について制御を行い、その情報を用いて横方向の制御を行うことが好適である。
【0244】
＜構成例１＞
図１０に実施の形態３に係る制御演算部２２の構成例１を示す。制御演算部２２は、実施の形態１での制御演算部４とは異なり、前後方向制御部２３、横方向制御部２５、および制御決定部２７で構成される。
図１０に実施の形態３に係る制御演算部２２の構成例１を示す。制御演算部２２は、実施の形態１での制御演算部４とは異なり、前後方向制御部２３、横方向制御部２５、および制御決定部２７で構成される。
【0245】
前後方向制御部２３は、参照経路、障害物情報、および状態量を用いて、ホライズンＮ間の前後方向系列を出力する機能を有する。
【0246】
横方向制御部２５は、前後方向系列、参照経路、障害物情報、および状態量を用いて、ホライズンＮ間の横方向最適系列を出力する機能を有する。
【0247】
制御決定部２７は、前後方向系列および横方向最適系列から、移動体８を制御するための移動体制御量を決定する。
【0248】
前後方向制御部２３は、速度計画部２４で構成される。速度計画部２４の出力について図１１を用いて説明する。図中の横軸は時刻、縦軸はその時刻での速度を表している。速度計画部２４は、ホライズンの開始時刻ｋ０からｋ０＋Ｎまでのそれぞれの時刻での速度を前後方向系列として演算し、出力する。特に図１１では、通常走行区間、カーブ区間、アプローチ区間、および停車区間での速度系列を示しており、通常走行では指定された速度、カーブ区間では参照経路の曲率に合わせた減速、アプローチ区間では障害物手前で停止するための減速、停車区間では停車を維持するような速度系列の例を示している。前後方向制御部２３は、このような動作をする速度計画部２４から前後方向系列を出力する。
【0249】
横方向制御部２５は、前後方向制御部２３からの前後方向系列を反映し、横方向最適系列を演算する。ここで、前後方向系列を考慮するため、式（１）を外部からのパラメータｑｋに依存するように書き直す。すなわち、下記の式（７２）のように書き直す。
【0250】
【数72】
【0251】
式（７２）では、Ａ、Ｂｕ、Ｂｗ、Ｃ、Ｄｕ、Ｄｗの行列が、確率変数ξｋと外部からの確定的なパラメータｑｋに依存していることを表している。本開示の場合、ｑｋは前後方向制御部２３からの前後方向系列に相当する。より具体的には、式（６４）のように状態方程式が速度ｖｘに依存しているため、ｑｋをホライズン間のｖｘとみなすことに相当する。
【0252】
この場合、確率系評価関数変換部１３で使用される式（２４）および式（４１）の写像を変更する必要がある。例えば、式（２４）の場合、ＧＡがｑｋに依存するようになる。すなわち、下記の式（７３）となる。
【0253】
【数73】
【0254】
ＧＡ（ｑｋ）の計算方法については、式（２１），（２２），（２３）の計算の過程において、Ａ０（ξｋ）ではなくＡ０（ξｋ，ｑｋ）として計算すれば良い。ＧＡ（ｑｋ）については、ｑｋが得られた際に逐次計算するとしても良いが、計算に時間がかかるためオンライン処理に向かない。そのため、事前にｑｋの範囲、例えば０［ｋｍ／ｈ］から１００［ｋｍ／ｈ］の範囲でしか制御しない場合には、その速度間をＮｑ点で分割し、その分割された点をｑ１，ｑ２，．．．，ｑＮｑとし、それぞれの分割点でのＧＡ（ｑ）のマップをオフライン処理で求める。オンライン処理では、オフライン処理で求めたマップを用いて内挿等により効率的にＧＡ（ｑｋ）を用いることができる。これにより、ｑｋに応じて写像を定義することができる。なお、ここではｉ．ｉ．ｄ．の場合の写像について述べたが、ＨＭＭの場合の写像についても同様の方法でｑｋに応じて写像を定義することができる。
【0255】
制御決定部２７は、前後方向系列および横方向最適系列から、移動体制御量を出力する。具体的には、実施の形態１と同様に、目標経路や目標軌道、移動体のアクチュエータへの指令値を出力する。
【0256】
実施の形態３に係る構成により、移動体の横方向のみではなく、前後方向も同時に制御可能な移動体制御装置を提供することができる。
【0257】
＜構成例２＞
図１２は、実施の形態３に係る制御演算部２８の構成例２を示す。この構成では、前後方向の制御に確率モデル予測制御を適用することによって、さらに性能向上が可能な移動体制御装置を提供する。構成例２によって、加速度の変化を抑えるなど、乗り心地を向上させることができる。構成例１との違いは、前後方向制御部２９が確率系最適化演算部３０と前後方向制御決定部３１で構成されている点である。本構成では、前後方向制御部２９においても確率モデル予測制御を用いることが特徴となる。
図１２は、実施の形態３に係る制御演算部２８の構成例２を示す。この構成では、前後方向の制御に確率モデル予測制御を適用することによって、さらに性能向上が可能な移動体制御装置を提供する。構成例２によって、加速度の変化を抑えるなど、乗り心地を向上させることができる。構成例１との違いは、前後方向制御部２９が確率系最適化演算部３０と前後方向制御決定部３１で構成されている点である。本構成では、前後方向制御部２９においても確率モデル予測制御を用いることが特徴となる。
【0258】
確率系最適化演算部３０は、参照経路、障害物情報、状態量を用いて、前後方向最適系列を出力する。
【0259】
移動体８の前後方向の状態方程式は、目標加速度ｕａから車速ｖｘまでの連続時間状態方程式を、時定数Ｔａの一次遅れ系としてモデル化すると、前後方向加速度αｘおよび重力加速度ｇ、参照経路の勾配θａを用いて、下記の式（７４）のように表される。
【0260】
【数74】
【0261】
ここで、ｘａ、Ａａ、およびＢａｕのそれぞれは、下記の式（７５）で表される。
【0262】
【数75】
【0263】
なお、θａは地図データベースなどに含まれるものとする。
【0264】
実施の形態１で説明した横方向の連続時間状態方程式の場合と同様に、サンプリング間隔をｈｋとした場合の式（６７）を用いた離散化の後、式（６８）のような出力を考慮し、式（２）の評価関数を構築すれば良い。
【0265】
このとき、評価関数のＱについてαｘを抑えるように調整を行えば、乗り心地を向上させることができる。また、障害物が前方にある場合には、制約条件を適切に設定することによって、障害物の手前で停止させることや、障害物が移動している場合はその速度に追従させるなどの挙動を実現することができる。
【0266】
確率系最適化演算部３０のその他の処理については、実施の形態１に説明した内容と同様であり、ホライズンＮ間の移動体８の前後方向最適系列ＵａＮが出力される。
【0267】
前後方向制御決定部３１は、前後方向最適系列から、前後方向系列を出力する機能を有する。具体的には、前後方向最適系列ＵａＮ、式（７４）、および式（５１）を用いて、ホライズンＮ間の速度の系列を出力する。
【0268】
また、本開示では、式（７５）のような状態方程式を使用したが、移動体８に合わせて変更する必要がある。
【0269】
実施の形態３の構成により、前後方向の乗り心地を考慮可能な移動体制御装置を提供することができる。
【0270】
＜ハードウェア構成＞
実施の形態１で説明した移動体制御装置１における状態量推定部２、参照経路生成部３、および制御演算部４の各機能は、処理回路により実現される。すなわち、移動体制御装置１は、状態量を推定し、参照経路を生成し、移動体制御量を演算するための処理回路を備える。処理回路は、専用のハードウェアであってもよく、メモリに格納されるプログラムを実行するプロセッサ（ＣＰＵ、中央処理装置、処理装置、演算装置、マイクロプロセッサ、マイクロコンピュータ、ＤＳＰ（Digital Signal Processor）ともいう）であってもよい。
実施の形態１で説明した移動体制御装置１における状態量推定部２、参照経路生成部３、および制御演算部４の各機能は、処理回路により実現される。すなわち、移動体制御装置１は、状態量を推定し、参照経路を生成し、移動体制御量を演算するための処理回路を備える。処理回路は、専用のハードウェアであってもよく、メモリに格納されるプログラムを実行するプロセッサ（ＣＰＵ、中央処理装置、処理装置、演算装置、マイクロプロセッサ、マイクロコンピュータ、ＤＳＰ（Digital Signal Processor）ともいう）であってもよい。
【0271】
処理回路が専用のハードウェアである場合、図１３に示すように、処理回路１００は、例えば、単一回路、複合回路、プログラム化したプロセッサ、並列プログラム化したプロセッサ、ＡＳＩＣ（Application Specific Integrated Circuit）、ＦＰＧＡ（Field Programmable Gate Array）、またはこれらを組み合わせたものが該当する。状態量推定部２、参照経路生成部３、および制御演算部４の各機能をそれぞれ処理回路１００で実現してもよく、各機能をまとめて１つの処理回路１００で実現してもよい。
【0272】
処理回路１００が図１４に示すプロセッサ２００である場合、状態量推定部２、参照経路生成部３、および制御演算部４の各機能は、ソフトウェア、ファームウェア、またはソフトウェアとファームウェアとの組み合わせにより実現される。ソフトウェアまたはファームウェアは、プログラムとして記述され、メモリ２０１に格納される。プロセッサ２００は、メモリ２０１に記録されたプログラムを読み出して実行することにより、各機能を実現する。すなわち、移動体制御装置１は、状態量を推定するステップ、参照経路を生成するステップ、移動体制御量を演算するステップが結果的に実行されることになるプログラムを格納するためのメモリ２０１を備える。また、これらのプログラムは、状態量推定部２、参照経路生成部３、および制御演算部４の手順または方法をコンピュータに実行させるものであるともいえる。ここで、メモリとは、例えば、ＲＡＭ（Random Access Memory）、ＲＯＭ（Read Only Memory）、フラッシュメモリ、ＥＰＲＯＭ（Erasable Programmable Read Only Memory）、ＥＥＰＲＯＭ（Electrically Erasable Programmable Read Only Memory）等の不揮発性または揮発性の半導体メモリ、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ＤＶＤ（Digital Versatile Disc）等、または、今後使用されるあらゆる記憶媒体であってもよい。
【0273】
なお、状態量推定部２、参照経路生成部３、および制御演算部４の各機能について、一部の機能を専用のハードウェアで実現し、他の機能をソフトウェアまたはファームウェアで実現するようにしてもよい。
【0274】
このように、処理回路は、ハードウェア、ソフトウェア、ファームウェア、またはこれらの組み合わせによって、上述の各機能を実現することができる。
【0275】
【0276】
なお、本開示の範囲内において、各実施の形態を自由に組み合わせたり、各実施の形態を適宜、変形、省略したりすることが可能である。
【0277】
本開示は詳細に説明されたが、上記した説明は、すべての態様において、例示であって、限定的なものではない。例示されていない無数の変形例が想定され得るものと解される。
【符号の説明】
【0278】
１ 移動体制御装置、２ 状態量推定部、３ 参照経路生成部、４ 制御演算部、５ 内界センサ、６ 位置姿勢推定装置、７ 地図データベース、８ 移動体、９ 指令値演算部、１０ アクチュエータ、１１ 確率系最適化演算部、１２ 制御決定部、１３ 確率系評価関数変換部、１４ 確定系最適化演算部、１５ 移動体制御装置、１６ 障害物移動予測部、１７ 制御演算部、１８ 障害物センサ、１９ 確率系最適化演算部、２０ 制約演算部、２１ 確定系最適化演算部、２２ 制御演算部、２３ 前後方向制御部、２４ 速度計画部、２５ 横方向制御部、２６ 確率系最適化演算部、２７ 制御決定部、２８ 制御演算部、２９ 前後方向制御部、３０ 確率系最適化演算部、３１ 前後方向制御決定部、５０ ステアリングホイール、５１ ステアリング軸、５２ 電動モータ、５３ ラックアンドピニオン機構、５４ タイロッド、５５ フロントナックル、５６ 車両駆動装置、５７ シャフト、５８ 加減速制御装置、５９ ブレーキ制御装置、６０ ブレーキ、６１ 操舵制御装置、６２ ピニオン軸、６３ ラック軸、６４ 前輪、６５ 後輪、７０ 車速センサ、７１ ＩＭＵセンサ、７２ 操舵角センサ、７３ 操舵トルクセンサ、８０ カメラ、１００ 処理回路、２００ プロセッサ、２０１ メモリ。
【要約】
本開示は、移動体の位置および姿勢が確率的な場合においても、参照経路への追従性を高くすることが可能な移動体制御装置を提供することを目的とする。本開示による移動体制御装置は、移動体を制御するための移動体制御量を演算する制御演算部を備え、制御演算部は、確率分布をもつ確率的なパラメータに依存する移動体の動特性をモデル化した状態方程式を制約条件とし、評価関数に基づく確率計画問題を逐次解くことによって最適系列を演算する確率系最適化演算部と、最適系列を移動体制御量に変換する制御決定部とを有し、確率系最適化演算部は、確率的なパラメータの確率過程モデルに基づいて、確率的な評価関数を確定的な評価関数に変換する確率系評価関数変換部と、確定的な最適化問題を解く確定系最適化演算部とを有する。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】