JP7130573B2 - 移動体制御装置、移動体、移動体制御方法及びプログラム - Google Patents

Description

本発明は、移動体制御装置、移動体、移動体制御方法及びプログラムに関する。

特許文献１には、軌道系交通システムにおいて、きめ細やかな操舵制御を可能とする自動操舵機構が開示されている。具体的には、特許文献１に記載のコントローラは、出発前に、出発地点から目的地点までの軌道条件、走行条件および車両条件から、軌道の変化点ごとに前後輪のパターン操舵角を表にしたパターン操舵角テーブルを作成し、出発点からの距離から当該パターン操舵角に基づいて舵角指令を算出している。

特開２００９－０２３５０９号公報

従来技術においては、一定速での走行を前提とし、運動学（幾何学的関係）を用いた操舵角のみをフィードフォワード操舵角として演算しているものがある。この場合、予め記憶した移動経路の地図情報に基づいて操舵角指令を算出し、各輪の操舵角を制御するので、走行経路の変化点においても予め舵取りされるから、安定した舵取り乃至走行を行うことができる。しかしながら、このような算出方法では、例えば、移動体が加減速する場合など、動力学が必要な操舵には未対応のため、フィードフォワード操舵角の精度に誤差が生じることが想定される。

本発明の目的は、動力学を考慮した、より精度の高いフィードフォワード操舵角の演算が可能な移動体制御装置、移動体、移動体制御方法及びプログラム提供することにある。

本発明の一態様によれば、移動体制御装置は、予め定められた軌道に沿って走行する移動体を制御する移動体制御装置であって、台車位置及び車体質量を含む移動体固有情報、移動体位置、移動体速度、及び、前記軌道の位置ごとの曲率を含む軌道情報を取得する情報取得部と、前記移動体固有情報、前記移動体位置、前記移動体速度、及び、前記軌道情報を用いて操舵角を演算するフィードフォワード操舵角演算部と、を備え、前記フィードフォワード操舵角演算部は、前記台車位置と、前記移動体位置における前記軌道の曲率との幾何学的関係に基づいて算出される運動学成分と、前記台車位置、前記車体質量、前記移動体速度、及び、前記移動体位置における前記軌道の曲率及び曲率変化率を含む運動方程式に基づいて算出される動力学成分との和によって前記操舵角を演算する。

本発明の一態様によれば、移動体制御装置は、前記軌道情報は、更に、前記軌道の位置ごとのバンク角を含み、前記フィードフォワード操舵角演算部は、更に、前記バンク角に応じて前記移動体の左右方向に作用する力を用いて前記動力学成分を算出する。

本発明の一態様によれば、移動体制御装置は、現時点における前記軌道に対する前記移動体のずれ量を取得して、当該ずれ量が小さくなるような操舵角を演算するフィードバック操舵角演算部をさらに備える。

本発明の一態様によれば、前記フィードフォワード操舵角演算部は、後台車の位置における前記軌道の接線である第１接線に対し、前台車の位置における前記軌道の接線である第２接線がなす角度と、前記第１接線に直交する方向における前記後台車と前記前台車との間の距離とに基づいて、前記運動学成分を算出する。

本発明の一態様によれば、移動体は、上述の移動体制御装置を備える。

本発明の一態様によれば、移動体制御方法は、予め定められた軌道に沿って走行する移動体を制御する方法であって、台車位置及び車体質量を含む移動体固有情報、移動体位置、移動体速度、及び、前記軌道の位置ごとの曲率を含む軌道情報を取得するステップと、前記移動体固有情報、前記移動体位置、前記移動体速度、及び、前記軌道情報を用いて操舵角を演算するステップと、を有し、前記操舵角を演算するステップは、前記台車位置と、前記移動体位置における前記軌道の曲率との幾何学的関係に基づいて算出される運動学成分と、前記台車位置、前記車体質量、前記移動体速度、及び、前記移動体位置における前記軌道の曲率及び曲率変化率を含む運動方程式に基づいて算出される動力学成分との和によって前記操舵角を演算するステップを有する。

本発明の一態様によれば、プログラムは、予め定められた軌道に沿って走行する移動体を制御する移動体制御装置のコンピュータに、台車位置及び車体質量を含む移動体固有情報、移動体位置、移動体速度、及び、前記軌道の位置ごとの曲率を含む軌道情報を取得するステップと、前記移動体固有情報、前記移動体位置、前記移動体速度、及び、前記軌道情報を用いて操舵角を演算するステップと、を実行させる。前記操舵角を演算するステップは、前記台車位置と、前記移動体位置における前記軌道の曲率との幾何学的関係に基づいて算出される運動学成分と、前記台車位置、前記車体質量、前記移動体速度、及び、前記移動体位置における前記軌道の曲率及び曲率変化率を含む運動方程式に基づいて算出される動力学成分との和によって前記操舵角を演算するステップを有する。

上述の発明の各態様によれば、動力学を考慮した、より精度の高いフィードフォワード操舵角の演算が可能となる。

