CN112757296B - 重力补偿方法及装置 - Google Patents

Abstract

一种力反馈机械臂的重力补偿方法及装置，该重力补偿方法包括：实时获取所述每个旋转电机中电机编码器的输出值，并根据旋转电机中编码器的当前输出值确定对应的当前转角；根据每个旋转电机的当前转角计算与其输出轴连接的连杆与水平方向的当前夹角；获取每个连杆的长度和重量，以及每个连杆的重心到其基部转轴的距离，并根据每个连杆的当前夹角、每个连杆的长度和每个连杆的重量，以及每个连杆的重心到其基部转轴的距离计算各个旋转电机的重力补偿力矩；将各个补偿力矩分别发送至对应的旋转电机，以使各个旋转电机根据接收到的补偿力矩进行输出力矩补偿。

Description

重力补偿方法及装置

技术领域

本发明涉及力补偿技术领域，特别是涉及一种重力补偿方法及装置。

背景技术

重力补偿在虚拟现实、航空等领域中有着广泛的应用，特别是在虚拟现实技术领域，重力补偿是其实现人机交互过程中重要的环节。随着计算机技术的快速发展，重力补偿的方式也有了较大的改进。

虚拟现实技术主要通过力反馈机械臂实现人机交互，在工作中机械臂末梢由使用者持握，并在一定工作空间内跟随持握者被动移动，同时按照一定的规律在持握点向用户产生作用力。该作用力反馈在使用者持握处，可以用于模拟人体的施力感受和触觉感受。为了保证用户感受的真实度和可信度，随动式力反馈机械臂的一项重要的性能指标是反馈力的精确程度，而由机械结构自重产生的误差则会降低这一精度，因此随动式机械臂必须对结构自重进行补偿。

一种当前市场可见的补偿办法是在机械臂的转轴处安装扭转弹簧，利用弹簧弹性变形产生的扭矩来抵消机械臂自重转矩。弹簧所产生的扭矩与其变形量成正比，重力转矩则与重力力臂长度成正比。扭转弹簧的安装位置可设计为达到当机械臂成竖直正立时，弹簧处于自由原长状态，随着机械臂向前倾倒，弹簧的扭转程度也逐渐加大。倾倒程度越大，重力力臂越大，同时弹簧的变形量也越大。如此在一定程度上可以实现对重力转矩的补偿。但是这种办法存在着明显的不足之处，诸如必须限制机械臂的摆动范围，不支持做整周转动，以及补偿不够精确，无法完全消除机械臂重力转矩的影响等。

发明内容

鉴于上述状况，有必要针对现有技术中的重力补偿方法限制机械臂的摆动范围和补偿不够精确的问题，提供一种力反馈机械臂的重力补偿方法及装置。

一种重力补偿方法，包括：

实时获取各个旋转电机中电机编码器的输出值，并根据所述旋转电机中编码器的当前输出值确定对应的当前转角，其中，所述电机连接在相邻两个连杆的铰接处，所述旋转电机的定子与其中一所述连杆的稍部通过转轴固定连接，所述旋转电机的输出轴与另一所述连杆的基部固定连接；

根据每个所述旋转电机的当前转角计算与其输出轴连接的所述连杆与水平方向的当前夹角；

获取每个所述连杆的长度和重量，以及每个所述连杆的重心到其基部转轴的距离，并根据每个所述连杆的当前夹角、每个所述连杆的长度和每个所述连杆的重量，以及每个所述连杆的重心到其基部转轴的距离计算各个所述旋转电机的重力补偿力矩；

将各个所述补偿力矩分别发送至对应的所述旋转电机，以使各个所述旋转电机根据接收到的补偿力矩进行输出力矩补偿。

进一步的，上述重力补偿方法，其中，所述根据所述旋转电机中编码器的当前输出值确定对应的当前转角的步骤包括：

获取所述旋转电机在预设转角时，在力补偿状态下所述电机编码器的标准输出值；

根据所述旋转电机中编码器的当前输出值、所述预设转角，以及所述标准输出值计算所述旋转电机的当前转角。

进一步的，上述重力补偿方法，其中，所述旋转电机的当前转角的计算公式为：

其中，a_i为旋转电机的当前转角，e_i为电机编码器的当前输出值，e_i0为电机编码器对应的标准输出值，a_i0为所述预设转角，E_i为电机编码器的最大输出值。

