CN100478142C - 一种移动机器人的位姿传感系统及其方法 - Google Patents

Abstract

本发明涉及机器人技术领域，特别是一种移动机器人的位姿传感系统及其方法。由光电传感器、传感器弹压机构、控制处理电路三部分组成，光电传感器通过图像处理探测到机器人的位姿变化，然后把这种变化转换成数字信号；传感器弹压机构保证光电传感器与地面保持合适的距离；控制处理电路包括一块单片机和通讯接口芯片，各种处理的程序固化在单片机中，对光电传感器送来的信息滤波，融合以及位姿估计处理以及与上位机的通讯。单片机采用位姿估计算法进行位姿估计；最后，通过通信端口把实时位姿发送给上位机，从而实现实时位姿估计。采用模块化设计，能够独立工作，电路简单可靠，抗干扰能力强，性价比很高。

Description

一种移动机器人的位姿传感系统及其方法

技术领域

本发明涉及机器人技术领域，特别是一种移动机器人的位姿传感系统及其方法。

背景技术

随着计算机技术和自动控制理论的发展，机器人在国防、工业、农业以及服务业都得到了越来越广泛的应用。移动机器人位姿估计问题在机器人学中是一个最基本的问题，具体而言也就是通过机器人本身所装配的内外传感器获取环境信息，从而确定机器人自身位置和姿态的问题。机器人位姿估计是机器人重建环境地图、规划运行路径和航迹跟踪的前提，因此，机器人的位姿估计是一个非常重要的问题。根据公开文献资料，在本发明以前，移动机器人所采用的位姿估计方法大多都是根据里程计信息，利用航位推算法(Dead-Reckoning)来初步推算移动机器人的位姿，再利用超声波传感器、激光测距仪、摄像头等传感器采集路标信息来修正初始估计的位姿，从而得到移动机器人相对精确的位姿，或者直接利用全局视觉进行位姿估计。限于环境的复杂性，以及里程计所用到的光电编码盘的物理特性，机器人在启动、停止时编码盘有较大的误差；另一方面，由于驱动轮和地面有可能出现打滑现象。这些因素使得利用里程计估计位姿不可靠，导致移动机器人移动较长距离之后误差很大，尤其是在角度上；视觉类姿态估计方法由于是基于图像处理的，一般需要配置摄像头、图像采集卡和计算机，而且还需要配置操作系统(Windows、Unix、Linux)，软硬件成本很高。另外，由于图像处理的计算复杂性以及对环境的敏感性，很难保证位姿估计的实时性、简单性和可靠性。

以对现有技术的检索，根据哈尔滨工业大学洪炳镕等人的中国专利“全自主型移动机器人及其智能控制系统”[申请号02132744.0]中的描述，涉及一种移动机器人位姿估计装置，其自述为：设置我方机器人坐标系，启动视觉系统寻找对方球门左右边框，根据球门识别模板算出球门左右边框的底坐标(X₁，Y₁)、(X₂，Y₂)，由曲线拟合公式将左右边框底坐标换算成左右边框相对机器人的距离和角度，以此实现机器人的定位。该专利技术的不足之处在于：①采用通用平台，成本高、可靠性低；②采用视觉类位姿估计方法，计算量大，难以保证实时性；③这种基于特征点识别的位姿估计方法误差比较大，而且对视觉传感器对环境色差、光照强弱等因素很敏感。进一步检索，上海交通大学徐化等人的中国专利“基于数字信号处理器的足球机器人光电定位装置”[申请号：200410018304.7]中涉及到一种光电传感器的应用，其自述为：本发明涉及一种基于数字信号处理器的足球机器人光电定位装置，由光电传感器、传感器桥架机构和控制系统组成，控制系统包括DSP数字信号处理器、缓冲驱动芯片、JTAG测试仿真接口、通信硬件端口和通信驱动芯片，传感器桥架机构安装在机器人底部并与地面压紧，光电传感器安装在传感器桥架机构上并保持与地面贴近，光电传感器的输出信号通过缓冲驱动芯片调理后发送到DSP的相应信号捕获单元实现信号采集，DSP处理器的计算结果经通信驱动芯片送入通信硬件端口，实现与上位机通信及数据上发，使机器人获得当前位置信息。

