FTP配置和使用

目的

机器人精度标定主要用于消除由于加工/装配等引入的固定的几何误差

标定原理

在机器人末端安装靶球, 控制机器人到不同构型, 并使用高精度测量设备采集靶球位置,通过优化修正运动学参数以减小误差

单点预测配准矩阵

使用机器人末端安装的1个靶标采集多组可以预测配准矩阵

$$^b_nT^n_mP_i=f(\theta)^b_mP_0$$

线性化得到$Ax=B$如下

$$\begin{align}\notag -t = & R\ P_0-t_0-R_0\ P \\\notag = & R\ P_0 -t_0 -r_1P_x -r_2P_y -r_3P_z \\ = & \begin{bmatrix} R &-I &-P_xI & -P_yI &-P_zI \end{bmatrix} \begin{bmatrix} P_0 \\ t_0 \\ r_1 \\ r_2 \\ r_3 \end{bmatrix}\notag \end{align}$$

使用最小二乘法求$x=\text{Pinv}(A)B$得到初始配准矩阵,以及零位下测量点位置

$\{b\}$: 机器人基坐标系

$\{n\}$: 测量坐标系(如NDI)

$\{t\}$: 机器人工具坐标系

$f(\theta)$: 机器人运动学中指数部分$T=f(\theta)T_0=\begin{bmatrix}
R & t\\
0 & 1
\end{bmatrix}T_0$

$^b_nT=\begin{bmatrix}
R_0 & t_0\\
0 & 1
\end{bmatrix}$

$R_0=[r_1,r_2,r_3]$

多点标定¹

目标为最小化误差

$$f=\sum_i\frac{1}{2}||e_i||_2^2 =\sum_i\frac{1}{2}||^b_nT^n_mP_i-f_i(\theta){}^b_mP_0||_2^2$$

增加扰动,简化下标

$$\begin{align} e_i = & TP_i-T_iP_0\\ = & dTTP_i-T_i(P_0+\delta P) \end{align}$$

指数映射位姿

$$\begin{align} e_i = & dTTP_i-T_i(P_0+\delta P)\\ \approx & (1+\hat{\xi})TP_i-T_i(P_0+\delta P)\\ = & e_i - \begin{bmatrix} -\begin{bmatrix} -[TP_i] & I\\ 0 &0 \end{bmatrix} &\begin{bmatrix} R &0 \\ 0 &1 \end{bmatrix} \end{bmatrix} \begin{bmatrix} \xi \\ \delta P \end{bmatrix} \\ \end{align}$$

Cayley映射位姿

$$\begin{align} e_i = & dTTP_i-T_i(P_0+\delta P)\\ = & (1-\frac{1}{2} \hat{\xi})^{-1}(1+\frac{1}{2} \hat{\xi})TP_i-T_i(P_0+\delta P)\\ \end{align}$$

令$e’_i=(1-\frac{1}{2} \hat{\xi})e_i$, 有$\lim _{\boldsymbol{\xi} \rightarrow \mathbf{0}} \mathbf{e}_{j}^{\prime}=\mathbf{e}_{j}$

更改目标函数对应误差为$e’_i$

$$\begin{align} e_i' = & (1+\frac{1}{2} \hat{\xi})TP_i-(1-\frac{1}{2} \hat{\xi})T_i(P_0+\delta P)\\ = & e_i' - \begin{bmatrix} -\begin{bmatrix} -[\frac{1}{2}(TP_i+T_iP_0)] & I\\ 0 &0 \end{bmatrix} &\begin{bmatrix} R &0 \\ 0 &1 \end{bmatrix} \end{bmatrix} \begin{bmatrix} \xi \\ \delta P \end{bmatrix} \end{align}$$

扰动求解

目标函数更改为

$$f=\sum_i(e_i-A_ix)^T(e_i-A_ix)$$

其中$x=\begin{bmatrix}
\xi \\
\delta P
\end{bmatrix}$,$P$可以是多个点排列,也可以是单个点

对应求解

$$\sum_i A_i^TA_ix=\sum_iA_i^Te_i$$

解为$x=\text{Pinv}(\sum_i A_i^TA_i)(\sum_iA_i^Te_i)$
然后更新$T=e^{\xi}T,P=P+\delta P$(指数映射),$T=\text{cayMap}(\xi)T,P=P+\delta P$(指数映射)

