CN112132857B - 动态环境混合视觉系统的动态物检测和静态地图重建方法 - Google Patents

Abstract

本发明提出动态环境混合视觉系统的动态物检测和静态地图重建方法，包括以下步骤；步骤S1：进行外参标定，获取全景相机和三维激光两传感器之间的坐标变换参数；步骤S2：将第t帧点云作为特征点投射到第t帧图像上，获取特征点的像素运动向量，并估算因小车运动而引起的特征点的人工运动向量来进行背景运动补偿，从而获得点云中动态点；步骤S3：对当前帧点云进行簇分割；步骤S4：利用点云数据中每个点索引唯一特性，结合动态点检测结果与分割结果，通过簇中动态点的占比进行判断，提取出动态物体；步骤S5：利用八叉树地图工具和该帧下的激光雷达里程计，对静态地图进行重建；本发明可鲁棒地、更为完整地进行动态物体提取和静态三维地图重建。

Description

动态环境混合视觉系统的动态物检测和静态地图重建方法

技术领域

本发明涉及动态物体检测和三维地图重建技术领域，尤其是动态环境混合视觉系统的动态物检测和静态地图重建方法。

背景技术

近年来，机器人技术得到蓬勃发展，其在定位和导航中的应用越来越广，因此，三维地图重建技术已经成为计算机视觉领域的研究热点之一。虽然室内或者室外环境下的三维地图可以通过深度摄像机和激光雷达等视觉传感器来获取，由于映射环境中存在移动物体，三维地图重建仍然是一项具有挑战性的工作。动态物体将会在地图上留下一系列“痕迹”，这将会形成不理想的特征从而影响机器人的对自身位置的判断和增加了导航的难度。

静态环境(视觉传感器处于静止状态)下的动态目标检测研究已经取得了很好的研究成果，许多著名的方法已经被提出来，例如帧差法、背景提取法和光流法等，它们在运动检测中都表现出一定的成效，但是机器人在运动过程中对周围环境的移动目标检测仍然是一个具有挑战的任务。由于机器人的自我运动，基于图像的方法如背景提取法无法获得稳定的背景模型来进一步完成目标检测，光流法因无法进一步区分前景运动和背景运动而失效；基于激光的方法主要是借助于栅格思想，利用点云配准算法如ICP或者CPD将此帧的点云投射到上一帧坐标下，然后比较栅格地图的前后变化来提取动态物体。近年来，基于深度学习的动态对象检测方法被陆续提出，并且取得了一定的成效，但是他们依赖于特定的场景，且其准确性受到数据集和训练过程的影响。因此，在动态环境下(视觉传感器处于运动状态)进行运动物体检测的研究有着重要的意义。

与仿真环境相比，实际环境中的图像和点云数据存在更多的噪声，因此漏检问题是不可避免的。通过分析，漏检问题可以分为两类情况，一类是特征点原本属于运动区域而没有被检测出来；另一类是特征点属于存在部分运动的非刚性物体中静态部分而没有被归结为同一个运动物体，因此提出一种基于点云分割辅助的方法来改善该问题是至关重要的。在室外进行三维地图重建时，随着时间的推移，地图的容量将会越来越大，这对计算机的性能是一个很大的考验，且地图中会留下很多冗余数据；同时三维点云数据不能直接运用于路径规划等任务。因此设计一种动态环境下混合视觉系统的动态物体检测和静态地图重建是具有重大意义的。

发明内容

本发明提出动态环境混合视觉系统的动态物检测和静态地图重建方法，可鲁棒地、更为完整地进行动态物体提取和静态三维地图重建。

本发明采用以下技术方案。

动态环境混合视觉系统的动态物检测和静态地图重建方法，所述混合视觉系统包括以移动物体承载的全景相机和三维激光雷达，其特征在于：所述方法包括以下步骤；

步骤S1：对混合视觉系统进行外参标定，获取全景相机和三维激光两传感器之间的坐标变换参数，作为混合视觉系统的外参数。

步骤S2：将第t帧点云作为特征点投射到第t帧图像上，利用光流算法获取特征点的像素运动向量，并估算因小车自我运动而引起的特征点的人工运动向量来进行背景运动补偿，从而获得点云中动态点。

