在機器人算法開發中,經常會遇到求解非線性方程。非線性方程的求解十分困難,這裏介紹兩種方法:
1. 二分法 2.牛頓迭代法
定義:
非線性方程,就是因變量與自變量之間的關係不是線性的關係,這類方程很多,
例如平方關係、對數關係、指數關係、三角函數關係等等.
求解此類方程往往很難得到精確解,經常需要求近似解問題.
二分法:
二分法又稱二分區間法,是求解非線性方程的近似根的一種常用的簡單方法.
二分法的基本思想是: 首先確定有根區間,將區間二等分, 通過判斷f(x)的符號,
逐步將有根區間縮小, 直至有根區間足夠地小, 便可求出滿足精度要求的近似根。
首先應確定方程在[a,b]區間確定存在至少一個實數根:
由高等數學知識知, 設f (x)爲區間[a,b]上的單值連續, 如果f (a)·f (b)<0 ,
則[a,b]中至少有一個實根。如果f (x)在[a,b]上還是單調地遞增或遞減,
則僅有一個實根.
二分法求根過程
設方程f(x)=0在區間[a,b]內有根,二分法就是逐步收縮有根區間,最後得出所求的根。具體過程如下
① 取有根區間[a,b]之中點, 將它分爲兩半x0=(a+b)/2,分點,這樣就可縮小有根區間
② 對壓縮了的有根區間[a1,b1]施行同樣的手法,即取中點x1=(a1+b1)/2,
將區間[a1,b1]再分爲兩半,然後再確定有根區間[a2,b2]],其長度是[a1,b1]的二分之一.
③ 如此反覆下去,若不出現f(xk)=0,即可得出一系列有根區間序列,每個區間都是前一個區間的一半,
因此[ak,bk]的長度:
b k − a k = b k − 1 − a k − 1 2 = . . . = b − a 2 k
b_k-a_k=\frac{b_{k-1}-a_{k-1}}{2}=...=\frac{b-a}{2^k}
b k − a k = 2 b k − 1 − a k − 1 = . . . = 2 k b − a
當k->∞時趨於零,區間最終收斂於一點x即爲所求的根.
只要二分足夠多次,便有:
∣ x ∗ − x k ∣ < ϵ ϵ 是 給 定 的 精 度 , 當 k ≥ l g ( b − a ) − l g ϵ l g 2 − 1 時 , 做 到 k + 1 次 二 分 , 計 算 得 到 的 x k 就 是 滿 足 滿 足 精 度 的 近 似 解 , 在 程 序 中 通 常 用 相 鄰 的 x k 與 x k − 1 的 差 的 絕 對 值 或 a k 與 b k 的 差 的 絕 對 值 是 否 小 於 ε 來 決 定 二 分 區 間 的 次 數 。
|x^*-x_k|<\epsilon \\
\epsilon 是給定的精度,當\\
k \ge\frac{lg(b-a)-lg\epsilon}{lg2}-1時,做到k+1次二分,\\
\\
計算得到的x_k就是滿足滿足精度的近似解,在程序中通常用相鄰的x_k與x_{k-1}的差的絕對值\\
或a_k與b_k的差的絕對值是否小於ε來決定二分區間的次數。
∣ x ∗ − x k ∣ < ϵ ϵ 是 給 定 的 精 度 , 當 k ≥ l g 2 l g ( b − a ) − l g ϵ − 1 時 , 做 到 k + 1 次 二 分 , 計 算 得 到 的 x k 就 是 滿 足 滿 足 精 度 的 近 似 解 , 在 程 序 中 通 常 用 相 鄰 的 x k 與 x k − 1 的 差 的 絕 對 值 或 a k 與 b k 的 差 的 絕 對 值 是 否 小 於 ε 來 決 定 二 分 區 間 的 次 數 。
流程圖:
牛頓迭代法
牛頓迭代法一種重要和常用的迭代法, 它的基本思想是將非線性函數f(x)逐步線性化, 從而將非線性方程f(x)=0近似地轉化爲線性方程求解。
牛頓迭代法的推導:
對於方程f(x)=0,設其近似根爲xk, 函數f(x)可在xk附近作泰勒展開
f ( x ) = f ( x k ) + f ′ ( x − x k ) + f ′ ′ ( x k ) ( x − x k ) 2 2 + . . . 忽 略 高 次 項 f ( x ) ≈ f ( x k ) + f ′ ( x k ) ( x − x k ) 設 x ∗ 爲 實 際 根 , 則 有 f ( x ∗ ) = 0 , 即 x ∗ = x k − f ( x k ) f ′ ( x k ) 這 就 是 牛 頓 迭 代 公 式 : x k + 1 = x k − f ( x k ) f ′ ( x k )
f(x)=f(x_k)+f^{'}(x-x_k)+\frac{f^{''}(x_k)(x-x_k)^2}{2}+...\\
忽略高次項\\
f(x)≈f(x_k)+f^{'}(x_k)(x-x_k)\\
設x^*爲實際根,則有f(x^*)=0,即\\
x^*=x_k-\frac{f(x_k)}{f^{'}(x_k)}\\
這就是牛頓迭代公式:\\
x_{k+1}=x_k-\frac{f(x_k)}{f^{'}(x_k)}\\
f ( x ) = f ( x k ) + f ′ ( x − x k ) + 2 f ′ ′ ( x k ) ( x − x k ) 2 + . . . 忽 略 高 次 項 f ( x ) ≈ f ( x k ) + f ′ ( x k ) ( x − x k ) 設 x ∗ 爲 實 際 根 , 則 有 f ( x ∗ ) = 0 , 即 x ∗ = x k − f ′ ( x k ) f ( x k ) 這 就 是 牛 頓 迭 代 公 式 : x k + 1 = x k − f ′ ( x k ) f ( x k )
流程圖:
算法實踐
根據S形曲線加減速算法
設置參數條件爲:
S = 5.3 V m = 5 V s = 0 V e = 30 A = 100 J = 600
S = 5.3\\
Vm = 5\\
V_s = 0\\
V_e = 30\\
A = 100\\
J = 600\\
S = 5 . 3 V m = 5 V s = 0 V e = 3 0 A = 1 0 0 J = 6 0 0
推導得知,按照V,J加速到Ve,位移S’>S,即需要降低Ve,解得合適的Ve使得S’=S.
S'計算公式如下:
{ t 1 = A J t 2 = V e − V s A − t 1 S ′ = ( V s + V e ) 2 ∗ ( 2 t 1 + t 2 )
\begin{cases}
t1=\frac{A}{J}\\
t_2=\frac{V_e-V_s}{A}-t_1\\
S'=\frac{(V_s+V_e)}{2}*(2t_1+t_2)\\
\end{cases}
⎩ ⎪ ⎨ ⎪ ⎧ t 1 = J A t 2 = A V e − V s − t 1 S ′ = 2 ( V s + V e ) ∗ ( 2 t 1 + t 2 )
使用二分法和牛頓法兩種方法迭代計算Ve.
二分法:
核心代碼:
//二分法
double a,b;
int n=0;
b = ve;
a = tmp;
while(1){
tmp = (a+b)/2;
n++;
S1 = CalSacc(vs,tmp,A,J);
if(fabs(S1-S)<1e-6){
break;
}
if(S1 > S){
b = tmp;
}
else{
a = tmp;
}
}
共計迭代了n=21次,近似解Ve=25.2738762
牛頓法:
核心代碼:
//牛頓法
double a,b,c;
int n=0;
tmp = (tmp+ve)/2;
while(1){
n++;
//ve 在 A*A/J+vs 和 ve之間
a = (vs+tmp)*(A/J+(tmp-vs)/A)-2*S;
b = A/J+(tmp-vs)/A+(vs+tmp)/A;
c = tmp-a/b;
if(fabs(c-tmp)<1e-6){
break;
}
tmp = c;
}
共計迭代了n=4次,近似解Ve=25.2738749
可知,牛頓迭代法的收斂速度遠快於二分法,有助於節約處理器的計算資源.