CN104634498B - 基于关节力的空间六维力测量方法 - Google Patents

Abstract

本发明提供的一种基于关节力的空间六维力测量方法，包括上平台和下平台，所述下平台由六个电机分别独立驱动的六个驱动杆与上平台连接，六个电机形成六个关节，所述上平台和下平台分别具有六个连接点，通过建立坐标系、运动学逆解、运动学正解以及Jacobi矩阵的求解；能够在极端环境中对机器人末端的六维力和力矩进行准确测量计算，并且能够适应恶劣的工况环境，可靠性强。

Description

基于关节力的空间六维力测量方法

技术领域

本发明涉及一种机器人的力和力矩测量方法，尤其涉及一种基于关节力的空间六维力测量方法。

背景技术

现有的工业、军事中，广泛运用到机器人，机器人在运动过程中所受到的六维力和力矩测量要求越来越高，而且环境也越来越复杂，通过现有的测量技术已经远远不能满足环境、频率、耦合以及简便等要求，比如：机器人在恶劣的环境中进行作业时，其末端与环境接触的地方产生的力或者力矩会对操作的运动轨迹、精度等产生重大影响，如将现有测量方式中的力传感器或速度传感器安装在操作末端上，由于末端所述的环境复杂多变，高温高压或者强光灯极端环境将严重影响力传感器或者速度传感器的测量精度，同时对传感器提出了极高的要求，而且装配复杂，操作准备的工作量大，然而，现有技术都需要借助上述的传感器来进行测量。

因此，需要提出一种对机器人的六维力和力矩的测量方法，能够在极端环境中对机器人传感器的六维力和力矩进行准确测量，并且能够适应恶劣的工况环境，可靠性强。

发明内容

有鉴于此，本发明的目的是提供一种基于关节力的空间六维力测量方法，能够在极端环境中对机器人传感器的六维力和力矩进行准确测量，并且能够适应恶劣的工况环境，可靠性强。

本发明提供的一种基于关节力的空间六维力测量方法，包括上平台和下平台，所述下平台由六个电机分别独立驱动的六个驱动杆与上平台连接形成并联机构，六个电机形成六个关节，所述上平台和下平台分别具有六个连接点；包括如下步骤：

S1.建立坐标系

在上平台建立工件坐标系O1-x1y1z1,在下平台建立基坐标系O-xyz，Bi和Ai分别为上下平台对应的六个连接点，其中：下平台的各连接点在基坐标系中的向量为(Aix，Aiy，Aiz)，上平台的各连接点在基坐标系中的向量表示为(Bix，Biy，Biz),在工件坐标系中的向量表示为(bix，biy，biz),工件坐标系在基坐标系中的位置矢量为O1，li为上平台和下平台对应点的长度，上平台相对于下平台的位姿x(t)、y(t)、z(t)、θ_x(t)、θ_y(t)、θ_z(t)表示为x、y、z、θ_x、θ_y、θ_z；

S2.运动学逆解

当上平台的位姿改变时，根据平面与平面上点的关系求出此时新点的坐标值Bi，

B_i＝Rb_i+O₁ (1.1)

其中，

R为上平台姿态的旋转矩阵，O₁为上平台上的工件坐标系相对于下平台基坐标系中的位移矢量，b_i为B_i点在工件坐标系中的位置矢量；

驱动杆在基坐标系o-xyz中的位置矢量为L_i＝B_i-A_i＝Rb_i+O₁-A_i，其中，(i＝1,2,...,6)，

驱动杆的实时长度可表示称被测运动问题位姿参数的函数：

B_i、b_i分别为上平台顶点在基坐标系、工件坐标系中的位置向量，A_i为下平台顶点在基坐标系中的位置向量，将被测物体的六个位姿参数代入到(1.2)式中，可求得驱动杆的长度；

S3.运动学正解

正解方程由逆解方程变化而来：

l_i ²＝(B_i-A_i)(B_i-A_i)^T，其中B_i，A_i分别表示上平台和下平台的连接点在基坐标系中的坐标；

f_i(B_i)＝f_i(x,y,z,θ_x,θ_y,θ_z)＝(Rb_i+O₁-A_i)(Rb_i+O₁-A_i)^T-l_i ²＝0，i＝1,2,...,6 (1.3)；

首先令x₁,x₂,x₃,x₄,x₅,x₆＝(x,y,z,θ_x,θ_y,θ_z)且初始点

(x,y,z,θ_x,θ_y,θ_z)＝(0,0,0,0,0,0)，然后将f_i(B_i)(i＝1,2,...,6)在B_i附近进行Taylor展开，取其一阶线性部分可得