第１の実施形態に係る移動体の全体構成を示す図である。 第１の実施形態に係る移動体の機能構成を示す第１の図である。 第１の実施形態に係る移動体の機能構成を示す第２の図である。 第１の実施形態に係る移動体の機能構成を示す第３の図である。 第１の実施形態に係るコントローラの処理フローを示す図である。 第１の実施形態に係るコントローラの処理を詳細に説明するための図である。 第２の実施形態に係るコントローラの処理を詳細に説明するための図である。

＜第１の実施形態＞
以下、第１の実施形態に係る移動体制御装置、及び、これを備える移動体について、図１～図６を参照しながら説明する。

（移動体の全体構成）
図１は、第１の実施形態に係る移動体の全体構成を示す図である。

移動体１は、軌道系交通システムに用いられる移動体（車両）であって、予め定められた軌道Ｗ１上を自動で走行する。軌道Ｗ１は、移動体１の現在位置から目的地までの移動経路Ｗに沿って予め規定されている。

図１に示すように、移動体１は、移動体筐体１Ａと、前側の左右輪である前台車ＴＦｌ、ＴＦｒと、後側の左右輪である後台車ＴＲｌ、ＴＲｒとを備える。移動体１は、前台車ＴＦｌ、ＴＦｒ、及び、後台車ＴＲｌ、ＴＲｒを個別に操舵可能とされている。

移動体筐体１Ａは、コントローラ１０（移動体制御装置）と、アクチュエータ１１Ｆ、１１Ｒと、操舵角センサ１２Ｆ、１２Ｒと、速度検知装置１３と、位置検知装置１４と、変位センサ１５Ｆ、１５Ｒとを有している。

コントローラ１０は、移動体１の走行制御全体を司る。コントローラ１０の機能の詳細については後述する。
アクチュエータ１１Ｆは、前台車ＴＦｌ、ＴＦｒの操舵機構である。アクチュエータ１１Ｒは、後台車ＴＲｌ、ＴＲｒの操舵機構である。アクチュエータ１１Ｆ、１１Ｒは、コントローラ１０から出力される操舵角指令に従って動作する。
操舵角センサ１２Ｆ、１２Ｒは、アクチュエータ１１Ｆ、１１Ｒがなす操舵角を検出するセンサである。
速度検知装置１３は、いわゆる速度センサであって、移動体１の走行速度を検知する。
位置検知装置１４は、例えば、ＧＮＳＳ（Global Navigation Satellite System）によって提供される測位情報から移動体１の現在位置を検知する。
変位センサ１５Ｆ、１５Ｒは、それぞれ、移動体筐体１Ａの前側及び後側に設置され、設置位置における軌道Ｗ１からのずれ量（変位）を検知するセンサである。

（移動体の機能構成）
図２～図４は、それぞれ、第１の実施形態に係る移動体の機能構成を示す図である。
図２に示すように、コントローラ１０は、ＣＰＵ１００と、接続インタフェース１０１と、メモリ１０２と、ストレージ１０３とを備えている。

ＣＰＵ１００は、予め用意されたプログラムに従って動作するプロセッサである。ＣＰＵ１００が実行する処理については後述する。
接続インタフェース１０１は、上述した各種アクチュエータ、センサ類との接続インタフェースである。
メモリ１０２は、いわゆる主記憶装置であって、ＣＰＵ１００がプログラムに従って動作するための命令、データ等が展開される。
ストレージ１０３は、いわゆる補助記憶装置であって、例えば、ＨＤＤ（Hard Disk Drive）、ＳＳＤ（Solid State Drive）等である。本実施形態においては、このストレージ１０３には、予め、移動体固有情報及び軌道情報が記録されている。

「移動体固有情報」とは、移動体１に関する固有の情報であって、当該移動体１の普遍的な値を示す情報である。具体的には、移動体固有情報には、移動体１の重さ（車体質量）が含まれる。また、移動体固有情報には、移動体筐体１Ａの重心と、前台車ＴＦｌ、ＴＦｒとの位置関係、及び、移動体筐体１Ａの重心と、後台車ＴＲｌ、ＴＲｒとの位置関係を示す情報（以下、「台車位置」とも表記する。）が含まれる。

「軌道情報」とは、軌道Ｗ１のスタートからゴールまでを所定の単位距離で分節してなる軌道Ｗ１上の位置ごとに紐づく情報群である。具体的には、図３に示すように、軌道情報は、軌道Ｗ１上の各位置（ｃ０、ｃ１、・・）における曲率（ｓ０、ｓ１、・・）、曲率変化率（ｓｘ０、ｓｘ１、・・）、及び、バンク角（φ０、φ１、・・）を示している。なお、曲率変化率とは、軌道Ｗ１における単位距離当たりの曲率（ｓ０、ｓ１、・・）の変化率である。他の実施形態に係るコントローラ１０は、軌道上の位置（ｃ０、ｃ１、・・）と、曲率（ｓ０、ｓ１、・・）との関係から曲率変化率を演算して取得する態様であってもよい。

図４を参照しながら、ＣＰＵ１００が有する機能について詳しく説明する。
ＣＰＵ１００は、予め用意されたプログラムに従って動作することで、以下の、情報取得部１０００、フィードフォワード操舵角演算部１００１、フィードバック操舵角演算部１００２、及び、操舵角指令出力部１００３としての機能を発揮する。

