CN104240297A - 一种救援机器人三维环境地图实时构建方法 - Google Patents

Abstract

一种救援机器人三维环境地图构建方法，该方法采用高效的加速鲁棒特性SURF算法进行特征点的识别和匹配；其次提出了区间分析法，生成粗匹配的位姿变换矩阵，该方法有较好的鲁棒性，在给定一组约束条件下可以包括所有可能的值；为了克服ICP算法可能使得迭代过程无法收敛到全局最优解的缺点，提出了一种改进的最近点选取方法：对点云A中的测点，求出另一点云B中与其最近的3个点，组成一个平面，该平面的经过测点的法向量与该平面的交点作为最近点,较好地提高了算法的精度。另外，采用惯性测量单元IMU和里程计数据与位姿变换矩阵融合的方法，使得算法有更好的鲁棒性，在特征点匹配不成功或者环境动态变化的情况下也能做到准确的定位。

Description

一种救援机器人三维环境地图实时构建方法

技术领域

本发明涉及基于救援机器人领域的三维重构方法，为三维地图构建提供解决方案，为灾难环境中的救援提供可视化的依据。

背景技术

在机器人研究领域，对未知环境的探索与建立一直以来是一个研究热点。三维环境地图的构建为机器人在未知环境下的探索和搜救工作提供了直观的依据。

文献“RGB-D mapping:Using depth camera for dense3D modeling of indoor environnments,12^thISER.2010,20:p22-25”公开了一种基于RGB-D传感器的三维重建方法,该方法也是目前使用最普遍的算法。该方法采用SIFT算法进行特征点的提取和匹配，再经过RANSAC算法剔除误匹配的点对，得到粗匹配的姿态转移矩阵，最后由ICP算法完成点云的拼接以优化初始位置。该算法采用的SIFT算法效率不高，时间复杂度大；ICP算法要求两个配对点云之间有包含关系，否则影响ICP算法的收敛结果，另外ICP算法的迭代精度与两个数据点集的初始相对位置密切相关，要求两组点云之间具有比较相近的初始位置；该算法适合在静态的环境中使用，在动态变化的环境中特征点的匹配效果较差。针对该三维重构算法存在的问题，本专利提出了一些自己的解决方法。

发明内容

鉴于背景技术存在的不足，本发明要解决的问题是提供一种能够提高精度和鲁棒性的救援机器人三维环境地图实时构建方法。

一种救援机器人三维环境地图实时构建方法，其特征在于包括以下步骤：

步骤一:从救援机器人的体感传感器上获取RGB图像，采用加速鲁棒特性算法(SURF)算法提取前后两个周期的RGB图像的特征点并提取相互匹配的特征点对p(X,Y,Z)和p'(X',Y',Z')，

步骤二：利用区间分析法得到位姿变换矩阵从而初步估计机器人的位姿,

将位姿变换矩阵定义为T＝(Rt),R是旋转变换矩阵且是正交矩阵，t是平移矢量，对任一对匹配点，满足

$\left(\begin{array}{c}{X}_i\\ Y_i\\ Z_i\end{array}\right)-T\left(\begin{array}{c}{X}_j\\ Y_j\\ Z_j\end{array}\right)=0---\left(1\right)$

其中X_i，Y_i，Z_i是前一周期RGB图像中的第i个特征点所在空间中的三维坐标，X_j，Y_j，Z_j是与特征点(X_i，Y_i，Z_i)相匹配的当前周期RGB图像中的第j个特征点的三维坐标，

定义旋转变换矩阵为：

其中φ，θ，是遵守左手法则的三个欧拉角，φ，θ，分别对应横滚角roll,俯仰角pitch,偏航角yaw,

定义平移矢量 $t=\left(\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right)---\left(3\right)$

其中，t_x,t_y,t_z分别对应三维空间中x轴、y轴、z轴方向的位移，

把(2)、(3)代入(1)得到三个方程：

从步骤一得到的相互匹配的特征点对中选取3个不在一条直线上的相互匹配的特征点对即：

p₁(X₁,Y₁,Z₁)与p₁'(X'₁,Y₁',Z'₁)、p₂(X₂,Y₂,Z₂)与p'₂(X'₂,Y₂',Z'₂)及p₃(X₃,Y₃,Z₃)与p'₃(X'₃,Y₃',Z'₃)，其中X₁,Y₁,Z₁是前一周期RGB图像中的特征点p₁的三维坐标值，X₂,Y₂,Z₂是前一周期RGB图像中的特征点p₂的三维坐标值，X₃,Y₃,Z₃是前一周期RGB图像中的特征点p₃的三维坐标值，X'₁,Y₁',Z'₁是当前周期RGB图像中的特征点p₁'的三维坐标值，X'₂,Y₂',Z'₂是当前周期RGB图像中的特征点p'₂的三维坐标值，X'₃,Y₃',Z'₃是当前周期RGB图像中的特征点p'₃的三维坐标值，

