[发明专利]一种基于智能机器人地面特征的PTAM改进方法

摘要

一种基于智能机器人地面特征的PTAM改进方法，首先，完成参数校正，这包括参数定义与相机校正；然后利用相机获取当前环境纹理信息，并构建四层高斯图像金字塔，运用FAST角点检测算法提取当前图像中的特征信息，建立角点特征间的数据关联，得到位姿估计模型；在地图的初始绘制阶段，获取两个关键帧将相机架设在移动机器人上；在初始化过程中，移动机器人开始移动，同时相机捕获当前场景中角点信息并建立关联；实现三维稀疏地图的初始化后，更新关键帧并利用极线搜索与块匹配方法建立特征点亚像素精度映射关系，结合位姿估计模型实现相机精确重定位。最后将匹配点投影到空间，完成当前全局环境三维地图创建。

主权项

一种基于智能机器人地面特征的PTAM改进方法，其特征在于：首先，完成参数校正，这包括参数定义与相机校正；接着对关键帧中的特征进行跟踪，再实现位姿的初始化，再此基础上对平面参数进行估计；然后利用相机获取当前环境纹理信息，并构建四层高斯图像金字塔，运用FAST角点检测算法提取当前图像中的特征信息，建立角点特征间的数据关联，得到位姿估计模型；在地图的初始绘制阶段，获取两个关键帧将相机架设在移动机器人上，通过相机与地面标定后开始进行初始化过程；在初始化过程中，移动机器人开始移动，同时相机捕获当前场景中角点信息并建立关联，由位姿估计模型获取当前位姿初始估计，再运用基于地面特征的位姿估计方法获取当前位姿的准确估计；实现三维稀疏地图的初始化后，更新关键帧并利用极线搜索与块匹配方法建立特征点亚像素精度映射关系，结合位姿估计模型实现相机精确重定位；最后将匹配点投影到空间，完成当前全局环境三维地图创建；同时利用全局与局部集束调整完成算法的一致性优化；具体包括以下步骤：步骤1，参数校正步骤1.1，参数定义(1)机器人位姿在移动机器人起始处建立世界坐标系，在机器人移动过程中，其位姿表示为：Tr=Rrtr01Rr=cosθsinθ0-sinθcosθ0001,tr=xy0---(1)]]>由上式可知移动机器人位姿由x＝(x，y,θ)描述，其中，x，y分别表示移动机器人在x轴，y轴方向的平移量，θ为移动机器人绕z轴的旋转角度；在世界坐标系下，相机的位姿表示为:Tc=Rctc01---(2)]]>其中，Tc∈SE(3)，SE(3)为特殊刚体变换群，Rc为3×3旋转矩阵，tc为3×1平移矩阵；该位姿矩阵建立了世界坐标系与相机坐标系下点pc，pw的对应关系，即pc＝Tcpw；本方法中相机固定在移动机器人上，相机与机器人间存在固定的旋转平移关系Tcr；在机器人移动过程中，若某一时间间隔机器人位姿增量为Trs，与之对应，相机的位姿增量为Tcs，则：Trs＝TrcTcsTcr                               (3)其中，(2)地平面标定参数将相机固定在移动机器人上，并将标定靶标平放在地面；相机通过获取具有标定靶标的图像信息建立相机与机器人间位姿关系；Tcp∈SE(3)为相机与标定靶标间的变换矩阵，Rcp，tcp分别表示Tcp的旋转与平移分量，其逆变换假定机器人的坐标原点与相机保持一致，且机器人的x轴与相机的z轴朝向相同；在靶标坐标系下，机器人的x方向向量由Tcp的第3行向量的前两个分量表示；此时，机器人坐标系与靶标坐标系的变换关系表示为：pr=TrpppTrp=cosγsinγ0xr-sinγcosγ0yr00100001---(4)]]>其中，γ为机器人坐标系与靶标坐标系的旋转角度，(xr,yr)为机器人原点在靶标坐标系下的坐标，该坐标由相机与靶标的平移向量确定；因此机器人与相机间的变换关系表示为Tcr＝TcpTpr，根据相机与靶标平面间的位姿关系，获取在相机坐标系下的地平面方程：(0,0,1,0)Tpcpc＝0                                  (5)其中，pc为相机坐标系下三维点，地平面参数表示为(nT,d)，且n＝(0,0,1)Rpc，d＝tp(3)；步骤1.2，相机校正由于透镜在制造上的原因会存在畸变，畸变主要分为由透镜形状引起的镜向畸变和由摄像机组装过程引起的切向畸变；本方法采用FOV模型实现对单目相机的矫正；该模型是由Devernay和Faugeras提出的一种针对广角相机去畸变方法，其数学描述如下：ud=u0v0+fu00fvrdruxurd=1ωarctan(2rutanω2)ru=tan(rdω)2tanω2---(6)]]>式中，xu是像素归一化坐标，ud是畸变后像素坐标，ω为畸变系数，rd,ru分别为矫正前后归一化坐标到坐标系中心的距离；利用相机模型，将图像像素坐标映射到归一化坐标平面，同时结合相机内参数矩阵K，实现图像畸变矫正，即：u＝Kxu                                  (7)步骤2，基于地面特征的初始化步骤2.1，特征跟踪FAST角点检测是由Edward Rosten和Tom Drummond提出的一种简单快速的角点检测算法；该算法利用像素点某个邻域内相邻像素点灰度值的差异，判断该像素点是否为角点，如下式：N=Σs∈c(uf)|I(s)-I(uf)|---(8)]]>其中，uf为候选角点，s为候选角点邻域内任意一点，c(uf)表示uf的邻域，I(s)为在s处图像灰度值，若N超出阈值条件，s处的候选角点即为角点；在相机跟踪线程前，系统利用相机获取当前环境纹理信息，并构建四层高斯图像金字塔，采用FAST的角点检测算法，结合块搜索的方式建立角点特征间的数据关联，利用投影关系预测三维点与当前帧中特征点的对应关系，并在位置附近固定区域进行搜索，以获取准确的匹配关系；步骤2.2，位姿初始化在机器人移动过程中，通过相机获取地面信息，并利用FAST算法实现角点特征的跟踪；根据上述数据关联信息，建立平面点的单应性关系，即：π(Hur)＝up                                     (9)其中，π(p)为投影函数，p＝(x,y,z),π(p)＝(x/z,y/z,1)，ur，up分别为参考帧与当前帧下对应特征点；根据相机平移量Tc以及地平面方程参数(nT,d)，建立平面点的单应性关系：H＝KTc(I|nd)TK‑1                               (10)其中，nd＝n/d，K为相机内参数矩阵；将式(3)代入式(10)，此时机器人的位姿与地面特征单应性关系描述为：H＝KTcrTrsTrc(I|nd)TK‑1                                (11)利用上述图像特征点的单应性关系，建立位姿估计模型：x=argminxf(x)f(x)=Σp||r(x)||2---(12)]]>其中，r(x)＝π(H(x)uref)‑up为单应性关系误差函数，即投影误差函数，H(x)为单应性矩阵；根据小位移假设，误差函数r的一阶泰勒展开表示为：r(ξ)=r(0)+∂r∂ξ|ξ=0ξ---(13)]]>此时，能量函数的极值条件为：∂f∂ξ=ΣJTr=0---(14)]]>即：ΣpJT(r(0)+Jξ)=0ξ=-(ΣpJTJ)-1ΣpJTr(0)---(15)]]>其中，通过迭代求解上式获取移动机器人的位姿估计；为了进一步提高定位精度，引入鲁棒性权值构建基于M‑estimation的位姿估计模型，此时能量函数表示为：f(x)=Σpw(r)||r||2---(16)]]>其中，w(r)为鲁棒性Tukey加权函数，此时，能量函数的极值条件为：ξ=-(ΣpJTw(r)J)-1ΣpJTw(r)r(0)---(17)]]>步骤2.3，平面参数估计机器人利用基于地面特征的单应性关系实现机器人定位，但由于非地面特征的影响，难以保证定位精度，因此采用一种快速的非地面特征去除方法；根据平行向量叉乘方式构建法向估计模型；由于角点检测存在误差，使得理想叉乘条件无法满足，法向估计模型适应度差；利用投影误差r作为判定依据，从而确定非地面特征信息；但随着非地面特征与观察点间距离减小，该方法区分度明显降低；相比上述方法，在位姿初始估计的基础上，利用单应性矩阵直接推导法向估计模型，通过获取其与地平面方程参数的差异，判别该角点是否为地面特征信息；根据平面单应性关系式，建立平面参数nd的估计模型：f(nd)=Σp||r(nd)||2---(18)]]>其中，r(nd)＝π(H(nd)uref)‑up为相应投影误差函数；与位姿估计求解过程类似，在小位移假设的基础上，根据误差函数的一阶泰勒公式建立式(18)的求解模型；此时，平面参数通过迭代式(18)获取：nd=nd+n^n^=-(ΣpJnTJn)-1ΣpJnTr(nd)---(19)]]>其中，步骤3，位姿估计位姿估计模块的主要功能是完成相机位姿的实时更新；其具体实施过程如下；在完成初始化工作后，根据投影误差，建立位姿估计模型，其数学描述如下：ξ=argminξΣObj(|ej|σj,σT)---(20)]]>其中，ej是投影误差，且为Tukey双权目标函数，σT为特征点匹配标准差的无偏估计值，μ为相机位姿六元组描述向量；对于跟踪中存在的失败情况，提出特征观察比值用以评估每帧图像的跟踪质量；若比值低于设定阈值，为跟踪效果较差，则继续跟踪，但是系统不会将该帧更新到地图中；若在多帧图像的跟踪中都出现了特征观察比值低于设定阈值的情况时，即认为属于“跟丢”的情况，那么就初始化跟踪过程；通过以上步骤实现了位姿的精确估计，跟踪过程得以正常进行；步骤4，地图创建步骤4.1，地图初始化系统利用利用地面特征与标准立体相机算法模型建立当前环境初始化地图；在地图的初始化过程中，利用图像中FAST角点匹配关系，结合RANSAC算法，采用基于地面特征的初始位姿估计方法，运用三角测量法计算当前特征点处的三维坐标，并确定起始点为全局世界坐标系原点，完成三维稀疏地图的初始化；在地图的初始绘制阶段，获取两个关键帧的具体过程如下，将相机架设在移动机器人上，通过相机与地面标定后开始进行初始化过程；在初始化过程中，移动机器人开始移动，同时相机捕获当前场景中角点信息并建立关联，通过求解式(12)获取当前位姿初始估计；当该初始估计超过设定阈值d0时，运用2.2中基于地面特征的位姿估计方法获取当前位姿的准确估计；步骤4.2，关键帧更新与极线搜索地图初始化时，仅仅包含两个关键帧，并且只能对邻近较小的体积空间进行描述，在相机离开初始位置时，需要添加新的关键帧以及地图特征；若相机与当前关键帧间图像帧数超出阈值条件，相机跟踪效果为最佳，且相机与最近的地图关键点距离尽量小时，将自动执行添加关键帧过程；由于实时性的约束，跟踪系统可能只是测度到了帧中可视特征的子集，因此映射线程需要重新计划和测量余下的地图特征；在关键帧的更新中，系统首先将会对新增加的关键帧中所有角点进行Shi‑Tomas评估，获取当前具有显著特征的角点信息，成功观测的特征点附近的显著点将会被丢弃，保留下来的显著性点将作为候选的地图节点；由于新的地图节点需要已知深度信息，仅靠单帧图像无法获取，因此需要选取与之最近的关键帧作为第二视图，利用极线搜索与块匹配方法建立特征点亚像素精度映射关系；在第二视图中，采用零均值SSD法，结合位姿估计模型实现相机精确重定位；最后将匹配点投影到空间，以生成当前全局环境三维地图；步骤4.3，优化利用最优的局部与全局LM集束调整算法实现当前地图全局一致性优化以提高鲁棒性；该集束调整算法的数学描述为：{{μ2...μN},{p′1...p′M}}=argmin{{μ},{p}}Σi∈X∈YΣj∈SiObj(|eji|σji,σT)---(21)]]>其中，σji为在第i个关键帧中，FAST特征点匹配标准差的无偏估计，μi表示第i个关键帧的位姿六元组描述向量，pi为全局地图中的点；通过不断的集束调整，使得式(21)中的稳定代价函数极小；全局集束调整所有关键帧的位姿以及地图点的位置，同时利用结构‑运动问题中的内在稀疏性使得整体矩阵分解的复杂度呈现立方级别的下降，由O((N+M)3)降至O(N3)，但在多数的估算情况中又受到生成的外点矢量积的制约，随着地图的生长，这个运算量会显著增加；针对以上问题，引入局部集束调整，这里只对关键帧的子集位姿进行调整，此时的目标函数变为：{{μχ∈X},{p′z∈Z}}=argmin{{μ},{p}}Σi∈X∈YΣj∈Z∩SiObj(i,j)---(22)]]>X为系列关键帧的待调整集合，Y为固定帧集，Z为地图点子集；集合X由地图中一个最新的关键帧以及4个最接近的关键帧构成，而关键帧的所有地图中的可视点又组成了集合Z，最后Y包含了所有由Z中测量数据所获取的关键帧；由此，局部集束调整优化了最新、最近的关键帧的位姿，更新了地图中的所有点，同时复杂度也显著降低，不超过O(NM)。