CN114161411B - 一种基于视觉的多足机器人运动学参数标定方法 - Google Patents

Abstract

本发明公开了一种基于视觉的多足机器人运动学参数标定方法。分别在机器人的机身和足端固定标定板，利用单目相机获得标定板的位姿，进而获得机器人机身与足端的相对位姿关系；根据机器人的关节角度数据，通过正运动学计算机身与足端的名义位姿关系，构建误差矩阵，利用最小二乘法估计运动学参数误差。本发明利用视觉技术依次对多足机器人的每条腿进行运动学参数标定，能够快速准确地完成运动学参数标定过程，可提高多足机器人足端的定位精度，从而改善多足机器人的行走效果。本方法仅需要单目相机和两块标定板即可完成多足机器人的运动学参数标定过程，无需专门的或昂贵的设备，成本较低，易于实现。

Description

一种基于视觉的多足机器人运动学参数标定方法

技术领域

本发明涉及机器人技术领域，特别是涉及一种基于视觉的多足机器人运动学参数标定方法。

背景技术

足式移动机器人相比于轮式移动机器人具有地形适应能力强、灵活性高等优点，因此被广泛应用于众多领域以替代人类完成各种复杂的工作。作为足式移动机器人的典型代表之一，六足机器人正逐渐成为机器人研究领域中的热点。

由于机器人的机械加工误差、安装误差、构件变形等原因，机器人的运动学参数存在一定的误差，包括各条腿关节长度、关节零位偏差等。这些尺寸上的误差会通过运动链传播，可能严重影响机器人足部的控制精度，从而影响足式机器人的行走效果。因此，对足式移动机器人开展运动学参数标定是十分重要的。

公开号为CN105808882A的发明专利申请公开了一种仿爬行类四足步行机器人运动参数的标定方法与装置，该方法建立了专用的标定装置，利用测量平台测量机器人的运动轨迹，通过计算其运动前后一系列的运动参数完成几何参数的标定。该方法需使用特制的标定平台，不具备通用性，并且标定平台的机械制造误差也会对标定过程产生影响。

公开号为CN107065558A的发明专利申请公开了一种基于机身姿态角度校正的六足机器人关节角度标定方法，该方法通过IMU获取六足机器人机身姿态角度，结合足式机器人系统运动学与逆运动学模型完成六足机器人支撑足各关节角度校正。该方法仅需要IMU和关节角度的数据即可完成标定，但容易受到足部打滑等因素的影响，且无法对关节角度之外的其他运动学参数进行标定。

发明内容

针对现有技术中存在的上述问题，本发明提供一种基于视觉的多足机器人运动学参数标定方法。该方法具有较高的精度，且成本较低，能够快速准确地完成多足机器人运动学参数地标定，从而保证多足机器人的运动轨迹精度。

一种基于视觉的多足机器人运动学参数标定方法，多足机器人包括机身和至少两条腿，每条腿包含至少两个串联的关节，腿的端部为足部，包括以下步骤：

(1)建立多足机器人的机身和各关节的坐标系，并建立运动学模型和误差模型；

(2)对相机进行标定，获得相机的内参矩阵；

(3)在机身和待标定的腿的足部上各固定一块标定板，将相机放置在合适的位置使得两块标定板均处在相机的视野范围内；

(4)控制正在标定的腿运动到工作范围内的一个随机位置，通过关节的编码器记录此时的关节角度；

(5)利用相机获得标定板在相机坐标系下的位姿，并通过坐标变换关系得到足部在机身坐标系下的位姿；

(6)利用当前的关节角度，通过正运动学计算出期望的足部位姿，并计算出期望的足部位姿与通过相机得到的足部位姿之间的误差；

(7)重复步骤(4)～(6)，得到多足机器人腿部处于不同位置时的一组数据；

(8)计算平均位姿误差，若小于预设的阈值则跳转到步骤(10)；否则执行步骤(9)；

(9)通过最小二乘法计算出多足机器人运动学参数的误差，并对运动学参数进行补偿，之后继续执行步骤(4)；

(10)将腿上的标定板更换到下一条待标定腿的足部，重复步骤(3)～(9)，直至多足机器人的多条腿均已完成标定。

本申请中多足机器人腿的数量可以是2条或2条以上，不同腿的数量均适用于本申请方法。优选的，所述多足机器人有2条腿、4条腿或6条腿。一般2条、4条或6条腿的机器人居多。

优选的，所述机身为矩形，各条腿分布于矩形机身的四条边上。比如，对于4足机器人，则在矩形机身的轴向两侧的两条边的两端各设置一条腿。对于6足机器人，则在矩形机身的轴向两侧的两条边的两端及中间位置各设置一条腿。

本申请中每条腿上包含的关节数量并没有严格限制，一般机器人每条腿包含三个关节，从机身到足部依次为侧摆关节、髋关节和膝关节。

本申请中相机使用单目相机即可，在标定过程中标定板均处于相机的视野中。

