CN102564416A - 一种镜面清洁机器人进行三维重构与定位的***及方法 - Google Patents

Abstract

一种镜面清洁机器人进行三维环境重构与定位的***，激光测距传感器、包括电机、偏心轮、支架的摇摆支架、绝对值编码器、MEMS惯性传感器、上位机及里程计，其中，包括电机、偏心轮和支架的摇摆支架，支架设置在镜面清洁机器人上，支架的前端设置电机，偏心轮连接所述电机，并传动在所述支架上，用以将电机的旋转运动转化为支架的摇摆运动；激光测距传感器，连动与所述支架，用于实时获得到障碍物之间的激光扫描数据；所述绝对值编码器，设置在激光测距传感器的摇摆轴，用于直接测量激光测距传感器相对于车体的摇摆角度；MEMS惯性传感器，安装所述摇摆支架上，用于测量车体在不平整地面运动时相对于地理水平面的横滚角和俯仰角；上位机：分别连接激光测距传感器、绝对值编码器、MEMS惯性传感器和里程计，用于接收包括激光测距传感器在内发送的传送数据，构建一3D轮廓图，提取三维数据点中的共面数据点，确定出衍架位置。

Description

一种镜面清洁机器人进行三维重构与定位的***及方法

发明领域

本发明一般涉及太阳能热发电领域，具体涉及一种镜面清洁机器人三维重构与定位的***及方法。

背景技术

太阳能热发电，也叫聚焦型太阳能热发电(Concentrating Solar Power，简称CSP)，通过大量反射镜以聚焦的方式将太阳能直射光聚集起来，加热工质，产生高温高压的蒸汽，蒸汽驱动汽轮机发电。

太阳能镜场主要由大量的衍架和安装在上面的平面反光镜组成，为提高单位土地面积的光热利用率，相邻衍架排列紧凑。镜场所在的高原地区全年温差较大，整个镜场地面平整度可能会发生变化。镜面清洁机器人(或称镜面车)用于高海拔地区太阳能光热发电镜场的镜面清洁。

镜面车需要在相邻衍架之间的间隙穿行，并利用安装在车上的清洗机构清洁平面反光镜。为保证镜场设施的安全，需要一种比较精确的定位方案来确定车体和衍架的相对位置关系，进而使镜面车稳定可靠的执行任务。

传统的室内外定位方式主要有：依靠激光测距传感器的相对定位，依靠里程计航位推算定位，通过接收GPS信号的绝对定位，也有采用以上几种方式的组合导航方案。以上方式都有各自独特优势，激光测距传感器应用于室内定位可以获得稳定的环境轮廓，里程计是一种简单有效的定位方式，GPS更是一种通用性极强的解决方案。

然而在本项目的应用中，对以上定位方式提出了新的挑战。GPS民用信号定位精度依然不能满足本应用的要求，并且GPS定位信号存在比较大的跳变，对于高频应用尚力不从心，此外还存在GPS信号丢失的风险。里程计只提供载体前进方向的位移变化量，对于平整度差的路面会存在较大误差，在单独应用时存在空转和滑行的故障风险。2D激光测距传感器在室外的应用一是受限于传感器本身的测距量程，二是室外环境轮廓特征不显著，尤其是本项目的环境是结构复杂的衍架，大大增加了不确定性。

总之，由于本应用中复杂的环境结构，以及平整度差的路面条件，常用的定位方式均不能满足要求，需要一种特殊的定位方案。

发明内容

本发明的第一目的在于提供一种镜面清洁机器人进行三维重构与定位的***，以解决现有技术中在平整度差的路面条件定位不能满足精度要求的技术问题。

本发明的第二目的在于提供一种镜面清洁机器人进行三维重构与定位的方法，以解决现有技术中在平整度差的路面条件定位不能满足精度要求的技术问题。

