CN106802658A - 一种全自动高精度室内快速定位方法 - Google Patents

Abstract

本发明公开了一种全自动高精度室内快速定位方法，包括硬件系统和软件系统。其中硬件系统包括彩色图像采集装置、激光散斑投射装置、激光散斑图像采集装置、云台控制装置、通信装置、中央控制装置；软件系统包括激光散斑投射系统、彩色图像与激光散斑图像采集系统、图像处理系统、图像匹配系统、模式识别系统、云台控制系统、中央控制系统、路径规划与控制系统；本发明方案采取软硬件结合，使室内定位装置完成从个人/家用服务机器人特征信息采集录入、室内物体图像采集与匹配识别、室内物体定位、路径规划、机器人运动控制整个过程的智能化和系统化。使室内定位、路径规划与控制方法更加精确和高效。

Description

一种全自动高精度室内快速定位方法

技术领域

本发明涉及机器人控制领域，具体涉及一种全自动高精度室内快速定位方法。

背景技术

随着现代经济的快速发展,人们的生活水平不断提高，越来越多的个人/家庭用服务机器人走进千家万户，让人们的家居生活更加舒适。据统计，2012年全球个人/家用服务机器人销量约为300万台，较2011年增长20％，销售额为12.21亿美元；2014年全球个人/家用服务机器人销量约为470万台，较2013年增长17.50％，销售额约为22.5亿美元。其中，家务机器人(主要包括吸尘机器人、除草机器人、泳池清理机器人、窗户清洗机器人等)的销量约为270万台，销售额约为7.99亿美元。娱乐休闲机器人(主要包括玩具机器人、教育训练机器人、娱乐用途机器人等)销量约为120万台；销售额约为9.11亿美元。根据IFR的数据预测，2015-2018年，未来清洁机器人会首先打入家庭市场，扫地机器人的全球销量将达到2500万台，拖地机器人的销量会达到49.6万台。未来三年内，个人/家庭用服务机器人的全球销量将高达2590万台，市场规模高达到122亿美元，超过2014年市场规模的5倍。

个人/家庭用服务机器人的工作主要是在室内环境下进行的，而室内环境与户外的空旷环境有着很大的不同，在室内机器人无法正常接收卫星定位信号和基站定位信号，使得定位的精度大大降低。同时，室内的环境相对拥挤，家具等物品的摆放和变动更增加了室内环境的复杂度和动态性。而在复杂的室内环境下工作的机器人，对定位的精度和速度也有着更高的要求，相对成熟的室外定位技术并不能很好的满足这些需求。为了让个人/家庭用服务机器人更好地在室内环境下正常工作，需要一种实用的室内定位解决方案，要求其在精度、覆盖范围、可靠性、成本、功耗、可扩展性和响应时间这几个方面都能够满足要求。

目前，普遍而公知的室内定位方法主要有惯性导航定位方法、室内无线定位方法、双目视觉定位方法以及激光雷达定位方法等，现在存在的室内定位与控制专利公开方案，在精度、覆盖范围、可靠性、成本、功耗、可扩展性和响应时间方面都存在一定的不足。

申请公布号CN104567931A采用的是IMU(Inertial Measurement Unit，惯性测量单元，包含三轴加速度计和三轴陀螺仪)与三轴磁力计相结合，构成AHRS(Attitude andHeading Reference System,航姿参考系统)，通过匹配已知地图，实现室内定位功能。该方法提出了一种航向漂移误差的消除方法，可有效解决由于静止时陀螺仪不工作导致的航向漂移误差，以此提高室内惯性导航系统的精度。但是在目标长时间运动时，陀螺仪和加速度计估计误差的积累仍然会对定位精度造成很大的影响。同时，该定位方法对地图有一定的依赖性，当室内环境发生变化时不能及时更新地图，大大降低了实用性和灵活性。

申请公布号CN106125045A采用基于Wi-Fi(Wireless-Fidelity，无线保真)的自适应混合室内定位方法，通过将接收点对应AP(Wireless Access Point，无线访问接入点)的RSSI(Received Signal Strength Indication，接收的信号强度指示)值转换为其到相应AP的距离，采用三边定位算法计算测试点的三边定位坐标，采用位置指纹定位算法计算测试点的指纹定位坐标，再根据样本数据计算二者的权重系数进行融合，实现在减少室内中心区域采样数量的同时保持较高的定位精度。该方法中，信号的传播会受到室内结构布局、建筑及家具材料差异的影响，而信号的穿墙、折射、反射、绕射等传播会直接产生NLOS(NonLine of Sight，非视距)误差，同时信号强度也存在着不稳定性，给出的定位精度低，大概能得到10ˉ20m的精度，有些情况可能更低。再者，加入指纹定位算法后，虽然可以使定位精度提高，达到3ˉ5m的精度，但由于系统依赖射频信号强度的指纹数据库,引入的指纹坐标数据库需要进行大量的信号强度检测，对于大规模的使用，数据库大，产生和维护成本相对较高，布局成本也相应提高，同时也在一定程度上造成可移植性差。

