專利名稱：防翻倒控制裝置的制作方法
技術(shù)領域：
本發(fā)明涉及一種防翻倒控制裝置，平衡控制成例如兩輪車或兩足步行機器 人那樣左右自由傾斜的本體不翻倒。
背景技術(shù)：
在專利文獻l中，提出了一種兩輪車行駛玩具，該玩具具有轉(zhuǎn)向部；能 夠利用轉(zhuǎn)向部進行轉(zhuǎn)向的前輪；后輪；根據(jù)前輪(把手)的方向相應搖動的飛
輪；驅(qū)動飛輪的第l驅(qū)動部；以及驅(qū)動后輪的第2驅(qū)動部。在該兩輪車的情況 下，根據(jù)前輪的方向，相應改變飛輪的方向，通過這樣，利用飛輪產(chǎn)生的陀螺 效應，在行駛中不容易翻倒。
但是，在前述兩輪車行駛玩具的情況下，由于根據(jù)前輪的方向，不過只相 應改變飛輪的方向，因此存在的問題是，如果是通常行駛中，即使能夠通過把 手操作來防止翻倒，但在停止時或極慢速行駛時，很難僅通過把手操作來取得 平衡，不能有效地防止翻倒。
在專利文獻2中，提出了一種防止翻倒的倒立控制玩具，該玩具將利用傾 斜檢測傳感器檢測出的傾斜輸入控制電路，用控制電路驅(qū)動電動機，用電動機 使慣性大的旋轉(zhuǎn)體旋轉(zhuǎn)，沿著要校正傾斜的方向的反方向提高旋轉(zhuǎn)體的轉(zhuǎn)速， 產(chǎn)生反作用力偶，通過這樣來防止翻倒。在該倒立控制玩具的情況下，由于通 過控制旋轉(zhuǎn)體的旋轉(zhuǎn)，來取得平衡，因此即使是停止時或極慢速前進時，也能 夠防止翻倒。
在前述倒立控制玩具的情況下，作為對玩具的傾斜進行檢測的傾斜檢測傳 感器，是采用通過用受光元件來接受從發(fā)光元件送出后被地面反射的光、來檢 測傾斜的光傳感器。但是，實際上不容易正確測定傾斜。即，在使用發(fā)光元件 及受光元件的傾斜傳感器的情況下，若對光要進行反射的地面是平坦的，則沒 有問題，但在地面有凹凸、或兩側(cè)部不存在地面的情況下(渡過寬度窄的橋上的情況等)，就不能正確地檢測傾斜。
再有，在前述倒立控制玩具的情況下，將筆直站立的狀態(tài)的受光量作為標 準值，通過取得差分，來檢測傾斜，但是筆直站立的狀態(tài)(垂直方向)不一定 是能夠取得平衡的狀態(tài)。例如，在玩具的重心位置從中心位置向左右偏移時， 或受到側(cè)風時，相對于垂直方向稍微傾斜的狀態(tài)是能夠傾斜的狀態(tài)，盡管應該 將該能夠取得平衡的狀態(tài)(角度)作為標準位置，但由于在上述的方法中是將 垂直方向作為標準位置，因此不能保持平衡，有翻倒的可能性。
另外，作為檢測本體傾斜的方法，也可以用角速度傳感器檢測角速度，并 將該檢測值進行積分，從而推定傾斜。但是，在將角速度輸出進行積分的方法 中，發(fā)生的問題是，噪聲或偏置產(chǎn)生累積，不能持續(xù)進行傾斜角推定及防翻倒 控制。再有，作為其它的傾斜檢測裝置，還有使用重塊的傾斜傳感器，在這種 情況下具有的缺點是，也不能檢測相對于平衡狀體的傾斜，而且響應性差，不
能瞬時檢測傾斜。
專利文獻1:特開2003-190654號公報 專利文獻2:特開平11-47454號公報

