机械臂运动学正逆解算法 / Algorithm of forward and inverse kinematics for manipulator

构型空间的确立

利用指数积建立空间构型，确定自由度机械臂的空间旋转轴矩阵S、机械臂初始状态下末端的齐次变换矩阵M及指数积构型下的机械臂空间，末端位姿（运动学正解结果）等参数。

参考资料：https://blog.csdn.net/gpeng832/article/details/73695271/?utm_medium=distribute.pc_relevant.none-task-blog-baidujs_title-0&spm=1001.2101.3001.4242

https://blog.csdn.net/gpeng832/article/details/73730624?utm_medium=distribute.pc_relevant.none-task-blog-baidujs_title-1&spm=1001.2101.3001.4242

运动学逆解算法流程图

运动学逆解算法推导过程

本运动学逆解算法利用指数积构建空间构型，利用牛顿-拉夫逊法对各自由度旋转角度进行迭代从而求出机械臂各自由度的数值解。以6自由度机械臂为例，其单位旋转矩阵S为：

其中ω为六自由度机械臂各自由度旋转轴单位角速度在三维空间下的表示，v为六自由度机械臂各自由度旋转轴单位线速度在三维空间下的表示。

如上图所示，θ_01为机械臂当前状态下各关节角度，T_0为机械臂末端当前状态下的齐次变换矩阵，T_f为机械臂末端目标状态下的齐次变换矩阵，V为机械臂从T_0至T_f的运动旋量。

通过指数积法表示空间构型，坐标原点至第一个自由度的齐次变换矩阵T_(0-1)可表示为：

其中e^([S_1 ] θ_i1 )为即为指数积坐标，可表示为：

同理可得机械臂各自由度间的齐次变换矩阵，即可知机械臂第i次迭代过程中末端正解可表示为：

其机械臂的运动旋量V表示为 ：

将机械臂正解对时间求导可得到T ̇为：

将机械臂正解求逆可得到T^(-1)为：

其运动旋量[V]∈se(3)为：

根据以上公式可得，当i=0时，其运动旋量V与各关节角度变换的关系：

其中J_0j定义为各个自由度的空间雅可比矩阵，其可表示为关于θ_0j的函数：

通过上述计算，旋量V可表示为：

单位时间内，各自由度角度的变化∆θ可表示为：

其中 为其机械臂空间雅可比的伪逆。此时下一组迭代法角度 表示为：

当运动旋量V小于等于可接受误差ε时停止迭代，此时角度组θ_i即为末端其次变化矩阵T_f的逆解。

利用上述算法，可计算任意自由度任意构型机械臂的运动学逆解，其视频如下：

10MB

simulink circle.mp4

7MB

servo_writing.mp4