本发明涉及电子技术领域，具体涉及一种定位方法以及系统。
背景技术：
随着电子技术的不断发展，即时定位与地图构建(simultaneouslocalizationandmapping，slam)的应用也越来越广泛。尤其的，随着智能机器人技术的发展，slam技术被普遍应用于智能机器人以实现机器人的自主行动。
在现有技术中，常见的slam技术包括基于激光雷达的激光定位以及基于摄像头的视觉定位。基于激光雷达的slam产品，其导航精度已经达到几厘米级，并具有稳定性高，鲁棒性好的特点。但是，通常使用的激光雷达只能获取两维平面信息，且感知分辨率低，而3d激光雷达由于价格昂贵，目前并不适合应用于消费级产品中。另一方面，基于摄像头的视觉slam系统可以构建环境的三维信息，并可提取高级图像语义特征，不足之处是受光照条件影响大，高动态环境下精度下降，对场景复杂度要求高。
技术实现要素：
本发明提供了一种定位方法，所述方法包括：
获取激光雷达数据、摄像头数据以及惯性测量数据；
对所述激光雷达数据、所述摄像头数据以及所述惯性测量数据进行数据融合，获取数据融合结果；
根据所述数据融合结果确定位姿信息。
在一实施例中，所述方法还包括：
对所述激光雷达数据、所述摄像头数据以及所述惯性测量数据进行统一坐标转换；
在统一的坐标系上对所述激光雷达数据、所述摄像头数据以及所述惯性测量数据进行时间对齐。
在一实施例中，对所述激光雷达数据、所述摄像头数据以及所述惯性测量数据进行数据融合，包括：
按照角点特征对所述激光雷达数据与所述摄像头数据进行数据融合，获取第一数据融合结果；
对所述第一数据融合结果与所述惯性测量数据进行数据融合。
在一实施例中，使用卡尔曼滤波进行数据融合。
在一实施例中，所述方法还包括：
获取3d视觉数据；
将所述位姿信息与所述3d视觉数据关联。
本发明还提出了一种存储介质，所述存储介质存储有可实现本发明的定位方法流程的程序代码。
本发明还提出了一种定位系统，所述系统包括：
激光雷达数据获取模块，其配置为获取激光雷达数据；
摄像头数据获取模块，其配置为获取摄像头数据；
惯性测量数据获取模块，其配置为获取惯性测量数据；
数据融合计算模块，其配置为对所述激光雷达数据、所述摄像头数据以及所述惯性测量数据进行数据融合，获取数据融合结果；
定位模块，其配置为根据所述数据融合结果确定位姿信息。
在一实施例中，所述系统还包含数据转换接口，所述数据转换接口配置为：
对所述激光雷达数据、所述摄像头数据以及所述惯性测量数据进行统一坐标转换；
在统一的坐标系上对所述激光雷达数据、所述摄像头数据以及所述惯性测量数据进行时间对齐。
在一实施例中，所述数据融合计算模块包含：
第一融合计算单元，其配置为按照角点特征对所述激光雷达数据与所述摄像头数据进行数据融合，获取第一数据融合结果；
第二融合计算单元，其配置为对所述第一数据融合结果与所述惯性测量数据进行数据融合。
在一实施例中，所述系统还包括：
3d视觉数据获取模块，其配置为获取3d视觉数据；
2d/3d关联模块，其配置为将所述位姿信息与所述3d视觉数据关联。
相较于现有技术，本发明的方法过程简单、定位精度高、硬件需求低，具有很高的实用价值以及推广价值。
本发明的其它特征或优点将在随后的说明书中阐述。并且，本发明的部分特征或优点将通过说明书而变得显而易见，或者通过实施本发明而被了解。本发明的目的和部分优点可通过在说明书、权利要求书以及附图中所特别指出的步骤来实现或获得。
附图说明
附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例共同用于解释本发明，并不构成对本发明的限制。在附图中：
图1～图3以及图6是根据本发明不同实施例的方法流程图；
图4以及图5是根据本发明一实施例的数据融合结果示意图；
图7～图10是根据本发明不同实施例的定位系统结构框图。
具体实施方式
以下将结合附图及实施例来详细说明本发明的实施方式，借此本发明的实施人员可以充分理解本发明如何应用技术手段来解决技术问题，并达成技术效果的实现过程并依据上述实现过程具体实施本发明。需要说明的是，只要不构成冲突，本发明中的各个实施例以及各实施例中的各个特征可以相互结合，所形成的技术方案均在本发明的保护范围之内。
在现有技术中，常见的slam技术包括基于激光雷达的激光定位以及基于摄像头的视觉定位。基于激光雷达的slam产品，其导航精度已经达到几厘米级，并具有稳定性高，鲁棒性好的特点。但是，通常使用的激光雷达只能获取两维平面信息，且感知分辨率低，而3d激光雷达由于价格昂贵，目前并不适合应用于消费级产品中。另一方面，基于摄像头的视觉slam系统可以构建环境的三维信息，并可提取高级图像语义特征，不足之处是受光照条件影响大，高动态环境下精度下降，对场景复杂度要求高。
针对上述现有技术中存在的问题，本发明提出了一种定位方法。在本发明的方法中，将激光定位与视觉定位结合，从而弥补单一定位方式的不足。并且进一步的，在定位中还融合了惯性测量，从而进一步提高定位精度与定位实时性。相较于现有技术，本发明的方法过程简单、定位精度高、硬件需求低，具有很高的实用价值以及推广价值。
接下来基于附图详细描述根据本发明实施例的方法的详细流程，附图的流程图中示出的步骤可以在包含诸如一组计算机可执行指令的计算机系统中执行。虽然在流程图中示出了各步骤的逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。
如图1所示，在一实施例中，定位方法的实施过程包括：
获取激光雷达数据(s111)；
获取摄像头数据(s112)；
获取惯性测量数据(s113)；
对激光雷达数据、摄像头数据以及惯性测量数据进行数据融合，获取数据融合结果(s120)；
根据数据融合结果确定位姿信息(s130)。
相较于现有技术，由于本发明的方法采用了基于激光雷达数据、摄像头数据以及惯性测量数据的融合定位，因此其定位精度以及定位实时性提到有效提高，尤其适合于小空间内的定位(例如室内环境定位)。将本发明的方法应用于智能机器人，可以有效提高智能机器人的定位导航的准确性，从而提高机器人实用性，改善机器人的用户体验。
在实际应用场景中，由于不同传感器采用的坐标系可能不一样(每个传感器输出数据的速率和时刻不一样)，因此，在一实施例中，在进行数据融合前首先要对激光雷达数据、摄像头数据以及惯性测量数据进行插值处理，使其时空对齐。
具体的，如图2所示，在一实施例中，在获取到激光雷达数据(s221)、摄像头数据(s212)以及所述惯性测量数据(s213)后进行时空对其(s223)，包括：
对激光雷达数据、摄像头数据以及惯性测量数据进行统一坐标转换(s221)；
在统一的坐标系上对激光雷达数据、摄像头数据以及惯性测量数据进行时间对齐(s222)。
在时空对齐(s223)后再进行数据融合(s220)。
进一步的，在一实施例中，在对激光雷达数据、摄像头数据以及惯性测量数据进行数据融合的过程中采用了二段式融合方法，即，首先对激光雷达数据、摄像头数据进行数据融合，然后将融合结果与惯性测量数据进行进一步融合。
如图3所示，在一实施例中，在获取到激光雷达数据(s321)、摄像头数据(s312)以及所述惯性测量数据(s313)后进行时空对其(s321)。时空对其(s321)后首先按照角点特征对激光雷达数据与摄像头数据进行数据融合(s322)，获取数据融合结果；接下来对步骤s322获取的数据融合结果与惯性测量数据进行数据融合(s323)。最后根据步骤s323获取的数据融合结果获取位姿信息。
进一步的，在一实施例中，在步骤s322以及s323使用卡尔曼滤波进行数据融合。在本发明其它实施例中步骤s322以及s323也可以采用其他融合方法。
具体的，在一实施例中，当所有的传感器数据完成了时空对齐后进行数据融合。
首先将激光雷达数据和摄像头数据按照角点特征进行融合：
从激光数据提取的激光角点特征在当前位置坐标系r中的极坐标表示为：
式1中：cir为激光角点特征的表示，ρr为该角点到坐标原点的距离，θr为方位角，φr为俯仰角。
摄像头提取到的竖直线特征在当前位置坐标系r下的方位角为：
式2中：θir为摄像头竖直线特征，yir为直角坐标系下竖直线到y轴的距离，xir为直角坐标系下竖直线到x轴的距离。
对于激光雷达对应的角点特征数据cir中的θr和摄像头提取的竖直线特征数据θir，两者都对应当前位置到角点的直线与当前位置坐标系r的x轴的夹角。因此，可以使用卡尔曼滤波的方法这两个信息进行融合，得到不确定性相对原来激光和摄像头视觉特征更小的融合后的角点特征，如图4和5所示。
图4所示为融合后的角点特征的不确定性椭圆。在图4中，椭圆面积的大小表示特征不确定性的大小，椭圆面积越小表示估计越准确，在上方的角点特征为经过融合的，其不确定性椭圆面积较小，下面椭圆比较大的为未融合的角点特征(因为视觉观测角度较小，不可能同时观测到所有激光检测到的角点特征)。
图5所示为激光角点特征和视觉竖直融合。在图5中，坐标系表示激光提取的角点特征，射线表示视觉提取的竖直线在当前位置坐标系下的方向。
进一步的，在一实施例中，在步骤s322采用的卡尔曼滤波公式为：
上式中：h对应激光测量值，即方位角度值θr；
h为观测矩阵，z为摄像头测量值，对应
k为卡尔曼滤波增益；
以上融合后产生的位置与姿态数据，记为x，可与惯性测量数据u做进一步融合。
具体的，在一实施例中，步骤s323采用的融合方法同样采用卡尔曼滤波，只要在上面的公式中用x代替h，u代替z即可。经过再次融合后，短时间内的位姿信息的精度得到进一步提高。
进一步的，在一实施例中，在获取到位姿信息后，还将位姿信息与3d视觉数据关联，从而进一步完善定位信息。具体的，在一实施例中，如图6所示，获取3d视觉数据(s614)(步骤s614可以与步骤s612基于同一摄像头硬件设备同步获取)；在获取到位姿信息(s630)后将位姿信息与3d视觉数据关联(s640)。这样，当进行路径规划时，结合融合后的2d信息和3d信息进行规划，可以选择到更合适的路径。
基于本发明的方法，本发明还提出了一种存储介质。该存储介质存储有可实现上述的定位方法流程的程序代码。
基于本发明的方法，本发明还提出了一种定位系统。如图7所示，在一实施例中，系统包括：
激光雷达数据获取模块711，其配置为获取激光雷达数据；
摄像头数据获取模块712，其配置为获取摄像头数据；
惯性测量数据获取模块713，其配置为获取惯性测量数据；
数据融合计算模块720，其配置为对激光雷达数据、摄像头数据以及惯性测量数据进行数据融合，获取数据融合结果；
定位模块730，其配置为根据数据融合结果确定位姿信息。
进一步的，在一实施例中，激光雷达数据获取模块711、摄像头数据获取模块712或惯性测量数据获取模块713包含对应的独立的数据采集设备(例如激光设备、摄像头、惯性测量设备)。在另一实施例中，激光雷达数据获取模块711、摄像头数据获取模块712或惯性测量数据获取模块713不包含对应的独立的数据采集设备，其仅包含对应的数据获取接口，接入外部的已存在的数据采集设备(例如机器人的摄像头)进行数据获取。
进一步的，如图8所示，在一实施例中，系统还包含数据转换接口840，数据转换接口840配置为对激光雷达数据、摄像头数据以及惯性测量数据进行时空对齐。
具体的，在一实施例中，数据转换接口840包含：
坐标变换单元，其配置为对激光雷达数据、摄像头数据以及惯性测量数据进行统一坐标转换；
时间对齐单元，其配置为在统一的坐标系上对激光雷达数据、摄像头数据以及惯性测量数据进行时间对齐。
进一步的，如图9所示，在一实施例中，数据融合计算模块920包含：
融合计算单元921，其配置为按照角点特征对激光雷达数据与摄像头数据进行数据融合，获取第一数据融合结果；
融合计算单元922，其配置为对第一数据融合结果与惯性测量数据进行数据融合。
进一步的，在一实施例中，融合计算单元921和/或融合计算单元922配置为使用卡尔曼滤波进行数据融合。
进一步的，如图10所示，在一实施例中，系统还包括：
3d视觉数据获取模块104，其配置为获取3d视觉数据；
2d/3d关联模块140，其配置为将定位模块130获取到的位姿信息与3d视觉数据关联。
具体的，在一实施例中，3d视觉数据获取模块104与摄像头数据获取模块102基于同一套摄像头硬件获取数据。
将本发明的系统应用于智能机器人系统。在一具体应用场景中，机器人的运动底盘带有一个惯性测量单元(inertialmeasurementunit，imu)传感器，底盘上方安装一个激光雷达，机器人的头部装有一个单目/双目摄像头。以上三个传感器均通过usb口或串口连接到一个本发明的系统，由该系统对三个传感器的数据进行融合，同时获得定位信息并建立环境地图(slam)，当用户发出移动指令时，导航模块根据地图及机器人的当前位置进行路径规划，控制底盘运动到指定位置。当在运动路径遇到新的障碍物时，导航模块将进行局部路径规划以避开该障碍物。
在具体的应用中，与某现有的基于激光雷达的slam系统相比，本系统的定位精度提高了17％。尤其的，由于激光雷达只能输出2d平面信息，缺乏高度信息，当机器人高度较高时，规划出的路径中假如存在类似桌子这样的障碍物，将会导致机器人碰撞到该障碍物，而本发明由于利用了摄像头提供的3d信息，可以避免该问题。
在具体的应用中，与采用单目摄像头的视觉slam系统相比，本系统定位精度提高了25％。尤其的，当光照条件不好，或者环境过于单调时，视觉slam系统将会特征匹配错误导致精度下降，甚至定位错误的可能性。而本发明的系统由于采用了三传感器融合的方案，当视觉传感器数据失真时，然后可以通过其他两个传感器的数据的平滑处理，来抑制测量误差，减少定位错误的概率。
在具体的应用中，现有的imu导航系统的短时定位定姿的精度很高，但是其定位误差会逐步积累，长时间工作以后误差将大到可能导致导航错误。本发明的系统实际定位精度与现有的imu导航系统相仿，但是由于视觉传感器和激光雷达传感器具备的闭环检测能力，累积误差能够逐步收敛到某一个较小的值。
虽然本发明所公开的实施方式如上，但所述的内容只是为了便于理解本发明而采用的实施方式，并非用以限定本发明。本发明所述的方法还可有其他多种实施例。在不背离本发明实质的情况下，熟悉本领域的技术人员当可根据本发明做出各种相应的改变或变形，但这些相应的改变或变形都应属于本发明的权利要求的保护范围。