重力补偿原理

1.力传感器数据分析

将六维力传感器三个力分量与力矩分量的零点值分别记为 ( F x 0 , F y 0 , F z 0 ) (F_{x0},F_{y0},F_{z0}) (Fx0,Fy0,Fz0)， ( M x 0 , M y 0 , M z 0 ) (M_{x0},M_{y0},M_{z0}) (Mx0,My0,Mz0)，

传感器与末端工装重力为 G G G,质心在六维力传感器坐标系中的坐标为 ( x , y , z ) (x,y,z) (x,y,z)，

重力 G G G在三个坐标轴方向上的分力与作用力矩分别为 ( G x , G y , G z ) (G_x,G_y,G_z) (Gx,Gy,Gz)， ( M g x , M g y , M g z ) (M_{gx},M_{gy},M_{gz}) (Mgx,Mgy,Mgz)。

根据力与力矩的关系可以得到（正方向由右手定则确定）：
{ M g x = G z × y − G y × z M g y = G x × z − G z × x M g z = G y × x − G x × y \left\{ \begin{array}{c} M_{gx}=G_z \times y-G_y \times z\\ M_{gy}=G_x \times z-G_z \times x\\ M_{gz}=G_y \times x-G_x \times y \end{array} \right. ⎩ ⎨ ⎧Mgx=Gz×y−Gy×zMgy=Gx×z−Gz×xMgz=Gy×x−Gx×y
将六维力传感器直接测量得到的三个方向的力分量与力矩分量分别记为 ( F x , F y , F z ) (F_x,F_y,F_z) (Fx,Fy,Fz)， ( M x , M y , M z ) (M_x,M_y,M_z) (Mx,My,Mz)，

假设标定的时候没有外部作用力在末端夹持器上，则力传感器所测得的力和力矩由负载重力及零点组成，则有
{ F x = G x + F x 0 F y = G y + F y 0 F z = G z + F z 0 \left\{ \begin{array}{c} F_x=G_x+F_{x0}\\ F_y=G_y+F_{y0}\\ F_z=G_z+F_{z0} \end{array} \right. ⎩ ⎨ ⎧Fx=Gx+Fx0Fy=Gy+Fy0Fz=Gz+Fz0

{ M x = M g x + M x 0 M y = M g y + M y 0 M z = M g z + M z 0 \left\{ \begin{array}{c} M_x=M_{gx}+M_{x0}\\ M_y=M_{gy}+M_{y0}\\ M_z=M_{gz}+M_{z0} \end{array} \right. ⎩ ⎨ ⎧Mx=Mgx+Mx0My=Mgy+My0Mz=Mgz+Mz0

联立得：
{ M x = ( F z − F z 0 ) × y − ( F y − F y 0 ) × z + M x 0 M y = ( F x − F x 0 ) × z − ( F z − F z 0 ) × x + M y 0 M z = ( F y − F y 0 ) × x − ( F x − F x 0 ) × y + M z 0 \left\{ \begin{array}{c} M_x=(F_z-F_{z0}) \times y-(F_y-F_{y0}) \times z+M_{x0}\\ M_y=(F_x-F_{x0}) \times z-(F_z-F_{z0}) \times x+M_{y0}\\ M_z=(F_y-F_{y0}) \times x-(F_x-F_{x0}) \times y+M_{z0} \end{array} \right. ⎩ ⎨ ⎧Mx=(Fz−Fz0)×y−(Fy−Fy0)×z+Mx0My=(Fx−Fx0)×z−(Fz−Fz0)×x+My0Mz=(Fy−Fy0)×x−(Fx−Fx0)×y+Mz0

{ M x = F z × y − F y × z + M x 0 + F y 0 × z − F z 0 × y M y = F x × z − F z × x + M y 0 + F z 0 × x − F x 0 × z M z = F y × x − F x × y + M z 0 + F x 0 × y − F y 0 × x \left\{ \begin{array}{c} M_x=F_z \times y - F_y \times z + M_{x0} + F_{y0} \times z -F_{z0} \times y \\ M_y=F_x \times z - F_z \times x + M_{y0} + F_{z0} \times x -F_{x0} \times z \\ M_z=F_y \times x - F_x \times y + M_{z0} + F_{x0} \times y -F_{y0} \times x \end{array} \right. ⎩ ⎨ ⎧Mx=Fz×y−Fy×z+Mx0+Fy0×z−Fz0×yMy=Fx×z−Fz×x+My0+Fz0×x−Fx0×zMz=Fy×x−Fx×y+Mz0+Fx0×y−Fy0×x

其中的 F x 0 , F y 0 , F z 0 , M x 0 , M y 0 , M z 0 , x , y , z F_{x0},F_{y0},F_{z0},M_{x0},M_{y0},M_{z0},x,y,z Fx0,Fy0,Fz0,Mx0,My0,Mz0,x,y,z为定值常数（忽略力传感器数据波动，并且末端工装不进行更换）

2.最小二乘法求解参数

2.1力矩方程

令
{ k 1 = M x 0 + F y 0 × z − F z 0 × y k 2 = M y 0 + F z 0 × x − F x 0 × z k 3 = M z 0 + F x 0 × y − F y 0 × x \left\{ \begin{array}{c} k_1=M_{x0} + F_{y0} \times z -F_{z0} \times y\\ k_2=M_{y0} + F_{z0} \times x -F_{x0} \times z\\ k_3=M_{z0} + F_{x0} \times y -F_{y0} \times x \end{array} \right. ⎩ ⎨ ⎧k1=Mx0+Fy0×z−Fz0×yk2=My0+Fz0×x−Fx0×zk3=Mz0+Fx0×y−Fy0×x
则：
[ M x M y M z ] = [ 0 F z − F y 1 0 0 − F z 0 F x 0 1 0 F y − F x 0 0 0 1 ] [ x y z k 1 k 2 k 3 ] \begin{equation} \left[ \begin{array}{c} M_x \\ M_y \\ M_z \end{array} \right]= \left[ \begin{array}{ccc} 0 & F_z & -F_y & 1 & 0 & 0 \\ -F_z & 0 & F_x & 0 & 1 & 0 \\ F_y & -F_x & 0 & 0 & 0 & 1 \end{array} \right] \left[ \begin{array}{c} x \\ y \\ z \\ k1 \\ k2 \\ k3 \end{array} \right] \end{equation} MxMyMz = 0−FzFyFz0−Fx−FyFx0100010001 xyzk1k2k3

控制机械臂末端夹持器的位姿，取N个不同的姿态(N ≥ \geq ≥ 3，要求至少有三个姿态下的机械臂末端夹持器的指向向量不共面），得到N组六维力数据传感器数据，

M = F ⋅ A M=F \cdot A M=F⋅A ，其中 A = [ x , y , z , k 1 , k 2 , k 3 ] T A=[x,y,z,k_1,k_2,k_3]^\mathrm{T} A=[x,y,z,k1,k2,k3]T，则矩阵的最小二乘解为 A = ( F T F ) − 1 ⋅ F T ⋅ M A=(F^\mathrm{T}F)^{-1}\cdot F^{\mathrm{T}}\cdot M A=(FTF)−1⋅FT⋅M，

由多组数据可以解出负载质心在六维力传感器坐标系中的坐标 ( x , y , z ) (x,y,z) (x,y,z)以及常数 ( k 1 , k 2 , k 3 ) (k_1,k_2,k_3) (k1,k2,k3)的值。

2.2力方程

在机器人的安装固定过程中，由于安装平台会存在一定的斜度，使得机器人的基坐标系与世界坐标系出现一定的夹角，同样也会使得传感器的测量值与实际值存在一定偏差，因此需要进行倾角的计算。

记世界坐标系为 O {O} O，基坐标系为 B B B，法兰坐标系为 W W W，力传感器坐标系为 C {C} C，则由基坐标系向世界坐标系的姿态变换矩阵为：
B O R = [ 1 0 0 0 c o s U s i n U 0 s i n U c o s U ] [ c o s V 0 s i n V 0 1 0 − s i n V 0 c o s V ] \begin{equation} ^O_BR= \left[ \begin{array}{ccc} 1 & 0 & 0 \\ 0 & cosU & sinU \\ 0 & sinU & cosU \end{array} \right] \left[ \begin{array}{ccc} cosV & 0 &sinV\\ 0 & 1 & 0\\ -sinV & 0 & cosV \\ \end{array} \right] \end{equation} BOR= 1000cosUsinU0sinUcosU cosV0−sinV010sinV0cosV
其中 U U U是绕世界坐标系 x x x轴的旋转角， V V V是绕基坐标系的 y y y轴的旋转角

由末端法兰到基坐标系的姿态变换矩阵为：
W B R = R z ( A ) R y ( B ) R x ( C ) ^B_WR=R_z(A) R_y(B) R_x(C) WBR=Rz(A)Ry(B)Rx(C)
A B C A B C ABC的值可以通过机器人示教器读出

因为力传感器安装到末端时，会存在 Z Z Z轴上的一定偏角 α \alpha α（可通过更换机器人参考坐标系手动将其重合，即偏角为0），由力传感器向法兰坐标系坐标系的姿态变换矩阵为：
C W R = [ c o s α − s i n α 0 s i n α c o s α 0 0 0 1 ] \begin{equation} ^W_CR= \left[ \begin{array}{ccc} cos\alpha & -sin\alpha & 0 \\ sin\alpha & cos\alpha & 0 \\ 0 & 0 & 1 \end{array} \right] \end{equation} CWR= cosαsinα0−sinαcosα0001
则由世界坐标系到六维力传感器坐标系的姿态变换矩阵为
0 C R = W C R ⋅ B W R ⋅ O B R = C W R T ⋅ W B R T ⋅ B O R T {^C_0R} = {^C_WR} \cdot {^W_BR} \cdot {^B_OR} = {^W_CR}^\mathrm{T} \cdot {^B_WR}^\mathrm{T} \cdot {^O_BR}^\mathrm{T} 0CR=WCR⋅BWR⋅OBR=CWRT⋅WBRT⋅BORT
重力在世界坐标系 O {O} O中的表示为 O G = [ 0 , 0 , − g ] T {^{O}G=[0,0,-g]^\mathrm{T}} OG=[0,0,−g]T,

通过坐标变换，将重力分解到力传感器坐标系 C {C} C得到
C G = O C R ⋅ O G = W C R ⋅ B W R ⋅ O B R ⋅ O G = C W R T ⋅ W B R T ⋅ B O R T ⋅ O G = C W R T ⋅ W B R T ⋅ [ c o s U s i n V ∗ g − s i n U ∗ g − c o s U c o s V ∗ g ] ^{C}G=^C_OR \cdot {^{O}G}= {^C_WR} \cdot {^W_BR} \cdot {^B_OR} \cdot {^{O}G} ={^W_CR}^\mathrm{T} \cdot {^B_WR}^\mathrm{T} \cdot {^O_BR}^\mathrm{T} \cdot {^{O}G} \\ ={^W_CR}^\mathrm{T} \cdot {^B_WR}^\mathrm{T} \cdot \left[ \begin{array}{c} cosUsinV * g \\ -sinU * g \\ -cosUcosV * g \end{array} \right] CG=OCR⋅OG=WCR⋅BWR⋅OBR⋅OG=CWRT⋅WBRT⋅BORT⋅OG=CWRT⋅WBRT⋅ cosUsinV∗g−sinU∗g−cosUcosV∗g

则：
[ F x , F y , F z ] T = [ G x , G y , G z ] T + [ F x 0 , F y 0 , F z 0 ] T = O C R ⋅ O G + [ F x 0 , F y 0 , F z 0 ] T [F_x,F_y,F_z]^{\mathrm{T}}=[G_x,G_y,G_z]^{\mathrm{T}}+[F_{x0},F_{y0},F_{z0}]^{\mathrm{T}}=^C_OR \cdot {^{O}G}+[F_{x0},F_{y0},F_{z0}]^{\mathrm{T}} [Fx,Fy,Fz]T=[Gx,Gy,Gz]T+[Fx0,Fy0,Fz0]T=OCR⋅OG+[Fx0,Fy0,Fz0]T

[ F x F y F z ] = [ C W R T ⋅ W B R T I ] [ c o s U s i n V ∗ g − s i n U ∗ g − c o s U c o s V ∗ g F x 0 F y 0 F z 0 ] \begin{equation} \left[ \begin{array}{c} F_x \\ F_y \\ F_z \end{array} \right]= \left[ \begin{array}{ccc} {^W_CR}^\mathrm{T} \cdot {^B_WR}^\mathrm{T} & I \end{array} \right] \left[ \begin{array}{c} cosUsinV * g \\ -sinU * g \\ -cosUcosV * g \\ F_{x0} \\ F_{y0} \\ F_{z0} \end{array} \right] \end{equation} FxFyFz =[CWRT⋅WBRTI] cosUsinV∗g−sinU∗g−cosUcosV∗gFx0Fy0Fz0
令：
{ L x = c o s U s i n V ∗ g L y = − s i n U ∗ g L z = − c o s U c o s V ∗ g \left\{ \begin{array}{c} L_x=cosUsinV * g\\ L_y=-sinU * g\\ L_z= -cosUcosV * g \end{array} \right. ⎩ ⎨ ⎧Lx=cosUsinV∗gLy=−sinU∗gLz=−cosUcosV∗g
则可以表示为:
[ F x F y F z ] = [ C W R T ⋅ W B R T I ] [ L x L y L z F x 0 F y 0 F z 0 ] \begin{equation} \left[ \begin{array}{c} F_x \\ F_y \\ F_z \end{array} \right]= \left[ \begin{array}{ccc} {^W_CR}^\mathrm{T} \cdot {^B_WR}^\mathrm{T} & I \end{array} \right] \left[ \begin{array}{c} L_x \\ L_y \\ L_z\\ F_{x0} \\ F_{y0} \\ F_{z0} \end{array} \right] \end{equation} FxFyFz =[CWRT⋅WBRTI] LxLyLzFx0Fy0Fz0

同理，取N个不同的姿态得到N组六维力传感器数据，

f = R ⋅ B f=R \cdot B f=R⋅B，其中 B = [ L x , L y , L z , F x 0 , F y 0 , F z 0 ] T B=[L_x,L_y,L_z,F_{x0},F_{y0},F_{z0}]^{\mathrm{T}} B=[Lx,Ly,Lz,Fx0,Fy0,Fz0]T，则矩阵的最小二乘解为 B = ( R T R ) − 1 ⋅ R T ⋅ f B=(R^\mathrm{T}R)^{-1} \cdot R^\mathrm{T}\cdot f B=(RTR)−1⋅RT⋅f，

由多组数据可以解出力传感器的零点值 ( F x 0 , F y 0 , F z 0 ) (F_{x0},F_{y0},F_{z0}) (Fx0,Fy0,Fz0)，安装倾角 U , V U,V U,V以及重力 g g g的大小。
g = L x 2 + L y 2 + L z 2 g=\sqrt{{L_x}^2 + {L_y}^2 + {L_z}^2}\\ g=Lx2+Ly2+Lz2

U = a r c s i n ( − L y g ) U = arcsin(\frac{-L_y}{g}) U=arcsin(g−Ly)

V = a r c t a n ( − L x L z ) V = arctan(\frac{-L_x}{L_z}) V=arctan(Lz−Lx)

3.外部接触力计算

由此可以得出外部接触力为：
{ F e x = F x − F x 0 − G x F e y = F y − F y 0 − G y F e z = F z − F z 0 − G z \left\{ \begin{array}{c} F_{ex}=F_x-F_{x0}-G_x\\ F_{ey}=F_y-F_{y0}-G_y\\ F_{ez}=F_z-F_{z0}-G_z \end{array} \right. ⎩ ⎨ ⎧Fex=Fx−Fx0−GxFey=Fy−Fy0−GyFez=Fz−Fz0−Gz

[ F e x F e y F e z ] = [ F x F y F z ] − [ O C R I ] [ 0 0 − g F x 0 F y 0 F z 0 ] \begin{equation} \left[ \begin{array}{c} F_{ex} \\ F_{ey} \\ F_{ez} \end{array} \right]= \left[ \begin{array}{c} F_{x} \\ F_{y} \\ F_{z} \end{array} \right]- \left[ \begin{array}{ccc} ^C_OR & I \end{array} \right] \left[ \begin{array}{c} 0 \\ 0 \\ -g \\ F_{x0} \\ F_{y0} \\ F_{z0} \end{array} \right] \end{equation} FexFeyFez = FxFyFz −[OCRI] 00−gFx0Fy0Fz0
又：
{ M x 0 = k 1 − F y 0 × z + F z 0 × y M y 0 = k 2 − F z 0 × x + F x 0 × z M z 0 = k 3 − F x 0 × y + F y 0 × x \left\{ \begin{array}{c} M_{x0}=k_1 - F_{y0} \times z +F_{z0} \times y \\ M_{y0}=k_2 - F_{z0} \times x +F_{x0} \times z\\ M_{z0}=k_3 - F_{x0} \times y +F_{y0} \times x \end{array} \right. ⎩ ⎨ ⎧Mx0=k1−Fy0×z+Fz0×yMy0=k2−Fz0×x+Fx0×zMz0=k3−Fx0×y+Fy0×x

C G = O C R ⋅ O G = [ G x , G y , G z ] T ^{C}G=^C_OR \cdot {^{O}G} = [G_x,G_y,G_z]^\mathrm{T} CG=OCR⋅OG=[Gx,Gy,Gz]T

{ M g x = G z × y − G y × z M g y = G x × z − G z × x M g z = G y × x − G x × y \left\{ \begin{array}{c} M_{gx}=G_z \times y-G_y \times z\\ M_{gy}=G_x \times z-G_z \times x\\ M_{gz}=G_y \times x-G_x \times y \end{array} \right. ⎩ ⎨ ⎧Mgx=Gz×y−Gy×zMgy=Gx×z−Gz×xMgz=Gy×x−Gx×y

即:
[ M g x M g y M g z ] = [ 0 − z y z 0 − x − y x 0 ] [ G x G y G z ] = [ 0 − z y z 0 − x − y x 0 ] ⋅ O C R ⋅ [ 0 0 − g ] \begin{equation} \left[ \begin{array}{c} M_{gx} \\ M_{gy} \\ M_{gz} \end{array} \right]= \left[ \begin{array}{ccc} 0 & -z & y\\ z & 0 &-x \\ -y & x & 0 \end{array} \right] \left[ \begin{array}{c} G_x \\ G_y \\ G_z \end{array} \right]= \left[ \begin{array}{ccc} 0 & -z & y\\ z & 0 &-x \\ -y & x & 0 \end{array} \right] \cdot ^C_OR \cdot \left[ \begin{array}{ccc} 0 \\ 0 \\ -g \end{array} \right] \end{equation} MgxMgyMgz = 0z−y−z0xy−x0 GxGyGz = 0z−y−z0xy−x0 ⋅OCR⋅ 00−g
得出外部接触力矩为：
{ M e x = M x − M x 0 − M g x M e y = M y − M y 0 − M g y M e z = M z − M Z 0 − M g z \left\{ \begin{array}{c} M_{ex}=M_x-M_{x0}-M_{gx}\\ M_{ey}=M_y-M_{y0}-M_{gy}\\ M_{ez}=M_z-M_{Z0}-M_{gz} \end{array} \right. ⎩ ⎨ ⎧Mex=Mx−Mx0−MgxMey=My−My0−MgyMez=Mz−MZ0−Mgz

[1]高培阳. 基于力传感器的工业机器人恒力磨抛系统研究[D].华中科技大学,2019.
[2]张立建,胡瑞钦,易旺民.基于六维力传感器的工业机器人末端负载受力感知研究[J].自动化学报,2017,43(03):439-447.DOI:10.16383/j.aas.2017.c150753.
[3]董浩. 轴孔装配工业机器人柔顺控制算法研究[D].河南理工大学,2018.

代码如下：

from numpy import *
import numpy as np
'''
Made by 水木皆Ming
重力补偿计算
'''
class GravityCompensation:M = np.empty((0, 0))F = np.empty((0, 0))f = np.empty((0, 0))R = np.empty((0, 0))x = 0y = 0z = 0k1 = 0k2 = 0k3 = 0U = 0V = 0g = 0F_x0 = 0F_y0 = 0F_z0 = 0M_x0 = 0M_y0 = 0M_z0 = 0F_ex = 0F_ey = 0F_ez = 0M_ex = 0M_ey = 0M_ez = 0def Update_M(self, torque_data):M_x = torque_data[0]M_y = torque_data[1]M_z = torque_data[2]if (any(self.M)):M_1 = matrix([M_x, M_y, M_z]).transpose()self.M = vstack((self.M, M_1))else:self.M = matrix([M_x, M_y, M_z]).transpose()def Update_F(self, force_data):F_x = force_data[0]F_y = force_data[1]F_z = force_data[2]if (any(self.F)):F_1 = matrix([[0, F_z, -F_y, 1, 0, 0],[-F_z, 0, F_x, 0, 1, 0],[F_y, -F_x, 0, 0, 0, 1]])self.F = vstack((self.F, F_1))else:self.F = matrix([[0, F_z, -F_y, 1, 0, 0],[-F_z, 0, F_x, 0, 1, 0],[F_y, -F_x, 0, 0, 0, 1]])def Solve_A(self):A = dot(dot(linalg.inv(dot(self.F.transpose(), self.F)), self.F.transpose()), self.M)self.x = A[0, 0]self.y = A[1, 0]self.z = A[2, 0]self.k1 = A[3, 0]self.k2 = A[4, 0]self.k3 = A[5, 0]# print("A= \n" , A)print("x= ", self.x)print("y= ", self.y)print("z= ", self.z)print("k1= ", self.k1)print("k2= ", self.k2)print("k3= ", self.k3)def Update_f(self, force_data):F_x = force_data[0]F_y = force_data[1]F_z = force_data[2]if (any(self.f)):f_1 = matrix([F_x, F_y, F_z]).transpose()self.f = vstack((self.f, f_1))else:self.f = matrix([F_x, F_y, F_z]).transpose()def Update_R(self, euler_data):# 机械臂末端到基坐标的旋转矩阵R_array = self.eulerAngles2rotationMat(euler_data)alpha = (0) * 180 / np.pi# 力传感器到末端的旋转矩阵R_alpha = np.array([[math.cos(alpha), -math.sin(alpha), 0],[math.sin(alpha), math.cos(alpha), 0],[0, 0, 1]])R_array = np.dot(R_alpha, R_array.transpose())if (any(self.R)):R_1 = hstack((R_array, np.eye(3)))self.R = vstack((self.R, R_1))else:self.R = hstack((R_array, np.eye(3)))def Solve_B(self):B = dot(dot(linalg.inv(dot(self.R.transpose(), self.R)), self.R.transpose()), self.f)self.g = math.sqrt(B[0] * B[0] + B[1] * B[1] + B[2] * B[2])self.U = math.asin(-B[1] / self.g)self.V = math.atan(-B[0] / B[2])self.F_x0 = B[3, 0]self.F_y0 = B[4, 0]self.F_z0 = B[5, 0]# print("B= \n" , B)print("g= ", self.g / 9.81)print("U= ", self.U * 180 / math.pi)print("V= ", self.V * 180 / math.pi)print("F_x0= ", self.F_x0)print("F_y0= ", self.F_y0)print("F_z0= ", self.F_z0)def Solve_Force(self, force_data, euler_data):Force_input = matrix([force_data[0], force_data[1], force_data[2]]).transpose()my_f = matrix([cos(self.U)*sin(self.V)*self.g, -sin(self.U)*self.g, -cos(self.U)*cos(self.V)*self.g, self.F_x0, self.F_y0, self.F_z0]).transpose()R_array = self.eulerAngles2rotationMat(euler_data)R_array = R_array.transpose()R_1 = hstack((R_array, np.eye(3)))Force_ex = Force_input - dot(R_1, my_f)print('接触力：\n', Force_ex)def Solve_Torque(self, torque_data, euler_data):Torque_input = matrix([torque_data[0], torque_data[1], torque_data[2]]).transpose()M_x0 = self.k1 - self.F_y0 * self.z + self.F_z0 * self.yM_y0 = self.k2 - self.F_z0 * self.x + self.F_x0 * self.zM_z0 = self.k3 - self.F_x0 * self.y + self.F_y0 * self.xTorque_zero = matrix([M_x0, M_y0, M_z0]).transpose()Gravity_param = matrix([[0, -self.z, self.y],[self.z, 0, -self.x],[-self.y, self.x, 0]])Gravity_input = matrix([cos(self.U)*sin(self.V)*self.g, -sin(self.U)*self.g, -cos(self.U)*cos(self.V)*self.g]).transpose()R_array = self.eulerAngles2rotationMat(euler_data)R_array = R_array.transpose()Torque_ex = Torque_input - Torque_zero - dot(dot(Gravity_param, R_array), Gravity_input)print('接触力矩：\n', Torque_ex)def eulerAngles2rotationMat(self, theta):theta = [i * math.pi / 180.0 for i in theta]  # 角度转弧度R_x = np.array([[1, 0, 0],[0, math.cos(theta[0]), -math.sin(theta[0])],[0, math.sin(theta[0]), math.cos(theta[0])]])R_y = np.array([[math.cos(theta[1]), 0, math.sin(theta[1])],[0, 1, 0],[-math.sin(theta[1]), 0, math.cos(theta[1])]])R_z = np.array([[math.cos(theta[2]), -math.sin(theta[2]), 0],[math.sin(theta[2]), math.cos(theta[2]), 0],[0, 0, 1]])# 第一个角为绕X轴旋转，第二个角为绕Y轴旋转，第三个角为绕Z轴旋转R = np.dot(R_x, np.dot(R_y, R_z))return Rdef main():#force_data = [-6.349214527290314e-05, 0.0016341784503310919, -24.31537437438965]torque_data= [-0.25042885541915894, 0.32582423090934753, 2.255179606436286e-05]euler_data = [-80.50866918099089, 77.83705434751874, -9.294185889510375 + 12]force_data1 = [-7.469202995300293, 2.3709897994995117, -23.0179500579834]torque_data1= [-0.2169264256954193, 0.3719269931316376, 0.10870222747325897]euler_data1 = [-105.99038376663763, 60.89987226261212, -10.733422007074305 + 12]force_data2 = [-14.45930004119873, 0.995974063873291, -19.523677825927734]torque_data2= [-0.19262456893920898, 0.3845194876194, 0.1622740775346756]euler_data2 = [-114.24258417090118, 43.78913507089547, -19.384088817327235 + 12]compensation = GravityCompensation()compensation.Update_F(force_data)compensation.Update_F(force_data1)compensation.Update_F(force_data2)compensation.Update_M(torque_data)compensation.Update_M(torque_data1)compensation.Update_M(torque_data2)compensation.Solve_A()compensation.Update_f(force_data)compensation.Update_f(force_data1)compensation.Update_f(force_data2)compensation.Update_R(euler_data)compensation.Update_R(euler_data1)compensation.Update_R(euler_data2)compensation.Solve_B()# compensation.Solve_Force(force_data, euler_data)# compensation.Solve_Force(force_data1, euler_data1)# compensation.Solve_Force(force_data2, euler_data2)## compensation.Solve_Torque(torque_data, euler_data)# compensation.Solve_Torque(torque_data1, euler_data1)# compensation.Solve_Torque(torque_data2, euler_data2)if __name__ == '__main__':main()

Made by 水木皆Ming

机械臂六维力传感器重力补偿原理相关推荐

六自由度机械臂基于力传感器的末端力控及拖动示教
近年来,工业机器人的运用范围不断扩大,金属成形.铸造行业.冶金行业等多种工业制造领域都可以见到机器人忙碌的身影,但是随着工艺标准的提升,越来越多的制造工艺仅靠工业机器人传统的位置控制难以胜任.如:精 ...
gazebo添加末端六维力传感器
gazebo添加末端六维力传感器建立六维力矩传感器模型将传感器拼接到机器人模型配置传感器gazebo参数常用的机器人只支持位置控制,但实际运用中,却对末端力控制有迫切的需求,其中一种常用的方法 ...
三关节机械臂上的传感器、执行器与控制板STM32F103连接
▌01 三关节机械臂三关节控制臂通过调试机械臂一体化控制电路:STM32F103控制器初步调试完成控制.本文描述了机械臂上的各个传感器.执行器与控制板之间的连接方法以及测试方法. 关于机械臂的 ...
ATI六维力传感器完整使用教程（婴儿式教学！！！！）
一.java环境安装配置参考博客:https://blog.csdn.net/qq_40881680/article/details/83585542?ops_request_misc=%257B% ...
ROBOTOUS电容式六维力传感器
ROBOTOUS六维力传感器-电容式六维力传感器电容式六维力传感器的核心部分是对压力敏感的电容器.力敏电容器的电容量是由电极面积和两个电极间的距离决定,当硅膜片两边存在压力差时,硅膜片产生形变,极板 ...
六维力传感器的温度特性和温度补偿
根据六维力传感器的种类温度特性和温度补偿的方法有所不同今天在这里来说明一下电容式传感器温度补偿的特性. 如果测量对象的材料温度上升而膨胀,因此即使在外力不变的情况下,传感器的输出值也会产生变化. 根 ...
六轴机械臂控制原理图_机械臂——六轴机械臂操作空间运动分析
机械臂建模分析:https://blog.csdn.net/Kalenee/article/details/81990130 MoveIt规划下的关节空间运动分析:http://www.guyueho ...
机械臂——六轴机械臂操作空间运动分析
机械臂建模分析:https://blog.csdn.net/Kalenee/article/details/81990130 MoveIt规划下的关节空间运动分析:http://www.guyueho ...
机械臂——六轴机械臂逆解
环境:MATLAB 2017B+Robotics Toolbox 9.10.0 前期准备:完成机械臂数学模型的建立+计算机械臂工作空间 https://blog.csdn.net/Kalenee/ar ...

机械臂六维力传感器重力补偿原理