CN103425100B - 基于力矩平衡的机器人直接示教控制方法 - Google Patents

Abstract

一种基于力矩平衡的机器人直接示教控制方法，其特征是通过测量及建模计算出机器人处于某几个特定的位姿和运动状态下的关节电机的电流值，获得计算机器人处于任意位姿和运动状态下平衡各关节重力和摩擦力所需的电流值的解析式。机器人示教时，以该解析式计算所得的电流值直接控制各关节电机，便可使机器人仿佛处在没有重力和摩擦力的环境下，此时即可对机器人进行直接示教。本发明具有控制简单、系统成本低廉、操作方便等优点。

Description

基于力矩平衡的机器人直接示教控制方法

技术领域

本发明涉及一种机器人技术，尤其是一种示教机器人控制技术，具体地说是一种基于力矩平衡的机器人直接示教控制方法。

背景技术

传统机器人的示教主要依赖于示教盒操作实现，但该示教方式操作繁琐、示教效率低、且对操作者的技术水平要求较高。以直接拖动机器人末端，移动机器人实现机器人示教的方式称为直接示教，较之示教盒示教，该示教方式更为灵活、直观、对操作者的要求也降低很多。

一种直接示教实现方式，通过切断驱动部件和关节的连接，由操作者直接拖动机器人实现示教，此即功率级脱离示教。显然这种示教方式劳动强度较高，示教的灵活性较差。

另一种直接示教控制以多维力传感器为基础，控制器通过多维力传感器检测出操作者的操作力，通过运算将其转换成为各关节的位置指令值，控制各关节电机运动，实现对机器人的直接示教。由于在机器人控制系统中引入多维力传感器存在成本昂贵、力传感器信号处理复杂等问题，因而该直接示教方法也未得到广泛的使用。

综上所述，研究一种低成本、实现简单、灵活性强的直接示教控制算法具有重要的工程意义。

发明内容

本发明的目的是针对现有的示教机器人需要以多维力传感器进行控制才能实现示教，且成本高、控制复杂等问题，发明一种低成本、适用于小型机器人的基于力矩平衡的机器人直接示教控制方法。

本发明的技术方案是：

一种基于力矩平衡的机器人直接示教控制方法，其特征是它包括以下步骤：

首先，通过测量机器人处于若干个位姿和运动状态下的关节电机的电流值，获得机器人处于任意位姿和运动状态下平衡各关节重力和摩擦力所需的电流值；从而得到处于任意位姿和运动状态下平衡各关节重力和摩擦力所需的电流值的解析式；

其次，通过直接力矩控制的方式，直接补偿机器人处于任意位姿和运动状态下各关节电机平衡重力矩和摩擦力矩所需施加的力矩值，使得机器人仿佛处于无重力和摩擦力的环境下，操作者可以很轻便的拖动机器人运动，进行直接示教；

第三，在机器人的示教工作模式下，由机器人控制器以所得解析式给出当前条件下补偿重力矩和摩擦力矩所需转矩的电流值，直接控制各关节电机，机器人各关节电机的伺服驱动部分工作于电流闭环控制状态。

通过测量机器人处于若干个位姿和运动状态下的关节电机的电流值，获得机器人处于任意位姿和运动状态下平衡各关节重力和摩擦力所需的电流值是指：

对于关节型机器人，当各关节坐标确定后，各关节对应的重力矩也就唯一确定,即：

$\stackrel{\→}{M}=M_0\·\stackrel{\→}{F}\left(\stackrel{\→}{q}\right)---\left(1\right)$

式中：

——为各关节对应重力矩；

——为各关节坐标；

$\stackrel{\→}{F}\left(\stackrel{\→}{q}\right)={\left[\begin{array}{cccc}{\stackrel{\→}{F}}_1\left(\stackrel{\→}{q}\right)& {\stackrel{\→}{F}}_2\left(\stackrel{\→}{q}\right)& ...& {\stackrel{\→}{F}}_n\left(\stackrel{\→}{q}\right)\end{array}\right]}^T$ ——为仅与关节坐标有关的向量，式中(1≤i≤n)为n×1维矩阵；

$M_0=\left[\begin{array}{cccc}{\stackrel{\→}{M}}_{11}& \stackrel{\→}{0}& ...& \stackrel{\→}{0}\\ \stackrel{\→}{0}& {\stackrel{\→}{M}}_{22}& ...& \stackrel{\→}{0}\\ ...& ...& ...& ...\\ \stackrel{\→}{0}& \stackrel{\→}{0}& ...& {\stackrel{\→}{M}}_{\mathrm{nn}}\end{array}\right]$ ——式中(1≤i≤n)为1×n维矩阵，矩阵中的各元素为仅与机器人自重及几何特征相关的常量矩阵；对于一个具体的机器人矩阵(1≤i≤n)中可能存在0元素，因而可丢弃矩阵M₀中0元素所在的列及中相应的行，式(1)可简化为式(2)所示形式，其中n≤k≤n²；

$\left[\begin{array}{c}{M}_1\\ M_2\\ ...\\ M_n\end{array}\right]=\left[\begin{array}{cccc}{m}_{11}& m_{12}& ...& m_{1k}\\ m_{21}& m_{22}& ...& m_{2k}\\ ...& ...& ...& ...\\ m_{n1}& m_{n2}& ...& m_{\mathrm{nk}}\end{array}\right]\·\left[\begin{array}{c}{f}_1\left(\stackrel{\→}{q}\right)\\ f_2\left(\stackrel{\→}{q}\right)\\ ...\\ f_k\left(\stackrel{\→}{q}\right)\end{array}\right]---\left(2\right)$

对于一个具体的机器人，为关节坐标的已知函数，M₀为一个常量矩阵，矩阵中元素的具体数值往往难以直接建模获得，由此可见，关节重力矩补偿量计算的核心问题是如何获得矩阵M₀；

常量矩阵M₀通过以下方法获得：

选取k个不同的关节位姿(1≤i≤k),使得 $\stackrel{\→}{F}\left({\stackrel{\→}{q}}_i\right)={\left[\begin{array}{cccc}{f}_1\left({\stackrel{\→}{q}}_i\right)& f_2\left({\stackrel{\→}{q}}_i\right)& ...& f_k\left({\stackrel{\→}{q}}_i\right)\end{array}\right]}^T$ (1≤i≤k)为一组线性无关的向量，通过测量对应时刻关节1的关节转矩M_1i，便可获得k组线性无关的方程组，求解该方程组，便可获得矩阵M₀中第一行的k个元素的具体数值；类似地，可测量出其余关节的关节转矩M_2i,M_3i...M_ni(1≤i≤k)，建立相应的方程组，便可求出矩阵M₀；

以电枢电流近似电磁转矩，即以电流的测量近似实现力矩的测量，以电流的控制近似实现力矩的控制；以系数矩阵乘式(1)两边可得：

$K_T^{-1}\·\stackrel{\→}{M}=K_T^{-1}\·M_0\·\stackrel{\→}{F}\left(\stackrel{\→}{q}\right)---\left(3\right)$

式中：矩阵 $K_T^{-1}={\left(\mathrm{diag}\left(\begin{array}{cccc}{K}_{T1}& K_{T2}& ...& K_{\mathrm{Tn}}\end{array}\right)\right)}^{-1}=\mathrm{diag}\left(\begin{array}{cccc}{K}_{T1}^{-1}& K_{T2}^{-1}& ...& K_{\mathrm{Tn}}^{-1}\end{array}\right),$ K_Ti(1≤i≤n)为第i个关节电机的转矩常量；

令 ${\stackrel{\→}{I}}_g=K_T^{-1}\·\stackrel{\→}{M},$ $I_0=K_T^{-1}\·M_0,$ 式(3)可写成：

${\stackrel{\→}{I}}_g=I_0\·\stackrel{\→}{F}\left(\stackrel{\→}{q}\right)---\left(4\right)$

即补偿示教过程中重力矩补偿量的计算转换为重力矩补偿量对应电流值的计算，同时常量矩阵M₀的获取过程也就转换为常量矩阵I₀的获取，类似于常量矩阵M₀的获取过程，通过测量的方式获得常量矩阵I₀，具体方法如下：

选取k个不同的关节位姿(1≤i≤k),使得 $\stackrel{\→}{F}\left({\stackrel{\→}{q}}_i\right)={\left[\begin{array}{cccc}{f}_1\left({\stackrel{\→}{q}}_i\right)& f_2\left({\stackrel{\→}{q}}_i\right)& ...& f_k\left({\stackrel{\→}{q}}_i\right)\end{array}\right]}^T$ (1≤i≤k)为一组线性无关的向量，通过测量处于对应时刻关节1电机的电流I_1i，便可获得k组线性无关的方程组，求解该方程组，便可获得矩阵I₀中第一行的k个元素的具体数值；类似地，可测量出其余关节的关节转矩I_2i,I_3i...I_ni(1≤i≤k)，建立相应的方程组，便可求出矩阵I₀中所有的元素；

当矩阵I₀确定后，各关节所需的重力矩补偿电流值便可由直接计算获得。

