CN113814967B - 基于视觉引导的全向移动机器人对接机构控制系统及方法 - Google Patents

Abstract

一种基于视觉引导的全向移动机器人对接机构控制系统及方法，所述控制系统，包括上位机、控制器、驱动器、光电检测装置、编码器以及相机，控制器与上位机连接，编码器和驱动器连接后与控制器连接，光电检测装置、工业相机与控制器连接，编码器固定在机械臂的关节处；该方法是，主动移动模块上的工业相机打开，搜索被动移动模块上的标码信息，然后解算信息得到主动移动模块和被动移动模块的相对位姿信息，主动移动模块根据设定的移动顺序逐渐靠近被动移动模块，当两个模块靠近至对接状态时停止移动并进入对接过程，机械臂末端移动至主动连接器上的对接位置，锁紧机构锁紧被动连接器。本发明对接准确可靠，降低了对接机构的设计难度。

Description

基于视觉引导的全向移动机器人对接机构控制系统及方法

技术领域

本发明涉及一种视觉引导下的用于全向移动机器人自动对接控制系统及其控制方法，属于可重构机器人对接领域。

背景技术

自重构模块化机器人是指在无人参与的情况下，一群结构相同的智能体单元能够根据变化的工作环境自主改变自身的构型以适应新的工作环境。在模块化多机器人研究领域，机器人之间分工协作及对接组合的实现方式，与模块化机器人作业范围及构形能力直接相关。其中，机器人相对位置的确定又是该问题的关键所在，只有在相对准确的相对位置信息的引导下，控制系统才能使得各机器人顺利进入对接区域，在自重构的过程中需要实现自重构模块之间的自主运动，位姿调整，对接机构对接、锁紧、松开、分离等一系列动作。

传统机器人由于结构功能单一，很难满足这些要求，因此需要设计模块机器人的概念。机器人模块间的不同连接能构成不同结构的机器人系统，可以适应不同的工作环境，增加了系统的适应性，同时模块化的思想使机器人的机械结构更加简单，制造成本更加低廉，增强了机器人系统的功能多样性和鲁棒性。模块机器人根据环境要求自主地变换构型，这就产生了自重构模块机器人。在自重构模块化机器人研究领域，自动对接技术是一个最具挑战性的研究方向。

针对多机器人户外对接的要求，需要机器人具有很强的运动以及位姿调整能力，因此设计四轮独立驱动独立控制的全向移动机器人。四轮独立驱动转向的根本思想就是根据转向稳定性、灵活性、机器人的运动状态、路面的情况等通过分别独立的控制机器人四个轮子的转向角达到稳定、可靠、安全转向的目的。同时做到在小范围内任意调整机器人位姿，在对接过程中全向移动机器人有更多的路径选择，方便系统做出更好的判断与控制。

机器人自重构的研究主要集中于模块化机器人与多移动机器人自主对接领域。自重构模块机器人的重构能力主要体现在自主重构与自主对接上，其中自重构机器人的自主对接主要依赖模块之间的协作，将需要重构的模块运动到指定位置然后执行对接操作实现对接，这种自主对接方式要求各机器人模块能准确的运动到对应的对接位置，同时要保证位姿偏差在可允许的范围内，对接机构设计的对接冗余量越小时，机器人相对位姿的偏差允许范围就越小，因此如何消除机器人的位姿偏差是一个难点。

而模块间的自主对接又涉及到可靠的位姿信息的获取，自重构模块机器人系统能够根据传感器获取的信息进行位置调整，在此过程中，传感器获取信息的准确度对实现可靠连接是至关重要的，在机器人自重构领域，信息的准确获取是极具挑战性的研究方向。

对于目前的对接系统研究，还面临着以下问题：

1)连接机构设计的对接冗余度较小，对接范围有局限性；

2)对接过程受地形影响较大，在复杂地形下，控制系统常常难以完成任务；

3)对位姿偏差信息获取能力尚有不足，尤其在多种传感器信息融合解算过程中，如何减小位姿信息误差是难点问题；

4)实时性问题，控制系统根据系统解算的位姿信息对机器人对接路径进行实时控制的准确性还有待提升。

发明内容

本发明针对现有全向移动机器人对接技术存在的不足，提供一种准确可靠、对位姿偏差适应能力强的基于视觉引导的全向移动机器人对接机构控制系统；同时提供一种该控制系统的控制方法，针对对接时相对位姿偏差的获取问题，提供一种基于视觉引导的空间线性回归解算方法。

本发明的基于视觉引导的全向移动机器人对接机构控制系统，采用以下技术方案：