申请公布号CN106204595A采用的方法是利用双目摄像机覆盖需要监视的场面范围，并将这些摄像机进行预标定，根据双目视觉原理监控恢复出每个双目摄像机的视频帧图像对应的立体稠密点云，再将这些点云统一到三维世界坐标下，融合成全景监视场景，最后将所有点云重建为表面三维模型，从而实现空间定位。该定位方法对摄像机的标定要求较高，不允许任意移动或者改变观测方向及画面变焦缩放，故操作性较低。同组的两个摄像机在进行双目定位时，要求两幅图像为同一时间采集的，故同步要求比较高。图像在采集、预处理、匹配等阶段对计算能力要求较高，同时双目视觉算法复杂，进一步增加了计算难度，严重影响了定位速度。而摄像机为被动接收图像信息，受光线影响比较大。

申请公布号CN106226780A采用的是二维激光雷达与二轴云台相结合，对整个空间进行扫描定位的方法，将二轴云台靠墙放置在中间位置，激光雷达置于云台顶部，通过给云台设置俯仰角0°～60°，激光扫描雷达在四维空间不断扫描，对采集的数据进行处理，建立室内环境模型，通过比对多组环境模型，得到多旋翼无人机相对于云台和激光扫描雷达的空间三维位置信息。此定位方法适用于较为空旷，障碍物不多的室内环境，且目前激光扫描雷达扫描频率低，一般只有10Hz左右，扫描角度有限，价格高昂，无法满足家用机器人的室内定位要求。

综上所述，虽然惯性导航定位方法、室内无线定位方法、双目视觉定位方法以及激光雷达定位方法可以在一定程度上实现室内定位功能，但惯性导航定位方法存在定位精度低、可靠性低、灵活性低等问题，室内无线定位方法存在定位精度低、可靠性低、响应时间长等问题，双目视觉定位方法存在可扩展性低、响应时间长、抗干扰能力弱等问题，激光雷达定位方法存在覆盖范围小、响应时间长、成本高等问题。

发明内容

本发明所要解决的主要技术问题是：为了克服现有室内定位技术存在的不足，提供一种全自动高精度室内快速定位方法，具备高精度、高可靠性、高可扩展性、高覆盖范围、低响应时间、低成本等特点。

为了解决上述的技术问题，本发明提供了一种全自动高精度室内快速定位方法，包括一定位系统，所述定位系统包括：硬件机械部分和软件系统；

所述硬件机械部分包括彩色图像采集装置、激光散斑投射装置、激光散斑图像采集装置、云台控制装置、站点控制装置、通信装置、中央控制装置组成；

软件系统包括激光散斑投射系统、彩色图像与激光散斑图像采集系统、图像处理系统、图像匹配系统、模式识别系统、云台控制系统、站点控制系统、通信系统、中央控制系统、路径规划与控制系统；

其中，硬件机械部分中：

彩色图像采集装置：采用高性能ARM处理器控制图像传感器对视野内的彩色图像信息进行采集，得到的信号经过FPGA进行预处理；同时配置光敏元件和无级补光灯，在光线较弱时进行可控强度的补光；

激光散斑投射装置：采用高性能ARM处理器控制红外投影模块投射特定的激光散斑图像；

激光散斑图像采集装置：采用高性能ARM处理器控制红外图像传感器对视野内的激光散斑图像信息进行采集，得到的信号经过FPGA进行预处理，并与红外投影模块投射的激光散斑图像进行比对，通过匹配标定的激光散斑图像与深度信息的关系，得到深度图信息；

云台控制装置：包括水平控制有感无刷电机、俯仰控制有感无刷电机、水平支撑轴承座、三轴加速度计与三轴陀螺仪、电机驱动模块、控制单元；控制单元与集成光学成像系统站点的控制中心进行通信，根据控制中心的需要控制云台水平转动与俯仰转动至特定角度，并反馈实际的转动角度给控制中心；

通信装置：集成光学成像系统站点的站点控制装置与中央控制装置通过Wi-Fi无线组网进行通信，中央控制装置随时添加和删除一个或多个集成光学成像系统站点，实现室内定位系统的高扩展性；中央控制装置提供给个人/家庭用服务机器人Wi-Fi组网通信和蓝牙通信两种通信接口，个人/家庭用服务机器人根据实际需求进行选择；

中央控制装置：包括运行Linux操作系统的微型计算机、Wi-Fi路由模块、蓝牙模块、人机交互模块；微型计算机通过Wi-Fi组网通信技术与各站点进行通信，计算融合各站点定位信息，实现室内定位系统的高覆盖范围。微型计算机与个人/家庭用服务机器人进行通信，根据个人/家庭用服务机器人的需求进行路径规划与控制；人机交互模块提供用户对室内定位系统进行设置的友好接口；

其中，软件系统部分中：

激光散斑投射系统：激光散斑图像生成函数接口；

彩色图像与激光散斑图像采集系统：彩色图像获取接口、激光散斑图像获取接口；

图像处理系统：FFT函数(Fast Fourier Transform，快速傅里叶变换)；

图像匹配系统：SURF函数(Speeded Up Robust Features,加速稳健特征)；

模式识别系统：BP神经网络；

云台控制系统：卡尔曼滤波函数、四元数姿态解算函数；

