CN101614816B - 一种室内移动机器人位姿检测装置及控制方法 - Google Patents

Abstract

一种室内移动机器人位姿检测装置及控制方法属于机器人导航技术领域。本发明可有效降低系统干扰，并且能够满足定位精度要求。本发明包括固定架，在固定架的两侧设置有左轮、右轮，左轮通过齿轮传动机构与上齿盘相啮合，右轮通过齿轮传动机构与下齿盘相啮合；上、下齿盘与转动架横轴两端的行星齿轮相啮合；在横轴上固定有立轴，立轴与编码器相连；在固定架的下方设置有从动轮，从动轮与编码器相连；编码器与控制电路相连。控制方法包括：进行初始化；判断液晶显示模块扫描时间是否到了，若到了，调用显示程序；若未到，判断计算时间是否到了；通过无线传输模块发送数据；判断计算时间是否到了，若到了，采集数据，并进行机器人当前位置坐标确定。

Description

一种室内移动机器人位姿检测装置及控制方法

技术领域：

本发明属于机器人导航技术领域，特别涉及一种室内移动机器人位姿检测装置及控制方法，用于室内移动机器人的定位和导航。

背景技术：

早在

年，美国就研制出世界上第一台工业机器人。最初的机器人多用于工业生产过程中，随着科学技术的发展及社会的需要，机器人经过数十年的发展已经渗透到生活中的各个领域，包括工业、农业、科研、军事、服务行业等，已成为人们生产和生活中不可缺少的一部分。

移动机器人是一种在复杂的环境下工作的具有自规划、自组织、自适应能力的机器人。在移动机器人的相关技术研究中，位姿检测可以说是其核心技术，也是其实现真正的智能化和完全的自主移动的前提和关键。在移动机器人应用中，精确的位置认知是一个基本问题。关于位姿检测的基本方法常见的有：里程计、惯性导航、磁罗盘、全球定位系统、路标导航和地图模型匹配。上述检测方法都有一些自身的缺陷，例如：惯性导航系统造价高，并且存在电系统的漂移问题；全球定位系统适合于室外的远距离定位；路标导航要求有很多标志性设置等。

发明内容：

针对现有技术存在的不足，本发明提供一种可有效降低系统干扰，并且能够满足定位精度要求的室内移动机器人位姿检测装置及控制方法。

为了实现上述目的，本发明采用如下技术方案，一种室内移动机器人位姿检测装置，包括固定架，在固定架的两侧分别设置有左轮、右轮，左轮通过左轮轴与第一齿轮传动机构相连，第一齿轮传动机构与上齿盘的上端相啮合，右轮通过右轮轴与第二齿轮传动机构相连，第二齿轮传动机构与下齿盘的下端相啮合；在上齿盘与下齿盘之间设置有转动架的横轴，上齿盘的下端、下齿盘的上端分别与转动架的横轴两端的行星齿轮相啮合；在转动架的横轴上固定有转动架的立轴，转动架的立轴与第一编码器相连；在固定架的下方设置有底端与左轮、右轮的底端在同一水平线上的从动轮，从动轮与第二编码器相连；所述的第一编码器、第二编码器分别与控制电路相连。

在固定架上还设置有人机交互模块，人机交互模块包括键盘、液晶显示模块及无线传输模块，所述的键盘、液晶显示模块及无线传输模块分别与控制电路相连。

所述的控制电路包括微处理器，微处理器分别与键盘电路、液晶显示模块、无线传输模块及电源电路相连；第一编码器和第二编码器的输出端分别与信号匹配电路的输入端相连；信号匹配电路的输出端一路经CPLD电路与微处理器相连，另一路直接与微处理器相连。

所述的室内移动机器人位姿检测装置的控制方法，包括如下步骤：

步骤一：进行系统初始化；

步骤二：判断液晶显示模块的扫描时间是否到了，若液晶显示模块的扫描时间到了，则调用显示程序，通过液晶显示模块进行显示；若液晶显示模块的扫描时间未到，则执行步骤四；

步骤三：通过无线传输模块的串口向上位机发送数据；