其中所述的对接机构包括主动移动模块和被动移动模块，主动移动模块上连接机械臂，机械臂的末端连接有被动连接器，被动移动模块上设置有主动连接器，主动连接器上设有锁紧机构；

所述控制系统，包括上位机、控制器、驱动器、光电检测装置、编码器以及工业相机，控制器与上位机连接，编码器和驱动器连接后与控制器连接，光电检测装置、工业相机与控制器连接，编码器固定在机械臂的关节处；工业相机固定在主动移动模块上，用于获取目标信息并解算作为控制对接的位姿信息；光电检测装置固定在主动连接器上。

所述控制器采用工控机。

所述光电检测装置采用光电开关，是传感器的一种，将发射端和接收端之间光的强弱变化转化为电流的变化以达到探测的目的。由于光电开关输出回路和输入回路是电隔离的(即电绝缘)，所以它可以在许多场合得到应用。

所述机械臂由连杆串联驱动，机械臂采用翻转式。

所述被动连接器是浮动接头。

所述主动连接器上带有U型对接沟槽。

所述锁紧机构设置在主动连接器上，包括锁紧电机、丝杠和夹头，丝杠安装在主动连接器上并与锁紧电机连接，夹头连接在丝杠上。锁紧电机驱动丝杠(梯形丝杠)带动夹头锁紧。

上述控制系统的控制方法，包括定位检测、接近运动、位姿调整、对接锁紧和共同运动；具体是：

当需要对接时，根据上位机的指令，主动移动模块上的工业相机打开，搜索视野中的被动移动模块上的标码信息，然后解算信息得到主动移动模块和被动移动模块的相对位姿信息；控制器(工控机，决策规划控制器)根据位姿信息做出判断，将运动指令发送给主动移动模块(具体是将运动指令发送给主动移动模块的运动控制器(底层STM32))，主动移动模块根据设定的移动顺序逐渐靠近被动移动模块，当两个模块靠近至对接状态时，控制器根据位姿信息做出判断，停止移动并进入对接过程，控制器控制驱动器向机械臂(具体是机械臂电机)传输电信号，驱动机械臂根据设置在机械臂关节处的编码器的伺服控制信号转动，当机械臂末端移动至主动连接器上的对接位置(U型对接沟槽)之后，主动连接器上的光电检测装置被触发，主动连接器上的锁紧机构运动，锁紧被动连接器，实现主动移动模块和被动移动模块共同运动。

所述解算信息得到两模块的相对位姿信息，是根据相机获取的被动移动模块上的标码信息来解算相对位姿信息，具体是：

相机得到在相机坐标系下各标码信息的坐标值及自身偏转角，即(X，Y，Z，θ)；由于各标码中心点在一条直线上，采用空间线性回归的方式解算直线，解算过程如下：

在一组数据中解算出下述数学表达式：z＝ax+by+c，

a、b、c都是未知参数，需要通过线性回归实现，将所有得到的有效数据带入直线方程后得到矩阵：

，

等号(＝)左边是原始数据(随机散列的点)，右边是确定的回归直线，将矩阵理解为一个子空间W，该子空间有三个基：

，

这样回归直线就变成了子空间W里的一个向量：

v＝aα₁+bα₂+cα₃(a，b，c∈R)，

这个向量v在W的超平面里，超平面W与超平面外一个向量z＝(z₁ z₂ z₃ z_n)^T已经确定，线性回归问题就转变为求v，使得v与z向量误差最小；

这里使用到向量投影的概念：有两个向量a、b，向量b在a上的投影为p＝ax，e＝b-p，e称为误差与a正交，因此有a^Te＝0＝a^T(b-p)＝a^T(b-ax)，得到：

这里称为向量投影矩阵；

利用向量投影矩阵，设有向量b和线性子空间M，线性子空间M用一组基表示为矩阵形式，这里用A表示，向量b在子空间上的投影为p＝Ax，e＝b-p，e正交于子空间上任意一个向量，得到下面等式：

A^T(b-p)＝A^T(b-Ax)＝0，

这里x代表的是一个向量，上式为A^Tb＝A^TAx，A是由线性不相关基组成的矩阵，A^TA必然是对称矩阵，得到：

x＝(A^TA)^-1A^Tb，

得到向量b在A上的投影：

p＝A(A^TA)^-1A^Tb，

其中A(A^TA)^-1A^T称为投影矩阵；利用投影矩阵，求解过程可变为：

，

当z的投影向量p等于v时误差最小，最初的矩阵改写为：

，

这样就求解到参数a、b、c的值：

，

相对位姿关系式为：

θ＝tan^-1(-a/b)

