JP2909358B2 - 傾斜角検出装置 - Google Patents
傾斜角検出装置Info
- Publication number
- JP2909358B2 JP2909358B2 JP20811893A JP20811893A JP2909358B2 JP 2909358 B2 JP2909358 B2 JP 2909358B2 JP 20811893 A JP20811893 A JP 20811893A JP 20811893 A JP20811893 A JP 20811893A JP 2909358 B2 JP2909358 B2 JP 2909358B2
- Authority
- JP
- Japan
- Prior art keywords
- inclination angle
- detection
- tilt angle
- angle
- liquid
- 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.)
- Expired - Fee Related
Links
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Description
【０００１】
【産業上の利用分野】本発明は、本体と、その本体に対
して重力により初期姿勢に復帰付勢された被検出体と、
前記本体に対する前記被検出体の姿勢を検出する傾斜角
検出手段とを備えた傾斜角検出装置に関する。
して重力により初期姿勢に復帰付勢された被検出体と、
前記本体に対する前記被検出体の姿勢を検出する傾斜角
検出手段とを備えた傾斜角検出装置に関する。
【０００２】
【従来の技術】上記傾斜角検出装置は、例えば、図２に
示すように、角型の容器２１を本体としてその内部に、
シリコンオイル等の適度の粘性を持つ被検出体としての
液体２３を入れるとともに、傾斜していない状態（図２
の状態）の容器２１に対して内部の液体２３が重力によ
り初期姿勢に復帰しているとき、即ち、液面が水平状態
のときに、同一形状の平行金属板を同一間隔で立設した
検出電極２２を、各平行金属板について同一液面になる
状態で傾斜角検出方向（図の左右方向）に一対配置し、
更に、夫々の検出電極２２の静電容量を測る容量検出部
２４を備えるものである。従って、容器２１が傾斜して
いない状態では両検出電極２２の静電容量は同一である
が、容器２１が傾斜すると、重力により初期姿勢（液面
水平状態）に復帰付勢されている液体２３の液面は、傾
斜の上側になる検出電極については下がるが、傾斜の下
側になる検出電極については上がって液面差が生じ、両
電極間の静電容量は同一ではなくなるので、この静電容
量の違いによって傾斜角を検出するようにしていた。
尚、以上より、検出電極２２と容量検出部２４とが、本
体（容器２１）に対する被検出体（液体２３）の姿勢を
検出する傾斜角検出手段を構成する。
示すように、角型の容器２１を本体としてその内部に、
シリコンオイル等の適度の粘性を持つ被検出体としての
液体２３を入れるとともに、傾斜していない状態（図２
の状態）の容器２１に対して内部の液体２３が重力によ
り初期姿勢に復帰しているとき、即ち、液面が水平状態
のときに、同一形状の平行金属板を同一間隔で立設した
検出電極２２を、各平行金属板について同一液面になる
状態で傾斜角検出方向（図の左右方向）に一対配置し、
更に、夫々の検出電極２２の静電容量を測る容量検出部
２４を備えるものである。従って、容器２１が傾斜して
いない状態では両検出電極２２の静電容量は同一である
が、容器２１が傾斜すると、重力により初期姿勢（液面
水平状態）に復帰付勢されている液体２３の液面は、傾
斜の上側になる検出電極については下がるが、傾斜の下
側になる検出電極については上がって液面差が生じ、両
電極間の静電容量は同一ではなくなるので、この静電容
量の違いによって傾斜角を検出するようにしていた。
尚、以上より、検出電極２２と容量検出部２４とが、本
体（容器２１）に対する被検出体（液体２３）の姿勢を
検出する傾斜角検出手段を構成する。
【０００３】
【発明が解決しようとする課題】しかしながら、上記従
来技術では、検出の安定性を確保する面から液体２３の
粘性をある程度大きくする必要がある。そのため、傾斜
角の変化が遅い場合はそれほど問題にはならないが、傾
斜角の変化が速い場合には、その変化に液体２３の初期
姿勢（液面水平状態）への復帰速度が追従できなくなる
ので、液体２３が初期姿勢に復帰する途中の状態での傾
斜角を測ることになり、正確な傾斜角が検出できないと
いう問題点があった。
来技術では、検出の安定性を確保する面から液体２３の
粘性をある程度大きくする必要がある。そのため、傾斜
角の変化が遅い場合はそれほど問題にはならないが、傾
斜角の変化が速い場合には、その変化に液体２３の初期
姿勢（液面水平状態）への復帰速度が追従できなくなる
ので、液体２３が初期姿勢に復帰する途中の状態での傾
斜角を測ることになり、正確な傾斜角が検出できないと
いう問題点があった。
【０００４】そこで、上記従来の傾斜角検出装置の応答
性の悪さを回避する手段として、レーザージャイロ等の
角速度検出センサの出力情報（角速度）を積分処理する
ことによって角度を求める角度検出装置が考えられる
が、この種の角度検出装置では、積分処理部等の出力が
時間的に変動してしまい、検出の安定性が悪いという別
の問題点があった。
性の悪さを回避する手段として、レーザージャイロ等の
角速度検出センサの出力情報（角速度）を積分処理する
ことによって角度を求める角度検出装置が考えられる
が、この種の角度検出装置では、積分処理部等の出力が
時間的に変動してしまい、検出の安定性が悪いという別
の問題点があった。
【０００５】本発明は、上記実情に鑑みてなされたもの
であって、その目的は、上記従来技術の欠点を解消すべ
く、被検出体の重力による初期姿勢への復帰特性を利用
した傾斜角検出装置において、傾斜角の変化が速い場合
にも、応答遅れなく極力正確な傾斜角を検出できるよう
にすることにある。
であって、その目的は、上記従来技術の欠点を解消すべ
く、被検出体の重力による初期姿勢への復帰特性を利用
した傾斜角検出装置において、傾斜角の変化が速い場合
にも、応答遅れなく極力正確な傾斜角を検出できるよう
にすることにある。
【０００６】
【課題を解決するための手段】本発明による傾斜角検出
装置の第１の特徴構成は、前記本体の傾斜角検出方向で
の角速度を検出する角速度検出手段が設けられ、前記被
検出体のダイナミクスを記述する状態方程式と、前記傾
斜角検出手段及び前記角速度検出手段の検出情報とに基
づいて傾斜角を推定する推定手段が設けられている点に
ある。
装置の第１の特徴構成は、前記本体の傾斜角検出方向で
の角速度を検出する角速度検出手段が設けられ、前記被
検出体のダイナミクスを記述する状態方程式と、前記傾
斜角検出手段及び前記角速度検出手段の検出情報とに基
づいて傾斜角を推定する推定手段が設けられている点に
ある。
【０００７】又、第２の特徴構成は、第１の特徴構成を
実施する際の好適な具体構成を特定するものであって、
前記推定手段が、Ｌｕｅｎｂｅｒｇｅｒのオブザーバー
を利用して構成されている点にある。
実施する際の好適な具体構成を特定するものであって、
前記推定手段が、Ｌｕｅｎｂｅｒｇｅｒのオブザーバー
を利用して構成されている点にある。
【０００８】
【作用】本発明の第１の特徴構成によれば、例えば本体
が速い速度で傾斜すると、重力により初期姿勢に復帰付
勢された被検出体の初期姿勢へ復帰する途中の姿勢が傾
斜角検出手段によって検出され、又、本体の傾斜角検出
方向での角速度が角速度検出手段によって検出される。
そして、推定手段が、被検出体のダイナミクスを記述す
る状態方程式と上記被検出体の復帰途中姿勢及び本体の
角速度の両検出情報とに基づいて、本体の傾斜角（これ
が求めるべき傾斜角である）を推定して求める。
が速い速度で傾斜すると、重力により初期姿勢に復帰付
勢された被検出体の初期姿勢へ復帰する途中の姿勢が傾
斜角検出手段によって検出され、又、本体の傾斜角検出
方向での角速度が角速度検出手段によって検出される。
そして、推定手段が、被検出体のダイナミクスを記述す
る状態方程式と上記被検出体の復帰途中姿勢及び本体の
角速度の両検出情報とに基づいて、本体の傾斜角（これ
が求めるべき傾斜角である）を推定して求める。
【０００９】又、第２の特徴構成によれば、推定手段
が、Ｌｕｅｎｂｅｒｇｅｒのオブザーバーを利用して、
上記傾斜角を推定して求める。以下、この場合について
詳述する。図３に、図２の傾斜角検出装置を例にして示
すように、本体である容器内の被検出体としての液体の
ダイナミクスは、以下の状態方程式で表される。
が、Ｌｕｅｎｂｅｒｇｅｒのオブザーバーを利用して、
上記傾斜角を推定して求める。以下、この場合について
詳述する。図３に、図２の傾斜角検出装置を例にして示
すように、本体である容器内の被検出体としての液体の
ダイナミクスは、以下の状態方程式で表される。
【００１０】
【数１】ｄθ1 ／ｄｔ＝ω1 ｄω1 ／ｄｔ＝−ｋ1 ω1 −ｋ2 θ1 ＋ｋ3 ω2 ｄθ2 ／ｄｔ＝ω2
【００１１】ここで、θ1 ：液面が水平となす角度 ω1 ：液面の変化速度（角速度） θ2 ：容器の傾斜角 ω2 ：傾斜角θ2 の変化速度（角速度） ｋ1 ：液体の内部摩擦抵抗による力の変換係数 ｋ2 ：液体が重力により水平状態に復帰しようとする力
の変換係数 ｋ3 ：液体を重力に逆らって傾斜させようとする外力の
変換係数 であり、ｋ1 、ｋ2 、ｋ3 は実験的に求めたものを使
う。上記数１を、状態変数ベクトルｘ、係数行列Ａ，Ｂ
を使って書き換えると数２になる。
の変換係数 ｋ3 ：液体を重力に逆らって傾斜させようとする外力の
変換係数 であり、ｋ1 、ｋ2 、ｋ3 は実験的に求めたものを使
う。上記数１を、状態変数ベクトルｘ、係数行列Ａ，Ｂ
を使って書き換えると数２になる。
【００１２】
【数２】
【００１３】又、傾斜角検出手段の出力ｙは、液面が水
平となす角度θ1 と容器の傾斜角θ 2 との角度差（θ1
−θ2 ）であり、下記の数３で表される。
平となす角度θ1 と容器の傾斜角θ 2 との角度差（θ1
−θ2 ）であり、下記の数３で表される。
【００１４】
【数３】ｙ＝Ｃ・ｘ Ｃ＝［1 0 -1］
【００１５】ここで、後の処理を容易にするために、ｘ
を行列Ｔで変換する。この変換後の状態方程式の形式は
上記数２及び数３で示される状態方程式の形式と同じで
あるが、各行列の中身は異なっている。
を行列Ｔで変換する。この変換後の状態方程式の形式は
上記数２及び数３で示される状態方程式の形式と同じで
あるが、各行列の中身は異なっている。
【００１６】
【数４】
【００１７】次に、上記状態方程式で表される系のＬｕ
ｅｎｂｅｒｇｅｒのオブザーバーを、下記数５及び図４
に示す。図において、点線で囲んだ範囲ＯＢがオブザー
バーの部分であり、残りの部分が上記状態方程式で表さ
れる系である。
ｅｎｂｅｒｇｅｒのオブザーバーを、下記数５及び図４
に示す。図において、点線で囲んだ範囲ＯＢがオブザー
バーの部分であり、残りの部分が上記状態方程式で表さ
れる系である。
【００１８】
【数５】
【００１９】そして、上記式（１）からルンゲクッタ法
等の解法を使って求めたｘe を下式（２）のようにＴで
逆変換してもとのｘe を得、最終的に傾斜角θ2 がｚ1-
ｙで与えられる（数２参照）。但し、式（１）を解くと
きに、ｙは傾斜角検出手段の出力を用い、ω2 は角速度
検出手段の出力を用いる。
等の解法を使って求めたｘe を下式（２）のようにＴで
逆変換してもとのｘe を得、最終的に傾斜角θ2 がｚ1-
ｙで与えられる（数２参照）。但し、式（１）を解くと
きに、ｙは傾斜角検出手段の出力を用い、ω2 は角速度
検出手段の出力を用いる。
【００２０】
【数６】
【００２１】
【発明の効果】本発明の第１の特徴構成によれば、例え
ば傾斜角の変化が速い場合においても、被検出体の重力
による初期姿勢への復帰特性を利用した傾斜角検出装置
の応答性の悪さを補って正確な傾斜角が検出できる傾斜
角検出装置が得られる。従って、本発明の傾斜角検出装
置によって、例えば、誘導用ビーム光に沿って走行する
作業車側で受光位置から車体位置を検出する場合に、車
体の傾斜の影響を補正して正確な車体位置が検出でき、
あるいは、車体の姿勢制御等における傾斜検出手段とし
て利用できるものとなる。
ば傾斜角の変化が速い場合においても、被検出体の重力
による初期姿勢への復帰特性を利用した傾斜角検出装置
の応答性の悪さを補って正確な傾斜角が検出できる傾斜
角検出装置が得られる。従って、本発明の傾斜角検出装
置によって、例えば、誘導用ビーム光に沿って走行する
作業車側で受光位置から車体位置を検出する場合に、車
体の傾斜の影響を補正して正確な車体位置が検出でき、
あるいは、車体の姿勢制御等における傾斜検出手段とし
て利用できるものとなる。
【００２２】又、第２の特徴構成によれば、第１の特徴
構成を実施する際の好適な手段が得られる。
構成を実施する際の好適な手段が得られる。
【００２３】
【実施例】以下、本発明を田植え用の作業車の走行制御
に適用した場合の実施例を図面に基づいて説明する。
に適用した場合の実施例を図面に基づいて説明する。
【００２４】図９に示すように、圃場内に設定された互
いに平行に並ぶ複数個の走行行程において、田植え用の
作業車Ｖが走行行程の長さ方向に沿って自動走行するよ
うに誘導するために、その走行用ガイドとなる誘導用ビ
ーム光Ａ１をその一端側から他端側に向けて走行行程の
長さ方向に沿って投射する誘導用レーザ光投射装置Ｂ１
が、複数個の走行行程のうちの隣接する一対の走行行程
によって共用されて設けられ、もって、互いに平行する
複数個の走行行程の夫々において前記誘導用ビーム光Ａ
１を投射できるように構成している。
いに平行に並ぶ複数個の走行行程において、田植え用の
作業車Ｖが走行行程の長さ方向に沿って自動走行するよ
うに誘導するために、その走行用ガイドとなる誘導用ビ
ーム光Ａ１をその一端側から他端側に向けて走行行程の
長さ方向に沿って投射する誘導用レーザ光投射装置Ｂ１
が、複数個の走行行程のうちの隣接する一対の走行行程
によって共用されて設けられ、もって、互いに平行する
複数個の走行行程の夫々において前記誘導用ビーム光Ａ
１を投射できるように構成している。
【００２５】又、前記走行行程の長さ方向における両端
部の位置を示すと共に、次の走行行程への回向動作の開
始位置を示すための回向用ビーム光Ａ２を、前記誘導用
ビーム光Ａ１の投射方向に対して直交する方向に向けて
投射する回向用レーザ光投射装置Ｂ２が、走行行程の長
さ方向における両端部夫々に対応して前記走行行程が並
ぶ圃場横側方箇所に設けられている。これにより、前記
作業車Ｖが各走行行程の終端部に達するに伴って、前記
作業車Ｖを次の走行行程に向けて１８０度方向転換させ
ながら、各走行行程を往復走行させることにより、所定
範囲の圃場における植え付け作業を連続して自動的に行
えるようにしている。
部の位置を示すと共に、次の走行行程への回向動作の開
始位置を示すための回向用ビーム光Ａ２を、前記誘導用
ビーム光Ａ１の投射方向に対して直交する方向に向けて
投射する回向用レーザ光投射装置Ｂ２が、走行行程の長
さ方向における両端部夫々に対応して前記走行行程が並
ぶ圃場横側方箇所に設けられている。これにより、前記
作業車Ｖが各走行行程の終端部に達するに伴って、前記
作業車Ｖを次の走行行程に向けて１８０度方向転換させ
ながら、各走行行程を往復走行させることにより、所定
範囲の圃場における植え付け作業を連続して自動的に行
えるようにしている。
【００２６】前記作業車Ｖの構成について説明すれば、
図８及び図９に示すように、左右一対の前輪３及び後輪
４を備えた車体５の後部に、苗植え付け装置６が、昇降
自在で且つ駆動停止自在に設けられている。又、図１に
示すように、前記前後輪３，４は、左右を一対として前
後で各別に操向操作自在に構成され、操向用の油圧シリ
ンダ７，８と、それらに対する電磁操作式の制御弁９，
１０とが設けられている。つまり、前輪３又は後輪４の
一方のみを操向する２輪ステアリング形式、前後輪３，
４を逆位相で且つ同角度に操向する４輪ステアリング形
式、前後輪３，４を同位相で且つ同角度に操向する平行
ステアリング形式の三種類のステアリング形式を選択使
用できるようになっている。
図８及び図９に示すように、左右一対の前輪３及び後輪
４を備えた車体５の後部に、苗植え付け装置６が、昇降
自在で且つ駆動停止自在に設けられている。又、図１に
示すように、前記前後輪３，４は、左右を一対として前
後で各別に操向操作自在に構成され、操向用の油圧シリ
ンダ７，８と、それらに対する電磁操作式の制御弁９，
１０とが設けられている。つまり、前輪３又は後輪４の
一方のみを操向する２輪ステアリング形式、前後輪３，
４を逆位相で且つ同角度に操向する４輪ステアリング形
式、前後輪３，４を同位相で且つ同角度に操向する平行
ステアリング形式の三種類のステアリング形式を選択使
用できるようになっている。
【００２７】図１中、１１はエンジンＥからの出力を変
速して前記前後輪３，４の夫々を同時に駆動する油圧式
無段変速装置、１２はその変速操作用の電動モータ、１
３は前記植え付け装置６の昇降用油圧シリンダ、１４は
その制御弁、１５は前記エンジンＥによる前記植え付け
装置６の駆動を断続する電磁操作式の植え付けクラッチ
である、又、作業車Ｖの走行並びに植え付け装置６の作
動を制御するためのマイクロコンピュータ利用の制御装
置１６が設けられ、この制御装置１６が、後述の各種セ
ンサの検出情報に基づいて、前記変速用モータ１２、前
記各制御弁９，１０，１４、及び、前記植え付けクラッ
チ１５の夫々を制御するように構成されている。
速して前記前後輪３，４の夫々を同時に駆動する油圧式
無段変速装置、１２はその変速操作用の電動モータ、１
３は前記植え付け装置６の昇降用油圧シリンダ、１４は
その制御弁、１５は前記エンジンＥによる前記植え付け
装置６の駆動を断続する電磁操作式の植え付けクラッチ
である、又、作業車Ｖの走行並びに植え付け装置６の作
動を制御するためのマイクロコンピュータ利用の制御装
置１６が設けられ、この制御装置１６が、後述の各種セ
ンサの検出情報に基づいて、前記変速用モータ１２、前
記各制御弁９，１０，１４、及び、前記植え付けクラッ
チ１５の夫々を制御するように構成されている。
【００２８】前記作業車Ｖには、図１に示すように、前
記前後輪３，４夫々の操向角を検出するポテンショメー
タ利用の操向角検出センサＲ１，Ｒ２、前記変速装置１
１の変速状態に基づいて間接的に前後進状態及び車速を
検出するポテンショメータ利用の車速センサＲ３、及び
前記変速装置１１の出力軸の回転数を計数して走行距離
を検出するためのエンコーダＳ４、及び、後述の回向用
光センサＳ３と操向制御用光センサ１７が設けられ、
又、車体５の横幅方向への傾斜角を検出する傾斜センサ
２０、及び、上記車体５の横幅方向への傾斜角の変化速
度（角速度）を検出するレーザージャイロ等からなる角
速度センサ２５（図８参照）の各種センサが設けられて
いる。そして、上記傾斜センサ２０と角速度センサ２５
の情報はマイクロコンピュータ利用の演算装置１９に入
力され、この演算装置１９にて後述のように補正処理さ
れた傾斜角の情報が前記制御装置１６に入力されてい
る。
記前後輪３，４夫々の操向角を検出するポテンショメー
タ利用の操向角検出センサＲ１，Ｒ２、前記変速装置１
１の変速状態に基づいて間接的に前後進状態及び車速を
検出するポテンショメータ利用の車速センサＲ３、及び
前記変速装置１１の出力軸の回転数を計数して走行距離
を検出するためのエンコーダＳ４、及び、後述の回向用
光センサＳ３と操向制御用光センサ１７が設けられ、
又、車体５の横幅方向への傾斜角を検出する傾斜センサ
２０、及び、上記車体５の横幅方向への傾斜角の変化速
度（角速度）を検出するレーザージャイロ等からなる角
速度センサ２５（図８参照）の各種センサが設けられて
いる。そして、上記傾斜センサ２０と角速度センサ２５
の情報はマイクロコンピュータ利用の演算装置１９に入
力され、この演算装置１９にて後述のように補正処理さ
れた傾斜角の情報が前記制御装置１６に入力されてい
る。
【００２９】図８及び図９にも示すように、誘導用ビー
ム光Ａ１に対する車体横幅方向での操向位置のずれをそ
の車体横幅方向での受光位置に基づいて検出するため
に、誘導用ビーム光Ａ１を受光する操向制御用光センサ
１７が作業車Ｖの横幅方向の右側の車体前方側に設けら
れ、更に、回向用ビーム光Ａ２を受光する回向用光セン
サＳ３が、車体左右何れの側からでも回向用ビーム光Ａ
２を受光できるように、操向制御用光センサ１７の前方
側の車体左右両側の夫々に設けられている。尚、回向用
光センサＳ３は回向用ビーム光Ａ２に対する受光の有無
のみを検出するように構成され、受光位置は判別できな
いようになっている。
ム光Ａ１に対する車体横幅方向での操向位置のずれをそ
の車体横幅方向での受光位置に基づいて検出するため
に、誘導用ビーム光Ａ１を受光する操向制御用光センサ
１７が作業車Ｖの横幅方向の右側の車体前方側に設けら
れ、更に、回向用ビーム光Ａ２を受光する回向用光セン
サＳ３が、車体左右何れの側からでも回向用ビーム光Ａ
２を受光できるように、操向制御用光センサ１７の前方
側の車体左右両側の夫々に設けられている。尚、回向用
光センサＳ３は回向用ビーム光Ａ２に対する受光の有無
のみを検出するように構成され、受光位置は判別できな
いようになっている。
【００３０】前記操向制御用光センサ１７について説明
を加えれば、図７及び図８に示すように、車体前後方向
に間隔ｄを隔て且つ上下方向にも間隔を隔てて配置され
た前後一対の光センサＳ１，Ｓ２から構成され、そし
て、前記誘導用ビーム光Ａ１が車体前後の何れの方向か
ら入射される場合でも差のない状態で受光できるように
するために、車体前後の各方向からの入射光を前記光セ
ンサＳ１，Ｓ２夫々の受光面に向けて反射する反射鏡１
８を備えている。前記前後一対の光センサＳ１，Ｓ２の
夫々は、複数個の受光素子Ｄを車体横幅方向に並設した
ものであって、横幅方向でのセンサ中心Ｄ０に位置する
受光素子の位置を基準として、誘導用ビーム光Ａ１を受
光した前後夫々の受光素子の位置Ｘ１，Ｘ２即ち受光位
置を検出できるように構成されている。
を加えれば、図７及び図８に示すように、車体前後方向
に間隔ｄを隔て且つ上下方向にも間隔を隔てて配置され
た前後一対の光センサＳ１，Ｓ２から構成され、そし
て、前記誘導用ビーム光Ａ１が車体前後の何れの方向か
ら入射される場合でも差のない状態で受光できるように
するために、車体前後の各方向からの入射光を前記光セ
ンサＳ１，Ｓ２夫々の受光面に向けて反射する反射鏡１
８を備えている。前記前後一対の光センサＳ１，Ｓ２の
夫々は、複数個の受光素子Ｄを車体横幅方向に並設した
ものであって、横幅方向でのセンサ中心Ｄ０に位置する
受光素子の位置を基準として、誘導用ビーム光Ａ１を受
光した前後夫々の受光素子の位置Ｘ１，Ｘ２即ち受光位
置を検出できるように構成されている。
【００３１】前記傾斜センサ２０は、図２に示すよう
に、角型の容器２１を本体としてその内部に、シリコン
オイル等の被検出体としての液体２３を入れるととも
に、傾斜していない状態（図２の状態）の容器２１に対
して内部の液体２３が重力により初期姿勢に復帰してい
るとき、即ち、液面が水平状態のときに、同一形状の平
行金属板を同一間隔で立設した検出電極２２を、各平行
金属板について同一液面になる状態で傾斜角検出方向
（図の左右方向）に一対配置し、更に、夫々の検出電極
２２の静電容量を測る容量検出部２４を備えている。
尚、傾斜センサ２０は、上記傾斜角検出方向が車体横幅
方向に一致するように設置する。従って、前記角速度セ
ンサ２５が、容器２１の傾斜角検出方向での角速度を検
出する角速度検出手段を構成する。そして、車体が横幅
方向に傾斜していない状態では両検出電極２２の静電容
量Ｃ１，Ｃ２は同一であるが、車体が横幅方向に傾斜す
ると、重力により初期姿勢（液面水平状態）に復帰付勢
されている液体２３の液面は、傾斜の上側になる検出電
極については下がるが、傾斜の下側になる検出電極につ
いては上がって液面差が生じ（図３参照）、両電極間の
静電容量は同一ではなくなるので、この静電容量の違い
の情報から演算手段１９が傾斜角を検出処理する。以上
より、検出電極２２と容量検出部２４とが、容器２１に
対する液体２３の姿勢を検出する傾斜角検出手段２２，
２４を構成する。
に、角型の容器２１を本体としてその内部に、シリコン
オイル等の被検出体としての液体２３を入れるととも
に、傾斜していない状態（図２の状態）の容器２１に対
して内部の液体２３が重力により初期姿勢に復帰してい
るとき、即ち、液面が水平状態のときに、同一形状の平
行金属板を同一間隔で立設した検出電極２２を、各平行
金属板について同一液面になる状態で傾斜角検出方向
（図の左右方向）に一対配置し、更に、夫々の検出電極
２２の静電容量を測る容量検出部２４を備えている。
尚、傾斜センサ２０は、上記傾斜角検出方向が車体横幅
方向に一致するように設置する。従って、前記角速度セ
ンサ２５が、容器２１の傾斜角検出方向での角速度を検
出する角速度検出手段を構成する。そして、車体が横幅
方向に傾斜していない状態では両検出電極２２の静電容
量Ｃ１，Ｃ２は同一であるが、車体が横幅方向に傾斜す
ると、重力により初期姿勢（液面水平状態）に復帰付勢
されている液体２３の液面は、傾斜の上側になる検出電
極については下がるが、傾斜の下側になる検出電極につ
いては上がって液面差が生じ（図３参照）、両電極間の
静電容量は同一ではなくなるので、この静電容量の違い
の情報から演算手段１９が傾斜角を検出処理する。以上
より、検出電極２２と容量検出部２４とが、容器２１に
対する液体２３の姿勢を検出する傾斜角検出手段２２，
２４を構成する。
【００３２】但し、液体２３が初期姿勢（液面水平状
態）に復帰するには所定の応答時間を要するので、傾斜
角の変化が速いときは、液面は水平状態に復帰しておら
ず、図３に示すように、車体５が例えば左側に傾斜角θ
2 傾斜しても、液面は途中の角度θ1 （水平状態では
０）の状態であり、傾斜センサ２０からは真の傾斜角θ
2と復帰途中の液面の角度θ1 との角度差（θ1 −
θ2 ）が得られるにすぎない。そこで、前記演算装置１
９を利用して、液体２３のダイナミクスを記述する状態
方程式と、前記傾斜角検出手段２２，２４即ち傾斜セン
サ２０の検出情報及び前記角速度センサ２５の検出情報
とに基づいて傾斜角θ2 を推定する推定手段が、Ｌｕｅ
ｎｂｅｒｇｅｒのオブザーバーを利用して構成されてい
る。即ち、図５に処理のフローチャートを示すように、
角速度センサ２５の出力である角速度ω2、及び傾斜セ
ンサ２０の出力ｙである上記角度差（θ1 −θ2 ）の両
データを取り込み、前述の式（１）の微分方程式を解
き、その解ｚ1 から傾斜角θ2 を（ｚ 1 −ｙ）として求
める。
態）に復帰するには所定の応答時間を要するので、傾斜
角の変化が速いときは、液面は水平状態に復帰しておら
ず、図３に示すように、車体５が例えば左側に傾斜角θ
2 傾斜しても、液面は途中の角度θ1 （水平状態では
０）の状態であり、傾斜センサ２０からは真の傾斜角θ
2と復帰途中の液面の角度θ1 との角度差（θ1 −
θ2 ）が得られるにすぎない。そこで、前記演算装置１
９を利用して、液体２３のダイナミクスを記述する状態
方程式と、前記傾斜角検出手段２２，２４即ち傾斜セン
サ２０の検出情報及び前記角速度センサ２５の検出情報
とに基づいて傾斜角θ2 を推定する推定手段が、Ｌｕｅ
ｎｂｅｒｇｅｒのオブザーバーを利用して構成されてい
る。即ち、図５に処理のフローチャートを示すように、
角速度センサ２５の出力である角速度ω2、及び傾斜セ
ンサ２０の出力ｙである上記角度差（θ1 −θ2 ）の両
データを取り込み、前述の式（１）の微分方程式を解
き、その解ｚ1 から傾斜角θ2 を（ｚ 1 −ｙ）として求
める。
【００３３】又、前記制御装置１６は、前記操向制御用
光センサ１７の受光情報に基づいて前記作業車Ｖが前記
誘導用ビーム光Ａ１に沿って自動走行するように操向制
御すると共に、前記作業車Ｖが一つの走行行程の終端部
に達するに伴って、その一つの走行行程に隣接する次の
走行行程の始端部に向けて前記作業車Ｖを回向動作させ
るように構成されている。
光センサ１７の受光情報に基づいて前記作業車Ｖが前記
誘導用ビーム光Ａ１に沿って自動走行するように操向制
御すると共に、前記作業車Ｖが一つの走行行程の終端部
に達するに伴って、その一つの走行行程に隣接する次の
走行行程の始端部に向けて前記作業車Ｖを回向動作させ
るように構成されている。
【００３４】前記操向制御について説明すれば、前記操
向制御用光センサ１７の前記前後一対の光センサＳ１，
Ｓ２の夫々の受光素子の受光位置Ｘ１，Ｘ２とその車体
前後方向での取り付け間隔ｄとに基づいて、図７及び下
式に示すように、前記誘導用ビーム光Ａ１の投射方向に
対する平面視での車体５の傾きφと横幅方向における位
置の偏位αとを求め、この傾きφと偏位αとが共に零と
なるように、目標操向角を設定して操向制御することに
なる。
向制御用光センサ１７の前記前後一対の光センサＳ１，
Ｓ２の夫々の受光素子の受光位置Ｘ１，Ｘ２とその車体
前後方向での取り付け間隔ｄとに基づいて、図７及び下
式に示すように、前記誘導用ビーム光Ａ１の投射方向に
対する平面視での車体５の傾きφと横幅方向における位
置の偏位αとを求め、この傾きφと偏位αとが共に零と
なるように、目標操向角を設定して操向制御することに
なる。
【００３５】
【数７】φ＝ｔａｎ-1（｜Ｘ１−Ｘ２｜／ｄ） α＝Ｘ１
【００３６】尚、この例では、前記横幅方向における位
置の偏位αは、前記一対の光センサＳ１，Ｓ２の一方
（Ｓ１）の受光位置としているが、前記傾きφによる誤
差が生じないようにするために、前記一対の光センサＳ
１，Ｓ２夫々の受光位置Ｘ１，Ｘ２の平均値を用いるよ
うにしてもよい。そして、前記作業車Ｖは、但し、本実
施例では、各走行行程では、前記前輪３のみを操向する
２輪ステアリング形式で操向制御するように構成してあ
る。
置の偏位αは、前記一対の光センサＳ１，Ｓ２の一方
（Ｓ１）の受光位置としているが、前記傾きφによる誤
差が生じないようにするために、前記一対の光センサＳ
１，Ｓ２夫々の受光位置Ｘ１，Ｘ２の平均値を用いるよ
うにしてもよい。そして、前記作業車Ｖは、但し、本実
施例では、各走行行程では、前記前輪３のみを操向する
２輪ステアリング形式で操向制御するように構成してあ
る。
【００３７】ここで、車体５が横幅方向に傾斜していな
い場合には上式に従って横幅方向の位置偏位αを求めれ
ばよいが、車体５が横幅方向に例えば傾斜角θ2 で傾斜
した場合には、図６に示すように、前後一対の光センサ
Ｓ１，Ｓ２の夫々の受光素子の受光位置Ｘ１，Ｘ２は、
車体５が水平状態のときの位置から傾斜側に移動するの
で、この移動による誤差を、下式にように補正処理にて
除去する。尚、傾きφについては、両センサの受光位置
Ｘ１，Ｘ２の移動量が同じになり、引き算されて誤差が
打ち消されるので、補正処理は行わない。
い場合には上式に従って横幅方向の位置偏位αを求めれ
ばよいが、車体５が横幅方向に例えば傾斜角θ2 で傾斜
した場合には、図６に示すように、前後一対の光センサ
Ｓ１，Ｓ２の夫々の受光素子の受光位置Ｘ１，Ｘ２は、
車体５が水平状態のときの位置から傾斜側に移動するの
で、この移動による誤差を、下式にように補正処理にて
除去する。尚、傾きφについては、両センサの受光位置
Ｘ１，Ｘ２の移動量が同じになり、引き算されて誤差が
打ち消されるので、補正処理は行わない。
【００３８】具体的には、一対の光センサＳ１，Ｓ２
は、前輪３の車軸位置から高さＨの位置で、且つ、セン
サの中央位置Ｄ０が車体５の横幅方向の中央から距離Ｌ
に位置するように設けられている。従って、車体５が横
幅方向に傾斜角θ2 傾いた場合の補正処理後の位置偏位
α１は、補正処理前の位置偏位αから下式のようにして
求まる。
は、前輪３の車軸位置から高さＨの位置で、且つ、セン
サの中央位置Ｄ０が車体５の横幅方向の中央から距離Ｌ
に位置するように設けられている。従って、車体５が横
幅方向に傾斜角θ2 傾いた場合の補正処理後の位置偏位
α１は、補正処理前の位置偏位αから下式のようにして
求まる。
【００３９】
【数８】 α１＝（Ｌ＋α）・ｃｏｓθ2 ＋Ｈ・ｓｉｎθ2 −Ｌ
【００４０】〔別実施例〕上記実施例では、被検出体で
ある液体２３の本体である容器２１に対する姿勢を検出
する傾斜角検出手段を、容器２１に固定された検出電極
２２とその検出電極２２の静電容量を検出する容量検出
部２４とによって構成し、上記姿勢を容器２１に対する
傾斜方向での液面差として静電容量の差に置き換えて検
出する場合を示したが、静電容量に置き換えずに、例え
ば、液面に浮いたフロートの上下位置を直接検出するよ
うにしてもよい。
ある液体２３の本体である容器２１に対する姿勢を検出
する傾斜角検出手段を、容器２１に固定された検出電極
２２とその検出電極２２の静電容量を検出する容量検出
部２４とによって構成し、上記姿勢を容器２１に対する
傾斜方向での液面差として静電容量の差に置き換えて検
出する場合を示したが、静電容量に置き換えずに、例え
ば、液面に浮いたフロートの上下位置を直接検出するよ
うにしてもよい。
【００４１】又、上記実施例では、被検出体として液体
を利用する傾斜角検出装置について示したが、これ以外
に、例えば、本体である容器内に設けた枢支点に、傾斜
検出方向に揺動自在な状態で被検出体である重りを垂下
し、この重りの揺動角によって傾斜角を検出する装置で
もよい。この場合の初期姿勢は重りが枢支点の垂直下方
に位置した姿勢になる。又、検出値の安定性を確保する
ために、重りを液中に浸したり、あるいは、機械的なダ
ンパを設けて検出の応答性が悪くなるので、本発明によ
って傾斜角を推定する必要が出てくる。
を利用する傾斜角検出装置について示したが、これ以外
に、例えば、本体である容器内に設けた枢支点に、傾斜
検出方向に揺動自在な状態で被検出体である重りを垂下
し、この重りの揺動角によって傾斜角を検出する装置で
もよい。この場合の初期姿勢は重りが枢支点の垂直下方
に位置した姿勢になる。又、検出値の安定性を確保する
ために、重りを液中に浸したり、あるいは、機械的なダ
ンパを設けて検出の応答性が悪くなるので、本発明によ
って傾斜角を推定する必要が出てくる。
【００４２】又、上記実施例では、角速度検出手段２５
としてレーザージャイロ式の角速度センサの例を示した
が、これに限るものではない。
としてレーザージャイロ式の角速度センサの例を示した
が、これに限るものではない。
【００４３】又、上記実施例では、推定手段１９として
利用したＬｕｅｎｂｅｒｇｅｒのオブザーバーの解を求
めるのに、式（１）を直接計算するものを示したが、こ
れ以外に、角速度ω2 と出力ｙとの組に対して予め上記
式（１）の解ｚを計算した結果をテーブルに記憶してお
き、そのテーブルを参照して所定の解ｚを得るようにす
ることもでき、この場合は、式（１）を直接計算するも
のに比べて高速に処理できる利点がある。又、推定手段
１９としては、Ｌｕｅｎｂｅｒｇｅｒのオブザーバーを
利用する場合に限るものではない。
利用したＬｕｅｎｂｅｒｇｅｒのオブザーバーの解を求
めるのに、式（１）を直接計算するものを示したが、こ
れ以外に、角速度ω2 と出力ｙとの組に対して予め上記
式（１）の解ｚを計算した結果をテーブルに記憶してお
き、そのテーブルを参照して所定の解ｚを得るようにす
ることもでき、この場合は、式（１）を直接計算するも
のに比べて高速に処理できる利点がある。又、推定手段
１９としては、Ｌｕｅｎｂｅｒｇｅｒのオブザーバーを
利用する場合に限るものではない。
【００４４】又、上記実施例では、本発明を田植え用の
作業車の走行制御装置に適用したものを例示したが、こ
れ以外に、例えば、車体の水平制御等の姿勢制御にも適
用できる。又、田植え機以外の農機及び各種作業車、並
びに、地上に固定した状態で傾斜角が変化する装置の傾
斜角検出にも適用できる。
作業車の走行制御装置に適用したものを例示したが、こ
れ以外に、例えば、車体の水平制御等の姿勢制御にも適
用できる。又、田植え機以外の農機及び各種作業車、並
びに、地上に固定した状態で傾斜角が変化する装置の傾
斜角検出にも適用できる。
【００４５】尚、特許請求の範囲の項に図面との対照を
便利にするために符号を記すが、該記入により本発明は
添付図面の構成に限定されるものではない。
便利にするために符号を記すが、該記入により本発明は
添付図面の構成に限定されるものではない。
【図１】制御構成のブロック図
【図２】傾斜検出手段の概略斜視図
【図３】傾斜検出手段の検出モデルを説明する図
【図４】オブザーバーの説明図
【図５】傾斜角推定処理のフローチャート
【図６】車体の傾斜による位置ずれ検出値の補正を説明
する背面図
する背面図
【図７】操向制御用光センサを説明する概略平面図
【図８】作業車及び誘導用ビーム光の投射状態を示す側
面図
面図
【図９】作業車の走行行程を示す概略平面図
２１ 本体 ２３ 被検出体 ２２，２４ 傾斜角検出手段 ２５ 角速度検出手段 １９ 推定手段
フロントページの続き (56)参考文献 特開 昭50−866（ＪＰ，Ａ) 特開 昭56−122911（ＪＰ，Ａ) 特開 昭61−59208（ＪＰ，Ａ) 特開 平１−163607（ＪＰ，Ａ) 特開 昭63−55411（ＪＰ，Ａ) 特開 平３−90808（ＪＰ，Ａ) (58)調査した分野(Int.Cl.6，ＤＢ名) G01C 9/00 - 9/36
Claims (2)
- 【請求項１】 本体（２１）と、その本体（２１）に対
して重力により初期姿勢に復帰付勢された被検出体（２
３）と、前記本体（２１）に対する前記被検出体（２
３）の姿勢を検出する傾斜角検出手段（２２，２４）と
を備えた傾斜角検出装置であって、 前記本体（２１）の傾斜角検出方向での角速度を検出す
る角速度検出手段（２５）が設けられ、 前記被検出体（２３）のダイナミクスを記述する状態方
程式と、前記傾斜角検出手段（２２，２４）及び前記角
速度検出手段（２５）の検出情報とに基づいて傾斜角を
推定する推定手段（１９）が設けられている傾斜角検出
装置。 - 【請求項２】 前記推定手段（１９）が、Ｌｕｅｎｂｅ
ｒｇｅｒのオブザーバーを利用して構成されている請求
項１記載の傾斜角検出装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP20811893A JP2909358B2 (ja) | 1993-08-24 | 1993-08-24 | 傾斜角検出装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP20811893A JP2909358B2 (ja) | 1993-08-24 | 1993-08-24 | 傾斜角検出装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH0763554A JPH0763554A (ja) | 1995-03-10 |
| JP2909358B2 true JP2909358B2 (ja) | 1999-06-23 |
Family
ID=16550938
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP20811893A Expired - Fee Related JP2909358B2 (ja) | 1993-08-24 | 1993-08-24 | 傾斜角検出装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP2909358B2 (ja) |
-
1993
- 1993-08-24 JP JP20811893A patent/JP2909358B2/ja not_active Expired - Fee Related
Also Published As
| Publication number | Publication date |
|---|---|
| JPH0763554A (ja) | 1995-03-10 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US8155828B2 (en) | Control method of traveling dolly | |
| US6134509A (en) | Road curvature estimating apparatus | |
| US6725140B2 (en) | Method and apparatus for determining lateral velocity of a motor vehicle in closed form for all road and driving conditions | |
| JPH0944240A (ja) | 移動車の誘導装置 | |
| GB2357743A (en) | An arrangement for guiding steering to assist parallel parking | |
| JP2001010518A (ja) | 車両用操舵制御装置 | |
| KR101117040B1 (ko) | 도립 진자형 이동 기구 | |
| JPH08137549A (ja) | 車両の誘導走行制御装置 | |
| JPH0390482A (ja) | 後輪操舵制御装置 | |
| JP2909358B2 (ja) | 傾斜角検出装置 | |
| US11858569B2 (en) | Vehicle | |
| JP3034121B2 (ja) | 無人車の制御装置 | |
| JP3479366B2 (ja) | 傾斜角検出装置 | |
| JPH0561147B2 (ja) | ||
| JP2946778B2 (ja) | 油圧駆動車両の走行制御装置 | |
| JPH03189805A (ja) | 車両の自動操舵方法及びその自動操舵装置 | |
| JPH11231939A (ja) | 無人車の操向制御方法及びその装置 | |
| JPH1120725A (ja) | 車両の操舵力推定装置 | |
| JPH06309029A (ja) | 移動車位置検出装置 | |
| JPH1114353A (ja) | 傾斜センサの補正装置 | |
| JP3334074B2 (ja) | 移動体の位置計測装置 | |
| JPH0950317A (ja) | ビーム光誘導式移動車の走行制御装置 | |
| JP2726995B2 (ja) | 車両の４輪操舵特性検査装置およびその方法 | |
| JPH0739205A (ja) | 作業車のステアリング制御機構 | |
| JPH058610U (ja) | 自律走行無人搬送車の位置検出装置 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| LAPS | Cancellation because of no payment of annual fees |