再将3个相互匹配的特征点对代入公式(4)并用区间分析法得到6个变量φ，θ，,t_x,t_y,t_z闭区间解集，其中φ，θ，分别对应横滚角roll,俯仰角pitch,偏航角yaw,t_x,t_y,t_z分别对应三维空间中x轴、y轴、z轴方向的位移，以此作为粗匹配的位姿变换矩阵，再把粗匹配的位姿变换矩阵和来自救援机器人上的惯性测量单元IMU的横滚角roll,俯仰角pitch,偏航角yaw三个数据进行融合，生成新的位姿变换矩阵，如果通过区间分析法得到的闭区间解集为空集，则从救援机器人上的惯性测量单元IMU获取横滚角roll、俯仰角pitch及偏航角yaw，并分别以横滚角roll、俯仰角pitch及偏航角yaw作为粗匹配的位姿变换矩阵中的三个欧拉角φ、θ、从救援机器人上的里程计获取x轴方向的位移d_x、y轴方向的位移d_y、z轴方向的位移d_z作为粗匹配的位姿变换矩阵中的三个位移t_x,t_y,t_z,生成位姿变换矩阵，

所述融合的方法如下：

将惯性测量单元IMU的横滚角roll、俯仰角pitch及偏航角yaw三个数据与粗匹配的位姿变换矩阵中的三个欧拉角φ、θ及分别进行差值运算，如果：

(1)所有差值都小于5度，则分别以两者的中值作为新的位姿变换矩阵中的三个变量φ、θ、的值，

(2)有差值在5～10度之间时，则分别以25％的惯性测量单元IMU的角度与75％的位姿变换矩阵中的对应角度之和作为新的位姿变换矩阵中的三个变量φ、θ、的值，

(3)所有差值都大于10度，则以惯性测量单元IMU的横滚角roll、俯仰角pitch及偏航角yaw和里程计的x轴方向位移d_x、y轴方向位移d_y、z轴方向位移d_z作为新的位姿变换矩阵中的六个变量φ、θ、t_x、t_y、t_z的值；

所述的区间分析法为：

i.从步骤一得到的相互匹配的特征点对中任选三个点对即p₁(X₁,Y₁,Z₁)与p₁'(X'₁,Y₁',Z'₁)、p₂(X₂,Y₂,Z₂)与p'₂(X'₂,Y₂',Z'₂)及p₃(X₃,Y₃,Z₃)与p'₃(X'₃,Y₃',Z'₃)，其中X₁,Y₁,Z₁是前一周期RGB图像中的特征点p₁的三维坐标值，X₂,Y₂,Z₂是前一周期RGB图像中的特征点p₂的三维坐标值，X₃,Y₃,Z₃是前一周期RGB图像中的特征点p₃的三维坐标值，X'₁,Y₁',Z'₁是当前周期RGB图像中的特征点p₁'的三维坐标值，X'₂,Y₂',Z'₂是当前周期RGB图像中的特征点p'₂的三维坐标值，X'₃,Y₃',Z'₃是当前周期RGB图像中的特征点p'₃的三维坐标值

ii.用向量外积法判断三点p₁，p₂，p₃是否共线，若三点共线，则返回步骤i，直到选取的三个特征点p₁，p₂，p₃不共线，进行步骤iii

iii.设定六个变量φ、θ、tx、ty、tz的初始区间：φ＝[-3.14,3.14]，θ＝[-3.14,3.14]，t_x＝[-10,10]，t_y＝[-10,10]，t_z＝[-10,10]

iv.先后把三个点对p₁(X₁,Y₁,Z₁)与p₁'(X'₁,Y₁',Z'₁)、p₂(X₂,Y₂,Z₂)与p'₂(X'₂,Y₂',Z'₂)及p₃(X₃,Y₃,Z₃)与p'₃(X'₃,Y₃',Z'₃)的坐标代入(4)，得到9个方程，即9个约束条件

v.把六个变量φ、θ、t_x、t_y、t_z的区间依次代入9个约束条件中，通过区间运算，得到缩小的闭区间解集:φ＝[φ₁,φ₂]、θ＝[θ₁,θ₂]、t_x＝[t_x1,t_x2]、t_y＝[t_y1,t_y2]、t_z＝[t_z1,t_z2]，其中φ₁,φ₂,θ₁,θ₂,t_x1,t_x2,t_y1,t_y2,t_z1,t_z2分别是所属区间的上下限，若有一个区间为空，则回到步骤i，重复以上步骤，否则运算结束，取对应区间的中间值为相应变量的值，即

t_x＝(t_x1+t_x2)/2,t_y＝(t_y1+t_y2)/2,t_z＝(t_z1+t_z2)/2,

以此作为粗匹配的位姿变换矩阵

步骤三：从救援机器人的体感传感器上获取与步骤一中前后两个周期的RGB图像所对应的前后两个周期的点云数据，采用改进的迭代最近点ICP算法对采集的点云中的点进行精确配准，对机器人的位姿进行优化，得到拼接的三维点云图，改进的迭代最近点ICP算法如下：设有第一三维点集W＝{w₁,w₂,...,w_n}和第二三维点集Q＝{q₁,q₂,...q_l}，其中n和l是分别两个点集中点的个数，且第一三维点集W由前一周期点云中的点构成，第二三维点集Q由当前周期点云中的点构成，

