CN113641174A - 一种滑移转向模块化机器人的自主对接控制方法 - Google Patents

Abstract

一种滑移转向模块化机器人的自主对接控制方法，将被对接机器人称为母头模块，将主动实施对接动作机器人称为公头模块，母头模块处于静止状态，公头模块处于运动状态；在母头模块上粘贴二维码作为靶标，在公头模块上加装摄像机，通过摄像机拍摄二维码图像，并解算二维码相对于摄像机坐标系的姿态四元数，获取模块间相对姿态；基于行为的步进控制，实现公头模块与母头模块的对接。本发明通过视觉与合作靶标，借助摄像机检测靶标的位姿，以准确估计两个待对接机器人的相对位姿，弥补了定位系统的误差；能够有效地解决滑移转向模块化机器人在自重构的对接过程中存在的问题，实现了单元模块精确对接控制。

Description

一种滑移转向模块化机器人的自主对接控制方法

技术领域

本发明涉及一种用于对滑移转向模块化机器人进行自主对接控制的方法，属于移动机器人对接技术领域。

背景技术

对于以四轮差速滑移转向轮式平台为单元模块的模块化自重构机器人，其变构过程的关键技术之一的自主对接技术，存在难度。该类型机器人由多个单元模块铰接构成，每个单元模块是具备独立完整的运动、感知和控制能力的四轮滑转轮式机器人，即多个机器人通过铰接连结为一个大型的机器人。四轮滑动转向机器人通过对两侧轮子的转速调节，驱动机器人移动。由于轮子本身无法改变滚动方向(阿克曼转向角)，滑动转向机器人通过轮子在地面的侧滑实现转向。因此，其运动中存在滑动误差，导致机器人运动轨迹控制困难。此外，在不同的地面条件下，滑动转向的动力学参数难以估计，导致机器人在不同地面上的滑动误差不一致，难以建立统一模型进行补偿。

在精确对接过程中滑移转向机器人存在以下问题：

1、精确控制难度大：滑动转向机器人的地面接触动力学参数难以建模估计，地面摩擦系数、轮胎磨损、轮胎气压、温度湿度等因素都会影响滑移量的估算。

2、定位系统存在偏差：在自重构过程中，多个移动机器人借助定位系统、轨迹规划技术和轨迹跟踪控制技术，由分散状态聚集到“准对接”状态。在汇聚过程中，定位系统存在一定误差，当两个待对接机器人的相对位姿偏差在定位系统误差范围内时，定位结果已经无法反应两机器人的准确的相对位姿，仅靠定位系统难以实施精确对接。

3、四轮滑动转向机器人无法横向移动，即速度方向不能垂直于轮子滚动前进方向。对于两个待对接机器人之间的横向偏差，机器人难以消除。

4、机器人具备矩形外型，在多机器人密集聚集状态下，大幅度原地转向将会导致机器人相撞，因此无法通过大幅度(90度)原地转向弥补横向误差。

发明内容

本发明针对现有滑移转向模块化机器人的对接技术存在的不足，提出一种滑移转向模块化机器人的自主对接控制方法，该方法是在视觉和合作靶标引导下的基于行为的步进控制，实现了单元模块精确对接控制。

本发明的滑移转向模块化机器人的自主对接控制方法，是：

将被对接机器人(滑移转向模块化机器人)称为母头模块，将主动实施对接动作的机器人称为公头模块，母头模块处于静止状态，公头模块处于运动状态；在母头模块上粘贴二维码作为靶标，在公头模块上加装摄像机(单目摄像机)，通过摄像机拍摄二维码图像，并解算二维码相对于摄像机坐标系的姿态四元数(公知属于，学术上有明确定义)，获取模块(母头模块和公头模块)相对姿态；基于行为的步进控制，实现公头模块与母头模块的对接；

使用θ_ori和x_angle描述二维码姿态，使用z_d描述二维码位置，θ_ori是二维码平面与摄像机坐标系x轴的夹角，用于描述二维码自身相对于摄像机坐标系的旋转；x_angle是二维码中心与摄像机坐标系原点的连线与摄像机坐标系z轴之间的夹角，用于描述二维码偏离摄像机中心的偏角；z_d是二维码中心与摄像机坐标系中心的连线长度，用于描述二维码距离摄像机的距离。

