重力补偿原理
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−FyFx0100010001xyzk1k2k3
控制机械臂末端夹持器的位姿，取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=1000cosUsinU0sinUcosUcosV0−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−x0GxGyGz=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
重力补偿计算
'''classGravityCompensation:
M = np.empty((0,0))
F = np.empty((0,0))
f = np.empty((0,0))
R = np.empty((0,0))
x =0
y =0
z =0
k1 =0
k2 =0
k3 =0
U =0
V =0
g =0
F_x0 =0
F_y0 =0
F_z0 =0
M_x0 =0
M_y0 =0
M_z0 =0
F_ex =0
F_ey =0
F_ez =0
M_ex =0
M_ey =0
M_ez =0defUpdate_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()defUpdate_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]])defSolve_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)defUpdate_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()defUpdate_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)))defSolve_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)defSolve_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)defSolve_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.y
M_y0 = self.k2 - self.F_z0 * self.x + self.F_x0 * self.z
M_z0 = self.k3 - self.F_x0 * self.y + self.F_y0 * self.x
Torque_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)defeulerAngles2rotationMat(self, theta):
theta =[i * math.pi /180.0for 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 R
defmain():#
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
本文转载自: https://blog.csdn.net/zhang1079528541/article/details/129503851
版权归原作者 水木皆Ming 所有， 如有侵权，请联系我们删除。
版权归原作者 水木皆Ming 所有， 如有侵权，请联系我们删除。