CN113103225A - 一种基于图像测量的移动机器人多阶段平稳自主对接方法 - Google Patents

Abstract

本发明属于机器人技术领域，主要涉及一种基于图像测量的移动机器人多阶段平稳自主对接方法，包括如下步骤：在移动机器人上放置AprilTag标签和摄像头，其朝向分别与捕获端和主动端相同，通过摄像头获得其它移动机器人AprilTag标签相对于自身主动端的位姿信息；通过参数设计获得改进跟踪微分器，采用改进的跟踪微分器实现对原始位姿信息的跟踪和滤波；通过轨迹规划，使两个移动机器人的对接机构趋于一条直线上，为精确对接做好准备；设计对接控制律，输入摄像头的实时反馈数据，上位机控制两个移动机器人的相对距离和姿态偏差角变量，最终实现精确对接。本发明方法通过移动机器人的自动对接，有效提升移动机器人的地形适应能力以及构型多样性。

Description

一种基于图像测量的移动机器人多阶段平稳自主对接方法

技术领域

本发明属于机器人技术领域，具体涉及一种基于图像测量的移动机器人多阶段平稳自主对接方法。

背景技术

机器人是一种柔性设备，它能通过编程来适应新的工作，然而实际应用中很少使用这种情况。但传统的机器人都是根据特定的应用范围来开发的，固然对那些任务明确的产业应用来讲，这种机器人已经足够满足实际需要了，然而由于市场全球化的竞争，机器人的应用范围要求越来越广，而每种机器人的构形仅能适应一定的有限范围，因此机器人的柔性不能满足市场变化的要求，解决这一难题的方法就是开发移动机器人系统，它是由一套具有各种尺寸和性能特征的可交换的模块组成，能够被装配成各种不同构形的机器人，以适应不同的工作。因此移动机器人系统的研究已引起越来越多的研究者和产业应用的爱好。

当前国内外，主要是美、日的一些研究小组对移动机器人的体系结构、变形策略和控制算法等进行了研究。移动机器人因其在工业、科技、军事等领域的重要作用而广泛受到国内外特别是发达国家的关注，移动机器人的研究目前已经成为机器人研究的一个重要方向，并已取得一些重要的成果。尤其是移动机器人融合了最新的机电、传感器及计算机控制技术，具有良好的自组织、自适应能力，能够根据环境和任务优化自身的结构，快速、有效地的完成任务，因此移动机器人的研究在军事、航天、核工业等领域具有重大的意义。移动机器人是一种能根据任务需要，重新组合构型的机器人，它是在模块化机器人基础上发展起来的，移动机器人就是利用一些不同尺寸和性能的可互换的连杆和模块，根据工作环境和任务装配成不同构型的机器人。这种组合并不是简单的机械装配，参与重构机器人的各模块本身就是一种集通信、控制、驱动、传动为一体的单元，重构后的机器人将能适应新的工作环境和工作任务，具有很好的柔性。比较常见的移动机器人，例如，蛇形机器人。运动机理特殊的蛇形机器人有广阔的应用情景，例如战场上的扫雷，爆破，空间站的柔性机械手臂，通过能力很强的行星地表探测器等；且其模块化结构和高冗余度非常适应于条件非常恶劣而又要求高可靠性的战场、外层空间等环境。

现有的普通机器人大多构型单一，地形适应能力不强，无法完成现代社会多样化的任务需求。移动机器人的应用中，有时需要对接成组合机器人来增强地形适应能力，而现在的移动机器人之间的对接过程基本上都是由操作人员来手动操作，既浪费人工又对操作者有较高要求，对接成功率也不高。

发明内容

本发明的目的是：旨在提供一种基于图像测量的移动机器人多阶段平稳自主对接方法，用于解决移动机器人无法自动对接及地形适应能力差的问题。

为实现上述技术目的，本发明采用的技术方案如下：

一种基于图像测量的移动机器人多阶段平稳自主对接方法，包括如下步骤：

A1、在移动机器人上放置AprilTag标签和摄像头，其朝向分别与捕获端和主动端相同，通过摄像头获得其它移动机器人AprilTag标签相对于自身主动端的位姿信息；