该专利技术应用于足球机器人领域，但只能确定机器人的位置，不能估计机器人的姿态，而机器人在二维平面上运动时，只要稍微受到干扰，其姿态就有可能发生变化，所以只用这一种定位装置根本就不能确定机器人的位姿，必须和其它测量角度的装置结合起来使用；另外，其电路比较复杂，开发成本较高，虽然在功能上用模块化设计，但是由于电路板和传感器分离，必须通过接线连接，不易安装，容易受到干扰。

发明内容

本发明旨在克服现有技术的缺陷，为当今移动机器人提供一种原理简单、造价低廉、性能优越的位姿传感系统和方法，使机器人能够在实时地、简单地、可靠地进行位姿估计。由于本传感系统在特定环境下无论是测试平移量还是旋转量都精度很高，不仅仅可用在移动机器人领域，还可以用于其它机械制造业。

本发明通过以下技术方案实现：位姿传感系统主要由光电传感器、传感器弹压机构、控制处理电路三部分组成。光电传感器的功能是通过图像处理探测到机器人的位姿变化，然后把这种变化转换成数字信号；传感器弹压机构保证光电传感器与地面保持合适的距离，能够可靠有效地工作，并且当地面上出现障碍物时使得位姿传感系统能够避开障碍物，从而实现提高精度、保护硬件的目的；控制处理电路包括一块单片机(MCU)和通讯接口芯片，各种处理功能的程序即固化在单片机中，主要功能是完成对光电传感器送来的信息滤波，融合以及位姿估计等一系列的处理以及与上位机的通讯功能。另外，本发明的技术方案中涉及的位姿传感系统不仅仅只是被动地给上位机发送位姿估计结果，也可以接收上位机的数据和命令完成初始化以及各种控制功能。

为了更好地说明本发明的工作原理，在此作两部分来说明：首先说明光电传感器的工作原理；然后说明本发明采用什么物理结构和方法来实现位姿估计的。本发明中的光电传感器基于反射式工作原理，通过内部发光二极管发出的光线照射到地面上(要求地面不是镜面发射)，然后通过光学透镜将图像传输到传感器内部，再由其中的图像分析芯片对移动过程中得到的一系列图像进行分析处理，从而得到相对移动的距离和方向，其内部电路再将相对位移转换成数字信号送给控制处理电路。本发明中采用了最少两个光电传感器，为了减少计算量，将其中一个光电传感器安装在机器人的中心点，另一个光电传感器安装在移动机器人自身坐标系X轴的正方向上，两者在一条直线上，当然，也可以任意安装。参照附图4，其中A点表示安装在移动机器人中心的光电传感器，B点表示安装在移动机器人自身坐标系X轴的正方向上的光电传感器，圆表示移动机器人本体，机器人初始位姿为[x_k-1，y_k-1，θ_k-1]^T，其中(x_k-1，y_k-1)表示机器人在平面的位置，θ_k-1表示机器人的方向。其运动了一步之后A到达A′，B到达B′，由于每一步步长很短，可以认为机器人先平移然后旋转，由以下公式1，2，3即可得，其中dx_k，dy_k(表示在光电传感器初始位姿的坐标系中的变化量)，可通过光电传感器直接得到，旋转方向可依据

的正负号确定，正为顺时针方向，负为逆时针方向。

