JP2004276708A

JP2004276708A - Relative attitude angle detection device

Info

Publication number: JP2004276708A
Application number: JP2003069689A
Authority: JP
Inventors: Kenichi Kohata; 健一降幡
Original assignee: Isuzu Motors Ltd
Current assignee: Isuzu Motors Ltd
Priority date: 2003-03-14
Filing date: 2003-03-14
Publication date: 2004-10-07

Abstract

<P>PROBLEM TO BE SOLVED: To detect a relative attitude angle with good accuracy and favorable responsiveness at low cost without applying special processing to a vehicle to be pulled in a relative attitude angle detection device for detecting a relative attitude angle of a pulling vehicle (tractor) and a vehicle to be pulled (trailer). <P>SOLUTION: Displacement sensors 50-1, 50-2 detect relative displacement of a second reference plane of the vehicle 30 to be pulled in a vertical direction against a first reference plane 12 of the pulling vehicle 10 respectively and rotation angle sensors 51-1, 51-2 detect movement displacement in the second reference plane direction at the detection points of the displacement sensors 50-1, 50-2 respectively. A displacement sensor 50-3 is installed at a position by a predetermined distance in a longitudinal direction of the pulling vehicle 10 from the displacement sensors 50-1, 50-2 and detect relative displacement of the second reference plane in the vertical direction against the first reference plane 12 of the pulling vehicle 10. An operation part 70 operates the relative attitude angle between both vehicles based on the detection result of the displacement sensors 50-1-50-3 and rotation angle sensors 51-1, 51-2. <P>COPYRIGHT: (C)2005,JPO&NCIPI

Description

