牛顿迭代法求解工业机械人运动学逆解（改进DH模型），Using Newton's Method to compute industrial robots inverse solution

前言求解求解工业机械人运动学逆解有几何法，解析法，迭代法，最近恰好学了数值分析地牛顿迭代法，机器人学老师又恰好布置了一个求解逆解的题，便用上了。牛顿迭代法（Newton’s Method）一元函数根据泰勒公式，我们有f(x)=f(x0)+f′(x0)(x−x0)+O(x−x0)2f(x)=f(x0)+f'(x0)(x-x0)+O(x-x0)^2f(x)=f(x0)+f′(x0)(x−x0)+O(x

敝之

3735人浏览 · 2020-05-12 18:56:43

敝之 · 2020-05-12 18:56:43 发布

前言

求解求解工业机械人运动学逆解有几何法，解析法，迭代法，最近恰好学了数值分析地牛顿迭代法，机器人学老师又恰好布置了一个求解逆解的题，便用上了。

牛顿迭代法（Newton’s Method）

一元函数

根据泰勒公式，我们有 $f(x)=f(x0)+f'(x0)(x-x0)+O(x-x0)^2$

忽略二次项，得到 $f (x) = f (x 0) + f^{'} (x 0) (x - x 0)$
从而得到求解公式： $x = x 0 +$ $\frac{f(x)-f(x0)}{f'(x0)}$

从而得到迭代公式： $X_n=X_{n-1}+$ $\frac{f(X_n)-f(X_{n-1)}}{f'(X_{n-1})}$

eg：求解 $2=x^2+sin(x)$ 的一个解
有： $f(x)=x^2+sin(x)-2$
有： $f^{'} (x) = 2 x + c o s (x)$
得到迭代公式： $X_n=X_{n-1}-$ $\frac{x_{n-1}^2+sin(X_{n-1})-2}{2X_{n-1}+cos(X_{n-1})}$
取迭代初值为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$

所以对于一元函数的迭代法：
1.给出迭代公式： $X_n=X_{n-1}-$ $\frac{f(X_{n-1)}}{f'(X_{n-1})}$
2.给出初值 $X_0$
关于一元函数牛顿迭代法的收敛性等知识，参见：Web

多元向量函数

类似于一元函数，多元函数的求解的迭代公式也是：
$X_n=X_{n-1}-f(X_{n-1})*{f'(X_{n-1})}^{-1}$
只是这里面的因变量 $X$ 是一个m维的列向量
而对于多元向量函数的求导，可以得到一个jacobi矩阵 ${f'(X_{n-1})}$
之后求逆得到 ${f'(X_{n-1})}^{-1}$

例如：对于 $F (u,v,w) = (f_1, f_2, f_3)$
其导数为：
$\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}$

逆解问题

题目

如图所示为一个4R机械手，非零的DH表参数为 $α 1 = - 90 ° ， d 2 = 1 ， α 2 = 45 ° ， d 3 = 1 ， a 3 = 1$ 。机械手初始角度为 $θ 1 = 0 ， θ 2 = 0 ， θ 3 = 90 ° ， θ 4 = 0$ （1）每个关节的运动范围为±180°，对于末端位姿 $^0P_4ORG=(0,1,\sqrt2)^T$ ，求解 $θ 1 - θ 3$ 。

给出改进DH表

i	$\alpha_{i-1}$	$a_{i-1}$	$d_i$	$\theta_i$
1	0	0	0	$a$
2	$90^o$	0	1	$b$
3	$45^o$	0	1	$c$
4	0	1	0	$d$

给出4个转换矩阵

这里有一个在线的矩阵计算网站（可以带入未知数）：Web

$^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)$

$^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)$

$^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)$

$^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)$

最后我们得到
$\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)$ = $\left(\begin{matrix} 0 \\ 1\\ 1.41421356 \\ 1 \end{matrix}\right)$

从而给出向量函数：
$f\left(\begin{matrix} a \\ b\\ c \end{matrix}\right)$ = $\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)$

之后通过python程序进行迭代，初值 $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()