进一步的，上述重力补偿方法，其中，所述连杆的当前夹角的计算公式为：

A_i＝A_i-1+a_i，(A_i-1+a_i)＜2π；

其中，A_i为连杆i与水平面的当前夹角，A_i-1为连杆i-1与水平面的当前夹角，a_i为旋转电机i的当前转角，m为

取整数部分的值。

进一步的，上述重力补偿方法，其中，所述并根据每个所述连杆的当前夹角、每个所述连杆的重量和每个所述连杆的重心到其基部转轴的距离计算各个所述旋转电机的重力补偿力矩的步骤包括：

根据当前连杆的当前夹角和所述当前连杆的重心到其基部转轴的距离计算所述当前连杆相对于所述当前连杆之前的每一所述旋转电机的重力力臂长度；

根据所述重力力臂长度以及所述当前连杆的当前夹角计算所述旋转电机的重力补偿力矩。

进一步的，上述重力补偿方法，其中，所述旋转电机的重力补偿力矩的计算公式为：

其中，M_i为重力补偿力矩，L_k为第k个连杆的长度，l_j和l_i分别为第j个和第i个连杆的重心到其基部转轴的距离，G_i为第i个连杆的重量，L_ij为连杆j对电机i的重力力臂长，cos(A_k)为第k个连杆的当前夹角，cos(A_j)为第j个连杆的当前夹角。

本发明实施例还提供了一种力反馈机械臂的重力补偿装置，包括：

转角模块，用于实时获取各个旋转电机中电机编码器的输出值，并根据所述旋转电机中编码器的当前输出值确定对应的当前转角，其中，所述电机连接在相邻两个连杆的铰接处，所述旋转电机的定子与其中一所述连杆的稍部通过转轴固定连接，所述旋转电机的输出轴与另一所述连杆的基部固定连接；

第一计算模块，用于根据每个所述旋转电机的当前转角计算与其输出轴连接的所述连杆与水平方向的当前夹角；

配置模块，用于获取每个所述连杆的重量和每个所述连杆的重心到其基部转轴的距离；

第二计算模块，用于根据每个所述连杆的当前夹角、每个所述连杆的重量和每个所述连杆的重心到其基部转轴的距离计算各个所述旋转电机的重力补偿力矩；

执行模块，用于将各个所述补偿力矩分别发送至对应的所述旋转电机，以使各个所述旋转电机根据接收到的补偿力矩进行输出力矩补偿。

进一步的，上述重力补偿装置，其中，所述转角模块具体用于：

获取所述旋转电机在预设转角时，在力补偿状态下所述电机编码器的标准输出值；

根据所述旋转电机中编码器的当前输出值、所述预设转角，以及所述标准输出值计算所述旋转电机的当前转角。

进一步的，上述重力补偿装置，其中，所述旋转电机的当前转角的计算公式为：

其中，a_i为旋转电机的当前转角，e_i为电机编码器的当前输出值，e_i0为电机编码器对应的标准输出值，a_i0为所述预设转角，E_i为电机编码器的最大输出值。

进一步的，上述重力补偿装置，其中，所述第二计算模块具体用于：

根据当前连杆的当前夹角和所述当前连杆的重心到其基部转轴的距离计算所述当前连杆相对于所述当前连杆之前的每一所述旋转电机的重力力臂长度；

根据所述重力力臂长度以及所述当前连杆的当前夹角计算所述旋转电机的重力补偿力矩。

本发明提出了一种基于软件控制的重力补偿方法，采用该方法不仅可以对机械臂的重力转矩实施精确补偿，同时还可允许机械臂转至任何方向。本计算方法运算速度快精度高，且不会有累积误差，可方便的部署于各类嵌入式系统。

附图说明

图1为本发明第一实施例中的力反馈机械臂的重力补偿方法的流程图；

图2为力反馈机械臂的示意图；

图3为本发明第一实施例中的力反馈机械臂的重力补偿方法的流程图.

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。

参照下面的描述和附图，将清楚本发明的实施例的这些和其他方面。在这些描述和附图中，具体公开了本发明的实施例中的一些特定实施方式，来表示实施本发明的实施例的原理的一些方式，但是应当理解，本发明的实施例的范围不受此限制。相反，本发明的实施例包括落入所附加权利要求书的精神和内涵范围内的所有变化、修改和等同物。

