CN110515088B - 一种智能机器人的里程计估计方法及系统 - Google Patents

Abstract

本发明公开一种智能机器人的里程计估计方法及系统，包括对智能机器人上的相机与激光雷达之间的外参进行标定；获取相机视场图像与激光雷达视场图像，建立智能机器人基于残差的里程计算模型；在相机视场图像与激光雷达视场图像共视区选择具有显著特征的特征点作为路标点，将与特征点距离最近的激光落点的深度作为该特征点的深度坐标，从而获取路标点的三维坐标；根据路标点的三维坐标与里程计算模型获取里程计。利用共视区具备显著特征的特征点直接和间接的图像特征获取路标点，使路标点的数量更多，通过共视区内与特征点重合或近似重合的激光落点在激光雷达视场图像中的深度获得多个路标点的三维坐标，实现里程计估计的高准确性与鲁棒性。

Description

一种智能机器人的里程计估计方法及系统

技术领域

本发明涉及机器人测量技术领域，具体是一种智能机器人的里程计估计方法及系统。

背景技术

随着智能移动机器人的应用领域的扩大，人们期望智能移动机器人在更多领域为人类服务，代替人类完成更复杂的工作。然而,智能移动机器人所处的环境往往是未知的、很难预测，首要考虑的问题就是自主导航，而定位问题是移动机器人导航中的核心问题之一,是实现机器人路径规划任务的前提。在移动机器人导航中，无论是局部实时避障还是全局规划，都需要精确知道机器人或障碍物的当前状态及位置，即里程计，以完成导航、避障及路径规划等任务。现有技术中的里程计估计过程中大多通过利用卡尔曼滤波器的方法进行多传感器融合。这种基于滤波器的方法只能在小的场景的情况下使用，对于大的地图场景会因为特征向量的维度过大导致计算时间的急剧上升，从而不能保证实时性，更致命的是，对于长时间的运动，该方法由于误差的累积会导致状态不确定度的上升，从而也就导致了该方法的准确性与鲁棒性均不强。

发明内容

本发明提供一种智能机器人的里程计估计方法及系统，用于克服现有技术中里程计估计准确性与鲁棒性均不强等缺陷，实现里程计估计的高准确性与鲁棒性。

为实现上述目的，本发明提供一种智能机器人的里程计估计方法，包括如下步骤：

步骤101，对智能机器人上的相机与激光雷达之间的外参进行标定；

步骤102，获取完成外参标定后的相机视场图像与激光雷达视场图像，建立智能机器人基于残差的里程计算模型；

步骤103，在相机视场图像与激光雷达视场图像共视区选择若干具有显著特征的特征点作为路标点，将与所述特征点距离最近的激光落点的深度作为该特征点的深度坐标，从而获取所有路标点的三维坐标；

步骤104，根据所述路标点的三维坐标与里程计算模型获取智能机器人的里程计。

进一步优选的，步骤101具体包括：

步骤201，根据相机与激光雷达之间的运动约束获取相机与激光雷达之间外参的初始估计；

步骤202，根据相机与激光雷达之间的互信息特征以及外参的初始估计获取相机与激光雷达之间外参的优化估计。

进一步优选的，步骤201具体包括：

步骤301，获取相机与激光雷达的运动轨迹；

步骤302，基于相机与激光雷达的运动约束与运动轨迹建立高斯赫尔默特模型；

步骤303，对高斯赫尔默特模型进行最小二乘估计，获取相机与激光雷达之间外参的初始估计。

进一步优选的，步骤301中，所述相机与激光雷达的运动轨迹为：

式中，r_ai为激光雷达的旋转轨迹，t_ai为激光雷达的平移轨迹，r_ci为相机的旋转轨迹，λt_ci为相机的平移轨迹，其中，λ为相机的未知尺度因子，λ的求取过程为：

若限定智能机器人做平移运动，此时相机与激光雷达也做同步的平移运动，则有：

||r_ci||＝||r_ai||＝0,||λt_ci||＝||t_ai||

进而得到：

令上式的一阶导数为0，则有：

最终得到：

