CN108582078A - 一种面向直接示教的机械臂零力控制方法 - Google Patents

Abstract

本发明公开了一种面向直接示教的机械臂零力控制方法，包括：S1、建立具有n个柔性关节的n自由度机械臂动力学模型；S2、构建所述机械臂的控制器，所述控制器用于进行重力补偿以及降低惯性力和摩擦力对所述柔性关节的影响；S3、辨识所述控制器中的重力项。本发明中的控制方法不仅可以补偿机械臂的重力，而且可以减小关节的自身的惯性力和摩擦力的影响，且方法简单，计算量较小，便于实现。

Description

一种面向直接示教的机械臂零力控制方法

技术领域

本发明涉及机器人控制技术领域，尤其是涉及一种面向直接示教的机械臂零力控制方法。

背景技术

随着机器人技术的发展，机器人在社会生产、生活服务等各方面扮演着越来越重要的角色。在一些领域中机器人已经能够代替人类从事一些繁重、重复、危险的活动，从而加快了自动化进程。近年来国内对工业机器人的应用需求也呈快速增长的趋势，在“工业4.0”和“中国制造2025”的背景下，为了适应现代工业快速多变的特点以及满足日益增长的复杂性要求，机器人不仅要能长期稳定地完成重复工作，还要具备智能化、网络化、开放性、人机友好性的特点。

然而，目前的大多数工业机器人都工作于和人类分开的空间中，人类只能通过示教板或者编程来使其实现特定的轨迹。由此引发了人们对于人机互动机器人的兴趣，产生了一种新的示教形式，能够使人和机器人在同一空间内进行人机互动，即直接示教技术。人机互动是工业机器人发展的一个重要领域，而直接示教是人机互动的一个重要发展方向，也是工业机器人继续发展与创新的一个重要方面，所以直接示教技术正在向利于快速示教编程和增强人机协作能力的方向发展。

工业机器人的示教就是操作者采用各种示教方法事先“告知”机器人所要进行的动作信息和作业信息等，这些信息大致分为四类：1)机器人位置和姿态信息，轨迹和路径点的信息；2)机器人任务动作顺序信息；3)机器人动作、作业时的附加条件信息；4)机器人的作业内容和机器人动作的速度、加速度等信息。

现有的机器人示教大致可分为实际机器人示教和虚拟机器人示教两大类，而在实际应用中机器人示教方式广泛使用的是示教盒示教，几乎所有的工业机器人都会配备专门的示教盒。示教盒示教是指通过操纵机器人专配的示教盒上的按键等可操作按钮，实现远距离控制机器人，来控制其完成预定的各种动作以及定位。这种示教方式除了能够对机器人的位置和姿态进行示教，还可以实现机器人的动作顺序以及作业条件内容等的示教。然而，使用该方式进行示教，操作机器人很难连续精确地沿着复杂的运动轨迹进行运动，一般只能对运动轨迹上的若干点进行示教，然后通过插补算法计算出示教点中间的轨迹。可以看出，该方法要求操作者具有一定的机器人技术知识和经验，示教效率较低，另外对于需要实现复杂运动轨迹的示教来说，该方式也同样不适用。

直接示教是相对于示教盒示教而言的，即由人手直接拖动机器人手臂，使机器人按照人的意愿完成特定动作的一种示教方式。根据示教过程中关节电动机的状态不同，直接示教可分为功率级脱离示教和伺服级接通示教。功率级脱离示教即机器人关节电动机处于自由状态，由人直接搬动机器人手臂，使机器人沿着人们设定的轨迹运动；伺服级接通示教是机器人关节处于被控制状态下的一种示教方式。由于功率级脱离示教的劳动强度大，因而工业机器人的直接示教普遍采用伺服级接通示教，并且常在机器人末端加装多位传感器来传达操作者的示教意图，引导机器人运动。与示教盒示教相比，直接示教法可以无需操作者过多掌握机器人的相关知识及经验，操作简单且快速，使得示教过程高效、实用。

在直接示教过程中，为了让操作人员使用较小的力来完成对机器臂的牵引示教，因此常采用零力控制方法，即采用各种算法对机械臂进行自身重力、摩擦力、惯性力等的补偿，使机械臂顺应外力作用运动，就好像它处在一个不受重力和摩擦力等环境下的控制。