式(1.4)中为x_i的线性方程组，对应的系数矩阵为J₁且系数矩阵对应的第i行第j列元素为通过求系数矩阵J₁的逆矩阵以及解方程组即可求得上平台的位姿；

S4.力Jacobi矩阵的求解

机构的速度Jacobi矩阵J为：

J＝[e^T(Rb×e)^T]^-1 (1.5)

对于六个关节驱动力(或力矩)组成的关节矢量为

τ＝[τ₁,τ₂,τ₃,τ₄,τ₅,τ₆]^T

关节矢量与运动平台的广义操作力矢量F＝[f₁,f₂,f₃,f₄,f₅,f₆]^T具有如下关系：

τ＝J^T(q)F (1.6)

力Jacobi矩阵J_F是静平衡状态下，关节力向操作力映射的线性关系，即F＝J_Fτ，则

进一步，所述测量方法还包括步骤S5:关节电机驱动电压测量与驱动力的计算：

六个电机共同驱动上平台运动，电机输出的驱动力F_ml与驱动电压u有如下关系：

F_ml(s)＝L[F_ml] (1.8)

U(s)＝L[u] (1.9)

其中，G(s)表示从驱动电压到输出驱动力的传递函数，驱动杆的输出驱动力：

关节驱动力矢量τ＝diag(F_ml1,F_ml2,F_ml3,F_ml4,F_ml5,F_ml6) (1.12)

然后通过动力学正解，则可以得到力矢量F＝J_Fτ。

进一步，所述上平台和下平台为六边形或圆形，上平台位于下平台的正上方。

本发明的有益效果：本发明的测量方法，能够在极端环境中对机器人传感器的六维力和力矩进行准确计算测量，避免了传统测量技术中环境因素对测量结果的影响，保证测量的准确度同时能够适应恶劣的工况环境，可靠性强；并且本方法的适应性广，还能够运用到并联机床、飞行模拟器、空间对接设备等领域中。

附图说明

下面结合附图和实施例对本发明作进一步描述：

图1为本发明的的并联结构结构示意图。

图2为本发明的坐标系示意图。

具体实施方式

图1为本发明的的并联结构结构示意图，图2为本发明的坐标系示意图，如图所示，本发明提供的一种基于关节力的空间六维力测量方法，包括上平台1和下平台4，所述上平台1由六个电机3分别独立驱动的六个驱动杆2与上平台1连接形成并联机构，六个电机3形成六个关节，所述上平台和下平台分别具有六个连接点；包括如下步骤：

S1.建立坐标系

在上平台建立工件坐标系O1-x1y1z1,在下平台建立基坐标系O-xyz，Bi和Ai分别为上下平台对应的六个连接点，其中：下平台的各连接点在基坐标系中的向量为(Aix，Aiy，Aiz)，上平台的各连接点在基坐标系中的向量表示为(Bix，Biy，Biz),在工件坐标系中的向量表示为(bix，biy，biz),工件坐标系在基坐标系中的位置矢量为O1，li为上平台和下平台对应点的长度，上平台相对于下平台的位姿x(t)、y(t)、z(t)、θ_x(t)、θ_y(t)、θ_z(t)表示为x、y、z、θ_x、θ_y、θ_z；

S2.运动学逆解

当上平台的位姿改变时，根据平面与平面上点的关系求出此时新点的坐标值Bi，

B_i＝Rb_i+O₁ (1.1)

其中，

R为上平台姿态的旋转矩阵，O₁为上平台上的工件坐标系相对于下平台基坐标系中的位移矢量，b_i为B_i点在工件坐标系中的位置矢量；

驱动杆在基座标系o-xyz中的位置矢量为L_i＝B_i-A_i＝Rb_i+O₁-A_i，其中，(i＝1,2,...,6)，

驱动杆的实时长度可表示称被测运动问题位姿参数的函数：

B_i、b_i分别为上平台顶点在基坐标系、工件坐标系中的位置向量，A_i为下平台顶点在基坐标系中的位置向量，将被测物体的六个位姿参数代入到(1.2)式中，可求得驱动杆的长度；

S3.运动学正解

正解方程由逆解方程变化而来：

l_i ²＝(B_i-A_i)(B_i-A_i)^T，其中B_i，A_i分别表示上平台和下平台的连接点在基坐标系中的坐标；

f_i(B_i)＝f_i(x,y,z,θ_x,θ_y,θ_z)＝(Rb_i+O₁-A_i)(Rb_i+O₁-A_i)^T-l_i ²＝0，i＝1,2,...,6 (1.3)；

首先令x₁,x₂,x₃,x₄,x₅,x₆＝(x,y,z,θ_x,θ_y,θ_z)且初始点

