2010年6月19日 星期六

Study Segway by NXT - Math model

http://4rdp.blogspot.com/2010/06/study-segway-by-nxt-math-model.html

研究過 Segway 平面軌跡,再來看看它的力學模型分析,這不是一件容易的事,我想了好幾天才想通,因為它還涉及轉動的問題,蠻複雜不容易思考,哈哈,還好現在不是學生身份,不然限時考試會不及格。

首先,複習牛頓力學三大運動定律:
一、無外力狀況下,動者恆動(等速運度),靜者恆靜。
二、F = m a,力 = 物體質量 x 加速度。
三、作用力與反作用力,力量相等,方向相反。

所有複雜的問題,將其切割成許多簡單的小問題,就會比較容易解決,最後再把它們湊起來,這樣也便利有興趣研究 Segway 的朋友了解。


m1 ─ 主體質量
m2 ─ 輪胎質量
L ─ 連桿長度
θ ─ 主體偏角
r ─ 輪胎半徑
g ─ 重力加速度

先單獨看輪胎的受力情形,質量 m2 的輪胎靜置在地上,重力 f2 = m2 * g,同樣地面也以相同大小但方向相反的反作用力支撐輪胎站立。

主體質量 m1,同樣因重力 f1 = m1 * g 向下,我們可以把這個力量分解成連桿上 f11與 f12兩相垂直分力。
分力 f11 會對連桿 L 產生力矩,如果輪胎 m2 被固定住不會位移的話,那麼 m1 就會順著連桿倒下來躺平。
f11 = f1 * sinθ = m1* g * sinθ












分力 f12 = f1 * cosθ 像是推著除草機,在輪胎 m2 可以再分解成 f13 與 f14 等兩相垂直分力。
因此,輪胎向下分力總合為

fd = f2 + f14
= m2 * g + f12 * cosθ
= m2 * g + m1 * g * cosθ * cosθ
= (m2 + m1 * cos2θ) * g
= ms * g

整個 Segway 的等效質量大小為

ms = m2 + m1* cos2θ




向下力量 fd 大小決定有多少摩擦力在這輪胎上

f3 = μV2 * fd = μV2 * ms * g

μV2 ─ 摩擦係數,不同 v2 速度係數會不同

f13 = f12 * sinθ = m1 * g * cosθ * sinθ

f13 會使輪胎向左移動,f4 為馬達對輪胎的施力,原本馬達出力轉矩與電流大小成正比,

但是在此簡化為電壓大小控制馬達施力 f4 = k * V

k ─ 馬達出力常數

V ─ 馬達供電百分比

因此輪胎 m2水平分力總合為 fh = f4 – f13– f3

現在讓 Segway 輪胎靜止,fh = 0,因此 f4 = f13+ f3

請注意,fh = 0 的平衡條件只是讓輪胎不動而已,但本體還是會傾倒。
如果想讓本體不倒下,必須 θ = 0 。

如何控制才能讓本體 θ = 0?我們可以增強 f4 讓輪胎前進速度比本體倒下速度快,
θ 就可以變小,接下來推倒相關公式 。

先看本體倒下速度 v1,知道力量大小及質量,透過牛頓第二運動定律 F = ma,可求得加速度 a,

知道加速度 a 利用直線運動方程式 v = v0 + a * t,就可以求得速度 v,
v0 為本體倒下的初速,t 為時間。

f11 = m1 * a1

v1 = v0 + a1 * t
= v0 + f11 / m1 * t
= v0 + f1* sinθ * t / m1
= v0 + (m1* g) * sinθ * t / m1
v1 = v0 + g * sinθ * t

再看輪胎前進使本體不倒的速度 v2,vs0 為 Segway 輪胎前進的初速。

fh = ms * a2

v2 = vs0 + a2 * t = vs0 + fh / ms * t
= vs0 + (f4 – f13 – f3) / ms * t
= vs0 + (k * V - m1 * g * cosθ * sinθμV2 * fd) / ms * t
= vs0 + (k * V - m1* g * cosθ * sinθμV2 * ms * g) / ms* t
= vs0 + [(k * V - m1 * g * cosθ * sinθ) / msμV2 * g] * t
v2 = vs0 + [(k * V - m1 * g * cosθ * sinθ) / (m2 + m1* cos2θ) – μV2 * g] * t

本體的運動速度

vb = v1 - v2

當 v1 = v2則維持 θ 的狀態,若 v1> v2則會倒下。

分析轉體運動,只要利用公式 弧長 = 角度(弳) x 半徑 (圓周長 = 2π x 半徑) 然後就可以推導出其它式子。

弧長 = 角度(弳) x 半徑

C = θ * R

本體速度 = 轉角速度(弳) x 連桿長度

vb = ω * L

知道本體運度速度 vb後,θ 角度變化就可以求出,ω 為本體轉角速度。

θ = θ0 + ω * t
= θ0 + vb / L * t
= θ0 + (v1 - v2) / L * t

總結

