并联机器人拉格朗日方法动力学求解位置

代码说明：

采用拉格朗日法对并联机器人进行动力学建模，用4阶龙格-库塔方法对并联机器人的位置，姿态进行仿真。得到12张图像，分别为速度图：x轴速度，y轴速度，z轴速度，alpha角速度，beta角速度，gamma角速度。位置图：x轴位置，y轴位置，z轴位置，alpha角度，beta角度，gamma角度。采用PD控制率，机器人初始值和控制矩阵参数在仿真程序中。