进一步优选的，所述标定板使用ChArUco标记板，并使用OpenCV处理相机照片，识别和获取标定板的位姿。优选使用OpenCV中ArUco模块提供的相关函数进行标定板的识别及位姿提取，具体使用cv：：aruco：：interpolateCornersCharuco函数检测ChArUco板的角点；使用cv：：aruco：：estimatePoseCharucoBoard函数估计ChArUco板的位姿。

优选的，步骤(5)中，假设机身和足部的标定板对应的坐标系分别为{M_B}和{M_F}，相机坐标系为{C}，利用相机分别获得两块标定板在相机坐标系下的位姿，用齐次变换矩阵和/>表示；之后通过坐标变换关系得到足部在机身坐标系下的位姿：其中，/>和/>表示标定板相对于机身、足部的固定位置，/>表示利用相机得到的足部在机身坐标系下的位姿。

优选的，步骤(6)中，利用当前的关节角度，通过正运动学计算出期望的足部位姿

其中，q₁，q₂，q₃为各关节的角度，为运动学参数，/>为各关节螺旋轴向量，s₁＝s₂＝s₃＝(0，0，0，0，0，1)^T，“∧”符号(即公式中字母上带有符号“∧”)表示将一个运动旋量向量写成矩阵形式：如/>则e为自然底数，/>为矩阵指数；

并计算出期望位姿与通过相机得到的位姿之间的误差Δe：

优选的，步骤(9)中，列出含有待辨识参数的方程，整理为y＝Jx的形式：

其中x为待辨识参数误差列向量，y为位姿误差列向量，为辨识矩阵，n为数据组数，通过最小二乘法计算出多足机器人运动学参数的误差：x＝(J^TJ)^-1J^Ty，用所求出的运动学参数误差对原有参数进行补偿：

和/>分别为补偿前后的运动学参数。

进一步优选的，步骤(7)中，得到n组多足机器人腿部处于不同位置时的数据，其中，n需要保证步骤(9)中的辨识矩阵非奇异，即n＞＝4，n大于等于4时就能确保计算的顺利进行，n如果小于4，则会造成计算过程可能进行不下去，无法得出最后的结果。当然，一般n取值越大，计算结果越精确，但相应的计算量也越大，当n大于30时计算结果趋于稳定，精度变化不明显，所以一般n取值在10～30范围内较为合适。

与现有技术相比，本发明的有益效果：

(1)本发明利用视觉技术依次对多足机器人的每条腿进行运动学参数标定，能够快速准确地完成运动学参数标定过程，可提高多足机器人足端的定位精度，从而改善多足机器人的行走效果。

(2)本方法仅需要单目相机和两块标定板即可完成多足机器人的运动学参数标定过程，无需专门的或昂贵的设备，成本较低，易于实现。

附图说明

图1为多足机器人运动学参数标定的流程图。

图2为六足机器人构型与坐标系示意图。

图3为标定过程的示意图。

具体实施方式

本发明多足机器人包括机身和至少两条腿，每条腿包含至少两个串联的关节，不同腿的数量均适用于本申请方法。机身为矩形，各条腿分布于矩形机身的四条边上。比如，对于4足机器人，则在矩形机身的轴向两侧的两条边的两端各设置一条腿。对于6足机器人，则在矩形机身的轴向两侧的两条边的两端及中间位置各设置一条腿。

以下实施方式中，以六足机器人且每条腿包含三个关节为例。

如图1所示为本申请六足机器人运动学参数标定的流程图。

如图2所示，本实施例所述的六足机器人包括机身和六条腿；六条腿分布于矩形机身的四条边上；每条腿包含三个关节，从机身到足部依次为侧摆关节、髋关节和膝关节。将六足机器人的每条腿视作三自由度的串联机器人，并依次对每条腿进行运动学参数标定。

所述的方法具体步骤如下：

步骤1：建立机器人机身、各个关节的坐标系，并建立运动学模型和误差模型。本实施例中采用局部指数积(POE)方法建立运动学模型和误差模型。

步骤2：对相机进行标定，获得相机的内参矩阵。本实施例中采用ChArUco标定板，使用OpenCV中提供的相关函数进行相机标定。

步骤3：如图3所示，在机器人机身和待标定的腿的足部上固定标定板，将相机放置在合适的位置使得两块标定板均处在相机的视野范围内。

步骤4：控制正在标定的腿运动到工作范围内的一个随机位置，通过关节的编码器记录此时的关节角度。

步骤5：假设机身和足部的标定板对应的坐标系分别为{M_B}和{M_F}，相机坐标系为{C}，利用相机分别获得两块标定板在相机坐标系下的位姿，用齐次变换矩阵和/>表示；之后通过坐标变换关系得到足部在机身坐标系下的位姿：其中，/>和/>表示标定板相对于机身、足部的固定位置。本实施例中采用ChArUco标定板，并使用OpenCV中ArUco模块提供的相关函数进行标定板的识别及位姿提取。具体使用cv：：aruco：：interpolateCornersCharuco函数检测ChArUco板的角点；使用cv：：aruco：：estimatePoseCharucoBoard函数估计ChArUco板的位姿。

