【三自由度机械臂运动学正逆解实现细节】

前言

使用craig版本的DH建模，即一般认为的M-DH。

坐标系

1.首先沿着转轴确定Z轴位置，方向自定。
2.找到与相邻两Z轴均相交的公法线（注意不能是公法面上的任意一条公法线，根据DH准则必须与两Z轴相交），Xi-1沿着Zi-1与Zi的公法线，方向自定。
3.由右手法则确定Y轴。
4.基坐标系尽量与坐标系1重合。
5.最后一个坐标系尽量与前一个重合。

DH参数

1.αi-1是Zi-1绕Xi-1旋转到Zi的角度，从Xi-1看逆时针为正。
2.ai-1是Zi-1沿着Xi-1到Zi的距离。
3.di是Xi-1沿着Zi到Xi的距离，可以看到X0是沿着Z1的反方向移动d1才与X1重合的，所以我在图中给d1前加了个负号。
4.θi即Zi的转角，也就是从Zi上看Xi与Xi-1间的夹角。

齐次变换矩阵

按照M-DH建立的DH参数都可以直接带入上面的变换矩阵中。
这里比较麻烦的是带未知数的矩阵运算，我找到这篇用python辅助计算的博客可以很好的解决：
https://blog.csdn.net/uuuuur/article/details/106608335
最后也是得到了正向运动学变换矩阵

逆解

这里用符号Ai代替Ti，假设给出的位姿为T

做一步变换

可以得到较为简洁的等式关系，更复杂的情况不能完全求出角度就继续左乘A2…
这里就不手打我的结果了
主要提一下这里比较重要的数学工具
1.θ = atan2(y,x) = atan2(sinθ，cosθ)
这个函数可以根据xy的正负确定所在象限，求出-pi到pi的角度，这在角度求解中很有用所以我的逆解最后都化成这个函数表示，尽管我有时已经知道了sinθ或cosθ。
2.
把等式化为Asin φ+Bcos φ+C=0的形式
可以直接得到

推导见https://zhuanlan.zhihu.com/p/338572834

验证逆解算法

这里采用输入十组原始点位然后通过逆解求出角度再通过变换矩阵计算出新的点位，对比点位是否有偏差判断逆解算法正确性。

import numpy as np
import math
import matplotlib.pyplot as plt


# 定义逆解算法和转换矩阵
def inverse_kinematics(pos):
    # 逆解算法
    value = pos[0] ** 2 + pos[1] ** 2 - 4
    if value < 0:
        print("无法计算：pos[0] ** 2 + pos[1] ** 2 - 4的值小于0")
        return -1
    else:
        a = -math.atan2(pos[1], -pos[0]) + math.atan2(2, math.sqrt(value))
    k1 = -1 - pos[2]
    k2 = pos[0] * math.cos(a) + pos[1] * math.sin(a)
    K