專利名稱：具有傾斜角推定機構(gòu)的移動體的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及具有傾斜角推定機構(gòu)的移動體，特別涉及具有傾斜角推定 機構(gòu)的倒立兩輪型車輛。
背景技術(shù)：
在倒立兩輪型車輛這樣的移動體中，為了維持倒立姿勢而需要高精度 地推定出傾斜角度。這不僅限于兩輪型車輛，即使在步行型移動體或履帶 驅(qū)動的移動體中，也要求高精度地推定出傾斜角度。
以往，作為推定傾斜角的機構(gòu)而有使用陀螺(角速度)傳感器的方式 或使用加速度傳感器的方式。在使用陀螺傳感器的方式中，可以通過對陀 螺傳感器的輸出信號進行積分而求出傾斜角度，但是在陀螺傳感器的輸出
信號上原本就重疊有偏移噪聲(offset noise)，因此存在著相對于真值偏 差逐漸變大的問題。
在使用加速度傳感器的方式中，使用通過二軸以上的加速度傳感器得 到的幾何學(xué)關(guān)系來計算傾斜角度。根據(jù)該方式，由于根據(jù)二軸重力加速度 的值來計算傾斜角度，因此存在著如果不是靜止的狀態(tài)則推定精度會變差 的問題。特別是在移動體進行了加減速的情況下，由于加減速而產(chǎn)生的成 分會作為噪聲而重疊在二軸加速度傳感器上。
另外，還提出了使用陀螺傳感器和三軸加速度傳感器并通過自動慣性 導(dǎo)航(INS，慣性導(dǎo)航系統(tǒng))來推定傾斜角度的方式。但是，無法解決上 述陀螺傳感器中的偏移噪聲的問題和加速度傳感器中的由于加減速而產(chǎn)生 的噪聲的問題。
另外，專利文獻1公開了按照每個頻帶切換使用陀螺傳感器和加速度 計的技術(shù)，但這不是與具有傾斜角推定機構(gòu)的移動體相關(guān)的技術(shù)。 專利文獻l:日本專利文獻特開平4 —346021號公報。

發(fā)明內(nèi)容
發(fā)明所要解決的問題
如上所述，雖然能夠通過使用陀螺傳感器和加速度傳感器求出車身傾 斜角度，但是在精度上存在問題。
本發(fā)明是為了解決上述問題而完成的，其目的在于提供一種具有能夠 高精度地推定出車身傾斜角度的傾斜角推定機構(gòu)的移動體。
用于解決問題的手段
本發(fā)明的移動體包括加速度傳感器，檢測二軸方向上的加速度；陀 螺傳感器，檢測車身的角速度；以及傾斜角推定機構(gòu)，推定車身的傾斜 角；其中，該傾斜角推定機構(gòu)具有根據(jù)所述加速度傳感器檢測出的加速 度來推定第一傾斜角的單元；按照所述移動體的線性模型方程并根據(jù)所述 陀螺傳感器檢測出的角速度來推定第二傾斜角的單元；以及通過將所述第 一傾斜角作為觀察器的參照值并進行反饋控制以使所述第二傾斜角追隨所 述第一傾斜角來推定傾斜角的單元。
這里，優(yōu)選的是，所述傾斜角推定機構(gòu)具有將用于調(diào)整所述第一傾斜 角的影響的觀察器增益K設(shè)定為任意值的設(shè)定單元。
另外，優(yōu)選的是，所述設(shè)定單元在移動體處于前后加速狀態(tài)的情況下 將所述觀察器增益K設(shè)定為小的值。
另外，優(yōu)選的是，所述設(shè)定單元根據(jù)加速度傳感器和陀螺傳感器的故 障狀態(tài)來設(shè)定該觀察器增益K。
另外，優(yōu)選的是，在所述加速度傳感器的后級設(shè)置有低通濾波器。
優(yōu)選實施方式中的移動體是倒立兩輪型車輛。
發(fā)明的效果
根據(jù)本發(fā)明，可以提供一種具有能夠高精度地推定出車身傾斜角度的 傾斜角推定機構(gòu)的移動體。