中央控制系统：贝叶斯最佳融合函数；

路径规划与控制系统：Dijkstra函数、PI控制函数、PD控制函数；

采用上述定位系统进行定位方法，包括以下步骤：

步骤S1：集成光学成像系统站点获取视野范围图像信息及记录当前云台偏转角；

步骤S2：站点计算中心处理获取的彩色图像并进行SURF(Speeded Up RobustFeatures,加速稳健特征)特征匹配识别，获得室内物体特征信息及二维位置信息；

步骤S3：站点计算中心解析获取的激光散斑图像，通过与事先标定得到的激光散斑图像进行匹配，得到室内物体的深度图信息；

步骤S4：站点计算中心结合步骤S1-S3获得的云台偏转角、室内物体特征信息、二维位置信息以及深度图信息，并结合该站点在空间中已知的位置信息，计算得到室内物体在三维空间中的位置信息；

步骤S5：中央控制系统通过无线组网通信获取多个站点信息，通过进行信息融合、修正与拼接，得到整个空间三维位置信息；

步骤S6：中央控制系统与目标机器人之间通过选定的无线通信方式进行通信，中央控制系统根据目标机器人工作需求，发送位置信息与控制命令给目标机器人；

所述步骤S1包括如下步骤：

步骤S11：云台根据需要转至一个特定角度G(α，β)；

步骤S12：彩色摄像机采集当前视野范围内的彩色图像信息；

步骤S13：红外投影机投射具有一定编码信息的激光散斑图像；

步骤S14：红外摄像机采集视野范围内红外投影机投射的图像照射在室内物体上后形成的激光散斑图像。

所述步骤S2包括如下步骤：

步骤S21：使用FFT对图像进行预处理；

步骤S22：使用SURF匹配算法进行特征匹配，具体包括如下步骤：通过计算Hessian矩阵的行列式和矩阵的迹来检测特征点，通过计算欧式距离度量两个特征点描述子的相似度，计算得到待配准图上的特征点到参考图像上所有特征点的欧氏距离集合；

步骤S23：通过对欧氏距离集合进行比较，计算得到最小欧氏距离和次最小欧氏距离，当最小欧氏距离和次最小欧氏距离的比值小于设定的阈值是，认为特征点与对应最小欧氏距离的特征点是匹配的，否则没有点与该特征点相匹配；

步骤S24：由匹配得到的特征点结合匹配库信息，设定特征标识符，并计算和标记二维空间信息。

所述步骤S4包括如下步骤：

步骤S41：事先通过使用标定算法针对彩色摄像机找到最优的摄像机内部参数和畸变向量，得到彩色摄像机矩阵，记为A；

步骤S42：事先通过使用标定算法针对红外摄像机找到最优的摄像机内部参数和畸变向量，得到红外摄像机矩阵，记为B；

步骤S43：彩色摄像机矩阵A和红外摄像机矩阵B分别结合云台角度G(α，β)，得到新的摄像机矩阵C和D；

步骤S44：根据三维情形下，标记的位置与其在二维空间的投影关系P＝A*[R|T]*M，其中：M表示三维空间的一个点；[R|T]表示一个[3|4]矩阵，该矩阵为一个欧氏空间变换；A表示摄像机矩阵或内部参数矩阵；P表示M在屏幕上的投影；因此有M＝[R|T]-1*A-1*P，结合步骤S43得到的矩阵C和D，融合二者及站点在空间中的已知位置信息，计算得到特征室内物体在三维空间中的位置信息；

所述步骤S5包括如下步骤：

步骤S51：中央控制系统通过无线组网通信获取多个站点在步骤S4中得到的室内特征物体在三维空间中的位置信息；

步骤S52：对多个站点获取的室内特征物体在三维空间中的位置信息进行互补滤波并使用贝叶斯最佳融合算法，消除因单个站点覆盖范围带来的局限，得到特征物体在整个三维空间中的位置信息；

步骤S53：根据步骤S52得到的特征物体在整个三维空间中的位置信息；

所述步骤S6包括如下步骤：

步骤S61：中央控制系统与目标机器人之间通过选定的无线通信方式进行通信，获取目标机器人的工作需求；

步骤S62：中央控制系统根据目标机器人当前位置与目标位置，结合空间中物体的分布状态，进行路径规划；

步骤S63：中央控制系统通过无线通信将位置信息与路径信息发送给目标机器人，控制目标机器人的运动。

相较于现有技术，本发明的技术方案具备以下有益效果：

