前言
求解求解工业机械人运动学逆解有几何法,解析法,迭代法,最近恰好学了数值分析地牛顿迭代法,机器人学老师又恰好布置了一个求解逆解的题,便用上了。
牛顿迭代法(Newton’s Method)
一元函数
根据泰勒公式,我们有 f ( x ) = f ( x 0 ) + f ′ ( x 0 ) ( x − x 0 ) + O ( x − x 0 ) 2 f(x)=f(x0)+f'(x0)(x-x0)+O(x-x0)^2 f(x)=f(x0)+f′(x0)(x−x0)+O(x−x0)2
忽略二次项,得到 f ( x ) = f ( x 0 ) + f ′ ( x 0 ) ( x − x 0 ) f(x)=f(x0)+f'(x0)(x-x0) f(x)=f(x0)+f′(x0)(x−x0)
从而得到求解公式: x = x 0 + x=x0+ x=x0+ f ( x ) − f ( x 0 ) f ′ ( x 0 ) \frac{f(x)-f(x0)}{f'(x0)} f′(x0)f(x)−f(x0)
从而得到迭代公式: X n = X n − 1 + X_n=X_{n-1}+ Xn=Xn−1+ f ( X n ) − f ( X n − 1 ) f ′ ( X n − 1 ) \frac{f(X_n)-f(X_{n-1)}}{f'(X_{n-1})} f′(Xn−1)f(Xn)−f(Xn−1)
eg:求解 2 = x 2 + s i n ( x ) 2=x^2+sin(x) 2=x2+sin(x)的一个解
有: f ( x ) = x 2 + s i n ( x ) − 2 f(x)=x^2+sin(x)-2 f(x)=x2+sin(x)−2
有: f ′ ( x ) = 2 x + c o s ( x ) f'(x)=2x+cos(x) f′(x)=2x+cos(x)
得到迭代公式: X n = X n − 1 − X_n=X_{n-1}- Xn=Xn−1− x n − 1 2 + s i n ( X n − 1 ) − 2 2 X n − 1 + c o s ( X n − 1 ) \frac{x_{n-1}^2+sin(X_{n-1})-2}{2X_{n-1}+cos(X_{n-1})} 2Xn−1+cos(Xn−1)xn−12+sin(Xn−1)−2
取迭代初值为5,则有迭代值表:
Times | Error | value |
---|---|---|
1 | 6.441741912821939 | 2.8566900265847246 |
2 | 1.2523689832054252 | 1.5015868288822545 |
3 | 0.08519497331557391 | 1.0939581456934517 |
4 | 0.000578368794295514 | 1.0617713087284162 |
5 | 2.7647929723428888e-08 | 1.0615497852219482 |
6 | 0.0 | 1.0615497746313838 |
可见迭代到第6次的时候,误差已经小于计算机的最大精度,则可以得到解 x = 1.0615497746313838 x= 1.0615497746313838 x=1.0615497746313838
所以对于一元函数的迭代法:
1.给出迭代公式: X n = X n − 1 − X_n=X_{n-1}- Xn=Xn−1− f ( X n − 1 ) f ′ ( X n − 1 ) \frac{f(X_{n-1)}}{f'(X_{n-1})} f′(Xn−1)f(Xn−1)
2.给出初值 X 0 X_0 X0
关于一元函数牛顿迭代法的收敛性等知识,参见:Web
多元向量函数
类似于一元函数,多元函数的求解的迭代公式也是:
X n = X n − 1 − f ( X n − 1 ) ∗ f ′ ( X n − 1 ) − 1 X_n=X_{n-1}-f(X_{n-1})*{f'(X_{n-1})}^{-1} Xn=Xn−1−f(Xn−1)∗f′(Xn−1)−1
只是这里面的因变量 X X X是一个m维的列向量
而对于多元向量函数的求导,可以得到一个jacobi矩阵 f ′ ( X n − 1 ) {f'(X_{n-1})} f′(Xn−1)
之后求逆得到 f ′ ( X n − 1 ) − 1 {f'(X_{n-1})}^{-1} f′(Xn−1)−1
例如:对于 F ( u , v , w ) = ( f 1 , f 2 , f 3 ) F (u,v,w) = (f_1, f_2, f_3) F(u,v,w)=(f1,f2,f3)
其导数为:
{ ∂ f 1 / ∂ u ∂ f 1 / ∂ v ∂ f 1 / ∂ w ∂ f 2 / ∂ u ∂ f 2 / ∂ v ∂ f 2 / ∂ w ∂ f 3 / ∂ u ∂ f 3 / ∂ v ∂ f 3 / ∂ w } \begin{Bmatrix}∂f_1/∂u & ∂f_1/∂v & ∂f_1/∂w\\\ ∂f_2/∂u & ∂f_2/∂v & ∂f_2/∂w \\ ∂f_3/∂u & ∂f_3/∂v & ∂f_3/∂w\end{Bmatrix} ⎩⎨⎧∂f1/∂u ∂f2/∂u∂f3/∂u∂f1/∂v∂f2/∂v∂f3/∂v∂f1/∂w∂f2/∂w∂f3/∂w⎭⎬⎫
逆解问题
题目
如图所示为一个4R机械手,非零的DH表参数为 α 1 = − 90 ° , d 2 = 1 , α 2 = 45 ° , d 3 = 1 , a 3 = 1 α1=-90°,d2=1,α2=45°,d3=1,a3=1 α1=−90°,d2=1,α2=45°,d3=1,a3=1。机械手初始角度为 θ 1 = 0 , θ 2 = 0 , θ 3 = 90 ° , θ 4 = 0 θ1=0,θ2=0,θ3=90°,θ4=0 θ1=0,θ2=0,θ3=90°,θ4=0(1)每个关节的运动范围为±180°,对于末端位姿 0 P 4 O R G = ( 0 , 1 , 2 ) T ^0P_4ORG=(0,1,\sqrt2)^T 0P4ORG=(0,1,2)T,求解 θ 1 − θ 3 θ1-θ3 θ1−θ3。
给出改进DH表
i | α i − 1 \alpha_{i-1} αi−1 | a i − 1 a_{i-1} ai−1 | d i d_i di | θ i \theta_i θi |
---|---|---|---|---|
1 | 0 | 0 | 0 | a a a |
2 | − 9 0 o -90^o −90o | 0 | 1 | b b b |
3 | 4 5 o 45^o 45o | 0 | 1 | c c c |
4 | 0 | 1 | 0 | d d d |
给出4个转换矩阵
这里有一个在线的矩阵计算网站(可以带入未知数):Web
1 0 T = ( cos ( A ) − sin ( a ) 0 0 sin ( a ) cos ( A ) 0 0 0 0 1 0 0 0 0 1 ) ^0_1T=\left(\begin{matrix} \cos\left(A\right) & -\sin\left(a\right) & 0 & 0 \\ \sin\left(a\right) & \cos\left(A\right) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}\right) 10T=⎝⎜⎜⎛cos(A)sin(a)00−sin(a)cos(A)0000100001⎠⎟⎟⎞
2 1 T = ( cos ( b ) − sin ( b ) 0 0 0 0 1 1 − sin ( b ) − cos ( b ) 0 0 0 0 0 1 ) ^1_2T=\left(\begin{matrix} \cos\left(b\right) & -\sin\left(b\right) & 0 & 0 \\ 0 & 0 & 1 & 1 \\ -\sin\left(b\right) & -\cos\left(b\right) & 0 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}\right) 21T=⎝⎜⎜⎛cos(b)0−sin(b)0−sin(b)0−cos(b)001000101⎠⎟⎟⎞
3 2 T = ( cos ( c ) − sin ( c ) 0 0 2 ∗ sin ( c ) 2 2 ∗ cos ( c ) 2 − 2 2 − 2 2 2 ∗ sin ( c ) 2 2 ∗ cos ( c ) 2 2 2 2 2 0 0 0 1 ) ^2_3T=\left(\begin{matrix} \cos\left(c\right) & -\sin\left(c\right) & 0 & 0 \\ \frac{\sqrt{2}*\sin\left(c\right)}{2} & \frac{\sqrt{2}*\cos\left(c\right)}{2} & \frac{-\sqrt{2}}{2} & \frac{-\sqrt{2}}{2} \\ \frac{\sqrt{2}*\sin\left(c\right)}{2} & \frac{\sqrt{2}*\cos\left(c\right)}{2} & \frac{\sqrt{2}}{2} & \frac{\sqrt{2}}{2} \\ 0 & 0 & 0 & 1 \end{matrix}\right) 32T=⎝⎜⎜⎜⎛cos(c)22∗sin(c)22∗sin(c)0−sin(c)22∗cos(c)22∗cos(c)002−222002−2221⎠⎟⎟⎟⎞
4 3 T = ( cos ( d ) − sin ( d ) 0 1 sin ( d ) cos ( d ) 0 0 0 0 1 0 0 0 0 1 ) ^3_4T=\left(\begin{matrix} \cos\left(d\right) & -\sin\left(d\right) & 0 & 1 \\ \sin\left(d\right) & \cos\left(d\right) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{matrix}\right) 43T=⎝⎜⎜⎛cos(d)sin(d)00−sin(d)cos(d)0000101001⎠⎟⎟⎞
最后我们得到
( C a C b C c − 2 2 S a − S a + 2 2 C a S b − 2 2 S a S c − 2 2 C 1 S 2 S 3 2 2 C a + C a + C b C c S a + 2 2 S a S b + 2 2 C a S c − 2 2 S a S b S c 2 2 C b − C b S b − 2 2 C b S c 1 ) \left(\begin{matrix} C_aC_bC_c-\frac{\sqrt{2}}2S_a-S_a+\frac{\sqrt{2}}2C_aS_b-\frac{\sqrt{2}}2S_aS_c-\frac{\sqrt{2}}2C_1S_2S_3 \\ \frac{\sqrt{2}}2C_a+C_a+C_bC_cS_a+\frac{\sqrt{2}}2S_aS_b+\frac{\sqrt{2}}2C_aS_c-\frac{\sqrt{2}}2S_aS_bS_c\\ \frac{\sqrt{2}}2C_b-C_bS_b-\frac{\sqrt{2}}2C_bS_c\\ 1 \end{matrix}\right) ⎝⎜⎜⎜⎛CaCbCc−22Sa−Sa+22CaSb−22SaSc−22C1S2S322Ca+Ca+CbCcSa+22SaSb+22CaSc−22SaSbSc22Cb−CbSb−22CbSc1⎠⎟⎟⎟⎞= ( 0 1 1.41421356 1 ) \left(\begin{matrix} 0 \\ 1\\ 1.41421356 \\ 1 \end{matrix}\right) ⎝⎜⎜⎛011.414213561⎠⎟⎟⎞
从而给出向量函数:
f ( a b c ) f\left(\begin{matrix} a \\ b\\ c \end{matrix}\right) f⎝⎛abc⎠⎞= ( C a C b C c − 2 2 S a − S a + 2 2 C a S b − 2 2 S a S c − 2 2 C 1 S 2 S 3 2 2 C a + C a + C b C c S a + 2 2 S a S b + 2 2 C a S c − 2 2 S a S b S c − 1 2 2 C b − C b S b − 2 2 C b S c − 1.41421356 ) \left(\begin{matrix} C_aC_bC_c-\frac{\sqrt{2}}2S_a-S_a+\frac{\sqrt{2}}2C_aS_b-\frac{\sqrt{2}}2S_aS_c-\frac{\sqrt{2}}2C_1S_2S_3 \\ \frac{\sqrt{2}}2C_a+C_a+C_bC_cS_a+\frac{\sqrt{2}}2S_aS_b+\frac{\sqrt{2}}2C_aS_c-\frac{\sqrt{2}}2S_aS_bS_c-1\\ \frac{\sqrt{2}}2C_b-C_bS_b-\frac{\sqrt{2}}2C_bS_c-1.41421356 \end{matrix}\right) ⎝⎜⎛CaCbCc−22Sa−Sa+22CaSb−22SaSc−22C1S2S322Ca+Ca+CbCcSa+22SaSb+22CaSc−22SaSbSc−122Cb−CbSb−22CbSc−1.41421356⎠⎟⎞
之后通过python程序进行迭代,初值
a
=
1
,
b
=
1
,
c
=
1
a=1,b=1,c=1
a=1,b=1,c=1
经过若干次迭代:
set | a | b | c |
---|---|---|---|
value | 6.281205209748613 | 6.279797563437053 | 4.715199903694137 |
error | 3.450120554700231e-08 | 4.7538592589102535e-06 | -1.3814274435475227e-06 |
true value | 0 | 0 | -pi/2 |
这里必须提到一点:
我在Linux与Windows上执行程序的效果是不同的,Windows上有一些解迭代不到,而Linux上都可以得到,这是python在Windows与Linux上小数的精度不同导致的。所以在程序中要尽可能保证数值的精确度。
程序如下:
import numpy as np
import math
from sympy import *
def main():
pi=3.1415926535
a=1
b=1
c=1
F=1/1.41421356
x,y,z= symbols("x y z")
rf1=cos(x)*cos(y)*cos(z)-F*sin(x)-sin(x)+F*cos(x)*sin(y)-F*sin(x)*sin(z)-F*cos(x)*sin(y)*sin(z)
rf2=F*cos(x)+cos(x)+cos(y)*cos(z)*sin(x)+F*sin(x)*sin(y)+F*cos(x)*sin(z)-F*sin(x)*sin(y)*sin(z)-1
rf3=F*cos(y)-cos(z)*sin(y)-F*cos(y)*sin(z)-1.41421356
f1= diff(rf1,x)#get derivtive
f2= diff(rf1,y)
f3= diff(rf1,z)
f4= diff(rf2,x)
f5= diff(rf2,y)
f6= diff(rf2,z)
f7= diff(rf3,x)
f8= diff(rf3,y)
f9= diff(rf3,z)
rf1 = lambdify([x,y,z],rf1)#return to real function
rf2 = lambdify([x,y,z],rf2)
rf3 = lambdify([x,y,z],rf3)
f1 = lambdify([x,y,z],f1)
f2 = lambdify([x,y,z],f2)
f3 = lambdify([x,y,z],f3)
f4 = lambdify([x,y,z],f4)
f5 = lambdify([x,y,z],f5)
f6 = lambdify([x,y,z],f6)
f7 = lambdify([x,y,z],f7)
f8 = lambdify([x,y,z],f8)
f9 = lambdify([x,y,z],f9)
while(1):
x0=f1(a,b,c)#give value to each elements
x1=f2(a,b,c)
x2=f3(a,b,c)
y0=f4(a,b,c)
y1=f5(a,b,c)
y2=f6(a,b,c)
z0=f7(a,b,c)
z1=f8(a,b,c)
z2=f9(a,b,c)
x=rf1(a,b,c)
y=rf2(a,b,c)
z=rf3(a,b,c)
org=np.array(
[
[a],
[b],
[c],
]
)
first=np.array(
[
[x],
[y],
[z],
]
)
m=np.array([
[x0,x1,x2],
[y0,y1,y2],
[z0,z1,z2],
])
m=np.linalg.inv(m)
ans=np.dot(m,first)
org=org-ans
a=org[0][0]
b=org[1][0]
c=org[2][0]
while a<0:#constraint the value
a=a+2*pi
while a>2*pi:
a=a-2*pi
while b<0:
b=b+2*pi
while b>2*pi:
b=b-2*pi
while c<0:
c=c+2*pi
while c>2*pi:
c=c-2*pi
ans1=rf1(a,b,c)
ans2=rf2(a,b,c)
ans3=rf3(a,b,c)
print(a,b,c)#value
print(ans1,ans2,ans3)#error
i=input()#
if __name__=="__main__":
main()
水平有限,欢迎指正批评