所述基于行为的步进控制，是在视觉(摄像机拍摄的二维码图像)引导下，反复进行偏离和对准运动，步进地逐步消除对接模块之间的偏差，实现精确对接。

所述基于行为的步进控制，包括单纵向对接、单横向对接和横纵耦合对接的步进控制；单纵向对接是指一个公头模块与一个母头模块首尾相接而横向没有其它模块，单纵向对接的步进控制采用偏转、后退、归准和前进(偏转-后退-归准-前进)的行为序列进行横向偏差消除；单横向对接是指当一个公头模块与一个母头模块横向相连而纵向没有其它模块，单横向对接的步进控制采用对接、平齐、偏转、前进和退齐(“对接-平齐-偏转-前进-退齐”)的行为序列进行横向偏差消除；横纵耦合对接是指一个公头模块与两个母头模块需要同时进行纵向对接和横向对接，在母头模块后面和侧面两个方向各放置一个二维码，在公头模块的前面和侧面对应放置两个摄像头，横纵耦合对接的步进控制采用对接、平齐、偏转、后退、进齐和修正(“对接-平齐-偏转-后退-进齐-修正”)的行为序列进行横向偏差消除；

构建以母头模块为中心，以公头模块相对于母头模块的距离为半径的三环行为区划分，由内至外为对接区、调整区和接近区；在对接区内实施对接判断，该区范围为二维码距离z_d[0,0.25]米范围的空间(z_d≤0.25)；调整区用于步进调整公头模块横向偏差，实现精确对准，范围为二维码距离[0.25,0.7]米范围空间(z_d≥0.25∧z_d≤0.7)；接近区用于公头模块靠近母头模块(z_d＞0.7)。

所述单纵向对接的步进控制各行为描述如下：

行为序列描述如下：

①对接行为：

当模块满足对接条件，即二维码的θ_ori和x_angle在以下对接阈值内时，模块执行对接行为；

上式中，θ_ori是二维码平面与摄像机坐标系x轴的夹角，x_angle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角；

对接行为的终止条件：当公头模块插入母头模块后，判定对接完成；

②偏转行为：

当公头模块位于对接区，且不满足对接条件时，公头模块执行偏转行为；该行为的条件由下式描述，偏转方向朝向母头模块轴线外侧。

上式中，θ_ori是二维码平面与摄像机坐标系x轴的夹角，ω是公头模块旋转角速度，z_d是二维码中心与摄像机坐标系中心的连线长度；

偏转行为的终止条件：当公头模块偏转至与母头模块平行时，或二维码偏离摄像机中心的偏角超过二维码可被检测的最大偏角范围时，偏转行为停止，即：

ω＝0，ifθ_ori∈[-0.1,0.1]∨x_angle∈[15,+∞)∨x_angle∈(-∞,-15)，

上式中，θ_ori是二维码平面与摄像机坐标系x轴的夹角，ω是公头模块旋转角速度，x_angle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角；

③后退行为：

当偏转行为终止，公头模块开始执行后退行为，由当前位置直线后退；当公头模块后退至接近环区内，后退行为停止，即：

上式中，v是公头模块前进线速度，z_d是二维码中心与摄像机坐标系中心的连线长度；

④归准行为：

当后退行为终止，公头模块开始执行归准行为，通过原地转向缩小二维码在摄像机画面中的偏角，当公头模块正对二维码时，归准行为停止，即：

ω＝0,ifx_angle∈[-0.7,0.7]，

上式中，ω是公头模块旋转角速度，x_angle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角；

⑤前进行为：

当归准行为终止，公头模块开始执行前进行为；公头模块控制左右轮转速向前运动，并对二维码在摄像机画面中的偏角进行伺服控制，确保模块正朝二维码运动；当模块移动至对接区边缘，前进行为结束，判断当前状态是否满足对接条件；若满足，执行对接行为；若否，执行偏转行为。

所述单横向对接的步进控制各行为的描述为：

①对接行为：

当模块满足对接条件，即二维码的θ_ori和x_angle在下式对接阈值内时，模块执行对接行为；

上式中，θ_ori是二维码平面与摄像机坐标系x轴的夹角，x_angle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角；

对接行为的终止条件：当公头插入母头后，判定对接完成；

②平齐行为：