测量关节电机电流时为提高重力矩电流值的测量精度，通过一种双向的摆动测量实验一次性测量获得关节重力矩和摩擦力矩值，一次性将各关节摩擦力矩补偿量一并测出，上文所述电流测量过程需针对每一单个关节一一实现，为提高单个关节重力矩的测量精度，当测量某一关节重力对应的电流值时，控制电机以恒定且微小的转速在测量位置附近的两个位置之间来回摆动，两个相邻位置与原始位置之间的夹角θ₁=θ₂<1°；由于摆角θ<1°，即cosθ>0.9998≈1，故在整个摆动过程中重力对关节所产生的转矩G·x·cosθ≈G·x被认为无变化；

1)当摆臂从水平位下部向水平位上部运动的过程中，由于摩擦力的方向确定，且摩擦力大小也确定（等于最大静摩擦力值），因而该过程中电机电枢电流为：

$I_{+}=\frac{G\&CenterDot;x\&CenterDot;\cos \mathrm{\&theta;}+M_{f\max }}{K_T}\&ap;\frac{G\&CenterDot;x+M_{f\max }}{K_T}---\left(5\right)$

2)类似地，当电机处于由水平位以上的位置运动到水平位置以下的过程中，电机电枢电流为：

$I_{-}=\frac{G\&CenterDot;x\&CenterDot;\cos \mathrm{\&theta;}-M_{f\max }}{K_T}\&ap;\frac{G\&CenterDot;x-M_{f\max }}{K_T}---\left(6\right)$

3)取上述两个过程所测电枢电流的平均值，即为实际重力矩对应的电枢电流值，同时取上述两个过程所测电枢电流之差再除以2，即即为摩擦力矩对应的电枢电流值。

所述的机器人系统中无力传感器，关节力矩的检测通过对电机电流的检测近似实现，关节力矩的控制通过对电机电流的控制近似实现。

本发明的有益效果：

1）可实现小型机器人的直接示教控制，与功率级脱离示教相比，示教过程灵活性显著提高、工人劳动强度显著减少；

2）无需多维力传感器，对机器人各关节的力矩的测量和控制可以对电机电流的测量和控制近似实现；

3）与基于多维力传感器的直接示教控制相比，信号处理的运算量较少、无需求解复杂的动力学方程，因而对CPU的运算能力的要求也大大降低。

4）基于直接测量获得的重力矩和摩擦力矩补偿力矩和电流比常规的建模计算方法更准确、更简单易行。

附图说明

图1是本发明的机器人控制系统结构图；

图2是本发明的直接力矩补偿的示教控制策略；

图3是本发明的重力和摩擦力的双向摆动测量方案；

图4是本发明的平面二连杆机器人的受力分析。

图5是平面二连杆机器人的电流测量过程。

具体实施方式

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

一种基于力矩平衡的机器人直接示教控制方法，其控制系统的结构图如图1所示，示教控制的具体步骤如下：

首先，通过测量机器人处于若干个位姿和运动状态下的关节电机的电流值，获得机器人处于任意位姿和运动状态下平衡各关节重力和摩擦力所需的电流值。

本发明在机器人示教时，以计算所得的电流值直接控制各关节电机，便可使机器人仿佛处在没有重力和摩擦力的环境下，此时对机器人的直接示教便很轻便、灵活，如图2所示。

对于关节型机器人，当各关节坐标确定后，各关节对应的重力矩也就唯一确定,即：

$\stackrel{\&RightArrow;}{M}=M_0\&CenterDot;\stackrel{\&RightArrow;}{F}\left(\stackrel{\&RightArrow;}{q}\right)---\left(1\right)$

式中：

——为各关节对应重力矩；

——为各关节坐标；

$\stackrel{\&RightArrow;}{F}\left(\stackrel{\&RightArrow;}{q}\right)={\left[\begin{array}{cccc}{\stackrel{\&RightArrow;}{F}}_1\left(\stackrel{\&RightArrow;}{q}\right)& {\stackrel{\&RightArrow;}{F}}_2\left(\stackrel{\&RightArrow;}{q}\right)& ...& {\stackrel{\&RightArrow;}{F}}_n\left(\stackrel{\&RightArrow;}{q}\right)\end{array}\right]}^T$ ——为仅与关节坐标有关的向量，式中(1≤i≤n)为n×1维矩阵；