步骤6：利用当前的关节角度，通过正运动学计算出期望的足部位姿并计算出期望位姿与通过相机得到的位姿之间的误差Δe；本实施例采用局部指数积方法进行运动学建模，具体公式如下：

其中，q₁，q₂，q₃为各关节的角度，为运动学参数，/>为各关节螺旋轴向量，本实施例中，/>“∧”符号表示将一个运动旋量向量写成矩阵形式：

如则 为矩阵指数；

步骤7：重复步骤4～6，得到n组机器人腿部处于不同位置时的数据；n需要保证后续的辨识矩阵非奇异，即n＞＝4。本实施例中取n＝20。

步骤8：计算n组数据的平均位姿误差判断是否达到要求精度，若小于预设的阈值则跳转到步骤10；否则执行步骤9。

步骤9：列出含有待辨识参数的方程，整理为y＝Jx的形式：

其中x为待辨识参数误差列向量，y为位姿误差列向量，为辨识矩阵。通过最小二乘法计算出机器人运动学参数的误差：x＝(J^TJ)^-1J^Ty。用所求出的运动学参数误差对原有参数进行补偿：

和/>分别为补偿前后的运动学参数。

之后继续执行步骤4。

步骤10：将腿上的标定板更换到下一条待标定的腿上，重复步骤3～9，直至机器人的六条腿均已完成标定。

Claims (5)

1.一种基于视觉的多足机器人运动学参数标定方法，多足机器人包括机身和至少两条腿，每条腿包含至少两个串联的关节，腿的端部为足部，其特征在于，包括以下步骤：

(1)建立多足机器人的机身和各关节的坐标系，并建立运动学模型和误差模型；

(2)对相机进行标定，获得相机的内参矩阵；

(3)在机身和待标定的腿的足部上各固定一块标定板，将相机放置在合适的位置使得两块标定板均处在相机的视野范围内；

(4)控制正在标定的腿运动到工作范围内的一个随机位置，通过关节的编码器记录此时的关节角度；

(5)利用相机获得标定板在相机坐标系下的位姿，并通过坐标变换关系得到足部在机身坐标系下的位姿；

(6)利用当前的关节角度，通过正运动学计算出期望的足部位姿，并计算出期望的足部位姿与通过相机得到的足部位姿之间的误差；

(7)重复步骤(4)～(6)，得到多足机器人腿部处于不同位置时的一组数据；

(8)计算平均位姿误差，若小于预设的阈值则跳转到步骤(10)；否则执行步骤(9)；

(9)通过最小二乘法计算出多足机器人运动学参数的误差，并对运动学参数进行补偿，之后继续执行步骤(4)；

(10)将腿上的标定板更换到下一条待标定腿的足部，重复步骤(3)～(9)，直至多足机器人的多条腿均已完成标定，

所述相机为单目相机，在标定过程中标定板均处于相机的视野中，

所述标定板使用ChArUco标记板，并使用OpenCV处理相机照片，识别和获取标定板的位姿，

步骤(5)中，假设机身和足部的标定板对应的坐标系分别为{M_B}和{M_F}，相机坐标系为{C}，利用相机分别获得两块标定板在相机坐标系下的位姿，用齐次变换矩阵和/>表示；之后通过坐标变换关系得到足部在机身坐标系下的位姿：其中，/>和/>表示标定板相对于机身、足部的固定位置，/>表示利用相机得到的足部在机身坐标系下的位姿，

步骤(6)中，利用当前的关节角度，通过正运动学计算出期望的足部位姿

其中，q₁,q₂,q₃为各关节的角度，为运动学参数向量，/>为各关节螺旋轴向量，s₁＝s₂＝s₃＝(0,0,0,0,0,1)^T，“^”符号表示将一个运动旋量向量写成矩阵形式，e为自然底数；并计算出期望位姿与通过相机得到的位姿之间的误差Δe：

步骤(9)中，列出含有待辨识参数的方程，整理为y＝Jx的形式：

其中x为待辨识参数误差列向量，y为位姿误差列向量，为辨识矩阵，n为数据组数，通过最小二乘法计算出多足机器人运动学参数的误差：x＝(J^TJ)^-1J^Ty，用所求出的运动学参数误差对原有参数进行补偿：

和/>分别为补偿前后的运动学参数。

2.如权利要求1所述基于视觉的多足机器人运动学参数标定方法，其特征在于，所述多足机器人有2条腿、4条腿或6条腿。

3.如权利要求1所述基于视觉的多足机器人运动学参数标定方法，其特征在于，所述机身为矩形，各条腿分布于矩形机身的四条边上。

4.如权利要求1所述基于视觉的多足机器人运动学参数标定方法，其特征在于，每条腿包含三个关节，从机身到足部依次为侧摆关节、髋关节和膝关节。

5.如权利要求1所述基于视觉的多足机器人运动学参数标定方法，其特征在于，步骤(7)中，得到n组多足机器人腿部处于不同位置时的数据，其中，n需要保证步骤(9)中的辨识矩阵非奇异，即n＞＝4。