A2、通过参数设计获得改进跟踪微分器，采用改进的跟踪微分器实现对原始位姿信息的跟踪和滤波；

A3、通过轨迹规划，使两个移动机器人的对接机构趋于一条直线上，为精确对接做准备；

A4、设计对接控制律，输入摄像头的实时反馈数据，上位机控制两个移动机器人的相对距离和姿态偏差角变量，最终实现精确对接。

2、根据权利要求1所述的一种基于图像测量的移动机器人多阶段平稳自主对接方法，其特征在于，步骤A1具体实现如下：

A101、输入摄像头拍摄的含有AprilTag标签的彩色图像；

A102、轮廓查找，自适应阈值分割，求解连通域，寻找可能组成AprilTag标签的轮廓；

A103、拟合四边形，对轮廓查找到的轮廓，进行直线拟合；

A104、位姿输出，解码并通过编码库判断四边形是否是AprilTag标签，若是，用摄像头内参和标签的物理边长构建位姿数据的PnP方程，求解得到标签在相机坐标系里的位置坐标以及在三个坐标轴上的旋转角度(yaw、pitch、roll)。

进一步，所述跟踪微分器二阶离散形式为：

其中，d＝r*h；d₀＝h*d；z＝e+h*x₂；

e＝x₁-v；v

输入信号；x₁为跟踪数据；x₂为x₁的导数；r为速度因子，h为滤波因子。

进一步，改进跟踪微分器设计如下：

选取k个数据为一队列，队列内数据的幅值和频率可以描述数据的稳定性；

用标准差σ来定量描述幅值的稳定性，数据的频率稳定性则用队内离群点数据的统计值来描述；

计算队列内的离群点阈值，s＝α·σ，α为阈值系数，取大于3；

统计队列内幅值大于离群点阈值s的数据个数n；

假设离群点按最大频率分布，可以得到队列数据最大频率值为

其中m为队列长度；

利用以上两个幅值和频率的定量描述值，可以得到队列速度因子r的表达式：

进一步，采用改进的跟踪微分器来对原始位姿信息的跟踪和滤波，具体步骤如下：

A201、确定队列内数据个数，将队列更新周期设定为AprilTag算法反馈位姿信息的周期；

A202、计算队内标准差，设定离群点阈值，统计队内离群点数量，最后计算速度因子；

A203、更新队内数据，剔除队首数据，队尾加入新的位姿数据，用速度因子不断更新的跟踪微分器对位姿信息进行滤波。

进一步，步骤A3的具体实现如下：

A301、对接主动方移动机器人根据位姿信息中的姿态偏差角来进行轨迹规划，通过设定起点角度与终点角度以及时间，进行多项式插补，上位机下发角速度控制对接主动方移动机器人旋转至与对接被动方移动机器人呈垂直状态；

A302、根据相对位置坐标再进行轨迹规划得到的线速度及角速度来使对接主动方移动机器人直线前进一段相应的距离，再旋转90°，使该对接主动方移动机器人的主动端朝向对接被动方移动机器人的捕获端。

进一步，步骤A4具体实现如下：

A401、设计对接控制律，输入摄像头的实时反馈数据，上位机控制两个移动机器人的相对距离和姿态偏差角等变量，使这些变量收敛为0；

A402、使对接主动方移动机器人摄像头中心和对接被动方移动机器人AprilTag标签中心的直线距离为ρ，对接主动方移动机器人朝向与两中心连线的夹角为α，姿态偏差角为θ，两个角度之和为β＝α+θ。可得：

设v为对接主动方移动机器人的速度；

A403、采用一个正定二次型李雅普诺夫函数：

其中，ρ和β分别代表了距离误差和角度误差，对该函数求导，可得：

式子可化为：

为了使

和

为非正，从而李雅普诺夫函数收敛，则线速度取v＝K₁ρcosα，角速度取

得：

可以得出末端对接过程的控制律为v＝K₁ρcosα，

其中ρ，α，θ都是摄像头能实时获得到的位姿信息中所包含的数据；