【０００１】
【発明の属する技術分野】
本発明は相対姿勢角検出装置に関し、特に牽引車両（トラクタ）と被牽引車両（トレーラ）の相対的な姿勢角を検出する相対姿勢角検出装置に関するものである。
【０００２】
【従来の技術】
連結車両は、牽引車両と被牽引車両間の相対姿勢角度により、車両の安定性を損なう可能性がある。このため、予め相対姿勢角度を検出し、連結車両を制御して安定性を保つことが行われている。
【０００３】
従来の連結車両の相対姿勢角検出装置としては、例えば、下記の角度検出装置（１）〜（６）がある。
（１）被牽引車両（トレーラ）のフレームの下面にキングピンを中心とする所定半径Ｒの円弧状の磁気スケールを埋設し、トラクタ（牽引車両）のカプラ側面部に上記磁気スケールの下側に所定の間隙をもって対向する磁気センサを取り付けて、トラクタに対するトレーラのヨー角を検出している（例えば、特許文献１参照）。
【０００４】
（２）例えば、トラクタに装着されたカプラ側にワイヤ部材の一端を固定し、トレーラ側にワイヤ部材の他端が長手方向に進退自在に支持され、ワイヤ部材がトレーラとトラクタとの間の相対的な連結角（相対姿勢角、ヨー角）の変化に伴い該他端側が長手方向に進退変位することでワイヤ案内部材に案内されて延出する。
【０００５】
このワイヤ案内部材がキングピンと同心円状に形成されているため、該進退変位はトレーラとトラクタの連結角に対応していることを利用して、連結角を測定している（例えば、特許文献２参照）。
（３）また、回転角検出器の回転軸をカプラの回転軸と一致するように、例えば、トラクタ側に設置する。そして、回転角検出器の両側にはそれぞれアームの一端を取り付け、アームの他端は、トレーラ側の設置した取付台に摺動自在にしておく。これにより、走行中のローリングやピッチングは、回転検出器の回転方向には影響は与えない。
【０００６】
トラクタがトレーラに対して旋回すると、アームが回転角検出器を同じ角度だけ回動させるので、回転角検出器は、トラクタとトレーラとの相対的なヨー角を検出する（例えば、特許文献３参照）。
（４）トラクタ及びトレーラにそれぞれ電子式の方向センサを搭載し、各方向センサで測定されたトラクタ及びトレーラの方位角に基づき、トラクタとトレーラとの間の相対的な屈曲角（相対姿勢角）を決定する（例えば、特許文献４参照）。
【０００７】
（５）例えば、トラクタに取り付けたスキャンレーザレーダによりトレーラの前面パネル上の複数点の距離と角度を検出し、これらの検出値からトラクタの後面パネルとトレーラの前面パネルの傾き角を算出することにより、トラクタとトレーラの相対的な連結角（相対姿勢角）を算出する（例えば、特許文献５参照）。
【０００８】
（６）オブザーバ等が、ヨーレート、横向き加速度、及びステアリング角度等の牽引車両の動き、並びに予め記憶された車両の諸元に基づき、相対姿勢角に対応する値を推定する（例えば、特許文献６参照）。
【０００９】
【特許文献１】
特開８−３３２９７３号公報（第２頁、図２及び図３）
【００１０】
【特許文献２】
特開平１１−１６００６３号公報（第２頁、図３）
【００１１】
【特許文献３】
特開平１１−２７８３２０号公報（第２頁、図６）
【００１２】
【特許文献４】
特開２００１−２２７９０５号公報（第２頁、図１）
【００１３】
【特許文献５】
特開２００１−３３４９６６号公報（第２頁、図１）
【００１４】
【特許文献６】
特開平１０−１０３７号公報（第５頁、図１）
【００１５】
【発明が解決しようとする課題】
このような従来の相対姿勢角検出装置（１）〜（４）は、頻繁に連結及び切離を繰り返したり、各種の被牽引車両を連結する連結車両においては、被牽引車両が磁気スケール、変位センサ、回転角センサ、又は方向センサ等を備えていなければならず、これらを備えていない被牽引車両に対応することができない。また、被牽引車両に磁気スケール、変位センサ、又は方向センサ等を搭載するための加工が必要になる。
【００１６】
また、相対姿勢角検出装置（５）は、レーザレーダは、非常にコストが高く実用的でないという問題がある。
また、角度推定装置（６）は、精度が悪く、牽引車両の動きから推定するため応答性に問題がある。
【００１７】
従って本発明は、牽引車両と被牽引車両との間の相対的な姿勢角を検出する相対姿勢角検出装置において、被牽引車両に特別な加工を施すことなく、低コストで、精度及び応答性良く相対姿勢角を検出することを課題とする。
【００１８】
【課題を解決するための手段】
上記の課題を解決するため、本発明の相対姿勢角検出装置は、被牽引車両と結合される牽引車両の車幅方向の位置に設置された第１及び第２の変位検出部であって、該牽引車両の第１の基準面に対して垂直方向における、該被牽引車両の第２の基準面の相対的な変位をそれぞれ検出するものと、該第１及び第２の変位検出部の検出点における該第２の基準面方向の移動変位を、それぞれ検出する第３及び第４の変位検出部と、該第１及び第２の変位検出部から該牽引車両の前後方向に所定の距離だけ離れた位置に設置された第５の変位検出部であって、該牽引車両の該第１の基準面に対する垂直方向における第２の基準面の相対的な変位を検出するものと、該第１から該第５までの変位検出部の検出結果に基づき両車両間の相対姿勢角を演算する演算部とを備えたことを特徴としている。
【００１９】
また、上記の本発明において、該第１、第２、及び第５の変位検出部が、各変位を伝えるための変位伝達機構と、変位センサとを備えることができる。
また、上記の本発明において、該第３及び第４の変位検出部が、それぞれ、該第１及び第２の変位検出部の先端に設置され、該第２の基準面に接触して該移動変位に伴って回転する回転部材と、該回転部材の回転角を検出する回転角センサとを備えることができる。
【００２０】
また、上記の本発明において、該相対姿勢角として、該牽引車両に対する該被牽引車両のピッチ角、ロール角、及びヨー角の内の少なくとも１つとすることができる。
また、上記の本発明において、該演算部は、該牽引車両上の座標系又は被牽引車両上の座標系に基づき該ピッチ角、該ロール角、及び該ヨー角を演算することができる。
【００２１】
すなわち、本発明の相対姿勢角検出装置は、カプラを介して結合される牽引車両の殆どのフレーム上面（第１の基準面）及び被牽引車両のフレーム下面（第２の基準面）が平面であることに着目し、例えば、牽引車両のフレーム上面に、カプラを挟む形で、該牽引車両の車幅方向に互いに相対する位置に第１及び第２の変位検出部を設置し、第１及び第２の変位検出部から所定の距離だけ前方又は後方に離れた位置に、第３の変位検出部を設置する。
【００２２】
第１、第２及び第５の変位検出部の変位伝達機構は、それぞれ、牽引車両のフレーム上面と被牽引車両のフレーム下面との間の該フレーム上面方向の変位を各変位センサに伝え、この変位を各変位センサが検出する。
第１及び第２の変位伝達機構の先端に設置された各回転部材は、該被牽引車両のフレーム下面に接触して、その移動変位に伴い回転する。この回転角を各回転角センサが検出する。
【００２３】
これらの第１、第２、及び第５までの変位センサの検出結果、並びに第３及び第４の回転角センサの検出結果に基づき、演算部が、牽引車両及び被牽引車両の相対姿勢角、すなわち、ピッチ角、ロール角、及びヨー角を、牽引車両上に座標系又は被牽引車両の座標系に基づき演算する。
【００２４】
【発明の実施の形態】
図１は、本発明に係る相対姿勢角検出装置１００の実施例を示しており、この相対姿勢角検出装置１００は、トラクタ１０のフレーム上面（第１の基準面）１２の車幅方向にカプラ２０を挟んだ状態で設置された変位検出部４０ａ＿１及び４０ａ＿２（以下、符号４０ａで総称することがある。）と、トラクタ１０とトレーラ３０を結合するカプラ２０の後方に設置された変位検出部４０ｂと、演算部７０とで構成されている。
【００２５】
変位検出部４０ａ＿１及び４０ａ＿２は、それぞれ、変位センサ５０＿１，５０＿２及び回転角センサ５１＿１，５１＿２を備え、変位検出部４０ｂは、変位センサ５０＿３を備えている。
図２は、一般的なカプラ２０を示しており、このカプラ２０は、トラクタ１０に固定されるカプラフレーム２４、キングピン（図示せず）で接続されたトレーラ３０の下部フレームの前部を支えるカプラベース２１、このカプラベース２１にトラクタ１０に対してローリング及びピッチング方向の回転をそれぞれが与えるローリングアクスル２２及びピッチングアクスル２３を備えている。
【００２６】
図３（１）は、変位検出部の構成例（１）を示しており、特に、図１に示した変位検出部４０ａの構成を示している。この変位検出部４０ａは、トレーラのフレーム下面に接触するキャスタホイール（回転部材）４１、このキャスタホイール４１の回転角を検出する回転角センサ５１、キャスタホイール（回転部材）４１を保持するキャスタホルダ４２ａ、このキャスタホルダ４２ａに固着されたロッド４３ａ、このロッド４３ａを上方に押し上げるリターンスプリング４６、及びロッド４３ａの相対変位を検出する変位センサ５０を備えている。
【００２７】
図３（２）は、図１に示した変位検出部４０ｂの構成例（１）を示している。この変位検出部４０ｂが、同図（１）に示した変位検出部４０ａと異なる点は、回転角センサ５１が無いことと、キャスタホルダ４２ｂが回転軸中心８０に回転することが可能なことである。
【００２８】
図４は、変位検出部４０ａの構成例（２）を示しており、特に、図１に示した変位検出部４０ａの構成例（２）を示しており、同図（１）は側面図、同図（２）は正面図を示している。
この変位検出部が、同図（１）に示した変位検出部４０ａと異なる点は、ロッド４３ａの代わりに支点５２を中心に回転することが可能なアーム４４を備えていることと、変位センサ５０の代わりに、アーム４４の回転角に基づき変位を検出する回転型の変位センサ５０ｂを備えていることと、リターンスプリング４６の代わりに、アーム４４の先端に設置されたキャスタホイール４１を上方に押し上げるねじりスプリング４７を備えていることである。
【００２９】
図５は、変位検出部の構成例（３）を示しており、特に、図１に示した変位検出部４０ｂの構成を示している。この変位センサ４０ｂが、図４に示した変位検出部４０ａと異なる点は、アーム４４の代わりに、デュアルリンクアーム４５を備えていること、回転角センサ５１が無いこと、及びキャスタホイール４１が垂直軸を中心として回転する機構を備えていることである。
【００３０】
図６は、キャスタホイール４１の変形例を示しており、このキャスタホイール４１は、ボール６０とボールホルダ６１で構成されている。
図３（２）及び図５に示した変位検出部４０ｂのキャスタホイール４１及びキャスタホルダ４２ｂの代わりに、図６のキャスタを用いることが可能である。このとき、ボールホルダ６１とロッド４３ｂ又は機構４８は回転させる必要がないので、固着してもよい。
【００３１】
図７は、トラクタ１０の側面方向から見た変位検出部４０ａ及び変位検出部４０ｂの配置を示している。変位検出部４０ａ＿１及びカプラ２０の反対側に隠れている変位検出部４０ａ＿２は、カプラ２０のピッチングアクスル２３の中心軸を含む垂直平面上又はその近傍に配置されている。
【００３２】
また、変位検出部４０ｂは、ピッチングアクスル２３の中心軸からｌ_θだけ後方の位置に配置されている。変位検出部４０ｂの上端はトレーラ３０の底面３１に接している。
図８は、トラクタ１０の正面方向から見た変位検出部４０ａ＿１及び４０ａ＿２の配置を示している。変位検出部４０ａ＿１及び４０ａ＿２は、ローリングアクスル２２の中心軸を含む垂直平面に対してそれぞれｌ_φだけ離れた対称な位置又はその近傍に配置されている。変位検出部４０ａ＿１及び４０ａ＿２の上端はトレーラ底面３１，３１’に接している。
【００３３】
変位検出部４０ｂは、カプラ２０の反対側に隠れて見えないが、ローリングアクスル２２の中心軸を含む垂直平面上又はその近傍に配置されている。
〔１〕牽引車両を基準とした相対姿勢角（θ，φ，ψ）の演算
変位検出部４０ａ＿１，４０ａ＿２，４０ｂの変位センサの検出変位を、それぞれｈ_１，ｈ_２，ｈ_３とする。
【００３４】
図７において、ピッチ角θは、次式で示すことができる。
【００３５】
【数１】