智能软件、硬件、机械三者高度集成化工作，在单个集成光学成像系统站点中，高度集成了彩色图像采集处理及智能补光、激光散斑图像投射与采集处理、二轴自动稳定云台控制与姿态反馈，集成光学成像系统站点能够独立完成局部区域的彩色图像、激光散斑图像的采集，彩色图像采集与智能补光构成反馈环、激光散斑图像投射与采集处理构成反馈环、二轴自动稳定云台控制与姿态反馈构成反馈环。同时，通过彩色图像进行室内物体的特征匹配识别，通过处理激光散斑图像得到深度图像，通过二轴自动稳定云台的姿态反馈得到站点当前的视角信息。不同于专利公开号CN105955258A公开的“基于Kinect传感器信息融合的机器人全局栅格地图构建方法”，在构建地图时需要由机器人搭载Kinect传感器在需要构建地图的空间中运动，根据采集的环境信息建立局部栅格地图，将地图中的栅格分为占用、空闲和未知三种状态，并通过让机器人不断探索未知状态的栅格来逐步完善地图，需要耗费大量的时间，已探知区域发生变化时也不能及时发现；类似地，专利公开号CN105904468A公开的“一种具有自主地图构建和无线充电的多功能巡逻机器人”，也是采用了由机器人搭载Kinect传感器采集环境图像信息并由导航模块进行自主地图的构建，也存在着时效性的问题；本方案中集成光学成像系统通过结合三个反馈环，最大限度地扩大了单个集成光学成像系统站点的室内定位覆盖范围，同时通过使用高性能的ARM+FPGA硬件电路，大大提高了图像处理的能力，缩短了处理时间，提高了整个室内定位系统的实时性。

不同于专利公开号CN106204595A公开的“一种基于双目摄像机的机场场面三维全景监视方法”，要求双目摄像机是固定方式的，不能够任意的运动，改变观测方向及画面变焦缩放，在使用两套双目摄像机时如果它们之间没有重叠则需要每一套摄像机进行预先的配准到统一的机场世界坐标系下，站点的加入与退出都需要预先配准；本方案中多站点集群协同工作，充分利用成熟且廉价的Wi-Fi无线组网通信技术的优势，在保证中央控制系统与各集成光学成像系统站点通信带宽的前提下，通过使用TCP/IP协议，使得集成光学成像系统站点加入与退出室内定位系统时非常的便捷。当有新的集成光学成像系统站点加入时，中央控制系统能够自动为新加入集成光学成像系统站点进行编号，同时通过使用贝叶斯最佳融合算法，对单目标进行多角度融合，并通过各集成光学成像系统站点的反馈信息，自动确定新加入集成光学成像系统站点的布局位置，并自动更新整个室内定位系统的站点布局数据库；当有集成光学成像系统站点因故障等因素退出时，中央控制系统能够识别到通信失败，同时标记故障的集成光学成像系统站点，并将其从整个室内定位系统中去除，并自动更新整个室内定位系统的站点布局数据库，待故障集成光学成像系统站点恢复正常工作时，再将其按上述新加入集成光学成像系统站点加入系统时的方式进行处理。本方案的多站点集群协同工作模式，能够最大程度地提高室内定位系统的定位精度，同时也使系统具有更高的可靠性和更强的自适应能力。

不同于专利公开号CN106227212A公开的“基于栅格地图和动态校准的精度可控室内导航系统及方法”，导航系统终端与被导物体合为一体，导航时需要结合事先铺好的栅格化地图进行路径规划，路径规划原则为走直线、拐直角弯的原则，在沿规划的路径运动时，如遇到障碍物才标记位置并重新规划路径；本发明方案中，中央控制系统在对个人/家庭用服务机器人的通信中提供了Wi-Fi无线通信和蓝牙无线通信两种常用的通信方式，中央控制系统提供的人机交互方式，可以让室内定位系统在添加新的个人/家庭用服务机器人特征信息的同时，自动查询个人/家庭用服务机器人生产厂家提供的相关数据，自动选择通信方式并自动设置通信数据包，同时在对个人/家庭用服务机器人进行路径规划时，结合该个人/家庭用服务机器人的相关运动特性参数，为其进行更为合理的实时路径规划与控制。中央控制系统与个人/家庭用服务机器人相互独立，使得个人/家庭用服务机器人可以精简结构、降低功耗，而中央控制系统也可以同时为对台个人/家庭用服务机器人通过路径规划与控制服务，大大提高了室内定位系统的利用率和适用性。

对比现有的其他专利公开方案，在1.机械装置方面：其他方案只有0维或者1维的运动扫描方式，扫描范围窄，室内定位覆盖率低；而本方案采用二轴自动稳定云台，使集成光学成像系统站点能够灵活稳定的进行大角度(超过180°)的扫描，极大地提高提高了室内定位的覆盖率，最大程度的限制死角区域。2.传感器方面：其他方案只使用单个惯性导航传感器；只使用单个激光扫描雷达；只使用一对双目视觉摄像机；这些都使其室内定位系统在进行定位时容易因为单个传感器的错误数据而导致定位错误，同时单个激光雷达和一对双目视觉摄像机在对整个空间的室内定位方面表现乏力，实时性不高；本系统通过采用红外投影仪投射激光散斑、红外摄像机采集激光散斑图像并计算获取深度图像，结合二轴自动稳定云台，以及多站点集群协同工作的方式，使室内定位系统在对整个空间进行室内定位的覆盖率和实时性得到很大提高，精度上每个站点的误差小于±5cm，多个站点的结合可以使误差在±1cm。3.路径规划方面：其他方案只进行了室内定位，并没有根据个人/家庭用服务机器人的工作需求进行路径规划，使得个人/家庭用服务机器人所走的路径并不是最优的；本系统使用根据个人/家庭用服务机器人的工作需求，采用根据室内物体大小和布局密集程度进行加权的Dijkstra算法进行路径规划。