步骤S3：利用改进的划区域欧几里得聚类算法，对当前帧点云进行簇分割。步骤S4：利用点云数据中每个点索引唯一特性，结合动态点检测结果与点云欧几里得分割结果，通过簇中动态点的占比对动态簇进行判断，从而提取出动态物体。

步骤S5：利用八叉树地图工具和该帧下的激光雷达里程计，对静态地图进行重建。

所述移动物体为移动小车平台；所述全景相机的相机坐标系中，相机坐标与拍摄图像坐标一致，全景相机无须相机内参标定；所述步骤S1包括；

步骤S1.1：将全景相机和三维激光雷达固定在移动小车平台上面，确保两者的相对位置不变；

步骤S1.2：调整棋盘格位置使其在全景相机传感器和三维激光雷达传感器中均可成像，并且成像位置位于图像的中层区域处；分别拍摄两幅棋盘格图像，之后利用PnP和Levenberg-Marquardt优化方法完成混合视觉系统外参标定，获得激光到相机的外参数矩阵[R_l2c,T_l2c]，下标l2c表示激光雷达到相机的坐标变换。

所述步骤S2包括；

步骤S2.1：利用RANSAC算法进行第t帧点云地面过滤，过滤后的点云在激光雷达下的坐标为p^l＝[x^l,y^l,z^l]^T，其对应在相机坐标系下的坐标为p^c＝[x^c,y^c,z^c]^T，其在第t帧图像上像素点坐标为pⁱ＝[u,v]^T，图像的高度为H，宽度为W,其中上标l、c和i分别表示了激光雷达坐标系、相机坐标系和图像坐标系，则从激光雷达坐标p^l转换到图像坐标pⁱ的公式如下为：

p^c＝[R_l2c,T_l2c]p^l 公式一

其中int(·)函数表示取整，arctan2(·)函数是将arctan(·)反正切函数的值域从

扩展到(-π,π)；

步骤S2.2：步骤S2.1中将点云数据投射到对应图像上作为特征点，利用光流算法，计算得特征点的像素运动向量v_f；

步骤S2.3：进行人工运动向量v_o的估计；方法是先利用loam算法从点云数据中估计移动小车的里程计，经过转换后得帧间里程计O_t:t+1，再将去除地面点的第t帧点云数据p_t乘上帧间里程计获得新的点云数据p'_t，最后分别将数据p'_t和p_t投射到图像上获取的像素值，其相减的结果作为人工运动向量v_o；公式如下所示：

O_t:t+1＝O_t+1:1 ^-1O_t:1 公式四

p'_t＝O_t:t+1p_t 公式五