$\left[\begin{array}{c}{x}_k\\ y_k\\ {\mathrm{\θ}}_k\end{array}\right]=\left[\begin{array}{c}{x}_{k-1}\\ y_{k-1}\\ {\mathrm{\θ}}_{k-1}\end{array}\right]+\left[\begin{array}{c}{d}_k\cos ({\mathrm{\θ}}_{k-1}-\frac{\mathrm{\π}}{2}+\mathrm{arctg}\frac{{\mathrm{dy}}_k}{{\mathrm{dx}}_k})\\ d_k\sin ({\mathrm{\θ}}_{k-1}-\frac{\mathrm{\π}}{2}+\mathrm{arctg}\frac{{\mathrm{dy}}_k}{{\mathrm{dx}}_k})\\ {\mathrm{\α}}_k\end{array}\right]$ (式1)

其中： $d_k=\left|\right|\stackrel{\→}{{\mathrm{AA}}^{\′}}\left|\right|=\sqrt{{\left({\mathrm{dx}}_k\right)}^2+{\left({\mathrm{dy}}_k\right)}^2}$ (式2)

${\mathrm{\α}}_k=\arg (\stackrel{\→}{{\mathrm{BB}}^{\′}}-\stackrel{\→}{\mathrm{BC}})\≈\frac{\left|\right|\stackrel{\→}{{\mathrm{BB}}^{\′}}-\stackrel{\→}{\mathrm{BC}}\left|\right|}{l}$ (式3)

由于光电传感器可以测得所需的各个量，这样就实现了一步位姿估计，再由航位推算法逐步积累可以得到移动机器人的位姿。整个工作过程如下：系统上电，上位机也可以根据要求给位姿传感系统中的单片机发送信息，完成位姿传感系统的初始化，当机器人运动，光电传感器则得到相应的运动信息，然后以数字信号的形式送给单片机来处理，在单片机里完成滤波，信息融合以及位姿估计等一系列的处理后，把处理结果送给上位机，使得机器人获得当前的位置和姿态。

本发明相对传统位姿估计方案而言具有显著的进步，主要包括以下几个方面：

1)用光电传感器与单片机实现位姿估计，电路极其简单、成本低、能够实时地、可靠地实现位姿估计。所采用的光电传感器中已经包含了针对这种图像的专门处理电路，而且这种光电传感器在全世界范围内已经得到了广泛的应用，成熟可靠，造价极其低廉。所采用的单片机电平标准和光电传感器电平标准相同，而且具有很强的拉电流、灌电流驱动能力，无需缓冲驱动电路，能够直接接收光电传感器的信号；这种高性能的单片机运算速度很快(最高80MIPS)，甚至优于一般的DSP，极强的抗干扰能力和极低的价格；另外，这种类型的单片机应用广泛，开发容易。能够有效地避免里程计位姿估计过程中所产生的误差，也避免了视觉类方法中复杂的计算量，高昂的软硬件成本，大大降低了开发维护的难度。

2)该位姿传感系统中的光电传感器对环境不敏感，在变化的自然光或环境色差下也能准确估计移动机器人的位姿，所以该位姿传感系统可靠性高。在使用多于两个光电传感器时，可靠性更高。即使其中某个光电传感器出现故障，该位姿传感系统亦能正常工作。

3)该位姿传感系统在软硬件上完全模块化设计，对外仅公开接口，这种面向对象的思想使得编程简单可靠，编程者不需要了解其内部电路及程序，降低了开发难度。不受平台限制，无论是在Windows，Linux，Unix下，还是在各种嵌入式系统如UC/OS2，WinCE下，都能够方便使用。

4)机械结构简单，方便制造，安装，直接安装在移动机器人底部，能够适应绝大多数室内环境。

5)定位精度高，当地面平整时，平移精度最高达3.175*10^-5m，旋转精度最高达(假设两个光电传感器相距1米)1.82*10^-4度，测试的速度范围较大，理想值0m/s～0.36m/s，能够适应绝大多数移动机器人的需要，并且重复定位精度很高。

附图说明

图1是本发明的机械结构以及其在移动机器人车体的安装示意图。