i.根据精度要求设定表示拼接准确度的阈值E₀，

ii.从第一三维点集W中的任选一点w_m∈W，在第二三维点集Q中，找出与所选点w_m距离最近的3个点q_k1,q_k2,q_k3，并由q_k1,q_k2,q_k3形成一个平面，把所述平面中经过所选点w_m的法向量和该平面的交点记为q_k，并将所选点w_m与交点q_k作为最近点对，遍历三维点集W中的所有点，得到最近点对云，将得到的第二三维点集中的最近点组成第三三维点集U＝{u₁,u₂,...u_n}，

iii.对步骤ii中所有最近点对进行最小二乘法迭代计算，利用四元素法计算出三维空间变换R',T'

iv.利用R',T'对W进行变换，得到第四三维点集W₁，W₁＝R'W+T'，并计算均方误差E，其中u_a和w_b分别是点集U和点集W中的所匹配的最近点，

v.判断步骤iv中的误差值E是否小于设定的阈值E₀,若小于，则停止迭代，否则W＝W₁,并返回步骤ii，直到误差E小于阈值E₀为止，

步骤四：绘制点云，更新全局地图，回到步骤一，继续上述过程，直到全局地图生成完成。

附图说明

图1是本发明中三维环境地图构建方法的流程图。

图2是本发明中区间分析法的流程图。

图3是本发明中改进的迭代最近点ICP算法流程图。

具体实施方式

如图1所示，是本发明三维环境地图构建方法的流程图。

步骤一:从救援机器人的体感传感器上获取RGB图像，采用加速鲁棒特性(SURF)算法提取前后两个周期的RGB图像的特征点并提取相互匹配的特征点对p(X,Y,Z)和p'(X',Y',Z')。

SURF算法引入尺度不变的特征，即每个检测到的特征点都伴随着对应的尺寸因子。SURF算法与SIFT算法相比，SIFT算法比较稳定，检测特征点更多，但是复杂度较高，而SURF算法运算更简单，效率高，运算时间更短，因此SURF算法是SIFT算法的加强版，效率高的同时又有更好的鲁棒性。

采用加速鲁棒特性SURF算法进行特征点的提取与匹配.特征点的提取通过计算近似的黑森Hessian矩阵的行列式的极值来确定特征点的位置。对于一幅图像I上的每一个像素点x，定义其在x处尺度为σ的黑森矩阵H(x,σ):

$H(x,\mathrm{\σ})=\left[\begin{array}{cc}{L}_{\mathrm{xx}}(x,\mathrm{\σ})& L_{\mathrm{xy}}(x,\mathrm{\σ})\\ L_{\mathrm{xy}}(x,\mathrm{\σ})& L_{\mathrm{yy}}(x,\mathrm{\σ})\end{array}\right],$ 其中L_xx(x,σ)是高斯二阶偏导数在x处对图像I的卷积，再计算每个H(x,σ)矩阵的行列式：det(H_approx)＝D_xxD_yy-(0.9D_xy)²，其中D_xx，D_yy，D_xy可以分别用L_xx，L_yy，L_xy来近似替代。若行列式的值为负，则像素点x不是局部极值点；若行列式的值为正，则像素点x是极值点。

然后构建图像I的尺度空间，即图像在不同解析度下的表示。加速鲁棒特性SURF算法通过不同尺寸作为过滤模板和积分图像求取H(x,σ)矩阵行列式的响应图像，然后在响应图像上采用3D非极大值抑制，求取各个不用尺度的斑点。使用3×3×3的模板在三维空间进行非极大值抑制，当行列式的值大于预设的阈值，且比邻近的26个点的相应值都大时，被选为特征点，然后采用三维线性插值法使其更精确。

为了保证特征矢量具有旋转不变性，统计特征点邻域内的Haar小波特征。即以特征点为中心，计算半径为6s(s为特征点所在的尺度值)的邻域内，统计60度扇形内所有点在水平和垂直方向的Haar小波响应总和(Haar小波边长取4s)，并给这些响应赋高斯权重系数，靠近特征点的响应权重大，而原理特征点的响应权重小，然后60度范围内的响应相加以形成新的矢量，遍历整个圆形区域，选择最长矢量的方向最为该特征点的主方向。通过对特征点逐个进行计算，得到每一个特征点的主方向。

进行完特征点的提取后进行特征点的匹配，有如下步骤：

1)在计算H矩阵的行列式时，同时计算出矩阵的迹。由于按亮度不同，将特征点分为两种：特征点及其周围小邻域的亮度大于背景区域，H矩阵的迹为正；特征点及其周围小邻域的亮度小于背景区域，H矩阵的迹为负。依次特性，先对两个特征点的H矩阵的迹进行比较：若同号，说明两个特征点具有相同对比度；若异号则说明对比度不同，放弃2)的相似性度量；

