买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：南昌智能新能源汽车研究院

摘要：本发明公开了一种基于视觉SLAM的室内停车场定位方法及系统，属于智能交通领域。该方法为：采用单目相机采集Apriltag图像数据，采用车载惯性测量单元（IMU）获取车辆IMU数据；提取图像中特征点信息及Apriltag的ID信息，解算IMU数据获取车辆的位姿信息；通过特征点信息及Apriltag的ID信息实现前后两帧间的视觉里程计；根据车辆的位姿信息，采用扩展卡尔曼滤波算法进行后端优化；采用词袋模型进行回环检测，更新地图，实现室内停车场建图和车辆的定位。本发明实现了室内停车场精准建图和定位。

主权项：1.一种基于视觉SLAM的室内停车场定位方法，其特征在于：包括如下步骤：步骤1：采用单目摄像机采集Apriltag图像数据，采用惯性测量单元获取车辆的IMU数据信息；所述图像数据包括多个Apriltag在内的室内停车场图像；所述IMU数据包含车辆姿态、角速度与线加速度信息；步骤2：对所述单目摄像机和惯性测量单元进行联合标定到车辆坐标系；步骤3：对所述单目摄像机获取的停车场图像进行预处理，采用SIFT算法提取所述停车场图像中特征点，并获取所述停车场图像中Apriltag位置及其ID；步骤4：对所述IMU数据进行预处理，解算出所述车辆的位姿信息；步骤5：对于获取的所述特征点和ID信息，结合车辆位姿信息，采用扩展卡尔曼滤波进行地图优化；步骤6：采用词袋模型进行回环检测，更新地图，实现室内停车场建图和车辆的定位；步骤3所述对所述单目摄像机获取的停车场图像进行预处理，采用SIFT算法提取所述停车场图像中特征点，并获取所述停车场图像中Apriltag位置及其ID包括：S301，所述预处理包括高斯平滑和DoG特征图计算；S302，采用SIFT算法提取所述停车场图像中特征点：通过高斯微分函数来识别所述停车场图像潜在的具有尺度和旋转不变的兴趣点，根据所述兴趣点定位关键点并确定特征方向，利用特征点邻域的梯度直方图确定特征点的主方向，并在特征点邻近区域生成描述算子，生成4x4x8维的SIFT特征向量，通过各所述关键点的所述SIFT特征向量，进行两两比较找出相互匹配的若干对特征点；S303，获取所述停车场图像中Apriltag位置及其ID：对所述停车场图像进行灰度化和二值化，并采用Sobel算子进行边缘检测，得到二值化边缘图像，对所述二值化边缘图像进行多边形分析，直到所有的二值点都被遍历，利用多边形凸包寻找算法，计算每一个多边形本身的凸包，并求所述凸包及其多边形的面积，将所述凸包及其多边形的面积进行比较，当所述多边形面积比所述凸包面积大时将所述多边形排除，保留满足条件的其余多边形，使用道格拉斯-普克算法对所述其余多边形进行四边形逼近，最后对检测到的四边形内部图案进行解码，得出每个图案对应的数字ID；步骤5所述对于获取的所述特征点和ID信息，结合车辆位姿信息，采用扩展卡尔曼滤波进行地图优化为：S501，依据上一时刻所述停车场图像中的特征点信息、ID信息及车辆位姿信息，来预测当前时刻的位姿和特征点的联合状态估计，其中，所述当前时刻的位姿和特征点的联合状态表示均值，方差由运动变化矩阵求解，具体步骤如下:构建基本方程：设离散时间，车辆的位置为，车辆从上一时刻运动到下一时刻，则车辆的运动方程为：；设所述特征点的信息为，所述惯性测量单元在位置处探测到，则观测方程为：；其中，表示k时刻的位置，为通过特征点估计出的车辆的运动，表示所述惯性测量单元得到的所述特征点的信息和ID信息，根据k时刻的最优状态估计预测k+1时刻的状态和误差的协方差矩阵： 4 5式4是状态测试方程，其中，是k时刻系统状态的最优估计，是根据k时刻的状态最优估计和时刻的系统输入对k+1时刻系统状态的预测；式5是误差的协方差矩阵预测方程，其中，是k时刻系统状态的最优估计对应的误差协方差矩阵，是系统状态的预测对应的误差协方差矩阵，是方程的雅克比矩阵，是k时刻系统过程噪声的协方差矩阵；S502，根据系统k+1时刻实际的所述特征点和ID信息，对上一步得到的k+1时刻系统状态的预测结果进行优化，得到在k+1时刻系统状态的最优估计，具体如下： 6其中，是k+1时刻系统状态的最优估计，是卡尔曼增益矩阵，其计算方法如下： 7其中，是方程的雅克比矩阵，是k+1时刻测量噪声的协方差矩阵；S503，继续更新k+1时刻的误差协方差矩阵，公式如下： 8其中，I为单位矩阵，是k+1时刻系统状态的最优估计对应的误差协方差矩阵；步骤6采用词袋模型进行回环检测，具体步骤如下：S601，构建一种包含所述特征点和ID信息相融合的字典，即单词的集合，表示为： 9S602，确定一帧图像中的单词，形成词袋向量，如下式表示： 10其中，1表示有该单词，0表示没有，分别表示第一个特征点和第一个ID信息，分别表示第n个特征点和第n个ID信息；S603，比较两帧图像的描述向量F的差异，相似度大于预设阈值的两幅图像作为一个检测到的回环，并更新地图，实现室内停车场建图和车辆的定位。

全文数据：

权利要求：

百度查询： 南昌智能新能源汽车研究院 一种基于视觉SLAM的室内停车场定位方法及系统