自动室内定位系统控制方法创新在于：

不同于专利公开号CN103258188A公开的“基于跨平台计算机视觉库的移动目标物体检测跟踪方法”，PC机进行数据分析处理包括了OpenCV视觉算法库、图像平滑滤波处理、图像阈值二值化处理、图像背景实时更新、检测运动目标物体轮廓、分割前景物和背景、跟踪运动目标物体一系列需要大量复杂运算的处理算法；本发明方案使用FFT算法与SURF算法相结合的更为快捷有效的方式，能够很好地降低噪声信号的干扰，提高处理效率和系统的实时响应性。

不同于专利公开号CN105758397A公开的“一种飞行器摄像定位方法”，使用单组两个摄像机进行图像采集，为防止飞行器两侧摄像机采集图像时刻的不同，两台摄像机需要接收同步信号后同时开始自动曝光，同时要求按特定结构布局为飞行器安装激光目标源，操作复杂，适用性不高；本发明方案中，使用多站点集群协同工作方式，使用贝叶斯最佳融合算法对同目标进行多传感器多角度融合，有效减少物体遮挡引起的误判，不需要对目标进行特别改造，同时通过多组数据的融合，过滤误差数据，使室内定位的精确度更高。

不同于专利公开号CN106127117A公开的“基于双目视觉快速高鲁棒性识别、定位的自动跟随行李箱”，下位机的控制算法采用基于速度和航向的综合模糊控制，根据高精度识别定位后的行李箱相对主人的水平距离和两者前后的偏移距离与给定跟随保持相对距离作对比，用来分别对速度和航向进行模糊控制，该模糊控制需要建立一定的模型，并依靠经验确定模糊隶属度；本发明方案中，使用基于参数自校定的PI控制的速度控制算法和基于参数自校定的PD控制的转向控制算法，不需要对个人/家庭用服务机器人建立模型，只需要根据中央控制系统结合个人/家庭用服务机器人的工作需求规划的路径，就可以对个人/家庭用服务机器人沿指定路径运动时的运动速度和转向进行更为有效地控制，使个人/家庭用服务机器人能够更为稳定、可靠、安全地运动。

附图说明

图1是本发明的集成光学成像系统站点组成图；

图2是本发明的站点布局图；

图中：

1.彩色摄像机； 2.红外投影仪； 3.红外摄像机

4.俯仰云台伺服电机； 5.水平云台伺服电机； 6.轴承座；

7.集成光学成像系统站点； 8.扫地机器人； 9.人形机器人；

10.内置式中央控制系统； 11.家具；

图3是本发明的系统结构图；

图4是本发明的定位与控制过程。

具体实施方式

为能进一步了解本发明的发明内容、特点及功效，兹例举以下实施例，并配合附图详细说明如下：

一种全自动高精度室内快速定位方法，包括一定位系统，所述定位系统包括：硬件机械部分和软件系统；

所述硬件机械部分包括彩色图像采集装置、激光散斑投射装置、激光散斑图像采集装置、云台控制装置、站点控制装置、通信装置、中央控制装置组成；

软件系统包括激光散斑投射系统、彩色图像与激光散斑图像采集系统、图像处理系统、图像匹配系统、模式识别系统、云台控制系统、站点控制系统、通信系统、中央控制系统、路径规划与控制系统；

其中，硬件机械部分中：

彩色图像采集装置：采用高性能ARM处理器控制图像传感器对视野内的彩色图像信息进行采集，得到的信号经过FPGA进行预处理；同时配置光敏元件和无级补光灯，在光线较弱时进行可控强度的补光；

激光散斑投射装置：采用高性能ARM处理器控制红外投影模块投射特定的激光散斑图像；

激光散斑图像采集装置：采用高性能ARM处理器控制红外图像传感器对视野内的激光散斑图像信息进行采集，得到的信号经过FPGA进行预处理，并与红外投影模块投射的激光散斑图像进行比对，通过匹配标定的激光散斑图像与深度信息的关系，得到深度图信息；

云台控制装置：包括水平控制有感无刷电机、俯仰控制有感无刷电机、水平支撑轴承座、三轴加速度计与三轴陀螺仪、电机驱动模块、控制单元；控制单元与集成光学成像系统站点的控制中心进行通信，根据控制中心的需要控制云台水平转动与俯仰转动至特定角度，并反馈实际的转动角度给控制中心；

通信装置：集成光学成像系统站点的站点控制装置与中央控制装置通过Wi-Fi无线组网进行通信，中央控制装置随时添加和删除一个或多个集成光学成像系统站点，实现室内定位系统的高扩展性；中央控制装置提供给个人/家庭用服务机器人Wi-Fi组网通信和蓝牙通信两种通信接口，个人/家庭用服务机器人根据实际需求进行选择；

中央控制装置：包括运行Linux操作系统的微型计算机、Wi-Fi路由模块、蓝牙模块、人机交互模块；微型计算机通过Wi-Fi组网通信技术与各站点进行通信，计算融合各站点定位信息，实现室内定位系统的高覆盖范围。微型计算机与个人/家庭用服务机器人进行通信，根据个人/家庭用服务机器人的需求进行路径规划与控制；人机交互模块提供用户对室内定位系统进行设置的友好接口；