式中，M表示相机与激光雷达平移运动的段数，||·||表示二范数。

进一步优选的，步骤302的过程具体为：

相机与激光雷达的运动约束为同步平移与旋转，则有：

式中，g_t(x,l_i)表示限定相机与激光雷达之间的旋转约束，g_r(x,l_i)表示限定相机与激光雷达之间的平移约束，R(·)表示相应角度轴矢量的旋转矩阵，x＝[η,ξ]，其中，x表示相机与激光雷达之间的外参，η表示相机与激光雷达之间外参中的旋转参数，ξ表示相机与激光雷达之间外参中的平移参数；

在无噪声的情况下,外参x满足：

式中，x*表示无噪声的情况下的外参；

获取相机、激光雷达实际运动轨迹与无噪声情况下运动轨迹的误差：

式中，

表示相机、激光雷达在无噪声情况下运动轨迹，ε_i表示相机、激光雷达实际运动轨迹与无噪声情况下运动轨迹的误差，其满足正太分布；

建立高斯赫尔默特模型：

subject to g_t(x,ε_i+l_i)＝0

g_r(x,ε_i+l_i)＝0

式中，

表示误差ε_i协方差矩阵的逆。

进一步优选的，步骤202具体包括：

步骤401，根据相机与激光雷达之间外参的初始估计建立相机与激光雷达之间的投影模型；

步骤402，利用投影模型获取相机与激光雷达的共视点；

步骤403，获取每一个共视点的灰度值与反射率，进而获得所有共视点的灰度值直方图与反射率直方图；

步骤404，获取灰度值直方图与反射率直方图的相似度，调整相机与激光雷达之间的外参估计使得灰度值直方图与反射率直方图的相似度最大，此时相机与激光雷达之间的外参估计即为外参的优化估计。

进一步优选的，步骤402还包括剔除共视点中的遮挡点，剔除遮挡点的过程为：

步骤501，对所有的共视点进行从1～N的编号，其中，N为共视点的总数；

步骤502，获取相机视场下的相机共视点序列与机关雷达视场下的激光雷达共视点序列，其中，相机共视点序列与激光雷达共视点序列的排列方式相同；

步骤503，以相机共视点序列与激光雷达共视点序列中的一个作为标准序列，另一个作为判定序列；

步骤504，若标准序列与判定序列相同，则判定无遮挡点，否则判定存在遮挡点，并提取出判定序列中所有相对于标准序列存在相互交换位置情况的点集组；

步骤505，判定点集组中深度值较大的点集中的点为遮挡点，将其从共视点中剔除。

进一步优选的，步骤103具体包括：

步骤601，获取相机视场图像中与激光雷达视场图像共视区二维的特征点；

步骤602，在所有的特征点中筛选出满足深度插值条件的特征点作为待插值点；

步骤603，获取与待插值点距离最短的激光落点，并根据所述激光落点在激光雷达视场图像中的位置获得该激光落点的深度，将该所述激光落点的深度插值于该激光落点对应的待插值点，即获取三维的路标点。

进一步优选的，步骤601具体包括：

步骤701，基于光度信息在相机视场图像内提取第一类特征点；

步骤702，基于ORB算法在相机视场图像内提取第二类特征点；

步骤703，合并第一类特征点与第二类特征点，即为相机视场图像中二维的特征点。

一种智能机器人的里程计估计系统，包括：存储器和处理器，所述智能机器人的里程计估计程序，所述处理器在运行所述程序时执行上述方法所述的步骤。

本发明提供的一种智能机器人的里程计估计方法及系统，基于相机视场图像和激光雷达视场图像的特征融合，利用共视区具备显著特征的特征点直接和间接的图像特征获取路标点，使得路标点的数量更多，通过共视区内与特征点重合或近似重合的激光落点在激光雷达视场图像中的深度获得多个路标点的三维坐标，实现里程计估计的高准确性与鲁棒性。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的附图。

图1为本发明实施例中智能机器人的里程计估计方法的流程示意图；

图2为本发明实施例中相机与激光雷达之间外参标定的流程示意图；

