末端工具重力辨识

哥斯拉-

已于 2023-02-14 15:37:43 修改

阅读量514

点赞数 3

文章标签：机器人

于 2023-02-03 15:10:24 首次发布

本文链接：https://blog.csdn.net/weixin_44562141/article/details/128858539

版权

辨识工具重力

法一：6点标定

已知，工件重力在世界坐标系的大小为 ${0,0,-G\}$ ,假设工件重心在工具坐标系下的坐标为 ${L_x,L_y,L_z\}$ ，重力在工件的三个坐标轴分量为 ${G_x,G_y,G_z\}$ ,重力矩在三个坐标轴的分量为 ${M_x,M_y,M_z\}$

根据力和力矩关系：
$\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}$
求重力 $G$ 与质心数据 ${L_x,L_y,L_z\}$ ，先测量力传感器处于这几个姿态下的力传感器读数：

在这里插入图片描述

计算质量数据和质心数据：

$G=\frac{F_{z2}-F_{z1}+F_{x4}-F_{x3}+F_{y6}-F_{y5}}{6}$

$L_x=\frac{T_{y1}-T_{y2}+T_{z6}-T_{z5}}{4G}$

$L_y=\frac{T_{x2}-T_{x1}+T_{z3}-T_{z4}}{4G}$

$L_z=\frac{T_{y4}-T_{y3}+T_{x5}-T_{x6}}{4G}$

法二：最小二乘参数辨识

将式一写成矩阵的形式

$\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}$

控制机器人变换末端姿态,取N个不同的负载姿态,为了避免矩阵奇异，至少要求有三个姿态下机器人末端的指向向量不共面，且尽量不接近共面。根据N组传感器数据可得：

$\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}$
即
$\boldsymbol{m}=F \boldsymbol{p}$
由伪逆得：
$\boldsymbol{p}=(F^TF)^{-1}F^T\boldsymbol{m}$