$M_0=\left[\begin{array}{cccc}{\stackrel{\&RightArrow;}{M}}_{11}& \stackrel{\&RightArrow;}{0}& ...& \stackrel{\&RightArrow;}{0}\\ \stackrel{\&RightArrow;}{0}& {\stackrel{\&RightArrow;}{M}}_{22}& ...& \stackrel{\&RightArrow;}{0}\\ ...& ...& ...& ...\\ \stackrel{\&RightArrow;}{0}& \stackrel{\&RightArrow;}{0}& ...& {\stackrel{\&RightArrow;}{M}}_{\mathrm{nn}}\end{array}\right]$ ——式中(1≤i≤n)为1×n维矩阵，矩阵中的各元素为仅与机器人自重及几何特征相关的常量矩阵；对于一个具体的机器人矩阵(1≤i≤n)中可能存在0元素，因而可丢弃矩阵M₀中0元素所在的列及中相应的行，式(1)可简化为式（2）所示形式，其中n≤k≤n²；

$\left[\begin{array}{c}{M}_1\\ M_2\\ ...\\ M_n\end{array}\right]=\left[\begin{array}{cccc}{m}_{11}& m_{12}& ...& m_{1k}\\ m_{21}& m_{22}& ...& m_{2k}\\ ...& ...& ...& ...\\ m_{n1}& m_{n2}& ...& m_{\mathrm{nk}}\end{array}\right]\&CenterDot;\left[\begin{array}{c}{f}_1\left(\stackrel{\&RightArrow;}{q}\right)\\ f_2\left(\stackrel{\&RightArrow;}{q}\right)\\ ...\\ f_k\left(\stackrel{\&RightArrow;}{q}\right)\end{array}\right]---\left(2\right)$

对于一个具体的机器人，为关节坐标的已知函数，M₀为一个常量矩阵，矩阵中元素的具体数值难以直接建模获得，由此可见，关节重力矩补偿量计算的核心问题是如何获得矩阵M₀。

M₀可通过以下方法获得：

选取k个不同的关节位姿(1≤i≤k),使得 $\stackrel{\&RightArrow;}{F}\left({\stackrel{\&RightArrow;}{q}}_i\right)={\left[\begin{array}{cccc}{f}_1\left({\stackrel{\&RightArrow;}{q}}_i\right)& f_2\left({\stackrel{\&RightArrow;}{q}}_i\right)& ...& f_k\left({\stackrel{\&RightArrow;}{q}}_i\right)\end{array}\right]}^T$ (1≤i≤k)为一组线性无关的向量，通过测量对应时刻关节1的关节转矩M_1i，便可获得k组线性无关的方程组，求解该方程组，便可获得矩阵M₀中第一行的k个元素的具体数值；类似地，可测量出其余关节的关节转矩M_2i,M_3i...M_ni(1≤i≤k)，建立相应的方程组，便可求出矩阵M₀中所有的元素。

本发明以电枢电流近似电磁转矩，即以电流的测量近似实现力矩的测量，以电流的控制近似实现力矩的控制。以系数矩阵乘式（1）两边可得：

$K_T^{-1}\&CenterDot;\stackrel{\&RightArrow;}{M}=K_T^{-1}\&CenterDot;M_0\&CenterDot;\stackrel{\&RightArrow;}{F}\left(\stackrel{\&RightArrow;}{q}\right)---\left(3\right)$

式中：矩阵 $K_T^{-1}={\left(\mathrm{diag}\left(\begin{array}{cccc}{K}_{T1}& K_{T2}& ...& K_{\mathrm{Tn}}\end{array}\right)\right)}^{-1}=\mathrm{diag}\left(\begin{array}{cccc}{K}_{T1}^{-1}& K_{T2}^{-1}& ...& K_{\mathrm{Tn}}^{-1}\end{array}\right),$ K_Ti(1≤i≤n)为第i个关节电机的转矩常量。

令 ${\stackrel{\&RightArrow;}{I}}_g=K_T^{-1}\&CenterDot;\stackrel{\&RightArrow;}{M},$ $I_0=K_T^{-1}\&CenterDot;M_0$ 式（3）可写成：

${\stackrel{\&RightArrow;}{I}}_g=I_0\&CenterDot;\stackrel{\&RightArrow;}{F}\left(\stackrel{\&RightArrow;}{q}\right)---\left(4\right)$

即补偿示教过程中重力矩补偿量的计算转换为重力矩补偿量对应电流值的计算，同时矩阵M₀的获取过程也就转换为矩阵I₀的获取。

类似于常量矩阵M₀的获取过程，亦可通过测量的方式获得常量矩阵I₀，具体如下：