图2是本发明控制系统原理框图。

图3是本发明主程序流程图。

图4是定位方法示意图。

具体实施方式

以下结合附图和实施例对本发明的技术方案作进一步的描述。

图1中：定位螺杆1，定位螺母2，机器人本体3，控制处理电路4，传感器弹压机构5，定位弹簧6，光电传感器7，万向球8，机器人驱动轮9。

如图1所示，整个传感器弹压机构5可以通过定位螺杆1，定位螺母2，定位弹簧6固定在机器人底部，其中将一根定位螺杆1安装在机器人的运动中心，一根安装在机器人的运动正方向。我们可以根据实际情况调节定位螺母2来实现定位螺杆1长度的调节，保证光电传感器基本与地面平行并且在有效的距离以内。定位弹簧6主要作用是保证传感器弹压机构5稳定，另外，当机器人越过障碍时又可以使传感器弹压机构5弹回到原来的位置。万向球8安装在传感器弹压机构5的两端，能够使光电传感器7和地面保持一定的距离，当地面有小的障碍物时，也可以使传感器弹压机构5弹起，从而保护传感器弹压机构5。当调节好传感器弹压机构5后，光电传感器7内的发光二极管照射到地面上，机器人运动时则采集图像至光电传感器7中的图像分析芯片中进行处理，然后计算出移动的距离和方向，直接传给单片机进行位姿估计处理，处理完毕后则通过通信口传给上位机。

图2是本发明控制系统，主要包括光电传感器7、单片机(MCU)12、通信驱动芯片13和通信端口14。其中，光电传感器7、单片机12、通信驱动芯片13和通信端口14顺序连接。

本发明中，将光电传感器7和控制电路做为一体，控制系统主要包括单片机芯片12，光电传感器7，通信驱动芯片13，通信端口14。高性能的单片机12集成了所有的FLASH，RAM，EEPROM，以及其它的接口，不需要任何其它外围配置，所以电路简单，抗干扰能力极强。内置的ICE(在线仿真)可以在目标板上仿真，下载程序，不需要插拨，不需要专门的仿真器和编程器，性价比很高。本实施例如图2所示：光电传感器直接以数字信号的方式输出X，Y方向的位移，所以可以直接与单片机相连接，单片机采集到数据后利用片内固化的程序进行位姿估计处理，更新机器人的位姿，然后通过通信端口发给上位机。

图3为本发明的主程序流程图。本发明主程序用于完成单片机的初始化，估计机器人当前位姿，对上位机通信等功能，也能根据不同环境，对由于地面凹凸不平产生的误差进行补偿。如图3所示，程序的控制过程为：1、系统上电后，首先向上位机发出请求，要求上位机给出初始参数，上位机接到请求则发出初始化命令，完成系统初始化工作，上电时初始化各项值均为默认值，任意时刻上位机发出修改参数的命令都可以改变设置值。初始化工主要包括对光电传感器和通信端口初始化，以及机器人初始位姿的确定与位姿估计的周期等参数；然后判断初始化是否成功，如果没有成功则返回初始化程序重新初始化；2、初始化成功后，采集光电传感器的信息，并进行信息滤波、融合等工作，开始接收光电传感器的输出值，再根据公式进行位姿估计，误差补偿也包含在位姿估计处理子程序中；3、当位姿估计计算完成后则通过通信端口把位姿估计值发送给上位机；最后接收上位机命令信息判断是否结束工作，如果收到结束命令，则程序结束，否则返回到步骤2开始进行下一次位姿估计，一直循环工作，不停地接收光电传感器信号，不停地进行位姿估计计算，不停地给上位机发送实时位姿信息，从而实现机器人位姿信息的实时更新。

Claims (7)