2)在1)的基础上对两个特征点描述子的相似性度量，采用欧式距离进行计算，欧式距离定义为：其中X_ik表示前一周期RGB图像中第i个特征描述子的第k个元素，X_jk表示当前周期RGB图像中第j个特征描述子的第k个元素，n表示特征向量的维数。

对于当前周期RGB图像上的特征点，计算其到前一周期RGB图像上所有特征点的欧氏距离，得到一个距离集合。通过对距离集合进行比较得到最小欧式距离和次欧式距离。设定阈值，一般为0.8，当最小欧式距离和次欧式距离的比值小于该阈值时，认为特征点与对应最小欧氏距离的特征点是匹配的，否则，没有点与该特征点相匹配。阈值越小，匹配越稳定，但匹配的特征点对也越少。

步骤二：利用区间分析法得到位姿变换矩阵从而初步估计机器人的位姿。相对于RGBD-slam等三维环境建图算法利用基于概率分布的RANSAC算法，区间分析法在剔除误匹配对的成功率上和RANSAC算法不相上下，经测试在一些情况下数据处理的时间更短，算法效率更高。

首先定义[x]是根据概率分布得到的随机变量x的闭区间，这种表示方式可以表达不精确概率密度函数下的随机变量，对一些不确定条件有较好的鲁棒性，且在给定一组约束的条件下可以包括所有可能的值。

其次定义一种区间运算，把算术运算和方程解法运用到区间上，举例如下：

[-1,3]+[3,7]＝[2,10]

[-1,3]·[3,7]＝[-7,21]

[-1,3]/[3,7]＝[-1/3,1]

cos[0,π]＝[-1,1]

区间分析法的思想如下：

例如考虑约束条件：x₃＝x₁+x₂，首先定义[x₁]＝[-∞,2],[x₂]＝[-∞,3],[x₃]＝[4,∞]。采用区间分析法得

x₃＝x₁+x₂→z∈[4,∞]∩([-∞,2]+[-∞,3])

＝[4,∞]∩[-∞,5]＝[4,5]

x₁＝x₃-x₂→z∈[-∞,2]∩([4,∞]-[-∞,3])

＝[-∞,2]∩[1,∞]＝[1,2]

x₂＝x₃-x₁→z∈[-∞,3]∩([4,∞]+[-∞,2])

＝[-∞,3]∩[2,∞]＝[2,3]

综上可以得到缩小的的区间x₁＝[1,2],x₂＝[2,3],x₃＝[4,5]。

再如考虑函数f(x)＝x₁×cos(x₂)+exp(x₃)，令y＝f(x)＝0，对方程进行分解得到：

[a₁]＝cos([x₂])

[a₂]＝[x₁]*[a₁]

[a₃]＝exp([x₃])

[y]＝[a₂]+[a₃]

[y]＝[y]∩{0}

其中[a₁]，[a₂]，[a₃]是定义的中间变量。若[y]是空集，则在该约束下的方程无解，否则[y]＝{0}，由上述方程组逆推得：

[a₂]＝[a₂]∩([y]-[a₃])

[a₃]＝[a₃]∩([y]-[a₂])

[x₃]＝[x₃]∩log([a₃])

[a₁]＝[a₁]∩([a₂]/[x₁])

[x₁]＝[x₁]∩([a₂]/[a₁])

[x₂]＝[x₂]∩cos^-1([a₁])

同理可以得到一个闭区间解集，即缩小的区间

其次将位姿变换矩阵定义为T＝(Rt),R是旋转变换矩阵且是正交矩阵，t是平移矢量，对任一对匹配点，满足

$\left(\begin{array}{c}{X}_i\\ Y_i\\ Z_i\end{array}\right)-T\left(\begin{array}{c}{X}_j\\ Y_j\\ Z_j\end{array}\right)=0---\left(1\right)$

其中X_i，Y_i，Z_i是前一周期RGB图像中的第i个特征点所在空间中的三维坐标，X_j，Y_j，Z_j是与特征点(X_i，Y_i，Z_i)相匹配的当前周期RGB图像中的第j个特征点的三维坐标，

定义旋转变换矩阵为：

其中φ，θ，是遵守左手法则的三个欧拉角，φ，θ，分别对应横滚角roll,俯仰角pitch,偏航角yaw,定义平移矢量 $t=\left(\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right)---\left(3\right)$

其中，t_x,t_y,t_z分别对应三维空间中x轴、y轴、z轴方向的位移，

把(2)、(3)代入(1)得到三个方程：