A404、上位机根据摄像头的反馈信息和该控制律来下发线速度和角速度给对接主动方机器人，完成自主对接。

采用上述技术方案的发明，具有如下优点：

1、在移动机器人遇到形态难以翻越的障碍或阶梯时，能够自主对接成组合机器人来增强越障能力；

2、提升了移动机器人的自主性和智能性，使对接过程不需要人工操作；

3、提高了移动机器人的对接成功率，在1m的距离内，对接成功率可达99％。

附图说明

本发明可以通过附图给出的非限定性实施例进一步说明；

图1为移动机器人的实物模型图；

图2为对接实验过程中摄像头采集到的距离偏差和角度偏差曲线图；

图3为末端对接过程中的控制律的模型图；

图4为末端对接过程中的控制框图。

具体实施方式

以下将结合附图和具体实施例对本发明进行详细说明，需要说明的是，在附图或说明书描述中，相似或相同的部分都使用相同的图号，附图中未绘示或描述的实现方式，为所属技术领域中普通技术人员所知的形式。另外，实施例中提到的方向用语，例如“上”、“下”、“顶”、“底”、“左”、“右”、“前”、“后”等，仅是参考附图的方向，并非用于限制本发明的保护范围。

如图1-4所示，一种基于图像测量的移动机器人多阶段平稳自主对接方法，包括如下步骤：

A1、在移动机器人上放置AprilTag标签和摄像头，其朝向分别与捕获端和主动端相同，通过摄像头获得其它移动机器人AprilTag标签相对于自身主动端的位姿信息；

A2、通过参数设计获得改进跟踪微分器，采用改进的跟踪微分器实现对原始位姿信息的跟踪和滤波；

A3、通过轨迹规划，使两个移动机器人的对接机构处在一条直线上，为精确对接做准备；

A4、设计对接控制律，输入摄像头的实时反馈数据，上位机控制两个移动机器人的相对距离和姿态偏差角变量，最终实现精确对接。

实施例1：位姿信息的获取

如图1所示，移动机器人的对接机构分为主动端和捕获端。在进入自主对接模式时，移动机器人首先要通过AprilTag算法获取其他的位姿信息。具体操作是：在机器人上放置AprilTag标签和摄像头，其朝向分别与捕获端和主动端相同。移动机器人就可以通过摄像头来获得其他机器人上的AprilTag标签相对于自身主动端的位姿信息，该数据即为其他机器人的捕获端位姿信息。

AprilTag算法的具体流程如下：

1.输入摄像头所拍摄的含有AprilTag标签的彩色图像。

2.轮廓查找。自适应阈值分割，求解连通域，寻找可能组成AprilTag标签的轮廓。

3.拟合四边形。对轮廓查找到的轮廓，进行直线拟合。

4.位姿输出。解码并通过编码库判断四边形是否是AprilTag标签，若是，用摄像头内参和标签的物理边长构建位姿数据的PnP方程，求解得到标签在相机坐标系里的位置坐标以及在三个坐标轴上的旋转角度(yaw、pitch、roll)。

实施例2：改进跟踪微分器的设计

跟踪微分器的二阶离散形式为：

其中，d＝r*h；d₀＝h*d；z＝e+h*x₂；

e＝x₁-v；v为输入信号；x₁为跟踪数据；x₂为x₁的导数；r为速度因子，h为滤波因子。

由上式可知，主要调节参数为速度因子和滤波因子。h越大，滤除噪声效果越强，但输出数据的相位延迟越大，一般取在采样周期附近。r越大，跟踪速度越快，但太大会使其与原信号重叠，其值与输入数据的幅值和频率呈正比关系。目前一般将速度因子设定为静态值，对于一些应用场合，其难以在全局内取得较好的滤波效果。本发明根据速度因子和数据频率与幅值的关系，采用划分输入数据队列的方法对速度因子进行动态更新，以获取更好的滤波效果。

本发明选取k个数据为一队列，队列内数据的幅值和频率可以描述数据的稳定性。用标准差σ来定量描述幅值的稳定性，数据的频率稳定性则用队内离群点数据的统计值来描述：