图3为本发明实施例中相机与激光雷达之间外参的初始估计的流程示意图；

图4为本发明实施例中相机与激光雷达之间外参的优化估计的流程示意图；

图5为本发明实施例中剔除遮挡点的流程示意图；

图6为本发明实施例中剔除遮挡点的示例结构图；

图7为本发明实施例中路标点获取过程流程示意图；

图8为本发明实施例中的仿真示例图。

本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要说明，本发明实施例中所有方向性指示(诸如上、下、左、右、前、后……)仅用于解释在某一特定姿态(如附图所示)下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。

另外，在本发明中如涉及“第一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。在本发明的描述中，“多个”的含义是至少两个，例如两个，三个等，除非另有明确具体的限定。

在本发明中，除非另有明确的规定和限定，术语“连接”、“固定”等应做广义理解，例如，“固定”可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接，还可以是物理连接或无线通信连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系，除非另有明确的限定。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。

另外，本发明各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。

如图1所示的一种智能机器人的里程计估计方法，包括如下步骤：

步骤101，对智能机器人上的相机与激光雷达之间的外参进行标定；

步骤102，获取完成外参标定后的相机视场图像与激光雷达视场图像，建立智能机器人基于残差的里程计算模型；

步骤103，在相机视场图像与激光雷达视场图像共视区选择若干具有显著特征的特征点作为路标点，将与所述特征点距离最近的激光落点的深度作为该特征点的深度坐标，从而获取所有路标点的三维坐标；

步骤104，根据所述路标点的三维坐标与里程计算模型获取智能机器人的里程计。

本实施例基于相机视场图像和激光雷达视场图像的特征融合，利用共视区具备显著特征的特征点直接和间接的图像特征获取路标点，使得路标点的数量更多，通过共视区内与特征点重合或近似重合的激光落点在激光雷达视场图像中的深度获得多个路标点的三维坐标，实现里程计估计的高准确性与鲁棒性。

步骤101中，相机与激光雷达之间的外参指的是相机与激光雷达之间做同步运动时的相对姿态，这里的姿态包括3自由度的旋转和3自由度的平移，参考图2，具体包括如下步骤：

步骤201，根据相机与激光雷达之间的运动约束获取相机与激光雷达之间外参的初始估计；

步骤202，根据相机与激光雷达之间的互信息特征以及外参的初始估计获取相机与激光雷达之间外参的优化估计。

通过将相机与激光雷达之间外参的标定分为两个阶段，第一阶段利用相机与激光雷达之间的运动约束,第二阶段利用相机与激光雷达测量的互信息，在没有初始值的情况下提供精确的外参估计，有效的提高了标定的准确性和鲁棒性。

参考图3,步骤201中，获取相机与激光雷达之间外参的初始估计具体包括：

步骤301，使得相机与激光雷达设定为时间同步，具体可以通过硬件触发器来同时触发相机与激光雷达，其中相机的内参(即焦距、相机中心、镜头失真系数)是预校准的，当智能机器人移动时，相机与激光雷达跟随智能机器人移动，进而获取此时相机与激光雷达的运动轨迹；

由于相机与激光雷达都是跟随智能机器人运动，因此相机与激光雷达之间的运动同步，因此满足运动之间的约束方程：Ax＝Bx，其中，A表示相机的运动轨迹，B表示激光雷达的运动轨迹，x表示相机与激光雷达之间的外参。

本实施例中，相机与激光雷达的运动轨迹表示为：

式中，r_ai为激光雷达的旋转轨迹，t_ai为激光雷达的平移轨迹，r_ci为相机的旋转轨迹，λt_ci为相机的平移轨迹，λ为相机的未知尺度因子；

旋转轨迹r_ai、r_ci与平移轨迹t_ai、t_ci的估算方式可以采用现有激光和视觉里程计方法，因此本实施例中不再赘述，本实施例中的相机为单目相机，由于单目相机无法确定目标的距离，因此在相机的平移轨迹中含有未知尺度因子λ，本实施例中首先需要求取相机的未知尺度因子，求取过程为：