一种镜面清洁机器人进行三维重构与定位的***，用于安装在所述镜面清洁机器人的车头部分，其进一步包括激光测距传感器、电机、偏心轮、摇摆支架、绝对值编码器、MEMS惯性传感器、上位机及里程计，其中，

偏心轮固定在电机上，并传动在所述摇摆支架上，用于将电机的旋转运动转化为支架的摇摆运动；

所述激光测距传感器，固定连接摇摆支架，用于随摇摆支架的摇摆运动而运动，并获得激光扫描数据；

所述绝对值编码器，设置在激光测距传感器的摇摆轴上，用于直接测量相对于车体的摇摆角度；

MEMS惯性传感器，用于测量车体在不平整地面运动时相对于地理水平面的横滚角和俯仰角；

里程计：安装在镜面清洁机器人上；

上位机：分别连接激光测距传感器、绝对值编码器、MEMS惯性传感器和里程计，用于接收包括激光测距传感器在内发送的传送数据，构建一3D轮廓图，提取三维数据点中的共面数据点，确定出衍架位置。

所述MEMS惯性传感器包括陀螺仪和加速度计，所述上位机进一步包括：

车体姿态测量计算单元，用于根据陀螺仪和加速度计的输出，计算出镜面清洁机器人这个载体的姿态；

车体姿态融合计算单元，用于将镜面清洁机器人这个载体的姿态，再考虑绝对值编码器获取摇摆云台的俯仰角α，最终计算出激光扫描器相对于当地地理水平面的实际姿态的横滚角γ和俯仰角θ；

激光数据姿态投投影修正计算单元，用于计算激光数据点在地理水平面的投影坐标信息；

激光数据平移修正计算单元，用于通用测量时间间隔内里程仪的增量进行修正，并将修正后的数据描绘到一三绘坐标系中，形成一扫描点云图；

定位计算单元，用于在该扫描点云图上定位上预设定的点。

一种镜面清洁机器人进行三维重构与定位的方法，包括：

接收陀螺仪和加速度计的输出，计算出镜面清洁机器人这个载体的姿态；

将镜面清洁机器人这个载体的姿态，再考虑绝对值编码器获取摇摆云台的俯仰角α，最终计算出激光扫描器相对于当地地理水平面的实际姿态的横滚角γ和俯仰角θ；

计算激光数据点在地理水平面的投影坐标信息；

通用测量时间间隔内里程仪的增量进行修正，并将修正后的数据描绘到一三绘坐标系中，形成一扫描点云图；

在该扫描点云图上定位上预设定的点。

与现有技术相比，本发明提供一种镜面清洁机器人进行三维重构与定位的***，本发明提供的***能够在复杂的环境结构以及平整度差的路面条件下，完成重构定位工作。

还有，本发明可以提供利用二维激光雷达进行三维重构与定位操作，成本非常低，而且实现方便。

附图说明

图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惯性传感器包括陀螺仪和加速度计，所述上位机进一步包括：

车体姿态测量计算单元，用于根据陀螺仪和加速度计的输出，计算出镜面清洁机器人这个载体的姿态；

车体姿态融合计算单元，用于将镜面清洁机器人这个载体的姿态，再考虑绝对值编码器获取摇摆云台的俯仰角α，最终计算出激光扫描器相对于当地地理水平面的实际姿态的横滚角γ和俯仰角θ；

激光数据姿态投投影修正计算单元，用于计算激光数据点在地理水平面的投影坐标信息；

激光数据平移修正计算单元，用于通用测量时间间隔内里程仪的增量进行修正，并将修正后的数据描绘到一三绘坐标系中，形成一扫描点云图；

定位计算单元，用于在该扫描点云图上定位上预设定的点。

针对上述的***，一种镜面清洁机器人进行三维重构与定位的方法(请参阅图4)，包括：

首先进行S110：接收陀螺仪和加速度计的输出，计算出镜面清洁机器人这个载体的姿态.(车体姿态测量)

说明：通过惯性传感器进行载体姿态测量这个算法是一个成熟的算法，在这里我们只是重复实现并应用到项目中。

陀螺仪数据进行卡尔曼滤波时间更新；根据加速度计的测量值判断车的运动状态，判断的结果决定是否利用加速度计的量测值进行卡尔曼滤波量测更新；以上完成后根据滤波器状态估计结果来解算姿态。

(目的：输入角速率(陀螺仪测量)和加速度(加速度计测量)，输出载体相对于当地地理水平面的姿态：俯仰角、横滚角。

为了说明清楚，下面分三部分说：1.坐标系和角度量的定义2.由加速度计测量值判断运动状态3.***方程4.姿态解算)

1.定义(请参阅图5)

载体坐标系b：原点o位于载体运动中心，ox_b轴朝向载体右方，oy_b轴朝向载体正前方，oz_b与ox_b、oy_b轴按照右手法则构成直角坐标系o-x_by_bz_b

当地地理坐标系n：原点O与b系原点重合，OX轴朝地理东向，OY轴朝地理北向，OZ与OY、OY轴按照右手法则构成直角坐标系O-XYZ

载体相对于地理坐标系的空间角位置可由俯仰角θ和横滚角γ来表示，定义如下：

俯仰角θ，载体纵轴相对于当地地平面的夹角.

横滚角γ，载体的纵向对称面相对于通过载体纵轴铅垂面所旋转的角度.

2.运动状态判断

将载体的运动状态分为两种：

1)无主动机动。这种状态下通过加速度计测量值可以确定载体相对地理水平面的姿态，因此可以用来作为一种冗余的信息来校正载体姿态；

2)有主动机动。与上面相反，加速度计测量不能用于确定载体姿态。

判断原理：当载体没有线加速度时，载体为无主动机动。此时加速度计测量值 $f^b={\left[\begin{array}{ccc}{f}_x^b& f_y^b& f_z^b\end{array}\right]}^T,$ 其模值应为1个当地重力加速度(9.8m/s²).

设

g为当地重力加速度值。

当

时，认为载体无主动机动；

当

时，认为载体有主动机动；

其中α_a为判断阈值，一般取不超过5％.

3.***方程

输入量：角速率量测 ${\mathrm{\ω}}_{\mathrm{nb}}^b={\left[\begin{array}{ccc}{\mathrm{\ω}}_{\mathrm{nbx}}^b& {\mathrm{\ω}}_{\mathrm{nby}}^b& {\mathrm{\ω}}_{\mathrm{nbz}}^b\end{array}\right]}^T,$ 加速度量测 $f^b={\left[\begin{array}{ccc}{f}_x^b& f_y^b& f_z^b\end{array}\right]}^T.$ 考虑与θ、γ有关的状态向量c₃

$c_3=\left[\begin{array}{c}{c}_{31}\\ c_{32}\\ c_{33}\end{array}\right]=\left[\begin{array}{c}-\sin \mathrm{\γ}\cos \mathrm{\θ}\\ \sin \mathrm{\θ}\\ \cos \mathrm{\γ}\cos \mathrm{\θ}\end{array}\right]$

以及等效陀螺漂移 ${\widetilde{\mathrm{\ϵ}}}^b=\left[\begin{array}{c}{\mathrm{\ϵ}}_x^b\\ {\mathrm{\ϵ}}_y^b\\ {\mathrm{\ϵ}}_z^b\end{array}\right]$

其动力学方程为^{[参考文献]}

$\left[\begin{array}{c}{\stackrel{\·}{c}}_3\\ {\stackrel{\stackrel{\·}{~}}{\mathrm{\ϵ}}}^b\end{array}\right]=\left[\begin{array}{cc}-{\left({\mathrm{\ω}}_{\mathrm{nb}}^b\right)}_{\×}& -{\left(c_3\right)}_{\×}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]\left[\begin{array}{c}{c}_3\\ {\widetilde{\mathrm{\ϵ}}}^b\end{array}\right]+\left[\begin{array}{c}-{\left(c_3\right)}_{\×}\\ 0_{3\×3}\end{array}\right]w_k^b---\left[1\right]$