在主动移动模块向被动移动模块靠近的过程中，设计了根据相对距离调整位姿阈值的方案，极大地改善了移动过程中位姿信息获取值跳变导致的全向移动机器人动作抖动的问题。同时工业相机位姿信息解算为机器人移动步进逻辑提供了支持。

本发明利用空间线性回归方案解算标码信息的视觉识别增强了两模块对接时的位姿精度问题，对接准确可靠，降低了对接机构的设计难度，可以实现多个移动模块之间的主动对接，同时根据相对距离调整位姿阈值的方案使得整体对接移动的步进逻辑更加简单。整体对接结构简单有效，避免了复杂机械机构带来的维修困难等问题，有较强的外界环境运动适应能力。

附图说明

图1是本发明中所采用的全向移动机器人对接机构的结构原理示意图。

图2是机械臂的结构示意图，(a)是外部结构示意图，(b)是内部结构示意图。

图3是锁紧机构的结构示意图，(a)是外部结构示意图，(b)是内部结构示意图。

图4是工业相机获取多标码信息示意图。

图5是空间线性回归示意图。

图6是全向移动机器人驱控感框架图。

图7是对接移动过程主动模块步进逻辑示意图。

图8是对接移动过程全向机器人速度调整示意图。

图9是对接流程图。

图中：1.主动移动模块，2.机械臂，3.被动移动模块，4.浮动接头，5.锁紧机构，6.工业相机，7.标码信息，8机械臂电机，9.锁紧电机，10.夹头，11.主动连接器，12.丝杠，13.光电开关

具体实施方式

本发明中所应用的可重构全向移动机器人对接机构如图1所示，包括主动移动模块1和被动移动模块3。主动移动模块1上连接机械臂2，机械臂2由连杆串联驱动。机械臂的末端连接有浮动接头4，如图2所示，机械臂2由机械臂电机8带动翻转，可以完成竖直方向230°的转动角度，在是指机械臂2端部安装有浮动接头4，可以完成10°的被动圆周摆动。被动移动模块3上设置有主动连接器11，被动连接器与主动连接器11之间设有锁紧机构5，主动连接器11上带有U型对接沟槽.锁紧机构5采用移动开放式锁紧机构，其结构如图3所示，丝杠12安装在主动连接器11上，锁紧电机9带动丝杠12转动，通过螺纹传动使连接在丝杠12上的夹头10移动，将浮动接头4压紧在主动连接器11上，达到对接锁紧的目的。该套对接机构的主动和被动自由度均由一个旋转电机提供。

上述控制系统的控制方法，包括定位检测、接近运动、位姿调整、对接锁紧和共同运动；当需要链接时，根据上位机的指令，主动移动模块1上安装的工业相机6打开，搜索视野中的被动移动模块3上的标码信息7，然后解算信息得到两模块的相对位姿信息。根据此信息主动移动模块根据设定的移动顺序逐渐靠近被动移动模块，当两个模块靠近至对接状态时，由决策规划控制器根据位姿信息做出判断，停止移动并进入对接过程，决策规划控制器控制驱动器向机械臂电机8传输电信号，驱动机械臂2运动，当机械臂2末端的浮动接头4移动至主动连接器11上的对接沟槽(U型槽)上之后，主动连接器11上的光电开关13被触发，锁紧电机9驱动的锁紧机构5开始旋转运动，完成对主动连接器11的锁紧，实现主动移动模块1和被动移动模块3共同运动。

本发明采用主动移动模块1上安装工业相机6来获取被动移动模块3上的标码信息7来解算相对位姿信息，为了减少误差使得信息更准确，采用多标码信息解算的方式，工业相机6获取多标码信息示意图如图4所示。本发明将得到各标码在相机坐标系下的坐标值及自身偏转角即(X，Y，Z，θ)。由于各标码中心点应该在一条直线上，本发明采用空间线性回归的方式解算直线，如图5，解算过程如下：

在一组数据中解算出下述数学表达式：z＝ax+by+c，

a、b、c都是未知参数，需要通过线性回归实现，将所有得到的有效数据带入直线方程后得到矩阵：

，

等号(＝)左边是原始数据(随机散列的点)，右边是确定的回归直线，将矩阵理解为一个子空间W，该子空间有三个基：

，

这样回归直线就变成了子空间W里的一个向量：

v＝aα₁+bα₂+cα₃(a，b，c∈R)，

这个向量v在W的超平面里，超平面W与超平面外一个向量z＝(z₁ z₂ z₃ z_n)^T已经确定，线性回归问题就转变为求v，使得v与z向量误差最小。