选取k个不同的关节位姿(1≤i≤k),使得 $\stackrel{\&RightArrow;}{F}\left({\stackrel{\&RightArrow;}{q}}_i\right)={\left[\begin{array}{cccc}{f}_1\left({\stackrel{\&RightArrow;}{q}}_i\right)& f_2\left({\stackrel{\&RightArrow;}{q}}_i\right)& ...& f_k\left({\stackrel{\&RightArrow;}{q}}_i\right)\end{array}\right]}^T$ (1≤i≤k)为一组线性无关的向量，通过测量处于对应时刻关节1电机的电流I_1i，便可获得k组线性无关的方程组，求解该方程组，便可获得矩阵I₀中第一行的k个元素的具体数值；类似地，可测量出其余关节的关节转矩I_2i,I_3i...I_ni(1≤i≤k)，建立相应的方程组，便可求出矩阵I₀中所有的元素。

当矩阵I₀确定后，各关节所需的重力矩补偿电流值便可由直接计算获得。

测量关节电机电流时为提高重力矩电流值的测量精度，通过一种双向的摆动测量实验一次性测量获得关节重力矩和摩擦力矩值，一次性将各关节摩擦力矩补偿量一并测出。上文所述电流测量过程需针对每一单个关节一一实现，为提高单个关节重力矩的测量过程，当测量某一关节重力对应的电流值时，控制电机以恒定且微小的转速在测量位置附近的两个位置之间来回摆动，两个相邻位置与原始位置之间的夹角θ₁=θ₂<1°；由于摆角θ<1°，即cosθ>0.9998≈1，故在整个摆动过程中重力对关节所产生的转矩G·x·cosθ≈G·x被认为无变化；

1)当电机驱动摆臂处于由图2所示位置2运动到位置1的过程中，由于摩擦力的方向确定，且摩擦力大小也确定（等于最大静摩擦力值），因而该过程中电机电枢电流为：

$I_{+}=\frac{G\&CenterDot;x\&CenterDot;\cos \mathrm{\&theta;}+M_{f\max }}{K_T}\&ap;\frac{G\&CenterDot;x+M_{f\max }}{K_T}---\left(5\right)$

2)类似地，当电机驱动摆臂处于由图2所示位置1运动到位置2的过程，电机电枢电流为：

$I_{-}=\frac{G\&CenterDot;x\&CenterDot;\cos \mathrm{\&theta;}-M_{f\max }}{K_T}\&ap;\frac{G\&CenterDot;x-M_{f\max }}{K_T}---\left(6\right)$

3)取上述两个过程所测电枢电流的平均值，即为实际重力矩对应的电枢电流值，同时取上述两个过程所测电枢电流之差再除以2，即即为摩擦力矩对应的电枢电流值。

其次，通过直接力矩控制的方式，直接补偿机器人处于任意位姿和运动状态下各关节电机平衡重力矩和摩擦力矩所需施加的力矩值，使得机器人仿佛处于无重力和摩擦力的环境下，操作者可以很轻便的拖动机器人运动，实现对机器人的直接示教；如图2所示。

第三，在机器人的示教工作模式下，机器人控制器直接给出当前条件下补偿重力矩和摩擦力矩所需转矩的电流值，机器人各关节电机的伺服驱动部分工作于电流闭环控制状态。

以平面二连杆关节机器人为例，本发明的具体实施方式为：

首先对二连杆机器人展开受力分析，如图4所示，其中J₁为肩关节，J₂为肘关节，l₁为连杆1的长度，l₂为负载质心距离关节J₂轴线的距离（为简化分析，这里假定了负载质心、连杆2质心和关节J₂轴线在纸平面的投影点三点共线），G₁、G₂、G_load分别为连杆1、连杆2的自重和负载，x₁为连杆1质心距离关节J₁轴线的距离，x₂为连杆2质心距离关节J₂轴线的距离，M₁、M₂分别为连杆1、连杆2的自重以及负载对关节J₁、J₂所产生的弯矩，θ₁、θ₂分别为关节J₁、J₂的扭转角。补偿机器人自重所需的各关节电流的计算式为其中 $\stackrel{\&RightArrow;}{I}=K_T^{-1}\&CenterDot;\stackrel{\&RightArrow;}{M}=\left[\begin{array}{c}{I}_1\\ I_2\end{array}\right],$ 为平衡关节J₁、J₂的重力矩所需的电枢电流矩； $I_0=K_T\&CenterDot;M_0=\left[\begin{array}{cc}\frac{G_1\&CenterDot;x_1+G_2\&CenterDot;l_1+G_{\mathrm{load}}\&CenterDot;l_1}{K_{T1}}& \frac{G_2\&CenterDot;x_2+G_{\mathrm{load}}\&CenterDot;l_2}{K_{T1}}\\ 0& \frac{G_2\&CenterDot;x_2+G_{\mathrm{load}}\&CenterDot;l_2}{K_{T2}}\end{array}\right],$ 由机器人连杆的物理参数、几何参数以及电枢常数确定。测量步骤如下：