当公头模块初始状态与母头模块不平齐时，公头模块执行平齐行为；通过控制公头模块线速度对x_angle进行伺服，通过控制公头模块角速度对θ_ori进行伺服，使公母模块位姿侧向平齐；

平齐行为的终止条件：当公头模块偏转至与母头模块平行时，或二维码偏离摄像机中心的偏角超过二维码可被检测的最大偏角范围时，偏转行为停止，即：

上式中，θ_ori是二维码平面与摄像机坐标系x轴的夹角，ω是公头模块旋转角速度，x_angle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角，v是公头模块前进线速度；

③偏转行为：

当平齐行为终止，公头模块开始执行偏转行为，由当前位置原地转向，朝向母头模块转动；当二维码偏离摄像机中心超过角度阈值时，偏转停止，即：

ω＝0，ifx_angle＞15，

上式中，ω是公头模块旋转角速度，x_angle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角；

④前进行为：

当偏转行为终止，公头模块开始执行前进行为；公头模块向其头部方向以恒定速度直线运动，当二维码偏离摄像机中心的偏角接近0时，前进行为结束；

v＝0，ifx_angle∈[-0.7,0.7]，

上式中，x_angle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角，v是公头模块前进线速度；

⑤退齐行为：

当前进行为终止，公头模块开始执行退齐行为，通过控制模块线速度对x_angle进行伺服，通过控制公头模块角速度对θ_ori进行伺服，使公头模块后退至与母头模块平齐状态；检测当前二维码与摄像机距离，若距离小于对接阈值，执行对接行为；若否，执行偏转行为。

所述横纵耦合对接的步进控制各行为描述为：

①对接行为：

当模块满足对接条件，即两个二维码的

和

在下式对接阈值内时，模块执行对接行为；

上式中，

分别是横向和纵向二维码平面与对应侧摄像机的摄像机坐标系x轴的夹角，ω是公头模块旋转角速度，z_d是二维码中心与摄像机坐标系中心的连线长度，

和

分别是横向和纵向二维码中心与对应侧摄像机的摄像机坐标系原点的连线与z轴之间的夹角，v是公头模块前进线速度；

对接行为的终止条件：当横向和纵向公头均插入母头后，判定对接完成；

②平齐行为：

当公头模块初始状态与横向母头模块不平齐时，模块执行平齐行为；通过控制公头模块线速度对x_angle进行伺服，通过控制公头模块角速度对θ_ori进行伺服，使公母模块位姿侧向偏平齐；

平齐行为的终止条件：当公头模块运动至与母头模块纵向平行和横向偏差15度时，平齐行为停止，即：

上式中，

是横向二维码平面与对应侧摄像机的摄像机坐标系x轴的夹角，ω是公头模块旋转角速度，z_d是二维码中心与摄像机坐标系中心的连线长度，

是横向二维码中心与对应侧摄像机的摄像机坐标系原点的连线与z轴之间的夹角，v是公头模块前进线速度；

③偏转行为：

当平齐行为终止，模块开始执行偏转行为，由当前位置原地转向，向远离母头模块方向转动；当二维码偏离摄像机中心超过角度阈值时，偏转停止，即：

上式中，ω是公头模块旋转角速度，

是横向二维码中心与对应侧摄像机的摄像机坐标系原点的连线与z轴之间的夹角；

④后退行为

当偏转行为终止，公头模块开始执行后退行为；公头模块向其尾部方向以恒定速度直线运动，当二维码偏离摄像机中心超过角度阈值时，后退行为结束，即；

上式中，

是横向二维码中心与对应侧摄像机的摄像机坐标系原点的连线与z轴之间的夹角，v是公头模块前进线速度；

⑤进齐行为：

当前进行为终止，模块开始执行进齐行为，通过控制公头模块线速度对

进行伺服，通过控制公头模块角速度对

进行伺服，使公头模块前进至与母头模块纵向平行和横向偏差15度的偏平齐状态；检测当前二维码与摄像机距离，若距离小于对接阈值，执行修正行为；若否，执行偏转行为；

⑥修正行为

当横向对接距离满足对接条件，而纵向对接距离大于对接阈值时，公头模块开始执行修正行为，通过控制公头模块线速度对

进行伺服，通过控制公头模块角速度对

进行伺服，使公头模块前进并插入纵向对接母头中；前进过程中检测

值，若