v_o＝T(p'_t)-T(p_t) 公式六

其中O_t:1和O_t+1:1分别是第t帧到第一帧的里程计和第t+1帧到第一帧的里程计，T(·)表示点云数据到图像的投射函数；

步骤S2.4：进行点云数据中动态点检测；方法为，假设人工运动向量的核心为该帧点云数据所有点都是静态的，同时图像静态背景光流表示着移动小车的运动状态，因此由光流法估计的像素运动向量v_f与人工运动向量v_o在运动区域上两者有着明显的差异、在静态区域上两者相似；

进一步对移动物体的运动情况进行分析，设移动物体的运动情况可以归结为两类：第一类为移动物体的运动方向与小车运动方向平行且反向，此时像素运动向量v_f与人工运动向量v_o之间的夹角约为0，且其两者模长存在着明显的差异；第二类为除去第一类运动情况以外的其他任意运动，此时v_f和v_o向量间的夹角存在着明显差异；

针对上诉移动物体的两种运动情况，使用与之对应的约束条件来检测出动态点，首先以向量间夹角阈值Th_θ为动态点的过滤条件1，大于阈值Th_θ的点将被当做动态点；对小于阈值Th_θ的点，以两者的向量差的模长为过滤条件2，进一步检测出第一类运动情况下的动态点，为了归一化向量差的模长，基于高斯函数模型的动态点概率被运用到该系统中，当概率p小于阈值Th_g时，则判定为动态点。概率p评估公式如下：

其中σ²是小于阈值T_θ的所有点的||v_f-v_o||的方差，||·||是向量的模长，模长差越大时，则概率p值越小。动态点检测过程分为两种情况，满足时则断定为动态点，如下公式所示：

其中＜v_f,v_o＞表示两向量间的夹角值；

为了提升检测动态点的鲁棒性，采用基于直方图统计的自适应阈值调整方法；该方法中，首先对像素运动向量v_f与人工运动向量v_o之间的夹角进行直方图统计，将夹角范围0-π以0.2弧度为间隔划分，针对直方图统计结果，取其第一次低谷位置处的值作为该帧下夹角阈值Th_θ。

所述步骤S3中的点云分割的方法包括：

步骤S3.1：以激光雷达坐标原点为圆心，第一个圆半径为15m，之后以半径差为15m做同心圆，对已去除地面点的点云数据进行区域划分。

步骤S3.2：对每一个子区域，以Euclidean clustering算法，即欧几里得聚类算法对点云进行分割；不同区域运用该区域内最大的欧几里得距离阈值

来完成点云分割；点到激光雷达中心的距离用表示为d，激光雷达的最大垂直分辨率表示为θ，则将每个区域内距离d最大值代入阈值Th_d估计公式中即可获得其对应的最大欧几里得距离阈值

阈值Th_d的估计公式如下所示：

其中tan(·)函数表示正切函数。

所述步骤S4中结合动态点检测与点云分割结果进行动态物体提取的方法包括：

步骤S4.1：根据激光雷达三维数据中点索引的唯一特性，对动态点进行归簇，也就是寻找每一个动态点所对应的点云簇，即点云分割结果；

步骤S4.2：记录每一簇中点的总数Q_c与该簇中动态点的个数Q_d，通过每个簇中动态点的占比R来判断该簇是否为动态物；若动态物提取的阈值表示为Th_R,则动态物提取过程如下公式所示：

R＞Th_R 公式十一。

所述步骤S5中的静态地图重建的方法包括：

步骤S5.1：设定第一帧为世界坐标系，将之后点云通过里程计转换到世界坐标系上，重建三维点云地图，第t帧在激光雷达坐标系下的静态点云数据表示为

第t帧到世界坐标系的里程计为O_t:1，第t帧在激光雷达坐标系下的点云数据转换到世界坐标系下数据表示为

则将第t帧点云数据更新到地图中的过程公式如下所示：

步骤S5.2：利用octomap算法对新进的点云数据进行八叉树地图更新。设t＝1,...,T时刻，对应观测到的数据为z₁,...,z_T，则第n个叶子点到T时刻记录的全部信息P(n|z_1:T)为：

其中P(n|z_1:T-1)表示第n个叶子点到T-1时刻记录的全部信息，P(n|z_T)表示第n个叶子在T时刻记录的信息，P(n)表示一个固定量的先验概率。

所述八叉树地图工具为octomap。

与现有技术相比，本发明具有以下有益效果：

1.本发明在动态点检测过程中，分析了环境中动态物体的运动类型，总结出可以划分为两种类型；针对这两种不同的运动类型，提出了一种先基于特征点像素运动向量与人工运动向量间夹角为约束条件过滤一部分动态点，再基于两向量差的模长为约束条件完整的检测出动态点。

2.本发明在动态点检测过程中，不同时刻的动态物体运动(速度和方向)可能会出现较大的不同，通过常数阈值的方法无法鲁棒性的检测出动态点；为了解决这个问题，提出了一种基于直方图统计的方法，分别获取该帧下的夹角阈值和模长差为基准的动态点概率阈值，从而实现自适应阈值目标。

本发明在动态物提取过程中，为了解决漏检，即动态物体提取不完整问题，提出了点云分割辅助动态物体提取方法。将动态点检测结果与点云分割结果相结合，根据点云中每个点的索引唯一性条件对每个动态点进行归簇，最后通过每个点云簇的动态点占比判断是否为动态物，从而完整的提取出动态物体。

附图说明

下面结合附图和具体实施方式对本发明进一步详细的说明：

附图1为本发明的算法流程示意图；

附图2为点云在图像上的像素运动向量v_f与人工运动向量v_o示意图；

附图3为动态点检测过程示意图；

附图4为夹角直方图统计示意图；

附图5为点云划区域欧几里得分割示意图；

附图6为动态点归簇示意图；

附图7为动态物提取结果示意图；

附图8为静态地图重建结果示意图。

具体实施方式

如图所示，动态环境混合视觉系统的动态物检测和静态地图重建方法，所述混合视觉系统包括以移动物体承载的全景相机和三维激光雷达，其特征在于：所述方法包括以下步骤；

步骤S1：对混合视觉系统进行外参标定，获取全景相机和三维激光两传感器之间的坐标变换参数，作为混合视觉系统的外参数。

步骤S2：将第t帧点云作为特征点投射到第t帧图像上，利用光流算法获取特征点的像素运动向量，并估算因小车自我运动而引起的特征点的人工运动向量来进行背景运动补偿，从而获得点云中动态点。

步骤S3：利用改进的划区域欧几里得聚类算法，对当前帧点云进行簇分割。步骤S4：利用点云数据中每个点索引唯一特性，结合动态点检测结果与点云欧几里得分割结果，通过簇中动态点的占比对动态簇进行判断，从而提取出动态物体。

步骤S5：利用八叉树地图工具和该帧下的激光雷达里程计，对静态地图进行重建。

所述移动物体为移动小车平台；所述全景相机的相机坐标系中，相机坐标与拍摄图像坐标一致，全景相机无须相机内参标定；所述步骤S1包括；

步骤S1.1：将全景相机和三维激光雷达固定在移动小车平台上面，确保两者的相对位置不变；

步骤S1.2：调整棋盘格位置使其在全景相机传感器和三维激光雷达传感器中均可成像，并且成像位置位于图像的中层区域处；分别拍摄两幅棋盘格图像，之后利用PnP和Levenberg-Marquardt优化方法完成混合视觉系统外参标定，获得激光到相机的外参数矩阵[R_l2c,T_l2c]，下标l2c表示激光雷达到相机的坐标变换。

所述步骤S2包括；

步骤S2.1：利用RANSAC算法进行第t帧点云地面过滤，过滤后的点云在激光雷达下的坐标为p^l＝[x^l,y^l,z^l]^T，其对应在相机坐标系下的坐标为p^c＝[x^c,y^c,z^c]^T，其在第t帧图像上像素点坐标为pⁱ＝[u,v]^T，图像的高度为H，宽度为W,其中上标l、c和i分别表示了激光雷达坐标系、相机坐标系和图像坐标系，则从激光雷达坐标p^l转换到图像坐标pⁱ的公式如下为：

p^c＝[R_l2c,T_l2c]p^l 公式一

其中int(·)函数表示取整，arctan2(·)函数是将arctan(·)反正切函数的值域从

扩展到(-π,π)；

步骤S2.2：步骤S2.1中将点云数据投射到对应图像上作为特征点，利用光流算法，计算得特征点的像素运动向量v_f；

步骤S2.3：进行人工运动向量v_o的估计；方法是先利用loam算法从点云数据中估计移动小车的里程计，经过转换后得帧间里程计O_t:t+1，再将去除地面点的第t帧点云数据p_t乘上帧间里程计获得新的点云数据p'_t，最后分别将数据p'_t和p_t投射到图像上获取的像素值，其相减的结果作为人工运动向量v_o；公式如下所示：

O_t:t+1＝O_t+1:1 ^-1O_t:1 公式四

p'_t＝O_t:t+1p_t 公式五

v_o＝T(p'_t)-T(p_t) 公式六

其中O_t:1和O_t+1:1分别是第t帧到第一帧的里程计和第t+1帧到第一帧的里程计，T(·)表示点云数据到图像的投射函数；

步骤S2.4：进行点云数据中动态点检测；方法为，假设人工运动向量的核心为该帧点云数据所有点都是静态的，同时图像静态背景光流表示着移动小车的运动状态，因此由光流法估计的像素运动向量v_f与人工运动向量v_o在运动区域上两者有着明显的差异、在静态区域上两者相似；

进一步对移动物体的运动情况进行分析，设移动物体的运动情况可以归结为两类：第一类为移动物体的运动方向与小车运动方向平行且反向，此时像素运动向量v_f与人工运动向量v_o之间的夹角约为0，且其两者模长存在着明显的差异；第二类为除去第一类运动情况以外的其他任意运动，此时v_f和v_o向量间的夹角存在着明显差异；

针对上诉移动物体的两种运动情况，使用与之对应的约束条件来检测出动态点，首先以向量间夹角阈值Th_θ为动态点的过滤条件1，大于阈值Th_θ的点将被当做动态点；对小于阈值Th_θ的点，以两者的向量差的模长为过滤条件2，进一步检测出第一类运动情况下的动态点，为了归一化向量差的模长，基于高斯函数模型的动态点概率被运用到该系统中，当概率p小于阈值Th_g时，则判定为动态点。概率p评估公式如下：

其中σ²是小于阈值T_θ的所有点的||v_f-v_o||的方差，||·||是向量的模长，模长差越大时，则概率p值越小。动态点检测过程分为两种情况，满足时则断定为动态点，如下公式所示：

其中＜v_f,v_o＞表示两向量间的夹角值；

为了提升检测动态点的鲁棒性，采用基于直方图统计的自适应阈值调整方法；该方法中，首先对像素运动向量v_f与人工运动向量v_o之间的夹角进行直方图统计，将夹角范围0-π以0.2弧度为间隔划分，针对直方图统计结果，取其第一次低谷位置处的值作为该帧下夹角阈值Th_θ。

所述步骤S3中的点云分割的方法包括：

步骤S3.1：以激光雷达坐标原点为圆心，第一个圆半径为15m，之后以半径差为15m做同心圆，对已去除地面点的点云数据进行区域划分。

步骤S3.2：对每一个子区域，以Euclidean clustering算法，即欧几里得聚类算法对点云进行分割；不同区域运用该区域内最大的欧几里得距离阈值

来完成点云分割；点到激光雷达中心的距离用表示为d，激光雷达的最大垂直分辨率表示为θ，则将每个区域内距离d最大值代入阈值Th_d估计公式中即可获得其对应的最大欧几里得距离阈值

阈值Th_d的估计公式如下所示：

其中tan(·)函数表示正切函数。

所述步骤S4中结合动态点检测与点云分割结果进行动态物体提取的方法包括：

步骤S4.1：根据激光雷达三维数据中点索引的唯一特性，对动态点进行归簇，也就是寻找每一个动态点所对应的点云簇，即点云分割结果；

步骤S4.2：记录每一簇中点的总数Q_c与该簇中动态点的个数Q_d，通过每个簇中动态点的占比R来判断该簇是否为动态物；若动态物提取的阈值表示为Th_R,则动态物提取过程如下公式所示：

R＞Th_R 公式十一。

所述步骤S5中的静态地图重建的方法包括：

步骤S5.1：设定第一帧为世界坐标系，将之后点云通过里程计转换到世界坐标系上，重建三维点云地图，第t帧在激光雷达坐标系下的静态点云数据表示为

第t帧到世界坐标系的里程计为O_t:1，第t帧在激光雷达坐标系下的点云数据转换到世界坐标系下数据表示为

则将第t帧点云数据更新到地图中的过程公式如下所示：

步骤S5.2：利用octomap算法对新进的点云数据进行八叉树地图更新。设t＝1,...,T时刻，对应观测到的数据为z₁,...,z_T，则第n个叶子点到T时刻记录的全部信息P(n|z_1:T)为：

其中P(n|z_1:T-1)表示第n个叶子点到T-1时刻记录的全部信息，P(n|z_T)表示第n个叶子在T时刻记录的信息，P(n)表示一个固定量的先验概率。

所述八叉树地图工具为octomap。

实施例：

以下用一个具体的应用实例对本发明的操作进行详细说明。

1)输入图像的分辨率为2048*1024，点云的垂直视角为+2\-24.8度、水平视角为360度；制作一个外形尺寸为600*450mm、格子尺寸为75*75mm、阵列为8*6的黑白格子棋盘，标定出激光雷达坐标到相机坐标的变换阵[R_l2c,T_l2c]为：

2)在动态物提取过程中，其结果如图7所示，其中第一列为对应的图像，第二列对应的是无点云分割辅助下的动态物提取结果，第三列对应的是在点云分割辅助下的动态物提取结果，可以看出来漏检问题得到了很大的改善，特别的，第三行表示非刚性物体存在部分运动的情况下，该系统也可以很好的将整个非刚性物体提取出来。从第一行至第三行对应的角度阈值参数Th_θ、模长差概率阈值参数Th_g、完整度提升程度C_d如下表1所示：

