前篇，正向解

https://www.jianshu.com/p/6df2169b5f90

image.png

定义：给定一个位置输出对应的舵机角度

运动学逆解是指已知机器人末端位姿，求解各运动关节的位置，它是机器人运动规划和轨迹控制的基础。
懒得解，不会解，想要偷懒，怕解错，算力性能够的情况下可以使用深度学习来做线性变换

下图片颜色图片之间一一对应

输入：（机械臂位置，单位毫米，这里省略了轴）

image.png

输出：（舵机角度，单位度）

image.png

模型预测的角度:(舵机角度，单位度)

image.png

具体可视化代码

from robot.gaitSolve import angle_to_pos as atp # 上一个文章的代码
def transAngel(data):
    p1 = data[1]*140-20
    maxp = min(20,p1-35)
    minp = p1-110
    p2 = data[2]*(maxp-minp)+minp
    return np.array([0,p1,p2])


a = np.random.rand(10000,3)
arr = []    
tmp = []
tmp2 = []

for p in a:
    p[0]=0
    tmp.append(transAngel(p))
    tmp2.append(model(torch.tensor(angle_to_pos(transAngel(p)),dtype=torch.float32).to(device)[0:3]).data.cpu().numpy()[0:3])
    arr.append(atp(transAngel(p)))
    
    p[0]=p[1]
    p[1]=0

arr = np.array(arr)
tmp = np.array(tmp)
tmp2 = np.array(tmp2)

import matplotlib.pyplot as plt
plt.figure(figsize=(6,6))
sc = plt.scatter(x = arr[:,0],y= -arr[:,1],c=a,s=1)
plt.show()

plt.figure(figsize=(6,6))
sc = plt.scatter(x = tmp[:,1],y= tmp[:,2],c=a,s=1)
plt.show()

plt.figure(figsize=(6,6))
sc = plt.scatter(x = tmp2[:,1],y= tmp2[:,2],c=a,s=1)
plt.show()

做法

1.生成一批舵机角度
2.根据角度生成舵机位置
3.将舵机位置作为输入，舵机位置为结果预测结果

模型结构

class NeuralNetwork(nn.Module):
    def __init__(self):
        super(NeuralNetwork, self).__init__()
        self.linear_relu_stack = nn.Sequential(
            nn.Linear(3, 32),
            nn.ReLU(),
            nn.Linear(32, 64),
            nn.Sigmoid(),
            nn.Linear(64, 32),
            nn.ReLU(),
            nn.Linear(32,3)
        )
        
    def forward(self, x):
        y_pred = self.linear_relu_stack(x)
        return y_pred

算法训练代码

model = NeuralNetwork().to(device) 
loss_fn = nn.MSELoss()  
optimizer = torch.optim.Adam(model.parameters(), lr=0.01)

# 迭代3000次
batches = 3000
batch_size = 1000

for i in range(batches):
    X,Y = getData(batch_size+i*3)
    
    Y_pred = model(X)
    loss = loss_fn(
        Y_pred, 
        Y
    )
    
    optimizer.zero_grad()
    loss.backward()
    optimizer.step()
    
    loss, batch = loss.item(), i
    print(f'loss: {loss} {batch}')
    

结果

经过测试，大部分的相对距离基本上小于1毫米，已经近似舵机进准度。

image.png

对于不在训练集的数据也能找到相对于的解
23-(-7) = 30度，比代码设计的35度要小

image.png

其他

模型的复杂性会影响精度

之前设置的复杂度低一点，方差大概在1mm左右，现在扩大一倍，复杂度多大4倍，精度也提升了4倍。

 #下面模型全部
            nn.Linear(3, 16),
            nn.ReLU(),
            nn.Linear(16, 32),
            nn.Sigmoid(),
            nn.Linear(32, 16),
            nn.ReLU(),
            nn.Linear(16,3)

散列问题

对于f(x)=y ,如果同一个y的找到多个x,那么就不是唯一对应的，那么如果直接拿这些结果来训练模型，会导致模型的loss不稳定,模型会在这部分结果附近震荡。

解决方法，干掉争议部分，可以减少模型复杂度，并且提高模型准度。
如下图，模型在第二张图部分表现在第一张图的位置会导致位置出现重叠的情况。
解决方法就是干掉这部分，将度舵机的夹角从10提升到35左右。
一来提高了三角架稳定性，二来降低了模型的震荡。

image.png