[发明专利]基于关节力的空间六维力测量方法

摘要

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

主权项

一种基于关节力的空间六维力测量方法，包括上平台和下平台，所述下平台由六个电机分别独立驱动的六个驱动杆与上平台连接形成并联结构，六个电机形成六个关节，所述上平台和下平台分别具有六个连接点，其特征在于：包括如下步骤：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，Bi＝Rbi+O1           (1.1)其中，R=cosθycosθx-sinθxcosθz+cosθxsinθysinθzsinθxsinθz+cosθxsinθycosθzsinθxsinθysinθxsinθysinθz+cosθxcosθzsinθxsinθycosθz-cosθxsinθz-sinθycosθysinθzcosθycosθz]]>R为上平台姿态的旋转矩阵，O1为上平台上的工件坐标系相对于下平台基坐标系中的位移矢量，bi为Bi点在工件坐标系中的位置矢量；驱动杆在基坐标系o‑xyz中的位置矢量为Li＝Bi‑Ai＝Rbi+O1‑Ai，其中，(i＝1,2,...,6)，驱动杆的实时长度可表示称被测运动问题位姿参数的函数：li=||Li||=|Bi-Ai|=(Rbi+O1-Ai)T(Rbi+O1-Ai)=hi(x,y,z,θx,θy,θz)---(1.2)]]>Bi、bi分别为上平台顶点在基坐标系、工件坐标系中的位置向量，Ai为下平台顶点在基坐标系中的位置向量，将被测物体的六个位姿参数代入到(1.2)式中，可求得驱动杆的长度；S3.运动学正解正解方程由逆解方程变化而来：li2＝(Bi‑Ai)(Bi‑Ai)T，其中Bi，Ai分别表示上平台和下平台的连接点在基坐标系中的坐标；fi(Bi)＝fi(x,y,z,θx,θy,θz)＝(Rbi+O1‑Ai)(Rbi+O1‑Ai)T‑li2＝0，i＝1,2,...,6   (1.3)；首先令x1,x2,x3,x4,x5,x6＝(x,y,z,θx,θy,θz)且初始点(x,y,z,θx,θy,θz)＝(0,0,0,0,0,0)，然后将fi(Bi)(i＝1,2,...,6)在Bi附近进行Taylor展开，取其一阶线性部分可得fi+Σk=16(xk-xk0)∂fi∂xk=0---(1.4)]]>式(1.4)中为xi的线性方程组，对应的系数矩阵为J1且系数矩阵对应的第i行第j列元素为通过求系数矩阵J1的逆矩阵以及解方程组即可求得上平台的位姿；S4.力Jacobi矩阵的求解机构的速度Jacobi矩阵J为：J＝[eT (Rb×e)T]‑1         (1.5)对于六个关节驱动力(或力矩)组成的关节矢量为τ＝[τ1,τ2,τ3,τ4,τ5,τ6]T关节矢量与运动平台的广义操作力矢量F＝[f1,f2,f3,f4,f5,f6]T具有如下关系：τ＝JT(q)F                    (1.6)力Jacobi矩阵JF是静平衡状态下，关节力向操作力映射的线性关系，即F＝JFτ，则JF=(JT)-1=(J-1)T=eT(Rb×e)TT=e(Rb×e)---(1.7).]]>