表1

其中完整度提升程度的度量方式为：

式中num_a表示改进后动态物点云总点数、num_b表示改进前动态物点云总点数。

3)在静态地图重建过程中，其结果如图8所示，从上至下，第一张图表示没有进行动态物提取操作的建图结果，可以看到有着一系列人影残留在地图上；第二张图表示经过动态物体移除处理的静态地图重建，其中一种包含了105个动态物体，准确提取出了98个。Octomap(八叉树地图)的参数设定为：最小分辨率为0.05，三维占据网格被击中概率ProbHit为0.7，三维空闲网格概率ProbMiss为0.4。

以上实验结果均表明了本发明的合理性和有效性。

以上所述为本发明的较佳实施方式，凡依本发明申请专利范围所做的均等变化与修饰，皆应属本发明的涵盖范围。

Claims (2)

1.动态环境混合视觉系统的动态物检测和静态地图重建方法，所述混合视觉系统包括以移动物体承载的全景相机和三维激光雷达，其特征在于：所述方法包括以下步骤；

步骤S1：对混合视觉系统进行外参标定，获取全景相机和三维激光两传感器之间的坐标变换参数，作为混合视觉系统的外参数；

步骤S2：将第t帧点云作为特征点投射到第t帧图像上，利用光流算法获取特征点的像素运动向量，并估算因小车自我运动而引起的特征点的人工运动向量来进行背景运动补偿，从而获得点云中动态点；

