机械臂样子是这样的
建模
上面是两边同时取平方后，将C12,S123 的平方项消掉，然后 采用二角和差公式（12与3）将1,2角消掉，留下theta3, 这里需要对theta1的0-180度每1度做一次运算
对于theta2，将方程Z=.....采用统一的二角和差化解后得到类似上面的方程
实际的机械臂模型如下图，大臂与小臂角度是有耦合的，但是他们4条边构成的是一个平行四边行(边长固定),所以大小臂的夹角有theta1跟theta2的组合来定
需要考虑theta2的取值范围会随theta1变化而变化，所以对计算出的theta1-theta3要做实际的校验才能执行
参考:https://blog.csdn.net/qq_39777417/article/details/96133017