2.1.1机器人动能和势能的求解
拉格朗日法利用系统能量的微分求出力和力矩，适用于较为复杂下的系统，比如六关节机器人。
拉格朗日函数为：
L=K-P
其中K是系统动能，P是系统势能，L是拉函数。
L对直线位移、角位移和时间求微分可得：
其中Fi是所有外力和，Ti是所有外力矩和，xi直线位移，θi角位移，t时间。
动能K的求解。
对机器人连杆来说，连杆上任意一点的速度vi为该点位置对时间的求导。
该点的动能dKi为。
该连杆的动能Ki为。
机器人总动能K为。
势能P的求解。
其中，gT是重力矩阵，ri表示连杆质心在坐标系中的位置。
因此拉格朗日函数可以表示为：
2.1.2求机器人的动力学方程
对朗格朗日函数求导可得机器人的运动方程，最终可化为：
其中，第一部分是角加速度-惯量项，第二部分是驱动器惯量项，第三部分是科氏力和向心力项，最后一步分是重力项。对六轴机器人，方程可化为如下：
代入各数据即可求出每个关节需要施加的力矩。
参考文献
[1] 机器人学导论，[美] John J. Craig 著，贠超 王伟 译，机械工业出版社；
[2]机器人学导论-分析、控制及应用，[美] Saeed B. Niku 著，孙富春 朱纪洪 刘国栋 等译，电子工业出版社。