一种机器人零力控制方法以及系统，方法包括：S1、基于惯性力、离心力、科里奥利力、粘摩擦力、静摩擦力、重力，建立机器人动力学模型；S2、基于所述机器人动力学模型以及反馈数据，计算机器人各个关节的外力力矩；S3、根据步骤S2中计算的外力力矩计算速度指令；S4、根据速度指令以及位置反馈计算位置指令。本发明通过建立动力学模型，可以直接计算各关节的外力力矩，无需借助力/转矩传感器，降低了系统成本和复杂性；采用位置指令控制的位置控制方式，而非采用直接转矩控制的方式，降低了系统安全性与稳定性的设计难度，另外，由于在计算外力力矩时考虑了惯性力，该发明还可适用于自重较大的机器人。
S1、基于惯性力、离心力、科里奥利力、粘摩擦力、静摩擦力、重力，建立机器人动力学模型；
S2、基于所述机器人动力学模型以及反馈数据，计算机器人各个关节的外力力矩；
其中，B(q)为N×N惯量矩阵，C1(q)为N×N离心力矩阵， 为N×N科里奥利力矩阵，Fv为N×N粘摩擦力矩阵，Fs为N×N静摩擦力矩阵，G(q)为N×1重力向量，τ为N×1关节力矩向量， 表示以 为自变量的符号函数，为加速度信息，为速度信息，q为位置信息，N为机器人关节数；
S21、将公式(1)中静摩擦力设为0，并将动力学模型中的 设为期望加速度 设为速度反馈 q设为位置反馈qf，得到如公式(2)所示的关节作用力力矩τnsf：S22、根据力矩常数Kt以及反馈电流If计算反馈力矩τf：KtIf＝τf；
S23、根据步骤S21中计算的关节作用力力矩τnsf以及步骤S22中计算的反馈力矩τf计算得到外力力矩τe：τf-τnsf＝τe。
2.根据权利要求1所述的机器人零力控制方法，其特征在于，所述步骤S3包括：S31、将公式(1)中静摩擦力以外的作用力设为0，并将动力学模型中的 设为速度反馈得到静摩擦力力矩τsf：S32、根据所述步骤S31中的静摩擦力力矩τsf以及步骤S21中的关节作用力力矩τnsf计算外力力矩死区作用域[τdn,τdp]，其中，τdn,τdp基于以下公式确定：其中，k1、k2表示力矩误差校正系数；
S33、根据步骤S23中计算得到的外力力矩τe，基于以下公式计算得到速度指令其中，k3为力矩速度映射系数，p∈(0,1)。
3.根据权利要求1所述的机器人零力控制方法，其特征在于，所述步骤S4基于以下公式计算得到位置指令qr；
其中，△t为位置插补周期，qf为位置反馈， 为速度指令。
动力学模型模块，用于基于惯性力、离心力、科里奥利力、粘摩擦力、静摩擦力、重力，建立机器人动力学模型；
外力力矩计算模块，用于基于所述动力学模型模块中的机器人动力学模型以及反馈数据，计算机器人各个关节的外力力矩；
力矩速度映射模块，用于根据外力力矩计算模块中计算的外力力矩计算速度指令；
速度位置映射模块，用于根据速度指令以及位置反馈计算位置指令；
其中，B(q)为N×N惯量矩阵，C1(q)为N×N离心力矩阵， 为N×N科里奥利力矩阵，Fv为N×N粘摩擦力矩阵，Fs为N×N静摩擦力矩阵，G(q)为N×1重力向量，τ为N×1关节力矩向量， 表示以 为自变量的符号函数，为加速度信息，为速度信息，q为位置信息，N为机器人关节数；
关节作用力力矩计算单元，用于将公式(1)中静摩擦力设为0，并将动力学模型中的 设为期望加速度 设为速度反馈 q设为位置反馈qf，得到如公式(2)所示的关节作用力力矩τnsf：反馈力矩计算单元，用于根据力矩常数Kt以及反馈电流If计算反馈力矩τf：KtIf＝τf；
外力力矩计算单元，用于根据关节作用力力矩计算单元计算的关节作用力力矩τnsf以及反馈力矩计算单元计算的反馈力矩τf计算得到外力力矩τe：τf-τnsf＝τe。
5.根据权利要求4所述的机器人零力控制系统，其特征在于，所述力矩速度映射模块包括：静摩擦力力矩计算单元，用于将公式(1)中静摩擦力以外的作用力设为0，并将动力学模型中的 设为速度反馈 得到静摩擦力力矩τsf：外力力矩死区作用域计算单元，用于根据所述静摩擦力力矩计算单元计算的静摩擦力力矩τsf以及关节作用力力矩计算单元计算的关节作用力力矩τnsf计算外力力矩死区作用域[τdn,τdp]，其中，τdn,τdp基于以下公式确定：其中，k1、k2表示力矩误差校正系数；
速度指令计算单元，用于根据外力力矩计算单元中计算得到的外力力矩τe，基于以下公式计算得到速度指令其中，k3为力矩速度映射系数，p∈(0,1)。
6.根据权利要求4所述的机器人零力控制系统，其特征在于，所述的根据速度指令以及位置反馈计算位置指令包括：基于以下公式计算得到位置指令qr；
其中，△t为位置插补周期，qf为位置反馈， 为速度指令。
机器人零力控制方法以及系统
技术领域
[0001] 本发明涉及零力控制领域，尤其涉及一种机器人零力控制方法以及系统。
背景技术
[0002] 目前工业机器人的示教过程主要依赖于示教器，这样的示教方式具有工作效率低、过程繁琐不直观、对操作人员知识水平要求高的特点。因此，有必要设计一种直观高效的机器人直接示教系统。直接示教系统的开发依赖于零力控制技术的实现。当前零力控制技术主要有两种实现方式：基于位置控制的零力控制技术和基于直接转矩控制的零力控制技术。
[0003] 基于位置控制的零力控制技术需借助于力/转矩传感器，测量出施加在机器人各关节的外力，然后将该外力与经动力学模型计算的力相加并转换为相应的位置指令。由于该方案需要力/转矩传感器，所以会极大提高系统的成本和复杂性。
[0004] 基于直接转矩控制的零力控制技术，首先通过动力学模型计算出各关节对应的重力及摩擦力，然后控制各关节输出对应大小的转矩，机器人在外力的作用下克服惯性力而运动。该方案不需要知道精确的加速度信息，但是由于其直接对力矩进行控制，对系统的稳定性设计提出了较大的挑战，同时，由于这样的实现方式不对惯性力做补偿，无法适用于自重较大的机器人。
发明内容
[0005] 本发明要解决的技术问题在于，针对现有技术的上述缺陷，提供一种机器人零力控制方法以及系统。
[0006] 本发明解决其技术问题所采用的技术方案是：构造一种机器人零力控制方法，包括：
[0007] S1、基于惯性力、离心力、科里奥利力、粘摩擦力、静摩擦力、重力，建立机器人动力学模型；
[0008] S2、基于所述机器人动力学模型以及反馈数据，计算机器人各个关节的外力力矩；
[0009] S3、根据步骤S2中计算的外力力矩计算速度指令；
[0010] S4、根据速度指令以及位置反馈计算位置指令；
[0011] 所述动力学模型为：
[0012]
[0013] 其中，B(q)为N×N惯量矩阵，C1(q)为N×N离心力矩阵， 为N×N科里奥利力矩阵，Fv为N×N粘摩擦力矩阵，Fs为N×N静摩擦力矩阵，G(q)为N×1重力向量，τ为N×1关节力矩向量， 表示以 为自变量的符号函数，为加速度信息，为速度信息，q为位置信息，N为机器人关节数；
[0014] 所述步骤S2包括：
[0015] S21、将公式(1)中静摩擦力设为0，并将动力学模型中的 设为期望加速度 设为速度反馈 q设为位置反馈qf，得到如公式(2)所示的关节作用力力矩τnsf：
[0016]
[0017] S22、根据力矩常数Kt以及反馈电流If计算反馈力矩τf：KtIf＝τf；
[0018] S23、根据步骤S21中计算的关节作用力力矩τnsf以及步骤S22中计算的反馈力矩τf计算得到外力力矩τe：τf-τnsf＝τe。
[0019] 在本发明所述的机器人零力控制方法中，所述步骤S3包括：
[0020] S31、将公式(1)中静摩擦力以外的作用力设为0，并将动力学模型中的 设为速度反馈 得到静摩擦力力矩τsf：
[0021] S32、根据所述步骤S31中的静摩擦力力矩τsf以及步骤S21中的关节作用力力矩τnsf计算外力力矩死区作用域[τdn,τdp]，其中，τdn,τdp基于以下公式确定：
[0022]
[0023] 其中，k1、k2表示力矩误差校正系数；
[0024] S33、根据步骤S23中计算得到的外力力矩τe，基于以下公式计算得到速度指令[0025]
[0026] 其中，k3为力矩速度映射系数，p∈(0,1)。
[0027] 在本发明所述的机器人零力控制方法中，所述步骤S4基于以下公式计算得到位置指令qr；
[0028]
[0029] 其中，△t为位置插补周期，qf为位置反馈， 为速度指令。
[0030] 本发明还公开了一种机器人零力控制系统，包括：
[0031] 动力学模型模块，用于基于惯性力、离心力、科里奥利力、粘摩擦力、静摩擦力、重力，建立机器人动力学模型；
[0032] 外力力矩计算模块，用于基于所述动力学模型模块中的机器人动力学模型以及反馈数据，计算机器人各个关节的外力力矩；
[0033] 力矩速度映射模块，用于根据外力力矩计算模块中计算的外力力矩计算速度指令；
[0034] 速度位置映射模块，用于根据速度指令以及位置反馈计算位置指令；
[0035] 所述动力学模型为：
[0036]
[0037] 其中，B(q)为N×N惯量矩阵，C1(q)为N×N离心力矩阵， 为N×N科里奥利力矩阵，Fv为N×N粘摩擦力矩阵，Fs为N×N静摩擦力矩阵，G(q)为N×1重力向量，τ为N×1关节力矩向量， 表示以 为自变量的符号函数，为加速度信息，为速度信息，q为位置信息，N为机器人关节数；
[0038] 所述外力力矩计算模块包括：
[0039] 关节作用力力矩计算单元，用于将公式(1)中静摩擦力设为0，并将动力学模型中的 设为期望加速度 设为速度反馈 q设为位置反馈qf，得到如公式(2)所示的关节作用力力矩τnsf：
[0040]
[0041] 反馈力矩计算单元，用于根据力矩常数Kt以及反馈电流If计算反馈力矩τf：KtIf＝τf；
[0042] 外力力矩计算单元，用于根据关节作用力力矩计算单元计算的关节作用力力矩τnsf以及反馈力矩计算单元计算的反馈力矩τf计算得到外力力矩τe：τf-τnsf＝τe。
[0043] 在本发明所述的机器人零力控制系统中，所述力矩速度映射模块包括：
[0044] 静摩擦力力矩计算单元，用于将公式(1)中静摩擦力以外的作用力设为0，并将动力学模型中的 设为速度反馈 得到静摩擦力力矩τsf：
[0045] 外力力矩死区作用域计算单元，用于根据所述静摩擦力力矩计算单元计算的静摩擦力力矩τsf以及关节作用力力矩计算单元计算的关节作用力力矩τnsf计算外力力矩死区作用域[τdn,τdp]，其中，τdn,τdp基于以下公式确定：
[0046]
[0047] 其中，k1、k2表示力矩误差校正系数；
[0048] 速度指令计算单元，用于根据外力力矩计算单元中计算得到的外力力矩τe，基于以下公式计算得到速度指令
[0049]
[0050] 其中，k3为力矩速度映射系数，p∈(0,1)。
[0051] 在本发明所述的机器人零力控制系统中，所述的根据速度指令以及位置反馈计算位置指令包括：基于以下公式计算得到位置指令qr；
[0052]
[0053] 其中，△t为位置插补周期，qf为位置反馈， 为速度指令。
[0054] 实施本发明的机器人零力控制方法以及系统，具有以下有益效果：本发明通过建立动力学模型，可以直接计算各关节的外力力矩，无需借助力/转矩传感器，降低了系统成本和复杂性；采用位置指令控制的位置控制方式，而非采用直接转矩控制的方式，降低了系统安全性与稳定性的设计难度，另外，由于在计算外力力矩时考虑了惯性力，该发明还可适用于自重较大的机器人。
附图说明
[0055] 下面将结合附图及实施例对本发明作进一步说明，附图中：
[0056] 图1是本发明的机器人零力控制方法的流程图；
[0057] 图2是本发明的机器人零力控制系统的结构示意图；
[0058] 图3是速度反馈与计算得到的外力力矩之间关系示意图。
具体实施方式
[0059] 为了对本发明的技术特征、目的和效果有更加清楚的理解，现对照附图详细说明本发明的具体实施方式。
[0060] 如图1所示，是本发明的机器人零力控制方法的流程图；
[0061] 本发明的机器人零力控制方法，包括：
[0062] S1、基于惯性力、离心力、科里奥利力、粘摩擦力、静摩擦力、重力，建立机器人动力学模型；
[0063] S2、基于所述机器人动力学模型以及反馈数据，计算机器人各个关节的外力力矩；
[0064] S3、根据步骤S2中计算的外力力矩计算速度指令；
[0065] S4、根据速度指令以及位置反馈计算位置指令。
[0066] 其中，步骤S1中的所述动力学模型为：
[0067]
[0068] 其中，B(q)为N×N惯量矩阵，C1(q)为N×N离心力矩阵， 为N×N科里奥利力矩阵，Fv为N×N粘摩擦力矩阵，Fs为N×N静摩擦力矩阵，G(q)为N×1重力向量，τ为N×1关节力矩向量， 表示以 为自变量的符号函数，为加速度信息，为速度信息，q为位置信息，N为机器人关节数。
[0069] 在公式(1)的基础上，所述步骤S2包括：
[0070] S21、将公式(1)中静摩擦力设为0，并将动力学模型中的 设为期望加速度，设为速度反馈 q设为位置反馈qf，得到如公式(2)所示的关节作用力力矩τnsf：
[0071]
[0072] S22、根据力矩常数以及反馈电流计算反馈力矩τf：KtIf＝τf；
[0073] S23、根据步骤S21中计算的关节作用力力矩τnsf以及步骤S22中计算的反馈力矩τf计算得到外力力矩τe：τf-τnsf＝τe。
[0074] 因此，通过建立动力学模型，可以直接计算各关节的外力力矩，无需借助力/转矩传感器，降低了系统成本和复杂性。
[0075] 在公式(1)的基础上，所述步骤S3包括：
[0076] S31、将公式(1)中静摩擦力以外的作用力设为0，并将动力学模型中的 设为速度反馈 得到静摩擦力力矩τsf： 因为真正使得机器人运动的外力力矩不包括静摩擦力，所以在估算外力力矩时需要将静摩擦力去掉。
[0077] S32、根据所述步骤S31中的静摩擦力力矩τsf以及步骤S21中的关节作用力力矩τnsf计算外力力矩死区作用域[τdn,τdp]，其中，τdn,τdp基于以下公式确定：
[0078]
[0079] 其中，k1、k2表示力矩误差校正系数；
[0080] S33、根据步骤S23中计算得到的外力力矩τe，基于以下公式计算得到速度指令[0081]
[0082] 其中，k3为力矩速度映射系数，p∈(0,1)，因此当外力力矩在0附近时，速度变化较快，当外力力矩较大时，嘟嘟变化较慢，也就说机器人反应变迟钝，更安全。此公式将力与速度在其他领域中比较常见的关系引入本发明。
[0083] 具体的，所述步骤S4包括：基于以下公式计算得到位置指令qr；
[0084]
[0085] 其中，△t为位置插补周期，qf为位置反馈，为速度指令。
[0086] 因此本发明采用位置指令的位置控制方式，而非采用直接转矩控制的方式，降低了系统安全性与稳定性的设计难度，同时，由于公式(1)中的机器人动力学模型之中考虑了惯性力，可以适用于自重较大的机器人。
[0087] 相应的本发明公开了一种机器人零力控制系统，包括：
[0088] 动力学模型模块，用于基于惯性力、离心力、科里奥利力、粘摩擦力、静摩擦力、重力，建立机器人动力学模型；
[0089] 外力力矩计算模块，用于基于所述动力学模型模块中的机器人动力学模型以及反馈数据，计算机器人各个关节的外力力矩；
[0090] 力矩速度映射模块，用于根据外力力矩计算模块中计算的外力力矩计算速度指令；
[0091] 速度位置映射模块，用于根据速度指令以及位置反馈计算位置指令。
[0092] 其中，所述动力学模型为：
[0093]
[0094] 其中，B(q)为N×N惯量矩阵，C1(q)为N×N离心力矩阵， 为N×N科里奥利力矩阵，Fv为N×N粘摩擦力矩阵，Fs为N×N静摩擦力矩阵，G(q)为N×1重力向量，τ为N×1关节力矩向量， 表示以 为自变量的符号函数，为加速度信息，为速度信息，q为位置信息，N为机器人关节数。
[0095] 其中，所述外力力矩计算模块包括：
[0096] 关节作用力力矩计算单元，用于将公式(1)中静摩擦力设为0，并将动力学模型中的 设为期望加速度，设为速度反馈 q设为位置反馈qf，得到如公式(2)所示的关节作用力力矩τnsf：
[0097]
[0098] 反馈力矩计算单元，用于根据力矩常数以及反馈电流计算反馈力矩τf：KtIf＝τf；
[0099] 外力力矩计算单元，用于根据关节作用力力矩计算单元计算的关节作用力力矩τnsf以及反馈力矩计算单元计算的反馈力矩τf计算得到外力力矩τe：τf-τnsf＝τe。
[0100] 其中，所述力矩速度映射模块包括：
[0101] 静摩擦力力矩计算单元，用于将公式(1)中静摩擦力以外的作用力设为0，并将动力学模型中的 设为速度反馈 得到静摩擦力力矩τsf：
[0102] 外力力矩死区作用域计算单元，用于根据所述静摩擦力力矩计算单元计算的静摩擦力力矩τsf以及关节作用力力矩计算单元计算的关节作用力力矩τnsf计算外力力矩死区作用域[τdn,τdp]，其中，τdn,τdp基于以下公式确定：
[0103]
[0104] 其中，k1、k2表示力矩误差校正系数；
[0105] 速度指令计算单元，用于根据外力力矩计算单元中计算得到的外力力矩τe，基于以下公式计算得到速度指令
[0106]
[0107] 其中，k3为力矩速度映射系数，p∈(0,1)。
[0108] 其中，所述的根据速度指令以及位置反馈计算位置指令包括：基于以下公式计算得到位置指令qr；
[0109]
[0110] 其中，△t为位置插补周期，qf为位置反馈，为速度指令。
[0111] 下面结合一个具体的实施例详细说明本发明。
[0112] 现以某SCARA机器人第一、二关节为零力控制实施对象，首先根据步骤S1建立动力学模型如下
[0113]
[0114]
[0115] 其中， 为第1，2连杆绕质心坐标系Z轴的转动惯量， 为第1，2连杆质量(含电机等转配部件)，a1，a2为第1，2连杆长度，l1，l2为第1，2连杆质心距离连杆起始处的长度，fv1、fv2、fs1、fs2分别为对应第一个连杆的粘摩擦和静摩擦系数，q1，q2，为第1，2连杆旋转的角度，速度及加速度，si，ci为第i个连杆位置的正余弦运算，τ1，τ2为第1，2连杆的力矩。
[0116] 然后按照步骤S21-S23计算得出外力力矩，如图3中实线曲线所示；根据上述步骤S31-S32并取k1＝k2＝0.05，得出外力力矩作用于死区，根据步骤S33，分别针对第一二关节取k3＝0.4106，p＝0.5及k3＝0.6584，p＝0.5，根据步骤S3，取△t＝0.0005s，映射外力力矩为速度指令，在通过步骤S4转换为位置指令发送至伺服，驱动机器人运动。
[0117] 为验证本发明的正确性，通过采集机器人速度反馈，如图3虚线曲线所示。通过图3可以看出，当外力力矩一直明显偏离0值时，关节速度呈明显增加或明显减小的趋势，一旦外力力矩在0值附近波动时，对应的反馈速度也有相应的波动，这表明本发明的正确性在实际机器人之上得到了验证。
[0118] 综上所述，实施本发明的机器人零力控制方法以及系统，具有以下有益效果：本发明通过建立动力学模型，可以直接计算各关节的外力力矩，无需借助力/转矩传感器，降低了系统成本和复杂性；采用位置控制的方式，而非采用直接转矩控制的方式，降低了系统安全性与稳定性的设计难度，另外，由于在计算外力力矩时考虑了惯性力，该发明还可适用于自重较大的机器人。
[0119] 上面结合附图对本发明的实施例进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可做出很多形式，这些均属于本发明的保护之内。