例如，日本研究者Tungpataratanawong等人于2005年针对工业机器人提出了一种基于阻抗控制策略的免力矩传感器的方案来处理机器人与环境(包括人)接触时的问题。根据其实验结果，该方案可以快速并准确检测到外力，并调整位置环和速度环给定，从而使机器人在操作者用手拖动机械臂时顺应操作者意图到达目标点，在此基础上可以完成免力矩传感器的机器人直接示教。然而，由于该方案原理是基于笛卡尔空间阻抗控制，因此只能实现对机器人末端位置的示教，而不能实现机器人任意姿态的示教。此外，由于其观测器和控制器设计比较复杂，需要对原机器人控制器作相应改变，实现起来也有一定难度。

再如，Goto等人提出的FFC方法中，提出基于力矩控制的零力控制方法来实现对轻型机器人的直接示教。研究中作者采用的是重力与摩擦力补偿，并提出了重力矩的自测量方案以及摩擦力矩的计算方案，在自制的小型平面二自由度机器人样机上实验表明该力矩补偿效果较明显。然而，作者提出的重力矩自测量方案和摩擦力矩计算方案都缺乏严格的理论推导，且没有推广到空间多自由度机器人；再者，该方案的本质是对机器人作直接力矩控制，通过力矩补偿来实现直接示教，力矩控制会降低系统稳定性；另外，从对动力学方程的分析可以知道，机器人示教运动中的惯性力、耦合项力都需由操作者克服，因而该方案不适用于自重较大的机器人。

再如，一种基于位置控制的零力控制系统，如图1所示，图1中K_P为伺服控制器的位置环增益，K_V为伺服控制器的速度环增益，K_T为伺服控制器的转矩常量，M_g为各关节对应重力矩，M_f为各关节对应摩擦力矩，M_F为外力等效到各关节的力矩，q_d为各关节位置指令值，q为各关节的旋转角度，表示各关节的角速度，表示各关节的角加速度，s表示微分环节。此例中，电动机输入力矩零力控制时各关节位置指令值从而，实时计算q_d的值可实现基于位置控制的零力控制算法。该方案中提出的基于位置控制的零力控制方法，需要采用外部的多维力矩传感器来感知机械臂末端的外力，从而控制机械臂各关节的位置来达到零力控制的效果。但是多维力矩传感器安装在机械臂的末端影响了机械臂正常使用空间，操作者只能通过牵引机械臂末端力矩传感器才能对机械臂进行示教，并且基于位置控制的零力控制方法的计算量比较复杂，除此之外还需要对机械臂进行复杂的动力学建模以及模型参数的辨识，无法排除一些非线性因素的影响。

再比如，一种基于力矩控制免力矩传感器的零力控制方法，其控制系统框图如图2所示，将关节电流近似等于关节力矩，建立机械臂的动力学模型，补偿重力、摩擦力达到零力控制的效果，并在二连杆上做相应的实验验证。其中，电动机输出转矩外力等效到各关节的力矩此例中，T_s表示各关节电机的输入力矩，D表示摩擦力矩阵，H为惯性项矩阵，g表示重力力矩，h表示非线性项。但该方案存在诸多缺陷，只是简单地补偿了机器人的重力矩和摩擦力矩，没有补偿惯性力项、柯氏力项、离心力项等，只能适用于轻型低速的机器人直接示教，存在很大的局限性；采用电流信号来近似代替负载力矩，虽然减少力矩传感器降低了成本，但其精度得不到保证，尤其在关节运动时两者差异加大；相关理论的验证只是在简单的二关节模型上得到验证，相关计算方案缺乏严格的理论推导，并且没有拓展到空间多自由度机器人，缺乏实验验证；需要对机械臂进行复杂的动力学建模以及模型参数的辨识，无法排除或减小一些非线性因素的影响。

以上背景技术内容的公开仅用于辅助理解本发明的发明构思及技术方案，其并不必然属于本专利申请的现有技术，在没有明确的证据表明上述内容在本专利申请的申请日前已经公开的情况下，上述背景技术不应当用于评价本申请的新颖性和创造性。

发明内容

为此，本发明提出了一种面向直接示教的机械臂零力控制方法，以解决上述机器人零力控制方案中所存在的控制精度不高、空间自由度适用性低、直接示教灵活性低的问题。

本发明为解决上述问题所提出的技术方案如下：

一种面向直接示教的机械臂零力控制方法，包括以下步骤：

S1、建立具有n个柔性关节的n自由度机械臂动力学模型；

S2、构建所述机械臂的控制器，所述控制器用于进行重力补偿以及降低惯性力和摩擦力对所述柔性关节的影响；

S3、辨识所述控制器中的重力项。

优选地，所述机械臂动力学模型为

其中：

矩阵M(q)表示机械臂的惯性项，且M(q)∈R^n×n；

矩阵表示机械臂的哥氏项，且

K表示n个柔性关节的刚度系数矩阵；

矩阵G表示机械臂柔性关节的重力项，且G∈Rⁿ；

q、分别表示n个柔性关节的旋转角度矩阵、角速度矩阵、角加速度矩阵，且q∈R^n×n；

T表示机械臂柔性关节的力矩传感器测量值矩阵，且T∈Rⁿ；

T_ext表示外部环境给n个柔性关节电机末端所施加的力矩的矩阵；

θ表示n个柔性关节的电机转子转动角度的矩阵；

表示n个柔性关节的电机转子转动加速度的矩阵，且

J表示n个柔性关节的电机转子转动惯量矩阵；

T_f、T_m分别表示所述柔性关节的摩擦力矩矩阵和电机输入力矩矩阵，且T_f,T_m∈Rⁿ。

优选地，步骤S1中基于所述柔性关节的动力学模型来构建所述机械臂动力学模型，所述柔性关节的动力学模型为

其中：

M表示与柔性关节相连的下一连杆及负载的转动惯量，θ表示柔性关节的电机转子转动角度，表示柔性关节的电机转子转动加速度；

为库伦摩擦力，T_v·θ为粘滞摩擦力；

优选地，步骤S2构建的所述控制器的模型为

T_m＝T+K_t·(G-T)

其中，K_t＝diag(K_t1,K_t2,L,K_tn)，K_ti表示第i个柔性关节的控制系数，i＝1,2,…,n。

优选地，根据所述控制器的模型，得到

从而，当各个柔性关节的控制系数K_ti＞1时，外力所克服的电机自身的惯性力和摩擦力减小为原来的

优选地，步骤S3中辨识的重力项是机械臂在不同位姿状态下的实时重力项。

优选地，所述实时重力项的计算过程如下：

当各柔性关节的旋转角度确定后，各柔性关节所受到的重力矩也唯一确定，即

G＝G₀·F(q)

n个柔性关节的重力矩矩阵G＝(G₁,G₂,L,G_n)^T，G_i表示第i个柔性关节的重力矩；

q＝(q₁,q₂,L,q_n)^T，q_i表示第i个柔性关节的旋转角度；

F(q)＝(F₁(q),F₂(q),···,F_n(q))^T表示仅与关节旋转角度有关的n×n维矢量矩阵；

其中，为常量；

G_ii为1×n维矩阵，i＝1,2,…,n，矩阵中各元素为与机械臂重力项有关的几何特征参数常量，当某一G_ii中的元素为0时，将所述某一G_ii所在的列舍弃，同时将F(q)中与所述某一G_ii所在的列对应的行舍弃，得到

其中，n≤k≤n²；g_ij表示需要求的未知量G₀中的元素，j＝1,2,…,k；f_j(q)表示仅与柔性关节的旋转角度有关的关系式；

对每个柔性关节，取k个不同的位姿q_ix，x＝1,2,L,k，q_ix表示第i个柔性关节的第x个位姿，使得F(q_ix)＝(f₁(q_i1),f₂(q_i2),f₃(q_i3)···,f_k(q_ik))^T中的各元素线性无关；

对第1个柔性关节，测量与k个位姿对应的k个关节转矩T_1x，T_1x即为k个位姿对应的k个重力矩G_1x，从而得到对应于k个不同位姿的k组线性无关方程，求解该线性无关方程即得到G₀中的第1行元素的值；采用同样的方法得到G₀中其它各行元素的值，从而求出G₀，当各柔性关节的旋转角度确定后，即F(q)已知时，由公式G＝G₀·F(q)计算实时重力项。

本发明提出的上述技术方案，与现有技术相比，具有以下有益效果：

1)采用关节力矩传感器来直接采集各柔性关节的力矩，解决了电流信号来近似代替负载力矩所带来的精确度低的问题；

2)适用于空间多自由度机器人的机械臂零力控制；

3)采用基于关节力矩的零力控制方法，操作者可直接拖动机械臂的任何位置来进行示教，提高了示教的灵活性，解决了只能拖动机械臂末端进行示教的局限性问题；

4)本发明中的控制方法不仅可以补偿机械臂的重力，而且可以减小关节的自身的惯性力和摩擦力的影响，且方法简单，计算量较小，便于实现。

附图说明

图1是一种现有的基于位置控制的零力控制系统原理框图；