满足对接条件，执行对接行为；若否，继续前进。

本发明能够有效地解决滑移转向模块化机器人在自重构的对接过程中存在的问题，实现了单元模块精确对接控制，具有以下特点：

1.在母头模块上粘贴二维码靶标，在公头模块上加装单目摄像机，通过视觉+合作靶标技术，借助摄像机检测靶标的位姿，以准确估计各个模块的相对位姿，弥补了定位系统的误差；

2.利用视觉+合作靶标作为测量反馈，构建模块的整体位姿控制闭环，将滑移偏差包含进整体控制闭环中，滑移产生的偏差可以转化为靶标偏转，进而通过步进控制逐步消除滑移偏差，有效地消除了环境条件对于滑移的影响；

3.基于行为的步进控制技术通过多次调整，可以逐步地、步进式地消除横向偏差；

4.在基于行为的步进控制技术的调整过程中，模块转向角度小，在密集聚集状态下，不会导致多模块之间发生碰撞。同时可使模块在运动全过程中，摄像机均可拍摄到二维码靶标，以获取实时相对位姿信息。

附图说明

图1是本发明滑移转向模块化机器人的自主对接控制方法的流程图。

图2是本发明中靶标位姿关键变量几何意义示意图。

图3是本发明中三环行为区划分示意图。

图4是本发明中纵向对接过程的示意图。

图5是本发明中单纵向对接流程图。

图6是本发明中纵向对接过程的示意图。

图7是本发明中横向对接流程图。

图8是本发明中横纵耦合对接过程的示意图。

图9是本发明中横纵耦合对接流程图。

具体实施方式

本发明的方法用于滑移转向模块化机器人的自主对接控制，其流程如图1所示。在母头模块上粘贴二维码靶标，在公头模块上加装单目摄像机，通过视觉+合作靶标技术，借助摄像机检测靶标的位姿，以准确估计待对接模块的相对位姿，弥补了定位系统的误差。

采用视觉+合作靶标引导实现自主对接。通过摄像头拍摄二维码图像，并解算二维码相对于摄像机坐标系的姿态四元数，获取模块间相对姿态。

使用θ_ori和x_angle描述二维码姿态，使用z_d描述二维码位置。三个关键变量几何意义如图2所示。图2中坐标系为摄像机坐标系。θ_ori是二维码平面与摄像机坐标系x轴的夹角，用于描述二维码自身相对于摄像机坐标系的旋转；x_angle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角，用于描述二维码偏离摄像机中心的偏角；z_d是二维码中心与摄像机坐标系中心的连线长度，用于描述二维码距离摄像机的距离。

基于行为的步进控制是在视觉引导下、反复多次进行“偏离—对准”运动，步进地逐步消除对接模块之间的偏差，实现精确对接。步进式的运动模式不会产生大范围的模块转动，因此可以避免密聚状态下的碰撞，同时可使模块在运动全过程中，摄像机均可拍摄到二维码靶标，以获取实时相对位姿信息。此外，基于行为的步进控制技术构建模块运动整体闭环，滑移产生的偏差可以转化为靶标偏转，进而通过步进控制逐步消除滑移偏差，因此能够解决因地面环境因素导致的滑移不确定问题。

针对模块化自重构机器人变构过程中常见的三类不同的对接方向，采用三种不同的行为序列实现步进控制，即：单纵向对接、单横向对接和横纵耦合对接。下文中，将待对接机器人称为母头模块，处于静止状态；将主动实施对接模块称为公头模块，处于运动状态。

1.单纵向对接

纵向对接过程如图4所示，单纵向对接流程如图5所示。

当两模块首尾相接，横向没有其它模块时，该对接类型为单纵向对接。对接母头模块静止不动，对接公头模块运动并实施对接。步进控制采用“偏转-后退-归准-前进”行为序列进行横向偏差消除，构建以对接母头模块为中心、以公头模块相对于母头模块的距离为半径的三环行为区划分，环区划分如图3所示，由内至外为对接区、调整区和接近区。

在对接区内实施对接判断，该区范围为二维码距离z_d[0,0.25]米范围的空间；调整区用于步进调整公头模块横向偏差，实现精确对准，范围为二维码距离[0.25,0.7]米范围空间；接近区用于公头模块靠近母头模块。分别具体表示为：z_d≤0.25，z_d≥0.25∧z_d≤0.7，z_d＞0.7。