区间分析法最主要的优点在于当约束方程的个数多于变量的个数时，能快速求解非线性系统的方程。本专利要解决的问题涉及六个未知变量φ，θ，tx,ty,tz，每一对匹配的特征点对都有3个方程。这意味着只需要两对特征点对就能得出六个变量的值，但得到的六个值可能出现较大的误差。因此用区间分析法可以规避这个问题。首先把足够多的点对依次代入方程(4)，利用上述区间运算进行求解，对每一点对三个约束条件的求解都能缩小变量的区间，直到达到预期的效果，如φ，θ，三个变量中每一个变量的区间长度都小于0.1rad，t_x,t_y,t_z三个变量中每一个变量的区间长度都小于0.05m。经测试，为了使区间达到这个精度，区间分析法需要的训练样本数量在不同的情况下都不一样，当步骤一匹配误差较大时，可能需要200个数据，计算时间过长，而匹配误差较小时，则只需要4个样本。针对这个问题，提出一种解决方案：从步骤一提取的点对中选取3对不共线且匹配准确的点对，代入(4)得到9个约束方程。经测试，通过对9个约束方程进行区间运算即可得到满足区间长度要求的6个区间，进而得到对应的6个变量的值。

因此，从步骤一得到的相互匹配的特征点对中选取3个不在一条直线上的相互匹配的特征点对即：p₁(X₁,Y₁,Z₁)与p₁'(X'₁,Y₁',Z'₁)、p₂(X₂,Y₂,Z₂)与p'₂(X'₂,Y₂',Z'₂)及p₃(X₃,Y₃,Z₃)与p'₃(X'₃,Y₃',Z'₃)，其中X₁,Y₁,Z₁是前一周期RGB图像中的特征点p₁的三维坐标值，X₂,Y₂,Z₂是前一周期RGB图像中的特征点p₂的三维坐标值，X₃,Y₃,Z₃是前一周期RGB图像中的特征点p₃的三维坐标值，X'₁,Y₁',Z'₁是当前周期RGB图像中的特征点p₁'的三维坐标值，X'₂,Y₂',Z'₂是当前周期RGB图像中的特征点p'₂的三维坐标值，X'₃,Y₃',Z'₃是当前周期RGB图像中的特征点p'₃的三维坐标值。

再将3个相互匹配的特征点对代入公式(4)并用区间分析法得到6个变量φ，θ，,tx,ty,tz闭区间解集，以此作为粗匹配的位姿变换矩阵，再把粗匹配的位姿变换矩阵和来自救援机器人上的惯性测量单元IMU的横滚角roll,俯仰角pitch,偏航角yaw三个数据进行融合，生成新的位姿变换矩阵；如果通过区间分析法得到的闭区间解集为空集，或匹配差较大，可能是环境发生了动态的变化，或者两幅RGB图像中特征点较少，例如两幅白墙的图片，无法匹配，不能生成位姿变换矩阵，则采用如下方式：从救援机器人上的惯性测量单元IMU获取横滚角roll、俯仰角pitch及偏航角yaw，并分别以横滚角roll、俯仰角pitch及偏航角yaw作为粗匹配的位姿变换矩阵中的三个欧拉角φ、θ、，从救援机器人上的里程计获取x轴方向的位移d_x、y轴方向的位移d_y、z轴方向的位移d_z作为粗匹配的位姿变换矩阵中的三个位移t_x,t_y,t_z,生成位姿变换矩阵。

所述融合的方法如下：

将惯性测量单元IMU的横滚角roll、俯仰角pitch及偏航角yaw三个数据与粗匹配的位姿变换矩阵中的三个欧拉角φ、θ及分别进行差值运算，如果：

(1)所有差值都小于5度，则分别以两者的中值作为新的位姿变换矩阵中的三个变量φ、θ、的值，

(2)有差值在5～10度之间时，则分别以25％的惯性测量单元IMU的角度与75％的位姿变换矩阵中的对应角度之和作为新的位姿变换矩阵中的三个变量φ、θ、的值，

(3)所有差值都大于10度，则以惯性测量单元IMU的横滚角roll、俯仰角pitch及偏航角yaw和里程计的x轴方向位移d_x、y轴方向位移d_y、z轴方向位移d_z作为新的位姿变换矩阵中的六个变量φ、θ、t_x、t_y、t_z的值；

所述的区间分析法为：

i.从步骤一得到的相互匹配的特征点对中任选三个点对即p₁(X₁,Y₁,Z₁)与p₁'(X'₁,Y₁',Z'₁)、p₂(X₂,Y₂,Z₂)与p'₂(X'₂,Y₂',Z'₂)及p₃(X₃,Y₃,Z₃)与p'₃(X'₃,Y₃',Z'₃)，其中X₁,Y₁,Z₁是前一周期RGB图像中的特征点p₁的三维坐标值，X₂,Y₂,Z₂是前一周期RGB图像中的特征点p₂的三维坐标值，X₃,Y₃,Z₃是前一周期RGB图像中的特征点p₃的三维坐标值，X'₁,Y₁',Z'₁是当前周期RGB图像中的特征点p₁'的三维坐标值，X'₂,Y₂',Z'₂是当前周期RGB图像中的特征点p'₂的三维坐标值，X'₃,Y₃',Z'₃是当前周期RGB图像中的特征点p'₃的三维坐标值,

ii.用向量外积法判断三点p₁，p₂，p₃是否共线，