图2是另一种现有的基于力矩控制免力矩传感器的零力控制系统原理框图；

图3是本发明具体实施例的零力控制方法中的三自由度机械臂示意图；

图4是图3所示例的机械臂的柔性关节简化模型；

图5是本发明具体实施例的零力控制方法中机械臂简化模型。

具体实施方式

下面结合附图和具体的实施方式对本发明作进一步说明。

本发明的具体实施方式提供一种面向直接示教的机械臂零力控制方法，包括以下步骤S1至S3：

步骤S1、建立具有n个柔性关节的n自由度机械臂动力学模型。该步骤具体包括：

首先，建立单个柔性关节的数学模型，如下式(1)：

参考图4，该单个柔性关节中的谐波减速器和力矩传感器等柔性元件可看成刚度系数为K的弹簧，M表示与该柔性关节相连的下一连杆及负载的转动惯量，J表示该柔性关节电机转子的转动惯量，电机输入力矩T_m使电机转子转动角度为θ，表示柔性关节的电机转子转动加速度，经过谐波减速器和力矩传感器等柔性元件将力矩T传输到该柔性关节的输出端带动连杆及负载转动，柔性关节旋转角度为q，表示柔性关节的角加速度，其中力矩T也是该柔性关节中力矩传感器的测量值；T_f表示该柔性关节电机的摩擦力矩，本发明中考虑库伦摩擦力和粘滞摩擦力，因此为库伦摩擦力，T_v·θ为粘滞摩擦力；当柔性关节处于脱功率静止状态时，使柔性关节运动的最小外力T_ext即需要克服的电机转子自身惯性力、连杆负载的惯性力以及柔性关节的摩擦力，为

其次，根据Spong等人对柔性关节动力学模型的假设和Ott等人对该动力学模型的扩展，可将式(1)扩展为具有n个这样的柔性关节的机械臂动力学模型，如下：

在上述公式(2)中，需要注意的是，加粗字体的字母表示矩阵/向量。其中，矩阵M(q)表示该机械臂的惯性项，M(q)∈R^n×n，R^n×n表示n×n维实数矩阵；矩阵表示机械臂的哥氏项，K是机械臂的柔性关节刚度系数向量，具有n个元素，每个元素分别代表一个柔性关节的刚度系数K，n个不同的刚度系数K(以下标区分)构成刚度系数向量K，即K＝(K₁,K₂,…,K_n)；矩阵G表示机械臂柔性关节的重力项，同样也是由n个柔性关节的重力项构成，G∈Rⁿ，Rⁿ表示n×1维实数矩阵；q、分别表示n个柔性关节的旋转角度矩阵、角速度矩阵、角加速度矩阵，且q∈R^n×n；T表示机械臂柔性关节的力矩传感器测量值，同样地，矩阵T也由n个力矩传感器测量值构成，且T∈Rⁿ；矩阵T_ext表示外部环境给n个柔性关节电机末端所施加的力矩构成的矩阵；矩阵θ表示n个柔性关节的电机转子转动角度构成的矩阵；矩阵表示n个柔性关节的电机转子转动加速度构成的矩阵，且矩阵J表示n个柔性关节的电机转子转动惯量构成的矩阵；矩阵T_f、T_m分别表示n个柔性关节的摩擦力矩矩阵和电机输入力矩矩阵，且T_f,T_m∈Rⁿ。

步骤S2、构建所述机械臂的控制器，所述控制器用于进行重力补偿以及降低惯性力和摩擦力对所述柔性关节的影响。当机械臂处于低速运动状态时，由式(2)可知，机械臂连杆负载受重力影响较大；为了使机械臂在较小外力的作用下运动，控制器需要对机械臂在不同位姿状态下的连杆负载重力进行补偿，并尽可能地减小机械臂自身惯性力和摩擦力的影响。因此，本发明提出了如下的控制器：

T_m＝T+K_t·(G-T) (3)

其中，K_t＝diag(K_t1,K_t2,L,K_tn)，K_ti表示第i个柔性关节的控制系数，i＝1,2,…,n。

将式(3)代入式(2)，得到：

由式(4)可知，机械臂的重力项已由该控制器补偿，但由于机械臂在不同的位姿状态下的重力项不同，所以控制器中补偿的重力项需要进行实时的重力辨识计算，从而补偿掉机械臂的重力影响；另外，从式(4)可知，当各个柔性关节的控制系数K_ti＞1时，外力所克服的电机自身的惯性力和摩擦力也减小为原来的