其中，软件系统部分中：

激光散斑投射系统：激光散斑图像生成函数接口；

彩色图像与激光散斑图像采集系统：彩色图像获取接口、激光散斑图像获取接口；

图像处理系统：FFT函数；

图像匹配系统：SURF函数；

模式识别系统：BP神经网络；

云台控制系统：卡尔曼滤波函数、四元数姿态解算函数；

中央控制系统：贝叶斯最佳融合函数；

路径规划与控制系统：Dijkstra函数、PI控制函数、PD控制函数；

采用上述定位系统进行定位方法，包括以下步骤：

步骤S1：集成光学成像系统站点获取视野范围图像信息及记录当前云台偏转角；

步骤S2：站点计算中心处理获取的彩色图像并进行SURF特征匹配识别，获得室内物体特征信息及二维位置信息；

步骤S3：站点计算中心解析获取的激光散斑图像，通过与事先标定得到的激光散斑图像进行匹配，得到室内物体的深度图信息；

步骤S4：站点计算中心结合步骤S1-S3获得的云台偏转角、室内物体特征信息、二维位置信息以及深度图信息，并结合该站点在空间中已知的位置信息，计算得到室内物体在三维空间中的位置信息；

步骤S5：中央控制系统通过无线组网通信获取多个站点信息，通过进行信息融合、修正与拼接，得到整个空间三维位置信息；

步骤S6：中央控制系统与目标机器人之间通过选定的无线通信方式进行通信，中央控制系统根据目标机器人工作需求，发送位置信息与控制命令给目标机器人；

所述步骤S1包括如下步骤：

步骤S11：云台根据需要转至一个特定角度G(α，β)；

步骤S12：彩色摄像机采集当前视野范围内的彩色图像信息；

步骤S13：红外投影机投射具有一定编码信息的激光散斑图像；

步骤S14：红外摄像机采集视野范围内红外投影机投射的图像照射在室内物体上后形成的激光散斑图像。

所述步骤S2包括如下步骤：

步骤S21：使用FFT对图像进行预处理；

步骤S22：使用SURF匹配算法进行特征匹配，具体包括如下步骤：通过计算Hessian矩阵的行列式和矩阵的迹来检测特征点，通过计算欧式距离度量两个特征点描述子的相似度，计算得到待配准图上的特征点到参考图像上所有特征点的欧氏距离集合；

步骤S23：通过对欧氏距离集合进行比较，计算得到最小欧氏距离和次最小欧氏距离，当最小欧氏距离和次最小欧氏距离的比值小于设定的阈值是，认为特征点与对应最小欧氏距离的特征点是匹配的，否则没有点与该特征点相匹配；

步骤S24：由匹配得到的特征点结合匹配库信息，设定特征标识符，并计算和标记二维空间信息。

所述步骤S4包括如下步骤：

步骤S41：事先通过使用标定算法针对彩色摄像机找到最优的摄像机内部参数和畸变向量，得到彩色摄像机矩阵，记为A；

步骤S42：事先通过使用标定算法针对红外摄像机找到最优的摄像机内部参数和畸变向量，得到红外摄像机矩阵，记为B；

步骤S43：彩色摄像机矩阵A和红外摄像机矩阵B分别结合云台角度G(α，β)，得到新的摄像机矩阵C和D；

步骤S44：根据三维情形下，标记的位置与其在二维空间的投影关系P＝A*[R|T]*M，其中：M表示三维空间的一个点；[R|T]表示一个[3|4]矩阵，该矩阵为一个欧氏空间变换；A表示摄像机矩阵或内部参数矩阵；P表示M在屏幕上的投影；因此有M＝[R|T]-1*A-1*P，结合步骤S43得到的矩阵C和D，融合二者及站点在空间中的已知位置信息，计算得到特征室内物体在三维空间中的位置信息；

所述步骤S5包括如下步骤：

步骤S51：中央控制系统通过无线组网通信获取多个站点在步骤S4中得到的室内特征物体在三维空间中的位置信息；

步骤S52：对多个站点获取的室内特征物体在三维空间中的位置信息进行互补滤波并使用贝叶斯最佳融合算法，消除因单个站点覆盖范围带来的局限，得到特征物体在整个三维空间中的位置信息；

步骤S53：根据步骤S52得到的特征物体在整个三维空间中的位置信息；

所述步骤S6包括如下步骤：

步骤S61：中央控制系统与目标机器人之间通过选定的无线通信方式进行通信，获取目标机器人的工作需求；

步骤S62：中央控制系统根据目标机器人当前位置与目标位置，结合空间中物体的分布状态，进行路径规划；

步骤S63：中央控制系统通过无线通信将位置信息与路径信息发送给目标机器人，控制目标机器人的运动。

实施例1

参见图1-图4，自动室内定位系统，包括：7.集成光学成像系统站点装置；10.内置式中央控制系统装置。所述内置式中央控制系统装置10与多个集成光学成像系统站点装置7相连。

