3轴码垛机械臂运动学逆解

（待后期更新运行效果图出来）

对于3轴码垛机械臂控制最基本的是对其建立运动学模型，而对于3轴码垛类型机械臂来说运动学模型，其本质就是给定空间3D坐标，求解3个轴的旋转角度。

如上图所示，左侧为实物坐标，右侧图为抽象到坐标系的几何表示，逆解过程就是知道末端坐标，而求解各个轴的旋转角度，进而转换为步进电机的步进数，下面我们利用立体几何，和解析几何知识来进行你运算分析。

一，假设条件

坐标系采用右手坐标系，如上图所示
机械臂的底座位于右手坐标系的XY平面
底座旋转轴的位置，即为坐标系的原点位置，即上图做所视的O点
我们把AB线所表示臂称之为大臂，BC线所表示的臂称之为小臂
OA线与XY平面的夹角为θ
AB线与Z轴的夹角为β
AB线与BC线之间的夹角为γ，即大臂与小臂的夹角
点O,A,B,C始终处于同一平面，且此平面与XZ平面之间的夹角为α
机械臂的初始位置为A，B，C三点的Y轴坐标为O，B，C点收回到距离Z轴最近的位置
初始位置α=0,β=β0,γ=γ0
已知条件：OA，AB，BC长度，θ角度是固定值

二、数学求解

已知C点的坐标(x,y,z)，基于以上假设条件，求解α,β,γ。
为了方便求解，我们将相关点进行坐标系投影，如下图所示：

此方程其实比较简单，只需依据直线OCxy=OAxy+AxyBxy+BxyCxy，即列出方程，如下为方程式关系：

根据上述方程式，即可解出α,β,γ的值，基于此三个角度值除以步进电机的步进角度，轻松计算出从上一个位置到现在位置需要步进多少步。

开环的机械臂：

 其可以从当前求出的需要的步进角度来跟上一次位置的求出的需要的步进角做差值得到跟达到上一次位置实际转的相加得出的就是新位置需要上一次位置补偿多少的角度才可以到达新位置。

不足：由于是开环，就可能存在丢步的情况，所以当运行次数多了的话，累计的丢步会导致机械手达到的位置偏差越来越大。

其需要再运行次数在一定量的时候要自动回原点，进行原点矫正。如果每次都进行回原点矫正，其会导致运行效率低。

闭环的机械臂：

 其可以从当前求出的需要的步进角度来跟上一次位置的求出的需要的步进角做差值得到跟达到上一次位置实际转的相加得出的就是新位置需要上一次位置补偿多少的角度才可以到达新位置。

优点：由于闭环，其不存在丢步的情况，所以其不用每次会原点矫正，其再断电的情况下才需要会原点矫正坐标。

参考博客：

[机械臂运动学系列]机械臂及其笛卡尔坐标正逆解算（三）

机械臂运动学逆解