其中 $w_k^n={\left[\begin{array}{ccc}{w}_{\mathrm{kx}}^b& w_{\mathrm{ky}}^b& w_{\mathrm{kz}}^b\end{array}\right]}^T$ 是***噪声序列，近似满足

(期望)，

(协方差/方差)，Q_k为陀螺仪噪声序列方差阵.

${\left({\mathrm{\ω}}_{\mathrm{nb}}^b\right)}_{\×}=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_{\mathrm{nbz}}^b& {\mathrm{\ω}}_{\mathrm{nby}}^b\\ {\mathrm{\ω}}_{\mathrm{nbz}}^b& 0& -{\mathrm{\ω}}_{\mathrm{nbx}}^b\\ -{\mathrm{\ω}}_{\mathrm{nby}}^b& {\mathrm{\ω}}_{\mathrm{nbx}}^b& 0\end{array}\right]$

${\left(c_3\right)}_{\×}=\left[\begin{array}{ccc}0& -c_{33}& c_{32}\\ c_{33}& 0& -c_{31}\\ -c_{32}& c_{31}& 0\end{array}\right]$

对以上方程进行整理，令

$X=\left[\begin{array}{c}{c}_3\\ {\widetilde{\mathrm{\ϵ}}}^b\end{array}\right],$ $F=\left[\begin{array}{cc}-{\left({\mathrm{\ω}}_{\mathrm{nb}}^b\right)}_{\×}& -{\left(c_3\right)}_{\×}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right],$ $G=\left[\begin{array}{c}-{\left(c_3\right)}_{\×}\\ 0_{3\×3}\end{array}\right]$

则式[1]可写成

$\stackrel{\·}{X}=\mathrm{FX}+{\mathrm{Gw}}^b---\left[2\right]$

当载体的运动状态为无主动机动时，加速度计测量值f^b满足关系式

$\frac{f^b}{g}=\left[\begin{array}{cc}{I}_{3\×3}& 0_{3\×3}\end{array}\right]\left[\begin{array}{c}{c}_3\\ {\widetilde{\mathrm{\ϵ}}}^b\end{array}\right]+v_k^b---\left[3\right]$

其中g为当地重力加速度，一般可取9.8m/s²。 $v_k^b={\left[\begin{array}{ccc}{v}_{\mathrm{kx}}^b& v_{\mathrm{ky}}^b& v_{\mathrm{kz}}^b\end{array}\right]}^T$ 是测量噪声序列，近似满足

(期望)，

(协方差/方差)，R_k为加速度计噪声序列方差阵.

整理一下上式，令

$Z=\frac{f^b}{g},H=\left[\begin{array}{cc}{I}_{3\×3}& 0_{3\×3}\end{array}\right]$

则式[3]可以写成

Z＝HX+v^b.......................................................[4]

对由[2]和[4]组成的***，采用卡尔曼滤波滤波算法进行迭代计算。卡尔曼滤波滤波算法包括时间更新和量测更新两步，在每个迭代周期陀螺仪量测值用于进行时间更新；运动状态判断的结果用于决定是否用加速度计量测值进行量测更新，若载体为无主动运动状态，则进行正常的量测更新，否则，直接利用时间更新的结果作为量测更新后的结果。

4.姿态解算

卡尔曼滤波滤波算法实时估计状态量 $X=\left[\begin{array}{c}{c}_3\\ {\widetilde{\mathrm{\ϵ}}}^b\end{array}\right]$ 的值，通过c₃来解算姿态角

θ＝arcsin(c₃₂)

${\mathrm{\γ}}_{\mathrm{main}}=\arctan (-\frac{c_{31}}{c_{33}})$