【００３６】
図８において、ロー角φは、次式で示すことができる。
【００３７】
【数２】

【００３８】
一方、ヨー角ψは、キャスタホイール４１の回転半径Ｒ、回転角センサ５１の検出結果ｎと、ロール角φ、及び各変位検出部４０ａ＿１，４０ａ＿２，及び４０ｂの設置位置（図１参照）、ピッチング等により発生する誤差αに基づき次のように計算される。
すなわち、被牽引車両３０のピッチ軸となるカプラ２０のピッチングアクスル２３が、牽引車両１０に固定されているため、同様に牽引車両に固定されている変位検出部４０ａ＿１及び４０ａ＿２のキャスタホイール４１の回転角は、互いに等しい値の誤差αを含む。
【００３９】
したがって、変位検出部４０ａ＿１及び４０ａ＿２からの検出結果である回転角を、それぞれｎ_１ｓ，ｎ_２ｓとし、ピッチング等により発生する回転数の誤差αを除いたキャスタホイール回転角ｎ_１ｒ，ｎ_２ｒとしたときの関係式は次のようになる。
【００４０】
【数３】

【００４１】
【数４】

【００４２】
次に、ピッチ角θ＝“０”、及びロール角φ＝“０”の場合であって、図９に示すようにヨー角ψが発生した場合、キャスタホイール４１＿１，４１＿２は、半径ｌ_φの円上をトレースし、このとき誤差α以外の誤差は無視できる程微少であるから、次式が成り立つ。
【００４３】
【数５】

【００４４】
キャスタホイール４１の回転によって移動する距離（変位）は、ヨー角ψ分の円弧の長さに等しいので、キャスタホイールの半径をＲとすると次式が成り立つ。
【００４５】
【数６】

【００４６】
図８に示したように、ロール角φが発生すると、被牽引車両３０のヨー角回転軸がカプラ上面に固定されており、しかも、ヨー角回転面であるカプラ上面はカプラのローリングアクスルからｈ_ｃｒだけ上方にあるため、ヨー角回転中心が移動し、かつ、回転面がロール角φだけ傾くことから、左右の変位検出部４０ａ＿１，４０ａ＿２のキャスタホイール４１は各々半径の異なる円周上をトレースすることになる。
【００４７】
ロール角φによる中心の移動量をα_ｒとすれば、次式が成り立つ。
【００４８】
【数７】

【００４９】
したがって、傾きφを考慮した回転半径は、それぞれ、次式で示すことができる。
【００５０】
【数８】

【００５１】
【数９】

【００５２】
φ≪１であることを考慮すると、回転半径は、それぞれ、次式で示すことができる。
【００５３】
【数１０】

【００５４】
【数１１】

【００５５】
したがって、回転数ｎ_１ｒ，ｎ_２ｒとヨー角ψとの関係は、それぞれ、次式で示すことができる。
【００５６】
【数１２】

【００５７】
【数１３】

【００５８】
したがって、この場合、左右のキャスタホイールの回転角の比ｎ_２ｒ／ｎ_１ｒは、上記の式（１３）を式（１２）で除した次式で示すことができる。
【００５９】
【数１４】

【００６０】
この式を変形すると回転数ｎ_１ｒと回転数ｎ_２ｒの差は、ｎ_１ｒを用いて次式で示すことができる。
【００６１】
【数１５】

【００６２】
ここで、ピッチ角θにより誤差αが発生したとすると、式（３）及び式（４）から次式が成り立つ。
【００６３】
【数１６】

【００６４】
式（３）、式（１５）、及び式（１６）から、誤差αは、ｎ_１ｓ，ｎ_２ｓ，φを用いて次式で示すことができる。
【００６５】
【数１７】

【００６６】
したがって、次式が成立する。
【００６７】
【数１８】

【００６８】
したがって、式（１２）、式（１８）より、ヨー角ψは次式で示すことができる。
【００６９】
【数１９】

【００７０】
〔２〕相対姿勢角（θ，φ，ψ）から被牽引車両の相対姿勢角（θ _ｔ，φ _ｔ，ψ _ｔ）への変換
次に、連結車両の運動を考えた場合、牽引車両１０の前後方向をｘ軸、水平方向（左右方向）をｙ軸、及び垂直方向をｚ軸とした直交座標系（ｘ，ｙ，ｚ）上の変位検出部で検出された相対姿勢角（ピッチ角θ、ロール角φ、ヨー角ψ）を、被牽引車両３０の前後方向をｘ’軸、水平方向をｙ’軸、垂直方向をｚ’軸としたを直交座標系（ｘ’，ｙ’，ｚ’）を基準とした相対姿勢角（ピッチ角θ_ｔ、ロール角φ_ｔ、ヨー角ψ_ｔ）に換算する必要がある。
【００７１】
図１０（１）は、３次元直交座標系を示しており、ｘ軸は牽引車両１０の前後方向に平行であり、ｙ軸は左右方向に平行であり、ｚ軸は垂直方向に平行であると仮定する。また、平面ｘｏｙは、被牽引車両３０の基準水平面、平面ｘ’ｏｙは、基準水平面ｘｏｙをｙ軸回りにθ〔ｒａｄ〕だけ回転させた平面、平面ｘ’ｏｙ’は、さらにｘ’軸回りにφ〔ｒａｄ〕だけ回転させた被牽引車両３０のフレーム底面と平行な平面を表している。
【００７２】
四角形ＡＢＣＤは、平面ｘｏｙ上にあり、原点ｏを中心とした一辺の長さが２ｌの正方形である。また、辺ＡＤとｙ軸の交点ａは、辺ＡＤの中点であり、線分ｏａの長さはｌである。四角形Ａ’Ｂ’Ｃ’Ｄ’、及び四角形Ａ”Ｂ”Ｃ”Ｄ”は、それぞれ、正方形ＡＢＣＤを底面、又は水平断面とした平面ｘｏｙに垂直な四角柱の平面ｘ’ｏｙ、及び平面ｘ’ｏｙ’による断面を表している。
【００７３】
平面ｘ’ｏｙ’上で相対ヨー角ψが発生した場合を考え、被牽引車両３０と牽引車両１０の相対ロール角φ_ｔを算出する。原点ｏを通る直線ｂ_２ｃ_２は平面ｘ’ｏｙ’上でｙ’軸と角度ψを成す。すなわち、平面ｘ’ｏｙ’上でｙ’軸からｚ’軸回りにψ〔ｒａｄ〕だけ回転した直線である。この直線ｂ_２ｃ_２は、被牽引車両３０の水平軸を表している。
【００７４】
求める角度は、直線ｂ_２ｃ_２と、直線ｂ_２ｃ_２を平面ｘｏｙ上に投影した直線ｂｃとの成す角ｂｏｂ_２＝φ_ｔである。
分かり易くするため、次のように補助線を引く。まず、点ｂ_２を通りｘ軸に平行な直線ｆｇを引く。次に辺ＡＤとｙ軸の交点ａと辺Ａ”Ｄ”とｙ’軸の交点ａ_２を結ぶ直線を引く。さらに、この点ａ_２を通りｚ軸に平行な直線を引き、この直線と、直線ｆｇ及び辺ＡＤとの交点をそれぞれ点ａ_３及び点ａ_４とする。さらに、点ａを通りｚ軸に平行な直線を引き、この直線と辺Ａ”Ｄ”及び直線ｆｇとの交点を、それぞれ、点ａ_５及び点ａ_６とする。
【００７５】
同図（２）は、これらの辺ＡＤ，Ａ’Ｄ’，Ａ”Ｄ”、直線ｆｇ、及び交点ａ，ａ２〜ａ_６，ｂ，ｂ_２を、平面ｘｏｚに投影した図である。
まず、実際に検出されるロール角は、平面ｙｏｚと平面ｘ’ｏｙ’の交線とｙ軸とが成す角である。これを角φ’とする。同図（１）において、角ｏａａ_５は直角であるから、次式が成り立つ。
【００７６】
【数２０】