發(fā)明內(nèi)容
因此，本發(fā)明的目的在于提供一種防翻倒控制裝置，該防翻倒控制裝置能 夠正確推定相對于能夠取得平衡的狀態(tài)的傾斜角，而且噪聲或偏置不累積，并 且能夠持續(xù)進行傾斜角推定及防翻倒控制。
為了達到上述目的，本發(fā)明是防翻倒控制裝置，具有左右自由傾斜的本 體；將檢測軸向著實質(zhì)上前后方向安裝在前述本體上的角速度傳感器；將旋轉(zhuǎn) 軸向著實質(zhì)上前后方向安裝在前述本體上的電動機；檢測前述電動機的旋轉(zhuǎn)位 置或旋轉(zhuǎn)速度的旋轉(zhuǎn)傳感器；以及與前述電動機的轉(zhuǎn)軸連接的慣性轉(zhuǎn)子，用前 述電動機使慣性轉(zhuǎn)子旋轉(zhuǎn)，利用隨著前述慣性轉(zhuǎn)子的旋轉(zhuǎn)而產(chǎn)生的反作用轉(zhuǎn) 矩，來校正前述本體的傾斜，其中，前述防翻倒控制裝置具有根據(jù)前述角速 度傳感器的角速度輸出w和給予前述電動機的轉(zhuǎn)矩指令t 。、來對前述本體相 對于平衡狀態(tài)的傾斜角進行推定的傾斜角推定單元，并且使用由前述傾斜角推 定單元推定的傾斜角推定值，來校正前述本體的傾斜。
5本發(fā)明有關(guān)的防翻倒控制裝置的動作原理雖與專利文獻2相同，是用電動
機使慣性轉(zhuǎn)子旋轉(zhuǎn)，利用隨著前述慣性轉(zhuǎn)子的旋轉(zhuǎn)而產(chǎn)生的反作用轉(zhuǎn)矩，來校 正前述本體的傾斜，但校正時必須高精度地檢測傾斜角。在本發(fā)明中，不是用
傳感器直接檢測傾斜角，另外也不是將角速度傳感器的角速度輸出進行積分來 求出。即，根據(jù)角速度傳感器的角速度輸出C^和給予電動機的轉(zhuǎn)矩指令T。，
來推定傾斜角。這里，所謂傾斜角，是相對于由重力產(chǎn)生的轉(zhuǎn)矩、因彎道行駛 而產(chǎn)生的離心力、以及由側(cè)風等而產(chǎn)生的干擾轉(zhuǎn)矩的總和成為零的平衡狀態(tài)下 的本體姿勢的偏差角度。使用傾斜角推定值，來控制慣性轉(zhuǎn)子的旋轉(zhuǎn)，反復進 行電動機轉(zhuǎn)矩控制，使得傾斜角收斂于0。例如，對于本體的平衡軸，當從本 體正面來看，傾斜角為左邊方向時，為了維持平衡姿勢，從本體正面來看，使 慣性轉(zhuǎn)子向左轉(zhuǎn)方向加速。反之，當從本體正面來看，傾斜角為右邊方向時， 為了維持平衡姿勢，從本體正面來看，使慣性轉(zhuǎn)子向右轉(zhuǎn)方向加速。
在本發(fā)明中，為了檢測本體的傾斜角，由于沒有使用傾斜角檢測傳感器， 因此即使在地面有凹凸時，或者像平衡木那樣在兩側(cè)部不存在地面時，也能夠 正確推定傾斜。另外，由于不需要將角速度傳感器的角速度輸出進行積分，因 此即使在角速度傳感器的輸出中含有噪聲或偏置時，也能夠持續(xù)進行傾斜角推 定，能夠持續(xù)進行防翻倒控制。再有，與以往的使用重塊的傾斜傳感器相比， 響應性遠比它好，能夠高精度地推定傾斜。這樣，在本發(fā)明中，由于能夠高精 度地推定本體相對于平衡軸的傾斜角，而且響應性好，因此能夠根據(jù)該傾斜角， 相應高精度地控制對電動機施加的轉(zhuǎn)矩。利用由電動機對慣性轉(zhuǎn)子所施加的轉(zhuǎn) 矩的反作用轉(zhuǎn)矩，能夠向防止本體翻倒的方向高精度地控制本體的傾斜角。其 結(jié)果，能夠制成即使是停止狀態(tài)或極慢速前進狀態(tài)也不倒的不倒體。
根據(jù)理想的實施形態(tài)，最好具有使用從目標傾斜角減去前述傾斜角推定 值的傾斜角偏差信號，生成傾斜角速度指令"2的傾斜角速度指令生成單元；以 及使用從前述傾斜角速度指令"2減去前述角速度傳感器的角速度輸出"！的傾 斜角速度偏差信號"2-""生成給予前述電動機的轉(zhuǎn)矩指令T。的轉(zhuǎn)矩指令生成 單元。在本發(fā)明中，首先設定目標傾斜角，從該目標傾斜角減去傾斜角推定值， 求出傾斜角偏差信號，根據(jù)該偏差信號，生成相對于本體的傾斜角速度指令" 2。接著，使用從傾斜角速度指令"2減去角速度傳感器的角速度輸出0^的傾斜角速度偏差信號"2-CO"能夠生成給予電動機的轉(zhuǎn)矩指令T。。根據(jù)理想的實施形態(tài)，最好具有根據(jù)傾斜角推定值，推定想要推倒本體 的外部轉(zhuǎn)矩的外部轉(zhuǎn)矩推定單元；以及使用外部轉(zhuǎn)矩推定值T 3向外部轉(zhuǎn)矩被抵 消的方向，來校正轉(zhuǎn)矩指令T。的轉(zhuǎn)矩校正單元。所謂外部轉(zhuǎn)矩，是因本體相對 于平衡軸產(chǎn)生傾斜而對本體施加的重力、以及因干擾而產(chǎn)生的傾斜方向的轉(zhuǎn) 矩。由于通過用前饋控制來補償外部轉(zhuǎn)矩，即使在傾斜角環(huán)及傾斜角速度環(huán)的 響應頻率低的情況下，也能夠持續(xù)進行防翻倒控制，因此能夠進行穩(wěn)定的控制。 根據(jù)理想的實施形態(tài)，最好具有使用電動機的旋轉(zhuǎn)速度、而且向著旋轉(zhuǎn) 速度減少的方向生成目標傾斜角的目標傾斜角生成單元。由于能夠利用重力轉(zhuǎn) 矩，放出慣性轉(zhuǎn)子具有的角動量，因此電動機的旋轉(zhuǎn)速度不會飽和，能夠持續(xù) 進行控制。能夠?qū)⒈景l(fā)明有關(guān)的防翻倒控制裝置用于自立行駛兩輪車。這種兩輪車具 有轉(zhuǎn)向部；能夠利用轉(zhuǎn)向部進行轉(zhuǎn)向的前輪；后輪；驅(qū)動后輪的后輪驅(qū)動部； 以及支持前輪及后輪能夠自由旋轉(zhuǎn)的車架，通過將本發(fā)明用于該兩輪車的防翻 倒控制，能夠提供行駛中當然不翻倒、即使在停止時或極慢速前進時也不翻倒 的兩輪車。另外，可以僅在停止時或極慢速前進時，使用本發(fā)明的防翻倒控制，在行駛中不使慣性轉(zhuǎn)子旋轉(zhuǎn)，而通過操作轉(zhuǎn)向部來取得平衡。如上所述，根據(jù)本發(fā)明，由于根據(jù)角速度傳感器的角速度輸出及電動機轉(zhuǎn) 矩指令，來推定相對于平衡狀態(tài)的傾斜角，因此與以往的使用傾斜角檢測傳感 器的情況不同，即使地面有凹凸，或者像平衡木那樣在周圍不存在地面，或者 地面有一些傾斜，也能夠正確推定相對于平衡狀態(tài)的傾斜角。另外，由于不需 要將角速度傳感器的角速度輸出進行積分，因此即使在角速度傳感器的輸出中 含有噪聲或偏置時，也能夠持續(xù)進行傾斜角推定，能夠持續(xù)進行防翻倒控制。 再有，與以往的使用重塊的傾斜傳感器相比，響應性遠比它好，能夠高精度地 推定傾斜。因而，能夠高精度地控制對電動機轉(zhuǎn)矩所施加的轉(zhuǎn)矩，能夠?qū)崿F(xiàn)即 使在停止狀態(tài)或極慢速前進狀態(tài)下如不翻倒的防翻倒控制裝置。