这里使用到向量投影的概念：有两个向量a、b，向量b在a上的投影为p＝ax，e＝b-p，e称为误差与a正交，因此有a^Te＝0＝a^T(b-p)＝a^T(b-ax)

可以得到：

这里称为向量投影矩阵。

利用向量投影矩阵，设有向量b和线性子空间M，线性子空间M可以用一组基表示为矩阵形式，这里用A表示，向量b在子空间上的投影为p＝Ax，e＝b-p，e正交于子空间上任意一个向量，得到下面等式：

A^T(b-p)＝A^T(b-Ax)＝0，

这里x代表的是一个向量，上式为A^Tb＝A^TAx，A是由线性不相关基组成的矩阵，A^TA必然是对称矩阵，可得：

x＝(A^TA)^-1A^Tb，

得到向量b在A上的投影：

p＝A(A^TA)^-1A^Tb，

其中A(A^TA)^-1A^T称为投影矩阵。利用投影矩阵，求解过程可变为：

，

当z的投影向量p等于v时误差最小，最初的矩阵可以改写为：

，

这样就可以求解到参数a、b、c的值：

，

相对位姿关系式为：

θ＝tan^-1(-a/b)

本发明中全向移动机器人实现驱动，控制与感知融为一体，具有行为、决策和控制复合一体化能力，整个模块的驱控感框架如图6所示。工控机作为决策规划层中枢，搭载Ubuntu和ROS机器人操作系统，配备环境感知模块实现定位及导航。底层传感模块包括IMU、超声波传感器等，用于姿态检测和碰撞急停。STM32芯片作为运动控制层中枢，采用CAN总线完成伺服控制器与驱动器之间的连接。用户层由运行于PC机上的操控平台构成。选用凌华科技生产的IB915AF-6600型嵌入式工控机主板，该工控机搭配容量为8G的金士顿低电压专用内存条，128G SSD MLC固态硬盘，相机采用工业单目相机，简单有效，采用24v锂电池进行供电。全向移动机器人电机驱动器分为行走驱动器与转向驱动器，行走驱动器选用KYDBL4875-2E驱动器，而转向驱动器选择Kinco FD133型号。KYDBL4875-2E驱动器为专业移动平台低速负载电机，配合相同厂家的驱动电机，可最大性能发挥驱动电机的能力。KincoFD133为较为成熟的电机驱动器，支持RS232、RS485以及CAN Open通信方式。

本发明设计了根据相对距离调整位姿阈值的对接移动的步进逻辑方案，可以表达为：

当相对位姿在阈值范围内时，不进行调整，当相对位姿在阈值范围外时，进行运动调整，可见随着距离的接近阈值范围在减小，两机器人的位姿误差也在减小，精度在逐渐提高。

整个对接接近过程分为自转行为、前进行为、后退行为、横移行为，由这些逻辑行为构成整个对接逻辑，如图7。为了保证对接速度与精度，对全向移动机器人的轮速进行了PID控制，如图8，做到误差大时调整速度快、误差小时调整速度慢。同时随着两对接模块的靠近(x、y值的减小)，对接的位姿阈值也在减小，对接的精度逐步提高，这样在距离较远时(位姿数据不精确)，阈值大移动速度快，距离较进时(位姿数据较为精确)，阈值小移动速度慢，即提高了对接速度也提高了对接精度。

如图9所示，上述控制系统对对接的控制过程可分为定位检测、靠近、对接锁紧、共同运动等环节。

当需要链接时，上位机给各单元模块发送指令，主动移动模块接收指令后开启工业相机搜索标码信息，解算信息确定被动移动模块的相对位姿信息，然后进行靠近，靠近过程中不断进行位姿调整，到达对接位置后，决策规划控制器根据信息判断是否满足对接条件，满足后对接机构进行运动，首先机械臂电机8根据设置在机械臂2关节处的编码器的伺服控制信号旋转，从而带动对接的浮动接头4旋转。此时由于前后机器人位姿通过本身的控制已经达到对接所需位置，转动的浮动接头4搭在主动对接器11的U型槽内部，主动对接器11上的光电开关13触发，锁紧电机9收到伺服信号旋转，通过丝杠12带动夹头10移动锁紧，到达指定位置后，锁紧电机9停止旋转，利用梯形丝杠的自锁能力将机械臂2(浮动接头4)锁紧，完成整个正向对接过程。