点p₁,p₂构成向量其中a₁＝X₂-X₁，a₂＝Y₂-Y₁，a₃＝Z₂-Z₁，点p₁,p₃构成向量其中b₁＝X₃-X₁，b₂＝Y₃-Y₁，b₃＝Z₃-Z₁，向量的向量积的模 $|\stackrel{\→}{a}\×\stackrel{\→}{b}|=\sqrt{{(a_2{b}_3-a_3{b}_2)}^2+{(a_3{b}_1-a_1{b}_3)}^2+{(a_1{b}_2-a_2{b}_1)}^2},$ 若|则三点共线，则返回步骤i，直到选取的三个特征点p₁，p₂，p₃不共线，进行步骤iii

iii.设定六个变量φ、θ、tx、ty、tz的初始区间：φ＝[-3.14,3.14]，θ＝[-3.14,3.14]，t_x＝[-10,10]，t_y＝[-10,10]，t_z＝[-10,10]

iv.先后把三个点对p₁(X₁,Y₁,Z₁)与p₁'(X'₁,Y₁',Z'₁)、p₂(X₂,Y₂,Z₂)与p'₂(X'₂,Y₂',Z'₂)及p₃(X₃,Y₃,Z₃)与p'₃(X'₃,Y₃',Z'₃)的坐标代入(4)，得到9个方程，即9个约束条件

v.把六个变量φ、θ、t_x、t_y、t_z的区间依次代入9个约束条件中，通过区间运算，得到缩小的闭区间解集：φ＝[φ₁,φ₂]、θ＝[θ₁,θ₂]、t_x＝[t_x1,t_x2]、t_y＝[t_y1,t_y2]、t_z＝[t_z1,t_z2]，其中φ₁,φ₂,θ₁,θ₂,t_x1,t_x2,t_y1,t_y2,t_z1,t_z2分别是所属区间的上下限，若有一个区间为空，则回到步骤i，重复以上步骤，否则运算结束，取对应区间的中间值为相应变量的值，即

t_x＝(t_x1+t_x2)/2,t_y＝(t_y1+t_y2)/2,t_z＝(t_z1+t_z2)/2,

以此作为粗匹配的位姿变换矩阵。

步骤三：从救援机器人的体感传感器上获取与步骤一中前后两个周期的RGB图像所对应的前后两个周期的点云数据，采用改进的迭代最近点ICP算法对采集的点云中的点进行精确配准，对机器人的位姿进行优化，得到拼接的三维点云图，改进的迭代最近点ICP算法如下：设有第一三维点集W＝{w₁,w₂,...,w_n}和第二三维点集Q＝{q₁,q₂,...q_l}，其中n和l是分别两个点集中点的个数，且第一三维点集W由前一周期点云中的点构成，第二三维点集Q由当前周期点云中的点构成，

i.根据精度要求设定拼接准确度的阈值E₀，E₀表示两个点云中的相匹配的点对的平均误差，一般设定E₀＝0.2mm，

ii.从第一三维点集W中的任选一点w_m∈W，在第二三维点集Q中，找出与所选点w_m欧式距离最近的3个点q_k1,q_k2,q_k3，并由q_k1,q_k2,q_k3形成一个平面，把所述平面中经过所选点w_m的法向量和该平面的交点记为q_k，并将所选点w_m与交点q_k作为最近点对，遍历三维点集W中的所有点，得到最近点对云，将得到的第二三维点集中的最近点组成第三三维点集U＝{u₁,u₂,...u_n}，

iii.对步骤ii中所有最近点对进行最小二乘法迭代计算，利用四元数法计算三维空间变换R',T'

设旋转向量为单位四元数 $q_r=\left[\begin{array}{c}{q}_0^{\′}\\ q_1^{\′}\\ q_2^{\′}\\ q_3^{\′}\end{array}\right],$ ，其中q₀>＝0,且q'₀ ²+q₁'²+q'₂ ²+q'₃ ²＝1，可以得到一个3×3的旋转矩阵R(q_r):

$R^{\′}=\left[\begin{array}{ccc}(q_0^{\′2}+q_1^{\′2}-q_2^{\′2}-q_3^{\′2})& 2(q_1^{\′}{q}_2^{\′}-q_0^{\′}{q}_3^{\′})& 2(q_1^{\′}{q}_3^{\′}+q_0^{\′}{q}_2^{\′})\\ 2(q_1^{\′}{q}_2^{\′}+q_0^{\′}{q}_3^{\′})& (q_0^{\′2}+q_2^{\′2}-q_1^{\′2}-q_3^{\′2})& 2(q_2^{\′}{q}_3^{\′}-q_0^{\′}{q}_1^{\′})\\ 2(q_1^{\′}{q}_3^{\′}-q_0^{\′}{q}_2^{\′})& 2(q_2^{\′}{q}_3^{\′}+q_0^{\′}{q}_1^{\′})& (q_0^{\′2}+q_3^{\′2}-q_1^{\′2}-q_2^{\′2})\end{array}\right]$