本发明实施例提供了一种力反馈机械臂的重力补偿方法，该算法可适用于现有技术中的力反馈机械臂的重力补偿，尤其适用于开式多连杆结构为基础的机械臂。该类机械臂第一节连杆基部与地面球铰连接，末梢与第二节连杆的基部铰接。之后每一节连杆的基部与上一节连杆的梢部铰接，梢部与下一节连杆的基部铰接。并且，该力反馈机械臂的每个关节处设置有支持恒力矩输出模式的旋转电机，或者可在关节处通过传动结构部署为旋转电机动力的输出点，具体实施时，保证各旋转电机的电机编码器旋转正方向一致。该关节处为连杆的铰接点，该旋转电机例如为力矩伺服电机。该旋转电机的定子和转子分别与前后两个连杆直接或通过传动结构间接相连，该旋转电机的输出轴装有精度不低于12位的编码器。

请参阅图1，为本发明第一实施例中的力反馈机械臂的重力补偿方法，该力反馈机械臂包括依序铰接的多个连杆，其中，相邻两个连杆通过一旋转电机连接。每一旋转电机连接前后两个连杆，旋转电机的定子通过转轴与前一连杆的稍部固定连接，旋转电机的转子通过输出轴与后一连杆的基部固定连接。该重力补偿方法包括步骤S11～S15。

步骤S11，实时获取所述每个所述旋转电机中电机编码器的输出值，并根据所述当前输出值确定所述各个所述旋转电机的当前转角。

旋转电机中的电机编码器用于记录旋转电机的旋转角度，实施获取各个旋转电机中电机编码器的当前输出值，根据该当前编码值即可确定对应的旋转电机的当前转角。

具体实施时，所述根据所述旋转电机中编码器的当前输出值确定对应的当前转角的步骤包括：

获取所述旋转电机为预设转角时，所述力反馈机械臂在力补偿状态下所述电机编码器的标准输出值；

根据所述旋转电机中编码器的当前输出值、所述预设转角，以及所述标准输出值计算所述旋转电机的当前转角。

进一步的，该当前转角a_i的计算公式为：

其中，a_i为旋转电机的当前转角，e_i为电机编码器的当前输出值，e_i0为电机编码器对应的标准输出值，a_i0为所述预设转角，E_i为电机编码器的最大输出值。

步骤S12，根据每个所述旋转电机的当前转角计算与其输出轴连接的所述连杆与水平方向的当前夹角。

根据旋转电机的旋转角度可以确定与其输出轴连接的连杆与水平面之间的当前夹角。该当前夹角A_i取值范围为[0,2π)，当超过180度是减去2π的倍数，具体的当前夹角的计算公式为：

A_i＝A_i-1+a_i，(A_i-1+a_i)＜2π；

其中，A_i为连杆i与水平面的目标角度，A_i-1为连杆i-1与水平面的目标角度，a_i为旋转电机i的当前转角，m为

取整数部分的值。

步骤S13，获取每个所述连杆的长度和重量，以及每个所述连杆的重心到其基部转轴的距离，并根据每个所述连杆的当前夹角、每个所述连杆的长度和每个所述连杆的重量，以及每个所述连杆的重心到其基部转轴的距离计算各个所述旋转电机的重力补偿力矩。

该力反馈机械臂中各个连杆的重量和长度，以及连杆的重心位置的测定一般常在三维模型中采用有限元或积分运算得出。该连杆的基部通过转轴与旋转电机的定子连接，其连杆的重心到基部转轴的距离也可测量得到。

根据每个连杆的当前夹角、每个连杆的重量和连杆的重心到其基部转轴的距离可计算旋转电机的重力补偿力矩，具体计算时，先计算连杆j对电机i的重力力臂长度L_ij，其计算公式为：

L_k为连杆的长度，l_j为连杆自身重心到其基部转轴的距离。其中，连杆的数量与旋转电机的数量相等均为n，i和j的取值范围为1，2，3，…，n。

表示从第i根连杆到j-1连杆的杆长在水平方向的投影长度；ι_j×cos(A_j)表示第j连杆的重心距离在水平方向的投影。

旋转电机的重力补偿力矩M_i的计算公式为：

其中，G_i为连杆重量。G_i×ι_j×cos(A_j)表示第i连杆的重力在第i旋转电机上产生的力矩，

表示从第i+1连杆到最后一根连杆的重力在第i电机上产生的力矩。

