根据物理学知识可知，
Ixx???y2?z2?dm，Iyy???x2?z2?dm，IZZ???x2?y2?dm
Ixy?Iyx??xydm，mx??xdm。
则有n个连杆机器人的系统总动能为
??1niiTK??ki????TraceuijIiuikqjqk
2i?1j?1k?1i?1n??其中ki为杆件传动装置的动能；Ii为传动装置的等效转动惯量。
4.3机器人的势能
如果机器人的总势能为P，而各杆件的势能为pi，则
?0pi??mig??T?i?i其中g??gxgygz0?为基坐标下重力的各分量。 所以机器人的总势能为
?oi??p??pi???mig??Tiri??4-19 i?1i?1??nn?ri?? 4—18 ?4.4 动力学方程
根据公式4-17和4-19可以建立机器人的拉格朗日函数
???01niiTL?K?P????TraceuijIiuikqjqk+mig??T2i?1j?1k?1????i?i??1n2ri?+Iqi4-20 ?ai?2i?1?对拉格朗日函数求导
?L?qi????Trace?UjkIJUji?qk?Iaiqi
j?ik?inj???nii??????d??L?njT???Trace?ujkIjUji?qk?2???TraceujmkIiuikqmqj+Iaiqi ?dt??q?j?1k?1i?1j?1k?1?i???n?????1njj?L1niijTT????TraceujmkIiuikqmqj+???TraceujmkIiuikqmqj+?migUjirj
2j?im?1k?1?qi2j?im?1k?1j?i????因此，工业机器人的动力学方程为
??d??L??LTi???dt??q??qi?i????TraceUjkIjUj?ik?1nj?Tji?q?Ik??aiqi????TraceUjkmIjUj?ik?1m?1??njj?Tjl?q?mqk???mgUij?injjirj
?将上式简化为矩阵符号形式
?i??Dikqk?Iaiqi???Dikmqkqm?Di 2-22
k?1k?1m?1n????nn??第一部分是角加速度惯量项，第二部分是驱动器惯量项，第三部分是向心力项。惯量项和重力项对于机器人系统的稳定性和定位精度至关重要。而向心力在机器人低速运动时可以忽略，但在高速运动时作用非常重要。
本章小结
本章首先介绍了机器人相邻两连杆之间关系的齐次变换矩阵A矩阵，其次介绍了利用A矩阵确定机器人运动学方程的方法，并建立了6自由度喷漆机器人的运动学方程，求解了雅克比矩阵。最后讨论了机器人的动力学方程，用拉格朗日动力学方程求解机器人的动能和势能，为下一章对机器人进行建模仿真奠定了坚实的理论参考。
各个关节运动量的计算是机器人控制程序设计的基础。
第四章 喷涂机器人轨迹规划
4.1轨迹规划概述
机器人要在作业空间内完成给定的任务，其手部运动必须按一定的轨
迹进行。轨迹的生成一般是先给定轨迹上的若干个点，将其运动学反解映射到关节空间，对关节中的相应点建立运动方程，然后对关节进行插值，从而实现作业空间的运动要求，这个过程称为轨迹规划。
期望轨迹是用来描述机器人手臂所期望的运动，是各关节位置及速度的时间函数。
路径规划是根据任务指令规划机器人手臂末端的运动来完成预定任务，从而形成各关节的轨迹，是根据机器人作业要求在具有障碍的环境内按照一定的标准，对末端执行器在工作过程中的状态（位置、速度、位姿、加速度等）进行设计，寻找一条从起始状态到目标状态的无碰撞路径。
路径规划的好坏直接影响机器人的作业质量，当关节变量的加速度在规划中发生突变时，将会产生冲击，如果机器人固有频率较低将产生低频振动，机器人启动和停止时手部抖动就是这种现象的表现。
图4.1 机械手工作原理
从图4.1可看出机械手要完成规定的任务，需要经过以下三点： 1、 按照作业任务规划好期望的轨迹。 2、 建立合适的机器人手臂动力学模型。
3、 通过这些模型确定控制方法，使设计达到期望的系统响应时间和性能。
工业机器人的运动按照其运动轨迹可以分为点到点运动和路径跟踪运动。点到点运动只关心特定的位置点而路径跟踪运动则关心整个运动路径。
4.2轨迹跟踪运动
轨迹跟踪运动，希望机器人的末端能够以特定的姿态沿给定的路径运动。为了保证机人的末端处在给定的路径上计算出路径上各个点的位置，以及在各个位置点上机器人所需要达到的姿态。上述计算路径上各点处的机器人位置与姿态的过程称为机器人笛卡尔空间的路径规划。根据规划出的各个路径点处的机器人位置与姿态，利用逆向运动学求取机械手各个关节的目标位置，通过控制各个关节的运动，使机器人的末端到达各个路径
点处的期望姿态。
轨迹跟踪是以点到点运动为基础的，而点到点运动的中间路径是不确定的，因而，轨迹跟踪运动只是在给定的路径点上能够保证机器人末端到达期望位姿，而在各个路径点中间不能保证机器人末端到达期望位姿。 为了使机器人末端尽可能的尽可能的接近期望轨迹，在进行机器人笛卡尔空间的路径规划时，两个路径点之间的距离应尽可能小。为了消除路径点之间机器人末端位姿的不确定性，通常对各个关节按照联动控制进行关节空间的运动规划。具体的说就是在进行关节空间的运动规划时，要使得各个关节具有相同的运动时间。
可见轨迹跟踪运动需要笛卡尔空间对机器人的末端位姿进行运动规划，还需要在机器人的关节空间进行运动规划。 4.2关节空间路径规划
机械手末端执行器的运动是由关节变量直接决定的，若能够在关节空间进行路径规划，既能省时又能够避免雅克比矩阵奇异时形成速度失控现象。但是由于直角坐标空间和关节坐标空间转换关系复杂，只有对端点位姿有要求而对端点之间路径没有要求的任务才能在关节空间直接进行规划，这些点包括路径起始点和终点，又包含机器人在运动过程中必须经过的一些特定点称为途经点。
通过求解逆运动学问题将途经点转换成一组关节变量，对于从起始点开始经过所有途经点，到达终点的机器人每个关节找一个平滑函数，各个关节在每段路径所需的时间是相等的，因此所有关节会在同一时间到达途经点。
如图所示，如果某关节在t0时刻的关节位置为q0，希望在tf时刻的位置为qf，有如图三条轨迹，轨迹1和2运动过程中会有波动，这不是我们所期望的，如果按照轨迹3运动，则机械手能够平稳的到达目标位置。
图3.1 关节运动路径
4.2.1 B样条插值
设m为样条的次数，在m?1个子区间以外的其它子区间上，B样条的取值都为0。B样条函数采用递归的方式进行定义。假设对于自变量x，有m?2个点xi，xi?1,?????,xi?m?1,构成m?1个子区间?xi,xi?1?,????xi?m,xi?m?1?。首先定义0次B样条函数，然后根据第m?1次B样条函数定义在区间[xi,xi?m?1)的第m次B样条函数。
?1,x??xi,xi?1? （3——33） Ni,0?x?????0,x?x,xii?1?其中Ni,0?x?是0次B样条函数，?xi,xi?1?是0次B样条函数的非0区间。
Ni,m?x??x?xix?xNi,m?1?x??i?m?1Ni?1,m?1?x? （3——34）
xi?m?xixi?m?1?xi?1其中Ni,m?x?是m次B样条函数，以此类推。
根据以上两式的B样条函数定义，可以得到1次、2次和3次的B
样条函数，既得
?x?xi,x??xi,xi?1???x?xiNi,1?x???i?1 （3——35）
xi?2?x?,x??xi?1,xi?2??x?xi?1?i?2