1、计算队列内的离群点阈值，s＝α·σ(α为阈值系数，取大于3)。

2、统计队列内幅值大于离群点阈值s的数据个数n。假设离群点按最大频率分布，可以得到队列数据最大频率值为

其中m为队列长度。

3、利用以上两个幅值和频率的定量描述值，可以得到队列速度因子r的表达式：

实施例3：采用改进的跟踪微分器来对原始位姿信息的跟踪和滤波

首先，确定队列内数据个数，将队列更新周期设定为AprilTag算法反馈位姿信息的周期。

1、计算队内标准差，设定离群点阈值，统计队内离群点数量，最后计算速度因子。

2、更新队内数据，剔除队首数据，队尾加入新的位姿数据。

3、用速度因子不断更新的跟踪微分器对位姿信息进行滤波。

实施例4：控制对接

在对接过程中有可能存在外界干扰、机器人自身打滑等现象，从而使机器人运动过程中在距离或是方向上产生偏差。若在对接最后时刻两机器人的对接机构不平行或是水平误差超过1cm，就会导致对接失败。

如图2-3所示，本发明的对接过程分为两个阶段。第一个阶段为调整对齐阶段，尽量使两个机器人的对接机构处在一条直线上，为第二阶段的精确对接做好准备。首先，对接主动方机器人根据位姿信息中的姿态偏差角来进行轨迹规划，通过设定起点角度与终点角度以及时间，进行多项式插补，上位机下发角速度控制对接主动方机器人旋转至与对接被动方机器人呈垂直状态。然后，根据相对位置坐标再进行轨迹规划得到的线速度及角速度来使对接主动方机器人直线前进一段相应的距离，最后再旋转90°，使该的主动端朝向另一机器人的捕获端。

第二个阶段为末端对接。设计对接控制律，输入摄像头的实时反馈数据，上位机控制两个移动机器人的相对距离和姿态偏差角等变量，使这些变量收敛为0，最终实现精确对接。设对接主动方移动机器人摄像头中心和对接被动方移动机器人标签中心的直线距离为ρ，对接主动方移动机器人朝向与两中心连线的夹角为α，姿态偏差角为θ，两个角度之和为β＝α+θ。可得：

设v为对接主动方机器人的速度。

采用一个正定二次型李雅普诺夫函数：

其中，ρ和β分别代表了距离误差和角度误差。

对该函数求导，可得：

式子可化为：

为了使

和

为非正，从而李雅普诺夫函数收敛。则线速度取v＝K₁ρcosα，角速度取

得：

可以得出末端对接过程的控制律为v＝K₁ρcosα，

其中ρ，α，θ都是摄像头能实时获得到的位姿信息中所包含的数据。上位机根据摄像头的反馈信息和该控制律来下发线速度和角速度给对接主动方机器人，完成自主对接。

本发明对一种基于图像测量的移动机器人多阶段平稳自主对接方法进行了详细介绍。具体实施例的说明只是用于帮助理解本发明的方法及其核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，改进和修饰也落入本发明权利要求的保护范围内。

Claims (7)

1.一种基于图像测量的移动机器人多阶段平稳自主对接方法，其特征在于，包括如下步骤：

A1、在移动机器人上放置AprilTag标签和摄像头，其朝向分别与捕获端和主动端相同，通过摄像头获得其它移动机器人AprilTag标签相对于自身主动端的位姿信息；

A2、通过参数设计获得改进跟踪微分器，采用改进的跟踪微分器实现对原始位姿信息的跟踪和滤波；

A3、通过轨迹规划，使两个移动机器人的对接机构趋于一条直线上，为精确对接做准备；

A4、设计对接控制律，输入摄像头的实时反馈数据，上位机控制两个移动机器人的相对距离和姿态偏差角变量，最终实现精确对接。

2.根据权利要求1所述的一种基于图像测量的移动机器人多阶段平稳自主对接方法，其特征在于，步骤A1具体实现如下：

A101、输入摄像头拍摄的含有AprilTag标签的彩色图像；