(x,y,z,θ_x,θ_y,θ_z)＝(0,0,0,0,0,0)，然后将f_i(B_i)(i＝1,2,...,6)在B_i附近进行Taylor展开，取其一阶线性部分可得

式(1.4)中为x_i的线性方程组，对应的系数矩阵为J₁且系数矩阵对应的第i行第j列元素为通过求系数矩阵J₁的逆矩阵以及解方程组即可求得上平台的位姿；

S4.力Jacobi矩阵的求解

速度Jacobi矩阵J是从关节空间向操作空间运动速度的传动比，反映的是输入与输出的微分关系，只与机构的类型、尺寸参数、末端运动平台的位姿以及机构输入参数有关，而与机构的真实运动速度无关，因此，通过速度jacobi矩阵的求解，就能够将机构的位姿、尺寸参数以及末端运动平台(即本发明中的上平台)等进行计算；

机构的速度Jacobi矩阵J为：

J＝[e^T(Rb×e)^T]^-1 (1.5)

对于六个关节驱动力(或力矩)组成的关节矢量为

τ＝[τ₁,τ₂,τ₃,τ₄,τ₅,τ₆]^T (1.6)

关节矢量与运动平台的广义操作力矢量F＝[f₁,f₂,f₃,f₄,f₅,f₆]^T具有如下关系：

τ＝J^T(q)F

力Jacobi矩阵J_F是静平衡状态下，关节力向操作力映射的线性关系，即F＝J_Fτ，则

本实施例中，所述测量方法还包括步骤S5:关节电机驱动电压测量与驱动力的计算：

六个电机共同驱动上平台运动，电机输出的驱动力F_ml与驱动电压u有如下关系：

F_ml(s)＝L[F_ml] (1.8)

U(s)＝L[u] (1.9)

其中，G(s)表示从驱动电压到输出驱动力的传递函数，驱动杆的输出驱动力：

关节驱动力矢量τ＝diag(F_ml1,F_ml2,F_ml3,F_ml4,F_ml5,F_ml6) (1.12)

然后通过动力学正解，则可以得到力矢量F＝J_Fτ。

本实施例中，所述上平台和下平台为六边形或圆形，上平台位于下平台的正上方，当然，上平台和下平台也可以采用其他的形状。

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本发明技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (3)

1.一种基于关节力的空间六维力测量方法，包括上平台和下平台，所述下平台由六个电机分别独立驱动的六个驱动杆与上平台连接形成并联结构，六个电机形成六个关节，所述上平台和下平台分别具有六个连接点，其特征在于：包括如下步骤：

S1.建立坐标系

在上平台的中心建立工件坐标系O1-x1y1z1,在下平台的中心建立基坐标系O-xyz，Bi和Ai分别为上下平台对应的六个连接点，其中：下平台的各连接点在基坐标系中的向量为(Aix，Aiy，Aiz)，上平台的各连接点在基坐标系中的向量表示为(Bix，Biy，Biz),在工件坐标系中的向量表示为(bix，biy，biz),工件坐标系在基坐标系中的位置矢量为O1，li为上平台和下平台对应点的长度，上平台相对于下平台的位姿x(t)、y(t)、z(t)、θ_x(t)、θ_y(t)、θ_z(t)表示为x、y、z、θ_x、θ_y、θ_z；

S2.运动学逆解

当上平台的位姿改变时，根据平面与平面上点的关系求出此时新点的坐标值Bi，

B_i＝Rb_i+O₁ (1.1)

其中，

$R=\left[\begin{array}{ccc}{\mathrm{cos\θ}}_y{\mathrm{cos\θ}}_x& -{\mathrm{sin\θ}}_x{\mathrm{cos\θ}}_z+{\mathrm{cos\θ}}_x{\mathrm{sin\θ}}_y{\mathrm{sin\θ}}_z& {\mathrm{sin\θ}}_x{\mathrm{sin\θ}}_z+{\mathrm{cos\θ}}_x{\mathrm{sin\θ}}_y{\mathrm{cos\θ}}_z\\ {\mathrm{sin\θ}}_x{\mathrm{sin\θ}}_y& {\mathrm{sin\θ}}_x{\mathrm{sin\θ}}_y{\mathrm{sin\θ}}_z+{\mathrm{cos\θ}}_x{\mathrm{cos\θ}}_z& {\mathrm{sin\θ}}_x{\mathrm{sin\θ}}_y{\mathrm{cos\θ}}_z-{\mathrm{cos\θ}}_x{\mathrm{sin\θ}}_z\\ -{\mathrm{sin\θ}}_y& {\mathrm{cos\θ}}_y{\mathrm{sin\θ}}_z& {\mathrm{cos\θ}}_y{\mathrm{cos\θ}}_z\end{array}\right]$

