一种基于稀疏视觉特征地图的无人车位置跟踪方法

专利主权项内容

1.一种基于稀疏视觉特征地图的无人车位置跟踪方法，其特征在于，包括以下步骤：步骤1，启动设有避障探索模块的无人车：设置无人车为随机探索环境的自动运动模式, 启动双目相机，采集环境图像，建立稀疏视觉特征地图，并以二进制文件形式将稀疏视觉特征地图数据存储至车载计算机本地磁盘，构成稀疏视觉特征地图数据库；步骤2，从无人车载计算机的本地磁盘读取地图数据文件至内存，设置为定位跟踪模式；定位成功后，接收用户设置的目标位置坐标信息；按如下步骤，有位置关系变化后，反复尝试重定位：2.1.当没有成功重定位时，由所述避障探索模块生成随机控制量控制无人车运动；2.2.运动后采集新的场景图像，与稀疏视觉特征地图数据库中的场景进行匹配；2.3.匹配成功后进行位姿估计，计算无人车相对于地图的相对位置关系，实现重定位；步骤3：计算目标位置跟踪控制角：设无人车中心为本体坐标系的原点，前方为z轴的正方向，右方为x轴正方向，建立本体坐标系；设无人车启动时的无人车中心为全局坐标系的原点，前方为z轴的正方向，右方为x轴正方向，建立世界坐标系；根据当前定位信息和目标位置信息，计算目标位置跟踪控制角：设置无人车在世界坐标系下的当前位置为x，z和朝向h，目标位置在世界坐标系下的坐标为x和z，可得到目标位置跟踪控制角，将目标位置变换至本体坐标系，计算公式如下：currcurrcurrttx＝(x-x)·sin(h)-(z-z)·cos(h)in_camtcurrcurrtcurrcurrz＝(x-x)·cos(h)+(z-z)·sin(h)in_camtcurrcurrtcurrcurr其中，x，z是目标位置在本体坐标系下的坐标；in_camin_cam计算参考向量v，v，计算公式如下：xzv＝x-0xin_camv＝z-0zin_cam计算目标位置跟踪控制角θ，计算公式如下：ctrlθ＝tan(v, v)；ctrl-1xz步骤4：根据车载距离传感器的障碍探测信息，计算局部的避障控制角；步骤5：加权融合目标位置跟踪控制角和局部的避障控制量，并将融合后的控制量发送给无人车执行，直至最终安全抵达目标位置；采用如下方式融合两个控制量，实现安全的目标跟踪：θ＝wθ+wθ；1obs2t其中，w、w为权重系数，且有w+w＝1.0；θ是局部避障控制角，θ是目标位置跟踪控制角。1212obst