一、前言

前面對六軸（或多軸）機械臂進行了一些研究，特別是利用幾何進行簡化，實現逆解，來完成比較簡單的搬用任務。然而，如果需要比較精確的操作任務，可能需要更加復雜的機械臂解算。其中比較復雜的是數值解算，這種解算往往涉及到比較復雜的機器人運動等原理知識，比較難以上手。但是，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及仿真環境資源：源碼鏈接