ADAMS/MATLAB联合仿真机械臂重力补偿问题

1 背景与重力补偿基本算法

现在有一个大型机械臂，要做一个缩比同构的小机械臂作为主控端来控制从端大机械臂。小臂的各个连杆可能比较重，要求用户在带着小臂运动的时候尽量感觉不到小臂各连杆的重量，并且在松手后小臂能立即静止不动。

由需求可知需要一个重力补偿算法，通过每个关节输出力矩来平衡掉各个连杆的重力。

首先第一个问题就是重力补偿算法的原理。我们不管用户的手如何去动小机械臂，每个马达只负责平衡掉重力的影响，也就是说，用户如果动作快一点的话是可以感觉到各个连杆的转动惯量，惯量这部分我们就不做别的处理了（貌似也没法处理）。

于是问题的本质就是一个静平衡问题：在重力作用下，在特定的一组关节角组合下，每个关节该输出多大的力矩才能让小机械臂保持静止。

参考战强老师的《机器人学：机构、运动学、动力学及运动规划》一书中P115关于机械臂连杆受力的描述，连杆i的受力情况如图。这里需要注意区分连杆i与关节i，在改进(前置)DH建模法里，连杆i从关节i开始，到关节i+1结束。而在我们进行三维建模时，我们建立的实体都是连杆，关节的概念是抽象的，一定要把这点搞清楚。

 对于每个连杆i，有力平衡方程与力矩平衡方程：