CN113641174A - 一种滑移转向模块化机器人的自主对接控制方法 - Google Patents
一种滑移转向模块化机器人的自主对接控制方法 Download PDFInfo
- Publication number
- CN113641174A CN113641174A CN202110906141.XA CN202110906141A CN113641174A CN 113641174 A CN113641174 A CN 113641174A CN 202110906141 A CN202110906141 A CN 202110906141A CN 113641174 A CN113641174 A CN 113641174A
- Authority
- CN
- China
- Prior art keywords
- module
- male
- dimensional code
- head module
- butt joint
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000003032 molecular docking Methods 0.000 title claims abstract description 83
- 238000000034 method Methods 0.000 title claims abstract description 41
- 230000006399 behavior Effects 0.000 claims abstract description 131
- 210000001503 joint Anatomy 0.000 claims abstract description 129
- 230000009471 action Effects 0.000 claims abstract description 76
- 230000008569 process Effects 0.000 claims abstract description 27
- 230000003068 static effect Effects 0.000 claims abstract description 5
- 230000008878 coupling Effects 0.000 claims description 15
- 238000010168 coupling process Methods 0.000 claims description 15
- 238000005859 coupling reaction Methods 0.000 claims description 15
- 125000004432 carbon atom Chemical group C* 0.000 claims description 9
- 238000012937 correction Methods 0.000 claims description 8
- 238000001514 detection method Methods 0.000 claims description 3
- 241000213006 Angelica dahurica Species 0.000 claims description 2
- 230000008030 elimination Effects 0.000 claims description 2
- 238000003379 elimination reaction Methods 0.000 claims description 2
- 238000011065 in-situ storage Methods 0.000 claims description 2
- 230000000007 visual effect Effects 0.000 claims 1
- 238000005516 engineering process Methods 0.000 description 10
- 238000010586 diagram Methods 0.000 description 5
- 238000003780 insertion Methods 0.000 description 3
- 230000037431 insertion Effects 0.000 description 3
- 230000003281 allosteric effect Effects 0.000 description 2
- 238000005096 rolling process Methods 0.000 description 2
- 230000002776 aggregation Effects 0.000 description 1
- 238000004220 aggregation Methods 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0225—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving docking at a fixed facility, e.g. base station or loading bay
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Multimedia (AREA)
- Electromagnetism (AREA)
- Manipulator (AREA)
Abstract
一种滑移转向模块化机器人的自主对接控制方法,将被对接机器人称为母头模块,将主动实施对接动作机器人称为公头模块,母头模块处于静止状态,公头模块处于运动状态;在母头模块上粘贴二维码作为靶标,在公头模块上加装摄像机,通过摄像机拍摄二维码图像,并解算二维码相对于摄像机坐标系的姿态四元数,获取模块间相对姿态;基于行为的步进控制,实现公头模块与母头模块的对接。本发明通过视觉与合作靶标,借助摄像机检测靶标的位姿,以准确估计两个待对接机器人的相对位姿,弥补了定位系统的误差;能够有效地解决滑移转向模块化机器人在自重构的对接过程中存在的问题,实现了单元模块精确对接控制。
Description
技术领域
本发明涉及一种用于对滑移转向模块化机器人进行自主对接控制的方法,属于移动机器人对接技术领域。
背景技术
对于以四轮差速滑移转向轮式平台为单元模块的模块化自重构机器人,其变构过程的关键技术之一的自主对接技术,存在难度。该类型机器人由多个单元模块铰接构成,每个单元模块是具备独立完整的运动、感知和控制能力的四轮滑转轮式机器人,即多个机器人通过铰接连结为一个大型的机器人。四轮滑动转向机器人通过对两侧轮子的转速调节,驱动机器人移动。由于轮子本身无法改变滚动方向(阿克曼转向角),滑动转向机器人通过轮子在地面的侧滑实现转向。因此,其运动中存在滑动误差,导致机器人运动轨迹控制困难。此外,在不同的地面条件下,滑动转向的动力学参数难以估计,导致机器人在不同地面上的滑动误差不一致,难以建立统一模型进行补偿。
在精确对接过程中滑移转向机器人存在以下问题:
1、精确控制难度大:滑动转向机器人的地面接触动力学参数难以建模估计,地面摩擦系数、轮胎磨损、轮胎气压、温度湿度等因素都会影响滑移量的估算。
2、定位系统存在偏差:在自重构过程中,多个移动机器人借助定位系统、轨迹规划技术和轨迹跟踪控制技术,由分散状态聚集到“准对接”状态。在汇聚过程中,定位系统存在一定误差,当两个待对接机器人的相对位姿偏差在定位系统误差范围内时,定位结果已经无法反应两机器人的准确的相对位姿,仅靠定位系统难以实施精确对接。
3、四轮滑动转向机器人无法横向移动,即速度方向不能垂直于轮子滚动前进方向。对于两个待对接机器人之间的横向偏差,机器人难以消除。
4、机器人具备矩形外型,在多机器人密集聚集状态下,大幅度原地转向将会导致机器人相撞,因此无法通过大幅度(90度)原地转向弥补横向误差。
发明内容
本发明针对现有滑移转向模块化机器人的对接技术存在的不足,提出一种滑移转向模块化机器人的自主对接控制方法,该方法是在视觉和合作靶标引导下的基于行为的步进控制,实现了单元模块精确对接控制。
本发明的滑移转向模块化机器人的自主对接控制方法,是:
将被对接机器人(滑移转向模块化机器人)称为母头模块,将主动实施对接动作的机器人称为公头模块,母头模块处于静止状态,公头模块处于运动状态;在母头模块上粘贴二维码作为靶标,在公头模块上加装摄像机(单目摄像机),通过摄像机拍摄二维码图像,并解算二维码相对于摄像机坐标系的姿态四元数(公知属于,学术上有明确定义),获取模块(母头模块和公头模块)相对姿态;基于行为的步进控制,实现公头模块与母头模块的对接;
使用θori和xangle描述二维码姿态,使用zd描述二维码位置,θori是二维码平面与摄像机坐标系x轴的夹角,用于描述二维码自身相对于摄像机坐标系的旋转;xangle是二维码中心与摄像机坐标系原点的连线与摄像机坐标系z轴之间的夹角,用于描述二维码偏离摄像机中心的偏角;zd是二维码中心与摄像机坐标系中心的连线长度,用于描述二维码距离摄像机的距离。
所述基于行为的步进控制,是在视觉(摄像机拍摄的二维码图像)引导下,反复进行偏离和对准运动,步进地逐步消除对接模块之间的偏差,实现精确对接。
所述基于行为的步进控制,包括单纵向对接、单横向对接和横纵耦合对接的步进控制;单纵向对接是指一个公头模块与一个母头模块首尾相接而横向没有其它模块,单纵向对接的步进控制采用偏转、后退、归准和前进(偏转-后退-归准-前进)的行为序列进行横向偏差消除;单横向对接是指当一个公头模块与一个母头模块横向相连而纵向没有其它模块,单横向对接的步进控制采用对接、平齐、偏转、前进和退齐(“对接-平齐-偏转-前进-退齐”)的行为序列进行横向偏差消除;横纵耦合对接是指一个公头模块与两个母头模块需要同时进行纵向对接和横向对接,在母头模块后面和侧面两个方向各放置一个二维码,在公头模块的前面和侧面对应放置两个摄像头,横纵耦合对接的步进控制采用对接、平齐、偏转、后退、进齐和修正(“对接-平齐-偏转-后退-进齐-修正”)的行为序列进行横向偏差消除;
构建以母头模块为中心,以公头模块相对于母头模块的距离为半径的三环行为区划分,由内至外为对接区、调整区和接近区;在对接区内实施对接判断,该区范围为二维码距离zd[0,0.25]米范围的空间(zd≤0.25);调整区用于步进调整公头模块横向偏差,实现精确对准,范围为二维码距离[0.25,0.7]米范围空间(zd≥0.25∧zd≤0.7);接近区用于公头模块靠近母头模块(zd>0.7)。
所述单纵向对接的步进控制各行为描述如下:
行为序列描述如下:
①对接行为:
当模块满足对接条件,即二维码的θori和xangle在以下对接阈值内时,模块执行对接行为;
上式中,θori是二维码平面与摄像机坐标系x轴的夹角,xangle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角;
对接行为的终止条件:当公头模块插入母头模块后,判定对接完成;
②偏转行为:
当公头模块位于对接区,且不满足对接条件时,公头模块执行偏转行为;该行为的条件由下式描述,偏转方向朝向母头模块轴线外侧。
上式中,θori是二维码平面与摄像机坐标系x轴的夹角,ω是公头模块旋转角速度,zd是二维码中心与摄像机坐标系中心的连线长度;
偏转行为的终止条件:当公头模块偏转至与母头模块平行时,或二维码偏离摄像机中心的偏角超过二维码可被检测的最大偏角范围时,偏转行为停止,即:
ω=0,ifθori∈[-0.1,0.1]∨xangle∈[15,+∞)∨xangle∈(-∞,-15),
上式中,θori是二维码平面与摄像机坐标系x轴的夹角,ω是公头模块旋转角速度,xangle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角;
③后退行为:
当偏转行为终止,公头模块开始执行后退行为,由当前位置直线后退;当公头模块后退至接近环区内,后退行为停止,即:
上式中,v是公头模块前进线速度,zd是二维码中心与摄像机坐标系中心的连线长度;
④归准行为:
当后退行为终止,公头模块开始执行归准行为,通过原地转向缩小二维码在摄像机画面中的偏角,当公头模块正对二维码时,归准行为停止,即:
ω=0,ifxangle∈[-0.7,0.7],
上式中,ω是公头模块旋转角速度,xangle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角;
⑤前进行为:
当归准行为终止,公头模块开始执行前进行为;公头模块控制左右轮转速向前运动,并对二维码在摄像机画面中的偏角进行伺服控制,确保模块正朝二维码运动;当模块移动至对接区边缘,前进行为结束,判断当前状态是否满足对接条件;若满足,执行对接行为;若否,执行偏转行为。
所述单横向对接的步进控制各行为的描述为:
①对接行为:
当模块满足对接条件,即二维码的θori和xangle在下式对接阈值内时,模块执行对接行为;
上式中,θori是二维码平面与摄像机坐标系x轴的夹角,xangle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角;
对接行为的终止条件:当公头插入母头后,判定对接完成;
②平齐行为:
当公头模块初始状态与母头模块不平齐时,公头模块执行平齐行为;通过控制公头模块线速度对xangle进行伺服,通过控制公头模块角速度对θori进行伺服,使公母模块位姿侧向平齐;
平齐行为的终止条件:当公头模块偏转至与母头模块平行时,或二维码偏离摄像机中心的偏角超过二维码可被检测的最大偏角范围时,偏转行为停止,即:
上式中,θori是二维码平面与摄像机坐标系x轴的夹角,ω是公头模块旋转角速度,xangle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角,v是公头模块前进线速度;
③偏转行为:
当平齐行为终止,公头模块开始执行偏转行为,由当前位置原地转向,朝向母头模块转动;当二维码偏离摄像机中心超过角度阈值时,偏转停止,即:
ω=0,ifxangle>15,
上式中,ω是公头模块旋转角速度,xangle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角;
④前进行为:
当偏转行为终止,公头模块开始执行前进行为;公头模块向其头部方向以恒定速度直线运动,当二维码偏离摄像机中心的偏角接近0时,前进行为结束;
v=0,ifxangle∈[-0.7,0.7],
上式中,xangle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角,v是公头模块前进线速度;
⑤退齐行为:
当前进行为终止,公头模块开始执行退齐行为,通过控制模块线速度对xangle进行伺服,通过控制公头模块角速度对θori进行伺服,使公头模块后退至与母头模块平齐状态;检测当前二维码与摄像机距离,若距离小于对接阈值,执行对接行为;若否,执行偏转行为。
所述横纵耦合对接的步进控制各行为描述为:
①对接行为:
上式中,分别是横向和纵向二维码平面与对应侧摄像机的摄像机坐标系x轴的夹角,ω是公头模块旋转角速度,zd是二维码中心与摄像机坐标系中心的连线长度,和分别是横向和纵向二维码中心与对应侧摄像机的摄像机坐标系原点的连线与z轴之间的夹角,v是公头模块前进线速度;
对接行为的终止条件:当横向和纵向公头均插入母头后,判定对接完成;
②平齐行为:
当公头模块初始状态与横向母头模块不平齐时,模块执行平齐行为;通过控制公头模块线速度对xangle进行伺服,通过控制公头模块角速度对θori进行伺服,使公母模块位姿侧向偏平齐;
平齐行为的终止条件:当公头模块运动至与母头模块纵向平行和横向偏差15度时,平齐行为停止,即:
上式中,是横向二维码平面与对应侧摄像机的摄像机坐标系x轴的夹角,ω是公头模块旋转角速度,zd是二维码中心与摄像机坐标系中心的连线长度,是横向二维码中心与对应侧摄像机的摄像机坐标系原点的连线与z轴之间的夹角,v是公头模块前进线速度;
③偏转行为:
当平齐行为终止,模块开始执行偏转行为,由当前位置原地转向,向远离母头模块方向转动;当二维码偏离摄像机中心超过角度阈值时,偏转停止,即:
④后退行为
当偏转行为终止,公头模块开始执行后退行为;公头模块向其尾部方向以恒定速度直线运动,当二维码偏离摄像机中心超过角度阈值时,后退行为结束,即;
⑤进齐行为:
当前进行为终止,模块开始执行进齐行为,通过控制公头模块线速度对进行伺服,通过控制公头模块角速度对进行伺服,使公头模块前进至与母头模块纵向平行和横向偏差15度的偏平齐状态;检测当前二维码与摄像机距离,若距离小于对接阈值,执行修正行为;若否,执行偏转行为;
⑥修正行为
当横向对接距离满足对接条件,而纵向对接距离大于对接阈值时,公头模块开始执行修正行为,通过控制公头模块线速度对进行伺服,通过控制公头模块角速度对进行伺服,使公头模块前进并插入纵向对接母头中;前进过程中检测值,若满足对接条件,执行对接行为;若否,继续前进。
本发明能够有效地解决滑移转向模块化机器人在自重构的对接过程中存在的问题,实现了单元模块精确对接控制,具有以下特点:
1.在母头模块上粘贴二维码靶标,在公头模块上加装单目摄像机,通过视觉+合作靶标技术,借助摄像机检测靶标的位姿,以准确估计各个模块的相对位姿,弥补了定位系统的误差;
2.利用视觉+合作靶标作为测量反馈,构建模块的整体位姿控制闭环,将滑移偏差包含进整体控制闭环中,滑移产生的偏差可以转化为靶标偏转,进而通过步进控制逐步消除滑移偏差,有效地消除了环境条件对于滑移的影响;
3.基于行为的步进控制技术通过多次调整,可以逐步地、步进式地消除横向偏差;
4.在基于行为的步进控制技术的调整过程中,模块转向角度小,在密集聚集状态下,不会导致多模块之间发生碰撞。同时可使模块在运动全过程中,摄像机均可拍摄到二维码靶标,以获取实时相对位姿信息。
附图说明
图1是本发明滑移转向模块化机器人的自主对接控制方法的流程图。
图2是本发明中靶标位姿关键变量几何意义示意图。
图3是本发明中三环行为区划分示意图。
图4是本发明中纵向对接过程的示意图。
图5是本发明中单纵向对接流程图。
图6是本发明中纵向对接过程的示意图。
图7是本发明中横向对接流程图。
图8是本发明中横纵耦合对接过程的示意图。
图9是本发明中横纵耦合对接流程图。
具体实施方式
本发明的方法用于滑移转向模块化机器人的自主对接控制,其流程如图1所示。在母头模块上粘贴二维码靶标,在公头模块上加装单目摄像机,通过视觉+合作靶标技术,借助摄像机检测靶标的位姿,以准确估计待对接模块的相对位姿,弥补了定位系统的误差。
采用视觉+合作靶标引导实现自主对接。通过摄像头拍摄二维码图像,并解算二维码相对于摄像机坐标系的姿态四元数,获取模块间相对姿态。
使用θori和xangle描述二维码姿态,使用zd描述二维码位置。三个关键变量几何意义如图2所示。图2中坐标系为摄像机坐标系。θori是二维码平面与摄像机坐标系x轴的夹角,用于描述二维码自身相对于摄像机坐标系的旋转;xangle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角,用于描述二维码偏离摄像机中心的偏角;zd是二维码中心与摄像机坐标系中心的连线长度,用于描述二维码距离摄像机的距离。
基于行为的步进控制是在视觉引导下、反复多次进行“偏离—对准”运动,步进地逐步消除对接模块之间的偏差,实现精确对接。步进式的运动模式不会产生大范围的模块转动,因此可以避免密聚状态下的碰撞,同时可使模块在运动全过程中,摄像机均可拍摄到二维码靶标,以获取实时相对位姿信息。此外,基于行为的步进控制技术构建模块运动整体闭环,滑移产生的偏差可以转化为靶标偏转,进而通过步进控制逐步消除滑移偏差,因此能够解决因地面环境因素导致的滑移不确定问题。
针对模块化自重构机器人变构过程中常见的三类不同的对接方向,采用三种不同的行为序列实现步进控制,即:单纵向对接、单横向对接和横纵耦合对接。下文中,将待对接机器人称为母头模块,处于静止状态;将主动实施对接模块称为公头模块,处于运动状态。
1.单纵向对接
纵向对接过程如图4所示,单纵向对接流程如图5所示。
当两模块首尾相接,横向没有其它模块时,该对接类型为单纵向对接。对接母头模块静止不动,对接公头模块运动并实施对接。步进控制采用“偏转-后退-归准-前进”行为序列进行横向偏差消除,构建以对接母头模块为中心、以公头模块相对于母头模块的距离为半径的三环行为区划分,环区划分如图3所示,由内至外为对接区、调整区和接近区。
在对接区内实施对接判断,该区范围为二维码距离zd[0,0.25]米范围的空间;调整区用于步进调整公头模块横向偏差,实现精确对准,范围为二维码距离[0.25,0.7]米范围空间;接近区用于公头模块靠近母头模块。分别具体表示为:zd≤0.25,zd≥0.25∧zd≤0.7,zd>0.7。
单纵向对接过程的行为类型描述如下。
①对接行为
当模块满足对接条件,即二维码的θori和xangle在对接阈值内时(式1),模块执行对接行为。
上式中,θori是二维码平面与摄像机坐标系x轴的夹角,xangle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角。
对接行为的终止条件:当公头模块插入母头模块后,由传感器检测插入动作,判定对接完成。
②偏转行为
当公头模块位于对接区,且不满足对接条件时,公头模块执行偏转行为。该行为的条件由下式描述。偏转方向朝向母头模块轴线外侧。
上式中,θori是二维码平面与摄像机坐标系x轴的夹角,ω是公头模块旋转角速度,zd是二维码中心与摄像机坐标系中心的连线长度。
偏转行为的终止条件:当公头模块偏转至与母头模块平行时,或二维码偏离摄像机中心的偏角超过二维码可被检测的最大偏角范围时,偏转行为停止,即:
ω=0,ifθori∈[-0.1,0.1]∨xangle∈[15,+∞)∨xangle∈(-∞,-15) (3)
上式中,θori是二维码平面与摄像机坐标系x轴的夹角,ω是公头模块旋转角速度,xangle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角。
③后退行为
当偏转行为终止,公头模块开始执行后退行为,由当前位置直线后退。当公头模块后退至接近环区内,后退行为停止。即:
上式中,v是公头模块前进线速度,zd是二维码中心与摄像机坐标系中心的连线长度。
④归准行为
当后退行为终止,公头模块开始执行归准行为,通过原地转向缩小二维码在摄像机画面中的偏角。当公头模块正对二维码时,归准行为停止。即:
ω=0,ifxangle∈[-0.7,0.7] (5)
上式中,ω是公头模块旋转角速度,xangle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角。
⑤前进行为
当归准行为终止,模块开始执行前进行为。模块控制左右轮转速向前运动,并对二维码在摄像机画面中的偏角进行伺服控制,确保模块正朝二维码运动。当模块移动至对接区边缘,前进行为结束,判断当前状态是否满足对接条件。若满足,执行对接行为;若否,执行偏转行为。
2.单横向对接
纵向对接过程如图6所示,横向对接流程如图7所示。
当两模块左右相连,纵向没有其它模块时,该对接类型为单横向对接。对接母头模块静止不动,对接公头模块运动并实施对接。步进控制采用“平齐-偏转-前进-退齐”行为序列进行横向偏差消除。
单横向对接过程的行为类型描述如下。
①对接行为
当模块满足对接条件,即二维码的θori和xangle在对接阈值内时(式6),模块执行对接行为。
上式中,θori是二维码平面与摄像机坐标系x轴的夹角,xangle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角。
对接行为的终止条件:当公头插入母头后,由传感器检测插入动作,判定对接完成。
②平齐行为
当公头模块初始状态与母头模块不平齐时,模块执行平齐行为。通过控制模块线速度对xangle进行伺服,通过控制模块角速度对θori进行伺服。采用pid控制器完成闭环控制,使公母模块位姿侧向平齐。
平齐行为的终止条件:当公头模块偏转至与母头模块平行时,或二维码偏离摄像机中心的偏角超过二维码可被检测的最大偏角范围时,偏转行为停止,即
上式中,θori是二维码平面与摄像机坐标系x轴的夹角,ω是公头模块旋转角速度,xangle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角,v是公头模块前进线速度。
③偏转行为
当平齐行为终止,模块开始执行偏转行为,由当前位置原地转向,朝向母头模块转动。当二维码偏离摄像机中心超过角度阈值时,偏转停止。即:
ω=0,ifxangle>15 (8)
上式中,ω是公头模块旋转角速度,xangle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角。
④前进行为
当偏转行为终止,公头模块开始执行前进行为。公头模块向其头部方向以恒定速度直线运动。当二维码偏离摄像机中心的偏角接近0时,前进行为结束。
v=0,ifxangle∈[-0.7,0.7] (9)
上式中,xangle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角,v是公头模块前进线速度。
⑤退齐行为
当前进行为终止,模块开始执行退齐行为,通过控制模块线速度对xangle进行伺服,通过控制模块角速度对θori进行伺服,使公头模块后退至与母头模块平齐状态。检测当前二维码与摄像机距离,若距离小于对接阈值,执行对接行为;若否,执行偏转行为。
3.横纵耦合对接
横纵耦合对接过程如图8所示,横纵耦合对接流程如图9所示。
当某模块需要同时进行纵向对接和横向对接时,该对接类型为横纵耦合对接。两个对接母头模块静止不动,对接公头模块运动并实施对接。在母头模块后、侧两个方向各放置一个二维码,在公头模块的前、侧对应面上放置两个摄像头。步进控制采用“平齐-偏转-后退-进齐-修正”行为序列进行横向偏差消除。
单横向对接过程的行为类型描述如下。
上式中,分别是横向、纵向二维码平面与对应侧摄像机的摄像机坐标系x轴的夹角,ω是公头模块旋转角速度,zd是二维码中心与摄像机坐标系中心的连线长度, 分别是横向、纵向二维码中心与对应侧摄像机的摄像机坐标系原点的连线与z轴之间的夹角,v是公头模块前进线速度。
对接行为的终止条件:当横向和纵向公头均插入母头后,由传感器检测插入动作,判定对接完成。
②平齐行为
当公头模块初始状态与横向母头模块不平齐时,模块执行平齐行为。通过控制模块线速度对xangle进行伺服,通过控制模块角速度对θori进行伺服。采用pid控制器完成闭环控制,使公母模块位姿侧向偏平齐。
平齐行为的终止条件:当公头模块运动至与母头模块纵向平行、横向偏差15度时,平齐行为停止,即:
上式中,是横向二维码平面与对应侧摄像机的摄像机坐标系x轴的夹角,ω是公头模块旋转角速度,zd是二维码中心与摄像机坐标系中心的连线长度,是横向二维码中心与对应侧摄像机的摄像机坐标系原点的连线与z轴之间的夹角,v是公头模块前进线速度。
③偏转行为
当平齐行为终止,模块开始执行偏转行为,由当前位置原地转向,向远离母头模块方向转动。当二维码偏离摄像机中心超过角度阈值时,偏转停止。即
④后退行为
当偏转行为终止,公头模块开始执行后退行为。公头模块向其尾部方向以恒定速度直线运动。当二维码偏离摄像机中心超过角度阈值时,后退行为结束。
⑤进齐行为
当前进行为终止,模块开始执行进齐行为,通过控制模块线速度对进行伺服,通过控制模块角速度对进行伺服,使公头模块前进至与母头模块纵向平行、横向偏差15度的偏平齐状态。检测当前二维码与摄像机距离,若距离小于对接阈值,执行修正行为;若否,执行偏转行为。
⑥修正行为
Claims (6)
1.一种滑移转向模块化机器人的自主对接控制方法,其特征是:
将被对接机器人称为母头模块,将主动实施对接动作机器人称为公头模块,母头模块处于静止状态,公头模块处于运动状态;在母头模块上粘贴二维码作为靶标,在公头模块上加装摄像机,通过摄像机拍摄二维码图像,并解算二维码相对于摄像机坐标系的姿态四元数,获取模块间相对姿态;基于行为的步进控制,实现公头模块与母头模块的对接。
2.如权利要求1所述的滑移转向模块化机器人的自主对接控制方法,其特征是:
所述基于行为的步进控制,是在视觉引导下,反复进行偏离和对准运动,步进地逐步消除对接模块之间的偏差,实现精确对接。
3.如权利要求1所述的滑移转向模块化机器人的自主对接控制方法,其特征是:
所述基于行为的步进控制,包括单纵向对接、单横向对接和横纵耦合对接的步进控制;单纵向对接是指一个公头模块与一个母头模块首尾相接而横向没有其它模块,单纵向对接的步进控制采用偏转、后退、归准和前进的行为序列进行横向偏差消除;单横向对接是指当一个公头模块与一个母头模块横向相连而纵向没有其它模块,单横向对接的步进控制采用对接、平齐、偏转、前进和退齐的行为序列进行横向偏差消除;横纵耦合对接是指一个公头模块与两个母头模块需要同时进行纵向对接和横向对接,在母头模块后面和侧面两个方向各放置一个二维码,在公头模块的前面和侧面对应放置两个摄像头,横纵耦合对接的步进控制采用对接、平齐、偏转、后退、进齐和修正的行为序列进行横向偏差消除;
构建以母头模块为中心,以公头模块相对于母头模块的距离为半径的三环行为区划分,由内至外为对接区、调整区和接近区;
在对接区内实施对接判断,该区范围为二维码距离[0,0.25]米范围的空间;调整区用于步进调整公头模块横向偏差,实现精确对准,范围为二维码距离[0.25,0.7]米范围空间;接近区用于公头模块靠近母头模块。
4.如权利要求3所述的滑移转向模块化机器人的自主对接控制方法,其特征是:所述单纵向对接的步进控制各行为描述如下:
①对接行为:
当模块满足对接条件,即二维码的θori和xangle在以下对接阈值内时,模块执行对接行为;
上式中,θori是二维码平面与摄像机坐标系x轴的夹角,xangle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角;
对接行为的终止条件:当公头模块插入母头模块后,判定对接完成;
②偏转行为:
当公头模块位于对接区,且不满足对接条件时,公头模块执行偏转行为;该行为的条件由下式描述,偏转方向朝向母头模块轴线外侧;
上式中,θori是二维码平面与摄像机坐标系x轴的夹角,ω是公头模块旋转角速度,zd是二维码中心与摄像机坐标系中心的连线长度;
偏转行为的终止条件:当公头模块偏转至与母头模块平行时,或二维码偏离摄像机中心的偏角超过二维码可被检测的最大偏角范围时,偏转行为停止,即:
ω=0,ifθori∈[-0.1,0.1]∨xangle∈[15,+∞)∨xangle∈(-∞,-15),
上式中,θori是二维码平面与摄像机坐标系x轴的夹角,ω是公头模块旋转角速度,xangle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角;
③后退行为:
当偏转行为终止,公头模块开始执行后退行为,由当前位置直线后退;当公头模块后退至接近环区内,后退行为停止,即:
上式中,v是公头模块前进线速度,zd是二维码中心与摄像机坐标系中心的连线长度;
④归准行为:
当后退行为终止,公头模块开始执行归准行为,通过原地转向缩小二维码在摄像机画面中的偏角,当公头模块正对二维码时,归准行为停止,即:
ω=0,ifxangle∈[-0.7,0.7],
上式中,ω是公头模块旋转角速度,xangle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角;
⑤前进行为:
当归准行为终止,公头模块开始执行前进行为;公头模块控制左右轮转速向前运动,并对二维码在摄像机画面中的偏角进行伺服控制,确保模块正朝二维码运动;当模块移动至对接区边缘,前进行为结束,判断当前状态是否满足对接条件;若满足,执行对接行为;若否,执行偏转行为。
5.如权利要求3所述的滑移转向模块化机器人的自主对接控制方法,其特征是:所述单横向对接的步进控制各行为的描述为:
①对接行为:
当模块满足对接条件,即二维码的θori和xangle在下式对接阈值内时,模块执行对接行为;
上式中,θori是二维码平面与摄像机坐标系x轴的夹角,xangle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角;
对接行为的终止条件:当公头插入母头后,判定对接完成;
②平齐行为:
当公头模块初始状态与母头模块不平齐时,公头模块执行平齐行为;通过控制公头模块线速度对xangle进行伺服,通过控制公头模块角速度对θori进行伺服,使公母模块位姿侧向平齐;
平齐行为的终止条件:当公头模块偏转至与母头模块平行时,或二维码偏离摄像机中心的偏角超过二维码可被检测的最大偏角范围时,偏转行为停止,即:
上式中,θori是二维码平面与摄像机坐标系x轴的夹角,ω是公头模块旋转角速度,xangle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角,v是公头模块前进线速度;
③偏转行为:
当平齐行为终止,公头模块开始执行偏转行为,由当前位置原地转向,朝向母头模块转动;当二维码偏离摄像机中心超过角度阈值时,偏转停止,即:
ω=0,ifxangle>15,
上式中,ω是公头模块旋转角速度,xangle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角;
④前进行为:
当偏转行为终止,公头模块开始执行前进行为;公头模块向其头部方向以恒定速度直线运动,当二维码偏离摄像机中心的偏角接近0时,前进行为结束;
v=0,ifxangle∈[-0.7,0.7],
上式中,xangle是二维码中心与摄像机坐标系原点的连线与z轴之间的夹角,v是公头模块前进线速度;
⑤退齐行为:
当前进行为终止,公头模块开始执行退齐行为,通过控制模块线速度对xangle进行伺服,通过控制公头模块角速度对θori进行伺服,使公头模块后退至与母头模块平齐状态;检测当前二维码与摄像机距离,若距离小于对接阈值,执行对接行为;若否,执行偏转行为。
6.如权利要求3所述的滑移转向模块化机器人的自主对接控制方法,其特征是:所述横纵耦合对接的步进控制各行为描述为:
①对接行为:
上式中,分别是横向和纵向二维码平面与对应侧摄像机的摄像机坐标系x轴的夹角,ω是公头模块旋转角速度,zd是二维码中心与摄像机坐标系中心的连线长度,和分别是横向和纵向二维码中心与对应侧摄像机的摄像机坐标系原点的连线与z轴之间的夹角,v是公头模块前进线速度;
对接行为的终止条件:当横向和纵向公头均插入母头后,判定对接完成;
②平齐行为:
当公头模块初始状态与横向母头模块不平齐时,模块执行平齐行为;通过控制公头模块线速度对xangle进行伺服,通过控制公头模块角速度对θori进行伺服,使公母模块位姿侧向偏平齐;
平齐行为的终止条件:当公头模块运动至与母头模块纵向平行和横向偏差15度时,平齐行为停止,即:
上式中,是横向二维码平面与对应侧摄像机的摄像机坐标系x轴的夹角,ω是公头模块旋转角速度,zd是二维码中心与摄像机坐标系中心的连线长度,是横向二维码中心与对应侧摄像机的摄像机坐标系原点的连线与z轴之间的夹角,v是公头模块前进线速度;
③偏转行为:
当平齐行为终止,模块开始执行偏转行为,由当前位置原地转向,向远离母头模块方向转动;当二维码偏离摄像机中心超过角度阈值时,偏转停止,即:
④后退行为:
当偏转行为终止,公头模块开始执行后退行为;公头模块向其尾部方向以恒定速度直线运动,当二维码偏离摄像机中心超过角度阈值时,后退行为结束,即;
⑤进齐行为:
当前进行为终止,模块开始执行进齐行为,通过控制公头模块线速度对进行伺服,通过控制公头模块角速度对进行伺服,使公头模块前进至与母头模块纵向平行和横向偏差15度的偏平齐状态;检测当前二维码与摄像机距离,若距离小于对接阈值,执行修正行为;若否,执行偏转行为;
⑥修正行为
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110906141.XA CN113641174A (zh) | 2021-08-09 | 2021-08-09 | 一种滑移转向模块化机器人的自主对接控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110906141.XA CN113641174A (zh) | 2021-08-09 | 2021-08-09 | 一种滑移转向模块化机器人的自主对接控制方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN113641174A true CN113641174A (zh) | 2021-11-12 |
Family
ID=78420060
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110906141.XA Pending CN113641174A (zh) | 2021-08-09 | 2021-08-09 | 一种滑移转向模块化机器人的自主对接控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113641174A (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116203974A (zh) * | 2023-05-06 | 2023-06-02 | 北京理工大学 | 一种控制移动机器人与设备对接的方法和装置 |
CN116979345A (zh) * | 2023-09-22 | 2023-10-31 | 中科开创(广州)智能科技发展有限公司 | 导轨移动式共享充电机器人精准定位方法、装置、设备及介质 |
Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US9479732B1 (en) * | 2015-11-10 | 2016-10-25 | Irobot Corporation | Immersive video teleconferencing robot |
CN109032178A (zh) * | 2018-08-06 | 2018-12-18 | 江苏科技大学 | 全驱动auv回收控制系统及自主回收方法 |
US20200021122A1 (en) * | 2018-07-11 | 2020-01-16 | Irobot Corporation | Docking station for autonomous mobile robots |
CN111625005A (zh) * | 2020-06-10 | 2020-09-04 | 浙江欣奕华智能科技有限公司 | 一种机器人充电方法、机器人充电的控制装置及存储介质 |
CN112180989A (zh) * | 2020-09-30 | 2021-01-05 | 苏州盈科电子有限公司 | 一种机器人充电方法及装置 |
CN112873188A (zh) * | 2021-01-21 | 2021-06-01 | 北京邮电大学 | 自组装模块化机器人单元、机器人、组装及控制方法 |
CN113103225A (zh) * | 2021-03-03 | 2021-07-13 | 重庆大学 | 一种基于图像测量的移动机器人多阶段平稳自主对接方法 |
EP4051526A1 (en) * | 2019-10-29 | 2022-09-07 | Domygo S.r.l. | Robot for the automatic electric charging of an electric vehicle, charging group and automotive apparatus provided with said robot |
-
2021
- 2021-08-09 CN CN202110906141.XA patent/CN113641174A/zh active Pending
Patent Citations (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US9479732B1 (en) * | 2015-11-10 | 2016-10-25 | Irobot Corporation | Immersive video teleconferencing robot |
US20200021122A1 (en) * | 2018-07-11 | 2020-01-16 | Irobot Corporation | Docking station for autonomous mobile robots |
CN109032178A (zh) * | 2018-08-06 | 2018-12-18 | 江苏科技大学 | 全驱动auv回收控制系统及自主回收方法 |
EP4051526A1 (en) * | 2019-10-29 | 2022-09-07 | Domygo S.r.l. | Robot for the automatic electric charging of an electric vehicle, charging group and automotive apparatus provided with said robot |
CN111625005A (zh) * | 2020-06-10 | 2020-09-04 | 浙江欣奕华智能科技有限公司 | 一种机器人充电方法、机器人充电的控制装置及存储介质 |
CN112180989A (zh) * | 2020-09-30 | 2021-01-05 | 苏州盈科电子有限公司 | 一种机器人充电方法及装置 |
CN112873188A (zh) * | 2021-01-21 | 2021-06-01 | 北京邮电大学 | 自组装模块化机器人单元、机器人、组装及控制方法 |
CN113103225A (zh) * | 2021-03-03 | 2021-07-13 | 重庆大学 | 一种基于图像测量的移动机器人多阶段平稳自主对接方法 |
Cited By (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116203974A (zh) * | 2023-05-06 | 2023-06-02 | 北京理工大学 | 一种控制移动机器人与设备对接的方法和装置 |
CN116979345A (zh) * | 2023-09-22 | 2023-10-31 | 中科开创(广州)智能科技发展有限公司 | 导轨移动式共享充电机器人精准定位方法、装置、设备及介质 |
CN116979345B (zh) * | 2023-09-22 | 2024-01-30 | 中科开创(广州)智能科技发展有限公司 | 导轨移动式共享充电机器人精准定位方法、装置、设备及介质 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN113641174A (zh) | 一种滑移转向模块化机器人的自主对接控制方法 | |
CN110347160B (zh) | 一种基于双摄像头扫码的自动引导车及其导航方法 | |
CN107544520B (zh) | 一种用于四轮载具自动驾驶的控制方法 | |
CN107901041A (zh) | 一种基于图像混合矩的机器人视觉伺服控制方法 | |
CN109159151A (zh) | 一种机械臂空间轨迹跟踪动态补偿方法和系统 | |
US20200206898A1 (en) | Biped robot gait control method and biped robot | |
CN109960145B (zh) | 移动机器人混合视觉轨迹跟踪策略 | |
CN112884842B (zh) | 双舵轮零位和采集装置外参同步标定方法及可移动平台 | |
JP2008250516A (ja) | 射影変換収束演算処理方法 | |
CN111476166B (zh) | 一种地面巡检机器人自助充电对准方法 | |
CN113110495A (zh) | 一种考虑外部干扰下移动机器人的编队控制方法 | |
CN108362296B (zh) | Auv与接驳站水下对接的末端视觉导引方法 | |
López-Nicolás et al. | Switching visual control based on epipoles for mobile robots | |
CN110703751A (zh) | 一种多移动机器人系统半自主编队及避障控制方法 | |
CN111487976B (zh) | 一种倒车轨迹跟踪方法 | |
Huang et al. | Realizing peg-and-hole alignment with one eye-in-hand high-speed camera | |
Kazemi et al. | Path planning for image-based control of wheeled mobile manipulators | |
Liu et al. | An improved hybrid error control path tracking intelligent algorithm for omnidirectional AGV on ROS | |
CN113814967B (zh) | 基于视觉引导的全向移动机器人对接机构控制系统及方法 | |
CN114740863A (zh) | 一种基于领航跟随的多机编队控制方法及装置 | |
Singh et al. | Position Based Path Tracking for Wheeled Mobile Robots. | |
CN110883770A (zh) | 一种基于位置和图像的机器人混合视觉伺服控制方法 | |
Rodrigues et al. | A framework for depth estimation and relative localization of ground robots using computer vision | |
CN210025292U (zh) | 一种基于薄膜力传感器的实验用蛇形机器人 | |
CN110455272B (zh) | 轨迹跟踪系统中的传感系统 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination |