足式机器人足眼标定方法

足眼式腿式机器人运动学标定

法比安·布洛赫利格、迈克尔·布勒施、P´ETERFANKHAUSER、马尔科·许特尔
和罗兰·齐瓦特Autonomous Systems Lab(ASL),ETH Zurich, Switzerland {blfabian,bloeschm,pfankhauser}@ethz.chwww.asl.ethz.ch

足式机器人在崎岖地形中运动时，依赖于对系统运动学的精确标定，以实现动态步 态的可靠运动跟踪和精确的足部定位。在我们的自动足眼标定方法中，将一个单目 相机安装在机器人上，用于观测装有增强现实（AR）标记的机器人移动足部。利 用这些测量数据，在固定时间窗口内构建一个非线性最小二乘问题，以估计33个未 知参数。该问题通过列文伯格‐马夸尔特算法高效求解，从而得到运动学参数和相 机参数的估计值。该方法已在实际的四足机器人上成功验证。

Keywords ：标定，运动学，机器人视觉，腿式机器人 机器人学。

1.引言

足式运动严重依赖于对机器人运动学参数的准确了解。这些参数包括身体 测量、连杆长度和关节角度偏移。微小的误差会在运动学链中传播，并显 著影响足端的位姿。因此，有必要减少这些系统性误差，而通过机器人运 动学的几何标定可以实现这一目标。为了避免繁琐且容易出错的手动标定， 我们可以采用自动标定程序。为此，需要获取机器人足端位置的冗余测量， 并将其与运动学模型进行比较。由此产生的偏差取决于标定参数，可通过 优化这些参数使相关误差最小化。一种优雅且低成本地获取机器人足端测 量的方法是使用刚性固定在机器人本体上的相机

图1. foot-eye calibration问题的概述。通过最小化两个模型与视觉观测之间的偏差，可以估 计相应的标定参数（括号内）。

主体部分。对于工业机器人臂和人形机器人，这通常被称为手眼标定。2,3
由于通常难以直接提取末端执行器的3D姿态信息，4常见的做法是将棋盘 格图案附加到末端执行器上。5最近的视觉标定流程方法不仅考虑几何参
数，还旨在通过引入不同的传感器模态，提供更通用的标定框架。6,7

本文旨在通过将Birbachet al.8的方法应用于四足机器人，为足式机 器人的标定做出贡献。我们的框架能够在一次标定运行中同时包含任意数 量的腿。在标定过程中，我们引入了所有连杆长度以及一个恒定的测量时 间偏移，结果表明这显著提升了标定精度。通过采用高效的变换描述和高 度优化的求解器库，我们实现了快速的优化运行时间（10–20秒）。

2.建模

图1展示了我们研究的足眼标定的基本组成部分：运动学模型用于描述相对 于机器人基座坐标系的腿部正向运动学。相机模型用于表示检测到的3D标 记位置与其在图像平面上投影之间的关系。估计过程的参数汇总于表1中。
我们区分了运动学参数pkin和相机参数pcam。此外，我们在时间 t获取关 节编码器数据zj(t)（位置和速度）以及2D图像标记位置

表1.标定参数汇总。与运动相关的运动学标定参数以粗体显示。

运动学参数pkin
身体测量	bx, by
连杆长度∗	lh, lt, ls
关节偏移量∗	θh,offset， θt,offset， θs,偏移量
小腿到足部角度∗	θf
足部到图案角度∗	θp
时间偏移	toffset

相机参数pcam
位置	tCB
方向	qCB
相机矩阵	K
畸变系数	D

Note： ∗针对每条腿分别估算。

位置zm(t)。我们假设所有测量值均受到加性高斯噪声的影响。在关节编码 器数据和标记物测量之间可以观察到一个恒定的时间延迟 toffset，因此该 时间延迟也被包含在标定过程中。通过将运动学与相机模型关联起来，我 们可以得到标记物在图像平面上的位置估计。

2.1.运动学模型

