三轴机械臂逆解（几何法）

 

from https://blog.csdn.net/mwlwlm/article/details/78153869

对于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，即列出方程，如下为方程式关系：

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

————————————————

版权声明：本文为CSDN博主「DiegoRobot」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。

原文链接：https://blog.csdn.net/mwlwlm/article/details/78153869

from http://www.skcircle.com/?id=1310

三轴机械臂逆解（几何法）

几何法逆解

从末端A点开始逆推，

第一步：求B点坐标

已知A点坐标X,Y和俯仰角γ，可以通过一下公式求出B点坐标

C#

Bx = val_x - a3*cos(pitch);
 
By = val_y - a3*sin(pitch);

第二步：求出理想的θ1

通过余弦定理公式计算出β

C#

cosbeta = (Bx^2 + By^2 + a1^2 - a2^2)/(2*a1*sqrt(Bx^2+By^2));
 
beta = acos(cosbeta)*180/pi;
 
alpha = atan2(By,Bx)*180/pi;

所以得出

C#

theta1 = alpha + beta

第三步：求出理想的θ2

通过余弦定理公式计算出（180 - θ2）

C#

costheta2 = -(Bx^2 + By^2 - a1^2 - a2^2)/(2*a1*a2);
 
theta2 = -(180 - acos(costheta2)*180/pi)

第四步：求出θ3

通过

γ = θ1 + θ2 + θ3

求出

θ3 = γ- θ1- θ2

 

至此，逆解的角度值全部计算完毕，下一步需要求解每个关节运动过程中的转动速度