特許ウォッチ

公開番号2025180219
公報種別公開特許公報(A)
公開日2025-12-11
出願番号2024087395
出願日2024-05-29
発明の名称進路制御装置、自動操舵装置、進路制御方法、およびプログラム
出願人三菱重工業株式会社
代理人個人,個人,個人,個人,個人
主分類G05D 1/229 20240101AFI20251204BHJP(制御;調整)
要約【課題】小さい演算負荷で前進および後退を許容する効率的な経路を高速に演算し、移動体へ進行方向および姿勢を指示することができる進路制御装置を提供する。
【解決手段】進路制御装置は、移動体の現在の位置および姿勢を計測する第1計測器と、計画経路の経由点の位置および前記経由点における前記移動体の姿勢を設定する経由点設定器と、前記移動体の現在の位置および姿勢を始点とし、前記経由点の位置および姿勢を終点とし、前記始点を含む第一の弧と、前記終点を含む第二の弧と、前記第一の弧および前記第二の弧の両方に接する直線とに基づいて、前記始点から前記終点への最短経路に沿って進むように、前進または後退を示す前記移動体の進行方向と、前記進行方向へ進む際の前記移動体の姿勢を示す進路方位とを計算して、前記移動体の移動を制御する移動制御装置に出力する進路制御器と、を備える。
【選択図】図2
特許請求の範囲【請求項１】
移動体の現在の位置および姿勢を計測する第１計測器と、
計画経路の経由点の位置および前記経由点における前記移動体の姿勢を設定する経由点設定器と、
前記移動体の現在の位置および姿勢を始点とし、前記経由点の位置および姿勢を終点とし、前記始点を含む第一の弧と、前記終点を含む第二の弧と、前記第一の弧および前記第二の弧の両方に接する直線とに基づいて、前記始点から前記終点への最短経路に沿って進むように、前進または後退を示す前記移動体の進行方向と、前記進行方向へ進む際の前記移動体の姿勢を示す進路方位とを計算して、前記移動体の移動を制御する移動制御装置に出力する進路制御器と、
を備える進路制御装置。
続きを表示（約 2,100 文字）【請求項２】
前記移動体の進路上の障害物の位置を検出する第２計測器と、
前記障害物の位置に基づいて前記障害物を取り囲むように設定した回避円の円周上に、前記障害物を前記回避円に沿って右に避ける臨時の右経由点の位置および姿勢と、左に避ける臨時の左経由点の位置および姿勢とを設定する臨時経由点設定器と、
前記計画経路の経由点、前記右経由点、および前記左経由点のいずれか一つにおける位置および姿勢を、次の経由点の位置および姿勢として前記進路制御器に出力する調停器と、
をさらに備える請求項１に記載の進路制御装置。
【請求項３】
前記第２計測器が検出した複数の前記障害物それぞれの位置から、前記障害物それぞれを取り囲む回避円を設定する回避円設定器と、
複数の前記障害物を要素とする集合から、複数の前記回避円が重なる要素を抽出して連結障害物集合を定める連結障害物抽出器と、
をさらに備え、
前記臨時経由点設定器は、前記連結障害物集合に基づいて複数の前記障害物を回避する前記右経由点および前記左経由点の位置および姿勢を設定する、
請求項２に記載の進路制御装置。
【請求項４】
前記臨時経由点設定器は、
前記連結障害物集合の要素に基づいて前記移動体から見て複数の前記回避円それぞれの右側を見る視線方位が最も右側となる要素を右側障害物と定め、複数の前記回避円それぞれの左側を見る視線方位が最も左側となる要素を左側障害物と定め、
右側障害物の回避円の円周上に前記右経由点の位置および姿勢を設定し、左側障害物の回避円の円周上に前記左経由点の位置および姿勢を設定する、
請求項３に記載の進路制御装置。
【請求項５】
前記障害物が移動体である場合に、自移動体と、前記障害物である相手移動体とが最接近するときの前記相手移動体の相対位置を予測する相対位置予測器をさらに備え、
前記臨時経由点設定器は、前記相対位置に基づいて、前記相手移動体を回避する臨時の経由点を設定する、
請求項２に記載の進路制御装置。
【請求項６】
前記障害物が前記移動体の右にあるかまたは左にあるかに基づいて、前記回避円の半径を変更する半径設定器をさらに備える、
請求項２に記載の進路制御装置。
【請求項７】
前記臨時経由点設定器は、半径が固定値である前記回避円に加えて、半径が可変値である第二の回避円を設定し、前記移動体の位置、前記障害物の位置、および前記移動体の回転半径に基づいて、前記第二の回避円の半径が前記回避円の半径以下の値となるように調整し、前記第二の回避円の円周上に、前記障害物を前記第二の回避円に沿って避ける第二の右経由点および第二の左経由点の位置および姿勢をさらに設定し、
前記進路制御器は、前記第二の回避円の半径が前記回避円の半径よりも小さく、かつ、前記移動体が前進している場合に、前記第一の弧と、前記第二の回避円である前記第二の弧と、前記第一の弧および前記第二の弧の両方に接する前記直線とに基づいて、前記移動体の進行方向を前方に制約して計算した前記進路方位を前記移動制御装置に出力する、
請求項２に記載の進路制御装置。
【請求項８】
請求項１から７のいずれか一項に記載の進路制御装置と、
前記進路制御装置から出力された前記進行方向および前記進路方位に基づいて、前記移動体を制御する移動制御装置と、
を備え、
前記移動制御装置は、
前記進行方向に基づいて、前記移動体の速力指令値を計算する速力制御器と、
前記進路方位と、前記速力指令値とに基づいて、前記移動体の舵角指令および推力指令を計算する運動制御器と、
を有する、
自動操舵装置。
【請求項９】
前記速力制御器は、
前記移動体が後退を許容する場合、前記進行方向が後退のときの速力指令値が負の値となるように計算し、
前記移動体が後退を許容しない場合、前記進行方向が後退のときの速力指令値がゼロまたは正の値となるように計算する、
請求項８に記載の自動操舵装置。
【請求項１０】
移動体の現在の位置および姿勢を計測するステップと、
計画経路の経由点の位置および前記経由点における前記移動体の姿勢を設定するステップと、
前記移動体の現在の位置および姿勢を始点とし、前記経由点の位置および姿勢を終点とし、前記始点を含む第一の弧と、前記終点を含む第二の弧と、前記第一の弧および前記第二の弧の両方に接する直線とに基づいて、前記始点から前記終点への最短経路に沿って進むように、前進または後退を示す前記移動体の進行方向と、前記進行方向へ進む際の前記移動体の姿勢を示す進路方位とを計算して、前記移動体の移動を制御する移動制御装置に出力するステップと、
を有する進路制御方法。
（【請求項１１】以降は省略されています）
発明の詳細な説明【技術分野】
【０００１】
本開示は、進路制御装置、自動操舵装置、進路制御方法、およびプログラムに関する。
続きを表示（約 1,900 文字）【背景技術】
【０００２】
特許文献１には、状態量検出装置、障害物検出装置、移動体制御装置を備える移動体制御システムが記載されている。移動体制御装置は、状態量検出装置から自機の現在の状態量を受け取ると、障害物検出装置が検知した障害物の位置に基づいて、自機が障害物に衝突しない条件としての障害物回避制約関数を算出する。また、移動体制御装置は、自機の状態遷移方程式に対する自機の状態量の不確かさと、自機に働く外乱を基にマージン関数を算出し、次いで、算出されたマージン関数を障害物回避制約関数に加えた関数を制約として、モデル予測制御を行い移動体の最適制御量を求める。その後、移動体制御装置は、求めた最適制御量を、移動体の制御量の目標値として移動体へ与える。
【０００３】
特許文献２には、ミリ波レーダーやカメラで検出したターゲットの未来の位置と、自車の未来の位置とを予測して、モデル予測制御による衝突を回避するための最適操作量を算出する衝突回避制御装置が記載されている。
【０００４】
特許文献３には、Ｄｕｂｉｎｓパスを利用して、自律移動体が障害物等の干渉を避ける経路を生成する経路生成装置が記載されている。Ｄｕｂｉｎｓ経路は始点を含む円弧と、終点を含む円弧と、二つの円弧に接する直線とからなる経路である。
【０００５】
非特許文献１にはＤｕｂｉｎｓパスを生成する技術が記載されている。また、非特許文献２には、前進のみの経路に限定されたＤｕｂｉｎｓパスを拡張し、後退を含めたＲｅｅｄｓ－Ｓｈｅｐｐパスを生成する技術が記載されている。
【先行技術文献】
【特許文献】
【０００６】
特開２０２３－１４９９１０号公報
特開２００７－２７６５０８号公報
特開２０２３－５９８１５号公報
【非特許文献】
【０００７】
L. E. Dubins, "On Curves of Minimal Length with a Constraint on Average Curvature, and with Prescribed Initial and Terminal Positions and Tangents", American Journal of Mathematics, 1957, Vol. 79, No.3, 497-516
James Reeds and Lawrence Shepp, "Optimal paths for a car that goes both forwards and backwards", Pacific journal of mathematics, 1990, Vol.145, No.2, 367-393
【発明の概要】
【発明が解決しようとする課題】
【０００８】
移動体が障害物を回避するには、障害物の位置に基づいて経路を生成する必要がある。障害物は移動体の進行方向に突然現れる場合もあるので、障害物を回避する経路はリアルタイムに生成しなければならない。このような場合には、特許文献１～２のモデル予測制御が有望である。しかしながら、モデル予測制御で行う最適化制御量の演算負荷が高いので、通常の自動操舵装置の計算部でリアルタイムに経路生成をするのは困難である。そうすると、従来技術では、通常の自動操舵装置の計算部とは別に、モデル予測制御のための演算装置を設けなければならない。また、移動体の姿勢が不連続に変化することを避けるために、経路を滑らかに曲線で表現しようとすると、経路上に経由点（ウェイポイント）を密に多数設定しなければならず、その分、さらに演算負荷が高くなる。特に、衝突回避のために臨時の経路を設定する際には、多数の経由点を計算する演算負荷が高い処理を行うことは、速応性の点で不利である。
【０００９】
また、従来の技術（例えば特許文献３）では、障害物等の影響を避けて移動体の経路を生成する際に、移動体が前進のみを行う前提で経路（Ｄｕｂｉｎｓパス）を生成している。このため、後退が可能な移動体に対しかえって距離が長く非効率的な経路が生成される可能性がある。
【００１０】
本開示の目的は、小さい演算負荷で前進および後退を許容する効率的な経路を高速に演算し、移動体へ進行方向および姿勢を指示することができる進路制御装置、自動操舵装置、進路制御方法、およびプログラムを提供することにある。
【課題を解決するための手段】
（【００１１】以降は省略されています）

関連特許