为了保证对接时的允许误差以及增强对两模块共同运动时外部冲击的适应能力，机械臂2末端加装了浮动接头4，可以完成10°的被动圆周摆动。在对接时，若机械臂2和主动对接器11之间存在一定对接偏角，则可通过浮动接头4被动的运动确保对接顺利进行。在两模块共同运动时，外界不平整地形等对模块的冲击将会使对接机构中各关节承受较大力，从而对机械结构产生损坏，而浮动接头4的存在可对该冲击进行有效缓冲。

Claims (4)

1.一种基于视觉引导的全向移动机器人对接机构控制系统，所述对接机构包括主动移动模块和被动移动模块，主动移动模块上连接机械臂，机械臂的末端连接有被动连接器，被动移动模块上设置有主动连接器，主动连接器上设有锁紧机构；其特征是：

所述控制系统，包括上位机、控制器、驱动器、光电检测装置、编码器以及工业相机，控制器与上位机连接，编码器和驱动器连接后与控制器连接，光电检测装置、工业相机与控制器连接，编码器固定在机械臂的关节处；工业相机固定在主动移动模块上，用于获取目标信息并解算作为控制对接的位姿信息；光电检测装置固定在主动连接器上；

当需要对接时，根据上位机的指令，主动移动模块上的工业相机打开，搜索视野中的被动移动模块上的标码信息，然后解算信息得到主动移动模块和被动移动模块的相对位姿信息；控制器根据位姿信息做出判断，将运动指令发送给主动移动模块，主动移动模块根据设定的移动顺序逐渐靠近被动移动模块，当两个模块靠近至对接状态时，控制器根据位姿信息做出判断，停止移动并进入对接过程，控制器控制驱动器向机械臂传输电信号，驱动机械臂根据设置在机械臂关节处的编码器的伺服控制信号转动，当机械臂末端移动至主动连接器上的对接位置之后，主动连接器上的光电检测装置被触发，主动连接器上的锁紧机构运动，锁紧被动连接器，实现主动移动模块和被动移动模块共同运动；

所述解算信息得到两模块的相对位姿信息，是根据相机获取的被动移动模块上的标码信息来解算相对位姿信息，具体是：

相机得到在相机坐标系下各标码信息的坐标值及自身偏转角，即(X，Y，Z，θ)；由于各标码中心点在一条直线上，采用空间线性回归的方式解算直线，解算过程如下：

在一组数据中解算出下述数学表达式：z＝ax+by+c，

a、b、c都是未知参数，需要通过线性回归实现，将所有得到的有效数据带入直线方程后得到矩阵：

，

等号“＝”左边是原始数据，所述原始数据为随机散列的点，右边是确定的回归直线，将矩阵理解为一个子空间W，该子空间有三个基：

，

这样回归直线就变成了子空间W里的一个向量：

v＝aα₁+bα₂+cα₃(a，b，c∈R)，

这个向量v在W的超平面里，超平面W与超平面外一个向量z＝(z₁ z₂ z₃ z_n)^T已经确定，线性回归问题就转变为求v，使得v与z向量误差最小；

这里使用到向量投影的概念：有两个向量a、b，向量b在a上的投影为p＝ax，e＝b-p，e称为误差与a正交，因此有a^Te＝0＝a^T(b-p)＝a^T(b-ax)，得到：

这里称为向量投影矩阵；

利用向量投影矩阵，设有向量b和线性子空间M，线性子空间M用一组基表示为矩阵形式，这里用A表示，向量b在子空间上的投影为p＝Ax，e＝b-p，e正交于子空间上任意一个向量，得到下面等式：

A^T(b-p)＝A^T(b-Ax)＝0，

这里x代表的是一个向量，上式为A^Tb＝A^TAx，A是由线性不相关基组成的矩阵，A^TA必然是对称矩阵，得到：

x＝(A^TA)^-1A^Tb，

得到向量b在A上的投影：

p＝A(A^TA)^-1A^Tb，

其中A(A^TA)^-1A^T称为投影矩阵；利用投影矩阵，求解过程可变为：

，

当z的投影向量p等于v时误差最小，最初的矩阵改写为：

，

这样就求解到参数a、b、c的值：

，

相对位姿关系式为：

θ＝tan^-1(-a/b)

2.根据权利要求1所述的基于视觉引导的全向移动机器人对接机构控制系统，其特征是：所述被动连接器是浮动接头。

3.根据权利要求1所述的基于视觉引导的全向移动机器人对接机构控制系统，其特征是：所述主动连接器上带有U型对接沟槽。

4.根据权利要求1所述的基于视觉引导的全向移动机器人对接机构控制系统，其特征是：所述锁紧机构设置在主动连接器上，包括锁紧电机、丝杠和夹头，丝杠安装在主动连接器上并与锁紧电机连接，夹头连接在丝杠上。