圖1為采用本發(fā)明有關(guān)的防翻倒控制裝置的自行車機器人的一個實施形態(tài)的立體圖。圖2為自行車機器人的側(cè)面圖。圖3為自行車機器人的控制方框圖。圖4為從自行車機器人的正面來看的模型圖。圖5為施加干擾時利用陀螺傳感器的本體角速度測定值。圖6為施加干擾時的電動機轉(zhuǎn)矩指令。 圖7為施加干擾時的本體傾斜角推定值。標號說明A自行車機器人(本體)1轉(zhuǎn)向用把手(轉(zhuǎn)向部) 2前輪3 后輪4 后輪驅(qū)動電動機(后輪驅(qū)動部)5 車架6 假人7 陀螺傳感器(角速度傳感器) 8慣性轉(zhuǎn)子9平衡用電動機10編碼器(旋轉(zhuǎn)傳感器)11控制基板12 電池20 計數(shù)器部21 電動機速度計算部 22目標傾斜角生成部 23 A/D部24傾斜角速度計算部 25傾斜角推定部 26校正轉(zhuǎn)矩指令生成部 27目標傾斜角速度生成部28 轉(zhuǎn)矩指令生成部
29 電動機轉(zhuǎn)矩指令電壓計算部
30 D/A部
具體實施例方式
以下，參照

