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个角求解调整方向，并逐步逼近“真相”。

# 优化器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

如上所述，只要几行代码就可以进行解算了。
经过运行，解算出的结果还是可以的，如果小幅的运动，误差会非常小。

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

在这里插入图片描述
如上图，第一个状态时正解的，第二个状态时逆解后计算出的末端6个状态，相差不是很大。
这里我们的lossfunciton是用了六个自由度的方差。
pytorch中我们要把未知数都转化成张量，并且可以梯度，才能优化求解。

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，比如先估计一个比较靠谱的初值，比如简化一下模型等，我们后面再继续探讨。

四、python源码

配套资源已经上传：源码链接

[------------本篇完--------------------------]

PS.扩展阅读

————————————————————————————————————————

对于python机器人编程感兴趣的小伙伴，可以进入如下链接阅读相关咨询

ps1.六自由度机器人相关文章资源

(1) 对六自由度机械臂的运动控制及python实现（附源码)
在这里插入图片描述

(2) N轴机械臂的MDH正向建模，及python算法
在这里插入图片描述

ps2.四轴机器相关文章资源

(1) 文章：python机器人编程——用python实现一个写字机器人
在这里插入图片描述

在这里插入图片描述

（2）python机器人实战——0到1创建一个自动是色块机器人项目-CSDN直播

(3)博文《我从0开始搭建了一个色块自动抓取机器人，并实现了大模型的接入和语音控制-(上基础篇)》的vrep基础环境
(3)博文《我从0开始搭建了一个色块自动抓取机器人，并实现了大模型的接入和语音控制-(上基础篇)》的vrep基础环境
(4)实现了语音输入+大模型指令解析+机器视觉+机械臂流程打通
在这里插入图片描述

ps3.移动小车相关文章资源

（1）python做了一个极简的栅格地图行走机器人，到底能干啥？[第五弹]——解锁蒙特卡洛定位功能-CSDN博客
(2) 对应python资源：源码地址
在这里插入图片描述

(3)python机器人编程——差速AGV机器、基于视觉和预测控制的循迹、自动行驶（上篇）_agv编程-CSDN博客
(4)python机器人编程——差速AGV机器、基于视觉和预测控制的循迹、自动行驶（下篇）_agv路线规划原则python-CSDN博客
对应python及仿真环境资源：源码链接
在这里插入图片描述