1、一种移动机器人的位姿传感系统，由光电传感器、传感器弹压机构、控制处理电路三部分组成，至少安装两个光电传感器，其中一个安装在机器人的中心点，另一个安装在机器人自身坐标系的X轴正方向上，光电传感器通过图像处理探测到机器人的相对位移和旋转角度，光电传感器的内部电路再将相对位移转换成数字信号送给控制处理电路；传感器弹压机构保证光电传感器与地面保持合适的距离；控制处理电路包括一块单片机和通讯驱动芯片，主程序、初始化子程序、位姿估计处理子程序和通信子程序固化在单片机中，单片机对光电传感器的内部电路送来的数字信息进行滤波、融合以及位姿估计处理以及与上位机的通讯。

2、根据权利要求1所述的移动机器人的位姿传感系统，其特征在于，光电传感器基于反射式工作，通过内部发光二极管发出的光线照射到地面上，然后通过光学透镜将图像传输到光电传感器内部，再由其中的图像分析芯片对移动过程中得到一系列图像进行分析处理，从而得到相对移动的距离和方向，其内部电路再将相对位移转换成数字信号送给控制处理电路。

3、根据权利要求1或2所述的移动机器人的位姿传感系统，其特征在于，两个光电传感器安装在一条直线上。

4、根据权利要求1所述的移动机器人的位姿传感系统，其特征在于，传感器弹压机构(5)通过定位螺杆(1)，定位螺母(2)，定位弹簧(6)固定在机器人底部，其中将一根定位螺杆(1)安装在机器人的运动中心，一根安装在机器人的运动正方向，根据实际情况调节定位螺母(2)来实现定位螺杆(1)长度的调节，保证光电传感器基本与地面平行并且在有效的距离以内，定位弹簧(6)主要作用是保证传感器弹压机构(5)稳定，另外，当机器人越过障碍时又可以使传感器弹压机构(5)弹回到原来的位置，万向球(8)安装在传感器弹压机构的两端，能够使光电传感器和地面保持一定的距离，当地面有小的障碍物时，也可以使传感器弹压机构(5)弹起，从而保护传感器弹压机构(5)，当调节好传感器弹压机构(5)后，光电传感器(7)内的发光二极管照射到地面上，机器人运动时采集图像至光电传感器中的图像分析芯片中进行处理，然后计算出移动的距离和方向，直接传给单片机进行位姿估计处理，处理完毕后则通过通信口传给上位机。

5、根据权利要求1所述的移动机器人的位姿传感系统，其特征在于，所述控制处理电路包括单片机(12)、通信驱动芯片(13)和通信端口(14)，光电传感器(7)、单片机(12)、通信驱动芯片(13)和通信端口(14)顺序连接。

6、根据权利要求5所述的移动机器人的位姿传感系统，其特征在于，单片机(12)集成有FLASH，RAM，EEPROM，内置的在线仿真ICE在目标板上仿真，下载程序。

7.一种移动机器人的位姿传感系统的方法，其步骤如下：1、系统上电后，首先向上位机发出请求，要求上位机给出初始参数，上位机接到请求则发出初始化命令，完成系统初始化工作，上电时初始化各项值均为默认值，任意时刻上位机发出修改参数的命令都可以改变设置值，初始化工作主要包括对光电传感器(7)和通信端口初始化，以及机器人初始位姿的确定与位姿估计的周期参数；然后判断初始化是否成功，如果没有成功则返回初始化程序重新初始化；2、初始化成功后，采集光电传感器信息，并进行信息滤波、融合工作，开始接收光电传感器的输出值，再根据公式进行位姿估计，误差补偿也包含在位姿估计处理子程序中；3、当位姿估计计算完成后则通过通信端口把位姿估计值发送给上位机；最后接收上位机命令信息判断是否结束工作，如果收到结束命令，则程序结束，否则返回到步骤2开始进行下一次位姿估计，一直循环工作，不停地接收光电传感器

信号，不停地进行位姿估计计算，不停地给上位机发送实时位姿信息，从而实现机器人位姿信息的实时更新。

Images