Dobot magician机械臂抓取实战---手眼标定（6）

没入&浅出

已于 2023-08-01 10:13:34 修改

阅读量1.1k

点赞数 2

分类专栏： magician 文章标签： ubuntu 机器人学习算法

于 2023-08-01 10:05:15 首次发布

本文链接：https://blog.csdn.net/weixin_64037619/article/details/131997864

版权

magician 专栏收录该内容

15 篇文章 17 订阅

订阅专栏

一、手眼标定

在做视觉机械臂抓取的时候我们需要进行一步非常重要的步骤，那就是手眼标定，那么手眼标定的主要目的就是求解末端坐标系和相机坐标系之间的坐标变换。

手眼标定有两种，一个是眼在手上（Eye-in-hand），另一个是眼在手外（Eye-to-hand）。

1、眼在手上（Eye-in-hand）

保持标定板不动，控制机械臂使相机拍摄不同机械臂位姿下的棋盘格。

（1）定义几个坐标系：P代表标定板坐标系，C代表相机坐标系，e代表机械臂末端坐标系，b代表基坐标系。

$_{e}^{c}M = _{p}^{c}M _{b}^{p}M _{e}^{b}M$

$_{b}^{p}M = _{p}^{c}M^{-1}* _{e}^{c}M *_{e}^{b}M^{-1}$

在拍摄n副图片后，棋盘格和机械臂坐标系关系不变

$_{p}^{c}M_{1}^{-1}*_{b}^{p}M *_{e}^{b}M_{1}^{-1} = _{p}^{c}M_{2}^{-1}* _{e}^{c}M *_{e}^{b}M^{-1}_{2}$

令 $A = _{p}^{c}M_{n} *_{p}^{c} M^{-1}_{n-1}$

$B = _{e}^{b}M_{n} ^{-1}*_{e}^{b} M_{n-1}$

$X = _{e}^{c}M$

所以上式可以写为AX = BX

2、眼在手外（Eye-to-hand）

标定板固定在机械臂末端执行器上不动，改变机械臂位姿，从不同角度拍摄。

$_{e}^{p}M = _{p}^{c}M* _{e}^{p}M* _{b}^{e}M$

由于标定板相对于机械臂末端是不变的

$_{e}^{p}M = _{p}^{c}M^{-1}*_{b}^{c}M*_{b}^{e}M^{-1}$

$_{p}^{c}M_{1}^{-1}* _{e}^{p}M* _{b}^{e}M_{1}^{-1} =_{p}^{c}M_{2}^{-1}*_{b}^{c}M*_{b}^{e}M^{-1}_{2}$

根据上面可得AX = BX

二、AX = BX 的求解（Tsai-Lenz算法）

1、罗德里格斯公式

罗德里格旋转公式，是描述空间中向量旋转的公式。

$R = \left ( I +\left ( 1-\cos \left ( \theta \right ) \right )K^{2}+\sin \left ( \theta \right )K \right )$

推导过程网上有很多，其中K为旋转轴， $\theta$ 是旋转角度，R为旋转矩阵

2、范数

范数（Norm）是一个数学概念，用于衡量向量或矩阵的大小。在线性代数和函数空间的理论中，范数是一种度量向量或矩阵的长度或大小的方法。

在实数向量空间中，对于一个n维向量x=(x₁, x₂, ..., xₙ)，它的范数通常表示为 $\left \| x \right \|$ ，它可以定义为：

L1范数（曼哈顿范数或绝对值范数）： $\left \|x \right \|_{1} = \left | x_{1} \right |+\left | x_{2} \right |+ ...+\left | x_{n} \right |$
L2范数（欧几里得范数或二范数）： $\left \| x \right \|_{2} = \sqrt{x_{1}^{2}+x_{2}^{2}+...x_{n}^{2}}$
Lp范数（p-范数）： $\left \| x_{p} \right \|=\begin{pmatrix} \left | x_{1} \right |P+\left | x_{2} \right |P+...+\left | x_{n} \right |P \end{pmatrix}\Lambda (\frac{1}{P})$ (其中p是一个正实数，p ≥ 1)
无穷范数（最大值范数）： ∥x∥ᵢₙᶠ = max(|x₁|, |x₂|, ..., |xₙ|)

对于矩阵来说，通常采用Frobenius范数，它定义为矩阵所有元素的平方和的平方根。