单纵向对接过程的行为类型描述如下。

①对接行为

当模块满足对接条件，即二维码的θ_ori和x_angle在对接阈值内时(式1)，模块执行对接行为。

上式中，θ_ori是二维码平面与摄像机坐标系x轴的夹角，x_angle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角。

对接行为的终止条件：当公头模块插入母头模块后，由传感器检测插入动作，判定对接完成。

②偏转行为

当公头模块位于对接区，且不满足对接条件时，公头模块执行偏转行为。该行为的条件由下式描述。偏转方向朝向母头模块轴线外侧。

上式中，θ_ori是二维码平面与摄像机坐标系x轴的夹角，ω是公头模块旋转角速度，z_d是二维码中心与摄像机坐标系中心的连线长度。

偏转行为的终止条件：当公头模块偏转至与母头模块平行时，或二维码偏离摄像机中心的偏角超过二维码可被检测的最大偏角范围时，偏转行为停止，即：

ω＝0，ifθ_ori∈[-0.1,0.1]∨x_angle∈[15,+∞)∨x_angle∈(-∞,-15) (3)

上式中，θ_ori是二维码平面与摄像机坐标系x轴的夹角，ω是公头模块旋转角速度，x_angle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角。

③后退行为

当偏转行为终止，公头模块开始执行后退行为，由当前位置直线后退。当公头模块后退至接近环区内，后退行为停止。即：

上式中，v是公头模块前进线速度，z_d是二维码中心与摄像机坐标系中心的连线长度。

④归准行为

当后退行为终止，公头模块开始执行归准行为，通过原地转向缩小二维码在摄像机画面中的偏角。当公头模块正对二维码时，归准行为停止。即：

ω＝0,ifx_angle∈[-0.7,0.7] (5)

上式中，ω是公头模块旋转角速度，x_angle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角。

⑤前进行为

当归准行为终止，模块开始执行前进行为。模块控制左右轮转速向前运动，并对二维码在摄像机画面中的偏角进行伺服控制，确保模块正朝二维码运动。当模块移动至对接区边缘，前进行为结束，判断当前状态是否满足对接条件。若满足，执行对接行为；若否，执行偏转行为。

2.单横向对接

纵向对接过程如图6所示，横向对接流程如图7所示。

当两模块左右相连，纵向没有其它模块时，该对接类型为单横向对接。对接母头模块静止不动，对接公头模块运动并实施对接。步进控制采用“平齐-偏转-前进-退齐”行为序列进行横向偏差消除。

单横向对接过程的行为类型描述如下。

①对接行为

当模块满足对接条件，即二维码的θ_ori和x_angle在对接阈值内时(式6)，模块执行对接行为。

上式中，θ_ori是二维码平面与摄像机坐标系x轴的夹角，x_angle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角。

对接行为的终止条件：当公头插入母头后，由传感器检测插入动作，判定对接完成。

②平齐行为

当公头模块初始状态与母头模块不平齐时，模块执行平齐行为。通过控制模块线速度对x_angle进行伺服，通过控制模块角速度对θ_ori进行伺服。采用pid控制器完成闭环控制，使公母模块位姿侧向平齐。

平齐行为的终止条件：当公头模块偏转至与母头模块平行时，或二维码偏离摄像机中心的偏角超过二维码可被检测的最大偏角范围时，偏转行为停止，即

上式中，θ_ori是二维码平面与摄像机坐标系x轴的夹角，ω是公头模块旋转角速度，x_angle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角，v是公头模块前进线速度。

③偏转行为

当平齐行为终止，模块开始执行偏转行为，由当前位置原地转向，朝向母头模块转动。当二维码偏离摄像机中心超过角度阈值时，偏转停止。即：

ω＝0，ifx_angle＞15 (8)

上式中，ω是公头模块旋转角速度，x_angle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角。

④前进行为

当偏转行为终止，公头模块开始执行前进行为。公头模块向其头部方向以恒定速度直线运动。当二维码偏离摄像机中心的偏角接近0时，前进行为结束。

v＝0，ifx_angle∈[-0.7,0.7] (9)

上式中，x_angle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角，v是公头模块前进线速度。

⑤退齐行为