步骤S3：利用改进的划区域欧几里得聚类算法，对当前帧点云进行簇分割；

步骤S4：利用点云数据中每个点索引唯一特性，结合动态点检测结果与点云欧几里得分割结果，通过簇中动态点的占比对动态簇进行判断，从而提取出动态物体；

步骤S5：利用八叉树地图工具和该帧下的激光雷达里程计，对静态地图进行重建；

所述步骤S3中的点云分割的方法包括：

步骤S3.1：以激光雷达坐标原点为圆心，第一个圆半径为15m，之后以半径差为15m做同心圆，对已去除地面点的点云数据进行区域划分；

步骤S3.2：对每一个子区域，以Euclidean clustering算法，即欧几里得聚类算法对点云进行分割；不同区域运用该区域内最大的欧几里得距离阈值

来完成点云分割；点到激光雷达中心的距离用表示为d，激光雷达的最大垂直分辨率表示为θ，则将每个区域内距离d最大值代入阈值Th_d估计公式中获得其对应的最大欧几里得距离阈值

阈值Th_d的估计公式如下所示：

其中tan(·)函数表示正切函数；

所述步骤S4中结合动态点检测与点云分割结果进行动态物体提取的方法包括：

步骤S4.1：根据激光雷达三维数据中点索引的唯一特性，对动态点进行归簇，也就是寻找每一个动态点所对应的点云簇，即点云分割结果；