步骤S14，将各个所述补偿力矩分别发送至对应的所述旋转电机，以使各个所述旋转电机根据接收到的补偿力矩进行输出力矩补偿。

将算得的补偿力矩发送给对应的旋转电机，通过上述步骤不断循环执行，可对连杆自重对各旋转电机形成的力矩进行动态补偿。

本实施例中补偿力矩的计算误差可来源于三个方面，分别是重量测定误差，重心位置误差，角度测量误差。市售三级电子称的检定分度数为1000-10000，重量测量误差可据此计为ε₁＝1‰。机械加工零件重心位置的测定一般常在三维模型中采用有限元或积分运算得出，其误差来源于圆整计算，在此亦估算为

ε₂＝1‰。角度测量误差取决于编码器精度，目前市售伺服电机编码器精度可达16位，也即误差为ε₃＝1/65536。总体精度可计算为：∏(1+ε_i)-1，约为2‰。由此可中，本输实施例中的补偿力矩的计算误差小，精度较高。

本实施例提出了一种基于软件控制算法的重力补偿方法，采用该方法不仅可以对机械臂的重力转矩实施精确补偿，同时还可允许机械臂转至任何方向。本计算方法运算速度快精度高，且不会有累积误差，可方便的部署于各类嵌入式系统。

为了清除的了解本发明的原理，下面以两连杆的力反馈机械臂为例说明本发明的重力补偿算法。如图2所示，该力反馈机械臂的第一连杆30通过第一旋转电机20与基座10连接，该第一旋转电机20的定子固定在基座10上，该第一旋转电机20的输出轴与第一连杆30的基部固定连接，该第一连杆30的稍部与第二旋转电机40的定子固定连接，该第二旋转电机40的输出轴与第二连杆50的基部固定连接。

其中，第一旋转电机20和第二旋转电机40的重力补偿力矩计算方法为：

获取第一电机20和第二电机40的电机编码器的当前输出值，分别为e₁和e₂；获取第一连杆30和第二连杆50的重量，分别为G₁和G₂；获取第一连杆20和第二连杆50的长度，分别为L₁和L₂；分别获取第一连杆30和第二连杆50的重心到其基部转轴的距离，分别为l₁和l₂。

获取第一旋转电机20在预设转角a₁₀时，力反馈机械臂在力补偿状态下各电机编码器的标准输出值e₁₀。并根据e₁、a₁₀、e₁₀，以及第一旋转电机20的编码器最大输出E₁计算所述旋转电机的当前转角，其计算公式为：

其中，第二旋转电机40的角度a₂计算方法与第一旋转电机20的相同，此处不予赘述。

根据第一旋转电机20的当前转角计算第一连杆30与水平面的夹角：

A₁＝A₀+a₁，A₁＜2π；

上式中A₀取值为0。

根据第二旋转电机40的当前转角计算第二连杆50与水平面的夹角：

当(A₁+a₂)＜2π时，A₂＝A₁+a₂；

当(A₁+a₂)＞2π时，

根据上述获取和计算得到的数据计算第一旋转电机20和第二旋转电机40的重力补偿力矩。其中，

第一旋转电机20的重力补偿力矩的计算公式为：

M₁＝G₁×ι₁×cos(A₁)+G₂×(L₁×cosA₁+ι₂×cosA₂)。

第二旋转电机40的重力补偿力矩计算公式为：

M₂＝G₂×ι₂×cos(A₂)。

请参阅图3，为本发明第二实施例中的力反馈机械臂的重力补偿装置，包括：

转角模块10，用于实时获取各个旋转电机中电机编码器的输出值，并根据所述旋转电机中编码器的当前输出值确定对应的当前转角，其中，所述电机连接在相邻两个连杆的铰接处，所述旋转电机的定子与其中一所述连杆的稍部通过转轴固定连接，所述旋转电机的输出轴与另一所述连杆的基部固定连接；

第一计算模块20，用于根据每个所述旋转电机的当前转角计算与其输出轴连接的所述连杆与水平方向的当前夹角；

配置模块30，用于获取每个所述连杆的重量和每个所述连杆的重心到其基部转轴的距离；

第二计算模块40，用于根据每个所述连杆的当前夹角、每个所述连杆的重量和每个所述连杆的重心到其基部转轴的距离计算各个所述旋转电机的重力补偿力矩；

