CN101637915A - 一种lego机器人自主定位用闪烁红外光源装置与定位方法 - Google Patents

Abstract

本发明公开了一种闪烁红外光源及其定位方法，该光源和定位方法可用于LEGO玩具机器人领域，尤其适合于多机器人的自主定位。所发明的闪烁红外光源包括：普通白炽灯、带有栅孔的可旋转灯罩、以及带动灯罩旋转的微型马达和支架。采用本发明的闪烁红外光源进行定位的主要原理是通过测量机器人所在位置与位于正方形场地三个顶点红外光源之间的方位夹角，通过求解所构成的三角形，最终得到机器人所处位置。在具体定位过程中需要3个栅孔数不同的闪烁红外光源、配有定向导管的红外传感器(LEGO构件之一)，定位过程还需要实现本发明所公开的定位算法的软件配合。本发明可实现机器人自主定位，克服了测量误差积累和场地打滑带来的定位误差，低成本，高性能。

Description

一种LEGO机器人自主定位用闪烁红外光源装置与定位方法

技术领域

本发明涉及机器人玩具领域，涉及一种基于三角形求解算法的机器人自主定位装置，尤其涉及一种LEGO(乐高，品牌)机器人自主定位用闪烁红外光源，适用于LEGO机器人构件组成的单个及多机器人系统。

背景技术

现有玩具机器人(如LEGO机器人)的定位十分简单，常见有两类，一类是通过光电探测沿着场地上特定图案行进(例如白色场地的黑色宽线条，或类似国际象棋棋盘的栅格场地)；另一类是朝着红外光线体的所在方向前进(例如足球机器人，足球本身就是个红外发光源，机器人则朝着足球所在地点前进)。上述方法存在的缺点是：因场地打滑、测量误差以及误差积累等因素的存在，严重影响机器人的定位精度

虽然在单机器人中上述定位方法及装置所引起的误差尚可接受，但在多机器人系统中，机器人之间相对位置的计算依赖于每个机器人的定位信息，这导致积累效应十分明显，上述传统定位方法和装置已不适用。

发明内容

本发明的目的是为了解决已有技术中玩具机器人的定位精度低的问题，提出一种闪烁红外光源装置及定位方法。

本发明的目的是通过下述技术方案实现的。

本发明的一种LEGO机器人自主定位用闪烁红外光源装置，包括：马达、支架、灯罩、白炽灯、灯座，它们之间的连接关系为：马达、灯座固定在支架上，马达的传动轴连接灯罩，灯罩上刻有纵向狭缝，简称栅孔，其数目可根据具体需要确定。灯罩上装有转动轴，连接马达后可以旋转，由于灯罩上的栅孔，灯罩旋转后就会使灯罩内部白炽灯发出的红外光线按一定频率被遮挡，从而整个光源就会在各个方向上发出闪烁的红外光线；外围部件有LEGO机器人，在该机器人的红外传感器前端固定有定向导管，如图3所示。该导管内壁覆盖有吸收红外线的物质，目的是减弱与导管不平行方向的入射红外线强度，从而使方向测量更精确；在机器人控制核心部件NXT上装有控制机器人的定位软件。

定位方法原理如下：

机器人活动形场地为正方形，如图4所示，机器人所在位置点P与正方形场地三个顶点D、A、B所成的两个夹角∠APB和∠DPA分别记为α和β；∠PAB记为θ，定义场地的边长为a，则通过求解三角形可以得到机器人的横纵坐标X和Y满足如下定位公式：