【００７７】
同図（２）において、直線ａａ_５と直線ａ_４ａ_２は、平行であるから次式が成り立つ。
【００７８】
【数２１】

【００７９】
角ａ_２ａｏは直角であるから、次式が成り立つ。
【００８０】
【数２２】

【００８１】
ここで、θは１０°以下であるから、θ〔ｒａｄ〕≪１であるとすると、ｃｏｓθ≒１であると看做せるので、次式が成り立つ。
【００８２】
【数２３】

【００８３】
すなわち、φ＝φ’と看做せることが分かる。
次に、線分ａａ_２の長さを求める。角ａ_２ａｏは直角であり、角ａｏａ_２はφ〔ｒａｄ〕、線分ａｏの長さがｌであるから、次式が成り立つ。
【００８４】
【数２４】

【００８５】
【数２５】

【００８６】
角ａａ_４ａ_２が直角であり、角ａａ_２ａ_４はθ〔ｒａｄ〕であるから、次式が成り立つ。
【００８７】
【数２６】

【００８８】
辺ＡＤと直線ｆｇが平行であることから、角ａ_２ａ_３ｂ_２は直角であり、角ａ_２ｂ_２ａ_３はθ〔ｒａｄ〕であるから、次式が成り立つ。
【００８９】
【数２７】

【００９０】
角ｂ_２ａ_２ｏは直角、角ａ_２ｏｂ_２はψ〔ｒａｄ〕であるから、次式が成り立つ。
【００９１】
【数２８】

【００９２】
【数２９】

【００９３】
上記の式（２７）〜（２９）から、次式が導き出せる。
【００９４】
【数３０】

【００９５】
線分ａ_３ａ_４の長さは、線分ａ_２ａ_４と線分ａ_２ａ_３の和であり、辺ＡＤと直線ｆｇが平行であることから線分ｂｂ_２は、次式で示すことができる。
【００９６】
【数３１】

【００９７】
ここで、角ｏｂｂ_２は直角であるから、次式が成り立つ。
【００９８】
【数３２】

【００９９】
ここで、φ_ｔ，φ，θ≪１であることを考慮すると、次式が成り立つ。
【０１００】
【数３３】

【０１０１】
次に、相対ピッチ角θ_ｔを求める。この相対ピッチ角θ_ｔは、上記のロール角φ_ｔを求めた条件において、座標軸ｘ，ｘ’をｙ，ｙ’に、ｙ，ｙ’をｘ，ｘ’に、角φ_ｔ，φ，θをそれぞれ角θ_ｔ，θ，−φに置き換えれば同様に求められ、次式が成り立つ。
【０１０２】
【数３４】

【０１０３】
次に、相対ヨー角ψ_ｔを求める。この相対ヨー角ψ_ｔは、直線ｂ_２ｃ_２を平面ｘｏｙ上に投影した直線ｂｃとｙ軸が成す角である。したがって、上記の式（２０）及び角ａａ_２ａ_５が直角であること、及び式（２３）から、次式が成り立つ。
【０１０４】
【数３５】

【０１０５】
また、上記の式（２５）及び（２８）から、次式が導き出せる。
【０１０６】
【数３６】

【０１０７】
したがって、線分ａ_５ｂ_２は次式で示すことができる。
【０１０８】
【数３７】

【０１０９】
この式（３７）、及び角ａ_５ａ_６ｂ_２が直角であることから、次式が成り立つ。
【０１１０】
【数３８】

【０１１１】
また、角ｂａｏが直角であることから、ヨー角ψ_ｔは、次式で示すことができる。
【０１１２】
【数３９】

【０１１３】
この式（３８）において、θ，φ≪１であることを考慮すると、次式が成り立つ。
【０１１４】
【数４０】

【０１１５】
したがって、ヨー角ψ_ｔは、次式で示すことができる。
【０１１６】
【数４１】