R为上平台姿态的旋转矩阵，O₁为上平台上的工件坐标系相对于下平台基坐标系中的位移矢量，b_i为B_i点在工件坐标系中的位置矢量；

驱动杆在基坐标系o-xyz中的位置矢量为L_i＝B_i-A_i＝Rb_i+O₁-A_i，其中，(i＝1,2,...,6)，

驱动杆的实时长度可表示称被测运动问题位姿参数的函数：

$l_i=\left|\right|L_i\left|\right|=\left|B_i-A_i\right|=\sqrt{{({\mathrm{Rb}}_i+O_1-A_i)}^T({\mathrm{Rb}}_i+O_1-A_i)}=h_i(x,y,z,{\mathrm{\θ}}_x,{\mathrm{\θ}}_y,{\mathrm{\θ}}_z)---\left(1.2\right)$

B_i、b_i分别为上平台顶点在基坐标系、工件坐标系中的位置向量，A_i为下平台顶点在基坐标系中的位置向量，将被测物体的六个位姿参数代入到(1.2)式中，可求得驱动杆的长度；

S3.运动学正解

正解方程由逆解方程变化而来：

l_i ²＝(B_i-A_i)(B_i-A_i)^T，其中B_i，A_i分别表示上平台和下平台的连接点在基坐标系中的坐标；

f_i(B_i)＝f_i(x,y,z,θ_x,θ_y,θ_z)＝(Rb_i+O₁-A_i)(Rb_i+O₁-A_i)^T-l_i ²＝0，i＝1,2,...,6 (1.3)；

首先令x₁,x₂,x₃,x₄,x₅,x₆＝(x,y,z,θ_x,θ_y,θ_z)且初始点

(x,y,z,θ_x,θ_y,θ_z)＝(0,0,0,0,0,0)，然后将f_i(B_i)(i＝1,2,...,6)在B_i附近进行Taylor展开，取其一阶线性部分可得

$f_i+\underset{k=1}{\overset{6}{\mathrm{\Σ}}}(x_k-x_{k_0})\frac{\∂f_i}{\∂x_k}=0---\left(1.4\right)$

式(1.4)中为x_i的线性方程组，对应的系数矩阵为J₁且系数矩阵对应的第i行第j列元素为通过求系数矩阵J₁的逆矩阵以及解方程组即可求得上平台的位姿；

S4.力Jacobi矩阵的求解

机构的速度Jacobi矩阵J为：

J＝[e^T (Rb×e)^T]^-1 (1.5)

对于六个关节驱动力(或力矩)组成的关节矢量为

τ＝[τ₁,τ₂,τ₃,τ₄,τ₅,τ₆]^T

关节矢量与运动平台的广义操作力矢量F＝[f₁,f₂,f₃,f₄,f₅,f₆]^T具有如下关系：

τ＝J^T(q)F (1.6)

力Jacobi矩阵J_F是静平衡状态下，关节力向操作力映射的线性关系，即F＝J_Fτ，则

$J_F={\left(J^T\right)}^{-1}={\left(J^{-1}\right)}^T={\left[\begin{array}{cc}{e}^T& {(Rb\×e)}^T\end{array}\right]}^T=\left[\begin{array}{c}e\\ (Rb\×e)\end{array}\right]---\left(1.7\right).$

2.根据权利要求1所述基于基于关节力的空间六维力测量方法，其特征在于：所述测量方法还包括步骤S5:关节电机驱动电压测量与驱动力的计算：

六个电机共同驱动上平台运动，电机输出的驱动力F_ml与驱动电压u有如下关系：

F_ml(s)＝L[F_ml] (1.8)

U(s)＝L[u] (1.9)

$G\left(s\right)=\frac{F_{ml}\left(s\right)}{U\left(s\right)}---\left(1.10\right)$

其中，G(s)表示从驱动电压到输出驱动力的传递函数，驱动杆的输出驱动力：

$F_{{\mathrm{ml}}_i}=L^{-1}\[F_{{\mathrm{ml}}_i}\left(s\right)\]=L^{-1}\[U_i\left(s\right)G\left(s\right)\]=L^{-1}\[L\[u_i\]G\left(s\right)\]---\left(1.11\right)$

关节驱动力矢量τ＝diag(F_ml1,F_ml2,F_ml3,F_ml4,F_ml5,F_ml6) (1.12)

然后通过动力学正解，则可以得到力矢量F＝J_Fτ。

3.根据权利要求1所述基于关节力的空间六维力测量方法，其特征在于：所述上平台和下平台为六边形或圆形，上平台位于下平台的正上方。