1）选取θ₁=0°、θ₂=90°，此时 $\stackrel{\&RightArrow;}{F}\left(\stackrel{\&RightArrow;}{q}\right)=\left[\begin{array}{c}1\\ 0\end{array}\right],$ 测量并记录此时关节电机的电枢电流为可知 ${\stackrel{\&RightArrow;}{I}}_{10}=I_0\&CenterDot;\left[\begin{array}{c}1\\ 0\end{array}\right]=\left[\begin{array}{c}\frac{G_1\&CenterDot;x_1+G_2\&CenterDot;l_1+G_{\mathrm{load}}\&CenterDot;l_1}{K_{T2}}\\ 0\end{array}\right],$ 具体测量过程如下：首先，如图5（a）所示，控制关节J1在位置1和位置2之间来回低速摆动而关节J2维持图示位置不变，测量两个方向运动过程中的电机J1的电流，两者的平均值即为中的第一个元素，两者之差的一半即为关节J1的摩擦力所对应的关节电机电枢电流值；其次，如图5（b）所示，控制关节J1维持图示位置不变而J2在位置1和位置2之间来回低速摆动,测量两个方向运动过程中的电机J2的电流，两者的平均值即为中的第二个元素（测量结果应近似等于0），两者之差的一半即为关节J2的摩擦力所对应的关节电机的电枢电流值。