情報取得部１０００は、軌道Ｗ１を走行する移動体１に関する各種情報を逐次取得する。具体的には、情報取得部１０００は、ストレージ１０３を参照して、移動体固有情報及び軌道情報を取得する。また、情報取得部１０００は、速度検知装置１３を介して移動体１の走行速度（以下、「移動体速度」とも表記する。）を取得する。また、情報取得部１０００は、位置検知装置１４を介して移動体１の現在位置（以下、「移動体位置」とも表記する。）を取得する。
また、情報取得部１０００は、変位センサ１５Ｆ、１５Ｒを介して、各々の設置位置における現時点の軌道Ｗ１からのずれ量（変位）を取得する。また、情報取得部１０００は、操舵角センサ１２Ｆ、１２Ｒを介して、アクチュエータ１１Ｆ、１１Ｒそれぞれにおける現時点の操舵角を取得する。

フィードフォワード操舵角演算部１００１は、情報取得部１０００が取得した各種情報（移動体固有情報、移動体位置、移動体速度、及び、軌道情報）を用いて、フィードフォワードで、アクチュエータ１１Ｆ、１１Ｒそれぞれに指令すべき操舵角を演算する。

図４に示すように、フィードフォワード操舵角演算部１００１は、更に、運動学成分演算部１００１ａと、動力学成分演算部１００１ｂとしての機能を有する。
運動学成分演算部１００１ａは、移動体固有情報に示される「台車位置」と、「移動体位置」における軌道Ｗ１の「曲率」との幾何学的関係に基づいて、指令すべき操舵角の運動学成分を算出する。
動力学成分演算部１００１ｂは、移動体固有情報に示される「台車位置」、「車体質量」、「移動体速度」、及び、「移動体位置」における軌道Ｗ１の「曲率」を含む運動方程式に基づいて、指令すべき操舵角の動力学成分を算出する。

フィードバック操舵角演算部１００２は、変位センサ１５Ｆ、１５Ｒ、及び、操舵角センサ１２Ｆ、１２Ｒを介して、現時点におけるずれ量と操舵角とを取得し、操舵角のフィードバック制御を行う。具体的には、フィードバック操舵角演算部１００２は、現時点における軌道Ｗ１に対する移動体１のずれ量を取得し、次の時刻に適用すべき操舵角として当該ずれ量が小さくなるような操舵角を演算する。

操舵角指令出力部１００３は、フィードフォワード操舵角演算部１００１によって演算されたフィードフォワード操舵角と、フィードバック操舵角演算部１００２によって演算されたフィードバック操舵角との合計を示す操舵角指令を、アクチュエータ１１Ｆ、１１Ｒに向けて出力する。

（移動体制御装置の処理フロー）
図５は、第１の実施形態に係るコントローラの処理フローを示す図である。
図６は、第１の実施形態に係るコントローラの処理の詳細な説明に用いる図である。
以下、図５、図６を参照しながら、コントローラが実行する各処理について詳しく説明する。

まず、ＣＰＵ１００の情報取得部１０００は、ストレージ１０３及び速度検知装置１３、位置検知装置１４を通じて各種情報を取得する（ステップＳ０１）。具体的には、情報取得部１０００は、「移動体位置」、「移動体速度」、移動体固有情報に含まれる「台車位置」、「車体質量」、及び、軌道情報（図３）を取得する。

次に、ＣＰＵ１００のフィードフォワード操舵角演算部１００１は、ステップＳ０１に基づいて導出されるフィードフォワード操舵角を演算する（ステップＳ０２）。このステップＳ０２において、運動学成分演算部１００１ａは、フィードフォワード操舵角の運動学成分を演算し（ステップＳ０２ａ）、動力学成分演算部１００１ｂは、フィードフォワード操舵角の動力学成分を演算する（ステップＳ０２ｂ）。

ステップＳ０２ａ及びステップＳ０２ｂの処理の内容について、図６を参照しながら詳しく説明する。

図６は、移動体１が、軌道Ｗ１の一部をなす半径Ｒの円軌道上を走行している場合における、移動体１と軌道Ｗ１との幾何学的関係を示している。図６は、移動体１の前後方向を±Ｘ方向で表し、移動体１の左右方向（走行面において前後方向に直交する方向）を±Ｙ方向で表し、移動体１の上下方向（走行面に直交する方向）を±Ｚ方向で表している。

以下の説明において、移動体１の前側左右に設置された２つの前台車ＴＦｌ、ＴＦｒは、各々の左右方向における中心（即ち、車両中心軸Ｅ上）に設置された単一の仮想的な前台車ＴＦｉに置き換えて説明する。また、以下の説明において、移動体１の後側左右に設置された２つの後台車ＴＲｌ、ＴＲｒを、各々の左右方向における中心位置（即ち、車両中心軸Ｅ上）に設置された単一の仮想的な後台車ＴＲｉに置き換えて説明する。