【０１１７】
以上により、牽引車両側で検出された相対姿勢角（φ，θ，ψ）を変換することによって得られた被牽引車両側に相対姿勢角（φ_ｔ，θ_ｔ，ψ_ｔ）を、連結車両を制御するためのデータとして利用することが可能になる。
【０１１８】
【発明の効果】
本発明の相対姿勢角検出装置によれば、被牽引車両に加工又はセンサの設置をすることなく、また、連結時又は切離時に特別な作業を必要とせずに、低コストで、精度及び応答性良く連結車両の相対姿勢角を検出することが可能になる。
【図面の簡単な説明】
【図１】本発明に係る相対姿勢角検出装置の実施例を示した平面図である。
【図２】一般的な牽引車両と被牽引車両を結合するカプラの構成例を示した斜視図である。
【図３】本発明に係る相対姿勢角検出装置における変位検出部の構成例（１）を示した図である。
【図４】本発明に係る相対姿勢角検出装置における変位検出部の構成例（２）を示した図である。
【図５】本発明に係る相対姿勢角検出装置における変位検出部の構成例（３）を示した図である。
【図６】本発明に係る相対姿勢角検出装置における変位検出部の検出端の構成例を示した図である。
【図７】本発明に係る相対姿勢角検出装置における変位検出部の配置例を示した側面図図である。
【図８】本発明に係る相対姿勢角検出装置における変位検出部の配置例を示した正面図である。
【図９】本発明に係る相対姿勢角検出装置における変位検出部の配置例を示した平面図である。
【図１０】本発明に係る相対姿勢角検出装置の演算部における相対姿勢角算出の原理を示した図である。
【符号の説明】
１００相対姿勢角検出装置
１０トラクタ、牽引車両１１トラクタフレーム
１２トラクタフレーム上面、第１の基準面
２０カプラ２１，２１’ カプラベース
２２，２２’ ローリングアクスル２３ピッチングアクスル
２４カプラフレーム３０，３０’ トレーラ、被牽引車両
３１，３１’ トレーラ底面、第２の基準面
４０ａ，４０ａ＿１，４０ａ＿２，４０ｂ変位検出部
４１，４１＿１，４１＿２回転部材、キャスタホイール
４２ａ，４２ｂキャスタホルダ４３ａ，４３ｂロッド
４４アーム４５デュアルリンクアーム
４６リターンスプリング４７ねじりスプリング
４８機構
５０，５０ｂ，５０ｃ，５０＿１〜５０＿３変位センサ
５１，５１＿１，５１＿２回転角センサ５２支点
６０ボール６１ボールホルダ
７０演算部８０回転軸中心
θ，θ_ｔピッチ角 φ，φ_ｔロール角
ψ，ψ_ｔヨー角
図中、同一符号は同一又は相当部分を示す。[0001]
TECHNICAL FIELD OF THE INVENTION
The present invention relates to a relative posture angle detection device, and more particularly to a relative posture angle detection device that detects a relative posture angle between a towed vehicle (tractor) and a towed vehicle (trailer).
[0002]
[Prior art]
The connected vehicle may impair the stability of the vehicle due to the relative attitude angle between the towing vehicle and the towed vehicle. For this reason, the relative attitude angle is detected in advance, and the connected vehicle is controlled to maintain stability.
[0003]
As a conventional relative posture angle detecting device for a connected vehicle, there are, for example, the following angle detecting devices (1) to (6).
(1) An arc-shaped magnetic scale having a predetermined radius R centered on a kingpin is buried in the lower surface of a frame of a towed vehicle (trailer), and a predetermined amount is provided below the magnetic scale in a coupler side surface of a tractor (towed vehicle). The yaw angle of the trailer with respect to the tractor is detected by mounting a magnetic sensor facing the tractor (see, for example, Patent Document 1).
[0004]
(2) For example, one end of a wire member is fixed to the coupler mounted on the tractor, the other end of the wire member is supported on the trailer side so as to be able to advance and retreat in the longitudinal direction, and the wire member is positioned between the trailer and the tractor. The other end is advanced and retracted in the longitudinal direction in accordance with a change in a typical connection angle (relative attitude angle, yaw angle), and is guided and extended by the wire guide member.
[0005]
Since the wire guide member is formed concentrically with the kingpin, the connection angle is measured by utilizing the fact that the advance / retreat displacement corresponds to the connection angle between the trailer and the tractor (for example, Patent Document 2). reference).
(3) The rotation axis of the rotation angle detector is set, for example, on the tractor side so as to coincide with the rotation axis of the coupler. One end of an arm is attached to each side of the rotation angle detector, and the other end of the arm is slidable on a mounting table provided on the trailer side. Thus, rolling or pitching during traveling does not affect the rotation direction of the rotation detector.
[0006]
When the tractor turns with respect to the trailer, the arm rotates the rotation angle detector by the same angle, so that the rotation angle detector detects a relative yaw angle between the tractor and the trailer (for example, see Patent Document 3). ).
(4) Each of the tractor and the trailer is equipped with an electronic direction sensor, and based on the azimuth of the tractor and the trailer measured by each direction sensor, the relative bending angle (relative attitude angle) between the tractor and the trailer. Is determined (for example, see Patent Document 4).
[0007]
(5) For example, detecting the distance and angle of a plurality of points on the front panel of the trailer by using a scanning laser radar attached to the tractor, and calculating the inclination angle of the rear panel of the tractor and the front panel of the trailer from these detected values. Calculates the relative connection angle (relative attitude angle) between the tractor and the trailer (for example, see Patent Document 5).
[0008]
(6) The observer or the like estimates a value corresponding to the relative attitude angle based on the movement of the towing vehicle such as the yaw rate, the lateral acceleration, and the steering angle, and the specifications of the vehicle stored in advance (for example, Patent Document 6). reference).
[0009]
[Patent Document 1]
Japanese Patent Application Laid-Open No. 8-332973 (page 2, FIGS. 2 and 3)
[0010]
[Patent Document 2]
JP-A-11-160063 (page 2, FIG. 3)
[0011]
[Patent Document 3]
JP-A-11-278320 (page 2, FIG. 6)
[0012]
[Patent Document 4]
JP 2001-227905 A (page 2, FIG. 1)
[0013]
[Patent Document 5]
JP 2001-334966 A (page 2, FIG. 1)
[0014]
[Patent Document 6]
JP-A-10-1037 (page 5, FIG. 1)
[0015]
[Problems to be solved by the invention]
Such conventional relative attitude angle detection devices (1) to (4) are frequently connected and disconnected, or in a connected vehicle that connects various towed vehicles, the towed vehicle has a magnetic scale and displacement. A sensor, a rotation angle sensor, a direction sensor, and the like must be provided, and it is not possible to cope with a towed vehicle that does not include these. Further, processing for mounting a magnetic scale, a displacement sensor, a direction sensor, and the like on the towed vehicle is required.
[0016]
Further, the relative attitude angle detecting device (5) has a problem that the laser radar is very expensive and not practical.
In addition, the angle estimation device (6) has poor accuracy, and has a problem in responsiveness because it is estimated from the movement of the towing vehicle.
[0017]
Therefore, the present invention provides a relative attitude angle detection device that detects a relative attitude angle between a towed vehicle and a towed vehicle, without performing any special processing on the towed vehicle, at low cost, with accuracy and responsiveness. It is an object to detect a relative attitude angle well.
[0018]
[Means for Solving the Problems]
In order to solve the above-mentioned problem, a relative posture angle detection device of the present invention is a first and a second displacement detection unit installed at a position in a vehicle width direction of a towed vehicle combined with a towed vehicle, Detecting a relative displacement of the second reference plane of the towed vehicle in a direction perpendicular to the first reference plane of the towed vehicle, and detecting the first and second displacement detection units; Third and fourth displacement detectors for detecting movement displacements of the point in the direction of the second reference plane, respectively, and a predetermined distance from the first and second displacement detectors in the front-rear direction of the towing vehicle. A fifth displacement detection unit installed at a distant position, wherein the fifth displacement detection unit detects a relative displacement of a second reference plane in a direction perpendicular to the first reference plane of the towing vehicle; Calculates the relative attitude angle between the two vehicles based on the detection results of the displacement detection units up to the fifth It is characterized in that an arithmetic unit that.
[0019]
Further, in the above-described present invention, the first, second, and fifth displacement detection units may include a displacement transmission mechanism for transmitting each displacement, and a displacement sensor.
In the present invention, the third and fourth displacement detectors are respectively installed at the tips of the first and second displacement detectors, and come into contact with the second reference plane to perform the movement. A rotating member that rotates with the displacement and a rotation angle sensor that detects a rotation angle of the rotating member can be provided.
[0020]
In the present invention, the relative attitude angle may be at least one of a pitch angle, a roll angle, and a yaw angle of the towed vehicle with respect to the towed vehicle.
Further, in the above-described present invention, the calculation unit can calculate the pitch angle, the roll angle, and the yaw angle based on a coordinate system on the towed vehicle or a coordinate system on the towed vehicle.
[0021]
That is, in the relative attitude angle detecting device of the present invention, the upper surface (first reference surface) of most of the towed vehicle and the lower surface (second reference surface) of the towed vehicle connected via the coupler are flat. Focusing on a certain point, for example, the first and second displacement detection units are installed at positions facing each other in the vehicle width direction of the tow vehicle, with the coupler being interposed therebetween, on the upper surface of the frame of the tow vehicle, A third displacement detection unit is installed at a position away from or behind the second displacement detection unit by a predetermined distance.
[0022]
The displacement transmission mechanisms of the first, second, and fifth displacement detection units transmit displacements of the frame upper surface direction between the upper surface of the towed vehicle frame and the lower surface of the towed vehicle frame to the respective displacement sensors. Each displacement sensor detects the displacement.
Each rotating member installed at the tip of the first and second displacement transmission mechanisms contacts the lower surface of the frame of the towed vehicle, and rotates with the displacement. Each rotation angle sensor detects this rotation angle.
[0023]
Based on the detection results of the first, second, and fifth displacement sensors, and the detection results of the third and fourth rotation angle sensors, the arithmetic unit calculates the relative attitude angles of the towed vehicle and the towed vehicle, That is, the pitch angle, the roll angle, and the yaw angle are calculated on the towed vehicle based on the coordinate system or the coordinate system of the towed vehicle.
[0024]
BEST MODE FOR CARRYING OUT THE INVENTION
FIG. 1 shows an embodiment of a relative attitude angle detecting device 100 according to the present invention. The relative attitude angle detecting device 100 is provided with a coupler in a vehicle width direction on a frame upper surface (first reference surface) 12 of a tractor 10. The displacement detectors 40a_1 and 40a_2 (hereinafter, may be collectively referred to as a reference numeral 40a) installed in a state where the tractor 20 is interposed therebetween, and the displacement detector 40b installed behind the coupler 20 coupling the tractor 10 and the trailer 30. And an operation unit 70.
[0025]
The displacement detection units 40a_1 and 40a_2 include displacement sensors 50_1 and 50_2 and rotation angle sensors 51_1 and 51_2, respectively, and the displacement detection unit 40b includes a displacement sensor 50_3.
FIG. 2 shows a typical coupler 20, which is a coupler frame 24 fixed to the tractor 10, a coupler supporting the front of the lower frame of a trailer 30 connected by kingpins (not shown). A base 21 is provided with a rolling axle 22 and a pitching axle 23 which respectively provide the coupler base 21 with rotation in the rolling and pitching directions with respect to the tractor 10.
[0026]
FIG. 3A illustrates a configuration example (1) of the displacement detection unit, and particularly illustrates a configuration of the displacement detection unit 40a illustrated in FIG. The displacement detection unit 40a includes a caster wheel (rotating member) 41 that contacts the lower surface of the frame of the trailer, a rotation angle sensor 51 that detects a rotation angle of the caster wheel 41, and a caster holder 42a that holds the caster wheel (rotating member) 41. A rod 43a fixed to the caster holder 42a, a return spring 46 for pushing the rod 43a upward, and a displacement sensor 50 for detecting a relative displacement of the rod 43a.
[0027]
FIG. 3B shows a configuration example (1) of the displacement detection unit 40b shown in FIG. This displacement detector 40b is different from the displacement detector 40a shown in FIG. 1A in that there is no rotation angle sensor 51 and that the caster holder 42b can rotate about the rotation shaft center 80. is there.
[0028]
FIG. 4 shows a configuration example (2) of the displacement detection unit 40a, and in particular, shows a configuration example (2) of the displacement detection unit 40a shown in FIG. 1, and FIG. FIG. 2B shows a front view.
This displacement detector is different from the displacement detector 40a shown in FIG. 1A in that an arm 44 that can rotate around a fulcrum 52 is provided instead of the rod 43a, and a displacement sensor Instead of 50, a rotary displacement sensor 50b for detecting displacement based on the rotation angle of the arm 44 is provided, and instead of the return spring 46, the caster wheel 41 installed at the tip of the arm 44 is moved upward. That is, a torsion spring 47 for pushing up is provided.
[0029]
FIG. 5 illustrates a configuration example (3) of the displacement detection unit, and particularly illustrates a configuration of the displacement detection unit 40b illustrated in FIG. This displacement sensor 40b is different from the displacement detection unit 40a shown in FIG. 4 in that a dual link arm 45 is provided instead of the arm 44, there is no rotation angle sensor 51, and the caster wheel 41 is vertically That is, a mechanism for rotating about an axis is provided.
[0030]
FIG. 6 shows a modified example of the caster wheel 41. The caster wheel 41 includes a ball 60 and a ball holder 61.
Instead of the caster wheel 41 and the caster holder 42b of the displacement detection unit 40b shown in FIGS. 3 (2) and 5, it is possible to use the casters of FIG. At this time, since the ball holder 61 and the rod 43b or the mechanism 48 do not need to be rotated, they may be fixed.
[0031]
FIG. 7 shows the arrangement of the displacement detection unit 40a and the displacement detection unit 40b viewed from the side of the tractor 10. The displacement detection unit 40a_1 and the displacement detection unit 40a_2 hidden on the opposite side of the coupler 20 are arranged on or near a vertical plane including the center axis of the pitching axle 23 of the coupler 20.
[0032]
The displacement detecting unit 40b, only l _theta from the central axis of the pitching axle 23 is disposed in the rear position. The upper end of the displacement detector 40b is in contact with the bottom surface 31 of the trailer 30.
FIG. 8 shows the arrangement of the displacement detection units 40a_1 and 40a_2 as viewed from the front of the tractor 10. Displacement detector 40a_1 and 40a_2 are arranged at positions symmetrical or near apart l _phi respectively vertical plane including the central axis of the rolling axle 22. The upper ends of the displacement detectors 40a_1 and 40a_2 are in contact with the trailer bottom surfaces 31, 31 '.
[0033]
The displacement detection unit 40b is hidden on the opposite side of the coupler 20 and cannot be seen, but is disposed on or near a vertical plane including the center axis of the rolling axle 22.
[1] the relative posture angle the towing vehicle as a reference (θ, φ, ψ) detecting displacement of the displacement sensor of the operational <br/> displacement detector 40a_1,40a_2,40b of each _h _1, h 2, _{h 3} And
[0034]
In FIG. 7, the pitch angle θ can be represented by the following equation.
[0035]
(Equation 1)