从基坐标系 B到足端坐标系 F的运动学链如图2所示。变换 T B F是通过 连接位于髋部(H)、大腿(T)和小腿(S)的腿部中间坐标系之间的连续变换得 到的。每个变换由一个平移向量t和随后的旋转q（单位四元数）进行参
数化。以变换 T B H（左腿）为例，其为沿平移向量tHB=(bx, by,0)T的平移 和绕旋转量qHB=(cos(θh(t)/2), sin(θh(t)/2) ·[1, 0, 0])T的旋转。下文中，我 们将仅指出每个变换对模型参数的依赖关系。最终从基坐标系到足端坐标 系的变换结果为
$$
T_B^F(p_{kin}, z_j(t)) = T_B^H(\theta_h(t), b_x, b_y) T_H^T(\theta_t(t), l_h) T_T^S(\theta_s(t), l_t) T_S^F(\theta_f, l_s),
$$
(1)
其中第 j个关节角度的估计值由下式给出
$$
\theta_j(t) = \theta_{j,\text{measured}}(t) + t_{\text{offset}} \dot{\theta} {j,\text{measured}}(t) + \theta {j,\text{offset}}.
$$
(2)
这包括两个校正项，分别考虑了时间偏移量 toffset和关节角度偏移量θj,
offset。我们将圆对称标记图案刚性地附着在机器人上，以获取其足部的视
觉观测值（见图2）。变换 T_F^P 描述了圆形图案绕足部 z‐轴的旋转，而 平移向量tPM 是中心

图2.足式机器人和圆形标记图案的坐标系（红色： x‐轴，绿色： y‐轴，蓝色： z‐轴）。特定 标记 m的位置由其已知的旋转角度 θm以及相对于圆形标记图案中心的径向偏移量 dp确定。
由于角偏移量 θp会被共同估计，因此圆形标记图案无需与足部坐标系对齐。
一个特定的标记坐标系 M相对于圆形图案坐标系 P。最后，相对于机器 人基座坐标系，该平移向量为
$$
t_{MB}(p_{kin}, z_j(t)) = T_B^F(p_{kin}, z_j(t)) T_F^P(\theta_p) t_{PM}(\theta_m, d_p).
$$
(3)

2.2.相机模型

为了描述标记在图像平面中的位置，我们首先使用相机外部参数将(3)中的 平移向量表示在相机坐标系 C中。这使我们得到
$$
t_{CM}(p_{kin}, p_{cam}, z_j(t)) = T_C^B(t_{CB}, q_{CB}) t_{MB}(p_{kin}, z_j(t)),
$$
(4)
我们使用具有三个径向畸变参数的径向‐切向畸变模型来模拟镜头的畸变， 并使用针孔相机模型将标记中心位置投影到图像平面上。tCM的归一化图 像投影坐标是
$$
x_n = \begin{pmatrix} u_n \ v_n \end{pmatrix} = \left( \frac{t_{CM,x}}{t_{CM,z}}, \frac{t_{CM,y}}{t_{CM,z}} \right),
$$
(5)
并应用畸变向量 D=(k1, k2,p1,p2, k3)可得
$$
x_d = \begin{pmatrix} u_d \ v_d \end{pmatrix} = (1 + k_1 r^2 + k_2 r^4 + k_3 r^6) x_n + \begin{pmatrix} 2p_1 u_n v_n + p_2(r^2 + 2u_n^2) \ p_1(r^2 + 2v_n^2) + 2p_2 u_n v_n \end{pmatrix},
$$
(6)
其中 $r^2 = u_n^2 + v_n^2$。最后，我们得到图像平面上标记点的建模像素位置为
$$
M(p_{kin}, p_{cam}, z_j(t)) = \begin{pmatrix} f_x u_d + c_x \ f_y v_d + c_y \end{pmatrix},
$$
(7) 主点坐标为(cx, cy) T，相机矩阵 K中的焦距（包含像素间距）为 fx和 fy。

3.标定