再取平移变换矩阵 $T^{\′}=\left[\begin{array}{c}{q}_4^{\′}\\ q_5^{\′}\\ q_6^{\′}\end{array}\right],$ 把三维点集W和U中对应的点对的坐标代入，使得

最小的R',T'就是所求的变换矩阵，其中u_a和w_b分别是点集U和点集W中的所匹配的最近点。

iv.利用R',T'对W进行变换，得到第四三维点集W₁，W₁＝R'W+T'，

v.判断步骤iii中的误差值E是否小于设定的阈值E₀,若小于，则停止迭代，否则W＝W₁,并返回步骤ii，直到误差E小于阈值E₀为止，

步骤四：绘制点云，更新全局地图，回到步骤一，继续上述过程，直到全局地图生成完成。

Claims (1)

1.一种救援机器人三维环境地图实时构建方法，其特征在于包括以下步骤：

步骤一:从救援机器人的体感传感器上获取RGB图像，采用加速鲁棒特性(SURF)算法提取前后两个周期的RGB图像的特征点并提取相互匹配的特征点对p(X,Y,Z)和p'(X',Y',Z')，其中X,Y,Z是特征点p的坐标，X',Y',Z'是特征点p'的坐标，

步骤二：利用区间分析法得到位姿变换矩阵从而初步估计机器人的位姿,

将位姿变换矩阵定义为T＝(Rt),R是旋转变换矩阵且是正交矩阵，t是平移矢量，对任一对匹配点，满足

$\left(\begin{array}{c}{X}_i\\ Y_i\\ Z_i\end{array}\right)-T\left(\begin{array}{c}{X}_j\\ Y_j\\ Z_j\end{array}\right)=0---\left(1\right)$

其中X_i，Y_i，Z_i是前一周期RGB图像中的第i个特征点所在空间中的三维坐标，X_j，Y_j，Z_j是与特征点(X_i，Y_i，Z_i)相匹配的当前周期RGB图像中的第j个特征点的三维坐标，

定义旋转变换矩阵为：

其中φ，θ，是遵守左手法则的三个欧拉角，φ，θ，分别对应横滚角roll,俯仰角pitch,偏航角yaw,

定义平移矢量 $t=\left(\begin{array}{c}{t}_x\\ t_y\\ t_z\end{array}\right)---\left(3\right)$

其中，t_x,t_y,t_z分别对应三维空间中x轴、y轴、z轴方向的位移，

把(2)、(3)代入(1)得到三个方程：

从步骤一得到的相互匹配的特征点对中选取3个不在一条直线上的相互匹配的特征点对即：p₁(X₁,Y₁,Z₁)与p₁'(X'₁,Y₁',Z'₁)、p₂(X₂,Y₂,Z₂)与p'₂(X'₂,Y₂',Z'₂)及p₃(X₃,Y₃,Z₃)与p'₃(X'₃,Y₃',Z'₃)，其中X₁,Y₁,Z₁是前一周期RGB图像中的特征点p₁的三维坐标值，X₂,Y₂,Z₂是前一周期RGB图像中的特征点p₂的三维坐标值，X₃,Y₃,Z₃是前一周期RGB图像中的特征点p₃的三维坐标值，X'₁,Y₁',Z'₁是当前周期RGB图像中的特征点p₁'的三维坐标值，X'₂,Y₂',Z'₂是当前周期RGB图像中的特征点p'₂的三维坐标值，X'₃,Y₃',Z'₃是当前周期RGB图像中的特征点p'₃的三维坐标值，

再将3个相互匹配的特征点对代入公式(4)并用区间分析法得到6个变量φ，θ，,t_x,t_y,t_z闭区间解集，其中φ，θ，分别对应横滚角roll,俯仰角pitch,偏航角yaw,t_x,t_y,t_z分别对应三维空间中x轴、y轴、z轴方向的位移，以此作为粗匹配的位姿变换矩阵，再把粗匹配的位姿变换矩阵和来自救援机器人上的惯性测量单元IMU的横滚角roll,俯仰角pitch,偏航角yaw三个数据进行融合，生成新的位姿变换矩阵，如果通过区间分析法得到的闭区间解集为空集，则从救援机器人上的惯性测量单元IMU获取横滚角roll、俯仰角pitch及偏航角yaw，并分别以横滚角roll、俯仰角pitch及偏航角yaw作为粗匹配的位姿变换矩阵中的三个欧拉角φ、θ、从救援机器人上的里程计获取x轴方向的位移d_x、y轴方向的位移d_y、z轴方向的位移d_z作为粗匹配的位姿变换矩阵中的三个位移t_x,t_y,t_z,生成位姿变换矩阵，

所述融合的方法如下：

将惯性测量单元IMU的横滚角roll、俯仰角pitch及偏航角yaw三个数据与粗匹配的位姿变换矩阵中的三个欧拉角φ、θ及分别进行差值运算，如果：

(1)所有差值都小于5度，则分别以两者的中值作为新的位姿变换矩阵中的三个变量φ、θ、的值，