所述集成光学成像系统站点装置7包括：1.彩色摄像机；2.红外投影仪；3红外摄像机；4俯仰云台伺服电机；5.水平云台伺服电机；6.轴承座；以及集成光学成像系统站点装置7内的电源模块、通信模块、处理模块，所述电源模块与通信模块、处理模块相连。

所述集成光学成像系统站点装置具体工作步骤如下：

对单个集成光学成像系统站点进行自动标定，其中包括：对彩色摄像机的参数标定、对红外摄像机采集的激光散斑与红外投影仪投射的激光散斑对应于深度信息的标定、云台转角与彩色摄像机融合参数的标定、云台转角与红外摄像机融合参数的标定、集成光学成像系统站点覆盖范围的标定。

根据室内空间大小、单个集成光学成像系统站点的覆盖范围以及实际需求，使用多个集成光学成像系统站点进行布局，覆盖整个空间范围，图2为其中一种布局方式。

各集成光学成像系统站点通过Wi-Fi无线组网通信，接收中央控制系统发送的进行异步图像采集命令，开始进行图像采集。

各集成光学成像系统站点通过实时检测环境光照强度，实时修正补光灯的光照强度，提高系统适应性。

各集成光学成像系统站点对自身采集的彩色图像信息进行预处理以及SURF特征匹配识别和模式识别、对自身采集的激光散斑图像信息进行深度图信息变换、对自身采集的云台转角信息进行融合。

各集成光学成像系统站点将自身计算得到的室内物体在三维空间中的位置信息通过无线通信传输给中央控制系统。

所述中央控制系统装置具体工作步骤如下：

中央控制系统通过人机交互进行相关设置并获取目标机器人特征信息。

中央控制系统通过Wi-Fi无线组网与各集成光学成像系统站点进行通信，在有新的集成光学成像系统站点加入组网时，为新加入组网的站点分配标识号。

中央控制系统将目标机器人特征信息下发给各集成光学成像系统站点，更新其特征数据库。

中央控制系统控制各集成光学成像系统站点进行异步图像采集，消除各集成光学成像系统站点间的相互干扰。

中央控制系统通过无线通信获取各集成光学成像系统站点传输过来的各自计算得到的室内物体在三维空间中的位置信息。

中央控制系统通过进行信息融合、修正与拼接，得到整个空间三维位置信息。

中央控制系统与目标机器人之间通过选定的无线通信方式进行通信，中央控制系统根据目标机器人工作需求和当前位置信息，为目标机器人进行路径规划。

中央控制系统通过无线通信将位置信息与路径信息发送给目标机器人，通过PI速度控制算法和PD转向控制算法控制目标机器人的运动。

通过中央控制系统与一个或多个集成光学成像系统站点的协同工作，完成本系统对室内物体的高精度快速定位功能，以及对目标机器人的控制功能。

以上所述，仅为本发明较佳的具体实施方式，但本发明的设计构思并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，利用此构思对本发明进行非实质性的改动，均属于侵犯本发明保护范围的行为。

Claims (1)

1.一种全自动高精度室内快速定位方法，其特征在于：包括一定位系统，所述定位系统包括：硬件机械部分和软件系统；

所述硬件机械部分包括彩色图像采集装置、激光散斑投射装置、激光散斑图像采集装置、云台控制装置、站点控制装置、通信装置、中央控制装置组成；

软件系统包括激光散斑投射系统、彩色图像与激光散斑图像采集系统、图像处理系统、图像匹配系统、模式识别系统、云台控制系统、站点控制系统、通信系统、中央控制系统、路径规划与控制系统；

其中，硬件机械部分中：

彩色图像采集装置：采用高性能ARM处理器控制图像传感器对视野内的彩色图像信息进行采集，得到的信号经过FPGA进行预处理；同时配置光敏元件和无级补光灯，在光线较弱时进行可控强度的补光；

激光散斑投射装置：采用高性能ARM处理器控制红外投影模块投射特定的激光散斑图像；

激光散斑图像采集装置：采用高性能ARM处理器控制红外图像传感器对视野内的激光散斑图像信息进行采集，得到的信号经过FPGA进行预处理，并与红外投影模块投射的激光散斑图像进行比对，通过匹配标定的激光散斑图像与深度信息的关系，得到深度图信息；

云台控制装置：包括水平控制有感无刷电机、俯仰控制有感无刷电机、水平支撑轴承座、三轴加速度计与三轴陀螺仪、电机驱动模块、控制单元；控制单元与集成光学成像系统站点的控制中心进行通信，根据控制中心的需要控制云台水平转动与俯仰转动至特定角度，并反馈实际的转动角度给控制中心；

通信装置：集成光学成像系统站点的站点控制装置与中央控制装置通过Wi-Fi无线组网进行通信，中央控制装置随时添加和删除一个或多个集成光学成像系统站点，实现室内定位系统的高扩展性；中央控制装置提供给个人/家庭用服务机器人Wi-Fi组网通信和蓝牙通信两种通信接口，个人/家庭用服务机器人根据实际需求进行选择；

