aubo-i5机械臂（2）-逆运动学求解

最新推荐文章于 2025-10-01 15:21:08 发布

原创

最新推荐文章于 2025-10-01 15:21:08 发布 · 5k 阅读

65 ·

CC 4.0 BY-SA版权

文章标签：

#matlab #自动化

本文介绍AUBO-I5六自由度机械臂逆运动学求解过程，包括理论推导、MATLAB实现及求解正确性的验证。提供完整MATLAB函数实现逆运动学求解。

1、逆运动学求解理论推导

2、matlab实现逆运动学求解

3、逆运动学求解正确性验证

1、逆运动学求解理论推导

对于AUBO-I5机械臂而言，其具有六个自由度，其中三个确定末端位置信息，另外三个确定末端姿态信息。以基座坐标为参照，由第一章正运动学AUBO-I5末端相对于他的姿态信息可以用 $_{6}^{0}\textrm{T}$ 进行表示，参考式子如下：

$_{6}^{0}\textrm{T}=\begin{bmatrix} nx & ox & ax & px \\ ny& oy & ay & py \\ nz& oz & az & pz\\ 0 & 0 & 0 & 1\end{bmatrix}$

更多理论与求解公式参考[1]曹继项. 基于视觉的AUBO机器人标定方法研究[D].长安大学,2018.

注：求解θ时应使用atan2(sinxθ,cosθ)函数，因为θ的范围在[-pi,pi],而其他反三角函数不符合，详情自己搜索，默认学过机器人的都懂。

2、matlab实现逆运动学求解

此代码为一个完整的逆运动学求解函数，可直接调用。

function q = aubo_ikin(H_t2o)
% q 为输出，表示关节坐标；
% H_t2o 为输入，表示末端到基座的 4X4 齐次变换矩阵。

%%%！！！定义100为无效值，判断某一个关节角度无法计算得出正确解时填充100！！！%%
%%
%%机械臂的连杆长度和连杆偏置值%%
d1=0.0985;
d2=0.1215;
d5=0.1025;
d6=0.094;
a2 = 0.408;
a3 = 0.376;

nx =H_t2o(1,1);   ox=H_t2o(1,2);   ax=H_t2o(1,3);
ny =H_t2o(2,1);   oy=H_t2o(2,2);   ay=H_t2o(2,3);
nz =H_t2o(3,1);   oz=H_t2o(3,2);   az=H_t2o(3,3);
px=H_t2o(1,4);    py=H_t2o(2,4);   pz=H_t2o(3,4);%%读取输入矩阵的各个元素

Theta = nan(6,8);%%预先开辟数组存放各组解%%
%%
%%根据公式计算各个关节角度%%
%%%%计算theta1%%%%
%%theta11、theta12为最终解%%
A=(ax*d6-px)^2+(d6*ay-py)^2-(d2)^2;
if abs(A)<=1e-5
    A=abs(A);
end
if A>=0
    theta11=atan2(d6*ay-py,ax*d6-px)-atan2(d2,sqrt(A));
    theta12=atan2(d6*ay-py,ax*d6-px)-atan2(d2,-sqrt(A));%%得出theta1的两个解%%
else
    theta11=100;
    theta12=100;  %%%不符合条件，填充无效值100（下同）%%
end   
    Theta(1,1:4)=theta11;
    Theta(1,5:8)=theta12;  %%将theta1存放至数组Theta中%%
%%
%%%%计算theta5%%%%
%%theta51、theta52、theta53、theta54为最终解%%
c51=ax*sin(theta11)-ay*cos(theta11);%theta11
s51=(1-(ay)^2)*(cos(theta11))^2+(1-(ax)^2)*(sin(theta11))^2-2*sin(theta