(2)有差值在5～10度之间时，则分别以25％的惯性测量单元IMU的角度与75％的位姿变换矩阵中的对应角度之和作为新的位姿变换矩阵中的三个变量φ、θ、的值，

(3)所有差值都大于10度，则以惯性测量单元IMU的横滚角roll、俯仰角pitch及偏航角yaw和里程计的x轴方向位移d_x、y轴方向位移d_y、z轴方向位移d_z作为新的位姿变换矩阵中的六个变量φ、θ、t_x、t_y、t_z的值；

所述的区间分析法为：

i.从步骤一得到的相互匹配的特征点对中任选三个点对即p₁(X₁,Y₁,Z₁)与p₁'(X'₁,Y₁',Z'₁)、p₂(X₂,Y₂,Z₂)与p'₂(X'₂,Y₂',Z'₂)及p₃(X₃,Y₃,Z₃)与p'₃(X'₃,Y₃',Z'₃)，其中X₁,Y₁,Z₁是前一周期RGB图像中的特征点p₁的三维坐标值，X₂,Y₂,Z₂是前一周期RGB图像中的特征点p₂的三维坐标值，X₃,Y₃,Z₃是前一周期RGB图像中的特征点p₃的三维坐标值，X'₁,Y₁',Z'₁是当前周期RGB图像中的特征点p₁'的三维坐标值，X'₂,Y₂',Z'₂是当前周期RGB图像中的特征点p'₂的三维坐标值，X'₃,Y₃',Z'₃是当前周期RGB图像中的特征点p'₃的三维坐标值，

ii.用向量外积法判断三点p₁，p₂，p₃是否共线，若三点共线，则返回步骤i，直到选取的三个特征点p₁，p₂，p₃不共线，进行步骤iii,

iii.设定六个变量φ、θ、t_x、t_y、t_z的初始区间：φ＝[-3.14,3.14]，θ＝[-3.14,3.14]，t_x＝[-10,10]，t_y＝[-10,10]，t_z＝[-10,10]，

iv.先后把三个点对p₁(X₁,Y₁,Z₁)与p₁'(X'₁,Y₁',Z'₁)、p₂(X₂,Y₂,Z₂)与p'₂(X'₂,Y₂',Z'₂)及p₃(X₃,Y₃,Z₃)与p'₃(X'₃,Y₃',Z'₃)的坐标代入(4)，得到9个方程，即9个约束条件，

v.把六个变量φ、θ、t_x、t_y、t_z的区间依次代入9个约束条件中，通过区间运算，得到缩小的闭区间解集：

φ＝[φ₁,φ₂]、θ＝[θ₁,θ₂]、t_x＝[t_x1,t_x2]、t_y＝[t_y1,t_y2]、t_z＝[t_z1,t_z2]，其中φ₁,φ₂,θ₁,θ₂,t_x1,t_x2,t_y1,t_y2,t_z1,t_z2分别是所属区间的上下限，若有一个区间为空，则回到步骤i，重复以上步骤，否则运算结束，取对应区间的中间值为相应变量的值，即

t_x＝(t_x1+t_x2)/2,t_y＝(t_y1+t_y2)/2,t_z＝(t_z1+t_z2)/2,

以此作为粗匹配的位姿变换矩阵，

步骤三：从救援机器人的体感传感器上获取与步骤一中前后两个周期的RGB图像所对应的前后两个周期的点云数据，采用改进的迭代最近点ICP算法对采集的点云中的点进行精确配准，对机器人的位姿进行优化，得到拼接的三维点云图，改进的迭代最近点ICP算法如下：设有第一三维点集W＝{w₁,w₂,...,w_n}和第二三维点集Q＝{q₁,q₂,...q_l}，其中n和l是分别两个点集中点的个数，且第一三维点集W由前一周期点云中的点构成，第二三维点集Q由当前周期点云中的点构成，

i.根据精度要求设定表示拼接准确度的阈值E₀，

ii.从第一三维点集W中的任选一点w_m∈W，在第二三维点集Q中，找出与所选点w_m距离最近的3个点q_k1,q_k2,q_k3，并由q_k1,q_k2,q_k3形成一个平面，把所述平面中经过所选点w_m的法向量和该平面的交点记为q_k，并将所选点w_m与交点q_k作为最近点对，遍历三维点集W中的所有点，得到最近点对云，将得到的第二三维点集中的最近点组成第三三维点集U＝{u₁,u₂,...u_n}，

iii.对步骤ii中所有最近点对进行最小二乘法迭代计算，利用四元数法计算出三维空间变换R,T

iv.利用R',T'对W进行变换，得到第四三维点集W₁，W₁＝R'W+T'，并计算均方误差E，其中u_a和w_b分别是点集U和点集W中的所匹配的最近点，

v.判断步骤iv中的误差值E是否小于设定的阈值E₀,若小于，则停止迭代，否则W＝W₁,并返回步骤ii，直到误差E小于阈值E₀为止，

步骤五：绘制点云，更新全局地图，回到步骤一，继续上述过程，直到全局地图生成完成。