多次迭代找到最优值

Cayley 映射

$$T = \begin{bmatrix} R & t \\ 0 & 1 \end{bmatrix} = \left(I - \frac{1}{2}\xi^\wedge\right)^{-1}\left(I + \frac{1}{2}\xi^\wedge\right)$$

分块矩阵求逆

$$I - \frac{1}{2}\xi^\wedge = \begin{bmatrix} I - \frac{1}{2}[\omega] & -\frac{1}{2}v \\ 0 & 1 \end{bmatrix}$$

分块上三角矩阵, 记为

$$M = \begin{bmatrix} A & B \\ 0 & C \end{bmatrix}$$

其中：
$A = I_3 - \frac{1}{2}[\omega] \in \mathbb{R}^{3\times3}$,
$B = -\frac{1}{2}v \in \mathbb{R}^{3\times1}$,
$C = 1 \in \mathbb{R}^{1\times1}$

对于分块上三角矩阵，其逆矩阵为：

$$M^{-1} = \begin{bmatrix} A^{-1} & -A^{-1}BC^{-1} \\ 0 & C^{-1} \end{bmatrix}= \begin{bmatrix} A^{-1} & \frac{1}{2}A^{-1}v \\ 0 & 1 \end{bmatrix}$$

Cayley 映射定义为

$$T = M^{-1} \left(I + \frac{1}{2}\xi^\wedge\right)$$

其中：

$$I + \frac{1}{2}\xi^\wedge = \begin{bmatrix} I + \frac{1}{2}[\omega] & \frac{1}{2}v \\ 0 & 1 \end{bmatrix}$$

旋转部分计算

$$\begin{aligned} R &= A^{-1} \left(I + \frac{1}{2}[\omega]\right) \\ &= \left(I + \frac{2[\omega] + [\omega]^2}{4 + \|\omega\|^2}\right)\left(I + \frac{1}{2}[\omega]\right) \end{aligned}$$

展开后利用恒等式 $[\omega]^3 = -[\omega]$ 简化：

$$R = I + \frac{4}{4+\|\omega\|^2}\left([\omega]_\times + \frac{1}{2}[\omega]^2\right)$$

平移部分计算

$$\begin{aligned} t &= A^{-1}\left(\frac{1}{2}v\right) + \left(\frac{1}{2}A^{-1}v\right)\\ &= \frac{1}{2}A^{-1}v + \frac{1}{2}A^{-1}v \\ &= A^{-1}v \end{aligned}$$

代入 $A^{-1}$ 闭式解：

$$t = \left(I + \frac{2[\omega]_\times + [\omega]^2}{4 + \|\omega\|^2}\right)v$$

利用恒等式 $[\omega]^2v = \omega(\omega^Tv) - |\omega|^2v$：

$$t = v + \frac{2[\omega]v + \omega(\omega^Tv) - \|\omega\|^2v}{4+\|\omega\|^2}$$

当 $\omega \perp v$（标定中常见情况$\xi =\begin{bmatrix}w \\v\end{bmatrix}=\begin{bmatrix}w \\r\times w \end{bmatrix}$）时，$\omega^Tv=0$，简化为：

$$t = \frac{4v + 2[\omega]v}{4+\|\omega\|^2}$$

这是代码中采用的高效形式。

6. 与指数映射对比

特性	Cayley 映射实现	指数映射
计算复杂度	无超越函数，仅基础矩阵运算	需计算 sin/cos 和 Rodrigues 公式
精度	精确闭式解，无近似	精确闭式解

附录

考虑到移动和平动的量纲不一致,容易造成雅可比条件数比较大,可以更改平动量纲(乘1个系数),只是等价替换不影响原理

参考

¹. Barfoot T D , Forbes J R , D’Eleuterio G M T .Vectorial parameterizations of pose[J].Robotica: International journal of information, education and research in robotics and artificial intelligence, 2022.DOI:10.1017/S0263574721001715. ↩

---

转载请注明来源，欢迎对文章中的引用来源进行考证，欢迎指出任何有错误或不够清晰的表达。可以邮件至 yangbenbo@whu.edu.cn