步骤四：判断计算时间是否到了，若计算时间到了，则采集CPLD电路输出的导航角数据和信号匹配电路输出的里程数据；并通过采集的导航角数据和里程数据进行机器人当前位置的坐标确定；然后，返回执行步骤二。

步骤四中所述的机器人当前位置的坐标确定的数学模型为：

$\left\{\begin{array}{c}{x}_1=x_0+l\·\cos \left(\frac{{\mathrm{\θ}}_0+{\mathrm{\θ}}_1}{2}\right)\\ y_1=y_0+l\·\sin \left(\frac{{\mathrm{\θ}}_0+{\mathrm{\θ}}_1}{2}\right)\\ l=\frac{2s}{{\mathrm{\θ}}_1-{\mathrm{\θ}}_0}\·\sin \left(\frac{{\mathrm{\θ}}_1-{\mathrm{\θ}}_0}{2}\right)\end{array}\right.$

式中，(x₀，y₀，θ₀)为机器人的初始位姿，(x₁，y₁，θ₁)为机器人的终止位姿，s为机器人实际运行轨迹的弧线长度，l为机器人实际运行轨迹弧线对应的弦长。

本发明的有益效果：

本发明的位姿检测装置利用机械机构实现对移动机器人的航向角测量，其内部传动机构是典型的齿轮组结构，它利用差速齿轮传动系统，根据车轮的转动，反应移动机器人的转动方向和转动角度，并利用光电编码器实现对移动机器人的里程测量。将测得的信号经过控制电路后得出移动机器人的即时坐标，实现移动机器人的自主定位；本发明的位姿检测装置工作可靠，不受光线、温度和磁场等影响，可有效降低系统干扰，并且能够满足定位精度的要求。

附图说明：

图1是本发明的检测装置的结构示意图；

图2是本发明的控制电路的电路原理框图；

图3是本发明的控制电路的电路原理图，其中：

图3a是本发明的控制电路中电源电路的原理图；

图3b是本发明的控制电路中无线传输模块的电路原理图；

图3c是本发明的控制电路中单片机及其外围电路的原理图；

图3d是本发明的控制电路中信号匹配电路的原理图；

图3e是本发明的控制电路中CPLD电路的原理图；

图3f是本发明的人机交互模块的电路原理图；

图4是本发明的控制方法的程序流程图；

图5是本发明的机器人运动分析示意图；

图6是本发明的测试曲线的示意图；

图7是航位推测法数学原理的示意图；

其中，图1中，1-左轮，2-行星齿轮，3-左轮轴，4-左齿轮，5-左过渡齿轮，6-上齿盘，7-立轴，8-第一编码器，9-转动架，10-右轮，11-右轮轴，12-右齿轮，13-右第一过渡齿轮，14-右第二过渡齿轮，15-下齿盘，16-第二编码器，17-从动轮，18-横轴，19-第一齿轮传动机构，20-第二齿轮传动机构，21-固定架，22-人机交互模块。

具体实施方式：

如图1所示，一种室内移动机器人位姿检测装置，包括固定架21，在固定架21的两侧分别设置有左轮1、右轮10，左轮1通过左轮轴3与第一齿轮传动机构19相连，第一齿轮传动机构19与上齿盘6的上端相啮合，右轮10通过右轮轴11与第二齿轮传动机构20相连，第二齿轮传动机构20与下齿盘15的下端相啮合；在上齿盘6与下齿盘15之间设置有转动架9的横轴18，上齿盘6的下端、下齿盘15的上端分别与转动架9的横轴18两端的行星齿轮2相啮合；在转动架9的横轴18上固定有转动架9的立轴7，转动架9的立轴7与第一编码器8相连；在固定架21的下方设置有底端与左轮1、右轮10的底端在同一水平线上的从动轮17，从动轮17与第二编码器16相连；所述的第一编码器8、第二编码器16分别与控制电路相连。在固定架21上还设置有人机交互模块22，人机交互模块22包括键盘、液晶显示模块及无线传输模块，所述的键盘、液晶显示模块及无线传输模块分别与控制电路相连。

所述的第一齿轮传动机构19包括左齿轮4和左过渡齿轮5，左轮轴3与左齿轮4相连，左齿轮4与左过渡齿轮5相啮合，左过渡齿轮5与上齿盘6的上端相啮合。所述的第二齿轮传动机构20包括右齿轮12、右第一过渡齿轮13和右第二过渡齿轮14，右轮轴11与右齿轮12相连，右齿轮12与右第一过渡齿轮13相啮合，右第一过渡齿轮13与右第二过渡齿轮14相啮合，右第二过渡齿轮14与下齿盘15的下端相啮合。

如图2所示，所述的控制电路包括微处理器，微处理器分别与键盘电路、液晶显示模块、无线传输模块及电源电路相连；第一编码器和第二编码器的输出端分别与信号匹配电路的输入端相连，由编码器为信号匹配电路提供输入信号；信号匹配电路的输出端一路经CPLD电路与微处理器相连，另一路直接与微处理器相连。

本发明的键盘电路的按键可终止当前的计算，并可清除计算结果，重新开始；同时可监测多个数据的最终结果，切换显示模式等。

本发明的控制电路中的微处理器采用Microchip公司生产的单片机dsPIC30F-6010，其带有正交编码器接口，可直接对编码器信号进行处理。如图3所示，QEI1和QEI2分别为第一编码器和第二编码器的信号输入接口，每一路编码器输出A、B两相信号；A、B两相信号先经过电流驱动芯片74LS541进行电流驱动后，再经过光电隔离芯片6N137进行光电隔离；然后，经处理后的第二编码器输出的A、B两相信号分别与单片机的QEA、QEB引脚相连，经处理后的第一编码器输出的A、B两相信号与CPLD电路的通用I/O口相连。CPLD电路通过相关时序功能电路对第一编码器输出的信号进行处理，并将处理结果通过相应通信时序传送给微处理器。本发明采用的单片机自身有两个串行通信接口，用MAX-232芯片完成电平转换，通过单片机的TXD和RXD引脚进行通信，用于将输出信号与上位机232接口相连；本发明还可利用无线传输模块的无线串口射频传输数据到上位机。

本发明的液晶显示模块采用LCD12864，该液晶显示模块在显示机器人的角度和里程信息等的同时，可将计算所得的坐标实时更新。键盘电路的按键管理采用芯片HD7279，单片机的I/O口分别与HD7279芯片的片选引脚CS、时钟引脚CLK及数据引脚DATA相连，用于管理键盘，读取当前按键值；当有键按下时引发中断，HD7279芯片的中断引脚与单片机的外部中断引脚IN4相连，引发单片机中断进行按键处理。LCD显示模块采用串行工作方式，利用单片机通用I/O口与LCD串行时钟和数据端口相连进行相应管理，通过另一单片机I/O口控制背光。

本发明的CPLD电路包括辨向电路的实现、滤波电路和倍频输出，根据CPLD的逻辑门延迟滤除编码器换向时输出的干扰信号，并通过检测脉冲的上升与下降沿实现编码器的4倍频，可更精确检测角度。

如图4所示，所述的室内移动机器人位姿检测装置的控制方法，包括如下步骤：

步骤一：进行系统初始化；

步骤二：判断液晶显示模块的扫描时间是否到了，若液晶显示模块的扫描时间到了，则调用显示程序，通过液晶显示模块进行显示；若液晶显示模块的扫描时间未到，则执行步骤四；

步骤三：通过无线传输模块的串口向上位机发送数据；

步骤四：判断计算时间是否到了，若计算时间到了，则采集CPLD电路输出的导航角数据和信号匹配电路输出的里程数据；并通过采集的导航角数据和里程数据进行机器人当前位置的坐标确定；然后，返回执行步骤二。

步骤四中所述的机器人当前位置坐标的确定的具体过程如下：

如图5所示，机器人的位姿可由广义坐标向量q＝[x_P，y_P，θ]^T表示，其中(x_P，y_P)为机器人上参考点P在二维平面内的投影坐标，[x_M，y_M，θ]^T为机器人在M点的位姿坐标向量，d为轮距，θ为机器人的导航角，即机器人前进方向同X轴之间的夹角，左轮、右轮的旋转角速度为ω_L和ω_R。由参考点P简化到点M，机器人的运动学模型和约束方程可分别简化为：

$\left[\begin{array}{c}{\stackrel{\·}{x}}_M\\ {\stackrel{\·}{y}}_M\\ \stackrel{\·}{\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{cc}\frac{r}{2}\cos \mathrm{\θ}& \frac{r}{2}\cos \mathrm{\θ}\\ \frac{r}{2}\sin \mathrm{\θ}& \frac{r}{2}\sin \mathrm{\θ}\\ -\frac{r}{d}& \frac{r}{d}\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_L\\ {\mathrm{\ω}}_R\end{array}\right]---1-1$

${\stackrel{\·}{x}}_M\sin \mathrm{\θ}-{\stackrel{\·}{y}}_M\cos \mathrm{\θ}=0---1-2$

由此可知，机器人方向的改变只是通过两个车轮之间的速度差值实现，而机器人运动轨迹则由一系列绕瞬时圆心旋转的小段圆弧组成。

航位推测法是建立在广义坐标向量的基础之上的，如图7所示，现假设机器人初始位姿为(x₀，y₀，θ₀)，弧线前进s距离，终止位姿为(x₁，y₁，θ₁)，根据图7可得出所述的机器人当前位置的坐标确定的数学模型：

$\left\{\begin{array}{c}{x}_1=x_0+l\·\cos \left(\frac{{\mathrm{\θ}}_0+{\mathrm{\θ}}_1}{2}\right)\\ y_1=y_0+l\·\sin \left(\frac{{\mathrm{\θ}}_0+{\mathrm{\θ}}_1}{2}\right)\\ l=\frac{2s}{{\mathrm{\θ}}_1-{\mathrm{\θ}}_0}\·\sin \left(\frac{{\mathrm{\θ}}_1-{\mathrm{\θ}}_0}{2}\right)\end{array}\right.$

式中，(x₀，y₀，θ₀)为机器人的初始位姿，(x₁，y₁，θ₁)为机器人的终止位姿，s为机器人实际运行轨迹的弧线长度，l为机器人实际运行轨迹弧线对应的弦长。

在本发明的位姿检测装置中，程序执行周期很短，机器人的位姿不会发生太大的变化，所以距离s一般很小，角度变化也很小，完全可以将机器人的轨迹看成是圆弧的一段。这样，根据机器人的初始位姿，通过固定周期反复调用上述数学模型的公式就能推算出机器人在任何时刻的位姿；本发明利用实时时钟每隔10ms调用一次上述数学模型的公式，完成相关计算。另外，当角度变化量θ₁-θ₀很小时，有l≈s，二者相差甚微，可以舍去公式 $l=\frac{2s}{{\mathrm{\θ}}_1-{\mathrm{\θ}}_0}\·\sin \left(\frac{{\mathrm{\θ}}_1-{\mathrm{\θ}}_0}{2}\right),$ 也就是将机器人的轨迹看成是由很多个小段直线进行逼近。

下面结合附图对本发明做进一步的说明：

本发明的位姿检测装置在使用过程中，对导航角的检测原理类似于古代的指南车。当本发明的位姿检测装置的左轮、右轮转速相同时，上齿盘、下齿盘以相同的转速反向旋转，此时，转动架的横轴不转，与转动架的横轴相连的转动架的立轴也不转。当左轮、右轮有转速差时，上齿盘、下齿盘也存在转速差，从而带动转动架的横轴转动，转动架的横轴带动转动架的立轴转动。由于转动架的立轴与第一编码器相连，就可通过第一编码器测量转动架的立轴的转动角度，转动架的立轴的转动角度正比于机器人的导航角，就可根据传动比转化成角度实时检测出导航角θ。本发明的位姿检测装置通过设置在固定架下方的从动轮及与之相连的第二编码器检测机器人运动的里程s。

本发明的位姿检测装置的控制电路再根据检测的导航角θ、里程s实时确定机器人当前相对于原点的位置坐标(x₁，y₁，θ₁)，即：位姿；并通过液晶显示模块实时显示机器人的位姿(x₁，y₁，θ₁)，通过无线传输模块将数据信息传送给上位机，由上位机实时绘制出机器人的运动坐标轨迹。

如图6所示，将本发明的位姿检测装置沿1、2、3条不同路径到达目标点坐标(2500，1500)，测量结果如表1.1、1.2所示：

表1.1实际到达目标点坐标

表1.2原地转圈测量转角

结果表明，本发明的位姿检测装置在室内实验环境中基本满足定位精度要求，从导航角和里程的测量上都可以达到一定的指标。通过实验得出，本发明的位姿检测装置可以应用于室内环境移动机器人的自主定位。

Claims (5)

1.一种室内移动机器人位姿检测装置，包括固定架(21)，在固定架(21)的两侧分别设置有左轮(1)、右轮(10)，其特征在于左轮(1)通过左轮轴(3)与第一齿轮传动机构(19)相连，第一齿轮传动机构(19)与上齿盘(6)的上端相啮合，右轮(10)通过右轮轴(11)与第二齿轮传动机构(20)相连，第二齿轮传动机构(20)与下齿盘(15)的下端相啮合；在上齿盘(6)与下齿盘(15)之间设置有转动架(9)的横轴(18)，上齿盘(6)的下端、下齿盘(15)的上端分别与转动架(9)的横轴(18)两端的行星齿轮(2)相啮合；在转动架(9)的横轴(18)上固定有转动架(9)的立轴(7)，转动架(9)的立轴(7)与第一编码器(8)相连；在固定架(21)的下方设置有底端与左轮(1)、右轮(10)的底端在同一水平线上的从动轮(17)，从动轮(17)与第二编码器(16)相连；所述的第一编码器(8)、第二编码器(16)分别与控制电路相连。

2.根据权利要求1所述的一种室内移动机器人位姿检测装置，其特征在于在固定架(21)上还设置有人机交互模块(22)，人机交互模块(22)包括键盘、液晶显示模块及无线传输模块，所述的键盘、液晶显示模块及无线传输模块分别与控制电路相连。

3.根据权利要求1或2所述的一种室内移动机器人位姿检测装置，其特征在于所述的控制电路包括微处理器，微处理器分别与键盘电路、液晶显示模块、无线传输模块及电源电路相连；第一编码器和第二编码器的输出端分别与信号匹配电路的输入端相连；信号匹配电路的输出端一路经CPLD电路与微处理器相连，另一路直接与微处理器相连。

4.权利要求1所述的室内移动机器人位姿检测装置的控制方法，其特征在于，包括如下步骤：

步骤一：进行系统初始化；

步骤二：判断液晶显示模块的扫描时间是否到了，若液晶显示模块的扫描时间到了，则调用显示程序，通过液晶显示模块进行显示；若液晶显示模块的扫描时间未到，则执行步骤四；

步骤三：通过无线传输模块的串口向上位机发送数据；

步骤四：判断计算时间是否到了，若计算时间到了，则采集CPLD电路输出的导航角数据和信号匹配电路输出的里程数据；并通过采集的导航角数据和里程数据进行机器人当前位置的坐标确定；然后，返回执行步骤二；若计算时间未到，则返回执行步骤四。

5.根据权利要求4所述的室内移动机器人位姿检测装置的控制方法，其特征在于，步骤四中所述的机器人当前位置的坐标确定的数学模型为：

$\left\{\begin{array}{c}{x}_1=x_0+l\·\cos \left(\frac{{\mathrm{\θ}}_0+{\mathrm{\θ}}_1}{2}\right)\\ y_1=y_0+l\·\left(\frac{{\mathrm{\θ}}_0+{\mathrm{\θ}}_1}{2}\right)\\ l=\frac{2s}{{\mathrm{\θ}}_1-{\mathrm{\θ}}_0}\·\sin \left(\frac{{\mathrm{\θ}}_1-{\mathrm{\θ}}_0}{2}\right)\end{array}\right.$

式中，(x₀，y₀，θ₀)为机器人的初始位姿，(x₁，y₁，θ₁)为机器人的终止位姿，s为机器人实际运行轨迹的弧线长度，l为机器人实际运行轨迹弧线对应的弦长。

Images