执行模块50，用于将各个所述补偿力矩分别发送至对应的所述旋转电机，以使各个所述旋转电机根据接收到的补偿力矩进行输出力矩补偿。

进一步的，上述重力补偿装置，其中，所述转角模块具体用于：

获取所述旋转电机在预设转角时，在力补偿状态下所述电机编码器的标准输出值；

根据所述旋转电机中编码器的当前输出值、所述预设转角，以及所述标准输出值计算所述旋转电机的当前转角。

进一步的，上述重力补偿装置，其中，所述旋转电机的当前转角的计算公式为：

其中，a_i为旋转电机的当前转角，e_i为电机编码器的当前输出值，e_i0为电机编码器对应的标准输出值，a_i0为所述预设转角，E_i为电机编码器的最大输出值。

进一步的，上述重力补偿装置，其中，所述第二计算模块具体用于：

根据当前连杆的当前夹角和所述当前连杆的重心到其基部转轴的距离计算所述当前连杆相对于所述当前连杆之前的每一所述旋转电机的重力力臂长度；

根据所述重力力臂长度以及所述当前连杆的当前夹角计算所述旋转电机的重力补偿力矩。

本发明实施例所提供的重力补偿装置，其实现原理及产生的技术效果和前述方法实施例相同，为简要描述，装置实施例部分未提及之处，可参考前述方法实施例中相应内容。

在流程图中表示或在此以其他方式描述的逻辑和/或步骤，例如，可以被认为是用于实现逻辑功能的可执行指令的定序列表，可以具体实现在任何计算机可读介质中，以供指令执行系统、装置或设备(如基于计算机的系统、包括处理器的系统或其他可以从指令执行系统、装置或设备取指令并执行指令的系统)使用，或结合这些指令执行系统、装置或设备而使用。就本说明书而言，“计算机可读介质”可以是任何可以包含、存储、通信、传播或传输程序以供指令执行系统、装置或设备或结合这些指令执行系统、装置或设备而使用的装置。

计算机可读介质的更具体的示例(非穷尽性列表)包括以下：具有一个或多个布线的电连接部(电子装置)，便携式计算机盘盒(磁装置)，随机存取存储器(RAM)，只读存储器(ROM)，可擦除可编辑只读存储器(EPROM或闪速存储器)，光纤装置，以及便携式光盘只读存储器(CDROM)。另外，计算机可读介质甚至可以是可在其上打印所述程序的纸或其他合适的介质，因为可以例如通过对纸或其他介质进行光学扫描，接着进行编辑、解译或必要时以其他合适方式进行处理来以电子方式获得所述程序，然后将其存储在计算机存储器中。

应当理解，本发明的各部分可以用硬件、软件、固件或它们的组合来实现。在上述实施方式中，多个步骤或方法可以用存储在存储器中且由合适的指令执行系统执行的软件或固件来实现。例如，如果用硬件来实现，和在另一实施方式中一样，可用本领域公知的下列技术中的任一项或他们的组合来实现：具有用于对数据信号实现逻辑功能的逻辑门电路的离散逻辑电路，具有合适的组合逻辑门电路的专用集成电路，可编程门阵列(PGA)，现场可编程门阵列(FPGA)等。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

以上所述实施例仅表达了本发明的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对本发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。因此，本发明专利的保护范围应以所附权利要求为准。

Claims (2)

1.一种重力补偿方法，其特征在于，包括：

实时获取各个旋转电机中电机编码器的输出值，并根据所述旋转电机中编码器的当前输出值确定对应的当前转角，其中，所述电机连接在相邻两个连杆的铰接处，所述旋转电机的定子与其中一所述连杆的稍部通过转轴固定连接，所述旋转电机的输出轴与另一所述连杆的基部固定连接；

根据每个所述旋转电机的当前转角计算与其输出轴连接的所述连杆与水平方向的当前夹角；

获取每个所述连杆的长度和重量，以及每个所述连杆的重心到其基部转轴的距离，并根据每个所述连杆的当前夹角、每个所述连杆的长度和每个所述连杆的重量，以及每个所述连杆的重心到其基部转轴的距离计算各个所述旋转电机的重力补偿力矩；

将各个所述补偿力矩分别发送至对应的所述旋转电机，以使各个所述旋转电机根据接收到的补偿力矩进行输出力矩补偿；