首先限定智能机器人做平移运动，此时相机与激光雷达也做同步的平移运动，则有：

||r_ci||＝||r_ai||＝0,||λt_ci||＝||t_ai||

进而得到：

令上式的一阶导数为0来进行求解，则有：

最终得到：

式中，M表示相机与激光雷达平移运动的段数，||·||表示二范数；

随后控制智能机器人做六自由度的运动，即能估算出相机与激光雷达各自的运动轨迹。

步骤302，基于相机与激光雷达的运动约束与运动轨迹建立高斯赫尔默特模型，其具体过程为：

相机与激光雷达的运动约束为同步平移与旋转，则有：

式中，g_t(x,l_i)表示限定相机与激光雷达之间的旋转约束，g_r(x,l_i)表示限定相机与激光雷达之间的平移约束，R(·)表示相应角度轴矢量的旋转矩阵，x＝[η,ξ]，其中，x表示相机与激光雷达之间的外参，η表示相机与激光雷达之间外参中的旋转参数，ξ表示相机与激光雷达之间外参中的平移参数；

在无噪声的情况下,外参x满足：

式中，x*表示无噪声的情况下的外参；

获取相机、激光雷达实际运动轨迹与无噪声情况下运动轨迹的误差：

式中，

表示相机、激光雷达在无噪声情况下运动轨迹，ε_i表示相机、激光雷达实际运动轨迹与无噪声情况下运动轨迹的误差，其满足正太分布；

因此能够建立高斯赫尔默特模型：

subject to g_t(x,ε_i+l_i)＝0

g_r(x,ε_i+l_i)＝0

式中，

表示误差ε_i协方差矩阵的逆。

步骤303，对高斯赫尔默特模型进行最小二乘估计，获取相机与激光雷达之间外参的初始估计，其中，对高斯赫尔默特模型进行最小二乘估计的具体求解过程可以参照文献“K.Huang and C.Stachniss,“Extrinsic multi-sensor calibration for mobilerobots using the gauss-helmert model,”in Proc.of the IEEE/RSJ Intl.Conf.onIntelligent Robots and Systems(IROS),2017,pp.1490–1496.”。

步骤102中，对于由激光雷达和摄像机共同观察到的场景点，即本实施例中的共视点，可以获取三种类型的测量：其深度、反射率和图像强度。激光雷达能够提供共视点的深度和反射率信息，反射率测量红外脉冲通过共识点反射到激光雷达接收器的百分比。这种反射率信息通常与真实环境中的图像强度信息类似。因此，可以通过使用互信息的方法度量反射率信息校和图像强度信息的相关性,以进一步细化相机与激光雷达之间的外参估计，参考图4，具体过程为：

步骤401，根据相机与激光雷达之间外参的初始估计建立相机与激光雷达之间的投影模型；

步骤402，利用投影模型获取相机与激光雷达的共视点其过程为：

首先利用投影模型对激光雷达的激光落点进行投影转换：

c＝π(K(R(η)·s+ξ))

式中，s表示激光雷达的激光落点，c表示经过投影模型投影转换后的激光雷达的激光落点，K表示相机的内部参数决定的相机矩阵，π(·)表示欧式标准化，它将激光落点由三位坐标转换为二维的图像坐标；

筛选出位于相机视场内的经过投影模型投影转换后的激光雷达的激光落点，即为相机与激光雷达的共视点；

在获取共视点后，由于相机的视场图像为一幅完整的图像，而激光雷达的落点图像为若干行顺序排列的激光落点，因此正常情况下，相机与激光雷达的共视点也应为若干行顺序排列的点，但是由于激光雷达和相机的不同的拍摄角度可能使得场景点被遮挡，即遮挡点，使得共视点的排列顺序被破坏，进而影响后续计算过程的精度，因此还需要剔除共视点中的遮挡点，参考图5，本实施例中剔除遮挡点的过程为：

步骤501，对所有的共视点进行从1～N的编号，其中，N为共视点的总数；

