[0012] Pos＝pos if map points>threshold，
[0013] pos代表当前帧所处位置，pos of IMU代表通过IMU元件获得的当前位置，map points代表一帧里面拥有的地图点，threshold代表系统预设阈值。
[0014] 优选地，还包括G20优化模块，所述G20优化模块对停车场模拟地图进行离线闭环检测，以对停车场模拟地图全局优化；及所述G20优化模块还对车辆二次地图定位进行优化，以得到车辆的最优定位信息。
[0015] 优选地，所述双目视觉SLAM模块还包括存储模块，所述存储模块用于存储双目视觉SLAM模块所生成的不同的停车场模拟地图，并提供给车辆视觉定位单元进行匹配及定位。
[0016] 优选地，所述环境特征参数包括车道线参数、车位参数、障碍物参数，所述环视泊车检测模块包括车道线检测单元、车位检测单元和障碍物检测单元，其中：车道线检测单元，用于为车辆提供车道线感知信息，并将所感知的信息传送至决策及控制模块以规划车辆行驶路线；车位检测单元，用于采用环视相机得出车辆所处位置的环视图并进行车位检测；障碍物检测单元，用于利用双目相机检测车辆四周是否存在障碍物，并将所检测的结果传送至决策及控制模块。
[0017] 本发明还公开了一种基于多视觉惯导融合的自主泊车方法，包括如下步骤：S10将所采集的停车场图像通过多视觉惯导融合生成停车场模拟地图，且获取车辆在停车场模拟地图坐标系下的实时定位信息传送至决策及控制模块，其中双目视觉SLAM模块包括惯性测量元件IMU，用以获取车辆当前位置的IMU信息；S20采集车辆当前位置数据检测当前环境特征参数检测，且将其检测结果传送至决策及控制模块；S30根据所接收的车辆在停车场模拟地图中地理定位信息和当前环境特征参数作出车辆自主泊车策略，以此控制车辆自主泊车的操作。
[0018] 优选地，所述S10包括：
[0019] S101通过双目相机采集车辆当前帧图像，对其处理得到该图像的深度信息，从该图像的深度信息中提取车辆当前帧图像的ORB特征；
[0020] S102根据车辆在停车场行驶从每帧车辆当前帧图像所提取的ORB特征通过多视觉惯导融合生成若干停车场SLAM图像，且由若干停车场SLAM图像组成停车场模拟地图；
[0021] S103用于车辆无人泊车进程中通过多视觉惯导融合的轨迹推导车辆在停车场模拟地图坐标系下的初始实时定位信息；
[0022] S104将车辆无人泊车进程中的当前帧图像的ORB特征与停车场SLAM图像的特征图像进行场景实时定位修正，将车辆在停车场模拟地图坐标系下的初始实时定位信息修正为车辆在停车场模拟地图坐标系下的实时定位信息，并输出至决策及控制模块。
[0023] 本发明技术方案通过采用多视觉惯导融合也就是SLAM融合IMU建立起更可靠的停车场模拟地图；又由于融合了IMU元件轨迹推算的位置，获取更为准确、稳定的车辆实时位置信息，再通过场景定位修正时获取车辆在停车场模拟地图坐标系下的实时定位信息输出至决策及控制模块，控制车辆自主泊车，全方位提高了自主泊车系统的正确性、高效性与鲁棒性。
附图说明
[0024] 为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的附图。
[0025] 图1为本发明一种基于多视角惯导融合的自主泊车系统的一实施例的功能模块图；
[0026] 图2为本发明中所述双目视觉SLAM模块的功能细化图；
[0027] 图3为本发明中所述车辆视觉定位单元的功能细化图；
[0028] 图4为本发明一种基于多视角惯导融合的自主泊车方法一实施例的方法流程图；
[0029] 图5为本发明中所述S10的方法流程细化图，
[0030] 本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
具体实施方式
[0031] 下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0032] 需要说明，若本发明实施例中有涉及方向性指示(诸如上、下、左、右、前、后……)，则该方向性指示仅用于解释在某一特定姿态(如附图所示)下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。
[0033] 另外，若本发明实施例中有涉及“第一”、“第二”等的描述，则该“第一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。另外，各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本发明要求的保护范围之内。
[0034] 如图1-5所示，本发明提出的基于多视觉惯导融合的自主泊车系统，包括双目视觉SLAM模块、环视泊车检测模块和决策及控制模块，其中：双目视觉SLAM模块，用于将所采集的停车场图像通过多视觉惯导融合生成停车场模拟地图，且获取车辆在停车场模拟地图坐标系下的实时定位信息传送至决策及控制模块，其中双目视觉SLAM模块包括惯性测量元件IMU，用以获取车辆当前位置的IMU信息；环视泊车检测模块，用于采集车辆当前位置数据检测当前环境特征参数检测，且将其检测结果传送至决策及控制模块；决策及控制模块，用于根据所接收的车辆在停车场模拟地图中地理定位信息和当前环境特征参数作出车辆自主泊车策略，以此控制车辆自主泊车的操作。
[0035] 在本实施例中，本发明主要分为机器视觉和决策控制两部分，机器视觉又分为双目视觉SLAM模块(即时定位与建图)和停车场环境检测模块，本发明利用双目相机采集车辆所处停车场的环境图像信息，并通过多视觉惯导融合处理生成停车场模拟地图，同时又通过环视泊车系统，在车辆巡航过程中检测停车场环境，且将所检测到的停车场环境信息发送至决策及控制模块做出车辆自主泊车策略，车辆自动巡航，获取车辆在停车场模拟地图坐标系下的实时定位信息传送至决策及控制模块，通过决策及控制模块的控制自主泊车。补充一下地是，双目视觉SLAM通过车辆位置当前帧图像的采集生成当前帧左右两视图的特征图，运用特征匹配算法与地图进行匹配，得到车辆在地图中的初始定位，再采用立体视觉SLAM的追踪定位算法计算车辆当前相对于被匹配地图帧的姿态，用此姿态跟该地图帧的离线轨迹姿态做RT变换(RT矩阵变换就是一个位置矩阵乘以一个RT矩阵，4*4矩阵，里面包含了R矩阵，T矩阵，分别表示角度和距离，地图帧的位置矩阵*RT矩阵＝车辆实际位置矩阵，其中RT矩阵通过追踪定位算法得到，由此得到一个新位置矩阵)，得到车辆当前在离线地图坐标下的姿态，即车辆在地图中的实时定位信息，比如车辆在地图中的方向，初始定位是在车辆当前帧图像坐标系与停车场模拟地图坐标系没有统一的情况下的位置，存有误差，车辆在停车场模拟地图坐标系下的实时定位信息就是修正了两个坐标系之间的误差，再通过车辆当前帧图像与停车场SLAM图像进行匹配修正(即场景匹配修正)，又通过IMU将提供稳定的轨迹推算功能，每隔一段时间调用上述方法，以基于停车场模拟地图的方式对车辆当前轨迹做修正，进一步地确保整套系统运作时的稳定性与正确性。
[0036] 惯性测量元件IMU获取IMU信息的车辆当前位置，是利用IMU元件做轨迹推算所获取的稳定的车辆当前位置信息。本系统引进了融合IMU(惯性测量单元)信息的立体视觉SLAM与基于特征地图的匹配模型，利用IMU测量的稳定性与立体视觉SLAM的定位功能，高效率的，高鲁棒性的实现了轨迹推算。
[0037] 本发明技术方案通过采用多视觉惯导融合也就是SLAM融合IMU建立起更可靠的停车场模拟地图；又由于融合了IMU元件轨迹推算的位置，获取更为准确、稳定的车辆实时位置信息，再通过场景定位修正时获取车辆在停车场模拟地图坐标系下的实时定位信息输出至决策及控制模块，控制车辆自主泊车，全方位提高了自主泊车系统的正确性、高效性与鲁棒性。
[0038] 优选地，所述双目视觉SLAM模块包括图像采集处理单元、建图单元、车辆视觉定位单元和场景定位修正单元，其中：图像采集处理单元，用于通过双目相机采集车辆当前帧图像，对其处理得到该图像的深度信息，从该图像的深度信息中提取车辆当前帧图像的ORB特征；建图单元，用于根据车辆在停车场行驶从每帧车辆当前帧图像所提取的ORB特征通过多视觉惯导融合生成若干停车场SLAM图像，且由若干停车场SLAM图像组成停车场模拟地图；车辆视觉定位单元，用于车辆无人泊车进程中通过多视觉惯导融合的轨迹推导车辆在停车场模拟地图坐标系下的初始实时定位信息；场景定位修正单元，用于将车辆无人泊车进程中的当前帧图像的ORB特征与停车场SLAM图像的特征图像进行场景实时定位修正，将车辆在停车场模拟地图坐标系下的初始实时定位信息修正为车辆在停车场模拟地图坐标系下的实时定位信息，并输出至决策及控制模块。
[0039] 优选地，所述场景定位修正单元包括一次定位修正子单元、二次定位修正子单元和输出子单元，其中：一次定位修正子单元，用于将车辆当前帧图像的ORB特征与停车场SLAM图像进行场景实时定位修正，将车辆在停车场模拟地图坐标系初始实时定位信息修正为其的一次实时定位信息；二次定位修正子单元，用于将所述一次实时定位信息和车辆当前帧图像进行矩阵转换完成二次定位修正，得到车辆在停车场模拟地图坐标系下的实时定位信息；输出子单元，用于将车辆在停车场模拟地图坐标系下的实时定位信息输出至决策及控制模块。
[0040] 优选地，所述多视觉惯导融合是指以停车场帧图像作为参考关键帧放入SLAM中，车辆当前帧图像放入SLAM，且与参考关键帧中的图像信息相匹配，根据匹配情况选用车辆当前帧图像或车辆当前位置的IMU信息作为车辆位置信息。
[0041] 优选地，所述根据匹配情况选用车辆当前帧图像或IMU信息的车辆当前位置作为车辆位置信息的公式为：
[0042] Pos＝pose of IMU if map points
[0043] 或Pos＝pos if map points>threshold，
[0044] pos代表当前帧所处位置，pos of IMU代表通过IMU元件获得的当前位置，map points代表一帧里面拥有的地图点，threshold代表系统预设阈值。
[0045] 优选地，还包括G20优化模块，所述G20优化模块对停车场模拟地图进行离线闭环检测，以对停车场模拟地图全局优化；及所述G20优化模块还对车辆二次地图定位进行优化，以得到车辆的最优定位信息。
[0046] 优选地，所述双目视觉SLAM模块还包括存储模块，所述存储模块用于存储双目视觉SLAM模块所生成的不同的停车场模拟地图，并提供给车辆视觉定位单元进行匹配及定位。
[0047] 在本实施例中，本发明在建图时，利用双目相机采集停车场的环境图像，通过每帧的左右目图像并使用特征匹配算法生成其特征图像，同时，利用IMU元件获得IMU信息的车辆位置信息，将IMU信息的车辆位置信息融合停车场环境图像的特征图像，组成停车场模拟地图，进一步地，在离线状态下，使用立体视觉SLAM对停车场模拟地图图像数据进行闭环检测，并对记录下来的轨迹进行全局优化，最终自主泊车系统将基于这份地图数据，在相同的环境下运行，运行过程中多视觉惯导融合将继续参与。所述停车场SLAM图像应该理解为对车辆在停车场的当前帧图像提取ORB特征通过SLAM融合IMU处理后，生成的停车场当前帧图像。
[0048] 在本实施例中，本发明完成建图后，开始了车辆在停车场模拟地图坐标系下的初始实时定位。将车辆巡航时的当前帧图像的坐标与停车场模拟地图坐标系统相统一，然后确定车辆当前位置在停车场模拟地图坐标系下的初始实时定位位置，即车辆在停车场模拟地图坐标系下的当前定位位置。
[0049] 车辆在停车场模拟地图的修正定位包括二次定位，一次定位为粗定位，是指将车辆的当前帧图像与停车场模拟地图进行场景匹配，应该理解地是，与停车场模拟地图的场景匹配有两层理解，第一层是指本发明只有在车辆初次来到一个停车场时才需巡航建图，建图模块每次所建立的停车场模拟地图均会存储起来，以便以后的匹配及调用，故车辆与停车场模拟地图的第一层匹配意思是指通过车辆当前帧图像的特征图像与若干停车场模拟地图进行场景识别；第二层是指若识别到相同或相似场景的停车场模拟地图后，将车辆当前帧图像的特征图像放入该停车场模拟地图的SLAM中，获取车辆在停车场模拟地图中的初始定位信息。二次定位为精定位，是指利用立体视觉SLAM中的追踪定位部分计算得到车辆当前帧图像与车辆在停车场模拟地图中的粗定位的位置信息的转换矩阵(RT转换)，精定位具体来说，采用立体视觉SLAM的追踪定位算法计算车辆当前相对于被匹配地图帧的姿态，用此姿态跟该地图帧的离线轨迹姿态做RT转换，，得到车辆当前在离线地图坐标下的姿态，即车辆在地图中的精定位。补充说明RT转换：就是一个位置矩阵乘以一个RT矩阵，4*4矩阵，里面包含了R矩阵，T矩阵，分别表示角度和距离，地图帧的位置矩阵*RT矩阵＝车辆实际位置矩阵，其中RT矩阵通过追踪定位算法得到，由此得到一个新位置矩阵。精定位是经过了匹配计算，修正误差后的定位，比如车辆在地图中的方向，粗定位就是不大准确的xyz里面有误差，精定位就是消减误差的xyz。往后巡航时，IMU将提供稳定的轨迹推算功能，每隔一段时间将调用上述方法，以基于地图的方式对车辆当前轨迹做修正，通过车辆在停车场模拟地图中的精定位，以导航车辆回归地图，消减积累误差，同时场景匹配成功后，也将把车辆位置坐标系与停车场模拟地图的坐标系相统一，为后续的导航提高了可能性，也确保整套系统运作时的稳定性与正确性。本发明的双目SLAM大部分的运算与优化被放到了离线工作中，使用IMU得到了比普通视觉SLAM更加稳定的轨迹推算，且运用特征地图匹配模型与SLAM，修正了完全只使用IMU的累计误差，全方位提高了自主泊车系统的正确性、高效性与鲁棒性。
[0050] 所述G20优化模块，是指图优化的开源框架，一个开源库，包含了图优化的功能。只需要把帧放进去即可得到位置信息，处理过程为：构造g2o优化器，把帧拥有地图点信息，最初位置信息(一般会先预给个初值)放入优化器中进而得到最终位置信息。所述G20优化模块是用来优化位置计算，在本发明中，所述G20优化模块在对停车场模拟地图进行离线闭环检测，以对停车场模拟地图全局优化；及所述G20优化模块还对车辆二次地图定位进行优化，以得到车辆的最优定位信息。通过G20优化模块进一步地修正位置信息误差。
[0051] 优选地，所述环境特征参数包括车道线参数、车位参数、障碍物参数，所述环视泊车检测模块包括车道线检测单元、车位检测单元和障碍物检测单元，其中：
[0052] 车道线检测单元，用于为车辆提供车道线感知信息，并将所感知的信息传送至决策及控制模块以规划车辆行驶路线；
[0053] 车位检测单元，用于采用环视相机得出车辆所处位置的环视图并进行车位检测；
[0054] 障碍物检测单元，用于利用双目相机检测车辆四周是否存在障碍物。
[0055] 在实施例中，本发明的环视泊车检测模块为环视泊车系统中的环境检测模块，所述车道线检测单元将所感知的信息传送至决策及控制模块以规划车辆行驶路线；所述车位检测单元将车位检测结果传送至决策及控制模块以提供可停靠的停车位的多项选择。所述障碍物检测单元将车辆四周障碍物的检测结果传送至决策及控制模块，以便车辆无人驾驶巡航的安全及提供给决策及控制模块做出最优泊车路线的参考。所述的环视有且不限于由若干个安装在车身周围的带180度广角镜头的鱼眼摄像头构成。
[0056] 以上所述仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是在本发明的发明构思下，利用本发明说明书及附图内容所作的等效结构变换，或直接/间接运用在其他相关的技术领域均包括在本发明的专利保护范围内。
