UR5构型的机械臂是目前机器人领域比较常见的一种，本文基于GuYueInvent PROBOT C800（《如何从Solidworks导出URDF模型 · 古月》课程中使用的机械臂）机械臂，从DH建模，到正逆运动学的推导，介绍其整个运动学建模的过程，并演示了在matlab 2015a和ROS-Rviz环境下的运动效果。
一、C800的DH模型：
DH建模法是建立串联机器人连杆坐标系最常用的方法，目前学术界同时存在标准DH和改进DH两种建模方法，本文是应用的标准DH建模法对机械臂建模，图1是GuYue Invent PROBOT C800机械臂的尺寸参数。
图1 C800机械臂尺寸参数
建立如图2所示的连杆坐标系，根据坐标系写出其DH参数表，见表1。
图2 C800机械臂连杆坐标系
二、C800的正运动学：
机械臂的正运动学是把机械臂的关节变量作为自变量，描述机械臂末端执行器的位置和姿态与机械臂基座之间的函数关系。
1.变换矩阵的推导：
2.末端执行器位姿推导：
至此，正运动学就推导完毕了。
三、C800的逆运动学：
机械臂的逆运动学就是已知末端执行器相对于基坐标系的位置和姿态，求对应的六个关节角度。
1.变换矩阵的求解：
2.关节1的求解：
3.关节5的求解：
4.关节6的求解：
5.关节3的求解：
6.关节2的求解：
7.关节4的求解：
四、效果演示
1.正运动学测试：
利用MATLAB 2015a robotic toolbox进行机械臂的建模，并验证正运动学推算的正确性。
以下是利用toolbox计算的程序。
同时，为了测试自己的正运动学算法是否正确，笔者是在VS中编写的C程序来测试，两者误差基本为0。
以下是利用关节角度控制机械臂运动的代码及演示图。
2.逆运动学测试：
在Ubuntu18.04 ROS-Melodic下，利用Rviz的可视化界面，测试逆运动学的实时效果，如图。
点击按钮，C800机械臂末端会朝着竖直向下的方向移动，机械臂控制器实时解算位姿，解出距离当前位姿最靠近的解（控制周期10ms）。
以上就是笔者基于GuYueInvent PROBOT C800型机械臂推演的运动学算法，并在MATLAB和ROS-RVIZ中测试验证算法可用性的整个流程，也借鉴了许多网络上的建模经验。希望本文能够对你产生帮助，谢谢欣赏！