2）选取θ₁=90°、θ₂=90°，此时 $\stackrel{\&RightArrow;}{F}\left(\stackrel{\&RightArrow;}{q}\right)=\left[\begin{array}{c}0\\ 1\end{array}\right],$ 测量并记录此时关节电机的电枢电流为可知 ${\stackrel{\&RightArrow;}{I}}_{20}=I_0\&CenterDot;\left[\begin{array}{c}0\\ 1\end{array}\right]=\left[\begin{array}{c}\frac{G_2\&CenterDot;x_2+G_{\mathrm{load}}\&CenterDot;l_2}{K_{T1}}\\ \frac{G_2\&CenterDot;x_2+G_{\mathrm{load}}\&CenterDot;l_2}{K_{T1}}\end{array}\right],$ 类似1）中描述的测量过程，如图5(c）和图5（d）所示，可分别测量获得中两个元素的数值（测量结果应近似相等）。（该测量过程可不必计算摩擦力，因为通过过程1）中的测量已可获得所有关节的摩擦力值）。

3）对比 I₀，即可获得待求矩阵I₀， $I_0=\left[\begin{array}{cc}{\stackrel{\&RightArrow;}{I}}_{10}& {\stackrel{\&RightArrow;}{I}}_{20}\end{array}\right].$

如此，我们既无需关注G₁、G₂、G_load等杆件质量，x₁、x₂等几何参数的具体数值，也无需知道准确的转矩常数K_T的数值，仅通过两个测量试验即可测量出矩阵I₀。机器人处于其它任何形位的重力矩补偿值对应的电枢电流值由式直接计算即可。

最后，在示教过程中，根据当前形位按上述方法计算相应的重力矩补偿值电枢电流值再根据各关节的实际运动方向（由关节电机的编码器反馈获得），迭加或减去各关节的摩擦力矩对应的电枢电流值作为最终各关节电流控制量，即可平衡机器人，此时拖动机器人运动显得较为灵活，轻便。

综上所述，本发明可概括为：

通过测量机器人处于某几个特定的位姿和运动状态下的关节电机的电流值，获得计算机器人处于任意位姿和运动状态下平衡各关节重力和摩擦力所需的电流值的解析式。机器人示教时，以该解析式计算所得的电流值直接控制各关节电机，便可使机器人仿佛处在没有重力和摩擦力的环境下，此时对机器人的直接示教便很轻便、灵活。具有控制简单、系统成本低廉、操作方便等优点。

本发明未涉及部分均与现有技术相同或可采用现有技术加以实现。

Claims (3)

1.一种基于力矩平衡的机器人直接示教控制方法，其特征是它包括以下步骤：

首先，通过测量机器人处于若干个位姿和运动状态下的关节电机的电流值，获得机器人处于任意位姿和运动状态下平衡各关节重力和摩擦力所需的电流值；从而得到处于任意位姿和运动状态下平衡各关节重力和摩擦力所需的电流值的解析式；

对于关节型机器人，当各关节坐标确定后，各关节对应的重力矩也就唯一确定,即：

$\stackrel{\&RightArrow;}{M}=M_0\&CenterDot;\stackrel{\&RightArrow;}{F}\left(\stackrel{\&RightArrow;}{q}\right)---\left(1\right)$

式中：

——为各关节对应重力矩；

——为各关节坐标；

$\stackrel{\&RightArrow;}{F}\left(\stackrel{\&RightArrow;}{q}\right)={\left[\begin{array}{cccc}{\stackrel{\&RightArrow;}{F}}_1\left(\stackrel{\&RightArrow;}{q}\right)& {\stackrel{\&RightArrow;}{F}}_2\left(\stackrel{\&RightArrow;}{q}\right)& ...& {\stackrel{\&RightArrow;}{F}}_n\left(\stackrel{\&RightArrow;}{q}\right)\end{array}\right]}^T$ ——为仅与关节坐标有关的向量，式中 $\stackrel{\&RightArrow;}{F_i}\left(\stackrel{\&RightArrow;}{q}\right)(1\&le;i\&le;n)$ 为n×1维矩阵；

$M_0=\left[\begin{array}{cccc}{\stackrel{\&RightArrow;}{M}}_{11}& \stackrel{\&RightArrow;}{0}& ...& \stackrel{\&RightArrow;}{0}\\ \stackrel{\&RightArrow;}{0}& {\stackrel{\&RightArrow;}{M}}_{22}& ...& \stackrel{\&RightArrow;}{0}\\ ...& ...& ...& ...\\ \stackrel{\&RightArrow;}{0}& \stackrel{\&RightArrow;}{0}& ...& {\stackrel{\&RightArrow;}{M}}_{nn}\end{array}\right]$ ——式中为1×n维矩阵，矩阵中的各元素为仅与机器人自重及几何特征相关的常量矩阵；对于一个具体的机器人矩阵中可能存在0元素，因而可丢弃矩阵M₀中0元素所在的列及中相应的行，式(1)可简化为式(2)所示形式，其中n≤k≤n²；

$\left[\begin{array}{c}{M}_1\\ M_2\\ ...\\ M_n\end{array}\right]=\left[\begin{array}{cccc}{m}_{11}& m_{12}& ...& m_{1k}\\ m_{21}& m_{22}& ...& m_{2k}\\ ...& ...& ...& ...\\ m_{n1}& m_{n2}& ...& m_{nk}\end{array}\right]\&CenterDot;\left[\begin{array}{c}{f}_1\left(\stackrel{\&RightArrow;}{q}\right)\\ f_2\left(\stackrel{\&RightArrow;}{q}\right)\\ ...\\ f_k\left(\stackrel{\&RightArrow;}{q}\right)\end{array}\right]---\left(2\right)$

对于一个具体的机器人，为关节坐标的已知函数，M₀为一个常量矩阵，常量矩阵M₀通过以下方法获得：

选取k个不同的关节位姿 ${\stackrel{\&RightArrow;}{q}}_i(1\&le;i\&le;k),$ 使得 $\stackrel{\&RightArrow;}{F}\left({\stackrel{\&RightArrow;}{q}}_i\right)={\left[\begin{array}{cccc}{f}_1\left({\stackrel{\&RightArrow;}{q}}_i\right)& f_2\left({\stackrel{\&RightArrow;}{q}}_i\right)& ...& f_k\left({\stackrel{\&RightArrow;}{q}}_i\right)\end{array}\right]}^T,(1\&le;i\&le;k)$ 为一组线性无关的向量，通过测量对应时刻关节1的关节转矩M_1i，便可获得k组线性无关的方程组，求解该方程组，便可获得矩阵M₀中第一行的k个元素的具体数值；类似地，可测量出其余关节的关节转矩M_2i,M_3i...M_ni(1≤i≤k)，建立相应的方程组，便可求出矩阵M₀；

以电枢电流近似电磁转矩，即以电流的测量近似实现力矩的测量，以电流的控制近似实现力矩的控制；以系数矩阵乘式(1)两边可得：

$K_T^{-1}\&CenterDot;\stackrel{\&RightArrow;}{M}=K_T^{-1}\&CenterDot;M_0\&CenterDot;\stackrel{\&RightArrow;}{F}\left(\stackrel{\&RightArrow;}{q}\right)---\left(3\right)$

式中：矩阵 $K_T^{-1}={\left(diag\left(\begin{array}{cccc}{K}_{T1}& K_{T2}& ...& K_{Tn}\end{array}\right)\right)}^{-1}=diag\left(\begin{array}{cccc}{K}_{T1}^{-1}& K_{T2}^{-1}& ...& K_{Tn}^{-1}\end{array}\right),$ K_Ti(1≤i≤n)为第i个关节电机的转矩常量；

令 ${\stackrel{\&RightArrow;}{I}}_g=K_T^{-1}\&CenterDot;\stackrel{\&RightArrow;}{M},I_0=K_T^{-1}\&CenterDot;M_0,$ 式(3)可写成：

${\stackrel{\&RightArrow;}{I}}_g=I_0\&CenterDot;\stackrel{\&RightArrow;}{F}\left(\stackrel{\&RightArrow;}{q}\right)---\left(4\right)$

即补偿示教过程中重力矩补偿量的计算转换为重力矩补偿量对应电流值的计算，同时常量矩阵M₀的获取过程也就转换为常量矩阵I₀的获取，具体方法如下：

选取k个不同的关节位姿 ${\stackrel{\&RightArrow;}{q}}_i(1\&le;i\&le;k),$ 使得 $\stackrel{\&RightArrow;}{F}\left({\stackrel{\&RightArrow;}{q}}_i\right)={\left[\begin{array}{cccc}{f}_i\left({\stackrel{\&RightArrow;}{q}}_i\right)& f_2\left({\stackrel{\&RightArrow;}{q}}_i\right)& ...& f_k\left({\stackrel{\&RightArrow;}{q}}_i\right)\end{array}\right]}^T,(1\&le;i\&le;k)$ 为一组线性无关的向量，通过测量处于对应时刻关节1电机的电流I_1i，便可获得k组线性无关的方程组，求解该方程组，便可获得矩阵I₀中第一行的k个元素的具体数值；类似地，可测量出其余关节的关节电机的电流I_2i,I_3i...I_ni(1≤i≤k)，建立相应的方程组，便可求出矩阵I₀中所有的元素；

当矩阵I₀确定后，各关节所需的重力矩补偿电流值便可由直接计算获得；

其次，通过直接力矩控制的方式，直接补偿机器人处于任意位姿和运动状态下各关节电机平衡重力矩和摩擦力矩所需施加的力矩值，使得机器人仿佛处于无重力和摩擦力的环境下，操作者可以很轻便的拖动机器人运动，进行直接示教；

第三，在机器人的示教工作模式下，由机器人控制器以所得解析式给出当前条件下补偿重力矩和摩擦力矩所需转矩的电流值，直接控制各关节电机，机器人各关节电机的伺服驱动部分工作于电流闭环控制状态。

2.根据权利要求1所述的基于力矩平衡的机器人直接示教控制方法，其特征是测量关节电机电流时为提高重力矩电流值的测量精度，通过一种双向的摆动测量实验一次性测量获得关节重力矩和摩擦力矩值，一次性将各关节摩擦力矩补偿量一并测出，为提高单个关节重力矩的测量精度，当测量某一关节重力对应的电流值时，控制电机以恒定且微小的转速在测量位置附近的两个位置之间来回摆动，两个相邻位置与原始位置之间的夹角θ₁＝θ₂＜1°；由于摆角θ＜1°，即cosθ＞0.9998≈1，故在整个摆动过程中重力对关节所产生的转矩G·x·cosθ≈G·x被认为无变化；G为该关节机械手臂的自重，x为关节机械手臂重心距离关节轴线的距离；

1)当摆臂从水平位下部向水平位上部运动的过程中，由于摩擦力的方向确定，且摩擦力大小也确定，因而该过程中电机电枢电流为：

$I_{+}=\frac{G\&CenterDot;x\&CenterDot;cos\mathrm{\&theta;}+M_{fmax}}{K_T}\&ap;\frac{G\&CenterDot;x+M_{fmax}}{K_T}---\left(5\right)$

2)类似地，当电机处于由水平位以上的位置运动到水平位置以下的过程中，电机电枢电流为：

$I_{-}=\frac{G\&CenterDot;x\&CenterDot;cos\mathrm{\&theta;}-M_{fmax}}{K_T}\&ap;\frac{G\&CenterDot;x-M_{fmax}}{K_T}---\left(6\right)$

3)取上述1)、2)两个过程所测电枢电流的平均值，记为即为实际重力矩对应的电枢电流值，同时取上述两个过程所测电枢电流之差再除以2，记为即为摩擦力矩对应的电枢电流值。

3.根据权利要求1所述的直接示教控制方法，其特征是所述的机器人系统中无力传感器，关节力矩的检测通过对电机电流的检测近似实现，关节力矩的控制通过对电机电流的控制近似实现。