步骤S4.2：记录每一簇中点的总数Q_c与该簇中动态点的个数Q_d，通过每个簇中动态点的占比R来判断该簇是否为动态物；若动态物提取的阈值表示为Th_R,则动态物提取过程如下公式所示：

R＞Th_R公式十一；

所述步骤S5中的静态地图重建的方法包括：

步骤S5.1：设定第一帧为世界坐标系，将之后点云通过里程计转换到世界坐标系上，重建三维点云地图，第t帧在激光雷达坐标系下的静态点云数据表示为

第t帧到世界坐标系的里程计为O_t:1，第t帧在激光雷达坐标系下的点云数据转换到世界坐标系下数据表示为

则将第t帧点云数据更新到地图中的过程公式如下所示：

步骤S5.2：利用octomap算法对新进的点云数据进行八叉树地图更新；设t＝1,...,T时刻，对应观测到的数据为z₁,...,z_T，则第n个叶子点到T时刻记录的全部信息P(n|z_1:T)为：

其中P(n|z_1:T-1)表示第n个叶子点到T-1时刻记录的全部信息，P(n|z_T)表示第n个叶子在T时刻记录的信息，P(n)表示一个固定量的先验概率；

所述移动物体为移动小车平台；所述全景相机的相机坐标系中，相机坐标与拍摄图像坐标一致，全景相机无须相机内参标定；所述步骤S1包括；

