专利名称：一种镜面清洁机器人进行三维重构与定位的系统及方法
技术领域：
本发明一般涉及太阳能热发电领域，具体涉及一种镜面清洁机器人三维重构与定位的系统及方法。
背景技术：
太阳能热发电，也叫聚焦型太阳能热发电(Concentrating Solar Power，简称 CSP)，通过大量反射镜以聚焦的方式将太阳能直射光聚集起来，加热工质，产生高温高压的蒸汽，蒸汽驱动汽轮机发电。太阳能镜场主要由大量的衍架和安装在上面的平面反光镜组成，为提高单位土地面积的光热利用率，相邻衍架排列紧凑。镜场所在的高原地区全年温差较大，整个镜场地面平整度可能会发生变化。镜面清洁机器人(或称镜面车)用于高海拔地区太阳能光热发电镜场的镜面清洁。镜面车需要在相邻衍架之间的间隙穿行，并利用安装在车上的清洗机构清洁平面反光镜。为保证镜场设施的安全，需要一种比较精确的定位方案来确定车体和衍架的相对位置关系，进而使镜面车稳定可靠的执行任务。传统的室内外定位方式主要有依靠激光测距传感器的相对定位，依靠里程计航位推算定位，通过接收GPS信号的绝对定位，也有采用以上几种方式的组合导航方案。以上方式都有各自独特优势，激光测距传感器应用于室内定位可以获得稳定的环境轮廓，里程计是一种简单有效的定位方式，GPS更是一种通用性极强的解决方案。然而在本项目的应用中，对以上定位方式提出了新的挑战。GPS民用信号定位精度依然不能满足本应用的要求，并且GPS定位信号存在比较大的跳变，对于高频应用尚力不从心，此外还存在GPS信号丢失的风险。里程计只提供载体前进方向的位移变化量，对于平整度差的路面会存在较大误差，在单独应用时存在空转和滑行的故障风险。2D激光测距传感器在室外的应用一是受限于传感器本身的测距量程，二是室外环境轮廓特征不显著，尤其是本项目的环境是结构复杂的衍架，大大增加了不确定性。总之，由于本应用中复杂的环境结构，以及平整度差的路面条件，常用的定位方式均不能满足要求，需要一种特殊的定位方案。
发明内容
本发明的第一目的在于提供一种镜面清洁机器人进行三维重构与定位的系统，以解决现有技术中在平整度差的路面条件定位不能满足精度要求的技术问题。本发明的第二目的在于提供一种镜面清洁机器人进行三维重构与定位的方法，以解决现有技术中在平整度差的路面条件定位不能满足精度要求的技术问题。一种镜面清洁机器人进行三维重构与定位的系统，用于安装在所述镜面清洁机器人的车头部分，其进一步包括激光测距传感器、电机、偏心轮、摇摆支架、绝对值编码器、 MEMS惯性传感器、上位机及里程计，其中，
偏心轮固定在电机上，并传动在所述摇摆支架上，用于将电机的旋转运动转化为支架的摇摆运动；所述激光测距传感器，固定连接摇摆支架，用于随摇摆支架的摇摆运动而运动，并获得激光扫描数据；所述绝对值编码器，设置在激光测距传感器的摇摆轴上，用于直接测量相对于车体的摇摆角度；MEMS惯性传感器，用于测量车体在不平整地面运动时相对于地理水平面的横滚角和俯仰角；里程计安装在镜面清洁机器人上；上位机分别连接激光测距传感器、绝对值编码器、MEMS惯性传感器和里程计，用于接收包括激光测距传感器在内发送的传送数据，构建一 3D轮廓图，提取三维数据点中的共面数据点，确定出衍架位置。所述MEMS惯性传感器包括陀螺仪和加速度计，所述上位机进一步包括车体姿态测量计算单元，用于根据陀螺仪和加速度计的输出，计算出镜面清洁机器人这个载体的姿态；车体姿态融合计算单元，用于将镜面清洁机器人这个载体的姿态，再考虑绝对值编码器获取摇摆云台的俯仰角α，最终计算出激光扫描器相对于当地地理水平面的实际姿态的横滚角Y和俯仰角θ ；激光数据姿态投投影修正计算单元，用于计算激光数据点在地理水平面的投影坐标信息；激光数据平移修正计算单元，用于通用测量时间间隔内里程仪的增量进行修正， 并将修正后的数据描绘到一三绘坐标系中，形成一扫描点云图；定位计算单元，用于在该扫描点云图上定位上预设定的点。一种镜面清洁机器人进行三维重构与定位的方法，包括接收陀螺仪和加速度计的输出，计算出镜面清洁机器人这个载体的姿态；将镜面清洁机器人这个载体的姿态，再考虑绝对值编码器获取摇摆云台的俯仰角 α，最终计算出激光扫描器相对于当地地理水平面的实际姿态的横滚角Y和俯仰角θ ；计算激光数据点在地理水平面的投影坐标信息；通用测量时间间隔内里程仪的增量进行修正，并将修正后的数据描绘到一三绘坐标系中，形成一扫描点云图；在该扫描点云图上定位上预设定的点。与现有技术相比，本发明提供一种镜面清洁机器人进行三维重构与定位的系统， 本发明提供的系统能够在复杂的环境结构以及平整度差的路面条件下，完成重构定位工作。还有，本发明可以提供利用二维激光雷达进行三维重构与定位操作，成本非常低， 而且实现方便。
图1为一种镜面清洁机器人进行三维重构与定位的部分系统结构正视示意图2为一种镜面清洁机器人进行三维重构与定位的部分系统结构后视示意图；图3为一种镜面清洁机器人进行三维重构与定位的部分系统结构侧视图；图4为一种镜面清洁机器人进行三维重构与定位的流程图；图5为一现有坐标定义图。
具体实施例方式以下结合附图，具体说明本发明。请参阅图1、图2、图3，一种镜面清洁机器人进行三维重构与定位的系统。用于安装在所述镜面清洁机器人的车头部分，其进一步包括激光测距传感器15、电机11、偏心轮 13、摇摆支架、绝对值编码器、MEMS惯性传感器、上位机及里程计，其中，偏心轮13固定在电机11上，并传动在摇摆支架上，用于将电机的旋转运动转化为支架的摇摆运动。激光测距传感器15，固定连接摇摆支架，用于随摇摆支架的摇摆运动而运动，并获得激光扫描数据；所述绝对值编码器，设置在激光测距传感器15的摇摆轴上，用于直接相对于车体摇摆角度；MEMS惯性传感器，用于测量车体在不平整地面运动时相对于地理水平面的横滚角和俯仰角；里程计安装在镜面清洁机器人上；上位机分别连接激光测距传感器15、绝对值编码器、MEMS惯性传感器和里程计， 用于接收包括激光测距传感器在内发送的传送数据，构建一 3D轮廓图，提取三维数据点中的共面数据点，确定出衍架位置。激光测距传感器沿着摇摆支点17摇摆，摇摆支点17可以固定在镜面清洁机器人上。摇摆支架至少包括上支架141和下支架142，上支架141和下支架142构成一中空的长方形状，偏心轮13设置在所述中空的长方形内，偏心轮13落在所述下支架142上，下支架的左竖支条可以通过支点18与激光测距传感器15固定连接，下支架142的下横支条与所竖支条连接，偏心轮13在下支架的半包围圈内运动，带动下支架142的摇摆运动。本实例中还包括一支撑条，支撑条的一端连接上支架的右竖支条的下端，另一端活动连接激光测距传感器上。这样，偏心轮13在下支架的半包围圈内运动时，至少能被支撑条托住。电机旋转带动偏心轮，使支架产生往复的摇摆运动，该摇摆运动的相对角度通过绝对值编码器测得。激光传感器固连于支架，在一个摇摆周期内定时采样一定数目的二维扫描帧(支架摇摆的幅度大约为20°，这个幅度是通过偏心轮安装偏心量可调的，在一个摇摆周期内每隔2°进行一次激光扫描数据的采集，即一个摇摆来回进行20帧激光扫描)。在本实例中所述偏心轮的直径等于所述下支架的左竖支条的高。所述激光测距传感器设置在摇摆支架上或电机的一端。激光测距传感器可以采用二维激光测距传感器，以减少成本，也可以直接采用三维激光测距传感器。在本实例中，所述MEMS惯性传感器包括陀螺仪和加速度计，所述上位机进一步包括
车体姿态测量计算单元，用于根据陀螺仪和加速度计的输出，计算出镜面清洁机器人这个载体的姿态；车体姿态融合计算单元，用于将镜面清洁机器人这个载体的姿态，再考虑绝对值编码器获取摇摆云台的俯仰角α，最终计算出激光扫描器相对于当地地理水平面的实际姿态的横滚角Y和俯仰角θ ；激光数据姿态投投影修正计算单元，用于计算激光数据点在地理水平面的投影坐标信息；激光数据平移修正计算单元，用于通用测量时间间隔内里程仪的增量进行修正， 并将修正后的数据描绘到一三绘坐标系中，形成一扫描点云图；定位计算单元，用于在该扫描点云图上定位上预设定的点。针对上述的系统，一种镜面清洁机器人进行三维重构与定位的方法(请参阅图 4)，包括首先进行SllO 接收陀螺仪和加速度计的输出，计算出镜面清洁机器人这个载体的姿态.(车体姿态测量)说明通过惯性传感器进行载体姿态测量这个算法是一个成熟的算法，在这里我们只是重复实现并应用到项目中。陀螺仪数据进行卡尔曼滤波时间更新；根据加速度计的测量值判断车的运动状态，判断的结果决定是否利用加速度计的量测值进行卡尔曼滤波量测更新；以上完成后根据滤波器状态估计结果来解算姿态。(目的输入角速率(陀螺仪测量)和加速度(加速度计测量)，输出载体相对于当地地理水平面的姿态俯仰角、横滚角。为了说明清楚，下面分三部分说1.坐标系和角度量的定义2.由加速度计测量值判断运动状态3.系统方程4.姿态解算)1.定义(请参阅图5)载体坐标系b 原点ο位于载体运动中心，OXb轴朝向载体右方，oyb轴朝向载体正前方，OZb与ο、、oyb轴按照右手法则构成直角坐标系0-xbyb、 当地地理坐标系η 原点0与b系原点重合，OX轴朝地理东向，OY轴朝地理北向， OZ与0Y、OY轴按照右手法则构成直角坐标系O-XYZ载体相对于地理坐标系的空间角位置可由俯仰角θ和横滚角Υ来表示，定义如下俯仰角θ，载体纵轴相对于当地地平面的夹角.横滚角Υ，载体的纵向对称面相对于通过载体纵轴铅垂面所旋转的角度.2.运动状态判断将载体的运动状态分为两种1)无主动机动。这种状态下通过加速度计测量值可以确定载体相对地理水平面的姿态，因此可以用来作为一种冗余的信息来校正载体姿态；幻有主动机动。与上面相反，加速度计测量不能用于确定载体姿态。判断原理当载体没有线加速度时，载体为无主动机动。此时加速度计测量值
fb = [fx /； f^J ,其模值应为1个当地重力加速度(9. 8m/s2) ·
设f；； =-,g为当地重力加速度值。当时，认为载体无主动机动；当时，认为载体有主动机动；其中α a为判断阈值，一般取不超过5% .3.系统方程输入量角速率量测<=CBbnby ω, J ,加速度量测f6 = [fx /； //J .考
虑与θ、Y有关的状态向量C3 C31 -sin χ cos 汐
以及等效陀螺漂移沪=Sby
其动力学方程为
权利要求
1.一种镜面清洁机器人进行三维重构与定位的系统，其特征在于，用于安装在所述镜面清洁机器人的车头部分，其进一步包括激光测距传感器、电机、偏心轮、摇摆支架、绝对值编码器、MEMS惯性传感器、上位机及里程计，其中，偏心轮固定在电机上，并传动在所述摇摆支架上，用于将电机的旋转运动转化为支架的摇摆运动；所述激光测距传感器，固定连接摇摆支架，用于随摇摆支架的摇摆运动而运动，并获得激光扫描数据；所述绝对值编码器，设置在激光测距传感器的摇摆轴上，用于直接测量相对于车体的摇摆角度；MEMS惯性传感器，用于测量车体在不平整地面运动时相对于地理水平面的横滚角和俯仰角；里程计安装在镜面清洁机器人上；上位机分别连接激光测距传感器、绝对值编码器、MEMS惯性传感器和里程计，用于接收包括激光测距传感器在内发送的传送数据，构建一 3D轮廓图，提取三维数据点中的共面数据点，确定出衍架位置。
2.如权利要求1所述的系统，其特征在于，所述激光测距传感器沿着摇摆支点摇摆，所述摇摆支点固定在所述镜面清洁机器人上。
3.如权利要求1或2所述的系统，其特征在于，所述摇摆支架至少包括上支架和下支架，所述上支架和下支架构成一中空的长方形状，所述偏心轮设置在所述中空的长方形内， 所述偏心轮落在所述下支架上，下支架的左竖支条与所述激光测距传感器固定连接，所述下支架的下横支条与所述左竖支条连接，所述偏心轮在下支架的半包围圈内运动，带动下支架的摇摆运动。
4.如权利要求3所述的系统，其特征在于，所述偏心轮的直径等于所述下支架的左竖支条的高。
5.如权利要求3所述的系统，其特征在于，还包括一支撑条，所述支撑条的一端连接所述上支架的右竖支条的下端，另一端活动连接激光测距传感器上。
6.如权利要求1所述的系统，其特征在于，所述激光测距传感器设置在摇摆支架上或电机的一端。
7.如权利要求2所述的系统，其特征在于，所述激光测距传感器采用二维激光测距传感器。
8.如权利要求1所述的系统，其特征在于，所述MEMS惯性传感器包括陀螺仪和加速度计，所述上位机进一步包括车体姿态测量计算单元，用于根据陀螺仪和加速度计的输出，计算出镜面清洁机器人这个载体的姿态；车体姿态融合计算单元，用于将镜面清洁机器人这个载体的姿态，再考虑绝对值编码器获取摇摆云台的俯仰角α，最终计算出激光扫描器相对于当地地理水平面的实际姿态的横滚角Y和俯仰角θ ；激光数据姿态投投影修正计算单元，用于计算激光数据点在地理水平面的投影坐标信息；激光数据平移修正计算单元，用于通用测量时间间隔内里程仪的增量进行修正，并将修正后的数据描绘到一三绘坐标系中，形成一扫描点云图； 定位计算单元，用于在该扫描点云图上定位上预设定的点。
9.一种镜面清洁机器人进行三维重构与定位的方法，其特征在于，包括 接收陀螺仪和加速度计的输出，计算出镜面清洁机器人这个载体的姿态；将镜面清洁机器人这个载体的姿态，再考虑绝对值编码器获取摇摆云台的俯仰角α， 最终计算出激光扫描器相对于当地地理水平面的实际姿态的横滚角Y和俯仰角θ ； 计算激光数据点在地理水平面的投影坐标信息；通用测量时间间隔内里程仪的增量进行修正，并将修正后的数据描绘到一三绘坐标系中，形成一扫描点云图；在该扫描点云图上定位上预设定的点。
10.如权利要求9所述的方法，其特征在于，在该扫描点云图上定位上预设定的点进一步包括在3D轮廓云图上提取特征平面；确定载体相对两侧衍架的位置和载体前向与衍架方向的夹角。
全文摘要
一种镜面清洁机器人进行三维环境重构与定位的系统，激光测距传感器、包括电机、偏心轮、支架的摇摆支架、绝对值编码器、MEMS惯性传感器、上位机及里程计，其中，包括电机、偏心轮和支架的摇摆支架，支架设置在镜面清洁机器人上，支架的前端设置电机，偏心轮连接所述电机，并传动在所述支架上，用以将电机的旋转运动转化为支架的摇摆运动；激光测距传感器，连动与所述支架，用于实时获得到障碍物之间的激光扫描数据；所述绝对值编码器，设置在激光测距传感器的摇摆轴，用于直接测量激光测距传感器相对于车体的摇摆角度；MEMS惯性传感器，安装所述摇摆支架上，用于测量车体在不平整地面运动时相对于地理水平面的横滚角和俯仰角；上位机分别连接激光测距传感器、绝对值编码器、MEMS惯性传感器和里程计，用于接收包括激光测距传感器在内发送的传送数据，构建一3D轮廓图，提取三维数据点中的共面数据点，确定出衍架位置。
文档编号G01C21/00GK102564416SQ201110455468
公开日2012年7月11日 申请日期2011年12月30日 优先权日2011年12月30日
发明者吴剑, 沈慧, 沈继中, 陶熠昆 申请人:浙江国自机器人技术有限公司