A102、轮廓查找，自适应阈值分割，求解连通域，寻找可能组成AprilTag标签的轮廓；

A103、拟合四边形，对轮廓查找到的轮廓，进行直线拟合；

A104、位姿输出，解码并通过编码库判断四边形是否是AprilTag标签，若是，用摄像头内参和标签的物理边长构建位姿数据的PnP方程，求解得到标签在相机坐标系里的位置坐标以及在三个坐标轴上的旋转角度(yaw、pitch、roll)。

3.根据权利要求1所述的一种基于图像测量的移动机器人多阶段平稳自主对接方法，其特征在于，所述跟踪微分器二阶离散形式为：

其中，d＝r*h；d₀＝h*d；z＝e+h*x₂；

e＝x₁-v；v为输入信号；x₁为跟踪数据；x₂为x₁的导数；r为速度因子，h为滤波因子。

4.根据权利要求3所述的一种基于图像测量的移动机器人多阶段平稳自主对接方法，其特征在于，改进跟踪微分器设计如下：

选取k个数据为一队列，队列内数据的幅值和频率可以描述数据的稳定性；

用标准差σ来定量描述幅值的稳定性，数据的频率稳定性则用队内离群点数据的统计值来描述；

计算队列内的离群点阈值，s＝α·σ，α为阈值系数，取大于3；

统计队列内幅值大于离群点阈值s的数据个数n；

假设离群点按最大频率分布，可以得到队列数据最大频率值为

其中m为队列长度；

利用以上两个幅值和频率的定量描述值，可以得到队列速度因子r的表达式：

5.根据权利要求4所述的一种基于图像测量的移动机器人多阶段平稳自主对接方法，其特征在于，采用改进的跟踪微分器来对原始位姿信息的跟踪和滤波，具体步骤如下：

A201、确定队列内数据个数，将队列更新周期设定为AprilTag算法反馈位姿信息的周期；

A202、计算队内标准差，设定离群点阈值，统计队内离群点数量，最后计算速度因子；

A203、更新队内数据，剔除队首数据，队尾加入新的位姿数据，用速度因子不断更新的跟踪微分器对位姿信息进行滤波。

6.根据权利要求1所述的一种基于图像测量的移动机器人多阶段平稳自主对接方法，其特征在于，步骤A3的具体实现如下：

A301、对接主动方移动机器人根据位姿信息中的姿态偏差角来进行轨迹规划，通过设定起点角度与终点角度以及时间，进行多项式插补，上位机下发角速度控制对接主动方移动机器人旋转至与对接被动方移动机器人呈垂直状态；

A302、根据相对位置坐标再进行轨迹规划得到的线速度及角速度来使对接主动方移动机器人直线前进一段相应的距离，再旋转90°，使该移动机器人的主动端朝向对接被动方移动机器人的捕获端。

7.根据权利要求1所述的一种基于图像测量的移动机器人多阶段平稳自主对接方法，其特征在于，步骤A4具体实现如下：

A401、设计对接控制律，输入摄像头的实时反馈数据，上位机控制两个移动机器人的相对距离和姿态偏差角等变量，使这些变量收敛为0；

A402、使对接主动方移动机器人的摄像头中心和对接被动方移动机器人的AprilTag标签中心的直线距离为ρ，对接主动方移动机器人朝向与两中心连线的夹角为α，姿态偏差角为θ，两个角度之和为β＝α+θ。可得：

设v为对接主动方移动机器人的速度；

A403、采用一个正定二次型李雅普诺夫函数：

其中，ρ和β分别代表了距离误差和角度误差，对该函数求导，可得：

式子可化为：

为了使

和

为非正，从而李雅普诺夫函数收敛，则线速度取v＝K₁ρcosα，角速度取

得：

可以得出末端对接过程的控制律为v＝K₁ρcosα，

其中ρ，α，θ都是摄像头能实时获得到的位姿信息中所包含的数据；

A404、上位机根据摄像头的反馈信息和该控制律来下发线速度和角速度给对接主动方机器人，完成自主对接。

Images