$\mathrm{\&gamma;}=\left\{\begin{array}{cc}{\mathrm{\&gamma;}}_{\mathrm{main}},& c_{33}\&GreaterEqual;0\\ {\mathrm{\&gamma;}}_{\mathrm{main}}-\mathrm{\&pi;},& c_{33}<0,{\mathrm{\&gamma;}}_{\mathrm{main}}>0\\ {\mathrm{\&gamma;}}_{\mathrm{main}}+\mathrm{\&pi;},& c_{33}<0,{\mathrm{\&gamma;}}_{\mathrm{main}}<0\end{array}\right.$

上部分可以参考文献[1]Henrik Rehbinder，Xiaoming Hu.Drift-free attitudeestimation for accelerated rigid bodies.Automatic，Volume 40，Issue 4，April 2004，Pages 653-659

接着进行S120：将镜面清洁机器人这个载体的姿态，再考虑绝对值编码器获取摇摆云台的俯仰角α，最终计算出激光扫描器相对于当地地理水平面的实际姿态的横滚角γ和俯仰角θ(姿态融合)。

激光测距传感器测量的每一帧数据时的姿态由两部分组成：车体的姿态和固联于车体的摇摆台姿态，通过前面的算法分别获取了这两部分的姿态参数，分别是载体相对于当地地理水平面的姿态参数：俯仰角θ和横滚角γ，和由绝对值编码器获取摇摆云台的俯仰角α，此时激光扫描器相对于当地地理水平面的实际姿态的计算公式为

横滚角γ，与载体横滚角一致，因为摇摆云台只在俯仰方向有摇摆运动，不影响横滚角

俯仰角β：sinβ＝sinα·cosγ+θ，β＝arcsin(sinα·cosγ+θ)(字母定义见图5及上述说明)

再进行S130：计算激光数据点在地理水平面的投影坐标信息(即激光数据姿态投影修正)。

从导航系即当地地理系n至载体坐标系b的变换通过绕三个轴的旋转来实现

三次转动的结果用方向余弦矩阵表示，记导航系至载体系的方向余弦矩阵

有

$C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\&gamma;}& \sin \mathrm{\&gamma;}\sin \mathrm{\&beta;}& -\sin \mathrm{\&gamma;}\cos \mathrm{\&beta;}\\ 0& \cos \mathrm{\&beta;}& \sin \mathrm{\&beta;}\\ \sin \mathrm{\&gamma;}& -\cos \mathrm{\&gamma;}\sin \mathrm{\&beta;}& \cos \mathrm{\&gamma;}\cos \mathrm{\&beta;}\end{array}\right]$

对于激光数据中的每一个角度-距离对(a_i dist_i)，首先用下面公式转化到直角坐标系坐标