步骤S1.1：将全景相机和三维激光雷达固定在移动小车平台上面，确保两者的相对位置不变；

步骤S1.2：调整棋盘格位置使其在全景相机传感器和三维激光雷达传感器中均可成像，并且成像位置位于图像的中层区域处；分别拍摄两幅棋盘格图像，之后利用PnP和Levenberg-Marquardt优化方法完成混合视觉系统外参标定，获得激光到相机的外参数矩阵[R_l2c,T_l2c]，下标l2c表示激光雷达到相机的坐标变换；

所述步骤S2包括；

步骤S2.1：利用RANSAC算法进行第t帧点云地面过滤，过滤后的点云在激光雷达下的坐标为p^l＝[x^l,y^l,z^l]^T，其对应在相机坐标系下的坐标为p^c＝[x^c,y^c,z^c]^T，其在第t帧图像上像素点坐标为pⁱ＝[u,v]^T，图像的高度为H，宽度为W,其中上标l、c和i分别表示激光雷达坐标系、相机坐标系和图像坐标系，则从激光雷达坐标p^l转换到图像坐标pⁱ的公式如下为：

p^c＝[R_l2c,T_l2c]p^l 公式一

其中int(·)函数表示取整，arctan2(·)函数是将arctan(·)反正切函数的值域从

扩展到(-π,π)；

步骤S2.2：步骤S2.1中将点云数据投射到对应图像上作为特征点，利用光流算法，计算得特征点的像素运动向量v_f；

步骤S2.3：进行人工运动向量v_o的估计；方法是先利用loam算法从点云数据中估计移动小车的里程计，经过转换后得帧间里程计O_t:t+1，再将去除地面点的第t帧点云数据p_t乘上帧间里程计获得新的点云数据p’_t，最后分别将数据p’_t和p_t投射到图像上获取的像素值，其相减的结果作为人工运动向量v_o；公式如下所示：

O_t:t+1＝O_t+1:1 ^-1O_t:1 公式四

p’_t＝O_t:t+1p_t 公式五

v_o＝T(p’_t)-T(p_t) 公式六

其中O_t:1和O_t+1:1分别是第t帧到第一帧的里程计和第t+1帧到第一帧的里程计，T(·)表示点云数据到图像的投射函数；

步骤S2.4：进行点云数据中动态点检测；方法为，假设人工运动向量的核心为该帧点云数据所有点都是静态的，同时图像静态背景光流表示着移动小车的运动状态，因此由光流法估计的像素运动向量v_f与人工运动向量v_o在运动区域上两者有着明显的差异、在静态区域上两者相似；

进一步对移动物体的运动情况进行分析，设移动物体的运动情况归结为两类：第一类为移动物体的运动方向与小车运动方向平行且反向，此时像素运动向量v_f与人工运动向量v_o之间的夹角约为0，且其两者模长存在着明显的差异；第二类为除去第一类运动情况以外的其他任意运动，此时v_f和v_o向量间的夹角存在着明显差异；

针对上诉移动物体的两种运动情况，使用与之对应的约束条件来检测出动态点，首先以向量间夹角阈值Th_θ为动态点的过滤条件1，大于阈值Th_θ的点将被当做动态点；对小于阈值Th_θ的点，以两者的向量差的模长为过滤条件2，进一步检测出第一类运动情况下的动态点，为了归一化向量差的模长，基于高斯函数模型的动态点概率被运用到该系统中，当概率p小于阈值Th_g时，则判定为动态点；概率p评估公式如下：

其中σ²是小于阈值Th_θ的所有点的||v_f-v_o||的方差，||·||是向量的模长，模长差越大时，则概率p值越小；动态点检测过程分为两种情况，满足时则断定为动态点，如下公式所示：

其中＜v_f,v_o＞表示两向量间的夹角值；

为了提升检测动态点的鲁棒性，采用基于直方图统计的自适应阈值调整方法；该方法中，首先对像素运动向量v_f与人工运动向量v_o之间的夹角进行直方图统计，将夹角范围0-π以0.2弧度为间隔划分，针对直方图统计结果，取其第一次低谷位置处的值作为该帧下夹角阈值Th_θ。

2.根据权利要求1所述的动态环境混合视觉系统的动态物检测和静态地图重建方法，其特征在于：所述八叉树地图工具为octomap。

Images