本發(fā)明的理想實施形態(tài)。 (第1實施形態(tài))
圖1 圖3所示為將本發(fā)明有關(guān)的防翻倒控制裝置用于自行車機器人的第 1實施形態(tài)。
該自行車機器人A具有轉(zhuǎn)向用把手l;能夠利用轉(zhuǎn)向用把手l進行轉(zhuǎn)向 的前輪2;后輪3;驅(qū)動后輪3的后輪驅(qū)動電動機4;支持前輪2及后輪3能夠
自由旋轉(zhuǎn)的車架5;以及騎在車架5上的假人6。測定傾斜角速度的陀螺傳感 器(角速度傳感器)7將它的檢測軸向著自行車A的實質(zhì)上前后方向，安裝在 車架5上。在假人6的胸部安裝慣性轉(zhuǎn)子8、驅(qū)動慣性轉(zhuǎn)子8的平衡用電動機 9、以及測定平衡用電動機9的旋轉(zhuǎn)角度的編碼器10。慣性轉(zhuǎn)子8及電動機9 的轉(zhuǎn)軸也向著自行車A的實質(zhì)上前后方向安裝。這里，所謂實質(zhì)上前后方向， 也可以相對于嚴格的前后方向上下偏離若干角度。在假人6的后背，裝備有控 制平衡用電動機9的控制基板11及電池12。控制基板ll上，裝有驅(qū)動電動機 9的驅(qū)動器、A/D轉(zhuǎn)換器、D/A轉(zhuǎn)換器、計數(shù)器、控制器等。
通常行駛中，通過操作把手l來取得平衡，防止翻倒。另外，在停止狀態(tài) 或極慢速前進狀態(tài)下，由于僅操作把手l難以取得平衡，因此利用驅(qū)動慣性轉(zhuǎn) 子8時的反作用進行控制，以便取得平衡。
自行車機器人A利用圖3所示的控制方框進行控制。該控制方框是控制基 板ll上放置的方框的一個例子。首先，在計數(shù)器部20中，對編碼器10的輸 出脈沖進行計數(shù)。在電動機速度計算部21中，將計數(shù)器部20的輸出變換為旋 轉(zhuǎn)角度之后，進行微分，求出平衡用電動機9的旋轉(zhuǎn)速度。也可以裝有除去干 擾用的LPF (低通濾波器)。
目標傾斜角生成部22對平衡用電動機9的旋轉(zhuǎn)速度乘以比例系數(shù)，在平 衡用電動機9的旋轉(zhuǎn)速度從自行車正面來看是左轉(zhuǎn)時，使得求出目標傾斜角為從自行車正面來看是右邊方向，而在平衡用電動機9的旋轉(zhuǎn)速度從自行車正面 來看是右轉(zhuǎn)時，使得求出目標傾斜角為從自行車正面來看是左邊方向。另外， 也可以追加積分器，使得慣性轉(zhuǎn)子8沒有殘留定常旋轉(zhuǎn)。
另外，在A/D部23中，測定陀螺傳感器7的角速度輸出。在傾斜角速度 計算部24中，對角速度輸出乘以變換系數(shù)，計算傾斜角速度c^。
在傾斜角推定部25中，根據(jù)傾斜角速度c^及電動機轉(zhuǎn)矩指令i2，計算基
于包含自行車本體(慣性轉(zhuǎn)子以外的部分)及慣性轉(zhuǎn)子8的系統(tǒng)的傾斜角方向 的運動方程式導出的、用后述的(式1S)表示的傾斜角，再為了使其具有適當 的推定速度，使環(huán)路穩(wěn)定，串聯(lián)增加一階延遲要素，通過這樣計算出傾斜角推 定值。具體來說，對使用(式1S)的計算值，串聯(lián)增加作為一階延遲要素的例 如1/(0. 1S+1)，但不限于此，可以增加成為適當?shù)耐贫ㄋ俣饶菢拥娜我庋舆t 要素。這里，所謂傾斜角，是相對于由重力產(chǎn)生的轉(zhuǎn)矩、因彎道行駛而產(chǎn)生的 離心力、以及由側(cè)風等而產(chǎn)生的干擾轉(zhuǎn)矩的總和成為零的平衡狀態(tài)下的本體姿 勢的偏差角度。
在校正轉(zhuǎn)矩指令生成部26中，對傾斜角推定值乘以變換系數(shù)，計算出對 自行車作用的外部轉(zhuǎn)矩推定值，生成校正轉(zhuǎn)矩。外部轉(zhuǎn)矩推定值)t3。
在目標傾斜角速度生成部27中，對目標傾斜角與傾斜角推定值的偏差乘 以比例增益，生成目標傾斜角速度"2。
在轉(zhuǎn)矩指令生成部28中，對于目標傾斜角速度co 2與傾斜角速度co t的偏 差，生成例如利用PI控制的轉(zhuǎn)矩指令t。。在電動機轉(zhuǎn)矩指令電壓計算部29中，
對于將轉(zhuǎn)矩指令t 。與校正轉(zhuǎn)矩t 3相加的電動機轉(zhuǎn)矩指令t 2，乘以變換系數(shù)，
生成指令電壓。最后，在D/A部30中，對驅(qū)動器輸出指令電壓，控制平衡用 電動機9的旋轉(zhuǎn)。
這里，對于用(式18)表示的推定傾斜角的計算式的導出方法，在下面進 行說明。
圖4所示為從前方來看包含慣性轉(zhuǎn)子8的自行車機器人A的模型。首先， 利用拉格朗日方程式，導出運動方程式。將自行車本體(慣性轉(zhuǎn)子以外的部分) 與慣性轉(zhuǎn)子8合在一起的整個的動能T及位能U如下所示。 [數(shù)學式1]<formula>formula see original document page 11</formula>利用廣義坐標及廣義速度的微分量如下所示,
<formula>formula see original document page 11</formula>其結(jié)果，作為運動方程式，得到下面的(式ll)及(式12) [數(shù)學式4]<formula>formula see original document page 12</formula>(式1 1 〉
<formula>formula see original document page 12</formula>(式1 2 )
若將(式12)加以變形，則成為<formula>formula see original document page 12</formula>(式13)
<formula>formula see original document page 12</formula>
(式1 3 )
將它代入(式li)，若用e,近似sine"則得到下式,
E數(shù)學式6]
<formula>formula see original document page 12</formula>(式1 4 )
根據(jù)(式14)，本體的運動與慣性轉(zhuǎn)子8的角度及角速度無關(guān)。 -本體傾斜角的推定-
本體傾斜角雖然也可以利用陀螺傳感器7的輸出的積分求得，但是由于偏 差產(chǎn)生累積，從而不正確，所以必須用別的方法來求。因此，用模型的運動方 程式，根據(jù)陀螺傳感器7輸出的本體傾斜角速度測定值及電動機轉(zhuǎn)矩，來推定 現(xiàn)在的傾斜角。若將運動方程式(式14)加以變形，則成為下式。 [數(shù)學式7]
<formula>formula see original document page 12</formula>
《式1 5》
另外，若設陀螺傳感器7輸出的本體傾斜角速度測定值為"H則 [數(shù)學式8]
<formula>formula see original document page 12</formula>(式1 6 )
另外，在有干擾轉(zhuǎn)矩"時的視在平衡傾斜角度為<formula>formula see original document page 13</formula>
因而，現(xiàn)在傾斜角度相對于視在平衡傾斜角度的偏差可根據(jù)(式15)用下 式推定。
<formula>formula see original document page 13</formula>
式中，為了使其具有適當?shù)耐贫ㄋ俣?，使環(huán)路穩(wěn)定，也可以串聯(lián)增加一階延遲
-外部轉(zhuǎn)矩前饋-
利用由(式18)推定的偏差角度，補償外部轉(zhuǎn)矩。作為下式，與轉(zhuǎn)矩相加。 [數(shù)學式11]
<formula>formula see original document page 13</formula>
若設下式，即
<formula>formula see original document page 13</formula>則由于運動方程式(式14)成為 [數(shù)學式13]
<formula>formula see original document page 13</formula>
因此能夠補償外部轉(zhuǎn)矩。 -目標傾斜角度生成-
以將運動方程式第2式(式13)進行積分的形式，將慣性轉(zhuǎn)子8的旋轉(zhuǎn)速 度*進行累積。由于電動機的旋轉(zhuǎn)速度有限制，因此必須利用位置控制進行補 償，使得利用重力轉(zhuǎn)矩放出存儲的旋轉(zhuǎn)速度。因此，如下述那樣決定目標傾斜 角。
在利用重力轉(zhuǎn)矩放出旋轉(zhuǎn)速度期間，假定設傾斜角為一定，則由于[數(shù)學式14]
《=0 …".(式2 2 )
因此運動方程式(式14)及(式13)分別成為(式23)及(式24)。 [數(shù)學式15]
r2 = r,十(加，G +m2/)g《=+附")g躉 …'..(式2 3 ) 么—f2 = Wc+加2似 ……(式24》
在想要以時間乙放出存儲的旋轉(zhuǎn)速度*時，由于需要的角加速度為
A ……(式2 5 )
因此，將(式24)與(式25)進行比較，可求出下式。 [數(shù)學式17]
《=—廠6《 ."...(式2 6)
因而，作為位置環(huán)的目標值(目標傾斜角)，只要設定(式27)即可。 [數(shù)學式18]
"，、 ……(式2 7)
放出時間TA例如只要設定為T^lsec等即可。
由于原理上沒有殘留定常偏差，因此傾斜角推定部25為了生成目標傾斜 角度，不需要積分要素，但實際上，有時慣性轉(zhuǎn)子8殘留有低速的定常旋轉(zhuǎn)。 這可以認為D/A偏置是形成的原因。雖然維持原樣沒有問題，但若對目標傾斜 角度的生成部分追加時間常數(shù)為IO秒左右的積分器，則能夠消除低速的定常 旋轉(zhuǎn)。
圖5 圖7所示為對于基于上述原理的包含慣性轉(zhuǎn)子的自行車機器人的穩(wěn) 定性進行測定的結(jié)果。圖5 圖7所示為對于沒有施加干擾的狀態(tài)下的自行車機器人用手指從橫向推按本體而施加干擾時的響應，圖5是表示由陀螺傳感器測定的本體角速度，圖6是表示電動機轉(zhuǎn)矩指令(額定轉(zhuǎn)矩/3V)，圖7是表 示本體傾斜角推定值。另外，采樣時間為lms。由圖7可知，施加干擾之前的傾斜角推定值穩(wěn)定維持在士0.05deg以內(nèi)， 能夠維持穩(wěn)定的平衡狀態(tài)。再有可知，即使加上干擾時，也迅速收斂于穩(wěn)定位 置。根據(jù)該實驗結(jié)果可以確認，本實施形態(tài)有關(guān)的自行車機器人能夠停止而且 不倒，即使對于干擾(包含定常階躍狀干擾)也能夠應對。以下，列出本發(fā)明的效果。(1) 由于不對陀螺傳感器7的角速度輸出進行積分，基于模型來推定傾 斜角，因此即使在陀螺傳感器7的輸出中含有噪聲或偏置時，也能夠持續(xù)進行 傾斜角推定，能夠持續(xù)進行自行車的防翻倒控制。因而，能夠制成即使是停止 狀態(tài)或極慢速前進狀態(tài)也不倒的自行車。(2) 能夠根據(jù)陀螺傳感器7的輸出來推定傾斜角，利用由平衡用電動機 對慣性轉(zhuǎn)子8施加的轉(zhuǎn)矩的反作用，控制自行車的傾斜角。因而，能夠制成即 使是停止狀態(tài)或極慢速前進狀態(tài)也不倒的自行車。(3) 關(guān)于傾斜角推定，由于求出相對于平衡位置的傾斜角，因此除了重 力轉(zhuǎn)矩，再加上有彎道行駛中的離心力等干擾轉(zhuǎn)矩時，也始終能夠推定根據(jù)相 對于平衡位置的傾斜角度產(chǎn)生的外部轉(zhuǎn)矩，因此能夠計算出將其抵消的校正轉(zhuǎn) 矩，即使在有干擾轉(zhuǎn)矩時，也能夠保持本體平衡。(4) 由于通過用前饋控制來補償外部轉(zhuǎn)矩，即使在傾斜角環(huán)、傾斜角速 度環(huán)的響應頻率低時，也能夠持續(xù)進行防翻倒控制，因此能夠進行穩(wěn)定的控制。(5) 由于生成目標傾斜角度，以防止慣性轉(zhuǎn)子的旋轉(zhuǎn)速度的飽和，因此 能夠在電動機旋轉(zhuǎn)速度飽和之前使傾斜角變化，利用重力轉(zhuǎn)矩放出慣性轉(zhuǎn)子8 具有的角動量。因而，能夠制成即使是停止狀態(tài)或極慢速前進狀態(tài)也能夠持續(xù) 進行防翻倒控制的控制裝置。若更詳細敘述，則如下所述。在從自行車正面來看傾斜角是左邊方向時，為了維持其姿勢，從自行車正 面來看必須向左轉(zhuǎn)方向使慣性轉(zhuǎn)子8加速。在從自行車正面來看傾斜角是右邊 方向時，為了維持其姿勢，從自行車正面來看必須向右轉(zhuǎn)方向使慣性轉(zhuǎn)子8加速。利用該方法，在電動機旋轉(zhuǎn)速度增大時，能夠主動使姿勢傾斜，利用重力 轉(zhuǎn)矩放出慣性轉(zhuǎn)子8具有的角動量，降低電動機旋轉(zhuǎn)速度。通過將慣性轉(zhuǎn)子8 安裝在轉(zhuǎn)軸上，由于電動機旋轉(zhuǎn)速度達到飽和之前的時間有余量，因此能夠進 行這樣的控制。關(guān)于目標傾斜角生成，是對電動機旋轉(zhuǎn)速度乘以比例系數(shù)求得，在電動機 旋轉(zhuǎn)速度從自行車正面來看是左轉(zhuǎn)時，使得目標傾斜角為從自行車正面來看是 右邊方向，而在電動機旋轉(zhuǎn)速度從自行車正面來看是右轉(zhuǎn)時，使得目標傾斜角 為從自行車正面來看是左邊方向。另外，由于還追加積分器，因此沒有D/A轉(zhuǎn) 換器的偏置等原因而殘留定常旋轉(zhuǎn)的情況。在上述實施形態(tài)中，說明了自行車機器人的防翻倒控制，但本發(fā)明不僅限定于此，也可以適用于例如專利文獻2所示那樣的倒立玩具或兩足步行機器人等的防翻倒控制。g卩，在兩足步行機器人的情況下，通過推定相對于平衡軸的 傾斜角，能夠始終實現(xiàn)穩(wěn)定的步行。再有，本發(fā)明也能夠適用于摩托車等兩輪車暫時停止時的防翻倒控制。另外，是用(式18)表示推定傾斜角偏差的計算式，但這只不過是表示一個例子，利用對象模型的傾斜角偏差的推定式可能與(式18)不同。
權(quán)利要求
1.一種防翻倒控制裝置，具有左右自由傾斜的本體；安裝在所述本體上的角速度傳感器，所述角速度傳感器的檢測軸向著所述本體的實質(zhì)上前后方向；安裝在所述本體上的電動機，所述電動機的旋轉(zhuǎn)軸向著所述本體的實質(zhì)上前后方向；檢測所述電動機的旋轉(zhuǎn)位置或旋轉(zhuǎn)速度的旋轉(zhuǎn)傳感器；以及與所述電動機的轉(zhuǎn)軸連接的慣性轉(zhuǎn)子，用所述電動機使慣性轉(zhuǎn)子旋轉(zhuǎn)，利用隨著所述慣性轉(zhuǎn)子的旋轉(zhuǎn)而產(chǎn)生的反作用轉(zhuǎn)矩來校正所述本體的傾斜，其特征在于，所述防翻倒控制裝置具有根據(jù)所述角速度傳感器的角速度輸出ω1和給予所述電動機的轉(zhuǎn)矩指令τ0、來對所述本體相對于平衡狀態(tài)的傾斜角進行推定的傾斜角推定單元，并且使用由所述傾斜角推定單元推定的傾斜角推定值來校正所述本體的傾斜。
2. 如權(quán)利要求l所述的防翻倒控制裝置，其特征在于，還具有 使用從目標傾斜角減去所述傾斜角推定值的傾斜角偏差信號來生成傾斜角速度指令0 2的傾斜角速度指令生成單元；以及使用從所述傾斜角速度指令"2減去所述角速度傳感器的角速度輸出c^的 傾斜角速度偏差信號"2-^來生成給予所述電動機的轉(zhuǎn)矩指令"的轉(zhuǎn)矩指令生成單元。
3. 如權(quán)利要求2所述的防翻倒控制裝置，其特征在于，還具有根據(jù)所述傾斜角推定值來推定想要推倒所述本體的外部轉(zhuǎn)矩的外部轉(zhuǎn)矩推定單元；以及使用外部轉(zhuǎn)矩推定值t 3向外部轉(zhuǎn)矩被抵消的方向來校正所述轉(zhuǎn)矩指令t 。 的轉(zhuǎn)矩校正單元。
4. 如權(quán)利要求2或3所述的防翻倒控制裝置，其特征在于，還具有 使用所述電動機的旋轉(zhuǎn)速度、而且向著所述旋轉(zhuǎn)速度減少的方向來生成所述目標傾斜角的目標傾斜角生成單元。
5.如權(quán)利要求1至4的任一項所述的防翻倒控制裝置，其特征在于， 所述本體是兩輪車，該兩輪車具有轉(zhuǎn)向部；能夠利用所述轉(zhuǎn)向部進行轉(zhuǎn)向的前輪； 后輪；驅(qū)動所述后輪的后輪驅(qū)動部；以及 支持所述前輪及后輪能夠自由旋轉(zhuǎn)的車架。
全文摘要
本發(fā)明提供一種防翻倒控制裝置，該防翻倒控制裝置能夠正確推定相對于能夠取得平衡的狀態(tài)的傾斜角，而且噪聲或偏置不累積，并能夠持續(xù)進行傾斜角推定及防翻倒控制。防翻倒控制裝置具有左右自由傾斜的自行車機器人A；將檢測軸向著實質(zhì)上前后方向安裝在自行車機器人A上的角速度傳感器7；將旋轉(zhuǎn)軸向著實質(zhì)上前后方向安裝在本體上的電動機9；檢測電動機9的旋轉(zhuǎn)位置或旋轉(zhuǎn)速度的旋轉(zhuǎn)傳感器10；以及與電動機9的轉(zhuǎn)軸連接的慣性轉(zhuǎn)子8，用電動機9使慣性轉(zhuǎn)子8旋轉(zhuǎn)，利用隨著慣性轉(zhuǎn)子8的旋轉(zhuǎn)而產(chǎn)生的反作用轉(zhuǎn)矩，來校正自行車機器人A的傾斜。具有根據(jù)角速度傳感器7的角速度輸出ω₁和給予電動機9的轉(zhuǎn)矩指令τ₀、來對相對于平衡狀態(tài)的傾斜角進行推定的傾斜角推定單元，并使用傾斜角推定值，來校正自行車機器人A的傾斜。
文檔編號B62J99/00GK101296838SQ20068004034
公開日2008年10月29日 申請日期2006年10月30日 優(yōu)先權(quán)日2005年12月1日
發(fā)明者平田篤彥 申請人:株式會社村田制作所