中央控制装置：包括运行Linux操作系统的微型计算机、Wi-Fi路由模块、蓝牙模块、人机交互模块；微型计算机通过Wi-Fi组网通信技术与各站点进行通信，计算融合各站点定位信息，实现室内定位系统的高覆盖范围。微型计算机与个人/家庭用服务机器人进行通信，根据个人/家庭用服务机器人的需求进行路径规划与控制；人机交互模块提供用户对室内定位系统进行设置的友好接口；

其中，软件系统部分中：

激光散斑投射系统：激光散斑图像生成函数接口；

彩色图像与激光散斑图像采集系统：彩色图像获取接口、激光散斑图像获取接口；

图像处理系统：FFT函数；

图像匹配系统：SURF函数；

模式识别系统：BP神经网络；

云台控制系统：卡尔曼滤波函数、四元数姿态解算函数；

中央控制系统：贝叶斯最佳融合函数；

路径规划与控制系统：Dijkstra函数、PI控制函数、PD控制函数；

采用上述定位系统进行定位方法，包括以下步骤：

步骤S1：集成光学成像系统站点获取视野范围图像信息及记录当前云台偏转角；

步骤S2：站点计算中心处理获取的彩色图像并进行SURF特征匹配识别，获得室内物体特征信息及二维位置信息；

步骤S3：站点计算中心解析获取的激光散斑图像，通过与事先标定得到的激光散斑图像进行匹配，得到室内物体的深度图信息；

步骤S4：站点计算中心结合步骤S1-S3获得的云台偏转角、室内物体特征信息、二维位置信息以及深度图信息，并结合该站点在空间中已知的位置信息，计算得到室内物体在三维空间中的位置信息；

步骤S5：中央控制系统通过无线组网通信获取多个站点信息，通过进行信息融合、修正与拼接，得到整个空间三维位置信息；

步骤S6：中央控制系统与目标机器人之间通过选定的无线通信方式进行通信，中央控制系统根据目标机器人工作需求，发送位置信息与控制命令给目标机器人；

所述步骤S1包括如下步骤：

步骤S11：云台根据需要转至一个特定角度G(α，β)；

步骤S12：彩色摄像机采集当前视野范围内的彩色图像信息；

步骤S13：红外投影机投射具有一定编码信息的激光散斑图像；

步骤S14：红外摄像机采集视野范围内红外投影机投射的图像照射在室内物体上后形成的激光散斑图像。

所述步骤S2包括如下步骤：

步骤S21：使用FFT对图像进行预处理；

步骤S22：使用SURF匹配算法进行特征匹配，具体包括如下步骤：通过计算Hessian矩阵的行列式和矩阵的迹来检测特征点，通过计算欧式距离度量两个特征点描述子的相似度，计算得到待配准图上的特征点到参考图像上所有特征点的欧氏距离集合；

步骤S23：通过对欧氏距离集合进行比较，计算得到最小欧氏距离和次最小欧氏距离，当最小欧氏距离和次最小欧氏距离的比值小于设定的阈值是，认为特征点与对应最小欧氏距离的特征点是匹配的，否则没有点与该特征点相匹配；

步骤S24：由匹配得到的特征点结合匹配库信息，设定特征标识符，并计算和标记二维空间信息。

所述步骤S4包括如下步骤：

步骤S41：事先通过使用标定算法针对彩色摄像机找到最优的摄像机内部参数和畸变向量，得到彩色摄像机矩阵，记为A；

步骤S42：事先通过使用标定算法针对红外摄像机找到最优的摄像机内部参数和畸变向量，得到红外摄像机矩阵，记为B；

步骤S43：彩色摄像机矩阵A和红外摄像机矩阵B分别结合云台角度G(α，β)，得到新的摄像机矩阵C和D；

步骤S44：根据三维情形下，标记的位置与其在二维空间的投影关系P＝A*[R|T]*M，其中：M表示三维空间的一个点；[R|T]表示一个[3|4]矩阵，该矩阵为一个欧氏空间变换；A表示摄像机矩阵或内部参数矩阵；P表示M在屏幕上的投影；因此有M＝[R|T]-1*A-1*P，结合步骤S43得到的矩阵C和D，融合二者及站点在空间中的已知位置信息，计算得到特征室内物体在三维空间中的位置信息；

所述步骤S5包括如下步骤：

步骤S51：中央控制系统通过无线组网通信获取多个站点在步骤S4中得到的室内特征物体在三维空间中的位置信息；

步骤S52：对多个站点获取的室内特征物体在三维空间中的位置信息进行互补滤波并使用贝叶斯最佳融合算法，消除因单个站点覆盖范围带来的局限，得到特征物体在整个三维空间中的位置信息；

步骤S53：根据步骤S52得到的特征物体在整个三维空间中的位置信息；

所述步骤S6包括如下步骤：

步骤S61：中央控制系统与目标机器人之间通过选定的无线通信方式进行通信，获取目标机器人的工作需求；

步骤S62：中央控制系统根据目标机器人当前位置与目标位置，结合空间中物体的分布状态，进行路径规划；

步骤S63：中央控制系统通过无线通信将位置信息与路径信息发送给目标机器人，控制目标机器人的运动。