当前进行为终止，模块开始执行退齐行为，通过控制模块线速度对x_angle进行伺服，通过控制模块角速度对θ_ori进行伺服，使公头模块后退至与母头模块平齐状态。检测当前二维码与摄像机距离，若距离小于对接阈值，执行对接行为；若否，执行偏转行为。

3.横纵耦合对接

横纵耦合对接过程如图8所示，横纵耦合对接流程如图9所示。

当某模块需要同时进行纵向对接和横向对接时，该对接类型为横纵耦合对接。两个对接母头模块静止不动，对接公头模块运动并实施对接。在母头模块后、侧两个方向各放置一个二维码，在公头模块的前、侧对应面上放置两个摄像头。步进控制采用“平齐-偏转-后退-进齐-修正”行为序列进行横向偏差消除。

单横向对接过程的行为类型描述如下。

①对接行为：当模块满足对接条件，即两个二维码的

在对接阈值内时(式10)，模块执行对接行为。

上式中，

分别是横向、纵向二维码平面与对应侧摄像机的摄像机坐标系x轴的夹角，ω是公头模块旋转角速度，z_d是二维码中心与摄像机坐标系中心的连线长度，

分别是横向、纵向二维码中心与对应侧摄像机的摄像机坐标系原点的连线与z轴之间的夹角，v是公头模块前进线速度。

对接行为的终止条件：当横向和纵向公头均插入母头后，由传感器检测插入动作，判定对接完成。

②平齐行为

当公头模块初始状态与横向母头模块不平齐时，模块执行平齐行为。通过控制模块线速度对x_angle进行伺服，通过控制模块角速度对θ_ori进行伺服。采用pid控制器完成闭环控制，使公母模块位姿侧向偏平齐。

平齐行为的终止条件：当公头模块运动至与母头模块纵向平行、横向偏差15度时，平齐行为停止，即：

上式中，

是横向二维码平面与对应侧摄像机的摄像机坐标系x轴的夹角，ω是公头模块旋转角速度，z_d是二维码中心与摄像机坐标系中心的连线长度，

是横向二维码中心与对应侧摄像机的摄像机坐标系原点的连线与z轴之间的夹角，v是公头模块前进线速度。

③偏转行为

当平齐行为终止，模块开始执行偏转行为，由当前位置原地转向，向远离母头模块方向转动。当二维码偏离摄像机中心超过角度阈值时，偏转停止。即

上式中，ω是公头模块旋转角速度，

是横向二维码中心与对应侧摄像机的摄像机坐标系原点的连线与z轴之间的夹角。

④后退行为

当偏转行为终止，公头模块开始执行后退行为。公头模块向其尾部方向以恒定速度直线运动。当二维码偏离摄像机中心超过角度阈值时，后退行为结束。

上式中，

是横向二维码中心与对应侧摄像机的摄像机坐标系原点的连线与z轴之间的夹角，v是公头模块前进线速度。

⑤进齐行为

当前进行为终止，模块开始执行进齐行为，通过控制模块线速度对

进行伺服，通过控制模块角速度对

进行伺服，使公头模块前进至与母头模块纵向平行、横向偏差15度的偏平齐状态。检测当前二维码与摄像机距离，若距离小于对接阈值，执行修正行为；若否，执行偏转行为。

⑥修正行为

当横向对接距离满足对接条件，而纵向对接距离大于对接阈值时，模块开始执行修正行为，通过控制模块线速度对

进行伺服，通过控制模块角速度对

进行伺服，使公头模块前进并插入纵向对接母头中。前进过程中检测

值，若

满足对接条件，执行对接行为；若否，继续前进。

Claims (6)

1.一种滑移转向模块化机器人的自主对接控制方法，其特征是：

将被对接机器人称为母头模块，将主动实施对接动作机器人称为公头模块，母头模块处于静止状态，公头模块处于运动状态；在母头模块上粘贴二维码作为靶标，在公头模块上加装摄像机，通过摄像机拍摄二维码图像，并解算二维码相对于摄像机坐标系的姿态四元数，获取模块间相对姿态；基于行为的步进控制，实现公头模块与母头模块的对接。

2.如权利要求1所述的滑移转向模块化机器人的自主对接控制方法，其特征是：

所述基于行为的步进控制，是在视觉引导下，反复进行偏离和对准运动，步进地逐步消除对接模块之间的偏差，实现精确对接。