[0036]
In FIG. 8, the row angle φ can be expressed by the following equation.
[0037]
(Equation 2)

[0038]
On the other hand, the yaw angle 、 is the rotation radius R of the caster wheel 41, the detection result n of the rotation angle sensor 51, the roll angle φ, the installation positions of the displacement detection units 40a_1, 40a_2, and 40b (see FIG. 1), pitching Is calculated as follows based on the error α generated by the above.
That is, since the pitching axle 23 of the coupler 20, which is the pitch axis of the towed vehicle 30, is fixed to the towed vehicle 10, the rotation of the caster wheels 41 of the displacement detection units 40a_1 and 40a_2 also fixed to the towed vehicle. The angles include errors α of equal values to each other.
[0039]
Therefore, the rotation angles as the detection results from the displacement detection units 40a_1 and 40a_2 are n _1s and n _2s , respectively, and the caster wheel rotation angles n _1r and n _2r excluding an error α of the rotation speed caused by pitching or the like. The relational expression at this time is as follows.
[0040]
[Equation 3]

[0041]
(Equation 4)

[0042]
Next, in the case where the pitch angle θ is “0” and the roll angle φ is “0”, and the yaw angle 発生 is generated as shown in FIG. 9, the caster wheels 41_1 and 41_2 have the radius l _φ Since the error is traced on the circle and errors other than the error α are negligible at this time, the following equation holds.
[0043]
(Equation 5)