θ = θ0 + (v1 - v2) / L * t
v1= v0 + g * sinθ * t
v2 = vs0 + [(k * V - m1 * g * cosθ * sinθ) / (m2 + m1* cos2θ) – μV2 * g] * t

以上推導供大家參考,若有錯誤請指正。

13 則留言:

  1. dear Bridan:是否應考慮靜摩擦力與動摩擦力?車身在靜止與運動二者會有不同的結果。

    回覆刪除
  2. 薛老師您好,

    謝謝您的提醒,目前只考慮靜摩擦,我會再補充上去。

    回覆刪除
  3. Dear Bridan: 我參考別人用Java 寫的程式,改編成NXC版本。作品我放在http://cmackimo.blogspot.com/2010/07/990712-segway.html

    ps.目前的情形,會有
    1.車子往後傾斜,車輪不會動?
    2.車子在行駛時,在數秒後車輪會有搖擺的現象(影片中可以看到),之後碰到障礙物後,就直接倒下。

    這些都是目前待改進的!

    回覆刪除
  4. 另外,想請教Bridan:像是

    http://ycchenster.blogspot.com/2010/03/nxt-segway-type-line-tracer-with-nxtcam.html

    的Segway,本身又可以line following,我猜想大概程式需要作multi task?可是,要維持往前走,又要維持平衡,真的有點挑戰性!^o^

    回覆刪除
  5. 薛老師您好,

    很高興看到您的新作,並且也有進展,從你的程式控制注意到有三樣東西應該是關鍵部份,K_pwr、power及SetOutput,我會花點時間研究。

    以陳永欽博士的Segway,他應該是利用WebCAM、Gyro控制,程式不一定需要多工,只要搭配協調OK,就可以平衡控制,我想您也可以辦到。

    回覆刪除
  6. 關於我前面提到:「往後傾斜,車子卻沒有向後退」,昨天我曾設定單獨設定
    task main(){
    int power=100;
    SetOutput(OUT_BC, Power, power,OutputMode,OUT_MODE_MOTORON,RunState,OUT_RUNSTATE_RUNNING,UpdateFlags,UF_UPDATE_MODE+UF_UPDATE_SPEED);
    }
    輪子會往前、若power=-100,輪子會往後。
    且我也觀察segway程式,讓power值顯示在LCD,power也確實介於-100~100,可是車子對於後傾,就是無動於衷。真的很詭異。

    回覆刪除
  7. 薛老師您好,

    這可能需要再觀察Gyro讀值,也許後傾時感應角度不良。

    另外,Segway機身左右是對稱,但是前後並不對稱,因此前進或後退力道與機身前傾狀況有差別。

    回覆刪除
  8. 謝謝Bridan的寶貴意見。其實比較弔詭的,雖然我的segway前後的確不對稱,但也不至於往後傾、車輪都不動。不過我先試著寫個程式,用藍芽將nxt的值傳回pc來觀察看看。

    回覆刪除
  9. 薛老師您好,

    用藍芽回傳數值並記錄是一個好方法,請參考前一篇問題答覆
    http://4rdp.blogspot.com/2009/05/vb-communication-with-nxt-over.html

    回覆刪除
  10. 為了製作兩輪式平衡機器人(NXT Segway bot),僅僅靠陀螺儀偵測機身的傾斜角度變化率是不夠的。
    由於陀螺儀可以偵測bot在傾斜的過程之移動角速度變化,可以利用積分的方式來計算其角度,但因陀螺儀常存在累積誤差的因素,致使使用一段時間(也許只是幾秒鐘),就可能因為累積誤差,而使bot傾斜,而面臨傾倒的命運。
    很多文獻致力於消弭累積誤差的探討,但終究因為離散時間,其累積誤差能改善的程度仍極有限。甚至要運用高等數學等方式來建立濾波器(filter),來改善其誤差程度。
    所以我還是買了一顆角度感測器,解決這個問題,讓角度感測器專職監測bot的傾斜角度,而讓陀螺儀專職計算其傾斜變化率。
    一顆感測器價格不菲,但投入這項研究,未能解決始終是心中永遠的一個創痛啊!只好忍痛買了。

    回覆刪除
  11. 謝謝薛老師寶『貴』的經驗,

    確實用推算出來的物理量存在累積誤差,還是要直接測量才能除去這些累積誤差。

    回覆刪除
  12. 結果我那個hitechnic angle sensor 功能和我的需求不一樣。(可能我誤會了他的意思,他的angle sensor 有一個十字插孔,可能是與馬達連接,主要是測旋轉的角度,範圍0~359。插孔有轉動,才會有讀值,並不是感應器。)
    好可惜。買錯了~~所以現在又退回研究陀螺儀的改良設計。

    回覆刪除
  13. 薛老師您好,

    真是好可惜,錯誤的投資。不過看到你的留言,有個想法提供參考,NXT馬達可以迴授角度,或許可以利用它來修正陀螺儀偏差。

    回覆刪除