辨识工具重力
法一：6点标定
已知，工件重力在世界坐标系的大小为 { 0 , 0 , − G } \{0,0,-G\} {0,0,−G},假设工件重心在工具坐标系下的坐标为 { L x , L y , L z } \{L_x,L_y,L_z\} {Lx,Ly,Lz}，重力在工件的三个坐标轴分量为 { G x , G y , G z } \{G_x,G_y,G_z\} {Gx,Gy,Gz},重力矩在三个坐标轴的分量为 { M x , M y , M z } \{M_x,M_y,M_z\} {Mx,My,Mz}
根据力和力矩关系：
{
M
x
=
G
z
∗
L
y
−
G
y
∗
L
z
M
y
=
G
x
∗
L
z
−
G
z
∗
L
x
M
z
=
G
y
∗
L
x
−
G
x
∗
L
y
(1)
\begin{cases} M_x=G_z*L_y-G_y*L_z \\ M_y=G_x*L_z-G_z*L_x\\\tag1 M_z=G_y*L_x-G_x*L_y \end{cases}
⎩
⎨
⎧Mx=Gz∗Ly−Gy∗LzMy=Gx∗Lz−Gz∗LxMz=Gy∗Lx−Gx∗Ly(1)
求重力
G
G
G 与质心数据
{
L
x
,
L
y
,
L
z
}
\{L_x,L_y,L_z\}
{Lx,Ly,Lz}，先测量力传感器处于这几个姿态下的力传感器读数：
计算质量数据和质心数据：
G = F z 2 − F z 1 + F x 4 − F x 3 + F y 6 − F y 5 6 G=\frac{F_{z2}-F_{z1}+F_{x4}-F_{x3}+F_{y6}-F_{y5}}{6} G=6Fz2−Fz1+Fx4−Fx3+Fy6−Fy5
L x = T y 1 − T y 2 + T z 6 − T z 5 4 G L_x=\frac{T_{y1}-T_{y2}+T_{z6}-T_{z5}}{4G} Lx=4GTy1−Ty2+Tz6−Tz5
L y = T x 2 − T x 1 + T z 3 − T z 4 4 G L_y=\frac{T_{x2}-T_{x1}+T_{z3}-T_{z4}}{4G} Ly=4GTx2−Tx1+Tz3−Tz4
L z = T y 4 − T y 3 + T x 5 − T x 6 4 G L_z=\frac{T_{y4}-T_{y3}+T_{x5}-T_{x6}}{4G} Lz=4GTy4−Ty3+Tx5−Tx6
法二：最小二乘参数辨识
将式一写成矩阵的形式
[ M x M y M z ] = [ 0 F z − F y − F z 0 F x F y − F x 0 ] [ x y z ] \begin{bmatrix} M_x \\ M_y\\M_z \end{bmatrix}=\begin{bmatrix} 0& F_z& -F_y \\ -F_z& 0& F_x\\F_y&-F_x&0\\\end{bmatrix}\begin{bmatrix} x \\ y\\z \end{bmatrix} MxMyMz = 0−FzFyFz0−Fx−FyFx0 xyz
控制机器人变换末端姿态,取N个不同的负载姿态,为了避免矩阵奇异，至少要求有三个姿态下机器人末端的指向向量不共面，且尽量不接近共面。根据N组传感器数据可得：
[
M
x
1
M
y
1
M
z
1
M
x
2
M
y
2
M
z
2
.
.
.
M
x
N
M
y
N
M
z
N
]
=
[
0
F
z
1
−
F
y
1
−
F
z
1
0
F
x
1
F
y
1
F
x
1
0
0
F
z
2
−
F
y
2
−
F
z
2
0
F
x
2
F
y
2
−
F
x
2
0
.
.
.
.
.
.
.
.
.
0
F
z
N
−
F
y
N
−
F
z
N
0
F
x
N
F
y
N
−
F
x
N
0
]
[
x
y
z
]
\begin{bmatrix} M_{x1} \\ M_{y1}\\M_{z1}\\ M_{x2} \\ M_{y2}\\M_{z2}\\ .\\.\\.\\M_{xN} \\ M_{yN}\\M_{zN}\\ \end{bmatrix}=\begin{bmatrix} 0& F_{z1}& -F_{y1} \\ -F_{z1}& 0& F_{x1}\\F_{y1}&F_{x1}&0\\ 0& F_{z2}& -F_{y2} \\ -F_{z2}& 0& F_{x2}\\F_{y2}&-F_{x2}&0\\ .&.&.\\ .&.&.\\ .&.&.\\ 0& F_{zN}& -F_{yN} \\ -F_{zN}& 0& F_{xN}\\F_{yN}&-F_{xN}&0\\ \end{bmatrix}\begin{bmatrix} x \\ y\\z \end{bmatrix}
Mx1My1Mz1Mx2My2Mz2...MxNMyNMzN
=
0−Fz1Fy10−Fz2Fy2...0−FzNFyNFz10Fx1Fz20−Fx2...FzN0−FxN−Fy1Fx10−Fy2Fx20...−FyNFxN0
xyz
即
m
=
F
p
\boldsymbol{m}=F \boldsymbol{p}
m=Fp
由伪逆得：
p
=
(
F
T
F
)
−
1
F
T
m
\boldsymbol{p}=(F^TF)^{-1}F^T\boldsymbol{m}
p=(FTF)−1FTm
两种方法在实际情况不太好实现，即使调零了，变换一下方向六维力传感器的读数也会变化，在工具不重的情况下影响很大，所以最好每个使用的位姿，都调一下零，或者知道六维力传感器的初始读数。
如何对末端工具进行补偿
已知工具重力在base坐标系(b)下的方向为 b P g = { 0 ; 0 ; − G } ^bP_g=\{0;0;-G\} bPg={0;0;−G},在工具坐标系（t）中的坐标为 L = { 0 ; 0 ; l Z } L=\{0;0;l_Z\} L={0;0;lZ}, 求 G x , G y , G z , T g x , T g y , T g z G_x,G_y,G_z,T_{gx},T_{gy},T_{gz} Gx,Gy,Gz,Tgx,Tgy,Tgz
则在工具坐标系下，重力的方向为：
t P = b a s e t R ⋅ b P g ^{t}P=^t_{base}R \cdot ^bP_g tP=basetR⋅bPg
力矩大小为：
n
=
L
×
t
P
n=L×^{t}P
n=L×tP
参考文献,
[1]张立建,胡瑞钦,易旺民.基于六维力传感器的工业机器人末端负载受力感知研究[J].自动化学报,2017,43(03):439-447.DOI:10.16383/j.aas.2017.c150753.
[2]赵敏. 装配机器人作业过程控制系统应用与软件开发[D].东南大学,2016.