[0044]
Since the distance (displacement) that the caster wheel 41 moves by rotation is equal to the length of the arc of the yaw angle 、, the following equation holds when the radius of the caster wheel is R.
[0045]
(Equation 6)

[0046]
As shown in FIG. 8, when the roll angle φ is generated, the yaw rotation axis of the towed vehicle 30 is fixed to the upper surface of the coupler, and the upper surface of the coupler, which is the yaw rotation surface, is h from the rolling axle of the coupler. _{Since the} rotation center of the yaw angle moves by _cr and the rotation surface tilts by the roll angle φ, the caster wheels 41 of the left and right displacement detection units 40a_1 and 40a_2 trace the circumferences having different radii. Will do.
[0047]
Assuming that the amount of movement of the center by the roll angle φ is α _r , the following equation holds.
[0048]
(Equation 7)

[0049]
Therefore, the turning radii in consideration of the inclination φ can be expressed by the following equations.
[0050]
(Equation 8)

[0051]
(Equation 9)

[0052]
Considering that φ≪1, the turning radii can be expressed by the following equations, respectively.
[0053]
(Equation 10)

[0054]
[Equation 11]

[0055]
Therefore, the relationship between the rotational speeds n _1r and n _2r and the yaw angle ψ can be expressed by the following equations, respectively.
[0056]
(Equation 12)

[0057]
(Equation 13)

[0058]
Therefore, in this case, the ratio n _2r / n _1r of the rotation angles of the left and right caster wheels can be expressed by the following equation obtained by dividing the above equation (13) by the equation (12).
[0059]
[Equation 14]

[0060]
When this equation is modified, the difference between the number of rotations n _1r and the number of rotations n _2r can be expressed by the following equation using n _1r .
[0061]
(Equation 15)

[0062]
Here, assuming that an error α occurs due to the pitch angle θ, the following expression is established from Expressions (3) and (4).
[0063]
(Equation 16)

[0064]
From Expressions (3), (15), and (16), the error α can be expressed by the following expression using n _1s , n _2s , and φ.
[0065]
[Equation 17]

[0066]
Therefore, the following equation is established.
[0067]
(Equation 18)

[0068]
Therefore, from Expressions (12) and (18), the yaw angle ψ can be expressed by the following expression.
[0069]
[Equation 19]