步骤S3、辨识所述控制器中的重力项。即计算控制器模型中未知的实时重力项，具体过程如下：

当各柔性关节的旋转角度确定后，各柔性关节所受到的重力矩也唯一确定：

G＝G₀·F(q)(5)

式(5)中，G表示n个柔性关节的重力矩的矩阵，G＝(G₁,G₂,L,G_n)^T，G_i表示第i个柔性关节的重力矩；n个柔性关节的旋转角度向量q＝(q₁,q₂,L,q_n)^T，q_i表示第i个柔性关节的旋转角度；F(q)＝(F₁(q),F₂(q),···,F_n(q))^T表示仅与关节旋转角度有关的n×n维矢量矩阵；

为常量，其中G_ii为1×n维矩阵，i＝1,2,…,n，矩阵中各元素为与机械臂重力项有关的几何特征参数常量，G_ii中的元素可能为0，当某一G_ii中的元素为0时，可G₀中该G_ii所在的列舍弃，同时将F(q)中与该G_ii所在的列对应的行舍弃，得到：

式(6)中，n≤k≤n²；g_ij即为需要求的未知数G₀中的元素，j＝1,2,…,k；f_j(q)表示仅与柔性关节旋转角度有关的关系式。

当机械臂各关节的旋转角度确定时，我们只需要得知G₀即可求出各关节所对应重力项。要求出G₀，首先，对每个柔性关节，取k个不同的位姿q_ix，x＝1,2,L,k，q_ix表示第i个柔性关节的第x个位姿，使得F(q_ix)＝(f₁(q_i1),f₂(q_i2),f₃(q_i3)···,f_k(q_ik))^T中的各元素线性无关；其次，对第1个柔性关节，测量与k个位姿对应的k个关节转矩T_1x，T_1x即为k个位姿对应的k个重力矩G_1x，从而得到对应于k个不同位姿的k组线性无关方程，求解该线性无关方程即得到G₀中的第1行元素的值；采用同样的方法得到G₀中其它各行元素的值，从而求出G₀，当各柔性关节的旋转角度确定后，即F(q)已知时，由公式G＝G₀·F(q)计算实时重力项。

下面通过如图3所示的3自由度机械臂来对本发明的前述控制方法进行进一步的阐述。

该3自由度机械臂设计的控制器如下，即式(3)：

T_m＝T+K_t·(G-T)

其中：

T_m1、T_m2、T_m3分别表示柔性关节1、2、3的关节电机输入力矩；

T₁、T₂、T₃分别表示柔性关节1、2、3的力矩传感器测量值；

K_t＝diag(K_t1,K_t2,K_t3)，K_t1、K_t2、K_t3分别表示柔性关节1、2、3的控制系数，且均大于1；

G₁、G₂、G₃分别表示3个柔性关节实际所受到的重力矩。

由图3可知，柔性关节1不受重力矩的影响，即G₁＝0，所以可以将图3所示的机械臂简化为图5所示的简化模型。

由图5可知，柔性关节2所承受的重力矩G₂为：

G₂＝(M₂y₂+M₃d₂+M_Ld₂)·sinq₂+(M₃y₃+M_Ld₃)·sin(q₂+q₃) (7)

柔性关节3所承受的重力矩G₃为：

G₃＝(M₃y₃+M_Ld₃)·sin(q₂+q₃) (8)

根据式(3)、(6)和(7)可得：

G＝G₀·F(q) (9)

本例中：

常量矩阵G₀的求法：

情况一、令q₂＝90°，q₃＝90°，则

情况二、令q₂＝0°，q₃＝90°，则

当机械臂在以上两种情况下静止时，由于各柔性关节力矩传感器的测量值为各关节所受的重力项，所以有

其中：

T₂为机械臂在上述情况一时，柔性关节2所受的重力矩测量值；T₂'、T₃'分别为机械臂在情况二时关节2、关节3所受的重力矩测量值。

综合式(9)～(14)可得：

最后，根据式(3)、(9)及(15)便可得出，本实施例的3自由度机械臂在不同位姿的情况下各个关节所需要输出的力矩，从而补偿掉重力矩，并减小惯性力和摩擦力对系统的影响。本实施例采用空间3自由度机械臂进行实验论证并适用于空间多自由度机械臂，解决了相关理论的验证只是在简单的平面二关节模型上得到验证，没有拓展到空间多自由度机器人的问题。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的技术人员来说，在不脱离本发明构思的前提下，还可以做出若干等同替代或明显变型，而且性能或用途相同，都应当视为属于本发明的保护范围。