なお、図６に示す前台車距離ｌｆは、移動体筐体１Ａの重心Ｃから前台車ＴＦｉまでの距離である。また、図６に示す後台車距離ｌｒは、移動体筐体１Ａの重心Ｃから後台車ＴＦｒまでの距離である。また、図６に示す台車間距離Ｌは、前台車ＴＦｉから後台車ＴＦｒまでの距離であって、本実施形態においては、前台車距離ｌｆと後台車距離ｌｒとの合計に一致する（Ｌ＝ｌｆ＋ｌｒ）。前台車距離ｌｆ及び後台車距離ｌｒは、移動体固有情報として予めストレージ１０３に記録されている。

また、半径Ｒの円軌道上を走行する移動体１の旋回半径ｒは、円軌道の原点Ｏから移動体筐体１Ａの重心Ｃまでの距離である。なお、旋回半径ｒは、円軌道の半径Ｒと、前台車距離ｌｆ又は後台車距離ｌｒとの幾何学的関係から算出可能なパラメータである。更に、円軌道の半径Ｒは、当該円軌道の曲率ｓに基づいて算出可能である（Ｒ＝１／ｓ）。

一般に、台車の操舵角を“δ”とし、台車の取り付け位置における速度ベクトルが車両中心軸Ｅに対してなす角度を“δｋ”としたとき、操舵角δ、角度δｋと、台車が発生する横力ｆ（±Ｙ方向に作用する力）との関係は、式（１）で表される。

式（１）を変形すると、台車に指令すべき操舵角は、式（２）で表現できる。

式（１）、（２）の“Ｋ”は、すべり角（δ－δｋ）に応じて一つの車輪が発生させる横力ｆの比例係数Ｋ（コーナリングパワー）である。本実施形態において、前台車、後台車は、それぞれ左右２つずつあるので式（１）、（２）では“２Ｋ”としている。なお、この比例係数Ｋは、移動体固有情報に含まれる情報の一つであって、予めストレージ１０３に記録されている既知の情報である。

前台車ＴＦｉについての操舵角δを“δｆ”とし、後台車ＴＲｉについての操舵角δを“δｒ”として式（２）に従えば、操舵角δｆ、δｒは、式（３）で表される。

式（３）における“δｆｋ”は、前台車ＴＦｉの取り付け位置における速度ベクトルが車両中心軸Ｅに対してなす角度である。また、式（３）における“δｒｋ”は、後台車ＴＲｉの取り付け位置における速度ベクトルが車両中心軸Ｅに対してなす角度である（図６参照）。
また、式（３）における“ｆｆ”は、前台車ＴＦｉがその取り付け位置に発生させる横力であり、式（３）における“ｆｒ”は、後台車ＴＲｉがその取り付け位置に発生させる横力である。式（３）に示すように、“ｆｆ／２Ｋ”を“δｆｄ”とおき、“ｆｒ／２Ｋ”を“δｒｄ”とおくと、前台車ＴＦｉ、後台車ＴＲｉのそれぞれに与えるべき操舵角δｆ、δｒは、力に依存しない成分（幾何学的関係から定まる成分）である運動学成分δｆｋ、δｒｋと、力に依存する成分である動力学成分δｆｄ、δｒｄとに分けることができる。

（運動学成分の算出手順）
運動学成分演算部１００１ａによる運動学成分δｆｋ、δｒｋの算出手順（ステップＳ０２ａ）について説明する。

運動学成分δｆｋ、δｒｋは、移動体１が存在する位置における軌道Ｗ１の曲率ｓ（＝１／Ｒ）を用いて式（４）で表すことができる。

そこで、運動学成分演算部１００１ａは、ステップＳ０２ａにおいて、軌道情報（図３）を参照し、位置検知装置１４を介して取得した「移動体位置」（現在位置）に対応する曲率ｓを特定する。そして、運動学成分演算部１００１ａは、特定した曲率ｓと、既知の台車間距離Ｌとを式（４）に代入して運動学成分δｆｋ、δｒｋを演算する。

（動力学成分の算出手順）
動力学成分演算部１００１ｂによる動力学成分δｆｄ、δｒｄの算出手順（ステップＳ０２ｂ）について説明する。

移動体１が図６に示す軌道Ｗ１に沿って移動するとき、移動体筐体１Ａの左右方向（±Ｙ方向）についての並進の運動方程式（式（５））と、重心Ｃ周りの回転の運動方程式（式（６））が成り立つ。なお、文章中においては、“θｄｅｓ”に１つのドットが付された記号を“θｄｅｓ（・）”と表記し、“θｄｅｓ”に２つのドットが付された記号を“θｄｅｓ（・・）”と表記する。

式（５）における“θｄｅｓ（・）”は、移動体１のヨー角速度である。式（５）左辺の“ｍｒθｄｅｓ（・）”は、車体質量ｍの移動体筐体１Ａが旋回半径ｒで旋回する際に、重心Ｃから原点Ｏに向かって発生する向心力である。即ち、式（５）に示すように、移動体筐体１Ａに発生する向心力“ｍｒθｄｅｓ（・）”は、前台車ＴＦｉが発生させる横力ｆｆと、後台車ＴＲｉが発生させる横力ｆｒとの合計と釣り合う。