步骤502，获取相机视场下的相机共视点序列与机关雷达视场下的激光雷达共视点序列，其中，相机共视点序列与激光雷达共视点序列的排列方式相同；

步骤503，以相机共视点序列与激光雷达共视点序列中的一个作为标准序列，另一个作为判定序列；

步骤504，若标准序列与判定序列相同，则判定无遮挡点，否则判定存在遮挡点，并提取出判定序列中所有相对于标准序列存在相互交换位置情况的点集组；

步骤505，判定点集组中深度值较大的点集中的点为遮挡点，将其从共视点中剔除。

例如，参考图6，在相机视场下的相机共视点序列为(1、2、3、4、5)，而在机关雷达视场下的激光雷达共视点序列为(1、2、5、3、5)，可以显而易见的看出点集(5)与点集(3、4)交换了位置，因此可以判断为此处出现了遮挡点，由于点集(3、4)的深度值大于点集(5)，因此可以判断为点3与点4为遮挡点，因此将点3与点4从共视点钟剔除。

步骤403，获取每一个共视点的灰度值与反射率，进而获得所有共视点的灰度值直方图与反射率直方图；

步骤404，获取灰度值直方图与反射率直方图的相似度，调整相机与激光雷达之间的外参估计使得灰度值直方图与反射率直方图的相似度最大，此时相机与激光雷达之间的外参估计即为外参的优化估计：

灰度值直方图与反射率直方图的相似度为：

MI(S,C)＝H(S)+H(C)-H(S,C)

式中，MI(S,C)表示灰度值直方图与反射率直方图的相似度，S表示共视点的反射率，C表示共视点的灰度值，H(S)表示反射率的不确定性，H(C)表示灰度值的不确定性，H(S,C)表示反射率与灰度值共同观察时的不确定性，上述灰度值直方图与反射率直方图的相似度的获取过程具体可以参考文献“G.Pandey,J.R.McBride,S.Savarese,and R.M.Eustice,“Automatic extrinsic calibration of vision and lidar by maximizing mutualinfor-mation,”Journal of Field Robotics(JFR),vol.32,no.5,pp.696–722,2015”；

因此相机与激光雷达之间外参的优化估计为：

式中，x为相机与激光雷达之间外参的初始估计，x′为相机与激光雷达之间外参的初优化估计。

经过上述过程完成对相机与激光雷达之间外参的标定后，即能通过标定完成后的相机与激光雷达完成智能机器人里程计的估计，具体过程如下：

步骤102中，建立基于残差的里程计算模型为：

式中，e_rep,i表示重投影残差，e_pho,j表示光度测量残差，

和

表示观测噪声协方差，a_on表示世界坐标系下已添加的路标点，a_wn表示图像中对应的路标点，φ表示相对运动，其中，观测噪声协方差通常是根据观测值分布在线估计的，其推到过程可以参考文献“Q.Yu,J.Xiao,H.Lu,and Z.Zheng,“Hybrid-residual-based rgbd visual odometry,”IEEE Access Journal,2018.”。

本实施例中的里程计算模型的实质为基于高斯声假设的相机运动的概率估计模型，由于相机安置在智能机器人上并跟随智能机器人移动，因此通过相机运动的概率估计模型即能获取智能机器人的里程计估计。

步骤103中，在相机视场图像与激光雷达视场图像共视区选择若干具有显著特征的特征点作为路标点，将与所述特征点距离最近的激光落点的深度作为该特征点的深度坐标，从而获取所有路标点的三维坐标：具有激光雷达说射出激光落点的强度值和深度测量值的像素均可能是路标点，然而,并非所有激光落点都是突出和稳定的。为了确保鲁棒性,本实施例通过使用强度显著性来选择路标点，强度显著性由强度梯度测量,具有较大强度梯度值的像素被认为更显著、更鲁棒，因此,本实施例从梯度图像中提取显著像素。为此,本实施例构造了基于强度梯度的梯度直方图，然后,根据梯度直方图从大到小提取路标点,直到路标点的数量大于预设阈值，这样,即能保证提取的路标点足够,并且都具有最大的强度梯度值，假设环境的照明是不变的,则不同图像中同一路标点的灰度值将相同，因此,将灰度值设置为路标点的强度值，参考图7，步骤103的实现过程具体包括：