Claims (7)

1.一种面向直接示教的机械臂零力控制方法，包括以下步骤：

S1、建立具有n个柔性关节的n自由度机械臂动力学模型；

S2、构建所述机械臂的控制器，所述控制器用于进行重力补偿以及降低惯性力和摩擦力对所述柔性关节的影响；

S3、辨识所述控制器中的重力项。

2.如权利要求1所述的机械臂零力控制方法，其特征在于：所述机械臂动力学模型为

其中：

矩阵M(q)表示机械臂的惯性项，且M(q)∈R^n×n；

矩阵表示机械臂的哥氏项，且

K表示n个柔性关节的刚度系数矩阵；

矩阵G表示机械臂柔性关节的重力项，且G∈Rⁿ；

q、分别表示n个柔性关节的旋转角度矩阵、角速度矩阵、角加速度矩阵，且q∈R^{n ×n}；

T表示机械臂柔性关节的力矩传感器测量值矩阵，且T∈Rⁿ；

T_ext表示外部环境给n个柔性关节电机末端所施加的力矩的矩阵；

θ表示n个柔性关节的电机转子转动角度的矩阵；

表示n个柔性关节的电机转子转动加速度的矩阵，且

J表示n个柔性关节的电机转子转动惯量矩阵；

T_f、T_m分别表示所述柔性关节的摩擦力矩矩阵和电机输入力矩矩阵，且T_f,T_m∈Rⁿ。

3.如权利要求2所述的机械臂零力控制方法，其特征在于：步骤S1中基于所述柔性关节的动力学模型来构建所述机械臂动力学模型，所述柔性关节的动力学模型为

其中：

M表示与柔性关节相连的下一连杆及负载的转动惯量；

θ表示柔性关节的电机转子转动角度；

表示柔性关节的电机转子转动加速度；

为库伦摩擦力，T_v·θ为粘滞摩擦力；

4.如权利要求3所述的机械臂零力控制方法，其特征在于：步骤S2构建的所述控制器的模型为

T_m＝T+K_t·(G-T)

其中，K_t＝diag(K_t1,K_t2,…,K_tn)，K_ti表示第i个柔性关节的控制系数，i＝1,2,…,n。

5.如权利要求4所述的机械臂零力控制方法，其特征在于：根据所述控制器的模型，得到

从而，当各个柔性关节的控制系数K_ti＞1时，外力所克服的电机自身的惯性力和摩擦力减小为原来的

6.如权利要求5所述的机械臂零力控制方法，其特征在于：步骤S3中辨识的重力项是机械臂在不同位姿状态下的实时重力项。

7.如权利要求6所述的机械臂零力控制方法，其特征在于：所述实时重力项的计算过程如下：

当各柔性关节的旋转角度确定后，各柔性关节所受到的重力矩也唯一确定，即

G＝G₀·F(q)

n个柔性关节的重力矩矩阵G＝(G₁,G₂,…,G_n)^T，G_i表示第i个柔性关节的重力矩；

q＝(q₁,q₂,…,q_n)^T，q_i表示第i个柔性关节的旋转角度；

F(q)＝(F₁(q),F₂(q),…,F_n(q))^T表示仅与关节旋转角度有关的n×n维矢量矩阵；

为常量；

G_ii为1×n维矩阵，i＝1,2,…,n，矩阵中各元素为与机械臂重力项有关的几何特征参数常量，当某一G_ii中的元素为0时，将所述某一G_ii所在的列舍弃，同时将F(q)中与所述某一G_ii所在的列对应的行舍弃，得到

其中，n≤k≤n²；g_ij表示需要求的未知量G₀中的元素，j＝1,2,…,k；f_j(q)表示仅与柔性关节的旋转角度有关的关系式；

对每个柔性关节，取k个不同的位姿q_ix，x＝1,2,…,k，q_ix表示第i个柔性关节的第x个位姿，使得F(q_ix)＝(f₁(q_i1),f₂(q_i2),f₃(q_i3)…,f_k(q_ik))^T中的各元素线性无关；

对第1个柔性关节，测量与k个位姿对应的k个关节转矩T_1x，T_1x即为k个位姿对应的k个重力矩G_1x，从而得到对应于k个不同位姿的k组线性无关方程，求解该线性无关方程即得到G₀中的第1行元素的值；采用同样的方法得到G₀中其它各行元素的值，从而求出G₀，当各柔性关节的旋转角度确定后，即F(q)已知时，由公式G＝G₀·F(q)计算实时重力项。