昨天提到線性內差法
原本的math運算式子
x_c = x_c + ( (左輪速+右輪速)/2 )*sin(車頭角度)*0.001;
y_c = y_c + ( (左輪速+右輪速)/2 )*cos(車頭角度)*0.001;
這樣的運算時間約為226uS(只有這二行的運算而已，還不包含其他的相關運算)。
利用線性內差查表法來做運算，建立120點，點和點中間可產生30點線性值，所以解析度約為0.1度
x_c = x_c + ( (左輪速+右輪速)/2 )*cos_ans/16384000;
// /16384*0.001 =/16384000。 16384是cos_ans的解析度被放大16384倍，*0.001是Ts為1mS
y_c = y_c + ( (左輪速+右輪速)/2 )*sin_ans/16384000;
// /16384*0.001 =/16384000。 16384是cos_ans的解析度被放大16384倍，*0.001是Ts為1mS
//cos_ans和sin_ans 為線性內差查表後所得的答案。
這樣的運算時間約為50uS
結論：利用線性內差可以不失原本sin和cos的特性，且可以用較短的時間做運算。
在角度很小時可以應用近似公式sin(theta)=theta，theta為經度。
回覆刪除這樣就完全不用查表或計算。
我想因為每次都可以取得角度差，而且角度都小於1度。
再套上三角函數合角公式，sin及cos皆可簡化到使用二個乘法及一個加法。
的確在角度很小時可以應用近似公式sin(theta)=theta，theta為弳度。只是電腦鼠的車頭角度是可能落在[0, 2pi]之間的。所以角度都小於1度的假設似乎不成立！
回覆刪除至於三角函數合角公式如果指的是sin(a+da) = sin(a)cos(da) + cos(a)sin(da)的話，雖然da會很小，但a可能蠻大的!
因為上次的cos(a)及sin(a)已知，可以經由記憶體存下來。而車頭轉動時，隨時間會產生一個角度差，此角度差就是da。sin(da)=da;cos(da)=1-da。合角公式右側就全為已知項，故可以得到新的sin(a+da)及cos(a+da)。新的sin(a+da)則更新為下一次要用的sin(a)。
回覆刪除至於第一次的sin(a)，就真的去跑數學函式庫的疊代公式。以後只需處理sin(da)就行了。
不過，有累積誤差這個問題要處理。可能要隔一段時間算sin(a)做一次校正。
因為我不清楚陀螺儀回傳值為相對角度還是絶對角度。若是用絶對角度則我的方法便無法使用。那我再提供另一方法。
回覆刪除使用查表法及泰勒展開式逼近法：
首先利用sin函數的對稱性將要查的角度縮減到0~pi/4。
至於pi/4~pi/2可以對應為1-sin(theta-pi/4)。
故僅需要建造0~pi/4的表格。
此表格可以讓sin及cos共用。
查表只是得到近似角度的值。接下來便利用泰勒展開式來逼近。
可以找GNU中Newlib中數學函式庫的原始碼，可以取得泰勒展開式中至X^13次方的係數。
因為已使用查表近似過一次，所以只要使用X^1及X^3的係數就可以得到很好的結果。
此方法因為不使用除法，故對小型微處理器來說，運算速度較快。
若是熟悉定點數算法，在可控制的精度下也會有很好的效能。
[版主回覆11/23/2008 18:32:45]陀螺儀的輸出為角速度輸出，經積分後可以得到旋轉角度。它是一個相對的旋轉角度值。
應該是說使用泰勒展開式及牛頓逼近法。
回覆刪除正確的公式我尚未完整推導過。
利用疊代的方式ㄧ直記憶著sin(a)與cos(a)的方法似乎可行，但精確度與累積誤差應該要再做仔細的討論。
回覆刪除sin(60)並不等於１- sin(60-45)。
in (pi/2, pi] => sin(a) = sin(pi - a);
正弦和餘弦函數的對稱性應該是以90度為範圍的。
如果使用內插計算後還要再用泰勒展開式或牛頓求根法來改善精確度的話，就有些浪費時間，不符合原來的想法了！
關於1-sin(theta-pi/4)這個公式是我弄錯了。pi/4在程式中的用法我解釋錯誤所造成。
回覆刪除回頭看NewLib中的註解是這樣說的。sin,cos的近似公式中取的項次少時在-pi/4~pi/4時較準。
sin的近似公式如下：
sin(x+y)=sin(x)+(1-0.5*x*x)*y
cos(x+y)=cos(x)+x*y
找出最近的x及用查表找出sin(x)，剩餘的角度就是y，然後套用上面的牛頓逼近法算一次就結束。
我在看定點數DSP的反組譯組合語言看到它是這樣做。
查表是將pi/2分成128等份。就可以確定每等份小於1度。分成128等份也是有原因，可以使用位元移位代替除法，快速取得查表索引。
至於sin逼近公式中出現的0.5也是使用位元移位來完成。
因為使用定點數DSP，使用乘法及移位除法會比除法快許多。
最後應是呼叫定點數轉成浮點數回傳到C函式。
又因為逼近公式在-pi/4至pi/4時較準，所以在算角度大於pi/4時的sin其實是呼叫對應角度的cos函式。所以pi/4是這樣來的，並不是我上篇說的那樣。
公式的正確性我有用matlab驗了一下，發現是沒有內差準。但精準度也達小數點下第四位。
看錯了，內差和牛頓法的結果相近到小數以下四位，但和實際有差異，只有精準到小數以下二位。
回覆刪除寫到有關於移位除法，才發現原來的算式中並沒有在使用。
回覆刪除像這種Constant的除法運算，可以化為乘法運算再加上移位除法，這樣會比除法來得快。尤其是在有乘法器的MCU上效果應是明顯。
像是乘以0.001可以化為*66後再右移16位元。當然為了怕溢位運算中要先轉成32位元整數，再進行整數乘法，右移完後再用16位元存起來。
另一個說法是0.001=66/65536。這樣不知是否比較容易理解。
[版主回覆11/23/2008 16:25:41]已做測試，謝謝您的提醒。
http://tw.myblog.yahoo.com/sn903209ss/article?mid=461&prev=200&next=456
忘了說用什麼工具做10進位及16進位運算。之前我是用小算盤的工程型式。
回覆刪除不過後來發現更好的工具，PowerToy Calc，是微軟出的計算機。對工程來說很有用。
[版主回覆11/23/2008 18:31:13]謝謝您提供這不錯的工具，還有圖形化界面，其實還滿贊的。
發現公式打錯了，牛頓近似公式
回覆刪除sin(x+y)=sin(x)+sin'(x)*y=sin(x)+cos(x)*y=sin(x)+(1-0.5*x*x)*y
cos(x+y)=cos(x)+cos'(x)*y=cos(x)-sin(x)*y=cos(x)-x*y。這個公式要修正。
今天早上，我們好幾個人ㄧ起研究Bee提供的這個公式
回覆刪除sin(x+y)=sin(x)+sin'(x)*y=sin(x)+cos(x)*y=sin(x)+(1-0.5*x*x)*y
其中cos(x) = 1 - 0.5*x^2應該是利用泰勒展開式針對x = 0展開後取前兩項得到的結果。
我們發現這個公式當x > 1時其實是不好的，因為這樣的泰勒展開式不會收歛！
假設x = 80/180*pi, y = 0.3/180*pi
sin(80.3/180*pi) = 0.985703469088854
sin(80/180*pi) + (1-0.5*(80/180*pi)^2)*0.3/180*pi = 0.984939826911352
的確是準到小數下兩位
但若使用以1度為分割的表，利用內插法查出來的值則是
sin(80/180*pi) + (sin(81/180*pi)-sin(80/180*pi))*0.3 = 0.985671929287087
可以準到小數下三位!
至於移位或整數運算的作法，下次討論！
但是當x值小於1時，因為泰勒展開式的特性是x次方的關係，反而比較準!
回覆刪除假設x = 25/180*pi, y = 0.3/180*pi
sin(25.3/180*pi) = 0.427357863387192
sin(25/180*pi) + (1-0.5*(25/180*pi)^2)*0.3/180*pi = 0.427355820409100
的確是準到小數下五位
但若使用以1度為分割的表，利用內插法查出來的值則是
sin(25/180*pi) + (sin(26/180*pi)-sin(25/180*pi))*0.3 = 0.427344127255213
可以準到小數下四位!