步骤601获取相机视场图像中与激光雷达视场图像共视区二维的特征点，其中，特征点指的是相机视场图像中的关键点，本实施例通过两种方式来确定特征点，一种方式是基于光度信息在相机视场图像内提取第一类特征点，即提取相机视场图像中灰度梯度大于一定阈值的点作为第一类特征点，同样的也能够采用其他的得方式提取第一类特征点，例如亮度，对比度等；另一种方式是基于ORB算法(Oriented FAST and Rotated BRIEF，快速特征点提取和描述的算法)在相机视场图像内提取第二类特征点，最后合并第一类特征点与第二类特征点，作为相机视场图像中总的二维特征点，通过两种方式来提取特征点，使得特征点的提取更加全面，同时在一定程度上增加特征点的数量，进而提升后续计算过程中里程计估计的准确性与鲁棒性；

步骤602，在所有的特征点中筛选出满足深度插值条件的特征点作为待插值点，本实施例中基于强度梯度的梯度直方图在特征点中选取待插值点，具体为满足下式的特征点为待插值点：

式中，p(c_j,c_i)表示待插值点的置信度，c_i表示所有的图像点，c_j表示激光与相机的共识点，(u,v)表示点的像素坐标，(r,g,b)表示像素点的RGB颜色信息，

表示像素点的方差，

表示像素颜色的方差；

步骤603，在两个视场图像共视区获取与待插值点距离最短的激光落点，并根据所述激光落点在激光雷达视场图像中的位置获得该激光落点的深度，将该所述激光落点的深度插值于该激光落点对应的待插值点，即获取三维的路标点。

最终根据路标点的三维坐标与里程计算模型即能获取智能机器人的里程计里程计估计。如图8所示的是依据本实施例中的里程计估计方法得到的运动轨迹生成的点云地图，以及对应场景的俯瞰照片。

其轨迹的平均位移误差为2％，相比于现有的单一传感器方法均有显著提升。证明本发明使用的混合残差方法能够有效提升里程计估计精度。

以上所述仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是在本发明的发明构思下，利用本发明说明书及附图内容所作的等效结构变换，或直接/间接运用在其他相关的技术领域均包括在本发明的专利保护范围内。

Claims (9)

1.一种智能机器人的里程计估计方法，其特征在于，包括如下步骤：

步骤101，对智能机器人上的相机与激光雷达之间的外参进行标定；

步骤102，获取完成外参标定后的相机视场图像与激光雷达视场图像，建立智能机器人基于残差的里程计算模型；

步骤103，在相机视场图像与激光雷达视场图像共视区选择若干具有显著特征的特征点作为路标点，将与所述特征点距离最近的激光落点的深度作为该特征点的深度坐标，从而获取所有路标点的三维坐标；

步骤104，根据所述路标点的三维坐标与里程计算模型获取智能机器人的里程计；

步骤103具体包括：

步骤601，获取相机视场图像中与激光雷达视场图像共视区二维的特征点；

步骤602，在所有的特征点中筛选出满足深度插值条件的特征点作为待插值点；

步骤603，获取与待插值点距离最短的激光落点，并根据所述激光落点在激光雷达视场图像中的位置获得该激光落点的深度，将该所述激光落点的深度插值于该激光落点对应的待插值点，即获取三维的路标点。

2.根据权利要求1所述智能机器人的里程计估计方法，其特征在于，步骤101具体包括：

步骤201，根据相机与激光雷达之间的运动约束获取相机与激光雷达之间外参的初始估计；

步骤202，根据相机与激光雷达之间的互信息特征以及外参的初始估计获取相机与激光雷达之间外参的优化估计。

3.根据权利要求2所述智能机器人的里程计估计方法，其特征在于，步骤201具体包括：

步骤301，获取相机与激光雷达的运动轨迹；

步骤302，基于相机与激光雷达的运动约束与运动轨迹建立高斯赫尔默特模型；

