python机器人编程——用pytorch实现六轴机械臂的正向和逆向数值解算，及python算法解析

一、前言

前面对六轴（或多轴）机械臂进行了一些研究，特别是利用几何进行简化，实现逆解，来完成比较简单的搬用任务。然而，如果需要比较精确的操作任务，可能需要更加复杂的机械臂解算。其中比较复杂的是数值解算，这种解算往往涉及到比较复杂的机器人运动等原理知识，比较难以上手。但是，python的生态挺全面，已经有很多牛人实现了数值解法，但是去看他们的代码，还是觉得费劲。此外，随着深度神经网络的兴起，很多是用建立深度学习神经网络，去利用机械臂输入输出的大数据，训练机械臂的控制模型，这个上手也比较慢。现在，我们提供一个相对简单的方法，来实现N轴机械臂的数值逆解，我们使用了一个优秀的人工智能库pytorch，只要在建立正向模型的基础上，几行代码就可以实现机械臂的数值逆解，可以供初学者学习之用。

二、实现原理

2.1正向建模

数值解算本质就是解非线性方程组。前提是我们需要把这个方程组給先建立起来。这里需要用到一些DH建模的知识。此部分已经在博文《N轴机械臂的MDH正向建模，及python算法》做了非常细致的讲解，本文不再赘述。

通过运算，我们可以把下图的机械臂建立一个模型，

所谓正向模型即f(t1,t2,t3,t4,t5,t6)，就是输入6个电机的旋转角度，可以得出末端的位姿，这里可以用位姿矩阵表示，也可以用四元素等其它表示。我们根据机械臂实际的尺寸，推导出的正向模型，非常复杂，如下：

这个明显是有一堆sin，cos组合的非线性矩阵。如果人去推导，估计得推导10年。

2.2张量化

金典得非线性方程组解法，可以用迭代法，牛顿，雅可比等。对于数学知识有限的初学者，我们没有那么多时间。我们使用了简单粗暴的玩法，那就是交给pytorch的反向传播，来让它帮我们寻找正确的6个角求解调整方向，并逐步逼近"真相"。

python 复制代码

# 优化器
    optimizer = torch.optim.Adam([t1,t2,t3,t4,t5,t6], lr=lr)
    
    # 进行优化
    for epoch in range(interval):    
        optimizer.zero_grad()
        loss = loss_function(t1,t2,t3,t4,t5,t6)
        loss.backward()
        optimizer.step()
        if epoch%100==0:
            print(f'Epoch {epoch}, Loss {loss.item()}')
        if loss.item()<maxerror:
            print("minloss:",loss.item())
            break

如上所述，只要几行代码就可以进行解算了。

经过运行，解算出的结果还是可以的，如果小幅的运动，误差会非常小。

python 复制代码

minloss: 0.0009870927315205336
dx,dy,dz error: Matrix([[-0.0145334085096351], [-0.00555758762822034], [-0.0203845457565990]

如上图，第一个状态时正解的，第二个状态时逆解后计算出的末端6个状态，相差不是很大。

这里我们的lossfunciton是用了六个自由度的方差。

pytorch中我们要把未知数都转化成张量，并且可以梯度，才能优化求解。

python 复制代码

t1 = torch.tensor([j1],requires_grad=True, dtype=torch.float32,device=device)
    t2 = torch.tensor([j2],requires_grad=True, dtype=torch.float32,device=device)
    t3= torch.tensor([j3],requires_grad=True, dtype=torch.float32,device=device)
    t4 = torch.tensor([j4],requires_grad=True, dtype=torch.float32,device=device)
    t5 = torch.tensor([j5],requires_grad=True, dtype=torch.float32,device=device)
    t6 = torch.tensor([j6],requires_grad=True, dtype=torch.float32,device=device)

2.3绘制3D动画及操作UI

为了直观感受机械臂的状态，和调试，我们还是增加了3D的手臂动态绘制功能，和滑动条状态显示功能：

三、结论

本篇利用了一个人工智能库工具pytorch的强大的梯度下降寻优机制，对复杂的N轴机械臂解算进行了求解，这种方法不需要太高深的数学知识，适合初学者学习使用。此种办法也有缺点，那就是计算较慢，这块需要后续优化，比如使用更先进的CPU和GPU，比如先估计一个比较靠谱的初值，比如简化一下模型等，我们后面再继续探讨。