[0070]
[2] Conversion from relative attitude angle (θ, φ, ψ) to relative attitude angle (θ _t , φ _t , ψ _t ) of towed vehicle Next, considering the motion of the connected vehicle, A relative attitude angle detected by a displacement detection unit on a rectangular coordinate system (x, y, z) with the front-rear direction of the towing vehicle 10 as an x-axis, a horizontal direction (left-right direction) as a y-axis, and a vertical direction as a z-axis. (Pitch angle θ, roll angle φ, yaw angle ψ) are expressed by an orthogonal coordinate system (x ′, x ′, y ′ axis in the front-rear direction, y ′ axis in the horizontal direction, and z ′ axis in the vertical direction) y ', z' has to be converted into a relative orientation angle relative to the) (pitch angle theta _t, roll angle phi _t, yaw angle [psi _t).
[0071]
FIG. 10A shows a three-dimensional orthogonal coordinate system, in which the x-axis is parallel to the front-rear direction of the towing vehicle 10, the y-axis is parallel to the left-right direction, and the z-axis is parallel to the vertical direction. Assume that The plane xoy is a reference horizontal plane of the towed vehicle 30, the plane x'oy is a plane obtained by rotating the reference horizontal plane xoy around the y axis by θ [rad], and the plane x'oy 'is further around the x' axis. 3 shows a plane parallel to the frame bottom surface of the towed vehicle 30 rotated by φ [rad].
[0072]
The square ABCD is a square having a length of 2 l on one side centered on the origin o on the plane xoy. The intersection a between the side AD and the y-axis is the middle point of the side AD, and the length of the line segment oa is l. The square A'B'C'D 'and the square A "B" C "D" are respectively a plane x'oy and a plane x of a quadrangular prism perpendicular to a plane xoy having a square ABCD as a bottom surface or a horizontal section. The cross section is represented by “oy”.
[0073]
Consider the case where the relative yaw angle ψ occurs on planes x'oy ', calculates the relative roll angle phi _t of the towing vehicle 10 and towed vehicle 30. A straight line b ₂ c ₂ passing through the origin point o forms an angle と with the y ′ axis on the plane x′oy ′. That is, it is a straight line rotated by ψ [rad] from the y ′ axis around the z ′ axis on the plane x′oy ′. This straight line b ₂ c ₂ represents the horizontal axis of the towed vehicle 30.
[0074]
Determined angle, the straight line _b 2 _{c 2,} the angular bob ₂ = φ _t formed between the straight line bc obtained by projecting the straight line _b 2 _{c 2} on the plane xoy.
For the sake of clarity, draw an auxiliary line as follows. First, draw a straight line parallel fg as x-axis the point _{b 2.} Then draw a straight line connecting the intersection a ₂ intersections a and the side A "D" and y 'axis of the side AD and the y-axis. Further, drawing a straight line parallel to this point a ₂ as z-axis, to the this straight line and the straight line fg and edges, respectively point intersection of the AD a ₃ and the point a _4. Further, a straight line is drawn parallel to the point a as z-axis, an intersection between the straight line and the side A "D" and the straight line fg, respectively the point a ₅ and point a _6.
[0075]
FIG (2), these sides AD, A'D ', A "D ", straight fg, and intersections _{a, a2~a 6, b,} and _{b 2,} a diagram obtained by projecting the plane xoz.
First, the actually detected roll angle is the angle formed by the intersection of the plane yoz and the plane x'oy 'and the y-axis. This is the angle φ ′. In FIG. (1), because the angular OAA ₅ are orthogonal, the following equation holds.
[0076]
(Equation 20)

[0077]
In FIG. (2), a straight line aa ₅ and the straight line _a 4 _{a 2,} the following equation holds because it is parallel.
[0078]
(Equation 21)

[0079]
Since the angle a ₂ ao is a right angle, the following equation holds.
[0080]
(Equation 22)

[0081]
Here, since θ is equal to or smaller than 10 °, if θ [rad] ≪1, it can be considered that cos θ ≒ 1 and the following equation holds.
[0082]
[Equation 23]

[0083]
That is, it can be seen that φ = φ ′.
Next, determine the length of the line segment aa _2. Since the angle a ₂ ao is a right angle, the angle aoa ₂ is φ [rad], and the length of the line segment ao is 1, the following expression is established.
[0084]
(Equation 24)

[0085]
(Equation 25)

[0086]
Since the angle aa ₄ a ₂ is a right angle and the angle aa ₂ a ₄ is θ [rad], the following expression holds.
[0087]
(Equation 26)

[0088]
Since the side AD and the straight line fg are parallel, the angle _{_a} ₂ _a 3 _b 2 at right angles, because the angle _{_a} 2 _b 2 _a ₃ a θ [rad], the following expression holds.
[0089]
[Equation 27]

[0090]
Since the angle b ₂ a ₂ o is a right angle and the angle a ₂ ob ₂ is ψ [rad], the following expression is established.
[0091]
[Equation 28]

[0092]
(Equation 29)

[0093]
From the above equations (27) to (29), the following equation can be derived.
[0094]
[Equation 30]

[0095]
The length of the line segment _a 3 _{a 4} is the sum of the line segment _a 2 _{a 4} and a line segment _a 2 _{a 3,} the line bb ₂ since the side AD and the straight line fg are parallel, in the following formula be able to.
[0096]
[Equation 31]

[0097]
Here, since the angle obb ₂ is a right angle, the following expression is established.
[0098]
(Equation 32)

[0099]
Here, considering that φ _t , φ, θ≪1, the following equation holds.
[0100]
[Equation 33]

[0101]
Next, determine the relative pitch angle theta _t. The relative pitch angle theta _t, in terms of obtaining the roll angle phi _t of the coordinate axes x, 'a y, y' x in, y, 'the x, x' y, the angle phi _t, phi, theta Can be obtained in the same manner by replacing the angles with angles θ _t , θ, and −φ, respectively, and the following expression is established.
[0102]
(Equation 34)

[0103]
Next, determine the relative yaw angle [psi _t. The relative yaw angle [psi _t is the angle linear bc and y-axis linear _b 2 _{c 2} is projected onto a plane xoy forms. Therefore, from the above equation (20), the fact that the angle aa ₂ a ₅ is a right angle, and the equation (23), the following equation is established.
[0104]
(Equation 35)

[0105]
From the above equations (25) and (28), the following equation can be derived.
[0106]
[Equation 36]

[0107]
Therefore, the line segment a ₅ b ₂ can be expressed by the following equation.
[0108]
(37)

[0109]
The equation (37), and since the corner _{_a} 5 _a 6 _{b 2} is square, the following equation holds.
[0110]
[Equation 38]

[0111]
Further, since the angle bao is a right angle, the yaw angle ψt can be _expressed by the following equation.
[0112]
[Equation 39]

[0113]
In Expression (38), considering that θ, φ≪1, the following expression holds.
[0114]
(Equation 40)

[0115]
Therefore, the yaw angle ｔt can be _expressed by the following equation.
[0116]
(Equation 41)

[0117]
As described above, the relative attitude angle (φ _t , θ _t , ψ _t ) obtained by converting the relative attitude angle (φ, θ, ψ) detected on the tow vehicle side is output to the connected vehicle. Can be used as data for controlling
[0118]
【The invention's effect】
ADVANTAGE OF THE INVENTION According to the relative attitude | position angle detection apparatus of this invention, low-cost, precision and response do not require processing or installation of a sensor in a towed vehicle, and do not require special work at the time of connection or disconnection. It is possible to detect the relative attitude angle of the connected vehicle efficiently.
[Brief description of the drawings]
FIG. 1 is a plan view showing an embodiment of a relative attitude angle detection device according to the present invention.
FIG. 2 is a perspective view illustrating a configuration example of a coupler that couples a general towed vehicle and a towed vehicle.
FIG. 3 is a diagram showing a configuration example (1) of a displacement detection unit in the relative attitude angle detection device according to the present invention.
FIG. 4 is a diagram illustrating a configuration example (2) of a displacement detection unit in the relative attitude angle detection device according to the present invention.
FIG. 5 is a diagram illustrating a configuration example (3) of a displacement detection unit in the relative attitude angle detection device according to the present invention.
FIG. 6 is a diagram showing a configuration example of a detection end of a displacement detection unit in the relative attitude angle detection device according to the present invention.
FIG. 7 is a side view showing an example of the arrangement of a displacement detection unit in the relative attitude angle detection device according to the present invention.
FIG. 8 is a front view showing an example of the arrangement of a displacement detection unit in the relative attitude angle detection device according to the present invention.
FIG. 9 is a plan view showing an example of the arrangement of a displacement detection unit in the relative attitude angle detection device according to the present invention.
FIG. 10 is a diagram illustrating a principle of calculating a relative attitude angle in a calculation unit of the relative attitude angle detection device according to the present invention.
[Explanation of symbols]
REFERENCE SIGNS LIST 100 relative posture angle detecting device 10 tractor, towing vehicle 11 tractor frame 12 tractor frame upper surface, first reference plane 20 coupler 21, 21 'coupler base 22, 22' rolling axle 23 pitching axle 24 coupler frame 30, 30 'trailer, Towed vehicle 31, 31 'Trailer bottom surface, second reference surface 40a, 40a_1, 40a_2, 40b Displacement detecting part 41, 41_1, 41_2 Rotary member, caster wheels 42a, 42b Caster holder 43a, 43b Rod 44 Arm 45 Dual link arm 46 return spring 47 torsion spring 48 mechanism 50, 50b, 50c, 50_1 to 50_3 displacement sensor 51, 51_1, 51_2 rotation angle sensor 52 fulcrum 60 ball 61 ball holder 70 calculation unit 80 rotation Center theta, theta _t pitch angle phi, phi _t roll angle [psi, [psi _t in yaw angle view, the same reference numerals denote the same or corresponding parts.

Claims

A first and a second displacement detection unit installed at a position in a vehicle width direction of a towed vehicle coupled to the towed vehicle, wherein the first and second displacement detection units are arranged in a direction perpendicular to a first reference plane of the towed vehicle. Detecting a relative displacement of the second reference plane of the towing vehicle, respectively;
Third and fourth displacement detectors for detecting displacements in the direction of the second reference plane at detection points of the first and second displacement detectors, respectively;
A fifth displacement detection unit installed at a position separated from the first and second displacement detection units by a predetermined distance in the front-rear direction of the tow vehicle, wherein the fifth displacement detection unit is provided with respect to the first reference plane of the tow vehicle; Detecting relative displacement of the second reference plane in the vertical direction;
A calculating unit that calculates a relative attitude angle between the two vehicles based on the detection results of the first to fifth displacement detecting units;
A relative attitude angle detection device comprising:

In claim 1,
A relative posture angle detection device, wherein the first, second, and fifth displacement detection units include a displacement transmission mechanism for transmitting each displacement, and a displacement sensor.

In claim 1,
A rotating member that is installed at the tip of the first and second displacement detecting units, respectively, and that contacts the second reference plane and rotates with the moving displacement; And a rotation angle sensor for detecting a rotation angle of the rotation member.

In claim 1,
A relative attitude angle detecting device, wherein the relative attitude angle is at least one of a pitch angle, a roll angle, and a yaw angle of the towed vehicle with respect to the towed vehicle.

In claim 4,
A relative attitude angle detection device, wherein the calculation unit calculates the pitch angle, the roll angle, and the yaw angle based on a coordinate system on the towed vehicle or a coordinate system on a towed vehicle.