式（６）における“θｄｅｓ（・・）”は、移動体１のヨー角加速度である。また、式（６）における“Ｉ”は、移動体筐体１Ａの慣性モーメントであり、車体質量ｍ、旋回半径ｒより算出可能である（Ｉ＝ｍｒ＾２）。式（６）左辺の“Ｉθｄｅｓ（・・）”は、移動体筐体１Ａに作用するトルクである。即ち、式（６）に示すように、移動体筐体１Ａを回転させるトルク“Ｉθｄｅｓ（・・）”は、前台車ＴＦｉがその取り付け位置に作用させる横力ｆｆに基づくトルク（ｌｆ・ｆｆ）と、後台車ＴＲｉがその取り付け位置に作用させる横力ｆｒに基づくトルク（－ｌｒ・ｆｒ）との合計と一致する。

更に、ヨー角速度θｄｅｓ（・）、及び、ヨー角加速度θｄｅｓ（・・）は、それぞれ、式（７）、式（８）で表される。

式（７）における“ｖ”は、移動体速度（走行速度）である。動力学成分演算部１００１ｂは、ステップＳ０２ｂにおいて、軌道情報（図３）を参照し、位置検知装置１４を介して取得した移動体位置（現在位置）に対応する曲率ｓを特定する。そして、動力学成分演算部１００１ｂは、速度検知装置１３を介して取得した移動体速度ｖと、軌道情報から特定した曲率ｓとを式（７）に代入することで、ヨー角速度θｄｅｓ（・）を算出する。

式（８）における“ｓｘ”は、曲率変化率である。また、式（８）における“ａ”は、移動体加速度（走行方向の加速度）である。移動体加速度ａは、速度検知装置１３を介して取得した移動体速度ｖを時間微分することで算出可能である。
式（８）における“ｓｘ”は、曲率変化率である。動力学成分演算部１００１ｂは、ステップＳ０２ｂにおいて、軌道情報（図３）を参照し、位置検知装置１４を介して取得した「移動体位置」（現在位置）に対応する曲率ｓ及び曲率変化率ｓｘを特定する。そして、動力学成分演算部１００１ｂは、速度検知装置１３を介して取得（算出）した移動体速度ｖ（移動体加速度ａ）と、軌道情報から特定した曲率ｓ、曲率変化率ｓｘとを式（８）に代入することで、ヨー角加速度θｄｅｓ（・・）を算出する。

式（５）及び式（６）で示した運動方程式を、前台車ＴＦｉの横力ｆｆ、及び、後台車ＴＲｉの横力ｆｒそれぞれについて解くと、式（９）となる。

ただし、式（９）では、“ｌｆ＋ｌｒ”を“Ｌ”とおいている。

次に、軌道Ｗ１のバンク角φを考慮して、前台車ＴＦｉ及び後台車ＴＲｉのそれぞれが、実際に発生すべき力ｆｆＴｉｒｅ、ｆｒＴｉｒｅを求める。前台車ＴＦｉ及び後台車ＴＲｉのそれぞれが実際に発生すべき力ｆｆＴｉｒｅ、ｆｒＴｉｒｅは、横力ｆｆ、ｆｒから、軌道Ｗ１のバンク角φによって発生する力ｆｆＢａｎｋ、ｆｒＢａｎｋを差し引くことで求められる。具体的には、前台車ＴＦｉ及び後台車ＴＲｉのそれぞれが実際に発生すべき力ｆｆＴｉｒｅ、ｆｒＴｉｒｅは、式（１０）のように表される。

式（１０）において、前台車ＴＦｉの取り付け位置における軌道Ｗ１のバンク角を“φｆ”とし、後台車ＴＲｉの取り付け位置における軌道Ｗ１のバンク角を“φｒ”としている。軌道Ｗ１のバンク角φによって発生する力ｆｆＢａｎｋ、ｆｒＢａｎｋは、移動体筐体１Ａに作用する重力（車体質量ｍと重力加速度ｇとの積）に応じて発生する左右方向の力である。

滑り角（δ－δｋ）に、コーナリングパワー“２Ｋ”を乗じたものが、前台車ＴＦｉ及び後台車ＴＲｉのそれぞれが発生すべき力ｆｆＴｉｒｅ、ｆｒＴｉｒｅである。したがって、前台車ＴＦｉに与えるべき操舵角の動力学成分δｆｄ、及び、後台車ＴＲｉに与えるべき操舵角の動力学成分δｒｄは、それぞれ、式（１１）で表される。

動力学成分演算部１００１ｂは、ステップＳ０２ｂにおいて、更に、式（１１）に、移動体１のヨー角速度θｄｅｓ（・）（式（７））、及び、ヨー角加速度θｄｅｓ（・・）（式（８））を代入することで、動力学成分δｆｄ、δｒｄを演算する。このとき、動力学成分演算部１００１ｂは、軌道情報（図３）を参照し、位置検知装置１４を介して取得した「移動体位置」（現在位置）に対応する前台車ＴＦｉのバンク角φｆ及び後台車ＴＲｉのバンク角φｒを特定する。