3.如权利要求1所述的滑移转向模块化机器人的自主对接控制方法，其特征是：

所述基于行为的步进控制，包括单纵向对接、单横向对接和横纵耦合对接的步进控制；单纵向对接是指一个公头模块与一个母头模块首尾相接而横向没有其它模块，单纵向对接的步进控制采用偏转、后退、归准和前进的行为序列进行横向偏差消除；单横向对接是指当一个公头模块与一个母头模块横向相连而纵向没有其它模块，单横向对接的步进控制采用对接、平齐、偏转、前进和退齐的行为序列进行横向偏差消除；横纵耦合对接是指一个公头模块与两个母头模块需要同时进行纵向对接和横向对接，在母头模块后面和侧面两个方向各放置一个二维码，在公头模块的前面和侧面对应放置两个摄像头，横纵耦合对接的步进控制采用对接、平齐、偏转、后退、进齐和修正的行为序列进行横向偏差消除；

构建以母头模块为中心，以公头模块相对于母头模块的距离为半径的三环行为区划分，由内至外为对接区、调整区和接近区；

在对接区内实施对接判断，该区范围为二维码距离[0,0.25]米范围的空间；调整区用于步进调整公头模块横向偏差，实现精确对准，范围为二维码距离[0.25,0.7]米范围空间；接近区用于公头模块靠近母头模块。

4.如权利要求3所述的滑移转向模块化机器人的自主对接控制方法，其特征是：所述单纵向对接的步进控制各行为描述如下：

①对接行为：

当模块满足对接条件，即二维码的θ_ori和x_angle在以下对接阈值内时，模块执行对接行为；

上式中，θ_ori是二维码平面与摄像机坐标系x轴的夹角，x_angle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角；

对接行为的终止条件：当公头模块插入母头模块后，判定对接完成；

②偏转行为：

当公头模块位于对接区，且不满足对接条件时，公头模块执行偏转行为；该行为的条件由下式描述，偏转方向朝向母头模块轴线外侧；

上式中，θ_ori是二维码平面与摄像机坐标系x轴的夹角，ω是公头模块旋转角速度，z_d是二维码中心与摄像机坐标系中心的连线长度；

偏转行为的终止条件：当公头模块偏转至与母头模块平行时，或二维码偏离摄像机中心的偏角超过二维码可被检测的最大偏角范围时，偏转行为停止，即：

ω＝0，ifθ_ori∈[-0.1,0.1]∨x_angle∈[15,+∞)∨x_angle∈(-∞,-15)，

上式中，θ_ori是二维码平面与摄像机坐标系x轴的夹角，ω是公头模块旋转角速度，x_angle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角；

③后退行为：

当偏转行为终止，公头模块开始执行后退行为，由当前位置直线后退；当公头模块后退至接近环区内，后退行为停止，即：

上式中，v是公头模块前进线速度，z_d是二维码中心与摄像机坐标系中心的连线长度；

④归准行为：

当后退行为终止，公头模块开始执行归准行为，通过原地转向缩小二维码在摄像机画面中的偏角，当公头模块正对二维码时，归准行为停止，即：

ω＝0,ifx_angle∈[-0.7,0.7]，

上式中，ω是公头模块旋转角速度，x_angle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角；

⑤前进行为：

当归准行为终止，公头模块开始执行前进行为；公头模块控制左右轮转速向前运动，并对二维码在摄像机画面中的偏角进行伺服控制，确保模块正朝二维码运动；当模块移动至对接区边缘，前进行为结束，判断当前状态是否满足对接条件；若满足，执行对接行为；若否，执行偏转行为。

5.如权利要求3所述的滑移转向模块化机器人的自主对接控制方法，其特征是：所述单横向对接的步进控制各行为的描述为：

①对接行为：

当模块满足对接条件，即二维码的θ_ori和x_angle在下式对接阈值内时，模块执行对接行为；

上式中，θ_ori是二维码平面与摄像机坐标系x轴的夹角，x_angle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角；

对接行为的终止条件：当公头插入母头后，判定对接完成；

②平齐行为：

当公头模块初始状态与母头模块不平齐时，公头模块执行平齐行为；通过控制公头模块线速度对x_angle进行伺服，通过控制公头模块角速度对θ_ori进行伺服，使公母模块位姿侧向平齐；

