JP2004276708A - 相対姿勢角検出装置 - Google Patents
相対姿勢角検出装置 Download PDFInfo
- Publication number
- JP2004276708A JP2004276708A JP2003069689A JP2003069689A JP2004276708A JP 2004276708 A JP2004276708 A JP 2004276708A JP 2003069689 A JP2003069689 A JP 2003069689A JP 2003069689 A JP2003069689 A JP 2003069689A JP 2004276708 A JP2004276708 A JP 2004276708A
- Authority
- JP
- Japan
- Prior art keywords
- displacement
- angle
- towed vehicle
- relative
- reference plane
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Withdrawn
Links
Images
Landscapes
- Length Measuring Devices With Unspecified Measuring Means (AREA)
Abstract
【課題】牽引車両（トラクタ）と被牽引車両（トレーラ）の相対的な姿勢角を検出する相対姿勢角検出装置に関し、被牽引車両に特別な加工を施すことなく、低コストで、精度及び応答性の良い相対姿勢角を検出する。
【解決手段】変位センサ５０＿１，５０＿２が、牽引車両１０の第１の基準面１２に対して垂直方向における、被牽引車両３０の第２の基準面の相対的な変位をそれぞれ検出し、回転角センサ５１＿１，５１＿２）が、変位センサ５０＿１，５０＿２の検出点における該第２の基準面方向の移動変位をそれぞれ検出し、変位センサ５０＿３が、変位センサ５０＿１，５０＿２から該牽引車両１０の前後方向に所定の距離だけ離れた位置に設置され、牽引車両１０の第１の基準面１２に対する垂直方向における第２の基準面の相対的な変位を検出し、演算部７０が、変位センサ５０＿１〜５０＿３、及び回転角センサ５１＿１，５１＿２の検出結果に基づき両車両間の相対姿勢角を演算する。
【選択図】 図１
【解決手段】変位センサ５０＿１，５０＿２が、牽引車両１０の第１の基準面１２に対して垂直方向における、被牽引車両３０の第２の基準面の相対的な変位をそれぞれ検出し、回転角センサ５１＿１，５１＿２）が、変位センサ５０＿１，５０＿２の検出点における該第２の基準面方向の移動変位をそれぞれ検出し、変位センサ５０＿３が、変位センサ５０＿１，５０＿２から該牽引車両１０の前後方向に所定の距離だけ離れた位置に設置され、牽引車両１０の第１の基準面１２に対する垂直方向における第２の基準面の相対的な変位を検出し、演算部７０が、変位センサ５０＿１〜５０＿３、及び回転角センサ５１＿１，５１＿２の検出結果に基づき両車両間の相対姿勢角を演算する。
【選択図】 図１
Description
【０００１】
【発明の属する技術分野】
本発明は相対姿勢角検出装置に関し、特に牽引車両（トラクタ）と被牽引車両（トレーラ）の相対的な姿勢角を検出する相対姿勢角検出装置に関するものである。
【０００２】
【従来の技術】
連結車両は、牽引車両と被牽引車両間の相対姿勢角度により、車両の安定性を損なう可能性がある。このため、予め相対姿勢角度を検出し、連結車両を制御して安定性を保つことが行われている。
【０００３】
従来の連結車両の相対姿勢角検出装置としては、例えば、下記の角度検出装置（１）〜（６）がある。
（１）被牽引車両（トレーラ）のフレームの下面にキングピンを中心とする所定半径Ｒの円弧状の磁気スケールを埋設し、トラクタ（牽引車両）のカプラ側面部に上記磁気スケールの下側に所定の間隙をもって対向する磁気センサを取り付けて、トラクタに対するトレーラのヨー角を検出している（例えば、特許文献１参照）。
【０００４】
（２）例えば、トラクタに装着されたカプラ側にワイヤ部材の一端を固定し、トレーラ側にワイヤ部材の他端が長手方向に進退自在に支持され、ワイヤ部材がトレーラとトラクタとの間の相対的な連結角（相対姿勢角、ヨー角）の変化に伴い該他端側が長手方向に進退変位することでワイヤ案内部材に案内されて延出する。
【０００５】
このワイヤ案内部材がキングピンと同心円状に形成されているため、該進退変位はトレーラとトラクタの連結角に対応していることを利用して、連結角を測定している（例えば、特許文献２参照）。
（３）また、回転角検出器の回転軸をカプラの回転軸と一致するように、例えば、トラクタ側に設置する。そして、回転角検出器の両側にはそれぞれアームの一端を取り付け、アームの他端は、トレーラ側の設置した取付台に摺動自在にしておく。これにより、走行中のローリングやピッチングは、回転検出器の回転方向には影響は与えない。
【０００６】
トラクタがトレーラに対して旋回すると、アームが回転角検出器を同じ角度だけ回動させるので、回転角検出器は、トラクタとトレーラとの相対的なヨー角を検出する（例えば、特許文献３参照）。
（４）トラクタ及びトレーラにそれぞれ電子式の方向センサを搭載し、各方向センサで測定されたトラクタ及びトレーラの方位角に基づき、トラクタとトレーラとの間の相対的な屈曲角（相対姿勢角）を決定する（例えば、特許文献４参照）。
【０００７】
（５）例えば、トラクタに取り付けたスキャンレーザレーダによりトレーラの前面パネル上の複数点の距離と角度を検出し、これらの検出値からトラクタの後面パネルとトレーラの前面パネルの傾き角を算出することにより、トラクタとトレーラの相対的な連結角（相対姿勢角）を算出する（例えば、特許文献５参照）。
【０００８】
（６）オブザーバ等が、ヨーレート、横向き加速度、及びステアリング角度等の牽引車両の動き、並びに予め記憶された車両の諸元に基づき、相対姿勢角に対応する値を推定する（例えば、特許文献６参照）。
【０００９】
【特許文献１】
特開８−３３２９７３号公報（第２頁、図２及び図３）
【００１０】
【特許文献２】
特開平１１−１６００６３号公報（第２頁、図３）
【００１１】
【特許文献３】
特開平１１−２７８３２０号公報（第２頁、図６）
【００１２】
【特許文献４】
特開２００１−２２７９０５号公報（第２頁、図１）
【００１３】
【特許文献５】
特開２００１−３３４９６６号公報（第２頁、図１）
【００１４】
【特許文献６】
特開平１０−１０３７号公報（第５頁、図１）
【００１５】
【発明が解決しようとする課題】
このような従来の相対姿勢角検出装置（１）〜（４）は、頻繁に連結及び切離を繰り返したり、各種の被牽引車両を連結する連結車両においては、被牽引車両が磁気スケール、変位センサ、回転角センサ、又は方向センサ等を備えていなければならず、これらを備えていない被牽引車両に対応することができない。また、被牽引車両に磁気スケール、変位センサ、又は方向センサ等を搭載するための加工が必要になる。
【００１６】
また、相対姿勢角検出装置（５）は、レーザレーダは、非常にコストが高く実用的でないという問題がある。
また、角度推定装置（６）は、精度が悪く、牽引車両の動きから推定するため応答性に問題がある。
【００１７】
従って本発明は、牽引車両と被牽引車両との間の相対的な姿勢角を検出する相対姿勢角検出装置において、被牽引車両に特別な加工を施すことなく、低コストで、精度及び応答性良く相対姿勢角を検出することを課題とする。
【００１８】
【課題を解決するための手段】
上記の課題を解決するため、本発明の相対姿勢角検出装置は、被牽引車両と結合される牽引車両の車幅方向の位置に設置された第１及び第２の変位検出部であって、該牽引車両の第１の基準面に対して垂直方向における、該被牽引車両の第２の基準面の相対的な変位をそれぞれ検出するものと、該第１及び第２の変位検出部の検出点における該第２の基準面方向の移動変位を、それぞれ検出する第３及び第４の変位検出部と、該第１及び第２の変位検出部から該牽引車両の前後方向に所定の距離だけ離れた位置に設置された第５の変位検出部であって、該牽引車両の該第１の基準面に対する垂直方向における第２の基準面の相対的な変位を検出するものと、該第１から該第５までの変位検出部の検出結果に基づき両車両間の相対姿勢角を演算する演算部とを備えたことを特徴としている。
【００１９】
また、上記の本発明において、該第１、第２、及び第５の変位検出部が、各変位を伝えるための変位伝達機構と、変位センサとを備えることができる。
また、上記の本発明において、該第３及び第４の変位検出部が、それぞれ、該第１及び第２の変位検出部の先端に設置され、該第２の基準面に接触して該移動変位に伴って回転する回転部材と、該回転部材の回転角を検出する回転角センサとを備えることができる。
【００２０】
また、上記の本発明において、該相対姿勢角として、該牽引車両に対する該被牽引車両のピッチ角、ロール角、及びヨー角の内の少なくとも１つとすることができる。
また、上記の本発明において、該演算部は、該牽引車両上の座標系又は被牽引車両上の座標系に基づき該ピッチ角、該ロール角、及び該ヨー角を演算することができる。
【００２１】
すなわち、本発明の相対姿勢角検出装置は、カプラを介して結合される牽引車両の殆どのフレーム上面（第１の基準面）及び被牽引車両のフレーム下面（第２の基準面）が平面であることに着目し、例えば、牽引車両のフレーム上面に、カプラを挟む形で、該牽引車両の車幅方向に互いに相対する位置に第１及び第２の変位検出部を設置し、第１及び第２の変位検出部から所定の距離だけ前方又は後方に離れた位置に、第３の変位検出部を設置する。
【００２２】
第１、第２及び第５の変位検出部の変位伝達機構は、それぞれ、牽引車両のフレーム上面と被牽引車両のフレーム下面との間の該フレーム上面方向の変位を各変位センサに伝え、この変位を各変位センサが検出する。
第１及び第２の変位伝達機構の先端に設置された各回転部材は、該被牽引車両のフレーム下面に接触して、その移動変位に伴い回転する。この回転角を各回転角センサが検出する。
【００２３】
これらの第１、第２、及び第５までの変位センサの検出結果、並びに第３及び第４の回転角センサの検出結果に基づき、演算部が、牽引車両及び被牽引車両の相対姿勢角、すなわち、ピッチ角、ロール角、及びヨー角を、牽引車両上に座標系又は被牽引車両の座標系に基づき演算する。
【００２４】
【発明の実施の形態】
図１は、本発明に係る相対姿勢角検出装置１００の実施例を示しており、この相対姿勢角検出装置１００は、トラクタ１０のフレーム上面（第１の基準面）１２の車幅方向にカプラ２０を挟んだ状態で設置された変位検出部４０ａ＿１及び４０ａ＿２（以下、符号４０ａで総称することがある。）と、トラクタ１０とトレーラ３０を結合するカプラ２０の後方に設置された変位検出部４０ｂと、演算部７０とで構成されている。
【００２５】
変位検出部４０ａ＿１及び４０ａ＿２は、それぞれ、変位センサ５０＿１，５０＿２及び回転角センサ５１＿１，５１＿２を備え、変位検出部４０ｂは、変位センサ５０＿３を備えている。
図２は、一般的なカプラ２０を示しており、このカプラ２０は、トラクタ１０に固定されるカプラフレーム２４、キングピン（図示せず）で接続されたトレーラ３０の下部フレームの前部を支えるカプラベース２１、このカプラベース２１にトラクタ１０に対してローリング及びピッチング方向の回転をそれぞれが与えるローリングアクスル２２及びピッチングアクスル２３を備えている。
【００２６】
図３（１）は、変位検出部の構成例（１）を示しており、特に、図１に示した変位検出部４０ａの構成を示している。この変位検出部４０ａは、トレーラのフレーム下面に接触するキャスタホイール（回転部材）４１、このキャスタホイール４１の回転角を検出する回転角センサ５１、キャスタホイール（回転部材）４１を保持するキャスタホルダ４２ａ、このキャスタホルダ４２ａに固着されたロッド４３ａ、このロッド４３ａを上方に押し上げるリターンスプリング４６、及びロッド４３ａの相対変位を検出する変位センサ５０を備えている。
【００２７】
図３（２）は、図１に示した変位検出部４０ｂの構成例（１）を示している。この変位検出部４０ｂが、同図（１）に示した変位検出部４０ａと異なる点は、回転角センサ５１が無いことと、キャスタホルダ４２ｂが回転軸中心８０に回転することが可能なことである。
【００２８】
図４は、変位検出部４０ａの構成例（２）を示しており、特に、図１に示した変位検出部４０ａの構成例（２）を示しており、同図（１）は側面図、同図（２）は正面図を示している。
この変位検出部が、同図（１）に示した変位検出部４０ａと異なる点は、ロッド４３ａの代わりに支点５２を中心に回転することが可能なアーム４４を備えていることと、変位センサ５０の代わりに、アーム４４の回転角に基づき変位を検出する回転型の変位センサ５０ｂを備えていることと、リターンスプリング４６の代わりに、アーム４４の先端に設置されたキャスタホイール４１を上方に押し上げるねじりスプリング４７を備えていることである。
【００２９】
図５は、変位検出部の構成例（３）を示しており、特に、図１に示した変位検出部４０ｂの構成を示している。この変位センサ４０ｂが、図４に示した変位検出部４０ａと異なる点は、アーム４４の代わりに、デュアルリンクアーム４５を備えていること、回転角センサ５１が無いこと、及びキャスタホイール４１が垂直軸を中心として回転する機構を備えていることである。
【００３０】
図６は、キャスタホイール４１の変形例を示しており、このキャスタホイール４１は、ボール６０とボールホルダ６１で構成されている。
図３（２）及び図５に示した変位検出部４０ｂのキャスタホイール４１及びキャスタホルダ４２ｂの代わりに、図６のキャスタを用いることが可能である。このとき、ボールホルダ６１とロッド４３ｂ又は機構４８は回転させる必要がないので、固着してもよい。
【００３１】
図７は、トラクタ１０の側面方向から見た変位検出部４０ａ及び変位検出部４０ｂの配置を示している。変位検出部４０ａ＿１及びカプラ２０の反対側に隠れている変位検出部４０ａ＿２は、カプラ２０のピッチングアクスル２３の中心軸を含む垂直平面上又はその近傍に配置されている。
【００３２】
また、変位検出部４０ｂは、ピッチングアクスル２３の中心軸からｌθだけ後方の位置に配置されている。変位検出部４０ｂの上端はトレーラ３０の底面３１に接している。
図８は、トラクタ１０の正面方向から見た変位検出部４０ａ＿１及び４０ａ＿２の配置を示している。変位検出部４０ａ＿１及び４０ａ＿２は、ローリングアクスル２２の中心軸を含む垂直平面に対してそれぞれｌφだけ離れた対称な位置又はその近傍に配置されている。変位検出部４０ａ＿１及び４０ａ＿２の上端はトレーラ底面３１，３１’に接している。
【００３３】
変位検出部４０ｂは、カプラ２０の反対側に隠れて見えないが、ローリングアクスル２２の中心軸を含む垂直平面上又はその近傍に配置されている。
〔１〕牽引車両を基準とした相対姿勢角（θ，φ，ψ）の演算
変位検出部４０ａ＿１，４０ａ＿２，４０ｂの変位センサの検出変位を、それぞれｈ１，ｈ２，ｈ３とする。
【００３４】
図７において、ピッチ角θは、次式で示すことができる。
【００３５】
【数１】
【００３６】
図８において、ロー角φは、次式で示すことができる。
【００３７】
【数２】
【００３８】
一方、ヨー角ψは、キャスタホイール４１の回転半径Ｒ、回転角センサ５１の検出結果ｎと、ロール角φ、及び各変位検出部４０ａ＿１，４０ａ＿２，及び４０ｂの設置位置（図１参照）、ピッチング等により発生する誤差αに基づき次のように計算される。
すなわち、被牽引車両３０のピッチ軸となるカプラ２０のピッチングアクスル２３が、牽引車両１０に固定されているため、同様に牽引車両に固定されている変位検出部４０ａ＿１及び４０ａ＿２のキャスタホイール４１の回転角は、互いに等しい値の誤差αを含む。
【００３９】
したがって、変位検出部４０ａ＿１及び４０ａ＿２からの検出結果である回転角を、それぞれｎ１ｓ，ｎ２ｓとし、ピッチング等により発生する回転数の誤差αを除いたキャスタホイール回転角ｎ１ｒ，ｎ２ｒとしたときの関係式は次のようになる。
【００４０】
【数３】
【００４１】
【数４】
【００４２】
次に、ピッチ角θ＝“０”、及びロール角φ＝“０”の場合であって、図９に示すようにヨー角ψが発生した場合、キャスタホイール４１＿１，４１＿２は、半径ｌφの円上をトレースし、このとき誤差α以外の誤差は無視できる程微少であるから、次式が成り立つ。
【００４３】
【数５】
【００４４】
キャスタホイール４１の回転によって移動する距離（変位）は、ヨー角ψ分の円弧の長さに等しいので、キャスタホイールの半径をＲとすると次式が成り立つ。
【００４５】
【数６】
【００４６】
図８に示したように、ロール角φが発生すると、被牽引車両３０のヨー角回転軸がカプラ上面に固定されており、しかも、ヨー角回転面であるカプラ上面はカプラのローリングアクスルからｈｃｒだけ上方にあるため、ヨー角回転中心が移動し、かつ、回転面がロール角φだけ傾くことから、左右の変位検出部４０ａ＿１，４０ａ＿２のキャスタホイール４１は各々半径の異なる円周上をトレースすることになる。
【００４７】
ロール角φによる中心の移動量をαｒとすれば、次式が成り立つ。
【００４８】
【数７】
【００４９】
したがって、傾きφを考慮した回転半径は、それぞれ、次式で示すことができる。
【００５０】
【数８】
【００５１】
【数９】
【００５２】
φ≪１であることを考慮すると、回転半径は、それぞれ、次式で示すことができる。
【００５３】
【数１０】
【００５４】
【数１１】
【００５５】
したがって、回転数ｎ１ｒ，ｎ２ｒとヨー角ψとの関係は、それぞれ、次式で示すことができる。
【００５６】
【数１２】
【００５７】
【数１３】
【００５８】
したがって、この場合、左右のキャスタホイールの回転角の比ｎ２ｒ／ｎ１ｒは、上記の式（１３）を式（１２）で除した次式で示すことができる。
【００５９】
【数１４】
【００６０】
この式を変形すると回転数ｎ１ｒと回転数ｎ２ｒの差は、ｎ１ｒを用いて次式で示すことができる。
【００６１】
【数１５】
【００６２】
ここで、ピッチ角θにより誤差αが発生したとすると、式（３）及び式（４）から次式が成り立つ。
【００６３】
【数１６】
【００６４】
式（３）、式（１５）、及び式（１６）から、誤差αは、ｎ１ｓ，ｎ２ｓ，φを用いて次式で示すことができる。
【００６５】
【数１７】
【００６６】
したがって、次式が成立する。
【００６７】
【数１８】
【００６８】
したがって、式（１２）、式（１８）より、ヨー角ψは次式で示すことができる。
【００６９】
【数１９】
【００７０】
〔２〕相対姿勢角（θ，φ，ψ）から被牽引車両の相対姿勢角（θ ｔ ，φ ｔ ，ψ ｔ ）への変換
次に、連結車両の運動を考えた場合、牽引車両１０の前後方向をｘ軸、水平方向（左右方向）をｙ軸、及び垂直方向をｚ軸とした直交座標系（ｘ，ｙ，ｚ）上の変位検出部で検出された相対姿勢角（ピッチ角θ、ロール角φ、ヨー角ψ）を、被牽引車両３０の前後方向をｘ’軸、水平方向をｙ’軸、垂直方向をｚ’軸としたを直交座標系（ｘ’，ｙ’，ｚ’）を基準とした相対姿勢角（ピッチ角θｔ、ロール角φｔ、ヨー角ψｔ）に換算する必要がある。
【００７１】
図１０（１）は、３次元直交座標系を示しており、ｘ軸は牽引車両１０の前後方向に平行であり、ｙ軸は左右方向に平行であり、ｚ軸は垂直方向に平行であると仮定する。また、平面ｘｏｙは、被牽引車両３０の基準水平面、平面ｘ’ｏｙは、基準水平面ｘｏｙをｙ軸回りにθ〔ｒａｄ〕だけ回転させた平面、平面ｘ’ｏｙ’は、さらにｘ’軸回りにφ〔ｒａｄ〕だけ回転させた被牽引車両３０のフレーム底面と平行な平面を表している。
【００７２】
四角形ＡＢＣＤは、平面ｘｏｙ上にあり、原点ｏを中心とした一辺の長さが２ｌの正方形である。また、辺ＡＤとｙ軸の交点ａは、辺ＡＤの中点であり、線分ｏａの長さはｌである。四角形Ａ’Ｂ’Ｃ’Ｄ’、及び四角形Ａ”Ｂ”Ｃ”Ｄ”は、それぞれ、正方形ＡＢＣＤを底面、又は水平断面とした平面ｘｏｙに垂直な四角柱の平面ｘ’ｏｙ、及び平面ｘ’ｏｙ’による断面を表している。
【００７３】
平面ｘ’ｏｙ’上で相対ヨー角ψが発生した場合を考え、被牽引車両３０と牽引車両１０の相対ロール角φｔを算出する。原点ｏを通る直線ｂ２ｃ２は平面ｘ’ｏｙ’上でｙ’軸と角度ψを成す。すなわち、平面ｘ’ｏｙ’上でｙ’軸からｚ’軸回りにψ〔ｒａｄ〕だけ回転した直線である。この直線ｂ２ｃ２は、被牽引車両３０の水平軸を表している。
【００７４】
求める角度は、直線ｂ２ｃ２と、直線ｂ２ｃ２を平面ｘｏｙ上に投影した直線ｂｃとの成す角ｂｏｂ２＝φｔである。
分かり易くするため、次のように補助線を引く。まず、点ｂ２を通りｘ軸に平行な直線ｆｇを引く。次に辺ＡＤとｙ軸の交点ａと辺Ａ”Ｄ”とｙ’軸の交点ａ２を結ぶ直線を引く。さらに、この点ａ２を通りｚ軸に平行な直線を引き、この直線と、直線ｆｇ及び辺ＡＤとの交点をそれぞれ点ａ３及び点ａ４とする。さらに、点ａを通りｚ軸に平行な直線を引き、この直線と辺Ａ”Ｄ”及び直線ｆｇとの交点を、それぞれ、点ａ５及び点ａ６とする。
【００７５】
同図（２）は、これらの辺ＡＤ，Ａ’Ｄ’，Ａ”Ｄ”、直線ｆｇ、及び交点ａ，ａ２〜ａ６，ｂ，ｂ２を、平面ｘｏｚに投影した図である。
まず、実際に検出されるロール角は、平面ｙｏｚと平面ｘ’ｏｙ’の交線とｙ軸とが成す角である。これを角φ’とする。同図（１）において、角ｏａａ５は直角であるから、次式が成り立つ。
【００７６】
【数２０】
【００７７】
同図（２）において、直線ａａ５と直線ａ４ａ２は、平行であるから次式が成り立つ。
【００７８】
【数２１】
【００７９】
角ａ２ａｏは直角であるから、次式が成り立つ。
【００８０】
【数２２】
【００８１】
ここで、θは１０°以下であるから、θ〔ｒａｄ〕≪１であるとすると、ｃｏｓθ≒１であると看做せるので、次式が成り立つ。
【００８２】
【数２３】
【００８３】
すなわち、φ＝φ’と看做せることが分かる。
次に、線分ａａ２の長さを求める。角ａ２ａｏは直角であり、角ａｏａ２はφ〔ｒａｄ〕、線分ａｏの長さがｌであるから、次式が成り立つ。
【００８４】
【数２４】
【００８５】
【数２５】
【００８６】
角ａａ４ａ２が直角であり、角ａａ２ａ４はθ〔ｒａｄ〕であるから、次式が成り立つ。
【００８７】
【数２６】
【００８８】
辺ＡＤと直線ｆｇが平行であることから、角ａ２ａ３ｂ２は直角であり、角ａ２ｂ２ａ３はθ〔ｒａｄ〕であるから、次式が成り立つ。
【００８９】
【数２７】
【００９０】
角ｂ２ａ２ｏは直角、角ａ２ｏｂ２はψ〔ｒａｄ〕であるから、次式が成り立つ。
【００９１】
【数２８】
【００９２】
【数２９】
【００９３】
上記の式（２７）〜（２９）から、次式が導き出せる。
【００９４】
【数３０】
【００９５】
線分ａ３ａ４の長さは、線分ａ２ａ４と線分ａ２ａ３の和であり、辺ＡＤと直線ｆｇが平行であることから線分ｂｂ２は、次式で示すことができる。
【００９６】
【数３１】
【００９７】
ここで、角ｏｂｂ２は直角であるから、次式が成り立つ。
【００９８】
【数３２】
【００９９】
ここで、φｔ，φ，θ≪１であることを考慮すると、次式が成り立つ。
【０１００】
【数３３】
【０１０１】
次に、相対ピッチ角θｔを求める。この相対ピッチ角θｔは、上記のロール角φｔを求めた条件において、座標軸ｘ，ｘ’をｙ，ｙ’に、ｙ，ｙ’をｘ，ｘ’に、角φｔ，φ，θをそれぞれ角θｔ，θ，−φに置き換えれば同様に求められ、次式が成り立つ。
【０１０２】
【数３４】
【０１０３】
次に、相対ヨー角ψｔを求める。この相対ヨー角ψｔは、直線ｂ２ｃ２を平面ｘｏｙ上に投影した直線ｂｃとｙ軸が成す角である。したがって、上記の式（２０）及び角ａａ２ａ５が直角であること、及び式（２３）から、次式が成り立つ。
【０１０４】
【数３５】
【０１０５】
また、上記の式（２５）及び（２８）から、次式が導き出せる。
【０１０６】
【数３６】
【０１０７】
したがって、線分ａ５ｂ２は次式で示すことができる。
【０１０８】
【数３７】
【０１０９】
この式（３７）、及び角ａ５ａ６ｂ２が直角であることから、次式が成り立つ。
【０１１０】
【数３８】
【０１１１】
また、角ｂａｏが直角であることから、ヨー角ψｔは、次式で示すことができる。
【０１１２】
【数３９】
【０１１３】
この式（３８）において、θ，φ≪１であることを考慮すると、次式が成り立つ。
【０１１４】
【数４０】
【０１１５】
したがって、ヨー角ψｔは、次式で示すことができる。
【０１１６】
【数４１】
【０１１７】
以上により、牽引車両側で検出された相対姿勢角（φ，θ，ψ）を変換することによって得られた被牽引車両側に相対姿勢角（φｔ，θｔ，ψｔ）を、連結車両を制御するためのデータとして利用することが可能になる。
【０１１８】
【発明の効果】
本発明の相対姿勢角検出装置によれば、被牽引車両に加工又はセンサの設置をすることなく、また、連結時又は切離時に特別な作業を必要とせずに、低コストで、精度及び応答性良く連結車両の相対姿勢角を検出することが可能になる。
【図面の簡単な説明】
【図１】本発明に係る相対姿勢角検出装置の実施例を示した平面図である。
【図２】一般的な牽引車両と被牽引車両を結合するカプラの構成例を示した斜視図である。
【図３】本発明に係る相対姿勢角検出装置における変位検出部の構成例（１）を示した図である。
【図４】本発明に係る相対姿勢角検出装置における変位検出部の構成例（２）を示した図である。
【図５】本発明に係る相対姿勢角検出装置における変位検出部の構成例（３）を示した図である。
【図６】本発明に係る相対姿勢角検出装置における変位検出部の検出端の構成例を示した図である。
【図７】本発明に係る相対姿勢角検出装置における変位検出部の配置例を示した側面図図である。
【図８】本発明に係る相対姿勢角検出装置における変位検出部の配置例を示した正面図である。
【図９】本発明に係る相対姿勢角検出装置における変位検出部の配置例を示した平面図である。
【図１０】本発明に係る相対姿勢角検出装置の演算部における相対姿勢角算出の原理を示した図である。
【符号の説明】
１００ 相対姿勢角検出装置
１０ トラクタ、牽引車両 １１ トラクタフレーム
１２ トラクタフレーム上面、第１の基準面
２０ カプラ ２１，２１’ カプラベース
２２，２２’ ローリングアクスル ２３ ピッチングアクスル
２４ カプラフレーム ３０，３０’ トレーラ、被牽引車両
３１，３１’ トレーラ底面、第２の基準面
４０ａ，４０ａ＿１，４０ａ＿２，４０ｂ 変位検出部
４１，４１＿１，４１＿２ 回転部材、キャスタホイール
４２ａ，４２ｂ キャスタホルダ ４３ａ，４３ｂ ロッド
４４ アーム ４５ デュアルリンクアーム
４６ リターンスプリング ４７ ねじりスプリング
４８ 機構
５０，５０ｂ，５０ｃ，５０＿１〜５０＿３ 変位センサ
５１，５１＿１，５１＿２ 回転角センサ ５２ 支点
６０ ボール ６１ ボールホルダ
７０ 演算部 ８０ 回転軸中心
θ，θｔ ピッチ角 φ，φｔ ロール角
ψ，ψｔ ヨー角
図中、同一符号は同一又は相当部分を示す。
【発明の属する技術分野】
本発明は相対姿勢角検出装置に関し、特に牽引車両（トラクタ）と被牽引車両（トレーラ）の相対的な姿勢角を検出する相対姿勢角検出装置に関するものである。
【０００２】
【従来の技術】
連結車両は、牽引車両と被牽引車両間の相対姿勢角度により、車両の安定性を損なう可能性がある。このため、予め相対姿勢角度を検出し、連結車両を制御して安定性を保つことが行われている。
【０００３】
従来の連結車両の相対姿勢角検出装置としては、例えば、下記の角度検出装置（１）〜（６）がある。
（１）被牽引車両（トレーラ）のフレームの下面にキングピンを中心とする所定半径Ｒの円弧状の磁気スケールを埋設し、トラクタ（牽引車両）のカプラ側面部に上記磁気スケールの下側に所定の間隙をもって対向する磁気センサを取り付けて、トラクタに対するトレーラのヨー角を検出している（例えば、特許文献１参照）。
【０００４】
（２）例えば、トラクタに装着されたカプラ側にワイヤ部材の一端を固定し、トレーラ側にワイヤ部材の他端が長手方向に進退自在に支持され、ワイヤ部材がトレーラとトラクタとの間の相対的な連結角（相対姿勢角、ヨー角）の変化に伴い該他端側が長手方向に進退変位することでワイヤ案内部材に案内されて延出する。
【０００５】
このワイヤ案内部材がキングピンと同心円状に形成されているため、該進退変位はトレーラとトラクタの連結角に対応していることを利用して、連結角を測定している（例えば、特許文献２参照）。
（３）また、回転角検出器の回転軸をカプラの回転軸と一致するように、例えば、トラクタ側に設置する。そして、回転角検出器の両側にはそれぞれアームの一端を取り付け、アームの他端は、トレーラ側の設置した取付台に摺動自在にしておく。これにより、走行中のローリングやピッチングは、回転検出器の回転方向には影響は与えない。
【０００６】
トラクタがトレーラに対して旋回すると、アームが回転角検出器を同じ角度だけ回動させるので、回転角検出器は、トラクタとトレーラとの相対的なヨー角を検出する（例えば、特許文献３参照）。
（４）トラクタ及びトレーラにそれぞれ電子式の方向センサを搭載し、各方向センサで測定されたトラクタ及びトレーラの方位角に基づき、トラクタとトレーラとの間の相対的な屈曲角（相対姿勢角）を決定する（例えば、特許文献４参照）。
【０００７】
（５）例えば、トラクタに取り付けたスキャンレーザレーダによりトレーラの前面パネル上の複数点の距離と角度を検出し、これらの検出値からトラクタの後面パネルとトレーラの前面パネルの傾き角を算出することにより、トラクタとトレーラの相対的な連結角（相対姿勢角）を算出する（例えば、特許文献５参照）。
【０００８】
（６）オブザーバ等が、ヨーレート、横向き加速度、及びステアリング角度等の牽引車両の動き、並びに予め記憶された車両の諸元に基づき、相対姿勢角に対応する値を推定する（例えば、特許文献６参照）。
【０００９】
【特許文献１】
特開８−３３２９７３号公報（第２頁、図２及び図３）
【００１０】
【特許文献２】
特開平１１−１６００６３号公報（第２頁、図３）
【００１１】
【特許文献３】
特開平１１−２７８３２０号公報（第２頁、図６）
【００１２】
【特許文献４】
特開２００１−２２７９０５号公報（第２頁、図１）
【００１３】
【特許文献５】
特開２００１−３３４９６６号公報（第２頁、図１）
【００１４】
【特許文献６】
特開平１０−１０３７号公報（第５頁、図１）
【００１５】
【発明が解決しようとする課題】
このような従来の相対姿勢角検出装置（１）〜（４）は、頻繁に連結及び切離を繰り返したり、各種の被牽引車両を連結する連結車両においては、被牽引車両が磁気スケール、変位センサ、回転角センサ、又は方向センサ等を備えていなければならず、これらを備えていない被牽引車両に対応することができない。また、被牽引車両に磁気スケール、変位センサ、又は方向センサ等を搭載するための加工が必要になる。
【００１６】
また、相対姿勢角検出装置（５）は、レーザレーダは、非常にコストが高く実用的でないという問題がある。
また、角度推定装置（６）は、精度が悪く、牽引車両の動きから推定するため応答性に問題がある。
【００１７】
従って本発明は、牽引車両と被牽引車両との間の相対的な姿勢角を検出する相対姿勢角検出装置において、被牽引車両に特別な加工を施すことなく、低コストで、精度及び応答性良く相対姿勢角を検出することを課題とする。
【００１８】
【課題を解決するための手段】
上記の課題を解決するため、本発明の相対姿勢角検出装置は、被牽引車両と結合される牽引車両の車幅方向の位置に設置された第１及び第２の変位検出部であって、該牽引車両の第１の基準面に対して垂直方向における、該被牽引車両の第２の基準面の相対的な変位をそれぞれ検出するものと、該第１及び第２の変位検出部の検出点における該第２の基準面方向の移動変位を、それぞれ検出する第３及び第４の変位検出部と、該第１及び第２の変位検出部から該牽引車両の前後方向に所定の距離だけ離れた位置に設置された第５の変位検出部であって、該牽引車両の該第１の基準面に対する垂直方向における第２の基準面の相対的な変位を検出するものと、該第１から該第５までの変位検出部の検出結果に基づき両車両間の相対姿勢角を演算する演算部とを備えたことを特徴としている。
【００１９】
また、上記の本発明において、該第１、第２、及び第５の変位検出部が、各変位を伝えるための変位伝達機構と、変位センサとを備えることができる。
また、上記の本発明において、該第３及び第４の変位検出部が、それぞれ、該第１及び第２の変位検出部の先端に設置され、該第２の基準面に接触して該移動変位に伴って回転する回転部材と、該回転部材の回転角を検出する回転角センサとを備えることができる。
【００２０】
また、上記の本発明において、該相対姿勢角として、該牽引車両に対する該被牽引車両のピッチ角、ロール角、及びヨー角の内の少なくとも１つとすることができる。
また、上記の本発明において、該演算部は、該牽引車両上の座標系又は被牽引車両上の座標系に基づき該ピッチ角、該ロール角、及び該ヨー角を演算することができる。
【００２１】
すなわち、本発明の相対姿勢角検出装置は、カプラを介して結合される牽引車両の殆どのフレーム上面（第１の基準面）及び被牽引車両のフレーム下面（第２の基準面）が平面であることに着目し、例えば、牽引車両のフレーム上面に、カプラを挟む形で、該牽引車両の車幅方向に互いに相対する位置に第１及び第２の変位検出部を設置し、第１及び第２の変位検出部から所定の距離だけ前方又は後方に離れた位置に、第３の変位検出部を設置する。
【００２２】
第１、第２及び第５の変位検出部の変位伝達機構は、それぞれ、牽引車両のフレーム上面と被牽引車両のフレーム下面との間の該フレーム上面方向の変位を各変位センサに伝え、この変位を各変位センサが検出する。
第１及び第２の変位伝達機構の先端に設置された各回転部材は、該被牽引車両のフレーム下面に接触して、その移動変位に伴い回転する。この回転角を各回転角センサが検出する。
【００２３】
これらの第１、第２、及び第５までの変位センサの検出結果、並びに第３及び第４の回転角センサの検出結果に基づき、演算部が、牽引車両及び被牽引車両の相対姿勢角、すなわち、ピッチ角、ロール角、及びヨー角を、牽引車両上に座標系又は被牽引車両の座標系に基づき演算する。
【００２４】
【発明の実施の形態】
図１は、本発明に係る相対姿勢角検出装置１００の実施例を示しており、この相対姿勢角検出装置１００は、トラクタ１０のフレーム上面（第１の基準面）１２の車幅方向にカプラ２０を挟んだ状態で設置された変位検出部４０ａ＿１及び４０ａ＿２（以下、符号４０ａで総称することがある。）と、トラクタ１０とトレーラ３０を結合するカプラ２０の後方に設置された変位検出部４０ｂと、演算部７０とで構成されている。
【００２５】
変位検出部４０ａ＿１及び４０ａ＿２は、それぞれ、変位センサ５０＿１，５０＿２及び回転角センサ５１＿１，５１＿２を備え、変位検出部４０ｂは、変位センサ５０＿３を備えている。
図２は、一般的なカプラ２０を示しており、このカプラ２０は、トラクタ１０に固定されるカプラフレーム２４、キングピン（図示せず）で接続されたトレーラ３０の下部フレームの前部を支えるカプラベース２１、このカプラベース２１にトラクタ１０に対してローリング及びピッチング方向の回転をそれぞれが与えるローリングアクスル２２及びピッチングアクスル２３を備えている。
【００２６】
図３（１）は、変位検出部の構成例（１）を示しており、特に、図１に示した変位検出部４０ａの構成を示している。この変位検出部４０ａは、トレーラのフレーム下面に接触するキャスタホイール（回転部材）４１、このキャスタホイール４１の回転角を検出する回転角センサ５１、キャスタホイール（回転部材）４１を保持するキャスタホルダ４２ａ、このキャスタホルダ４２ａに固着されたロッド４３ａ、このロッド４３ａを上方に押し上げるリターンスプリング４６、及びロッド４３ａの相対変位を検出する変位センサ５０を備えている。
【００２７】
図３（２）は、図１に示した変位検出部４０ｂの構成例（１）を示している。この変位検出部４０ｂが、同図（１）に示した変位検出部４０ａと異なる点は、回転角センサ５１が無いことと、キャスタホルダ４２ｂが回転軸中心８０に回転することが可能なことである。
【００２８】
図４は、変位検出部４０ａの構成例（２）を示しており、特に、図１に示した変位検出部４０ａの構成例（２）を示しており、同図（１）は側面図、同図（２）は正面図を示している。
この変位検出部が、同図（１）に示した変位検出部４０ａと異なる点は、ロッド４３ａの代わりに支点５２を中心に回転することが可能なアーム４４を備えていることと、変位センサ５０の代わりに、アーム４４の回転角に基づき変位を検出する回転型の変位センサ５０ｂを備えていることと、リターンスプリング４６の代わりに、アーム４４の先端に設置されたキャスタホイール４１を上方に押し上げるねじりスプリング４７を備えていることである。
【００２９】
図５は、変位検出部の構成例（３）を示しており、特に、図１に示した変位検出部４０ｂの構成を示している。この変位センサ４０ｂが、図４に示した変位検出部４０ａと異なる点は、アーム４４の代わりに、デュアルリンクアーム４５を備えていること、回転角センサ５１が無いこと、及びキャスタホイール４１が垂直軸を中心として回転する機構を備えていることである。
【００３０】
図６は、キャスタホイール４１の変形例を示しており、このキャスタホイール４１は、ボール６０とボールホルダ６１で構成されている。
図３（２）及び図５に示した変位検出部４０ｂのキャスタホイール４１及びキャスタホルダ４２ｂの代わりに、図６のキャスタを用いることが可能である。このとき、ボールホルダ６１とロッド４３ｂ又は機構４８は回転させる必要がないので、固着してもよい。
【００３１】
図７は、トラクタ１０の側面方向から見た変位検出部４０ａ及び変位検出部４０ｂの配置を示している。変位検出部４０ａ＿１及びカプラ２０の反対側に隠れている変位検出部４０ａ＿２は、カプラ２０のピッチングアクスル２３の中心軸を含む垂直平面上又はその近傍に配置されている。
【００３２】
また、変位検出部４０ｂは、ピッチングアクスル２３の中心軸からｌθだけ後方の位置に配置されている。変位検出部４０ｂの上端はトレーラ３０の底面３１に接している。
図８は、トラクタ１０の正面方向から見た変位検出部４０ａ＿１及び４０ａ＿２の配置を示している。変位検出部４０ａ＿１及び４０ａ＿２は、ローリングアクスル２２の中心軸を含む垂直平面に対してそれぞれｌφだけ離れた対称な位置又はその近傍に配置されている。変位検出部４０ａ＿１及び４０ａ＿２の上端はトレーラ底面３１，３１’に接している。
【００３３】
変位検出部４０ｂは、カプラ２０の反対側に隠れて見えないが、ローリングアクスル２２の中心軸を含む垂直平面上又はその近傍に配置されている。
〔１〕牽引車両を基準とした相対姿勢角（θ，φ，ψ）の演算
変位検出部４０ａ＿１，４０ａ＿２，４０ｂの変位センサの検出変位を、それぞれｈ１，ｈ２，ｈ３とする。
【００３４】
図７において、ピッチ角θは、次式で示すことができる。
【００３５】
【数１】
図８において、ロー角φは、次式で示すことができる。
【００３７】
【数２】
一方、ヨー角ψは、キャスタホイール４１の回転半径Ｒ、回転角センサ５１の検出結果ｎと、ロール角φ、及び各変位検出部４０ａ＿１，４０ａ＿２，及び４０ｂの設置位置（図１参照）、ピッチング等により発生する誤差αに基づき次のように計算される。
すなわち、被牽引車両３０のピッチ軸となるカプラ２０のピッチングアクスル２３が、牽引車両１０に固定されているため、同様に牽引車両に固定されている変位検出部４０ａ＿１及び４０ａ＿２のキャスタホイール４１の回転角は、互いに等しい値の誤差αを含む。
【００３９】
したがって、変位検出部４０ａ＿１及び４０ａ＿２からの検出結果である回転角を、それぞれｎ１ｓ，ｎ２ｓとし、ピッチング等により発生する回転数の誤差αを除いたキャスタホイール回転角ｎ１ｒ，ｎ２ｒとしたときの関係式は次のようになる。
【００４０】
【数３】
【数４】
次に、ピッチ角θ＝“０”、及びロール角φ＝“０”の場合であって、図９に示すようにヨー角ψが発生した場合、キャスタホイール４１＿１，４１＿２は、半径ｌφの円上をトレースし、このとき誤差α以外の誤差は無視できる程微少であるから、次式が成り立つ。
【００４３】
【数５】
キャスタホイール４１の回転によって移動する距離（変位）は、ヨー角ψ分の円弧の長さに等しいので、キャスタホイールの半径をＲとすると次式が成り立つ。
【００４５】
【数６】
図８に示したように、ロール角φが発生すると、被牽引車両３０のヨー角回転軸がカプラ上面に固定されており、しかも、ヨー角回転面であるカプラ上面はカプラのローリングアクスルからｈｃｒだけ上方にあるため、ヨー角回転中心が移動し、かつ、回転面がロール角φだけ傾くことから、左右の変位検出部４０ａ＿１，４０ａ＿２のキャスタホイール４１は各々半径の異なる円周上をトレースすることになる。
【００４７】
ロール角φによる中心の移動量をαｒとすれば、次式が成り立つ。
【００４８】
【数７】
したがって、傾きφを考慮した回転半径は、それぞれ、次式で示すことができる。
【００５０】
【数８】
【数９】
φ≪１であることを考慮すると、回転半径は、それぞれ、次式で示すことができる。
【００５３】
【数１０】
【数１１】
したがって、回転数ｎ１ｒ，ｎ２ｒとヨー角ψとの関係は、それぞれ、次式で示すことができる。
【００５６】
【数１２】
【数１３】
したがって、この場合、左右のキャスタホイールの回転角の比ｎ２ｒ／ｎ１ｒは、上記の式（１３）を式（１２）で除した次式で示すことができる。
【００５９】
【数１４】
この式を変形すると回転数ｎ１ｒと回転数ｎ２ｒの差は、ｎ１ｒを用いて次式で示すことができる。
【００６１】
【数１５】
ここで、ピッチ角θにより誤差αが発生したとすると、式（３）及び式（４）から次式が成り立つ。
【００６３】
【数１６】
式（３）、式（１５）、及び式（１６）から、誤差αは、ｎ１ｓ，ｎ２ｓ，φを用いて次式で示すことができる。
【００６５】
【数１７】
したがって、次式が成立する。
【００６７】
【数１８】
したがって、式（１２）、式（１８）より、ヨー角ψは次式で示すことができる。
【００６９】
【数１９】
〔２〕相対姿勢角（θ，φ，ψ）から被牽引車両の相対姿勢角（θ ｔ ，φ ｔ ，ψ ｔ ）への変換
次に、連結車両の運動を考えた場合、牽引車両１０の前後方向をｘ軸、水平方向（左右方向）をｙ軸、及び垂直方向をｚ軸とした直交座標系（ｘ，ｙ，ｚ）上の変位検出部で検出された相対姿勢角（ピッチ角θ、ロール角φ、ヨー角ψ）を、被牽引車両３０の前後方向をｘ’軸、水平方向をｙ’軸、垂直方向をｚ’軸としたを直交座標系（ｘ’，ｙ’，ｚ’）を基準とした相対姿勢角（ピッチ角θｔ、ロール角φｔ、ヨー角ψｔ）に換算する必要がある。
【００７１】
図１０（１）は、３次元直交座標系を示しており、ｘ軸は牽引車両１０の前後方向に平行であり、ｙ軸は左右方向に平行であり、ｚ軸は垂直方向に平行であると仮定する。また、平面ｘｏｙは、被牽引車両３０の基準水平面、平面ｘ’ｏｙは、基準水平面ｘｏｙをｙ軸回りにθ〔ｒａｄ〕だけ回転させた平面、平面ｘ’ｏｙ’は、さらにｘ’軸回りにφ〔ｒａｄ〕だけ回転させた被牽引車両３０のフレーム底面と平行な平面を表している。
【００７２】
四角形ＡＢＣＤは、平面ｘｏｙ上にあり、原点ｏを中心とした一辺の長さが２ｌの正方形である。また、辺ＡＤとｙ軸の交点ａは、辺ＡＤの中点であり、線分ｏａの長さはｌである。四角形Ａ’Ｂ’Ｃ’Ｄ’、及び四角形Ａ”Ｂ”Ｃ”Ｄ”は、それぞれ、正方形ＡＢＣＤを底面、又は水平断面とした平面ｘｏｙに垂直な四角柱の平面ｘ’ｏｙ、及び平面ｘ’ｏｙ’による断面を表している。
【００７３】
平面ｘ’ｏｙ’上で相対ヨー角ψが発生した場合を考え、被牽引車両３０と牽引車両１０の相対ロール角φｔを算出する。原点ｏを通る直線ｂ２ｃ２は平面ｘ’ｏｙ’上でｙ’軸と角度ψを成す。すなわち、平面ｘ’ｏｙ’上でｙ’軸からｚ’軸回りにψ〔ｒａｄ〕だけ回転した直線である。この直線ｂ２ｃ２は、被牽引車両３０の水平軸を表している。
【００７４】
求める角度は、直線ｂ２ｃ２と、直線ｂ２ｃ２を平面ｘｏｙ上に投影した直線ｂｃとの成す角ｂｏｂ２＝φｔである。
分かり易くするため、次のように補助線を引く。まず、点ｂ２を通りｘ軸に平行な直線ｆｇを引く。次に辺ＡＤとｙ軸の交点ａと辺Ａ”Ｄ”とｙ’軸の交点ａ２を結ぶ直線を引く。さらに、この点ａ２を通りｚ軸に平行な直線を引き、この直線と、直線ｆｇ及び辺ＡＤとの交点をそれぞれ点ａ３及び点ａ４とする。さらに、点ａを通りｚ軸に平行な直線を引き、この直線と辺Ａ”Ｄ”及び直線ｆｇとの交点を、それぞれ、点ａ５及び点ａ６とする。
【００７５】
同図（２）は、これらの辺ＡＤ，Ａ’Ｄ’，Ａ”Ｄ”、直線ｆｇ、及び交点ａ，ａ２〜ａ６，ｂ，ｂ２を、平面ｘｏｚに投影した図である。
まず、実際に検出されるロール角は、平面ｙｏｚと平面ｘ’ｏｙ’の交線とｙ軸とが成す角である。これを角φ’とする。同図（１）において、角ｏａａ５は直角であるから、次式が成り立つ。
【００７６】
【数２０】
同図（２）において、直線ａａ５と直線ａ４ａ２は、平行であるから次式が成り立つ。
【００７８】
【数２１】
角ａ２ａｏは直角であるから、次式が成り立つ。
【００８０】
【数２２】
ここで、θは１０°以下であるから、θ〔ｒａｄ〕≪１であるとすると、ｃｏｓθ≒１であると看做せるので、次式が成り立つ。
【００８２】
【数２３】
すなわち、φ＝φ’と看做せることが分かる。
次に、線分ａａ２の長さを求める。角ａ２ａｏは直角であり、角ａｏａ２はφ〔ｒａｄ〕、線分ａｏの長さがｌであるから、次式が成り立つ。
【００８４】
【数２４】
【数２５】
角ａａ４ａ２が直角であり、角ａａ２ａ４はθ〔ｒａｄ〕であるから、次式が成り立つ。
【００８７】
【数２６】
辺ＡＤと直線ｆｇが平行であることから、角ａ２ａ３ｂ２は直角であり、角ａ２ｂ２ａ３はθ〔ｒａｄ〕であるから、次式が成り立つ。
【００８９】
【数２７】
角ｂ２ａ２ｏは直角、角ａ２ｏｂ２はψ〔ｒａｄ〕であるから、次式が成り立つ。
【００９１】
【数２８】
【数２９】
上記の式（２７）〜（２９）から、次式が導き出せる。
【００９４】
【数３０】
線分ａ３ａ４の長さは、線分ａ２ａ４と線分ａ２ａ３の和であり、辺ＡＤと直線ｆｇが平行であることから線分ｂｂ２は、次式で示すことができる。
【００９６】
【数３１】
ここで、角ｏｂｂ２は直角であるから、次式が成り立つ。
【００９８】
【数３２】
ここで、φｔ，φ，θ≪１であることを考慮すると、次式が成り立つ。
【０１００】
【数３３】
次に、相対ピッチ角θｔを求める。この相対ピッチ角θｔは、上記のロール角φｔを求めた条件において、座標軸ｘ，ｘ’をｙ，ｙ’に、ｙ，ｙ’をｘ，ｘ’に、角φｔ，φ，θをそれぞれ角θｔ，θ，−φに置き換えれば同様に求められ、次式が成り立つ。
【０１０２】
【数３４】
次に、相対ヨー角ψｔを求める。この相対ヨー角ψｔは、直線ｂ２ｃ２を平面ｘｏｙ上に投影した直線ｂｃとｙ軸が成す角である。したがって、上記の式（２０）及び角ａａ２ａ５が直角であること、及び式（２３）から、次式が成り立つ。
【０１０４】
【数３５】
また、上記の式（２５）及び（２８）から、次式が導き出せる。
【０１０６】
【数３６】
したがって、線分ａ５ｂ２は次式で示すことができる。
【０１０８】
【数３７】
この式（３７）、及び角ａ５ａ６ｂ２が直角であることから、次式が成り立つ。
【０１１０】
【数３８】
また、角ｂａｏが直角であることから、ヨー角ψｔは、次式で示すことができる。
【０１１２】
【数３９】
この式（３８）において、θ，φ≪１であることを考慮すると、次式が成り立つ。
【０１１４】
【数４０】
したがって、ヨー角ψｔは、次式で示すことができる。
【０１１６】
【数４１】
以上により、牽引車両側で検出された相対姿勢角（φ，θ，ψ）を変換することによって得られた被牽引車両側に相対姿勢角（φｔ，θｔ，ψｔ）を、連結車両を制御するためのデータとして利用することが可能になる。
【０１１８】
【発明の効果】
本発明の相対姿勢角検出装置によれば、被牽引車両に加工又はセンサの設置をすることなく、また、連結時又は切離時に特別な作業を必要とせずに、低コストで、精度及び応答性良く連結車両の相対姿勢角を検出することが可能になる。
【図面の簡単な説明】
【図１】本発明に係る相対姿勢角検出装置の実施例を示した平面図である。
【図２】一般的な牽引車両と被牽引車両を結合するカプラの構成例を示した斜視図である。
【図３】本発明に係る相対姿勢角検出装置における変位検出部の構成例（１）を示した図である。
【図４】本発明に係る相対姿勢角検出装置における変位検出部の構成例（２）を示した図である。
【図５】本発明に係る相対姿勢角検出装置における変位検出部の構成例（３）を示した図である。
【図６】本発明に係る相対姿勢角検出装置における変位検出部の検出端の構成例を示した図である。
【図７】本発明に係る相対姿勢角検出装置における変位検出部の配置例を示した側面図図である。
【図８】本発明に係る相対姿勢角検出装置における変位検出部の配置例を示した正面図である。
【図９】本発明に係る相対姿勢角検出装置における変位検出部の配置例を示した平面図である。
【図１０】本発明に係る相対姿勢角検出装置の演算部における相対姿勢角算出の原理を示した図である。
【符号の説明】
１００ 相対姿勢角検出装置
１０ トラクタ、牽引車両 １１ トラクタフレーム
１２ トラクタフレーム上面、第１の基準面
２０ カプラ ２１，２１’ カプラベース
２２，２２’ ローリングアクスル ２３ ピッチングアクスル
２４ カプラフレーム ３０，３０’ トレーラ、被牽引車両
３１，３１’ トレーラ底面、第２の基準面
４０ａ，４０ａ＿１，４０ａ＿２，４０ｂ 変位検出部
４１，４１＿１，４１＿２ 回転部材、キャスタホイール
４２ａ，４２ｂ キャスタホルダ ４３ａ，４３ｂ ロッド
４４ アーム ４５ デュアルリンクアーム
４６ リターンスプリング ４７ ねじりスプリング
４８ 機構
５０，５０ｂ，５０ｃ，５０＿１〜５０＿３ 変位センサ
５１，５１＿１，５１＿２ 回転角センサ ５２ 支点
６０ ボール ６１ ボールホルダ
７０ 演算部 ８０ 回転軸中心
θ，θｔ ピッチ角 φ，φｔ ロール角
ψ，ψｔ ヨー角
図中、同一符号は同一又は相当部分を示す。
Claims (5)
- 被牽引車両と結合される牽引車両の車幅方向の位置に設置された第１及び第２の変位検出部であって、該牽引車両の第１の基準面に対して垂直方向における、該被牽引車両の第２の基準面の相対的な変位をそれぞれ検出するものと、
該第１及び第２の変位検出部の検出点における該第２の基準面方向の移動変位を、それぞれ検出する第３及び第４の変位検出部と、
該第１及び第２の変位検出部から該牽引車両の前後方向に所定の距離だけ離れた位置に設置された第５の変位検出部であって、該牽引車両の該第１の基準面に対する垂直方向における第２の基準面の相対的な変位を検出するものと、
該第１から該第５までの変位検出部の検出結果に基づき両車両間の相対姿勢角を演算する演算部と、
を備えたことを特徴とする相対姿勢角検出装置。 - 請求項１において、
該第１、第２、及び第５の変位検出部が、各変位を伝えるための変位伝達機構と、変位センサとを備えたことを特徴とする相対姿勢角検出装置。 - 請求項１において、
該第３及び第４の変位検出部が、それぞれ、該第１及び第２の変位検出部の先端に設置され、該第２の基準面に接触して該移動変位に伴って回転する回転部材と、該回転部材の回転角を検出する回転角センサとを備えたことを特徴とする相対姿勢角検出装置。 - 請求項１において、
該相対姿勢角が、該牽引車両に対する該被牽引車両のピッチ角、ロール角、及びヨー角の内の少なくとも１つであることを特徴とした相対姿勢角検出装置。 - 請求項４において、
該演算部が、該牽引車両上の座標系又は被牽引車両上の座標系に基づき該ピッチ角、該ロール角、及び該ヨー角を演算することを特徴とした相対姿勢角検出装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2003069689A JP2004276708A (ja) | 2003-03-14 | 2003-03-14 | 相対姿勢角検出装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP2003069689A JP2004276708A (ja) | 2003-03-14 | 2003-03-14 | 相対姿勢角検出装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| JP2004276708A true JP2004276708A (ja) | 2004-10-07 |
Family
ID=33286647
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP2003069689A Withdrawn JP2004276708A (ja) | 2003-03-14 | 2003-03-14 | 相対姿勢角検出装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP2004276708A (ja) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2008149607A1 (ja) * | 2007-06-08 | 2008-12-11 | Isuzu Motors Limited | 車両の横転防止装置 |
| WO2010115966A1 (de) * | 2009-04-09 | 2010-10-14 | Jost-Werke Gmbh | Verfahren und steuerungssystem für eine sattelkupplungsverschiebeeinrichtung mit erfassung der knickstellung von zugfahrzeugen und auflieger |
| WO2021118502A1 (en) * | 2019-12-12 | 2021-06-17 | Bmc Otomoti̇v Sanayi̇ Ve Ti̇caret Anoni̇m Şi̇rketi̇ | A towing device for semi-tractors with adjustable ride height |
-
2003
- 2003-03-14 JP JP2003069689A patent/JP2004276708A/ja not_active Withdrawn
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2008149607A1 (ja) * | 2007-06-08 | 2008-12-11 | Isuzu Motors Limited | 車両の横転防止装置 |
| WO2010115966A1 (de) * | 2009-04-09 | 2010-10-14 | Jost-Werke Gmbh | Verfahren und steuerungssystem für eine sattelkupplungsverschiebeeinrichtung mit erfassung der knickstellung von zugfahrzeugen und auflieger |
| CN102387957A (zh) * | 2009-04-09 | 2012-03-21 | 约斯特-韦克有限公司 | 带牵引车辆和半挂车的弯折状态检测的用于鞍座连接器移动装置的方法和控制系统 |
| CN102387957B (zh) * | 2009-04-09 | 2015-11-25 | 约斯特-韦克有限公司 | 带牵引车辆和半挂车的弯折状态检测的用于鞍座连接器移动装置的方法和控制系统 |
| WO2021118502A1 (en) * | 2019-12-12 | 2021-06-17 | Bmc Otomoti̇v Sanayi̇ Ve Ti̇caret Anoni̇m Şi̇rketi̇ | A towing device for semi-tractors with adjustable ride height |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US9851721B2 (en) | System and method for calculating the orientation of a device | |
| EP2604973B1 (en) | Six-direction directing device | |
| JPH10307030A (ja) | 無人車両のための運動追跡装置 | |
| US8552362B2 (en) | System and method for linear and angular measurements of a moving object | |
| CN102016495A (zh) | 用于进行底盘测量的方法和装置 | |
| CN109141410B (zh) | Agv组合导航的多传感器融合定位方法 | |
| KR20080015811A (ko) | 차량용 휠 서스펜션 | |
| JP3809698B2 (ja) | 搬送装置 | |
| CN107228663A (zh) | 一种自动导引运输车的定位系统和方法 | |
| JP2010048816A (ja) | ホイールの位置および方向を動的に測定する方法 | |
| KR20190116500A (ko) | 차재기, 연산 장치 및 프로그램 | |
| JP2008215958A (ja) | タイヤ試験装置 | |
| CN110489807B (zh) | 一种摇臂悬架结构巡视器的局部精确定位方法 | |
| JP4382266B2 (ja) | 移動体の位置計測方法及び計測装置 | |
| CN116147665B (zh) | 一种基于矢量地图的卫惯组合导航系统标定精度验证方法 | |
| CN112810664A (zh) | 轨道线路曲率半径在线实时测量系统及方法 | |
| WO2018126911A1 (zh) | 定位方法、装置、业务处理系统以及计算机可读存储介质 | |
| TWI394944B (zh) | 車輛姿態估測系統與方法 | |
| JP3334074B2 (ja) | 移動体の位置計測装置 | |
| JPH05126558A (ja) | 拘束装置および車輪姿勢角計測装置 | |
| CN119717832B (zh) | 一种基于全站仪的放样机器人导航方法 | |
| CN120503215B (zh) | 一种基于正向运动学模型的机器人位姿获取方法及系统 | |
| CN224151730U (zh) | 一种陀螺仪与单编码器融合的全局定位模块 | |
| JP2960372B2 (ja) | 空間座標測定装置 | |
| JP2005189083A (ja) | 自動車用角速度または自動車用角度センサ、およびそれらの故障検出方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| A300 | Withdrawal of application because of no request for examination |
Free format text: JAPANESE INTERMEDIATE CODE: A300 Effective date: 20060606 |