圖l是用于說明本發(fā)明實施方式的移動體的傳感器座標(biāo)系的圖；圖2是表示本發(fā)明實施方式的移動體的傾斜角推定方法的框圖3是表示本發(fā)明實施方式的移動體的傾斜角推定方法的流程圖;
圖4是表示本發(fā)明實施方式的移動體的傾斜角推定方法的流程圖。
標(biāo)號說明
l車身
2車輪
io移動體
具體實施例方式
圖1表示了本發(fā)明的具有傾斜角推定機構(gòu)的移動體的傳感器座標(biāo)系。
如圖l所示，該移動體IO具有車身l和經(jīng)由未圖示的驅(qū)動機構(gòu)而固定在車 身l上的車輪2。
將與該移動體10的前方(即，移動方向、傾斜(pitch)方向)正交 并沿近似水平方向(即，側(cè)傾(roll)方向)延伸的軸作為X軸，將該移 動體10的前方(即，移動方向、傾斜方向)作為Y軸，將鉛垂方向作為 Z軸。在本例中，將連結(jié)車身1和車輪2的旋轉(zhuǎn)中心的線與Z軸所成的角 度作為車身傾斜角。
移動體IO包括測量Y軸方向和Z軸方向上的加速度的加速度傳感器 和設(shè)置在其后級的截止頻率低的低通濾波器。可以根據(jù)由這兩個加速度傳 感器檢測出的Y軸方向和Z軸方向上的加速度u (1) 、 u (2)，按照下 式求出車身傾斜角(即，傾角(pitchangle))"。
<formula>formula see original document page 5</formula> (1)
另外，該移動體10包括測量x軸方向上的角速度的單軸陀螺傳感器
(角速度傳感器)。并且，在移動體10中，通過計算機來實現(xiàn)輸入加速
度傳感器和陀螺傳感器的檢測信號和控制信號并推定(計算)出車身傾斜 角的傾斜角推定機構(gòu)。
該傾斜角推定機構(gòu)包括根據(jù)加速度傳感器檢測出的加速度來推定傾 斜角的單元；根據(jù)陀螺傳感器檢測出的角速度來推定傾斜角的單元；以及根據(jù)通過加速度傳感器得到的傾斜角和通過陀螺傳感器得到的傾斜角并將
移動體的線性模型方程(linear model equation)作為觀察器來推定傾斜角 的單元。此時，推定傾斜角的單元將通過陀螺傳感器得到的傾斜角作為觀 察器的參照值，并按照使推定值反饋追隨通過陀螺傳感器得到的傾斜角的 方式來進行推定。
圖2是用于說明本發(fā)明的傾斜角推定處理的框圖。如圖所示，向Ii輸 入控制信號，向12輸入由Y軸加速度傳感器檢測出的Y軸方向上的加速度 信號，向13輸入由Z軸加速度傳感器檢測出的Z軸方向上的加速度信號， 向14輸入由單軸陀螺傳感器檢測出的角速度信號。
首先，根據(jù)輸入給12的Y軸方向上的加速度和輸入給13的Z軸方向上 的加速度，按照上述式(1)以幾何學(xué)方式計算出車身傾斜角。這里計算 出的傾斜角對于直流(DC)成分 低頻帶來說具有高精度，但是由于當(dāng)伴 隨著加減速而運動時加速度傳感器被施加了噪聲，因此對于中頻帶 高頻 帶來說精度低。
接著，輸入控制信號、計算出的車身傾斜角、由陀螺傳感器檢測出的 角速度，并且通過由外力觀察器進行的反饋處理最終計算出傾斜角、傾斜 角速度、以及外力，分別從02、 03輸出。另外，在圖2中，將與外 力觀察器相關(guān)的式y(tǒng) (n) =Cx (n) +Du (n)作為式(2)，將x (n+ 1) 二Ax (n) +Bu (n)作為式(3)。
接著，對外力觀察器的設(shè)計的詳細(xì)情況進行說明。
首先，對車身傾斜角進行線性模型化。倒立兩輪車的車身模型(運動 方程式)可以如下來表示。
徵l]
(m,/2 + /, + n2乙)々 + (m'H _ /i2/ )》'- "hg/ 7 + /r _ 0 )- -朋 ": (OT,r/ _ "V卿)力.+ {(wi, + /n2)/ 2 +乙+ -々)-股 (5:
這里，r/:車身的傾斜角，0:車輪的旋轉(zhuǎn)角，m1:車身的質(zhì)量，m2:
車輪的質(zhì)量，1:車身重心距車軸的距離，J1:關(guān)于車身重心的慣性力矩， Jm:馬達轉(zhuǎn)子的慣性力矩，Jw:關(guān)于車輪軸的慣性力矩，n:齒輪比(馬 達車輪=1: n) ， r:車輪半徑，fh驅(qū)動系統(tǒng)的阻力(resistance)，
6U
馬達的轉(zhuǎn)矩值。
在上述式(4) 、 (5)中，由于在本發(fā)明中能夠推定出車身傾斜角7/ 即可，因此僅考慮式(4)。另外，在求傾斜角速度的情況下利用式 (5)。如果通過7;來表示式(4)的左邊，則成為式(6)。 傲2]
(;n，/2 + A + nV")力.+ /力- /n,g/ t; = -/iu - (m'r/ _ n、 )<j +(6)
將該式(6)作為基本模型來生成外力觀察器。這里，式(6)右邊的 0項作為w方程式的外力而發(fā)揮作用。在本發(fā)明中，如果忽視該0項，則 推定誤差會變大，因此為了避免這種情況，構(gòu)成了將外力f也作為狀態(tài)量 來進行考慮的外力觀察器。
下面，對外力觀察器的基本模型的導(dǎo)出進行說明。現(xiàn)在，通過下式 (7)來表示狀態(tài)量。
徵3]
X
(7)
這里，f為外力。
當(dāng)使用式7表示的狀態(tài)j 示成下式(8) 、 (9)。 [數(shù)4]
:將式6轉(zhuǎn)換成狀態(tài)方程式表示方式時，可以表
<formula>formula see original document page 7</formula>
這里，對于外力f來說，在采樣周期的期間內(nèi)為取恒定值的階躍 (stepwise)狀，該期間的微分值為O。下面，對外力觀察器的增益的確定方法進行說明。 外力觀察器可以通過下式(10)來表示。
^ - j義+ + K(Y - Ci ) (10) X:推定值
\ K:觀測器增益
對于式(10)中的觀察器增益(反饋增益)K來說，針對各個狀態(tài)量 r/、 r/點、f而分別設(shè)定為Kl、 K2、 K3。根據(jù)式(10)中的觀察器增益K 的大小來決定推定的收斂速度。S卩，根據(jù)由式(10)求出的特性方程式的 極的大小來決定推定的收斂速度。
當(dāng)K= (Kl， K2, K3)時，如果Kl大，則接近于通過加速度傳感器 得到的推定值，如果K2大，則接近于通過陀螺傳感器得到的推定值。由 于在低頻中判斷加速度傳感器的推定值正確，因此增大K2，在中 高頻 中增大K2。在能夠預(yù)先推定出動作頻率的情況下，根據(jù)該頻率來設(shè)定K 即可。
具體地說，考慮以下(A) 、 (B)之間的平衡來決定K。
(A) 觀察器增益K大(極距離實軸遠)時
在該情況下，會大大地受到作為加速度傳感器輸入的觀測量的影響。 因此，對于推定值來說，使觀測量優(yōu)先，基本模型的影響變小。因此，容 易受到傳感器噪聲的影響。另外，在該情況下，推定的收斂速度變高。
(B) 觀察器增益K小(極距離實軸近)時
在該情況下，大大地使作為加速度傳感器輸入的觀測量平滑化。換言 之，減小觀測量的影響。因此，基本上推定值遵照基本模型。因此，難以 受到傳感器噪聲的影響。另外，在該情況下，推定的收斂速度變低。
對于觀察器增益K來說，原樣輸出根據(jù)陀螺傳感器推定出的傾斜角的 高頻成分，使低頻帶和直流成分成為根據(jù)加速度傳感器推定出的傾斜角， 由此能夠在從直流成分到高頻帶的寬的頻帶中高精度地推定出傾斜角。
在本發(fā)明中使用的外力觀察器用于推定傾斜角的直流成分，因此其響 應(yīng)可以慢一些。所謂響應(yīng)慢意味著能夠相應(yīng)地降低噪聲，因此能夠?qū)崿F(xiàn)精 度更高的推定。另外，由于使其成為響應(yīng)慢的觀察器，因此極設(shè)定得比較 靠近實軸。在本發(fā)明中，如上所述，與傾斜角對應(yīng)的運動方程式(模型信息)被 利用于外力觀察器。根據(jù)該模型信息和由單軸陀螺傳感器檢測出的角速度 在中頻帶 高頻帶中高精度地推定出傾斜角。并且，在本發(fā)明中，不但將 根據(jù)單軸陀螺傳感器推定出的傾斜角作為觀察器的參照值，還將根據(jù)二軸 加速度傳感器推定出的傾斜角作為觀察器的參照值，進行反饋控制以使推 定值追隨該參照值。由此，能夠在從直流成分到高頻帶的寬的頻帶中高精 度地推定出傾斜角。另外，對于加速度傳感器的噪聲來說，通過恰當(dāng)?shù)卦O(shè) 定觀察器增益K，能夠降低該噪聲的影響。
最后通過使設(shè)計出的連續(xù)系統(tǒng)外力觀察器離散化，能夠設(shè)計外力觀察
器o
另外，在本發(fā)明中，通過控制機構(gòu)來控制觀察器增益K。圖3表示了 該控制的流程圖。首先，控制機構(gòu)判斷由陀螺傳感器等檢測出的角速度是 否處于預(yù)先確定的預(yù)定范圍內(nèi)(S101)。這里，以角速度為基準(zhǔn)的原因在 于能夠檢測出移動體是否處于前后加速狀態(tài)。因此，在圖3所示的例子中 根據(jù)角速度檢測出移動體是否處于前后加速狀態(tài)，但是不限于此，也可以 根據(jù)其他參數(shù)來檢測是否處于該前后加速狀態(tài)?？刂茩C構(gòu)在判斷出角速度 處于預(yù)定范圍內(nèi)的情況下，不執(zhí)行改變觀察器增益K的值的控制。
接著，控制機構(gòu)在判斷出角速度不處于預(yù)定范圍內(nèi)、即處于預(yù)定范圍 外的情況下，還判斷角速度是否比預(yù)定范圍高(S102)。在判斷的結(jié)果是 控制機構(gòu)判斷出角速度比預(yù)定范圍高的情況下，執(zhí)行減小觀察器增益K的 值的控制(S103)。由此，由于處于前后加速狀態(tài)而處于加速度傳感器容 易產(chǎn)生干擾噪聲的狀態(tài)，因此能夠降低加速度傳感器的影響。
在判斷的結(jié)果是控制機構(gòu)判斷出角速度不比預(yù)定范圍高、即比預(yù)定范 圍低的情況下，執(zhí)行增大觀察器K的值的控制(S104)。另外，對于觀察 器增益K的值的大小的控制來說，也可以選擇減小觀察器增益K而進行了 推定處理的情況下和增大觀察器增益K而進行了推定處理的情況下的各自 的輸出值中的某個輸出值。
接下來，對傳感器的故障控制進行說明。本發(fā)明包括陀螺傳感器和加 速度傳感器，在某個傳感器發(fā)生了故障的情況下，按照如下方式來進行控
9制不執(zhí)行基于發(fā)生了故障的傳感器的傾斜角推定，而是僅根據(jù)沒有發(fā)生 故障的傳感器來執(zhí)行傾斜角推定。
具體地說，控制機構(gòu)判斷是否通過監(jiān)視器等檢測出了斷線或傳感器故 障等故障(S201)?？刂茩C構(gòu)在判斷為檢測出了故障的情況下，判斷是否
是陀螺傳感器的故障(S202)?？刂茩C構(gòu)在判斷出是陀螺傳感器的故障的 情況下，通過沒有發(fā)生故障的加速度傳感器來推定車身傾斜角(S203)。 另一方面，控制機構(gòu)在判斷出不是陀螺傳感器的故障的情況下，判斷是否 是加速度傳感器的故障(S204)。在判斷的結(jié)果是控制機構(gòu)判斷出是加速 度傳感器的故障的情況下，通過沒有發(fā)生故障的陀螺傳感器來推定車身傾 斜角(S205)?？刂茩C構(gòu)在判斷為不是加速度傳感器的故障的情況下停止 推定處理(S206)。
另外，在本發(fā)明中使用了移動體的模型信息，能夠提高傾斜角的推定 精度。例如，在由于車輛的構(gòu)造方面的限制而使得傾斜角不會大于等于預(yù) 定值的情況下，如果推定值超過了該預(yù)定值，則在將該推定值限制為該預(yù) 定值的基礎(chǔ)上來進行推定處理，由此真值與推定值的偏差受到了限制，從 而能夠防止推定精度變差。
另外，在上述例子中移動體10是倒立兩輪型的不穩(wěn)定車輛，但是不 限于此，也可以是步行型移動體或履帶驅(qū)動的移動體。另外，移動體10 也可以是搭乘型的移動體，還可以是組裝到機器人的下半身的移動體或搭 載貨物而移動的移動體。
產(chǎn)業(yè)上的可利用性
本發(fā)明涉及具有傾斜角推定機構(gòu)的移動體，特別是可以廣泛地利用于 具有傾斜角推定機構(gòu)的倒立兩輪型車輛。
權(quán)利要求
1. 一種移動體，包括加速度傳感器，檢測二軸方向上的加速度；陀螺傳感器，檢測車身的角速度；以及傾斜角推定機構(gòu)，推定車身的傾斜角；其中，該傾斜角推定機構(gòu)具有根據(jù)所述加速度傳感器檢測出的加速度來推定第一傾斜角的單元；按照所述移動體的線性模型方程并根據(jù)所述陀螺傳感器檢測出的角速度來推定第二傾斜角的單元；以及通過將所述第一傾斜角作為觀察器的參照值并進行反饋控制以使所述第二傾斜角追隨所述第一傾斜角來推定傾斜角的單元。
2. 如權(quán)利要求1所述的移動體，其中，所述傾斜角推定機構(gòu)具有將用于調(diào)整所述第一傾斜角的影響的觀察器 增益K設(shè)定為任意值的設(shè)定單元。
3. 如權(quán)利要求2所述的移動體，其中，所述設(shè)定單元在移動體處于前后加速狀態(tài)的情況下將所述觀察器增益 K設(shè)定為小的值。
4. 如權(quán)利要求2所述的移動體，其中，所述設(shè)定單元根據(jù)加速度傳感器和陀螺傳感器的故障狀態(tài)來設(shè)定該觀 察器增益K。
5. 如權(quán)利要求1至4中任一項所述的移動體，其中， 在所述加速度傳感器的后級設(shè)置有低通濾波器。
6. 如權(quán)利要求1至5中任一項所述的移動體，其中， 所述移動體是倒立兩輪型車輛。
全文摘要
本發(fā)明提供一種具有能夠高精度地推定出車身傾斜角度的傾斜角推定機構(gòu)的移動體。本發(fā)明的移動體包括加速度傳感器，檢測二軸方向上的加速度；陀螺傳感器，檢測車身的角速度；以及傾斜角推定機構(gòu)，推定車身的傾斜角。該傾斜角推定機構(gòu)具有根據(jù)加速度傳感器檢測出的加速度來推定與加速度傳感器相應(yīng)的傾斜角的單元；根據(jù)所述陀螺傳感器檢測出的角速度來推定第二傾斜角的單元；以及根據(jù)通過加速度傳感器得到的傾斜角和通過陀螺傳感器得到的傾斜角，將移動體的線性模型方程作為觀察器來推定傾斜角的單元。
文檔編號B62K3/00GK101443628SQ20078001755
公開日2009年5月27日 申請日期2007年9月27日 優(yōu)先權(quán)日2006年10月5日
發(fā)明者不破稔夫, 中島一誠 申請人:豐田自動車株式會社