平齐行为的终止条件：当公头模块偏转至与母头模块平行时，或二维码偏离摄像机中心的偏角超过二维码可被检测的最大偏角范围时，偏转行为停止，即：

上式中，θ_ori是二维码平面与摄像机坐标系x轴的夹角，ω是公头模块旋转角速度，x_angle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角，v是公头模块前进线速度；

③偏转行为：

当平齐行为终止，公头模块开始执行偏转行为，由当前位置原地转向，朝向母头模块转动；当二维码偏离摄像机中心超过角度阈值时，偏转停止，即：

ω＝0，ifx_angle＞15，

上式中，ω是公头模块旋转角速度，x_angle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角；

④前进行为：

当偏转行为终止，公头模块开始执行前进行为；公头模块向其头部方向以恒定速度直线运动，当二维码偏离摄像机中心的偏角接近0时，前进行为结束；

v＝0，ifx_angle∈[-0.7,0.7]，

上式中，x_angle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角，v是公头模块前进线速度；

⑤退齐行为：

当前进行为终止，公头模块开始执行退齐行为，通过控制模块线速度对x_angle进行伺服，通过控制公头模块角速度对θ_ori进行伺服，使公头模块后退至与母头模块平齐状态；检测当前二维码与摄像机距离，若距离小于对接阈值，执行对接行为；若否，执行偏转行为。

6.如权利要求3所述的滑移转向模块化机器人的自主对接控制方法，其特征是：所述横纵耦合对接的步进控制各行为描述为：

①对接行为：

当模块满足对接条件，即两个二维码的

和

在下式对接阈值内时，模块执行对接行为；

上式中，

分别是横向和纵向二维码平面与对应侧摄像机的摄像机坐标系x轴的夹角，ω是公头模块旋转角速度，z_d是二维码中心与摄像机坐标系中心的连线长度，

和

分别是横向和纵向二维码中心与对应侧摄像机的摄像机坐标系原点的连线与z轴之间的夹角，v是公头模块前进线速度；

对接行为的终止条件：当横向和纵向公头均插入母头后，判定对接完成；

②平齐行为：

当公头模块初始状态与横向母头模块不平齐时，模块执行平齐行为；通过控制公头模块线速度对x_angle进行伺服，通过控制公头模块角速度对θ_ori进行伺服，使公母模块位姿侧向偏平齐；

平齐行为的终止条件：当公头模块运动至与母头模块纵向平行和横向偏差15度时，平齐行为停止，即：

上式中，

是横向二维码平面与对应侧摄像机的摄像机坐标系x轴的夹角，ω是公头模块旋转角速度，z_d是二维码中心与摄像机坐标系中心的连线长度，

是横向二维码中心与对应侧摄像机的摄像机坐标系原点的连线与z轴之间的夹角，v是公头模块前进线速度；

③偏转行为：

当平齐行为终止，模块开始执行偏转行为，由当前位置原地转向，向远离母头模块方向转动；当二维码偏离摄像机中心超过角度阈值时，偏转停止，即：

上式中，ω是公头模块旋转角速度，

是横向二维码中心与对应侧摄像机的摄像机坐标系原点的连线与z轴之间的夹角；

④后退行为：

当偏转行为终止，公头模块开始执行后退行为；公头模块向其尾部方向以恒定速度直线运动，当二维码偏离摄像机中心超过角度阈值时，后退行为结束，即；

上式中，

是横向二维码中心与对应侧摄像机的摄像机坐标系原点的连线与z轴之间的夹角，v是公头模块前进线速度；

⑤进齐行为：

当前进行为终止，模块开始执行进齐行为，通过控制公头模块线速度对

进行伺服，通过控制公头模块角速度对

进行伺服，使公头模块前进至与母头模块纵向平行和横向偏差15度的偏平齐状态；检测当前二维码与摄像机距离，若距离小于对接阈值，执行修正行为；若否，执行偏转行为；

⑥修正行为

当横向对接距离满足对接条件，而纵向对接距离大于对接阈值时，公头模块开始执行修正行为，通过控制公头模块线速度对

进行伺服，通过控制公头模块角速度对

进行伺服，使公头模块前进并插入纵向对接母头中；前进过程中检测

值，若

满足对接条件，执行对接行为；若否，继续前进。

Images