$\left\{\begin{array}{c}{x}_i^b={\mathrm{dist}}_i\&CenterDot;\sin a_1\\ y_i^b={\mathrm{dist}}_i\&CenterDot;\cos a_i\end{array}\right.$

增加z分量0值后乘以方向余弦矩阵的逆即得到激光数据点在地理水平面的投影坐标

$\left[\begin{array}{c}{x}_j^n\\ y_j^n\\ z_j^n\end{array}\right]={\left(C_n^b\right)}^{-1}\left[\begin{array}{c}{x}_i^b\\ y_i^b\\ 0\end{array}\right]$

随后进行S140：通用测量时间间隔内里程仪的增量进行修正，并将修正后的数据描绘到一三绘坐标系中，形成一扫描点云图(激光数据平移修正)

在一个测量周期内，记录逐帧激光数据的同时车体有可能还发生位移，这就造成记录的激光数据不在同一个位置，需要通过测量时间间隔内里程仪的增量进行平移修正。

设两帧激光数据对应的量测姿态分别为(β₁ γ₁)和(β₂ γ₂)，两帧之间里程计的增量为(δx δy δa).

做一个简化处理，将里程增量按照前一帧的激光姿态投影到地理水平面：

$\left[\begin{array}{c}\mathrm{\&delta;}{x}^n\\ \mathrm{\&delta;}{y}^n\\ *\end{array}\right]={\left(C_n^{b1}\right)}^{-1}\left[\begin{array}{c}\mathrm{\&delta;x}\\ \mathrm{\&delta;y}\\ 0\end{array}\right]$

然后将后一帧激光数据根据参数(δxⁿ δyⁿ δa)进行平移和旋转，按如下公式计算

$\left[\begin{array}{c}{x}_i^{n0}\\ y_i^{n0}\end{array}\right]=\left[\begin{array}{cc}\cos \mathrm{\&delta;a}& \sin \mathrm{\&delta;a}\\ -\sin \mathrm{\&delta;a}& \cos \mathrm{\&delta;a}\end{array}\right]\left[\begin{array}{c}{x}_i^n\\ y_i^n\end{array}\right]+\left[\begin{array}{c}-\mathrm{\&delta;}{x}^n\\ -\mathrm{\&delta;}{y}^n\end{array}\right]$

假设一个测量周期内共获得N帧激光数据，约定根据上述两个步骤的处理，将各帧激光数据对齐到：原点位于第1帧激光数据的原点，且投影到当地水平面。

经过以上算法步骤的处理，在一个测量周期内的各帧激光数据在空间和时间上对准，将这些修正后的数据描绘到同一个三维坐标系中，这样一幅扫描点云图就可以完全描述环境的3D轮廓。

最后进行S150：在该扫描点云图上定位上预设定的点。

在获得环境3D轮廓的基础上，考虑到衍架虽然结构复杂，激光扫描的轮廓具有很大的不确定性，但是衍架结构在3D空间上还是具有很多平面的特征，在3D轮廓云图上表现为一些共面的数据点，采用霍夫变换提取三维数据点中的共面数据点，对结果进行聚类等操作后确定出衍架位置，通过这一显著特征可以确定衍架和激光测距传感器的相对位置关系。

这部分包括：1.在3D轮廓云图上提取特征平面2.确定载体相对两侧衍架的位置和载体前向与衍架方向的夹角。

1.提取特征平面

空间平面方程：

z＝a_xx+a_yy+d

对于构建出来的3D轮廓云图，对其中的每个数据点[x_i y_i z_i]^T进行霍夫变换，每一个数据点对应参数空间中的一个空间平面。3D云图中有N个点，在参数空间中就有N个平面，这些平面相交与一个点(不会正好相交于一个点，但相对集中在一个较小的区域)，这个点的霍夫反变换就对应3D云图中共面数据点组成的平面。

上部分算法可参考文献：

[1]http://en.wikipedia.org/wiki/Hough_transform

[2]http://homepages.inf.ed.ac.uk/rbf/HIPR2/hough.htm

[3]Vosselman，G.，Dijkman，S.3D Building Model Reconstruction fromPoint Clouds and Ground Plans. International Archives of thePhotogrammetry，Remote Sensing and Spatial Information Sciences，vol34，part 3/W4，October 22 24，2001，Annapolis，MA，USA，pp.37-44.

2.相对位置定位

设从前一步骤得到可以表示一侧衍架平面的方程：

z＝a_xx+a_yy+d

激光中心到平面距离根据下面式子计算：

$\mathrm{dis}\tan \mathrm{ce}=\frac{\left|d\right|}{\sqrt{a_x^2+a_y^2+1}}$

载体前向和衍架平面之间的夹角按下面式子计算：

以上公开的仅为本申请的几个具体实施例，但本申请并非局限于此，任何本领域的技术人员能思之的变化，都应落在本申请的保护范围内。

Claims (10)

1.一种镜面清洁机器人进行三维重构与定位的***，其特征在于，用于安装在所述镜面清洁机器人的车头部分，其进一步包括激光测距传感器、电机、偏心轮、摇摆支架、绝对值编码器、MEMS惯性传感器、上位机及里程计，其中，

偏心轮固定在电机上，并传动在所述摇摆支架上，用于将电机的旋转运动转化为支架的摇摆运动；

所述激光测距传感器，固定连接摇摆支架，用于随摇摆支架的摇摆运动而运动，并获得激光扫描数据；

所述绝对值编码器，设置在激光测距传感器的摇摆轴上，用于直接测量相对于车体的摇摆角度；

MEMS惯性传感器，用于测量车体在不平整地面运动时相对于地理水平面的横滚角和俯仰角；

里程计：安装在镜面清洁机器人上；

上位机：分别连接激光测距传感器、绝对值编码器、MEMS惯性传感器和里程计，用于接收包括激光测距传感器在内发送的传送数据，构建一3D轮廓图，提取三维数据点中的共面数据点，确定出衍架位置。

2.如权利要求1所述的***，其特征在于，所述激光测距传感器沿着摇摆支点摇摆，所述摇摆支点固定在所述镜面清洁机器人上。

3.如权利要求1或2所述的***，其特征在于，所述摇摆支架至少包括上支架和下支架，所述上支架和下支架构成一中空的长方形状，所述偏心轮设置在所述中空的长方形内，所述偏心轮落在所述下支架上，下支架的左竖支条与所述激光测距传感器固定连接，所述下支架的下横支条与所述左竖支条连接，所述偏心轮在下支架的半包围圈内运动，带动下支架的摇摆运动。

4.如权利要求3所述的***，其特征在于，所述偏心轮的直径等于所述下支架的左竖支条的高。

5.如权利要求3所述的***，其特征在于，还包括一支撑条，所述支撑条的一端连接所述上支架的右竖支条的下端，另一端活动连接激光测距传感器上。

6.如权利要求1所述的***，其特征在于，所述激光测距传感器设置在摇摆支架上或电机的一端。

7.如权利要求2所述的***，其特征在于，所述激光测距传感器采用二维激光测距传感器。

8.如权利要求1所述的***，其特征在于，

所述MEMS惯性传感器包括陀螺仪和加速度计，所述上位机进一步包括：

车体姿态测量计算单元，用于根据陀螺仪和加速度计的输出，计算出镜面清洁机器人这个载体的姿态；

车体姿态融合计算单元，用于将镜面清洁机器人这个载体的姿态，再考虑绝对值编码器获取摇摆云台的俯仰角α，最终计算出激光扫描器相对于当地地理水平面的实际姿态的横滚角γ和俯仰角θ；

激光数据姿态投投影修正计算单元，用于计算激光数据点在地理水平面的投影坐标信息；

激光数据平移修正计算单元，用于通用测量时间间隔内里程仪的增量进行修正，并将修正后的数据描绘到一三绘坐标系中，形成一扫描点云图；

定位计算单元，用于在该扫描点云图上定位上预设定的点。

9.一种镜面清洁机器人进行三维重构与定位的方法，其特征在于，包括：

接收陀螺仪和加速度计的输出，计算出镜面清洁机器人这个载体的姿态；

将镜面清洁机器人这个载体的姿态，再考虑绝对值编码器获取摇摆云台的俯仰角α，最终计算出激光扫描器相对于当地地理水平面的实际姿态的横滚角γ和俯仰角θ；

计算激光数据点在地理水平面的投影坐标信息；

通用测量时间间隔内里程仪的增量进行修正，并将修正后的数据描绘到一三绘坐标系中，形成一扫描点云图；

在该扫描点云图上定位上预设定的点。

10.如权利要求9所述的方法，其特征在于，在该扫描点云图上定位上预设定的点进一步包括：

在3D轮廓云图上提取特征平面；

确定载体相对两侧衍架的位置和载体前向与衍架方向的夹角。

Images