整个标定框架在机器人操作系统（ROS）中实现。我们通过移动机器人足 端使其处于相机视野范围内来进行标定。在图像采集方面，使用带有广角 镜头的PointGreyChameleon3相机。ARToolKit标记检测器9在图像中 检测特定标记 m。由于该标记具有唯一ID，因此可轻松确定对应的足端。
根据标记的时间戳和ID获取相应的关节编码器数据，并依据公式(7)估算建 模的标记位置。标定过程通过寻找使观测值最可能成立的模型参数来完成， 即我们试图最小化建模的标记位置与测量的图像标记位置之间的误差。为 此，需优化残差
$$
f_{m,k}(p_{kin}, p_{cam}) = z_m(t_k) - M(p_{kin}, p_{cam}, z_j(t_k)),
$$
(8)
描述了在时间 tk检测到的标记点 m 与其建模位置之间的差异。在收集
有限时间范围内的所有检测到的标记点的残差后，我们得到校准参数（p
∗ kin,p ∗ cam），其为代价函数的最小化参数。
$$
(p^ _{kin}, p^ {cam}) = \arg\min {(p_{kin}, p_{cam})} \left[ \frac{1}{2} \sum_{m,k} \rho(|f_{m,k}(p_{kin}, p_{cam})|^2) \right].
$$
(9)
损失函数 ρ(•)用于减小大残差的影响，这些大残差主要由错误的标记检测 引起。我们使用C++库GoogleCeres对这个非线性最小二乘问题进行建模 和求解。10关于求解器迭代的初始猜测值，我们使用可用的CAD值作为长 度测量的初始值，并将角度偏移、小腿到足部角度和时间偏移设置为零。
对于相机内参，我们使用通过ROS相机标定器获得的标定文件。为了获得 相机外参和足部到图案角度的初始值，我们运行了一次成本较低的预标定， 仅将这些参数作为自由参数。

4.实验结果

记录并评估了三个数据集，使用所提出的标定框架对四足机器人 StarlETH的前腿进行标定。11选择机器人运动的方式使得标记点在图像 的不同区域中出现。每个数据集的持续时间约为30秒，求解器耗时10–20 秒，包括预标定步骤。表1中的所有参数均包含在标定过程中，除了身体尺 寸 bx，因为在仅考虑前腿且联合估计相机外参时该参数不可观测。这总共 产生了33个联合估计参数。图3显示了记录过程的快照以及标定的视觉结 果。包含相机内参显著改善了结果，并降低了优化残差。即使在图像边缘 区域（径向畸变较大的地方），标定模型也能准确地描绘出所有标记点的 位姿。

与运动相关的运动学参数的数值结果总结于表2中。校准后的长度测 量值与其对应的计算机辅助设计值之间的偏差约为五毫米（除 ls外）。标 准差相对较低，表明结果具有较高的精度和较低的数据集依赖性。获得的 关节角度偏移平均约为三度，标准差为一度，这对于提升机器人性能而言 确实值得关注。

图3. Top:数据集记录期间的机器人构型示例。 Bottom:校正后的相机图像叠加了腿部的标 定后运动学链（使用了标定后的内参）。建模的与观测到的标记位姿吻合良好。

表2.StarlETH的估计运动学参数。标定在所有三个数据集上分别进行，表中显示了均 值和标准差。

连杆长度∗(毫米)
	left			right
lh	lt	ls	lh	lt	ls
71.5 ± 0.9	194 ± 2	251 ± 7	62.6 ± 2.6	205 ± 1.9	269 ± 2

关节偏移（度）
	left			right
θh,偏移量	θt,偏移量	θs,偏移量	θh,偏移量	θt,偏移量	θs,偏移量
−3.8 ± 0.4	−2.5 ± 1.5	1.1 ± 2.5	−2.2 ± 0.3	2.2 ± 0.6	−5.6 ± 0.9

小腿到足部角度 θf(deg)	躯体测量∗(毫米)
left	right	by
24.8 ± 1.4	30.3 ± 1.2	182 ± 1

Note：对应的 CAD值为 lh= 68.5毫米、lt= 200毫米、ls= 235毫米和 by= 185毫米。柄部长度的值对应于之前较短的柄部段形状。
性能。同时，相机标定参数也得到了合理的结果。关节测量与图像之间的 时间偏移约为 −0.05秒。在图4中可以明显看出将 toffset包含在标定过程 中的重要性，图中绘制了标定后的像素误差。如果不考虑时间偏移，则可 以观察到与运动相关的误差。

图4.第一个数据集的像素误差结果。（a）完整的33自由度标定，均方根误差为2.3。（b）无 时间偏移的标定（32自由度），均方根误差为3.7。

5.结论

提出了一种快速且自动的足眼标定框架，该框架具有通用性，可同时对数 量可变的运动学腿链进行标定。该方法在四足机器人StarlETH上成功进行 了验证，整体运行时间仅为40–50秒。为了获得机器人足部的冗余测量， 我们临时在机器人上安装了相机和AR标记图案。随后，通过最小化测量得 到的标记位置与建模的标记位置之间的误差，估算了最可能的标定参数。
对标定参数的估计精度在机器人长度测量中达到了五毫米范围内。未来工 作可能包括对镜头畸变模型的评估或系统性的可观测性分析。