所述并根据每个所述连杆的当前夹角、每个所述连杆的重量和每个所述连杆的重心到其基部转轴的距离计算各个所述旋转电机的重力补偿力矩的步骤包括：

根据当前连杆的当前夹角和所述当前连杆的重心到其基部转轴的距离计算所述当前连杆相对于所述当前连杆之前的每一所述旋转电机的重力力臂长度；

根据所述重力力臂长度以及所述当前连杆的当前夹角计算所述旋转电机的重力补偿力矩；

所述旋转电机的重力补偿力矩的计算公式为：

其中，M_i为重力补偿力矩，L_k为第k个连杆的长度，l_j和l_i分别为第j个和第i个连杆的重心到其基部转轴的距离，G_i为第i个连杆的重量，L_ij为连杆j对电机i的重力力臂长，cos(A_k)为第k个连杆的当前夹角，cos(A_j)为第j个连杆的当前夹角；

所述根据所述旋转电机中编码器的当前输出值确定对应的当前转角的步骤包括：

获取所述旋转电机在预设转角时，在力补偿状态下所述电机编码器的标准输出值；

根据所述旋转电机中编码器的当前输出值、所述预设转角，以及所述标准输出值计算所述旋转电机的当前转角；

所述旋转电机的当前转角的计算公式为：

其中，a_i为旋转电机的当前转角，e_i为电机编码器的当前输出值，e_i0为电机编码器对应的标准输出值，a_i0为所述预设转角，E_i为电机编码器的最大输出值；

所述连杆的当前夹角的计算公式为：

A_i＝A_i-1+a_i，(A_i-1+a_i)＜2π；

其中，A_i为连杆i与水平面的当前夹角，A_i-1为连杆i-1与水平面的当前夹角，a_i为旋转电机i的当前转角，m为

取整数部分的值。

2.一种力反馈机械臂的重力补偿装置，其特征在于，包括：

转角模块，用于实时获取各个旋转电机中电机编码器的输出值，并根据所述旋转电机中编码器的当前输出值确定对应的当前转角，其中，所述电机连接在相邻两个连杆的铰接处，所述旋转电机的定子与其中一所述连杆的稍部通过转轴固定连接，所述旋转电机的输出轴与另一所述连杆的基部固定连接；

第一计算模块，用于根据每个所述旋转电机的当前转角计算与其输出轴连接的所述连杆与水平方向的当前夹角；

配置模块，用于获取每个所述连杆的重量和每个所述连杆的重心到其基部转轴的距离；

第二计算模块，用于根据每个所述连杆的当前夹角、每个所述连杆的重量和每个所述连杆的重心到其基部转轴的距离计算各个所述旋转电机的重力补偿力矩；

执行模块，用于将各个所述补偿力矩分别发送至对应的所述旋转电机，以使各个所述旋转电机根据接收到的补偿力矩进行输出力矩补偿；

所述第二计算模块具体用于：

根据当前连杆的当前夹角和所述当前连杆的重心到其基部转轴的距离计算所述当前连杆相对于所述当前连杆之前的每一所述旋转电机的重力力臂长度；

根据所述重力力臂长度以及所述当前连杆的当前夹角计算所述旋转电机的重力补偿力矩；

所述旋转电机的重力补偿力矩的计算公式为：

其中，M_i为重力补偿力矩，L_k为第k个连杆的长度，l_j和l_i分别为第j个和第i个连杆的重心到其基部转轴的距离，G_i为第i个连杆的重量，L_ij为连杆j对电机i的重力力臂长，cos(A_k)为第k个连杆的当前夹角，cos(A_j)为第j个连杆的当前夹角；

所述转角模块具体用于：

获取所述旋转电机在预设转角时，在力补偿状态下所述电机编码器的标准输出值；

根据所述旋转电机中编码器的当前输出值、所述预设转角，以及所述标准输出值计算所述旋转电机的当前转角；

所述旋转电机的当前转角的计算公式为：

其中，a_i为旋转电机的当前转角，e_i为电机编码器的当前输出值，e_i0为电机编码器对应的标准输出值，a_i0为所述预设转角，E_i为电机编码器的最大输出值；

所述连杆的当前夹角的计算公式为：

A_i＝A_i-1+a_i，(A_i-1+a_i)＜2π；

其中，A_i为连杆i与水平面的当前夹角，A_i-1为连杆i-1与水平面的当前夹角，a_i为旋转电机i的当前转角，m为

取整数部分的值。

Images