$\left\{\begin{array}{c}\tan \mathrm{\θ}=-\frac{1-\cot \mathrm{\β}}{1-\cot \mathrm{\α}}\\ X=\frac{a}{\tan (\mathrm{\θ}-\mathrm{\β})+\tan \mathrm{\θ}}=\frac{(1+\tan \mathrm{\θ}\tan \mathrm{\β})}{2\tan \mathrm{\θ}+{\tan }^2\mathrm{\θ}\tan \mathrm{\β}-\tan \mathrm{\β}}a\\ Y=X\tan \mathrm{\θ}\end{array}\right.$

于是只要测得α和β就可计算出tanθ，进而定位机器人。

本发明的一种LEGO机器人自主定位用闪烁红外光源装置定位方法的具体步骤如下：

[1]将闪烁频率不同的三个闪烁红外光源放置在正方形场地的三个顶点，如图4所示，其中D点光源闪烁频率最低，B点光源闪烁频率最高。然后进行测量初始化，机器人逆时针转动红外传感器直到探测到有红外信号时停止转动，此时传感器指向三个信号灯中的某个；

[2]测量所指向信号灯闪烁频率，计为f₁；

[3]继续逆时针转动红外传感器直到探测到有红外信号时停止转动，读取此次红外传感器转动的角度，计为δ₀；

[4]测量此时传感器所指信号灯闪烁频率，计为f₂；

[5]继续逆时针转动红外传感器直到探测到有红外信号时停止转动，读取此次红外传感器转动的角度，计为δ₁。

[6]测量此时传感器所指信号灯闪烁频率，计为f₃

[7]比较所测三个频率，根据角度间的关系，得不同情况下图4中的α和β，具体规律为：

当f₁＜f₂＜f₃时，α＝δ₁，β＝δ₀

当f₃＜f₁＜f₂时，α＝δ₀，β＝360-δ₀-δ₁

当f₁＜f₂＜f₃时，α＝360-δ₀-δ₁，β＝δ₁

[8]根据定位公式即可计算出机器人横、纵坐标。

有益效果

本发明的定位装置与方法的优点在于克服了普通定位方法中由于各种误差及误差积累导致的定位不准确的缺点。

本发明的闪烁红外光源与常见的红外发光管光源相比，优点在于本发明光源几乎具有360度的散射角，而普通红外发光管的散射角只有约30度；与带通断电子开关的白炽灯相比，本发明的闪烁频率范围十分宽，便于控制软件的实现。而且本发明的实现成本很低。

附图说明

图1  本发明的闪烁红外光源结构及原理示意图；

图2  本发明的旋转灯罩的栅孔示意图(灯罩底视图)；

图3  本发明的LEGO机器人红外传感器改造示意图(加装定向导管)；

图4  本发明定位原理示意图；

图5  本发明实施例定位原理示意图；

其中，1-马达、2-支架、3-灯罩、4-白炽灯、5-灯座、6-灯罩不透明部分、7-灯罩透明部分(栅孔)、8-定向导管、9-红外传感器(LEGO组件之一)、10-最低闪烁频率红外光源、11-中间闪烁频率红外光源、12-最高闪烁频率红外光源、13-机器人、14-红外传感器(可旋转，LEGO构件之一)、15-角度α、16-角度β、17-角度θ、ABCD为正方形、α＝∠APB、β＝∠APD、θ＝∠PAB

具体实施方式

下面以LEGO机器人为例，结合附图和实施例对本发明的具体定位方法及控制软件流程做详细说明。

本发明的一种LEGO机器人自主定位用闪烁红外光源装置如图1所示，包括：马达(1)、支架(2)、灯罩(3)、白炽灯(4)、灯座(5)；马达(1)、灯座(5)固定在支架上(2)，马达(1)的传动轴连接灯罩(3)，灯罩(3)上刻有纵向狭缝，简称栅孔(7)如图2所示，其数目可根据具体需要确定。灯罩上装有转动轴，连接马达后可以旋转，由于灯罩上的栅孔，灯罩旋转后就会使灯罩内部白炽灯发出的红外光线按一定频率被遮挡，从而整个光源就会在各个方向上发出闪烁的红外光线；外围部件有LEGO机器人，在该机器人的红外传感器前端固定有定向导管，如图3所示。

本发明的一种LEGO机器人自主定位用闪烁红外光源装置定位方法如图5所示，具体步骤如下：

[1]分别在正方形场地的三个顶点放置闪烁红外光源，三个光源的灯罩上栅孔数分别为2、4、8；三个光源的马达选择相同型号，并保证他们的转速相近。正方形场地的边长6米。

[2]组装LEGO机器人，其上带有可以旋转的红外传感器，红外传感器前端使用密封胶(硅酸橡胶)固定一个长约3厘米的塑料管作为定向导管，该管内壁覆盖炭黑目的是吸收照射在管壁上的红外线，以减弱其反射光线强度。

[3]进行测量初始化，在软件的控制下红外传感器逆时针转动，每旋转0.5度测量一次，直到探测到有红外信号停止转动，此时传感器指向三个闪烁红外光源中的某个。此过程中，一旦转动180度后仍然没有探测到红外光源，则说明系统出现错误，报错。

[4]测量信号灯闪烁频率，计为f₁：取时长2秒，每0.01秒测量一次光强，记录光线由暗到明变化次数，即得所测频率。

[5]继续逆时针转动红外传感器40度，这样做主要为了提高定位速度，考虑到即将要测量的δ₀和δ₁的均大于45度，所以从40度以后开始探测。

[6]每旋转0.5度测量一次，直到探测到红外信号时停止转动，读取此次红外传感器转动的角度，该角度与[5]中所转动的40度的和计为δ₀。

[7]重复[2]的测量步骤，测量传感器所指信号灯闪烁频率，但该频率计为f₂。

[8]重复[3]、[4]的测量步骤，读取此次红外传感器转动的角度，但该角度计为δ₁。

[9]重复[2]的测量步骤，测量此时传感器所指信号灯闪烁频率，但该频率计为f₃

[10]比较所测三个频率的大小，由于三个频率之间呈倍数关系，所以频率大小的判断不必很精确即可得出正确结果。根据角度间的关系，得不同情况下图1中的α和β，具体规律为：

f₁＜f₂＜f₃时，α＝δ₁，β＝δ₀

f₃＜f₁＜f₂时，α＝δ₀，β＝360-δ₀-δ₁

f₁＜f₂＜f₃时，α＝360-δ₀-δ₁，β＝δ₁

[11]根据前文所给出的定位公式首先计算出tanθ、tanβ、cotα、cotβ，进而计算出机器人的横、纵坐标X、Y。

本发明的定位公式推导如下：

对边长为a的正方形机器人场地(图4)，有下述推导：

根据正弦定理，在ΔAPD中有

同样根据正弦定理，在ΔABP中

两式相除并展开得 $\frac{\sin \mathrm{\α}}{\sin \mathrm{\β}}=\frac{\sin \mathrm{\θ}\cos \mathrm{\α}+\cos \mathrm{\θ}\sin \mathrm{\α}}{\cos \mathrm{\θ}\cos \mathrm{\β}+\sin \mathrm{\θ}\sin \mathrm{\β}};$ 等号两侧分子同时除以cosα，分母同时除以cosβ，得 $\frac{\tan \mathrm{\α}}{\tan \mathrm{\β}}=\frac{\sin \mathrm{\θ}+\cos \mathrm{\θ}\tan \mathrm{\α}}{\cos \mathrm{\θ}+\sin \mathrm{\θ}\tan \mathrm{\β}};$ 右侧分子分母同时除以cosθ，有 $\frac{\tan \mathrm{\α}}{\tan \mathrm{\β}}=\frac{\tan \mathrm{\θ}+\tan \mathrm{\α}}{1+\tan \mathrm{\θ}\tan \mathrm{\β}};$ 解得 $\tan \mathrm{\θ}=-\frac{1-\cot \mathrm{\β}}{1-\cot \mathrm{\α}}.$

由于正方形ABCD的垂直方向有EP+PF＝a，即

Xcot∠PDA+Xtanθ＝a＝＝＝＝＞Xcot(90°+θ-β)+Xtanθ＝a，其中X为横坐标。

进而解得：

$\left\{\begin{array}{c}\tan \mathrm{\θ}=-\frac{1-\cot \mathrm{\β}}{1-\cot \mathrm{\α}}\\ X=\frac{a}{\tan (\mathrm{\θ}-\mathrm{\β})+\tan \mathrm{\θ}}=\frac{(1+\tan \mathrm{\θ}\tan \mathrm{\β})}{2\tan \mathrm{\θ}+{\tan }^2\mathrm{\θ}\tan \mathrm{\β}-\tan \mathrm{\β}}a\\ Y=X\tan \mathrm{\θ}\end{array}\right.;.$

Claims (2)

1、一种LEGO机器人自主定位用闪烁红外光源装置，其特征在于包括：马达、支架、灯罩、白炽灯、灯座，它们之间的连接关系为：马达、灯座固定在支架上，马达的传动轴连接灯罩，灯罩上刻有纵向狭缝，简称栅孔，其数目可根据具体需要确定；灯罩上装有转动轴，连接马达后可以旋转，由于灯罩上的栅孔，灯罩旋转后就会使灯罩内部白炽灯发出的红外光线按一定频率被遮挡，从而整个光源就会在各个方向上发出闪烁的红外光线；外围部件有LEGO机器人，在该机器人的红外传感器前端固定有定向导管，该导管内壁覆盖有吸收红外线的物质，目的是减弱与导管不平行方向的入射红外线强度，从而使方向测量更精确；在机器人控制核心部件NXT上装有控制机器人的定位软件。

2、一种LEGO机器人自主定位用闪烁红外光源装置的定位方法，其特征在于具体实现步骤如下：

[1]将闪烁频率不同的三个闪烁红外光源放置在正方形场地的三个顶点，其中D点光源闪烁频率最低，B点光源闪烁频率最高；然后进行测量初始化，机器人逆时针转动红外传感器直到探测到有红外信号时停止转动，此时传感器指向三个信号灯中的某个；

[2]测量所指向信号灯闪烁频率，计为f₁；

[3]继续逆时针转动红外传感器直到探测到有红外信号时停止转动，读取此次红外传感器转动的角度，计为δ₀；

[4]测量此时传感器所指信号灯闪烁频率，计为f₂；

[5]继续逆时针转动红外传感器直到探测到有红外信号时停止转动，读取此次红外传感器转动的角度，计为δ₁；

[6]测量此时传感器所指信号灯闪烁频率，计为f₃；

[7]比较所测三个频率，根据角度间的关系，得不同情况下的角度α和β，具体规律为：

当f₁＜f₂＜f₃时，α＝δ₁，β＝δ₀

当f₃＜f₁＜f₂时，α＝δ₀，β＝360-δ₀-δ₁

当f₁＜f₂＜f₃时，α＝360-δ₀-δ₁，β＝δ₁

[8]根据定位公式即可计算出机器人横、纵坐标。

Images