步骤303，对高斯赫尔默特模型进行最小二乘估计，获取相机与激光雷达之间外参的初始估计。

4.根据权利要求3所述智能机器人的里程计估计方法，其特征在于，步骤301中，所述相机与激光雷达的运动轨迹为：

式中，r_ai为激光雷达的旋转轨迹，t_ai为激光雷达的平移轨迹，r_ci为相机的旋转轨迹，λt_ci为相机的平移轨迹，其中，λ为相机的未知尺度因子，λ的求取过程为：

若限定智能机器人做平移运动，此时相机与激光雷达也做同步的平移运动，则有：

||r_ci||＝||r_ai||＝0,||λt_ci||＝||t_ai||

进而得到：

令上式的一阶导数为0，则有：

最终得到：

式中，M表示相机与激光雷达平移运动的段数，||·||表示二范数。

5.根据权利要求3所述智能机器人的里程计估计方法，其特征在于，步骤302的过程具体为：

相机与激光雷达的运动约束为同步平移与旋转，则有：

式中，g_t(x,l_i)表示限定相机与激光雷达之间的旋转约束，g_r(x,l_i)表示限定相机与激光雷达之间的平移约束，R(·)表示相应角度轴矢量的旋转矩阵，x＝[η,ξ]，其中，x表示相机与激光雷达之间的外参，η表示相机与激光雷达之间外参中的旋转参数，ξ表示相机与激光雷达之间外参中的平移参数；

在无噪声的情况下,外参x满足：

式中，x*表示无噪声的情况下的外参；

获取相机、激光雷达实际运动轨迹与无噪声情况下运动轨迹的误差：

式中，

表示相机、激光雷达在无噪声情况下运动轨迹，ε_i表示相机、激光雷达实际运动轨迹与无噪声情况下运动轨迹的误差，其满足正态分布；

建立高斯赫尔默特模型：

subject to g_t(x,ε_i+l_i)＝0

g_r(x,ε_i+l_i)＝0

式中，

表示误差ε_i协方差矩阵的逆。

6.根据权利要求2所述智能机器人的里程计估计方法，其特征在于，步骤202具体包括：

步骤401，根据相机与激光雷达之间外参的初始估计建立相机与激光雷达之间的投影模型；

步骤402，利用投影模型获取相机与激光雷达的共视点；

步骤403，获取每一个共视点的灰度值与反射率，进而获得所有共视点的灰度值直方图与反射率直方图；

步骤404，获取灰度值直方图与反射率直方图的相似度，调整相机与激光雷达之间的外参估计使得灰度值直方图与反射率直方图的相似度最大，此时相机与激光雷达之间的外参估计即为外参的优化估计。

7.根据权利要求6所述智能机器人的里程计估计方法，其特征在于，步骤402还包括剔除共视点中的遮挡点，剔除遮挡点的过程为：

步骤501，对所有的共视点进行从1～N的编号，其中，N为共视点的总数；

步骤502，获取相机视场下的相机共视点序列与机关雷达视场下的激光雷达共视点序列，其中，相机共视点序列与激光雷达共视点序列的排列方式相同；

步骤503，以相机共视点序列与激光雷达共视点序列中的一个作为标准序列，另一个作为判定序列；

步骤504，若标准序列与判定序列相同，则判定无遮挡点，否则判定存在遮挡点，并提取出判定序列中所有相对于标准序列存在相互交换位置情况的点集组；

步骤505，判定点集组中深度值较大的点集中的点为遮挡点，将其从共视点中剔除。

8.根据权利要求1所述智能机器人的里程计估计方法，其特征在于，步骤601具体包括：

步骤701，基于光度信息在相机视场图像内提取第一类特征点；

步骤702，基于ORB算法在相机视场图像内提取第二类特征点；

步骤703，合并第一类特征点与第二类特征点，即为相机视场图像中二维的特征点。

9.一种智能机器人的里程计估计系统，包括：存储器和处理器，所述智能机器人的里程计估计程序，所述处理器在运行所述程序时执行所述权利要求1～8任一项方法所述的步骤。

Images