Frobenius范数： ∥A∥ₖ = √(ΣᵢΣⱼ |aᵢⱼ|²) (其中aᵢⱼ表示矩阵A的第i行第j列的元素)

所以通俗的说，范数就是为了方便度量而定义出的一个概念，主要就是面对复杂空间和多维数组时，选取出一个统一的量化标准，以方便度量和比较。请务必记住，范数是人为定义的一种度量方法。

3、反对称矩阵

反对称矩阵（Skew-Symmetric Matrix），也称为斜对称矩阵或反对阵，是一种特殊类型的方阵（即行数和列数相等的矩阵）。在一个反对称矩阵中，对角线上的元素必须都为零，而非对角线上的元素满足一个特殊的性质：第i行第j列的元素等于第j行第i列元素的相反数。

形式化地，对于一个n阶的反对称矩阵A，其特征是：

1、对角线元素：aᵢᵢ = 0（其中1 ≤ i ≤ n）

2、非对角线元素：aᵢⱼ = -aⱼᵢ（其中1 ≤ i, j ≤ n）

例如，一个3阶的反对称矩阵可以表示为：

$A = \begin{vmatrix} 0 & a_{12} & a_{13}\\ -a_{12}& 0 & a_{23}\\ - a_{13}& -a_{23} & 0 \end{vmatrix}$

反对称矩阵的一个重要性质是，它的特征值（Eigenvalues）要么为0，要么成对出现且为共轭复数。换句话说，所有的特征值的实部都是0，而虚部可能不为零。

4、求解

定义 $H_{gj}$ 为机械臂末端执行器在不同位姿下之间的相对位姿之间的变换关系； $H_{cj}$ 为相机坐标系之间的相对位姿的变换关系， $H_{cg}$ 为相机坐标系到末端执行器之间的变换关系。

那么 $AX = BX \Rightarrow H_{gj}H_{cg}=H_{cg}H_{cj}$

$\bigl(\begin{smallmatrix} R_{gj} &T_{gj} \\ 0 & 1 \end{smallmatrix}\bigr)\bigl(\begin{smallmatrix} R_{cg} & T_{cg}\\ 0& 1 \end{smallmatrix}\bigr) = \bigl(\begin{smallmatrix} R_{cg} &T_{cg} \\ 0& 1 \end{smallmatrix}\bigr)\bigl(\begin{smallmatrix} R_{cj} &T_{cj} \\ 0 & 1 \end{smallmatrix}\bigr)$

上式进行展开得到 $\left\{\begin{matrix} R_{gj}R_{cg}= R_{cg}R_{cj}\\ \left ( R_{gj}-1 \right )T_{cg}=R_{cg}T_{cj}-T_{gj} \end{matrix}\right.$ $\left\{\begin{matrix} P_{gj} = 2\sin \frac{\theta _{gj}}{2}N_{gj}\\ P_{cj} = 2\sin \frac{\theta _{cj}}{2}N_{cj} \end{matrix}\right.$

利用罗德里格斯公式将旋转矩阵转化为旋转向量得到下面式子

$r_{gj} = rod(R_{gj})$

$r_{cj} = rod(R_{cj})$

将得到向量进行二范数和归一化

$\theta _{gj} = \left \| r_{gj} \right \|_{2}$ （二范数）

$N_{gj} = \frac{r_{gj}}{\theta gj}$ $N_{gj} = \frac{r_{gj}}{\theta gj}$ (归一化)

$\theta _{cj} = \left \| r_{cj} \right \|_{2}$

$N_{cj} = \frac{r_{cj}}{\theta cj}$

使用罗德里格斯公式表示姿态变换

$\left\{\begin{matrix} P_{gj} = 2\sin \frac{\theta _{gj}}{2}N_{gj}\\ P_{cj} = 2\sin \frac{\theta _{cj}}{2}N_{cj} \end{matrix}\right.$

$\left ( skew(P_{gj}+P_{cj}) \right )*P_{cg^{'}} = P_{cj}-P_{gj}$

$P_{cg}=\frac{2P_{cg^{'}}}{\sqrt{1+\left | P_{cg^{'}}\right |^{2}}}$

$R_{cg}=\begin{pmatrix} 1-\frac{\left | P_{cg} \right |^{2}}{2} \end{pmatrix}I+\frac{1}{2}\begin{pmatrix} P_{cg}P_{cg}^{T}+\sqrt{4-\left | P_{cg} \right |^{2}}skew(P_{cg}) \end{pmatrix}$