知財求人 - 知財ポータルサイト「IP Force」
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2024022942
(43)【公開日】2024-02-21
(54)【発明の名称】制御装置
(51)【国際特許分類】
G05B 11/36 20060101AFI20240214BHJP
G05D 1/43 20240101ALI20240214BHJP
G08G 1/16 20060101ALI20240214BHJP
【ＦＩ】
G05B11/36 505Z
G05B11/36 G
G05D1/02 H
G08G1/16 A
【審査請求】未請求
【請求項の数】12
【出願形態】ＯＬ
(21)【出願番号】P 2022126401
(22)【出願日】2022-08-08
(71)【出願人】
【識別番号】000000974
【氏名又は名称】川崎重工業株式会社
(74)【代理人】
【識別番号】100131200
【弁理士】
【氏名又は名称】河部 大輔
(72)【発明者】
【氏名】大月 智史
(72)【発明者】
【氏名】中島 健一
【テーマコード（参考）】
5H004
5H181
5H301
【Ｆターム（参考）】
5H004GA29
5H004GB12
5H004HA07
5H004HB07
5H004JA11
5H004KB01
5H004KC01
5H004LA12
5H004LB06
5H181AA01
5H181AA25
5H181BB04
5H181CC03
5H181CC04
5H181CC14
5H181LL01
5H181LL02
5H181LL04
5H181LL07
5H181LL08
5H181LL09
5H301AA01
5H301CC03
5H301CC06
5H301CC10
5H301GG07
5H301GG08
5H301GG09
5H301GG16
5H301MM09
(57)【要約】
【課題】瞬時に障害に対応して、障害を回避する。
【解決手段】移動体１を移動させるためのアクチュエータ１２の操作量を演算する制御装置１００は、移動体１を目標位置へ移動させるための指令値を設定する設定器２２と、障害を回避するための指令値の制約条件を設定する制限器２５と、指令値が制約条件によって定められる制限範囲外である場合に、指令値を制限範囲内へ補正する補正器２６と、指令値に基づいて操作量を演算する操作量演算器２８とを備える。
【選択図】図５
【解決手段】移動体１を移動させるためのアクチュエータ１２の操作量を演算する制御装置１００は、移動体１を目標位置へ移動させるための指令値を設定する設定器２２と、障害を回避するための指令値の制約条件を設定する制限器２５と、指令値が制約条件によって定められる制限範囲外である場合に、指令値を制限範囲内へ補正する補正器２６と、指令値に基づいて操作量を演算する操作量演算器２８とを備える。
【選択図】図５
【特許請求の範囲】
【請求項1】
移動体を移動させるためのアクチュエータの操作量を演算する制御装置であって、
前記移動体を目標位置へ移動させるための指令値を設定する設定器と、
障害を回避するための前記指令値の制約条件を設定する制限器と、
前記指令値が前記制約条件によって定められる制限範囲外である場合に、前記指令値を前記制限範囲内へ補正する補正器と、
前記指令値に基づいて前記操作量を演算する操作量演算器とを備える制御装置。
前記移動体を目標位置へ移動させるための指令値を設定する設定器と、
障害を回避するための前記指令値の制約条件を設定する制限器と、
前記指令値が前記制約条件によって定められる制限範囲外である場合に、前記指令値を前記制限範囲内へ補正する補正器と、
前記指令値に基づいて前記操作量を演算する操作量演算器とを備える制御装置。
【請求項2】
請求項１に記載の制御装置において、
前記制限器は、前記制約条件を逐次演算して、前記制約条件を更新する制御装置。
前記制限器は、前記制約条件を逐次演算して、前記制約条件を更新する制御装置。
【請求項3】
請求項１に記載の制御装置において、
前記指令値としての前記移動体の指令速度に基づいてサーボ制御を実行するサーボ制御器をさらに備え、
前記補正器は、前記指令速度を前記制限範囲内へ補正して最終的な指令速度を前記サーボ制御器へ出力し、
前記サーボ制御器は、前記最終的な指令速度に基づいて指令力を出力し、
前記操作量演算器は、前記指令力に基づいて前記操作量を演算する制御装置。
前記指令値としての前記移動体の指令速度に基づいてサーボ制御を実行するサーボ制御器をさらに備え、
前記補正器は、前記指令速度を前記制限範囲内へ補正して最終的な指令速度を前記サーボ制御器へ出力し、
前記サーボ制御器は、前記最終的な指令速度に基づいて指令力を出力し、
前記操作量演算器は、前記指令力に基づいて前記操作量を演算する制御装置。
【請求項4】
請求項３に記載の制御装置において、
前記制限器は、障害の回避に加え、前記サーボ制御器の制御特性に関する制約条件を設定する制御装置。
前記制限器は、障害の回避に加え、前記サーボ制御器の制御特性に関する制約条件を設定する制御装置。
【請求項5】
請求項３に記載の制御装置において、
前記制限器は、障害の回避に加え、前記アクチュエータに関する制約条件を設定する制御装置。
前記制限器は、障害の回避に加え、前記アクチュエータに関する制約条件を設定する制御装置。
【請求項6】
請求項１に記載の制御装置において、
前記制限器は、前記移動体と障害との間に所定の距離が確保されるように前記制約条件を設定する制御装置。
前記制限器は、前記移動体と障害との間に所定の距離が確保されるように前記制約条件を設定する制御装置。
【請求項7】
請求項１に記載の制御装置において、
前記制限器は、前記移動体のうち複数の保護部位が障害を回避するように、前記複数の保護部位に対応する複数の前記制約条件を設定する制御装置。
前記制限器は、前記移動体のうち複数の保護部位が障害を回避するように、前記複数の保護部位に対応する複数の前記制約条件を設定する制御装置。
【請求項8】
請求項１に記載の制御装置において、
前記制限器は、前記制約条件を調節可能に設定する制御装置。
前記制限器は、前記制約条件を調節可能に設定する制御装置。
【請求項9】
請求項１に記載の制御装置において、
補正後の前記指令値が所定の閾値未満となる場合に、
前記制限器は、前記制約条件を変更して、
前記補正器は、変更された前記制限範囲内へ前記指令値を補正する制御装置。
補正後の前記指令値が所定の閾値未満となる場合に、
前記制限器は、前記制約条件を変更して、
前記補正器は、変更された前記制限範囲内へ前記指令値を補正する制御装置。
【請求項10】
請求項１に記載の制御装置において、
補正後の前記指令値が所定の閾値未満となる場合に、前記補正器は、前記指令値を予備補正して、予備補正された前記指令値を前記制限範囲内へ補正する制御装置。
補正後の前記指令値が所定の閾値未満となる場合に、前記補正器は、前記指令値を予備補正して、予備補正された前記指令値を前記制限範囲内へ補正する制御装置。
【請求項11】
請求項１に記載の制御装置において、
前記設定器は、前記指令値として指令速度を設定し、
前記制限器は、前記移動体の運動学モデルに基づいて前記制約条件を設定する制御装置。
前記設定器は、前記指令値として指令速度を設定し、
前記制限器は、前記移動体の運動学モデルに基づいて前記制約条件を設定する制御装置。
【請求項12】
請求項１に記載の制御装置において、
前記指令値の補正を前記移動体が備える表示装置又は警報器を介してユーザに報知する報知制御器をさらに備える制御装置。
前記指令値の補正を前記移動体が備える表示装置又は警報器を介してユーザに報知する報知制御器をさらに備える制御装置。
【発明の詳細な説明】
【技術分野】
【0001】
ここに開示された技術は、制御装置に関する。
【背景技術】
【0002】
従来より、移動体の経路を生成する技術に関し、移動体と障害との衝突を回避するように経路を生成して自動運転を行う技術が知られている。例えば、特許文献１には、複数のノードを直線で連結した経路を想定し、障害を回避するようにノードを移動させることによって最適な経路を演算する技術が開示されている。
【先行技術文献】
【特許文献】
【0003】
【特許文献1】米国特許第１１１６１５０１号明細書
【発明の概要】
【発明が解決しようとする課題】
【0004】
ところで、前述のように障害を回避する自動運転においては、障害に衝突しない将来の目標位置及び最適な時系列操作量が逐次演算される。そのような演算は複雑であり、演算負荷が大きい。そのため、瞬時の演算が難しく、急に出現した障害又は動きが急峻な障害等に瞬時に対応することが困難である。
【0005】
ここに開示された技術は、かかる点に鑑みてなされたものであり、その目的とするところは、瞬時に障害に対応して、障害を回避することにある。
【課題を解決するための手段】
【0006】
本開示の技術は、移動体を移動させるためのアクチュエータの操作量を演算する制御装置が対象である。制御装置は、前記移動体を目標位置へ移動させるための指令値を設定する設定器と、障害を回避するための前記指令値の制約条件を設定する制限器と、前記指令値が前記制約条件によって定められる制限範囲外である場合に、前記指令値を前記制限範囲内へ補正する補正器と、前記指令値に基づいて前記操作量を演算する操作量演算器とを備える。
【発明の効果】
【0007】
前記制御装置によれば、瞬時に障害に対応して、障害を回避することができる。
【図面の簡単な説明】
【0008】
【発明を実施するための形態】
【0009】
以下、例示的な実施形態を図面に基づいて詳細に説明する。図１は、運転支援システム１０００の概要図である。制御装置１００は、移動体１に搭載され、移動体１の自動運転を実行する。移動体１は、運転支援システム１０００に含まれる。運転支援システム１０００は、複数の移動体１を含んでいてもよい。運転支援システム１０００は、移動体１の他に、自動運転を支援する支援装置２００を備える。支援装置２００は、移動体１と通信可能に接続されている。支援装置２００は、移動体１の運転を支援するための地図情報を移動体１へ送信する。制御装置１００は、地図情報に基づいて経路を生成し、自動運転を実行する。支援装置２００と移動体１との通信は、中継装置１２１を介して行われる。中継装置１２１は、例えば、管制塔１２０に設置されている。支援装置２００と中継装置１２１とは、インターネット等の通信ネットワークＮを介して接続されている。中継装置１２１と移動体１とは、無線通信を行う。
【0010】
地図情報には、地理的情報が含まれる。地理的情報は、道路、通路、航路及び地物（建物、樹木及び岩石など、自然及び人工に関わらず地上又は海上にある物）の少なくとも１つを含む。地理的情報は、道路図、施設図、航路図等の一般的な地図に関する情報であり得る。地図情報は、地理的情報の他に、障害に関する情報を含んでいてもよい。障害に関する情報は、障害の位置、速度及び種別の少なくとも１つを含む。
【0011】
－移動体－
移動体１は、例えば、車両である。移動体１は、船舶又は自律ロボットであってもよい。図２は、移動体１の概略的なハードウェア構成を示す図である。移動体１は、監視センサ１１とアクチュエータ１２と通信器１３と位置検出器１４と入力装置１５と表示装置１６と警報器１７と制御装置１００とを有している。
移動体１は、例えば、車両である。移動体１は、船舶又は自律ロボットであってもよい。図２は、移動体１の概略的なハードウェア構成を示す図である。移動体１は、監視センサ１１とアクチュエータ１２と通信器１３と位置検出器１４と入力装置１５と表示装置１６と警報器１７と制御装置１００とを有している。
【0012】
監視センサ１１は、移動体１が移動する環境内において所定範囲の監視情報を取得する。監視情報には、地形、建物、地物、設備又は移動体等が含まれ得る。監視センサは、カメラ、ＬｉＤＡＲ（Light Detection And Ranging）、赤外線センサ、レーザ距離計及びドップラＬｉＤＡＲの少なくとも１つを含む。例えば、カメラは、静止画又は動画を撮影する。レーザ距離計は、グリーンレーザを採用していてもよい。この例では、監視センサ１１は、ＬｉＤＡＲである。監視センサ１１は、所定の監視領域内の物体の点群データを監視情報として取得する。
【0013】
アクチュエータ１２は、移動体１の駆動源である。アクチュエータ１２は、第１アクチュエータ１２Ａと第２アクチュエータ１２Ｂとを含んでいてもよい。例えば、移動体１が車両の場合、第１アクチュエータ１２Ａは、電動モータ又はエンジンであり、第２アクチュエータ１２Ｂは、ステアリング装置（詳しくは、操舵角を調節する電動モータ）である。
【0014】
通信器１３は、外部装置と無線通信を行う。位置検出器１４は、移動体１の位置を検出する。例えば、位置検出器１４は、ＧＮＳＳ（Global Navigation Satellite System）受信機又はＩＭＵ（慣性計測ユニット：Inertial Measurement Unit）である。入力装置１５は、ユーザが各種情報を入力するための装置である。例えば、移動体１が車両である場合、入力装置１５は、カーナビのタッチパネルである。移動体１が車両である場合、表示装置１６は、例えばカーナビのディスプレイである。表示装置１６は、地図情報から生成される地図又は経路を表示してもよい。警報器１７は、ブザー又はランプである。警報器１７は、制御装置１００からの指令に応じて、音又は光を発する。
【0015】
図３は、制御装置１００のハードウェア構成を示す図である。制御装置１００は、移動体１の全体を制御する。制御装置１００は、移動体１の自動運転を実行する。つまり、制御装置１００は、アクチュエータ１２を動作させて移動体１を移動させる。制御装置１００は、制御器１８と記憶器１９とメモリ１１０とを有している。
【0016】
制御器１８は、各種の演算処理を行う。例えば、制御器１８は、ＣＰＵ（Central Processing Unit）等のプロセッサで形成されている。制御器１８は、ＭＣＵ（Micro Controller Unit）、ＭＰＵ（Micro Processor Unit）、ＦＰＧＡ（Field Programmable Gate Array）、ＰＬＣ（Programmable Logic Controller）、システムＬＳＩ等で形成されていてもよい。
【0017】
記憶器１９は、制御器１８で実行されるプログラム及び各種データを格納している。例えば、記憶器１９は、制御プログラムを格納している。記憶器１９は、不揮発性メモリ、ＨＤＤ（Hard Disc Drive）又はＳＳＤ（Solid State Drive）等で形成される。メモリ１１０は、データ等を一時的に格納する。例えば、メモリ１１０は、揮発性メモリで形成される。
【0018】
制御器１８がアクチュエータ１２を操作することによって、移動体１は自律運転を行う。制御器１８は、地図情報に基づいて経路を生成する。制御器１８は、生成された経路に沿って移動体１が移動するように、監視センサ１１及び位置検出器１４による検出結果に基づいてアクチュエータ１２を操作する。このとき、制御器１８は、移動体１と障害との干渉を回避するようにアクチュエータ１２を操作する。
【0019】
図４は、制御器１８の機能ブロック図である。制御器１８は、記憶器１９からプログラムをメモリ１１０に読み出して展開することによって、各種機能を実現する。具体的には、制御器１８は、移動体１の状態及び環境の状態を推定する状態推定器１１１と、アクチュエータ１２の指令操作量を演算する制御演算器１１２として機能する。
【0020】
状態推定器１１１には、地図情報、監視センサ１１の検出結果及び位置検出器１４の検出結果が入力される。例えば、状態推定器１１１は、カルマンフィルタを用いて、移動体１の状態推定を行う。状態推定器１１１は、環境中の障害の状態推定も行う。状態推定器１１１は、ＳＬＡＭ（Simultaneous Localization and Mapping）技術を用いて、自己位置推定と環境地図生成とを実行してもよい。状態推定器１１１は、障害の位置等を含む地図情報、移動体１の現在位置及び速度等の移動体に関する情報、障害の移動体１に対する相対位置及び速度等の障害に関する情報、並びに、外乱の情報を出力する。
【0021】
尚、障害には、地物、建物、移動体１以外の他の移動体、工事現場、不整地、浅瀬、養殖場等が含まれる。
【0022】
図５は、制御演算器１１２の詳細な機能ブロック図である。制御演算器１１２は、移動体１の経路を生成する経路生成器２１と、移動体１の指令速度を設定する設定器２２と、指令速度の制限条件を設定する制限器２５と、指令速度を補正する補正器２６と、移動体１の実速度を指令速度に追従させるような指令力を演算するサーボ制御器２７と、指令力に基づいてアクチュエータ１２の操作量を演算する操作量演算器２８と、指令速度の補正を表示装置１６又は警報器１７を介してユーザに報知する報知制御器２９とを有している。
【0023】
経路生成器２１は、記憶器１９に保存された目的地を読み込む。記憶器１９には、目的地が予め保存されている。記憶器１９には経由地が保存されている場合もある。その場合、経路生成器２１は、経由地を記憶器１９から読み込む。経路生成器２１には、状態推定器１１１から地図情報及び移動体に関する情報が入力される。
【0024】
経路生成器２１は、地図情報に基づいて移動体１の現在位置から目的地までの経路を生成する。経路生成器２１は、地図情報を参照して、地物等に衝突しないような経路を生成する。環境内に道路、通路又は航路が設定されている場合には、経路生成器２１は、道路、通路又は航路に沿った経路を生成する。例えば、経路生成器２１は、Ａ－ｓｔａｒ探索アルゴリズム又はＲＲＴ－ｓｔａｒアルゴリズムを活用して経路を生成する。経路生成器２１は、経路上のウェイポイント、即ち、経路における現在位置から比較的近い目標位置を設定し、目標位置を指令演算器２３へ出力する。
【0025】
設定器２２は、経路に基づいて演算された指令速度と入力装置１５を介して入力された指令速度との何れか一方を指令速度として設定する。詳しくは、設定器２２は、経路生成器２１によって生成された経路に基づいて指令速度を演算する指令演算器２３と、出力する指令速度を入力装置１５からの指令速度と指令演算器２３からの指令速度とで切り替える切替器２４とを有している。入力装置１５からの指令速度は、ユーザのマニュアル操作によって入力される指令速度である。
【0026】
指令演算器２３には、状態推定器１１１から移動体に関する情報及び外乱の情報が入力される。指令演算器２３は、移動体１が目標位置（即ち、ウェイポイント）に移動するための指令速度を演算する。例えば、指令演算器２３は、ＰＩＤ制御によって指令速度を演算する。つまり、指令演算器２３は、移動体１の現在位置と目標位置との差分に基づいて指令速度を演算する。
【0027】
あるいは、指令演算器２３は、モデル予測制御（ＭＰＣ：Model Predictive Control）によって指令速度を演算してもよい。経路生成器２１によって生成された経路を基本経路とすると、指令演算器２３は、モデル予測制御では、基本経路に含まれる局所的な経路を生成しつつ、局所的な経路に沿って移動する際の指令速度を演算する。局所的な経路は、移動体１の現在位置から目標位置までの経路である。指令演算器２３は、移動体１及び障害の現在の状態量から移動体１及び障害の将来の状態量を予測して、移動体１の現在位置から目標位置までの最適な経路及びそれに応じた指令速度を算出する。この場合、指令演算器２３には、状態推定器１１１から障害に関する情報も入力される。
【0028】
切替器２４は、補正器２６に入力する指令速度を入力装置１５からの指令速度と指令演算器２３からの指令速度とで切り替える。自動運転の実行中は、基本的には、切替器２４は、指令演算器２３からの指令速度を補正器２６へ入力する。一方、マニュアル操作によって入力装置１５から指令速度が入力された場合には、切替器２４は、入力装置１５からの指令速度を補正器２６へ入力する。
【0029】
制限器２５は、指令速度を制約する制約条件を設定する。詳しくは、制限器２５には、状態推定器１１１から移動体に関する情報、障害に関する情報及び外乱の情報が入力される。制限器２５は、移動体１が障害を回避するための指令速度の制約条件を設定する。例えば、制限器２５は、移動体１の運動学モデル、即ち、キネマティクスモデルを用いて制約条件を設定する。具体的には、制限器２５は、移動体１の運動学モデルを用いて、移動体１と障害との間に所定の距離（以下、「安全距離」ともいう）が確保されるように指令速度の制約条件を設定する。
【0030】
制限器２５は、複数の制約条件を設定し得る。例えば、回避を同時に考慮すべき障害が複数存在する場合には、制限器２５は、それぞれの障害に対応する複数の制約条件を設定し得る。さらに、回避を同時に考慮すべき移動体１の保護部位が複数設定されている場合には、制限器２５は、それぞれの保護部位に対応する複数の制約条件を設定し得る。そのため、複数の障害及び複数の保護部位を考慮すべき場合には、障害と保護部位との組み合わせに応じた数の制約条件が設定され得る。
【0031】
制限器２５は、障害の回避に加え、指令速度に対する出力特性に関する制約条件を設定する。例えば、制限器２５は、サーボ制御器２７の制御特性に関する制約条件を設定する。そのため、制限器２５には、サーボ制御器２７から制御状態量も入力される。さらに、制限器２５は、障害の回避に加え、アクチュエータ１２に関する制約条件を設定する。制御特性に関する制約条件及びアクチュエータ１２に関する制約条件は、出力特性に関する制約条件の一例である。
【0032】
制限器２５は、制約条件を逐次演算して、制約条件を更新する。つまり、移動体１及び障害の状況は、経時的に変化し得る。制限器２５は、移動体１及び障害の状況の経時的変化に対応して、制約条件を更新することができる。制限器２５は、制約条件を補正器２６へ出力する。
【0033】
また、制限器２５は、調節可能な制約条件を設定してもよい。例えば、移動体１の保護部位及び障害の種別の少なくとも一方に応じて緩和可能な制約条件については調節可能に設定してもよい。指令速度が適切に補正されない場合には、制限器２５は、調節可能な制約条件を変更する。
【0034】
補正器２６は、制約条件によって定められる制限範囲と指令速度とを照らし合わせて、最終的な指令速度を決定する。詳しくは、指令速度が制限範囲内であれば、補正器２６は、指令速度をそのまま最終的な指令速度として決定する。指令速度が制限範囲外であれば、補正器２６は、指令速度を制限範囲内に補正して、最終的な指令速度とする。制限範囲は、有限個、例えば、数個の選択肢のみで構成されるわけではなく、連続的な幅、即ち、拡がりを有している。つまり、補正器２６は、連続的な拡がりを有する範囲の中から適切な速度に指令速度を補正する。例えば、補正器２６は、二次計画法等によって指令速度を制限範囲内に入るように最適化する。補正器２６は、最終的な指令速度をサーボ制御器２７へ出力する。
【0035】
サーボ制御器２７には、状態推定器１１１から移動体に関する情報及び外乱の情報が入力される。サーボ制御器２７は、移動体１の実速度を指令速度に追従させるための指令力を演算する。サーボ制御器２７は、例えば、ＰＩＤ制御を行う。尚、サーボ制御器２７の制御は、ＰＩＤ制御に限定されず、ロバスト制御等であってもよい。サーボ制御器２７は、指令力を操作量演算器２８へ出力する。
【0036】
操作量演算器２８は、指令力を複数のアクチュエータ１２に分配して、複数のアクチュエータ１２のそれぞれの指令操作量を演算する。アクチュエータ１２が電動モータの場合には、例えば、操作量は、電動モータの回転速度又はトルクである。アクチュエータ１２がエンジンの場合には、例えば、操作量は、アクセル開度及び燃料噴射量である。
【0037】
それぞれのアクチュエータ１２は、指令操作量に応じて動作する。アクチュエータ１２には、アクチュエータ１２を動作させるための固有の制御器が設けられている場合がある。例えば、アクチュエータ１２が電動モータである場合には、アクチュエータ１２は、サーボアンプをさらに有している。その場合、サーボアンプが指令操作量に応じて電動モータを動作させる。その結果、移動体１は、推力を発揮し、目標位置へ向かって移動する。
【0038】
報知制御器２９には、経路生成器２１から経路及び目標位置が入力される。また、報知制御器２９には、補正器２６から最終指令速度u及び補正の有無が入力される。報知制御器２９は、指令速度が補正される旨を表示装置１６及び警報器１７を介してユーザに報知する。
【0039】
具体的には、報知制御器２９は、経路、目標位置及び最終指令速度uを表示装置１６に表示させる。例えば、表示装置１６は、地図上に経路及び目標位置を重畳させて表示し、さらには、最終指令速度uをベクトル表示する。このとき、表示装置１６は、指令速度が補正されているか否かによって最終指令速度uの表示形態を変更する。これにより、ユーザは、移動体１の進行状況及び指令速度の補正の有無を知ることができる。尚、表示装置１６は、「補正中」又は「制限中」のような、指令速度が補正されている旨を表示してもよい。この場合、表示装置１６は、最終指令速度uの表示形態を変更しなくてもよい。
【0040】
さらに、指令速度が補正される場合には、報知制御器２９は、指令速度の補正を報知するための指令を警報器１７へ出力する。警報器１７は、指令に応じて警報を発報する。例えば、警報器１７は、警報としての音又は光を発する。これにより、ユーザは、指令速度が補正されていることを知ることができる。
【0041】
尚、報知制御器２９は、表示装置１６及び警報器１７の何れか一方だけを介して指令速度の補正をユーザに報知してもよい。
【0042】
－制約条件及び制限範囲－
続いて、制約条件及び制限範囲について詳細に説明する。
続いて、制約条件及び制限範囲について詳細に説明する。
【0043】
制限器２５及び補正器２６は、例えば、制御バリア関数（ＣＢＦ：Control Barrier Function）を用いて、指令速度を制限する。具体的には、以下の式（１）に示す評価関数Ｊ及び式（２）に示す制約条件に基づいて指令速度を制限する。
【0044】
【数1】
【0045】
【数2】
【0046】
バリア関数hiは、安全状態を表す関数である。バリア関数hiは、hi≧０を満たすときに、制御対象が安全となるように設定されている。式（２）は、制御対象をhi≧０の範囲に収束させるための不等式制約条件である。
【0047】
移動体１の運動方程式は、式（４）のように、指令値uに対してＡｆｆｉｎｅな形式をとる場合が多いため、制約条件は、線形な式（５）のように変換できる。制限器２５は、式（６）のような制約条件を設定する。
【0048】
【数3】
【0049】
補正器２６は、制約条件を満たしつつ、評価関数Jが最小になる指令値uを求める。その結果、補正器２６は、入力指令値urefからの変化量が最小で且つ制約条件を満足する指令値uを求める。例えば、補正器２６は、凸最適化の計算手法、例えば、内点法又は二次計画法によって指令値uを求める。補正器２６は、入力指令値urefを指令値uに補正し、最終的な指令値uとして出力する。尚、入力指令値urefが制限範囲内の場合には、評価関数Jが０となる。つまり、補正器２６は、評価関数Jが０か否かによって、入力指令値urefが制限範囲内か否かを判定する。入力指令値urefが制限範囲内の場合には、補正器２６は、入力指令値urefをそのまま最終的な指令値ｕとして出力する。
【0050】
図６は、補正器２６による指令値uの補正の一例を示すイメージ図である。この例では、指令値uは指令速度であり、直交するＸ軸及びＹ軸からなる座標系におけるベクトル（u, v）で表される。ハッチングが付された領域は、制約条件を満たす制限範囲である。破線は、制約条件Ｒ１を示し、制限範囲の境界となる。補正器２６に入力された入力指令速度urefが制限範囲外である場合、補正器２６は、制限範囲内に含まれ且つ入力指令速度urefからの変化量が最小となる最終指令速度uを求める。図６からわかるように、境界Ｂ上の点のうち入力指令速度urefから最も近い点が最終指令速度uとなる。
【0051】
こうして、補正器２６は、入力指令速度urefを制限範囲内に含まれる最終指令速度uに補正する。最終指令速度uを制限範囲内に制限することによって、制御対象である移動体１が安全に維持される。安全は、バリア関数hiによって定義される。
【0052】
このような制約条件の演算及び指令値uを制限範囲内へ補正する演算は、目標位置を再設定する演算や経路（局所的な経路を含む）を再設定する演算に比べて計算負荷が小さい。そのため、制限器２５及び補正器２６は、最適な最終指令値uを早急に求めることができる。例えば、移動体１の近傍に障害が急に現れた場合、又は、障害が急速に移動した場合に、制限器２５及び補正器２６は、最適な最終指令値uを瞬時に求めて、移動体１に障害との干渉を瞬時に回避させることができる。
【0053】
＝追加的な制約＝
以上の説明の制約条件は、移動体１が障害を回避するための制約条件であり、安全上の制約条件である。制限器２５は、安全上の制約条件に加えて、安全性以外の制約条件を併せて考慮してもよい。例えば、安全性以外の制約条件としては、制御特性上の制約条件及びアクチュエータ特性上の制約条件等の出力特性上の制約条件がある。サーボ制御器２７から出力される指令力は、サーボ制御器２７の制御特性上の制約条件を受け得る。あるいは、移動体１が過度に加速しないように指令力に上限が設定される等、サーボ制御器２７の制御特性に意図的に制約が設定される場合もあり得る。さらに、アクチュエータ１２は、最大出力等の特性を有する。制限器２５は、これら安全性以外の制約条件の少なくとも１つを考慮してもよい。
以上の説明の制約条件は、移動体１が障害を回避するための制約条件であり、安全上の制約条件である。制限器２５は、安全上の制約条件に加えて、安全性以外の制約条件を併せて考慮してもよい。例えば、安全性以外の制約条件としては、制御特性上の制約条件及びアクチュエータ特性上の制約条件等の出力特性上の制約条件がある。サーボ制御器２７から出力される指令力は、サーボ制御器２７の制御特性上の制約条件を受け得る。あるいは、移動体１が過度に加速しないように指令力に上限が設定される等、サーボ制御器２７の制御特性に意図的に制約が設定される場合もあり得る。さらに、アクチュエータ１２は、最大出力等の特性を有する。制限器２５は、これら安全性以外の制約条件の少なくとも１つを考慮してもよい。
【0054】
まず、制御特性上の制約条件について説明する。サーボ制御器２７から出力される指令力は、サーボ制御器２７の応答性及び追従性等の制御特性の影響、即ち、制約を受ける。サーボ制御器２７の伝達関数は、サーボ制御器２７の制御特性を表している。制限器２５は、サーボ制御器２７の逆伝達関数に基づいて、制御特性上の制約条件を指令速度に対する制約条件に変換する。具体的には、制限器２５には、サーボ制御器２７の状態量が入力される。制限器２５は、サーボ制御器２７の状態量に基づいてサーボ制御器２７の逆伝達関数を求める。制限器２５は、逆伝達関数を用いて、指令速度に対する、制御特性上の制約条件を求める。
【0055】
次に、アクチュエータ特性上の制約条件について説明する。アクチュエータ１２が全体として発揮できる推力の範囲、即ち、限界は、アクチュエータ１２の配置、出力上限及び出力下限並びに操作量演算器２８の分配ロジック等のアクチュエータ特性に応じて決まっている。そのため、アクチュエータ１２の発揮できる推力の限界がアクチュエータ１２の配置等のアクチュエータ特性に基づいて予め求められ、記憶器１９に保存されている。アクチュエータ１２による推力の限界は、サーボ制御器２７から操作量演算器２８へ出力される指令力への制約とみなすことができる。指令力への制約は、前述の如く、サーボ制御器２７の逆伝達関数を用いて、指令速度に対する制約条件に変換することができる。つまり、アクチュエータ１２による推力の限界は、指令速度に対する制約条件に変換することができる。制限器２５は、記憶器１９に保存されたアクチュエータ１２による推力の限界からサーボ制御器２７の逆伝達関数を用いて、指令速度に対する、アクチュエータ特性上の制約条件を求める。
【0056】
制限器２５は、制御特性上の制約及びアクチュエータ特性上の制約条件の両方を考慮する場合には、両方の制約条件を合算して、出力特性上の制約条件を求める。図７は、出力特性上の制約条件を考慮しない、補正器２６による指令値uの補正の一例を示すイメージ図である。図８は、出力特性上の制約条件を考慮した、補正器２６による指令値uの補正の一例を示すイメージ図である。図７において、ハッチングが付された領域は、制限範囲であり、破線は、制約条件Ｒ１を示し、制限範囲の境界となる。一点鎖線は、出力特性上の制約に相当する指令値の制約ある。図８において、ハッチングが付された領域は、制限範囲であり、破線は、制約条件Ｒ１及びＲ２を示し、制限範囲の境界となる。制約条件Ｒ１は、安全上の制約条件であり、制約条件Ｒ２は、出力特性上の制約条件である。
【0057】
出力特性上の制約条件を考慮しない場合、補正器２６は、入力指令速度urefを、安全上の制約条件Ｒ１のみに基づいて補正する。最終指令速度uは、制約条件Ｒ１上、即ち、制限条件の境界上に位置する。その後、サーボ制御器２７は、最終指令速度uに基づいて指令力を演算し、操作量演算器２８は、指令力に基づいて第１アクチュエータ１２Ａ及び第２アクチュエータ１２Ｂのそれぞれの指令操作量を演算する。このとき、指令力は、サーボ制御器２７の制御特性の影響を受ける。また、アクチュエータ１２よる総合的な推力は、第１アクチュエータ１２Ａ及び第２アクチュエータ１２Ｂのアクチュエータ特性の影響を受ける。その結果、移動体１に与えられる推力は、最終指令速度uに適切に対応する推力ではなく、制御特性上の制約及びアクチュエータ特性上の制約を受けた推力となる。このような推力の制約を、指令速度uの制約に変換すると、図７の一点鎖線となる。つまり、指令速度uが一点鎖線の制約を満たすようにさらに補正されたのと等しい推力が発揮されることになる。図７の例では、実質的な指令速度u’は、制約条件Ｒ１よりも制限範囲の内側へ離れている。これは、より安全な動作点での運転を意味し、必要以上に指令速度uを制限した運転とみなすこともできる。
【0058】
一方、出力特性上の制約条件を考慮する場合、制限器２５は、図８に示すように、安全上の制約条件Ｒ１と出力特性上の制約条件Ｒ２との両方を設定する。補正器２６は、制約条件Ｒ１及びＲ２によって定められる制限範囲内へ入力指令速度urefを補正する。その結果、補正器２６は、制限範囲の境界上における、入力指令速度urefから最も近い点に入力指令速度urefを補正する。この例では、制約条件Ｒ１と制約条件Ｒ２との交点に入力指令速度urefが補正される。こうして求められた最終指令速度uは、図７と比較すると、ｕ軸方向の速度は同じであるが、ｖ軸方向への速度は増大している。その結果、移動体１は、安全を確保しつつ、出力特性上の制約内でできる限り大きな推力を発揮する。
【0059】
＝可変の制約条件＝
制限器２５は、調節可能、即ち、可変の制約条件を設定することができる。例えば、障害には、不整地又は気流が激しい場所等のように、通行可能ではあるが、通行を回避することが好ましい障害がある。制限器２５は、制約条件の対象となる障害の種別に応じて、制約条件を可変とするか固定とするかを切り替えてもよい。通行可能な障害に対しては、制限器２５は、制約条件を調節可能、即ち、緩和可能としてもよい。例えば、制約条件が厳しい場合には、制限範囲内に含まれる指令値uが存在しない場合や、制限範囲内に含まれる指令値uが微小になる場合があり得る。その結果、移動体１を適切に移動させることが困難となる。そのような場合、制限器２５は、可変の制約条件を緩和するように変更して、制限範囲を拡大する。その結果、制限範囲内に含まれる指令値uを設定することが可能となる、又は、制限範囲内に含まれる指令値uが増大する可能性がある。
制限器２５は、調節可能、即ち、可変の制約条件を設定することができる。例えば、障害には、不整地又は気流が激しい場所等のように、通行可能ではあるが、通行を回避することが好ましい障害がある。制限器２５は、制約条件の対象となる障害の種別に応じて、制約条件を可変とするか固定とするかを切り替えてもよい。通行可能な障害に対しては、制限器２５は、制約条件を調節可能、即ち、緩和可能としてもよい。例えば、制約条件が厳しい場合には、制限範囲内に含まれる指令値uが存在しない場合や、制限範囲内に含まれる指令値uが微小になる場合があり得る。その結果、移動体１を適切に移動させることが困難となる。そのような場合、制限器２５は、可変の制約条件を緩和するように変更して、制限範囲を拡大する。その結果、制限範囲内に含まれる指令値uを設定することが可能となる、又は、制限範囲内に含まれる指令値uが増大する可能性がある。
【0060】
あるいは、制限器２５は、保護部位と障害との間に安全距離が確保されるように制約条件を設定する場合、安全距離が低減されるように制約条件を調節可能、即ち、緩和可能に設定してもよい。安全距離は、保護部位と障害との干渉が確実に回避されるように、必要最小距離に余裕分を付加して設定されている。制限器２５は、安全距離のうち余裕分の範囲で制約条件を調節可能としてもよい。例えば、移動体１に複数の保護部位が設定され且つ、複数の保護部位に保護の優先順位が設定されている場合、制限器２５は、優先順位が相対的に低い保護部位に対応する制約条件を可変としてもよい。具体的には、保護部位は第１保護部位と第１保護部位よりも優先順位の低い第２保護部位とが含まれている場合、制限器２５は、第１保護部位に対応する制約条件を固定し、第２保護部位に対応する制約条件を可変としてもよい。あるいは、制限器２５は、適切な指令値uが設定されるまで、優先順位の低い順に、保護部位に対応する制約条件を可変として順次緩和してもよい。
【0061】
制限器２５は、可変の制約条件については、式（７）のようにスラック変数σiを導入する。
【0062】
【数4】
【0063】
この場合、制限器２５は、スラック変数を含む制約条件を補正器２６へ出力する。補正器２６は、評価関数Jを式（８）のように設定し、スラック変数σiの重みを調節する。固定の制約条件はスラック変数σiを含んでいないので、補正器２６は、評価関数Jにおいて固定の制約条件のスラック変数σiの重みを０とする。制限器２５及び補正器２６は、このようにして、制限範囲を調節、即ち、拡大することができる。
【0064】
【数5】
【0065】
図９は、固定の制約条件と可変の制約条件とを含む場合の補正器２６による指令値uの補正の一例を示すイメージ図である。図９では、複数の制約条件として、固定の第１制約条件R1と可変の第２制約条件R2とが含まれている。制限器２５は、スラック変数σ2を調整することによって第２制約条件R2を変更し、結果として、制限範囲を変更することができる。
【0066】
例えば、補正後の指令値uが適切でない場合などには、制限器２５は、可変の制約条件を調節することによって、入力指令値urefの補正をやり直してもよい。制約条件が変更されることによって、補正後の指令値uが制約条件の変更前と比較して変化し得る。これにより、補正後の指令値uが改善される可能性がある。
【0067】
＝デッドロック対策＝
入力指令速度urefが制限範囲内に補正された結果、最終指令速度uが略０になる場合がある。つまり、移動体１が略停止する。この状態をデットロックという。デッドロックに陥る場合には、制約条件を変更するか又は補正前の指令値uを変更して、デッドロックを解消させる。
入力指令速度urefが制限範囲内に補正された結果、最終指令速度uが略０になる場合がある。つまり、移動体１が略停止する。この状態をデットロックという。デッドロックに陥る場合には、制約条件を変更するか又は補正前の指令値uを変更して、デッドロックを解消させる。
【0068】
具体的には、制約条件が調節可能な場合には、補正器２６は、制限器２５に可変の制約条件を変更させる。例えば、デッドロックの可能性がある場合、補正器２６は、その旨を制限器２５へ通知する。制限器２５は、可変の制約条件を変更し、変更後の制約条件を補正器２６へ出力する。制約条件の変更によって制限範囲が変化するので、補正後の指令値uは、０からずれる、即ち、０以外の値となる可能性がある。
【0069】
図１０は、デッドロック発生時に制約条件を変更する場合の補正器２６による指令値uの補正の一例を示すイメージ図である。図１０の例では、制約条件を変更しない場合、補正器２６が入力指令速度urefを制限範囲内に補正すると、最終指令速度uが０になり、デッドロックに陥る（二点鎖線参照）。この場合には、補正器２６は、制限器２５に制約条件を変更させる。例えば、前述の図９のように、可変の制約条件が設定されている場合には、制限器２５は、可変の制約条件を変更する。図１０の例では、制限器２５は、第２制約条件R2を変更する。補正器２６は、第２制約条件R2の変更後に、入力指令速度urefを制限範囲内に補正する。制限範囲が変更されるので、最終指令速度uは、０からずれ、０以外の値となる。その結果、移動体１は、デッドロックに陥ることなく、移動することができる。
【0070】
あるいは、補正器２６は、入力指令値urefを予備補正してから、制限範囲内へ補正する。補正器２６は、入力指令値urefを所定量だけ予備的に補正する。予備補正の所定量は、予備補正のたびに変更されてもよい。あるいは、所定量は、一定であってもよい。入力指令値urefの予備補正によって、入力指令値urefと制限範囲との関係性が変化するため、入力指令値urefから補正量が最小となる制限範囲内の指令値uも変化し得る。その結果、補正後の指令値uは、０からずれる、即ち、０以外の値となる可能性がある。
【0071】
図１１は、デッドロック発生時に予備補正を行う場合の補正器２６による指令値uの補正の一例を示すイメージ図である。図１１の例では、予備補正がない場合、補正器２６が入力指令速度urefを制限範囲内に補正すると、最終指令速度uが０になり、デッドロックに陥る（二点鎖線参照）。この場合には、補正器２６は、入力指令速度urefをまず予備補正する。補正器２６は、予備補正後の入力指令速度uref’を制限範囲内に補正する。最終指令速度uは、０からずれ、０以外の値となる。その結果、移動体１は、デッドロックに陥ることなく、移動することができる。
【0072】
尚、デッドロックに陥る場合は、補正器２６は、デッドロックの状態で待機してもよい。時間の経過と共に障害の状態が変化し、それにより制約条件が変化する可能性がある。その場合、デッドロックが自然と解消する。例えば、他の移動体に対応する制約条件に起因してデッドロックが発生した場合、他の移動体が移動すると、制約条件が変化して、他の移動体との干渉の可能性が低減し得る。その場合、制限範囲が拡大され、デッドロックが解消する。あるいは、前述のような制約条件の変更、入力指令値urefの予備補正、及び、待機等の対策を行ってもデッドロックが解消しない場合には、経路生成器２１は、経路を生成し直してもよい。
【0073】
【0074】
まず、制御装置１００は、ステップＳ１０１において、経路を生成する。具体的には、経路生成器２１は、地図情報、移動体に関する情報等に基づいて、移動体１の現在位置から経由地を介して目的地に至る経路を生成する。さらに、経路生成器２１は、経路上のウェイポイント、即ち、現在位置から経路上の直近の目標位置を決定する。
【0075】
次に、制御装置１００は、ステップＳ１０２において、入力指令速度urefを算出する。具体的には、指令演算器２３は、移動体１が目標位置に移動するための入力指令速度urefを演算する。
【0076】
そして、制御装置１００は、ステップＳ１０３において、指令速度uの制約条件を設定する。具体的には、制限器２５は、移動体に関する情報及び障害に関する情報に基づいて制約条件を演算する。このとき、制限器２５は、出力特性上の制約条件も設定する。
【0077】
続いて、制御装置１００は、ステップＳ１０４において、入力指令速度urefが制限範囲に入っているか否かを判定する。具体的には、補正器２６は、評価関数J及び制約条件を用いて、入力指令速度urefに対して評価関数Jが０になるか否かを判定する。評価関数Jが０になる場合には、補正器２６は、入力指令速度urefが制限範囲に入っていると判定する。
【0078】
入力指令速度urefが制限範囲に含まれる場合には、制御装置１００は、ステップＳ１０５において、入力指令速度urefを最終指令速度uとして決定する。入力指令速度urefが制限範囲外の場合は、制御装置１００は、ステップＳ１０７において、入力指令速度urefを制限範囲内へ補正する。具体的には、補正器２６は、前述のように評価関数J及び制約条件を用いて、入力指令速度urefを制限範囲内へ補正する。
【0079】
続いて、制御装置１００は、ステップＳ１０８において、入力指令速度urefの補正によってデッドロックが発生するか否かを判定する。具体的には、補正器２６は、補正後の指令速度uが所定の閾値未満となるか否かを判定する。閾値は、０に近い微小な値である。補正後の指令速度uが閾値未満とならない場合には、制御装置１００は、ステップＳ１０５において、補正後の指令速度uを最終指令速度uとして決定する。
【0080】
補正後の指令速度uが閾値未満となる場合には、制限器２５は、ステップＳ１０９において、制約条件の緩和が可能か否かを判定する。例えば、制限器２５は、保護部位又は障害の種別に応じて制約条件が緩和可能か否かを判定する。
【0081】
制約条件の緩和が可能な場合には、補正器２６は、ステップＳ１１０において、制約条件を緩和する。その後、補正器２６は、ステップＳ１０７において、入力指令速度urefを制限範囲内へ補正し、ステップＳ１０８においてデッドロックを判定する。制約条件の変更によって制限範囲が変更されているので、入力指令速度urefは、先の補正とは異なる最終指令速度uに補正され得る。その結果、デッドロックが発生しない状態となっていることが多い。
【0082】
一方、制約条件の緩和が不能な場合には、補正器２６は、ステップＳ１１１において、入力指令速度urefを予備補正する。その後、補正器２６は、ステップＳ１０７において、予備補正後の入力指令速度uref’を制限範囲内へ補正し、ステップＳ１０８においてデッドロックを判定する。入力指令速度urefは、予備補正されているので、先の補正とは異なる最終指令速度uに補正され得る。その結果、デッドロックが発生しない状態となっていることが多い。
【0083】
このように、デッドロックが解消されるまでステップＳ１１０又はＳ１１１が実行される。これにより、最終指令速度uは、いずれは閾値より大きな値に補正される。
【0084】
最終指令速度uが決定されると、制御装置１００は、ステップＳ１０６において、最終指令速度uに応じてアクチュエータ１２を動作させ、移動体１を移動させる。具体的には、サーボ制御器２７は、最終指令速度uに応じて指令力を演算し、操作量演算器２８は、指令力に基づいて複数のアクチュエータ１２のそれぞれの指令操作量を演算する。その後、複数のアクチュエータ１２は、それぞれの指令操作量に基づいて動作する。これにより、移動体１が目標位置へ向かって移動する。
【0085】
このとき、制御装置１００、具体的には、報知制御器２９は、生成された経路を表示装置１６に表示させる。制御装置１００は、最終指令速度uをベクトル等の形態で表示装置１６に表示させる。さらに、制御装置１００は、入力指令速度urefが補正された場合、即ち、障害の回避を実行している場合には、表示装置１６及び警報器１７の少なくとも一方を介して、入力指令速度urefの補正をユーザに報知する。このとき、制御装置１００は、デッドロックの有無、制限条件の緩和又は、入力指令速度urefの予備補正等の具体的な補正内容を表示装置１６及び警報器１７の少なくとも一方を介して報知してもよい。
【0086】
このような処理が繰り返されることによって、移動体１が経路に沿って移動する。その際、移動体１と障害との干渉の可能性がある場合には、指令速度uが瞬時に制限され、安全が確保される。
【0087】
尚、ユーザから入力指令速度urefが入力装置１５を介して入力される場合には、ステップＳ１０１及びＳ１０２に代えて、入力装置１５を介したマニュアル入力が行われる。そして、ステップＳ１０３以降の処理は、自動運転の場合と同様であり、制約条件が設定され（ステップＳ１０３）、マニュアル入力された入力指令速度urefが制限範囲内か否か判定され（ステップＳ１０４）、入力指令速度urefが制限範囲外であれば、入力指令速度urefが制限範囲内に入るように補正される（ステップＳ１０５）。その他の処理も同様である。つまり、マニュアル入力であっても、移動体１と障害との干渉の可能性がある場合には、指令速度uが瞬時に制限され、安全が確保される。
【0088】
【0089】
移動体１の運動学モデルを指令速度u＝（u,v,γ）Tに対してＡｆｆｉｎｅな形に整理すると、下記の式のように表される。kは、移動体１における部位を識別するための番号である。
【0090】
【数6】
【0091】
【0092】
車両の運動学モデルを指令速度u＝(U, γ)Tに対してＡｆｆｉｎｅな形に整理すると、下記式のようになる。
【0093】
【数7】
【0094】
ここで、Uは、牽引車１Ａに与えられる前進速度であり、γは、牽引車１Ａのヨーレートである。θは、地図座標系における被牽引車１Ｂの角度であり、βは、被牽引車１Ｂに対する牽引車１Ａの相対角度である。（xt, yt）は、地図座標系における被牽引車１Ｂの保護部位の位置座標である。L1は、牽引車１Ａにおける、被牽引車１Ｂの連結点から保護部位までの距離である。L2は、被牽引車１Ｂにおける、被牽引車１Ｂの保護部位から牽引車１Ａとの連結点までの距離である。Mは、牽引車１Ａの連結点と被牽引車１Ｂの連結点との距離である。
【0095】
制限器２５は、このような運動学モデル、即ち、キネマティクスモデルを用いて移動体１をモデル化して、制約条件を設定する。
【0096】
例えば、図１３の例において、移動体１が自動運転によって岸壁に接岸する際に、制限器２５は、移動体１の保護部位と障害の距離が安全距離（即ち、必要最小距離に余裕分を加えた距離）よりも大きくなることを制約条件として設定する。例えば、図１３の船舶の例において保護部位が船首で、障害が岸壁である場合に、船首と岸壁が安全距離を保つための制約条件は、下記式のように設定される。
【0097】
【数8】
【0098】
式（１１）を時間微分すると、下記式（１２）のようになる。
【0099】
【数9】
【0100】
ここで、式（１２）の右辺は、２×（岸壁方向単位ベクトル）×（自船運動学のヤコビアン）×（自船と岸壁の相対速度）である。
【0101】
式（１１）及び（１２）に基づいて、指令速度uの制約条件が以下のように設定される。
【0102】
【数10】
【0103】
この制約条件のもと、補正器２６は、船首と岸壁とが安全距離を保ちつつ、移動体１を岸壁に接近させるための指令速度uを算出できる。最終的に、移動体１は、目的地、即ち、接岸する位置に到達する。このときも、船首と岸壁との間には、必要最小距離に余裕分を加えた距離が確保されている。尚、制限器２５が船尾についても同様の制約条件を設定することによって、船尾と岸壁との間にも安全距離を確保することができる。さらに、移動体１の周辺に他の船舶が出現した場合には、制限器２５が他の船舶に対する制約条件、例えば、船首と他の船舶との制約条件及び船尾と他の船舶との制約条件を設定することによって、移動体１と他の船舶との間にも安全距離を確保することができる。
【0104】
さらに、制限器２５は、移動体１の移動状況に応じて制約条件を変更してもよい。例えば、前述の例では、船首と岸壁との距離が安全距離に略等しくなると、移動体１は、それ以上岸壁に接近しない。そこで、制限器２５は、制約条件を緩和する。例えば、制限器２５は、制約条件において、保護領域半径rk及び岸壁マージンΔsの少なくとも一方を減少させる。つまり、必要最小距離及び余裕分の少なくとも一方が減少する。その結果、移動体１はより岸壁に接近することができる。制限器２５は、移動体１が目的地に到達するまで制約条件を緩和することによって、移動体１を目的地に到達させることができる。尚、制約条件の緩和は、制約条件を直接変更することに限定されず、補正器２６が、前述の可変の制約条件を含む評価関数Jにおいて対応するスラック変数σiの重みづけを変更する、例えば、減少させることによっても実現できる。
【0105】
このような制御装置１００によれば、障害を回避するための指令値の制約条件が設定され、制約条件で定められる制限範囲から指令値が外れている場合には、指令値が制限範囲内に補正される。つまり、指令値が制約条件を満たすように制限される。これにより、移動体１は障害を回避しつつ、目標位置へ移動する。このように、指令値の制限によって障害の回避が実現されるため、目標位置又は経路を再生成する必要がない。指令値の制限に必要な演算量は、目標位置又は経路の再計算に比べて少ない。つまり、指令値の制限を迅速に実行することができ、障害に対して瞬時に対応することができる。
【0106】
さらに、指令値は制限範囲内に補正されるので、補正される指令値の選択肢は拡がりを有する。つまり、移動体１を単に停止させるとか、移動体１を決まった向きに（例えば、左向きに）一律に方向転換させるといった対応と異なり、移動体１は柔軟な対応を取ることができる。つまり、移動体１は、指令値が制限されながらも、目標位置へ向けた移動を継続することができる。
【0107】
また、制約条件は逐次更新されるので、時々刻々と変化する移動体１及びその周辺環境に対応して制約条件が設定される。つまり、指令値は常に一律に制限されるのではなく、移動体１及びその周辺環境の変化に応じた制約条件によって制限される。
【0108】
さらに、制御装置１００は、指令値として指令速度を制限する。そのため、制御装置１００は、制約条件の演算、及び、指令速度を制限範囲内へ補正するための演算を移動体１の運動学モデル、即ち、キネマティクスモデルを用いて実現することができる。これにより、移動体１のダイナミクスモデルを用いて演算する場合と比較して、計算式が単純化され、ユーザによる設計が容易になる。
【0109】
また、制御装置１００は、サーボ制御器２７よりも前段において指令値を制限しているので、外乱の影響及びモデル誤差をサーボ制御によって低減することができる。例えば、サーボ制御器２７の後段において指令値を制限すると、外乱の影響等を低減するために調節された指令値が変更されることになる。その結果、サーボ制御による外乱の影響及びモデル誤差の低減効果が小さくなってしまう。サーボ制御器２７よりも前段において指令値が制限されることによって、安全を確保しつつ、外乱の影響及びモデル誤差を適切に低減することができる。
【0110】
また、制御装置１００は、可変の制約条件を設定することによって、移動体１の適切な移動を実現するように制約条件を調節できる。例えば、移動体１の円滑な移動が実現できない場合には、制約条件を緩和することによって、移動体１の移動を円滑にすることができ得る。
【0111】
さらに、制御装置１００は、デッドロックが生じる場合に、制約条件の変更、又は、入力指令値の予備補正によってデッドロックを解消することができる。
【0112】
また、制御装置１００は、指令値が制限範囲外である場合には、その旨を表示装置１６及び警報器１７の少なくとも一方を介してユーザに報知する。これにより、ユーザは、障害の回避が実行されていることを知ることができる。さらに、制御装置１００は、経路及び指令値の少なくとも一方を、指令値が制限範囲外か否かにかかわらず、表示装置１６に表示させる。これにより、ユーザは、障害の回避を行っているか否かにかかわらず、移動体１の経路及び指令値の少なくとも一方を知ることができる。
【0113】
《その他の実施形態》
以上のように、本出願において開示する技術の例示として、前記実施形態を説明した。しかしながら、本開示における技術は、これに限定されず、適宜、変更、置き換え、付加、省略などを行った実施の形態にも適用可能である。また、前記実施形態で説明した各構成要素を組み合わせて、新たな実施の形態とすることも可能である。また、添付図面および詳細な説明に記載された構成要素の中には、課題解決のために必須な構成要素だけでなく、前記技術を例示するために、課題解決のためには必須でない構成要素も含まれ得る。そのため、それらの必須ではない構成要素が添付図面や詳細な説明に記載されていることをもって、直ちに、それらの必須ではない構成要素が必須であるとの認定をするべきではない。
以上のように、本出願において開示する技術の例示として、前記実施形態を説明した。しかしながら、本開示における技術は、これに限定されず、適宜、変更、置き換え、付加、省略などを行った実施の形態にも適用可能である。また、前記実施形態で説明した各構成要素を組み合わせて、新たな実施の形態とすることも可能である。また、添付図面および詳細な説明に記載された構成要素の中には、課題解決のために必須な構成要素だけでなく、前記技術を例示するために、課題解決のためには必須でない構成要素も含まれ得る。そのため、それらの必須ではない構成要素が添付図面や詳細な説明に記載されていることをもって、直ちに、それらの必須ではない構成要素が必須であるとの認定をするべきではない。
【0114】
例えば、運転支援システム１０００における移動体１は、前述の例に限定されない。例えば、移動体１は、車両又は船舶に限定されず、自動運転を実行可能であれば、任意の移動体が採用され得る。移動体１は、自律ロボット（例えば、キャリアパレット）、又は飛行装置（例えば、ドローン）等であってもよい。
【0115】
運転支援システム１０００に含まれる移動体１の個数は、図１の例に限定されず、任意である。
【0116】
アクチュエータ１２は、移動体１の移動に関するアクチュエータである。アクチュエータ１２は、電動モータ、エンジン及びステアリング装置に限定されない。例えば、移動体１が船舶の場合は、プロペラ及びスラスタ等がアクチュエータである。移動体１がドローンの場合は、プロペラがアクチュエータである。
【0117】
制約条件によって制限される対象は、指令速度に限定されない。アクチュエータ１２よりも前段の指令値であれば、任意の指令値が制限の対象となる。つまり、前述の説明において、「指令速度」は「指令値」と読み替えることができる。例えば、指令力が制限されてもよい。その場合、制御演算器２１１の詳細な機能ブロックは、図１５に示すようになる。図１５の制御演算器２１１では、設定器２２からの指令速度がサーボ制御器２７へ直接入力される。サーボ制御器２７から出力される指令力が補正器２２６へ入力される。補正器２２６は、制限器２２５からの制約条件によって定められる制限範囲内へ指令力を補正する。この例では、補正器２２６は、操作量演算器としても機能し、アクチュエータ１２の指令操作量も演算する。制限器２２５は、移動体１の動力学モデル、即ち、ダイナミクスモデルを用いて、力指令を用いて、移動体１と障害との間に所定の距離が確保されるように指令力の制約条件を設定する。例えば、外乱の影響及びモデル誤差が小さい場合には、このようにサーボ制御の後段において指令値を制限してもよい。
【0118】
制限器２５は、制御特性上に制約条件及びアクチュエータ特性上の制約条件を設定しているが、これに限定されない。安全上の制約条件以外の制約条件は、必須ではない。
【0119】
フローチャートは、一例に過ぎない。フローチャートにおけるステップを適宜、変更、置き換え、付加、省略等を行ってもよい。また、フローチャートにおけるステップの順番を変更したり、直列的な処理を並列的に処理したりしてもよい。例えば、デッドロックの発生が判定された後に、制約条件の緩和（ステップＳ１０９及びＳ１１０）を行わずに、指令値の予備補正（ステップＳ１１１）がまず行われてもよい。指令値の予備補正を行ってもデッドロックが解消しない場合に、制約条件の緩和が行われてもよい。
【0120】
あるいは、デッドロックの判定以降のステップＳ１０８，Ｓ１０９，Ｓ１１０及びＳ１１１を省略してもよい。すなわち、ステップＳ１０７において指令値が補正された後は、補正後の指令値に指令値が決定され（ステップＳ１０５）、移動体１の駆動が行われてもよい（ステップＳ１０６）。このときデッドロックが生じた場合には、移動体１は停止し得る。しかし、時間の経過とともに、障害を含む周辺環境が変化して、デッドロックが自然と解消し得る。
【0121】
本明細書中に記載されている構成要素により実現される機能は、当該記載された機能を実現するようにプログラムされた、汎用プロセッサ、特定用途プロセッサ、集積回路、ＡＳＩＣｓ（Application Specific Integrated Circuits）、ＣＰＵ（a Central Processing Unit）、従来型の回路、及び／又はそれらの組合せを含む、回路（circuitry）又は演算回路（processing circuitry）において実装されてもよい。プロセッサは、トランジスタ及びその他の回路を含み、回路又は演算回路とみなされる。プロセッサは、メモリに格納されたプログラムを実行する、プログラマブルプロセッサ（programmed processor）であってもよい。
【0122】
本明細書において、回路（circuitry）、ユニット、手段は、記載された機能を実現するようにプログラムされたハードウェア、又は実行するハードウェアである。当該ハードウェアは、本明細書に開示されているあらゆるハードウェア、又は、当該記載された機能を実現するようにプログラムされた、又は、実行するものとして知られているあらゆるハードウェアであってもよい。
【0123】
当該ハードウェアが回路（circuitry）のタイプであるとみなされるプロセッサである場合、当該回路、手段、又はユニットは、ハードウェアと、当該ハードウェア及び又はプロセッサを構成する為に用いられるソフトウェアの組合せである。
【0124】
本開示の技術をまとめると、以下のようになる。
【0125】
［１］移動体１を移動させるためのアクチュエータ１２の操作量を演算する制御装置１００は、前記移動体１を目標位置へ移動させるための指令値を設定する設定器２２と、障害を回避するための指令値の制約条件を設定する制限器２５と、前記指令値が前記制約条件によって定められる制限範囲外である場合に、前記指令値を前記制限範囲内へ補正する補正器２６と、前記指令値に基づいて前記操作量を演算する操作量演算器２８とを備える。
【0126】
この構成によれば、移動体１を目標位置に移動させるための指令値が暫定的に設定される。このとき、障害を回避するように指令値を制限する制約条件が設定される。制約条件は、指令値の制限範囲を定める。暫定的な指令値が制限範囲外の場合には、指令値は制限範囲内へ補正される。指令値に基づいて求められた操作量によれば、移動体は、障害を回避しつつ、目標位置へ向かって移動する。このとき、制御装置１００は、障害を回避するように目標位置を変更したり、経路を変更したりする必要がない。つまり、制御装置１００は、指令値を制限するだけで、目標位置又は経路を変更するための演算を行う必要がない。その結果、制御装置１００の演算負荷を低減されるので、指令値を迅速に制限でき、障害に対して瞬時に対応することができる。
【0127】
［２］ ［１］に記載の制御装置１００において、前記制限器２５は、前記制約条件を逐次演算して、前記制約条件を更新する。
【0128】
この構成によれば、制約条件は、常に一定ではなく、刻々と変化する移動体１及びその周辺環境に対応して更新される。その結果、状況に応じた制限範囲が設定され、移動体１の安全が適切に確保される。
【0129】
［３］ ［１］又は［２］に記載の制御装置１００において、前記指令値としての前記移動体１の指令速度に基づいてサーボ制御を実行するサーボ制御器２７をさらに備え、前記補正器２６は、前記指令速度を前記制限範囲内へ補正して最終的な指令速度を前記サーボ制御器２７へ出力し、前記サーボ制御器２７は、前記最終的な指令速度に基づいて指令力を出力し、前記操作量演算器２８は、前記指令力に基づいて前記操作量を演算する。
【0130】
この構成によれば、サーボ制御器２７よりも前段で指令値が制限されるため、サーボ制御器２７によるサーボ制御によって調節された指令力が制限されずに出力される。つまり、外乱の影響又はモデル誤差を低減させる指令力が出力される。この指令力に基づいて操作量が演算されるので、移動体１を安定的且つ精度よく移動させることができる。
【0131】
［４］ ［１］乃至［３］の何れか１つに記載の制御装置１００において、前記制限器２５は、障害の回避に加え、前記サーボ制御器２７の制御特性に関する制約条件を設定する。
【0132】
この構成によれば、サーボ制御器２７の制御特性上の制約も考慮して、適切な操作量を演算することができる。詳しくは、サーボ制御器２７は、固有の制御特性を有し、全ての指令速度に対して期待通りの指令力を出力するわけではない。サーボ制御器２７の制御特性上の制約により、指令速度通りの指令力が出力されない場合があり得る。制御特性上の制約条件を予め考慮して制限範囲が設定されることによって、安全を確保しつつ且つ制御特性の影響を受け難い範囲に指令速度を補正することができる。これにより、指令速度から期待される通りの指令力が出力され、結果として、指令速度に適切に対応する操作量が演算される。
【0133】
［５］ ［１］乃至［４］の何れか１つに記載の制御装置１００において、前記制限器２５は、障害の回避に加え、前記アクチュエータ１２に関する制約条件を設定する。
【0134】
この構成によれば、アクチュエータ１２の特性上の制約を考慮して、適切な推力を発揮させることができる。詳しくは、アクチュエータ１２は、固有のアクチュエータ特性を有し、全ての指令力に対して期待通りの推力を発揮するわけではない。つまり、アクチュエータ特性上の制約により、指令速度通りの推力が出力されない場合があり得る。アクチュエータ特性上の制約条件を予め考慮して制限範囲が設定されることによって、安全を確保しつつ且つアクチュエータ特性の影響を受け難い範囲に指令速度を補正することができる。これにより、指令速度に適切に対応する推力が発揮される。
【0135】
［６］ ［１］乃至［５］の何れか１つに記載の制御装置１００において、前記制限器２５は、前記移動体１と障害との間に所定の距離が確保されるように前記制約条件を設定する。
【0136】
この構成によれば、移動体１は、移動体１と障害との間に所定の距離が確保しつつ、目標位置に向かって移動することができる。
【0137】
［７］ ［１］乃至［６］に記載の制御装置１００において、前記制限器２５は、前記移動体１のうち複数の保護部位が障害を回避するように、前記複数の保護部位に対応する複数の前記制約条件を設定する。
【0138】
この構成によれば、移動体１が目標位置へ向かって移動する際に、複数の保護部位の安全を確保することができる。
【0139】
［８］ ［１］乃至［７］の何れか１つに記載の制御装置１００において、前記制限器２５は、前記制約条件を調節可能に設定する。
【0140】
この構成によれば、制限器２５は、制約条件を柔軟に変更することができる。例えば、指令値を制限範囲内に補正した結果、移動体１の適切な移動を実現できない場合もあり得る。そのような場合に、制約条件を調節して制限範囲を変更することによって、移動体１を適切に移動させ得る。
【0141】
［９］ ［１］乃至［８］の何れか１つに記載の制御装置１００において、補正後の前記指令値が所定の閾値未満となる場合に、前記制限器２５は、前記制約条件を変更して、前記補正器２６は、変更された前記制限範囲内へ前記指令値を補正する。
【0142】
この構成によれば、補正後の指令値が微小になって移動体１の適切な移動を実現できない場合に、制約条件が変更される。制約条件が変更されると、制限範囲が変更され、補正後の指令値も変化して閾値以上となる可能性がある。つまり、制約条件を変更することによって、補正後の指令値が閾値未満となる状況が解消され得る。
【0143】
［１０］ ［１］乃至［９］の何れか１つに記載の制御装置１００において、補正後の前記指令値が所定の閾値未満となる場合に、前記補正器２６は、前記指令値を予備補正して、予備補正された前記指令値を前記制限範囲内へ補正する。
【0144】
この構成によれば、補正後の指令値が微小になって移動体１の適切な移動を実現できない場合に、指令値が予備補正される。指令値が予備補正されると、指令値を制限範囲内へ補正するための補正量も変化するため、補正後の指令値が閾値以上となる可能性がある。つまり、指令値を予備補正することによって、補正後の指令値が閾値未満となる状況が解消され得る。
【0145】
［１１］ ［１］乃至［１０］の何れか１つに記載の制御装置１００において、前記設定器２２は、前記指令値として指令速度を設定し、前記制限器２５は、前記移動体１の運動学モデルに基づいて前記制限範囲を演算する。
【0146】
この構成によれば、いわゆるキネマティクスによって移動体１がモデル化される。それにより、計算式が単純化され、ユーザによる設計が容易になる。
【0147】
［１２］ ［１］乃至［１１］の何れか１つに記載の制御装置１００において、前記指令値の補正を前記移動体１が備える表示装置１６又は警報器１７を介してユーザに報知する報知制御器２９をさらに備える。
【0148】
この構成によれば、指令値の補正が行われていることをユーザが知ることができるこれにより、ユーザは、移動体１の運転状況を的確に知ることができる。例えば、移動体１の自動運転中にユーザがオーバーライドを実行する際に、指令値の補正の有無を参考にすることができる。
【符号の説明】
【0149】
１００ 制御装置
１ 移動体
１２ アクチュエータ
１６ 表示装置
１７ 警報器
２２ 設定器
２５ 制限器
２６ 補正器
２７ サーボ制御器
２８ 操作量演算器
２９ 報知制御器
１ 移動体
１２ アクチュエータ
１６ 表示装置
１７ 警報器
２２ 設定器
２５ 制限器
２６ 補正器
２７ サーボ制御器
２８ 操作量演算器
２９ 報知制御器
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】