以上のようにして、フィードフォワード操舵角演算部１００１は、式（４）に基づいて操舵角δｆ、δｒの運動学成分δｆｋ、δｒｋを算出し、式（１１）に基づいて動力学成分δｆｄ、δｒｄを算出する。

図５に戻り、次に、フィードバック操舵角演算部１００２は、変位センサ１５Ｆ、１５Ｒを介して取得した変位と、操舵角センサ１２Ｆ、１２Ｒを介して取得した現時点の操舵角を取得して、フィードバック操舵角を演算する（ステップＳ０３）。本実施形態に係るフィードバック操舵角演算部１００２が実行するフィードバック制御は、既知の技術で実現可能であるため、詳細な説明を省略する。

次に、操舵角指令出力部１００３は、フィードフォワード操舵角演算部１００１によって算出されたフィードフォワード操舵角（δｆ、δｄ）と、フィードバック操舵角演算部１００２によって算出されたフィードバック操舵角とを統合し、新たな操舵角指令として、アクチュエータ１１Ｆ、１１Ｒに向けて出力する（ステップＳ０４）。

（作用、効果）
以上に説明した通り、第１の実施形態に係るコントローラ１０（移動体制御装置）は、移動体固有情報（台車位置ｌｆ、ｌｒ、車体質量ｍ、コーナリングパワーＫ）、移動体位置、移動体速度、及び、軌道情報（図３）を用いて、前台車ＴＦｉ、後台車ＴＲｉそれぞれに与えるべき操舵角δｆ、δｒを演算するフィードフォワード操舵角演算部１００１を具備することを特徴とする。そして、このフィードフォワード操舵角演算部１００１は、台車位置ｌｆ、ｌｒと、移動体位置（現在位置）における軌道Ｗ１の曲率ｓとの幾何学的関係に基づいて算出される運動学成分と、台車位置、車体質量ｍ、移動体速度ｖ、及び、移動体位置（現在位置）における軌道の曲率ｓ及び曲率変化率ｓｘを含む運動方程式に基づいて算出される動力学成分との和によって操舵角δｆ、δｄを演算する。

本実施形態に係るコントローラ１０は、上記のような特徴を有することで、以下のような効果を奏する。即ち、運動学成分（幾何学的関係に基づく操舵角）だけでなく、移動体１に作用する力に応じた成分である動力学成分を考慮して操舵角を算出することができる。これにより、移動体１が加減速した場合や、軌道Ｗ１にバンク角が存在する場合も、加速度やバンク角に応じた適切な操舵角を算出することができる。また、通常の軌道は、直線軌道（曲率ｓ＝０）の軌道と、曲率ｓ＝ｓ０（ｓ０＞０）の円軌道との境界には、曲率ｓを０からｓ０に緩やかに変化させるための緩和曲線が設けられる。本実施形態に係るコントローラ１０によれば、この緩和曲線の特性（即ち、軌道情報曲率変化率ｓｘ）も動的に考慮される。

＜第２の実施形態＞
次に、第２の実施形態に係る移動体制御装置、及び、これを備える移動体について、図７を参照しながら説明する。なお、第２の実施形態に係る移動体１の機能構成等については第１の実施形態と同様であるため図示を省略する。

（運動学成分の算出手順）
図７は、第２の実施形態に係るコントローラ１０の機能構成を説明するための図である。
第１の実施形態に係る運動学成分演算部１００１ａは、式（４）に基づいて操舵角の運動学成分δｆｋ、δｒｋを算出するものとして説明した。しかし、式（４）は、曲率ｓが一定の円軌道（近似曲線）を走行することを前提としているため、実際の軌道Ｗ１における曲率ｓの変化点前後では誤差が生じ得る。そこで、第２の実施形態に係る運動学成分演算部１００１ａは、以下に説明する手順で運動学成分δｆｋ、δｒｋを算出する。

図７に示すように、移動体筐体１Ａの後台車ＴＲｉの取り付け位置を、ｘｙ座標の原点Ｏとする。ｘｙ座標のｘ軸は、原点Ｏにおける軌道Ｗ１の接線に一致する。また、ｘｙ座標のｙ軸は、ｘ軸（原点Ｏにおける軌道Ｗ１の接線）に直交する。
また、移動体筐体１Ａの前台車ＴＦｉの取り付け位置を示す点Ｐは、ｘ軸の正の位置に存在するものとする。

軌道Ｗ１上の任意の位置を原点Ｏからの距離ｃで表すものとすると、軌道Ｗ１の軌道形状は、式（１２）のように定義される。

また、軌道Ｗ１上の任意の位置（ｘ、ｙ）における接線が原点Ｏにおける接線（ｘ軸）に対してなす角度を“θ”とすると、式（１３）が成り立つ。

更に、この角度θを用いて、曲率変化率ｓｘを式（１４）で表すことができる。

式（１４）によれば、軌道情報に事前に記録されている曲率変化率ｓｘを原点Ｏからの距離ｃで積分することで、その軌道Ｗ１上の任意の位置における角度θを算出することができる。

ここで、後台車ＴＲｉから前台車ＴＦｉまでの範囲では、軌道Ｗ１とｘ軸が接近しているので、“ｄｘ＝ｄｃ”と近似できる。そうすると、式（１３）に基づいて式（１５）が成り立つ。

式（１５）によれば、式（１５）の右辺“ｔａｎθ”を原点Ｏからの距離ｃで積分することで、その軌道Ｗ１上の任意の位置におけるｙ座標値を算出することができる。つまり、式（１４）及び式（１５）を用いることで、軌道情報（図３）の曲率変化率ｓｘから、軌道Ｗ１上の任意の位置におけるｙ座標の値を算出することができる。

図７によれば、原点Ｏに位置する後台車ＴＲｉの操舵角の運動学成分δｒｋは、車体中心軸Ｅを基準として角度（－Ψ）で表される。したがって、運動学成分δｒｋは、式（１６）のようにして求められる。

式（１６）における“ｙｆ”は、点Ｐ（前台車ＴＦｉの取り付け位置）のｙ座標値であって、式（１５）より“ｔａｎθ”を原点Ｏからの距離ｃ（＝台車間距離Ｌ）で積分することにより得ることができる。

また、図７によれば、点Ｐに位置する前台車ＴＦｉの操舵角の運動学成分δｆｋは、車体中心軸Ｅを基準として角度（θｆ－Ψ）で表される。ここで、“θｆ”は、点Ｐにおける角度θであって、式（１４）より曲率変化率ｓｘを原点Ｏからの距離ｃ（＝台車間距離Ｌ）で積分することにより得ることができる。したがって、前台車ＴＦｉの運動学成分δｆｋは、式（１７）で求められる。

（作用、効果）
以上の通り、第２の実施形態に係るコントローラ１０（運動学成分演算部１００１ａ）は、後台車ＴＲｉの位置（原点Ｏ）における軌道Ｗ１の接線（ｘ軸（第１接線））に対し前台車ＴＦｉの位置（点Ｐ）における軌道Ｗ１の接線（第２接線）がなす角度θｆと、ｘ軸に直交する方向（ｙ軸方向）における後台車ＴＲｉと前台車ＴＦｉとの間の距離ｙｆとに基づいて、運動学成分を算出する。
このようにすることで、軌道情報に記録された曲率変化率ｓｘに基づいて、後台車の位置に対応する軌道Ｗ１の接線、及び、前台車の位置に対応する軌道Ｗ１の接線の方向が計算されるので、円軌道の近似曲線を用いずに操舵角の運動学成分が算出される。したがって、軌道Ｗ１における曲率ｓの変化点前後で生じる誤差を低減できる。

＜第２の実施形態の変形例＞
第２の実施形態の変形例に係る移動体制御装置、及び、これを備える移動体について説明する。

変形例に係るコントローラ１０は、積分区間を分割して考える区分解析積分により、計算量を落として解くことを特徴とする。以下、詳細に説明する。

台車間距離Ｌの範囲でみれば角度θ（図７）は微小である。したがって、式（１８）のように近似することができる。

式（１８）によれば、第２の実施形態で説明した微分方程式（式（１４）、式（１５））は、式（１９）のように表される。

つまり、曲率変化率ｓｘを１回積分したものが“θ”であり、２回積分したものが“ｙ”となる。

ところで、軌道Ｗ１を構成する形状要素は、「直線」、「曲線」（円軌道）、「緩和曲線」のいずれかであるから、各形状要素の形状区間ｉにおける曲率は、以下の一次式（式（２０））で表すことができる。

式（２０）において、“ｃｉ”は各形状要素の区間入口からの相対距離であり、“ａｉ”、“ｂｉ”は、形状要素に応じて定まる定数である。例えば、形状区間ｉの形状要素が「直線」である場合、“ａｉ＝０”、“ｂｉ＝０”である。また、形状区間ｉの形状要素が「曲線」である場合、“ａｉ＝０”、“ｂｉ＝ｂｉ１”（ｂｉ１＝「曲線」の曲率に応じた定数）である。また、形状区間ｉの形状要素が「緩和曲線」である場合、“ａｉ＝ａｉ１”（ａｉ１＝「緩和曲線」の曲率変化率に応じた定数）、“ｂｉ＝ｂｉ１”（ｂｉ１＝「緩和曲線」の区間入口の曲率ｓに応じた定数）である。

後台車から前台車までの間にＮ個の形状区間ｉがあるとすると、１番目の出口の“ｙ”、“θ”は式（２１）となる。

後の区間も同様に計算して加算すれば良いから、全体の計算結果は以下の漸化式（式（２２））で表される。

ただし、“ｉ”は１～Ｎであり、ｙ０＝０、θ０＝０である。

上記変形例に係るコントローラ１０によれば、各形状要素の形状区間ｉごとの計算でよくなるため、計算精度を維持しつつ、計算量を低減することができる。これにより、例えば、オンラインで精度の高い操舵角の計算ができる。

上述の第１～第２の実施形態において、上述したコントローラ１０の各種処理の過程は、プログラムの形式でコンピュータ読み取り可能な記録媒体に記憶されており、このプログラムをコンピュータが読み出して実行することによって上記各種処理が行われる。また、コンピュータ読み取り可能な記録媒体とは、磁気ディスク、光磁気ディスク、ＣＤ－ＲＯＭ、ＤＶＤ－ＲＯＭ、半導体メモリ等をいう。また、このコンピュータプログラムを通信回線によってコンピュータに配信し、この配信を受けたコンピュータが当該プログラムを実行するようにしてもよい。

上記プログラムは、上述した機能の一部を実現するためのものであってもよい。更に、上述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるもの、いわゆる差分ファイル（差分プログラム）であってもよい。

以上のとおり、本発明に係るいくつかの実施形態を説明したが、これら全ての実施形態は、例として提示したものであり、発明の範囲を限定することを意図していない。これらの実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で種々の省略、置き換え、変更を行うことができる。これらの実施形態及びその変形は、発明の範囲や要旨に含まれると同様に、特許請求の範囲に記載された発明とその均等の範囲に含まれる。

１ 移動体
１０ コントローラ（移動体制御装置）
１００ ＣＰＵ
１０００ 情報取得部
１００１ フィードフォワード操舵角演算部
１００１ａ 運動学成分演算部
１００１ｂ 動力学成分演算部
１００２ フィードバック操舵角演算部
１００３ 操舵角指令出力部
１０１ 接続インタフェース
１０２ メモリ
１０３ ストレージ
１１Ｆ、１１Ｒ アクチュエータ
１２Ｆ、１２Ｒ 操舵角センサ
１３ 速度検知装置
１４ 位置検知装置
１５Ｆ、１５Ｒ 変位センサ

Claims (7)

1. 予め定められた軌道に沿って走行する移動体を制御する移動体制御装置であって、
   台車位置及び車体質量を含む移動体固有情報、移動体位置、移動体速度、及び、前記軌道の位置ごとの曲率を含む軌道情報を取得する情報取得部と、
   前記移動体固有情報、前記移動体位置、前記移動体速度、及び、前記軌道情報を用いて操舵角を演算するフィードフォワード操舵角演算部と、
   を備え、
   前記フィードフォワード操舵角演算部は、
   前記台車位置と、前記移動体位置における前記軌道の曲率との幾何学的関係に基づいて算出される運動学成分と、
   前記台車位置、前記車体質量、前記移動体速度、及び、前記移動体位置における前記軌道の曲率及び曲率変化率を含む運動方程式に基づいて算出される動力学成分と
   の和によって前記操舵角を演算する
   移動体制御装置。
2. 前記軌道情報は、更に、前記軌道の位置ごとのバンク角を含み、
   前記フィードフォワード操舵角演算部は、更に、前記バンク角に応じて前記移動体の左右方向に作用する力を用いて前記動力学成分を算出する
   請求項１に記載の移動体制御装置。
3. 現時点における前記軌道に対する前記移動体のずれ量を取得して、当該ずれ量が小さくなるような操舵角を演算するフィードバック操舵角演算部をさらに備える
   請求項１または請求項２に記載の移動体制御装置。
4. 前記フィードフォワード操舵角演算部は、
   後台車の位置における前記軌道の接線である第１接線に対し、前台車の位置における前記軌道の接線である第２接線がなす角度と、前記第１接線に直交する方向における前記後台車と前記前台車との間の距離とに基づいて、前記運動学成分を算出する
   請求項１から請求項３のいずれか一項に記載の移動体制御装置。
5. 請求項１から請求項４のいずれか一項に記載の移動体制御装置を備える
   移動体。
6. 予め定められた軌道に沿って走行する移動体を制御する方法であって、
   台車位置及び車体質量を含む移動体固有情報、移動体位置、移動体速度、及び、前記軌道の位置ごとの曲率を含む軌道情報を取得するステップと、
   前記移動体固有情報、前記移動体位置、前記移動体速度、及び、前記軌道情報を用いて操舵角を演算するステップと、
   を有し、
   前記操舵角を演算するステップは、
   前記台車位置と、前記移動体位置における前記軌道の曲率との幾何学的関係に基づいて算出される運動学成分と、
   前記台車位置、前記車体質量、前記移動体速度、及び、前記移動体位置における前記軌道の曲率及び曲率変化率を含む運動方程式に基づいて算出される動力学成分と
   の和によって前記操舵角を演算するステップを有する
   移動体制御方法。
7. 予め定められた軌道に沿って走行する移動体を制御する移動体制御装置のコンピュータに、
   台車位置及び車体質量を含む移動体固有情報、移動体位置、移動体速度、及び、前記軌道の位置ごとの曲率を含む軌道情報を取得するステップと、
   前記移動体固有情報、前記移動体位置、前記移動体速度、及び、前記軌道情報を用いて操舵角を演算するステップと、
   を実行させ、
   前記操舵角を演算するステップは、
   前記台車位置と、前記移動体位置における前記軌道の曲率との幾何学的関係に基づいて算出される運動学成分と、
   前記台車位置、前記車体質量、前記移動体速度、及び、前記移動体位置における前記軌道の曲率及び曲率変化率を含む運動方程式に基づいて算出される動力学成分と
   の和によって前記操舵角を演算するステップを有する
   プログラム。

Images