多传感器融合的列车定位方法、装置、定位系统及列车与流程

1.本发明一般涉及列车定位技术领域，具体涉及一种多传感器融合的列车定位方法、装置、定位系统及列车。


背景技术：

2.列车定位单元作为列车控制系统的核心子系统，能够提供当前的列车位置和速度信息，防止追尾、碰撞等事故的发生，对列车的安全运行起着至关重要的作用。传统的列车定位方式大多采用里程计、轨道电路、应答器等。随着铁路对现代化、智能化系统的需求逐步增加，由于受各种限制条件(安装位置、运行环境等)，传统的定位方式存在各种弊端，难以满足列控系统的需求。
3.目前现有主要采用轮速传感器和应答器相结合的方式，但是由于应答器是单点定位，轮速传感器又有累积误差，很难形成连续的高精度定位；并且当线路很长时，为了满足安全性要求，需要安装大量的应答器，造成很高的成本。


技术实现要素：

4.鉴于现有技术中的上述缺陷或不足，期望提供一种多传感器融合的列车定位方法、装置、定位系统及列车。
5.第一方面，本发明一种多传感器融合的列车定位方法，包括：
6.根据列车的当前位置选择从gnss/imu传感系统、odo/imu传感系统、lidar/imu传感系统中分别获取列车的位姿数据；
7.将列车的位姿数据进行融合，得到列车的定位结果。
8.作为可选的方案，在根据列车的当前位置选择从gnss/imu传感系统、odo/imu传感系统、lidar/imu传感系统中分别获取列车的位姿数据之前，包括：
9.根据车载地图数据确定列车的当前位置是位于站内还是位于站外；
10.当列车的当前位置位于站内时，则从gnss/imu传感系统、odo/imu传感系统和lidar/imu传感系统分别获取列车位姿数据；
11.当列车的当前位置位于站外时，则从gnss/imu传感系统和odo/imu传感系统分别获取列车位姿数据。
12.作为可选的方案，根据列车的当前位置选择从gnss/imu传感系统、odo/imu传感系统、lidar/imu传感系统中分别获取列车的位姿数据，包括：
13.分别获取gnss传感器感测到的gnss数据、imu传感器感测到的imu数据、odo传感器感测到的odo数据和lidar传感器感测到的lidar数据；
14.将gnss数据、odo数据和lidar数据分别与imu数据两两组合得到与gnss/imu传感系统、odo/imu传感系统、lidar/imu传感系统分别对应的列车位姿数据。
15.作为可选的方案，将gnss数据、odo数据和lidar数据分别与imu数据两两组合得到与gnss/imu传感系统、odo/imu传感系统、lidar/imu传感系统分别对应的列车位姿数据，包
括：
16.根据gnss数据和imu数据确定列车的第一位置、速度和第一姿态信息；
17.根据odo数据和imu数据确定列车的里程和第二姿态信息；
18.根据lidar数据和imu数据确定列车的第二位置和第三姿态信息。
19.作为可选的方案，将列车的位姿数据进行融合，得到列车的定位结果，包括：
20.将列车的第一位置、速度和第一姿态信息输入第一卡尔曼滤波器，对建立的列车状态模型进行预测和更新，融合得到列车的位姿数据的第一估计值；
21.将列车的里程和第二姿态信息输入第二卡尔曼滤波器，对列车状态模型进行预测和更新，融合得到列车的位姿数据的第二估计值；
22.将列车的第二位置和第三姿态信息输入第三卡尔曼滤波器，对列车状态模型进行预测和更新，融合得到列车的位姿数据的第三估计值；
23.将第一估计值、第二估计值和第三估计值输入第四卡尔曼滤波器，对列车状态模型进行预测得到列车的第四估计值，并将第四估计值分别反馈至第一滤波器和第三滤波器进行更新融合，直到收敛得到列车的定位结果。
24.第二方面，本发明提供一种多传感器融合的列车定位装置，包括：
25.获取模块，用于根据列车的当前位置选择从gnss/imu传感系统、odo/imu传感系统、lidar/imu传感系统中分别获取列车的位姿数据；
26.融合模块，用于将列车的位姿数据进行融合，得到列车的定位结果。
27.作为可选的方案，装置还包括：
28.判断模块，用于根据车载地图数据确定列车的当前位置位于站内还是位于站外；
29.第一获取模块，用于当列车的当前位置位于站内时，则从gnss/imu传感系统、odo/imu传感系统和lidar/imu传感系统分别获取列车的位姿数据；
30.第二获取模块，用于当列车的当前位置位于站外时，则从gnss/imu传感系统和odo/imu传感系统分别获取列车的位姿数据。
31.第三方面，本发明提供一种列车定位系统，系统包括：设置于列车上的gnss传感器、imu传感器、odo传感器和lidar传感器，gnss传感器用于采集列车的第一位置和速度、imu传感器用于采集列车的姿态信息、odo传感器用于采集列车的里程以及lidar传感器用于采集列车的第二位置；
32.以及处理器，处理器分别与gnss传感器、imu传感器、odo传感器和lidar传感器信号连接，处理器包括第一滤波器、第二滤波器、第三滤波器和第四滤波器；
33.第一滤波器，与gnss传感器的输出端和imu传感器的输出端连接，用于将第一位置、速度和姿态信息融合得到列车位姿数据的第一估计值；
34.第二滤波器，与odo传感器的输出端和imu传感器的输出端连接，用于将里程和姿态信息融合得到列车位姿数据的第二估计值；
35.第三滤波器，与lidar传感器的输出端和imu传感器的输出端连接，用于将第二位置和姿态信息融合得到列车位姿数据的第三估计值；
36.第四滤波器，分别于第一滤波器、第二滤波器、第三滤波器的输出端连接，用于将第一估计值、第二估计值和第三估计值进行融合，并将融合得到的第四估计值分别反馈至第一滤波器和第三滤波器进行更新融合，直至收敛得到列车的定位结果。
37.第四方面，本发明提供一种列车，列车包括处理器，处理器用于执行第一方面的列车定位方法。
38.第五方面，本发明提供一种计算机可读存储介质，其上存储有计算机程序，计机程序用于实现第一方面的列车定位方法。
39.本技术根据多个传感器，结合列车的位置情况，将不同传感系统采集的列车的位姿数据，进行融合得到列车的定位结果，其中多个传感器包括gnss传感器、imu传感器、odo传感器、以及lidar传感器等，本技术提出的多传感器融合定位方法，有效地提高了列车定位的精确性，在实现精确定位的同时，极大地降低了开发成本。
附图说明
40.通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本技术的其它特征、目的和优点将会变得更明显：
41.图1为本发明的实施例的一种多传感器融合的列车定位方法的应用场景示意图；
42.图2为本发明的实施例的一种多传感器融合的列车定位方法的流程示意图；
43.图3为本发明的实施例的一种多传感器融合的列车定位方法的框架结构示意图；
44.图4为本发明的实施例的一种多传感器融合的列车定位方法的数据融合结构框架示意图；
45.图5为本发明的实施例的另一种多传感器融合的列车定位方法的流程示意图；
46.图6为本发明的实施例的一种多传感器融合的列车定位装置的结构示意图；
47.图7为本发明的实施例的另一种多传感器融合的列车定位装置的结构示意图；
48.图8为本发明的实施例的一种多传感器融合的列车定位系统的结构示意图；
49.图9为本发明的实施例的一种计算机系统的结构示意图。
具体实施方式
50.下面结合附图和实施例对本技术作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与发明相关的部分。
51.需要说明的是，在不冲突的情况下，本技术中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本技术。
52.本技术中涉及到的技术术语定义如下：
53.全球卫星导航系统：全称global navigation satellite system，简称gnss；gnss传感器是指采用全球卫星导航系统进行定位的传感器。
54.惯性测量单元：一种测量物体三轴姿态角及加速度的装置，全称inertial measurement unit，简称imu；imu传感器是指动态测量、定位和导航所需的角速率和加速度参数。
55.测量车轮速度：全称odometer，简称odo；odo传感器是指通过计数器记录1s内odo的脉冲个数，来计算列车1s内的速度，odo传感器也可以称为里程计。
56.激光雷达：简称lidar。lidar传感器是指利用发射激光传感束，然后根据光束返回至传感器的时间测距。
57.误差卡尔曼滤波器：全称error-state kalman filter，简称ekf。
58.列车在行驶过程中，需要实时地提供列车当前的列车位置和速度信息，以保证控制中心能够合理准确安排轨道线路上的列车的运行，防止追尾、碰撞等事故的发生，对列车的安全运行起着至关重要的作用。
59.目前列车定位主要采用轮速传感器、轨道电路、应答器相结合的方式，轮速传感器通过测量速度累计得到列车的里程；应答器虽然精度较高，但必须在一定的测量范围以内(应答器附近)，且需要在铁路沿线安装大量的应答器，不易维护、成本较高；轨道电路是根据经过的列车同两条钢轨形成一个电路回流，从而检测到列车位置，但是只能获取到列车在区间内，而不能判断到列车的具体位置，定位误差较大，雷达受车速影响会有一定的误差波动，因此只能作为一种辅助传感器。
60.随着卫星定位技术的发展，为列车定位提供了新的方法。目前全球导航卫星系统(gnss)包括美国的gps系统，俄罗斯的glonass系统，欧洲的galileo系统以及中国的北斗系统。卫星定位设备朝向高精度、实时、低成本方向发展。本技术将卫星定位系统引入列车定位系统，可以显著降低轨道旁信号机、轨道电路、计轴设备以及应答器的数量，这对于降低轨道建设、维护成本具有重要意义。但是每种传感器都有各自的优缺点，例如gnss具有全天候、全范围覆盖但一受到环境干扰的特点；imu和odo具有短时精度高、输出频率高的特点，但是误差随着时间累计而增加；lidar定位精度高但易受到环境影响。
61.基于上述问题，本技术的实施例提供一种多传感器融合的列车定位方法，采用多传感器的数据融合的方法，提高列车定位的稳定性和定位精度。
62.在车辆行驶过程中，通过列车的当前位置从gnss/imu传感系统、odo/imu传感系统、lidar/imu传感系统中分别获取列车的位姿数据，再将gnss/imu传感系统、odo/imu传感系统以及lidar/imu传感系统的列车位姿数据进行融合得到列车的定位结果。
63.上述gnss/imu传感系统、odo/imu传感系统以及lidar/imu传感系统中的数据分别来自于将gnss传感器采集的gnss数据、imu传感器采集的imu数据、odo传感器采集的odo数据、lidar传感器采集的lidar数据，将gnss传感器采集的gnss数据、odo传感器采集的odo数据和lidar传感器采集的lidar数据分别与imu传感器采集的imu数据进行两两组合，得到与gnss/imu传感系统对应列车的位姿数据、或者与odo/imu传感系统对应列车的位姿数据，或者与lidar/imu传感系统对应的列车的位姿数据，利用融合算法将gnss/imu传感系统、odo/imu传感系统以及lidar/imu传感系统的列车的位姿数据进行融合得到列车位姿数据的估计值。
64.可以理解，本技术实施例提出的多传感器融合的列车定位方法，减少列车定位对地面设备的依赖，无需轨道旁信号机、从而减少轨道电路、计轴设备和应答器等地面设备。在列车上，如高铁、动车或地铁上配置采集模块，采集模块包括有gnss传感器、odo传感器、lidar传感器以及imu传感器，用于采集列车的gnss数据、odo数据、lidar数据、imu数据、列车周围的环境信息等。其中，gnss数据包括但不限于经度、纬度、高度等；odo数据包括列车运行方向的速度，速度累积得到列车的里程；lidar数据包括列车的位置三维坐标以及列车周围的环境信息，例如其他列车的行驶速度、行驶轨迹、与本列车的横向及纵向距离，还包括周围建筑物及设施等；imu数据包括加速度、航向角、俯仰角等。
65.还可以理解，本技术的一种多传感器融合的列车定位方法可以由列车自身的车载
控制系统或配置在列车上的终端设备执行。如图1所示的列车定位系统，该定位系统可以包括采集模块、组合模块和处理模块，采集模块包括用于接收设置在列车上的gnss传感器、odo传感器、imu传感器和lidar传感器的传感数据的模块；组合模块包括gnss/imu传感系统、odo/imu传感系统、lidar/imu传感系统；gnss/imu传感系统用于将gnss传感器采集的gnss数据与imu传感器采集的imu数据进行组合，odo/imu传感系统用于将odo传感器采集的odo数据与imu传感器采集的imu数据进行组合，lidar/imu传感系统用于将lidar传感器采集的lidar数据与imu传感器采集的imu数据进行组合。
66.处理模块包括多个滤波器，例如第一滤波器、第二滤波器、第三滤波器和第四滤波器。其中第一滤波器用于接收gnss/imu传感系统输出的组合数据；第二滤波器用于接收odo/imu传感系统输出的组合数据；第三滤波器用于接收lidar/imu传感系统输出的组合数据。第四滤波器用于对第一滤波器、第二滤波器、第三滤波器的输出结果进行融合更新，直到导航误差最小，得到列车的精确定位信息。
67.采集模块将采集的列车位置、速度、姿态信息、里程和周围环境信息等发送给组合模块，组合模块组合得到采集列车的位姿数据后，分别发送给第一滤波器、第二滤波器、第三滤波器进行融合分别得到第一估计值、第二估计值和第三估计值，将第一估计值、第二估计值和第三估计值输入到第四滤波器融合得到列车的定位结果。
68.其中，终端设备可以是硬件，也可以是软件。当终端设备为硬件时，可以是电子设备，包括但不限于平板电脑、笔记本电脑台式计算机等。当终端设备为软件时，可以安装在前述的各种电子设备中。其也可以实现为单个或多个软件或软件模块。本技术实施例对电子设备的具体类型不作任何限制。
69.图2为根据本技术实施例示出的一种多传感器融合的列车定位方法的流程示意图。该方法由配置在车辆上的终端设备执行，请见图2，该方法包括如下步骤：
70.s101、根据列车的当前位置选择从gnss/imu传感系统、odo/imu传感系统、lidar/imu传感系统中分别获取列车的位姿数据；
71.具体的，在本实施例中，列车行驶过程中，gnss实时采列车的gnss数据，包括经度、纬度、高度等数据；imu实时采集列车的imu数据，包括加速度、航向角、俯仰角等；odo实时采集列车的odo数据，包括速度，速度累积得到列车的里程；lidar实时采集列车的lidar数据，包括位置以及列车周围环境信息，列车周围环境信息包括但不限于其他列车的行驶速度、行驶轨迹、与本列车的横向及纵向距离、周围建筑物及设施等。
72.由于每个传感器都有自身的弊端，单独的传感器的数据无法准确反映列车的位姿信息，位姿信息包括列车的位置和姿态信息，例如：经度、纬度、高度、速度、加速度、俯仰角、偏航角、翻滚角等。为了解决该问题，利用多传感器数据融合定位，提高定位的稳定性和定位的精度。
73.将gnss数据和imu数据通过gnss/imu传感系统组合，odo数据和imu数据通过odo/imu传感系统组合、lidar数据和imu数据通过lidar/imu传感系统组合，补偿了单一传感器采集的数据不全面，避免无法准确反映列车的位姿数据的问题。
74.根据列车的当前位置选择从gnss/imu传感系统、odo/imu传感系统、lidar/imu传感系统中分别获取列车的位姿数据。
75.上述根据列车的当前位置确定从哪些传感系统中获取位姿数据，降低了对处理器
的计算能力要求。
76.例如，在有建筑物遮挡、树木遮挡或列车进入隧道内时，gnss的信号较差，如果仅通过gnss数据，是无法准确地定位列车位置。而lidar信号较强，并能够采集列车周围环境信息，从而实现列车的准确定位。在外部环境没有遮挡物时，仅通过gnss数据和imu数据以及odo数据，即可得到准确的定位数据。
77.本技术根据列车的位置选择获取列车的位姿数据，在保证实现列车精确定位的前提下，可以降低处理器的数据处理要求，从而节省了设备成本。
78.选择从gnss/imu传感系统、odo/imu传感系统分别获取列车的位姿数据；或者从gnss/imu传感系统、odo/imu传感系统、lidar/imu传感系统分别获取列车的位姿数据，可以根据列车的粗略位置进行评估。
79.由于gnss具有全天候、全范围覆盖但易受到环境干扰的特点，例如当列车行驶隧道或者在列车站台内时，可能gnss信号较弱甚至丢失，如果仅利用odo/imu传感系统的位姿数据作为补充，仍旧不能达到精准定位的需求，本技术利用lidar数据辅助gnss，解决在gnss信号较弱的场景，自动获取lidar数据和gnss数据以及odo数据和imu数据，来提高定位精度。
80.当列车行驶无遮挡场景中，处理器可以自动地从gnss/imu传感系统、odo/imu传感系统分别获取列车的位姿数据。
81.通过自动识别列车的行驶场景，在多传感器中选择读取部分传感器的传感数据，其自适应地对传感器提供的数据进行处理，有效地提高数据处理效率，并降低数据处理的成本。
82.s102、将列车的位姿数据进行融合，得到列车的定位结果。
83.具体的，当列车行驶的轨道线路上没有遮挡物时，将从gnss/imu传感系统、odo/imu传感系统分别获取列车的位姿数据进行融合；在列车行驶的轨道线路上有遮挡物时，将从gnss/imu传感系统、odo/imu传感系统、lidar/imu传感系统分别获取的列车的位姿数据进行融合。其中融合过程可采用融合算法或融合器执行，例如卡尔曼滤波算法或卡尔曼滤波器。列车的定位结果包括姿态(翻滚角、俯仰角和偏航角)，位置(经度、纬度和高度)，速度、陀螺仪数据和加速度。
84.本技术的实施例，根据列车的当前位置，从gnss、imu、odo、以及lidar传感器的采集数据中选择部分数据或全部数据进行融合计算，得到列车的定位结果，有效地保证定位结果稳定可靠，并在实现精确定位的同时，极大地降低了开发成本。
85.进一步地，图3示出了本发明的实施例中列车定位方法的框架结构示意图。如图3所示，作为可实现的方式，在s101、根据列车的当前位置选择从gnss/imu传感系统、odo/imu传感系统、lidar/imu传感系统中分别获取列车的位姿数据之前，包括：
86.根据车载地图数据确定列车的当前位置是位于站内还是位于站外；
87.当列车的当前位置位于站内时，则从gnss/imu传感系统、odo/imu传感系统和lidar/imu传感系统分别获取列车位姿数据；
88.当列车的当前位置位于站外时，则从gnss/imu传感系统和odo/imu传感系统分别获取列车位姿数据。
89.具体的，列车上存储有车载地图，列车行驶过程中，处理器可从车载地图中读出列
车的当前位置。在车载地图上还以标注轨道线路沿线上的建筑物、数目等其遮挡物。例如，处理器根据列车在车载地图的上的坐标，相应地站台在车载地图上的也有坐标区间，判断列车的坐标，在站台的坐标区间内还是区间外。如果列车的坐标在站台的坐标区间内，则说明列车的当前位置位于站内，如果列车的坐标在站台的坐标区间外，则说明列车的当前位置位于站外。
90.列车在站内需要精准停车，对于列车定位的精确度要求比较高，而列车在站外时，列车正常状态下一直处于行驶状态，对于列车的定位精确度要求相对低一些。
91.本实施方式中，当列车的当前位置处于站内时，gnss信号被确定为不可用或者较弱状态，则处理器会从gnss/imu传感系统、odo/imu传感系统和lidar/imu传感系统分别获取列车位姿数据；当列车的当前位置处于站外时，gnss信号被确定为可用状态，则处理器从gnss/imu传感系统和odo/imu传感系统分别获取列车位姿数据。
92.本实施方式中，由于列车在运行状态正常的情况下，列车在站外包括有树木遮挡或隧道中行驶时，列车一直处于行驶状态，无需精准停车定位，此时对于列车的定位要求精确度相对较低。虽然列车在树木遮挡或隧道中行驶，其获取的gnss信号较弱，但是有imu/odo传感系统作为补充，相对于单独利用gnss定位系统对列车进行定位，本技术采用gnss，imu和odo进行融合定位，其精准度明显增加。本实施方式根据列车的当前位置自动识别定位场景，例如站内和站外，在保证列车精确定位的同时，有效地减少了数据处理量。
93.进一步地，s101、从gnss/imu传感系统、odo/imu传感系统和lidar/imu传感系统分别获取列车位姿数据，具体包括如下：
94.分别获取gnss传感器感测到的gnss数据、imu传感器感测到的imu数据、odo传感器感测到的odo数据和lidar传感器感测到的lidar数据；
95.将gnss数据、odo数据和lidar数据分别与imu数据两两组合得到与gnss/imu传感系统、odo/imu传感系统、lidar/imu传感系统分别对应的列车位姿数据。
96.其中，如上所述，gnss数据包括但不限于经度、纬度、高度等；odo数据包括列车运行方向的速度，速度累积得到列车的里程；lidar数据包括列车的位置三维坐标以及列车周围的环境信息，例如其他列车的行驶速度、行驶轨迹、与本列车的横向及纵向距离，还包括周围建筑物及设施等；imu数据包括加速度、航向角、俯仰角等。gnss数据、odo数据和lidar数据作为普测值，imu作为观测值，通过gnss数据、odo数据和lidar数据去修正imu数据的误差，可以更精确得到列车位姿。gnss/imu传感系统、odo/imu传感系统、lidar/imu传感系统将各自的数据组合后，得到一个修正后的列车位姿数据。例如：列车在当前位置按照100m/s的速度匀速行驶，预测下一秒列车的位姿，如果只通过观测值则可确定出的是列车在当前位置向前行驶了100m，在当前位置的基础上确定出下一秒列车的坐标位置，但是无法得知列车在1s内加速度变化，偏航角变化或翻滚角的变化。结合imu数据，imu数据实时的反映列车行驶过程中的加速度、偏航角或翻滚角的变化，因此能够得知列车在1s内实时变化的列车的位姿，包括列车的坐标位置以及列车的、加速度、偏航角等。
97.在具体的实施例中，gnss数据、odo数据和lidar数据分别与imu数据两两组合得到与gnss/imu传感系统、odo/imu传感系统、lidar/imu传感系统分别对应的列车位姿数据，包括：
98.根据gnss数据和imu数据确定列车的第一位置、速度和第一姿态信息；
99.根据odo数据和imu数据确定列车的里程和第二姿态信息；
100.根据lidar数据和imu数据确定列车的第二位置和第三姿态信息。
101.需要说明的是，此处的第一位置、第一姿态信息、第二姿态信息、第二位置以及第三姿态信息中的“第一”和“第二”只是用于区分。对于列车来说，gnss、imu、odo和lidar采集的数据结合后反映列车的位姿数据应该是一致的。即就是，对于gnss/imu传感系统和lidar/imu传感系统均能确定得到列车的位置、速度和姿态信息，同一辆列车同一时刻下实际位姿信息是相同的，只是采用不同的系统雷车位姿数据之间存在误差而已。
102.进一步地，在上述实施例的基础上，为了进一步说明上述实施例，图4示出了本发明的实施例的中数据融合结构框架。图5示出了本发明的实施例的另一种多传感器融合的列车定位方法的流程示意图。
103.如图4和图5所示，该方法包括如下步骤：
104.s101、根据列车的当前位置选择从gnss/imu传感系统、odo/imu传感系统、lidar/imu传感系统中分别获取列车的位姿数据；
105.具体的，本实施例的步骤s101与上一实施例中的步骤s101相同，此处不在赘述。
106.进一步地，s102、将列车的位姿数据进行融合，得到列车的定位结果，具体包括：
107.s1021、将列车的第一位置、速度和第一姿态信息输入第一卡尔曼滤波器，对建立的列车状态模型进行预测和更新，融合得到与列车的位姿数据的第一估计值；
108.将列车的第一位置、速度和第一姿态信息输入第一卡尔曼滤波器，第一卡尔曼滤波器采用松耦合方式，并采用ekf跟踪和补偿误差。
109.具体如下：gnss传感器采集得到列车的第一位置信息，包括经度、纬度、高度以及速度；imu传感器采集得到列车的第一姿态信息，第一姿态信息包括翻滚角、俯仰角、偏航角、陀螺仪数据、加速度。根据imu采集到的第一姿态信息可以计算得到列车的位置信息。通过gnss采集到的第一位置信息修正通过imu采集到的数据计算得到的列车的位置信息可得到准确的列车的位姿信息。
110.gnss/imu传感系统的状态变量包括姿态误差δ
ψ
(翻滚角、俯仰角和偏航角)，位置误差δ
p
(经度、纬度和高度)，速度误差δv，陀螺仪偏差和加速度计偏差其中，将通过gnss得到的位置记为p
gnss
，通过imu采集到的第一姿态信息计算得到的位置记为p
imu
，两者做矢量相减运算得到系统的误差即为x＝[p
gnss-p
imu
]。因此，通过误差信息表示系统的状态变量x如下：
[0111][0112]
卡尔曼滤波器主要分为预测和更新。列车的状态模型包括建立的运动和观测模型，如下：
[0113]
x(k)＝f(k-1)x(k-1) w(k-1)
[0114]
z(k)＝h(k)x(k) v(k)
[0115]
其中，x(k)为tk时刻列车的预测状态；f(k-1)为状态转移矩阵；z(k)为加入噪声的观测量；h(k)为转换矩阵；w(k-1)和v(k)为状态噪声和测量噪声，状态噪声和测量噪声服从零均值高斯分布，它们对应的协方差矩阵分别为q(k)和r(k)，w(k-1)的初始值可根据经验值设置，v(k)为根据imu、gnss各传感器设备质量自适应调节。
[0116]
预测状态量：
[0117]
预测误差协方差矩阵：p(k|k-1)＝f(k-1)p(k-1)f
t
(k-1) q(k-1)
[0118]
更新卡尔曼增益：
[0119]
k(k)＝p(k|k-1)h
t
(k)[h(k)p(k|k-1)h
t
(k) r(k)]-1
[0120]
更新最优估计状态量：
[0121][0122]
更新最优估计协方差：
[0123]
p(k)＝[i-k(k)h(k|k-1)]p(k|k-1)
[0124]
其中，表示tk时刻的预测位姿状态量，表示t
k-1
时刻的最优位姿状态量，p(k|k-1)表示tk时刻的预测误差协方差矩阵，p(k-1)是t
k-1
时刻最优误差协方差矩阵，r(k)表示系统测量噪声协方差矩阵，k(k)表示滤波增益，p(k)表示最优估计的误差协方差矩阵，表示列车的位姿数据的第一估计值，即为融合后的列车状态，由预测和更新得到的状态两部分组成。列车在行驶过程中，重复上述预测和更新过程，及完成卡尔曼滤波过程，最终得到较为准确的列车位姿数据的第一估计值。
[0125]
s1022、将列车的里程和第二姿态信息输入第二卡尔曼滤波器，对列车状态模型进行预测和更新，融合得到与列车的位姿数据的第二估计值；
[0126]
将里程数据和第二姿态信息输入第二卡尔曼滤波器，跟踪和补偿误差。
[0127]
具体如下：odo传感器采集得到列车的里程，里程数据可表示列车的位置信息；imu传感器采集得到列车的第二姿态信息，第二姿态信息包括翻滚角、俯仰角、偏航角、陀螺仪数据、加速度。根据imu传感器采集得到的第二姿态信息可以计算得到列车的位置信息。通过odo采集的里程数据修正通过imu采集得到的第二姿态信息计算得到的列车的位置信息可得到准确的列车的位姿信息。
[0128]
odo/imu传感系统状态包括姿态误差δ
ψ
(翻滚角、俯仰角和偏航角)，位置误差δ
p
(经度、纬度和高度)，速度误差δv，陀螺仪偏差和加速度计偏差其中，由odo获得的位置记为p
odo
和imu采集到的第二姿态信息计算得到的位置记为p
imu
，两者做矢量相减运算得到系统的误差即为x＝[p
odo-p
imu
]。因此，通过误差信息表示系统的状态变量x可以表示为：
[0129][0130]
卡尔曼滤波器主要分为预测和更新。列车的状态模型包括建立的运动和观测模型。如下：
[0131]
x(k)＝f(k-1)x(k-1) w(k-1)
[0132]
z(k)＝h(k)x(k) v(k)
[0133]
其中，x(k)为tk时刻列车的预测状态；f(k-1)为状态转移矩阵；z(k)为加入噪声的观测量；h(k)为转换矩阵；w(k-1)和v(k)为状态和测量噪声，服从零均值高斯分布，它们的协方差矩阵分别为q(k)和r(k)。，w(k-1)的初始值可根据经验值设置，v(k)为根据imu、odo各传感器设备质量自适应调节。
[0134]
预测状态量：
[0135]
预测误差协方差矩阵：p(k|k-1)＝f(k-1)p(k-1)f
t
(k-1) q(k-1)
[0136]
更新卡尔曼增益：
[0137]
k(k)＝p(k|k-1)h
t
(k)[h(k)p(k|k-1)h
t
(k) r(k)]-1
[0138]
更新最优估计状态量：
[0139][0140]
更新最优估计协方差：
[0141]
p(k)＝[i-k(k)h(k|k-1)]p(k|k-1)
[0142]
其中，表示tk时刻的预测位姿状态量，表示t
k-1
时刻的最优位姿状态量，p(k|k-1)表示tk时刻的预测误差协方差矩阵，p(k-1)是t
k-1
时刻最优误差协方差矩阵，r(k)表示系统测量噪声协方差矩阵，k(k)表示滤波增益，p(k)表示最优估计的误差协方差矩阵，表示列车的位姿数据的第二估计值，即为融合后的列车状态，由预测和更新得到的状态两部分组成。列车在行驶过程中，重复上述预测和更新过程，及完成卡尔曼滤波过程，最终得到较为准确的列车位姿数据的第二估计值。
[0143]
s1023、将列车的第二位置和第三姿态信息输入第三卡尔曼滤波器，对列车状态模型进行预测和更新，融合得到列车的位姿数据的第三估计值；
[0144]
lidar的定位不受光照影响，稳定性高，定位精度高。lidar采集得到的点云数据中确定出列车的第二位置，imu传感器采集得到列车的第三姿态信息，第三姿态信息包括翻滚角、俯仰角、偏航角、陀螺仪数据、加速度。根据imu传感器采集得到的第三姿态信息可以计算得到列车的位置信息。通过lidar确定列车的第二位置修正iimu传感器采集得到的第三姿态信息计算得到的列车的位置信息可得到准确的列车的位姿信息。
[0145]
lidar/imu传感系统状态包括姿态误差δ
ψ
(翻滚角、俯仰角和偏航角)，位置误差δ
p
(经度、纬度和高度)，速度误差δv，陀螺仪偏差和加速度计偏差其中，由于lidar定位可以输出列车的位置和航向角，将通过lidar得到的位置记为p
lidar
，航向角记为h
pidar
，通过imu采集到的第二位姿数据计算得到位置记为p
imu
，航向角记为h
imu
，两者对应做矢量相减运算得到系统的误差记为。运算得到系统的误差记为。因此通过误差信息表示系统的状态变量x，如下：
[0146][0147]
卡尔曼滤波器主要分为预测和更新。首先建立系统模型如下：
[0148]
x(k)＝f(k-1)x(k-1) w(k-1)
[0149]
z(k)＝h(k)x(k) v(k)
[0150]
其中，x(k)为tk时刻列车的预测状态；f(k-1)为状态转移矩阵；z(k)为加入噪声的观测值；h(k)为转换矩阵；w(k-1)和v(k)为状态和测量噪声，服从零均值高斯分布，它们的协方差矩阵分别为q(k)和r(k)。
[0151]
预测状态量：
[0152]
预测误差协方差矩阵：p(k|k-1)＝f(k-1)p(k-1)f
t
(k-1) q(k-1)
[0153]
更新卡尔曼增益：
[0154]
k(k)＝p(k|k-1)h
t
(k)[h(k)p(k|k-1)h
t
(k) r(k)]-1
[0155]
更新最优估计状态量：
[0156][0157]
更新最优估计协方差：
[0158]
p(k)＝[i-k(k)h(k|k-1)]p(k|k-1)
[0159]
其中，表示tk时刻的预测位姿状态量，表示t
k-1
时刻的最优位姿状态量，p(k|k-1)表示tk时刻的预测误差协方差矩阵，p(k-1)是t
k-1
时刻最优误差协方差矩阵，r(k)表示系统量测噪声协方差矩阵，k(k)表示滤波增益，p(k)表示最优估计的误差协方差矩阵，表示列车的位姿数据的第三估计值，即为融合后的列车状态，由预测和更新得到的状态两部分组成。列车在行驶过程中，重复上述预测和更新过程，及完成卡尔曼滤波过程，最终得到较为准确的列车位姿数据的第三估计值。
[0160]
s1024、将第一估计值、第二估计值和第三估计值输入第四卡尔曼滤波器，对列车状态模型进行预测得到列车的第四估计值，并将第四估计值分别反馈至第一滤波器和第三滤波器进行更新融合，直到收敛得到列车的定位结果。
[0161]
具体的，本实施中分别采用第一卡尔曼滤波器、第二卡尔曼滤波器和第三卡尔曼滤波器对gnss/imu传感系统、odo/imu传感系统和lidar/imu传感系统对应的列车位姿数据进行融合得到第一估计值、第二估计值和第三估计值，然后再将第一估计值、第二估计值和第三估计值分别输入到第四卡尔曼滤波器中进行全局估计。相比于现有的集中式卡尔曼滤波器，本技术的方式会降低数据维数，减少计算量。
[0162]
其中第四滤波器和上述第一滤波器、第二滤波器、第三滤波器共用系统模型，系统的过程信息按照如下分配：
[0163][0164]
其中，i＝1，2，3，m，分别对应gnss/imu传感系统对应的第一滤波器，odo/imu传感系统对应的第二滤波器，lidar/imu传感系统对应的第三滤波器，第四滤波器，β是信息分配因子，并满足β1 β2 β3 βm＝1。将第一滤波器、第二滤波器、第三滤波器统称为子滤波器，第四滤波器称为主滤波器。
[0165]
则各子滤波器和主滤波器的预测如下：
[0166][0167][0168]
其中，i＝1，2，3，m，表示tk时刻的最优误差协方差矩阵，表示tk时刻的最优位姿状态量；由于主滤波器没有测量值，所以主滤波器没有测量更新，测量更新只在各个子滤波器中进行，主滤波器预测后的输出值作为第一滤波器和第三滤波器的输入值，对子滤波器进行更新，计算方式如下：
[0169]
[0170][0171][0172]
其中i＝1，2，3；
[0173]
可认为主滤波器的更新如下：
[0174][0175][0176]
假设各子系统之间相互独立，那么最终全局信息可用如下方式估计：
[0177][0178][0179]
其中，下标f对应全局信息。重复对上述预测各更新过程得到列车的精确位置，列车的定位结果包括预测状态量和更新后的状态量，本技术的实施例得到的列车的定位结果的表现形式为三维矢量坐标。
[0180]
综上所述，本发明的多传感器融合的列车定位方法，将gnss、imu、odo、以及lidar传感器的采集数据进行融合，根据列车的当前位置获取不同系统采集的列车的位姿数据；对从gnss/imu传感系统、odo/imu传感系统、lidar/imu传感系统分别获取的列车的位姿数据先分别进行融合得到估计值，再输入到主滤波器中进行融合获得列车的定位结果，有利于降低数据维数，减少计算量。本技术提出的多传感器融合定位方法，有效地提高了列车定位的精确性，在实现精确定位的同时极大地降低了对处理器计算力要求，可以减轻开发成本。
[0181]
另一方面，本技术的实施例提供一种多传感器融合的列车定位装置，如图6所示，该装置包括：
[0182]
获取模块301，用于根据列车的当前位置选择从gnss/imu传感系统、odo/imu传感系统、lidar/imu传感系统中分别获取列车的位姿数据；
[0183]
融合模块302，用于将列车的位姿数据进行融合，得到列车的定位结果。
[0184]
进一步地，如图7所示，本技术的实施例提供另一种多传感器融合的列车定位装置，包括：
[0185]
判断模块300，用于根据车载地图数据确定列车的当前位置位于站内还是位于站外；
[0186]
第一获取模块3011，当列车的当前位置位于站内时，则从gnss/imu传感系统、odo/imu传感系统和lidar/imu传感系统分别获取列车的位姿数据；
[0187]
第二获取模块3012，当列车的当前位置位于站外时，则从gnss/imu传感系统和odo/imu传感系统分别获取列车的位姿数据。
[0188]
可选地，获取模块301还用于执行：
[0189]
分别获取gnss传感器感测到的gnss数据、imu传感器感测到的imu数据、odo传感器感测到的odo数据和lidar传感器感测到的lidar数据；
[0190]
将gnss数据、odo数据和lidar数据分别与imu数据两两组合得到与gnss/imu传感
系统、odo/imu传感系统、lidar/imu传感系统分别对应的列车位姿数据。
[0191]
可选地，获取模块301还用于执行：
[0192]
根据gnss数据和imu数据确定列车的第一位置、速度和第一姿态信息；
[0193]
根据odo数据和imu数据确定列车的里程和第二姿态信息；
[0194]
根据lidar数据和imu数据确定列车的第二位置和第三姿态信息。
[0195]
进一步地，融合模块302用于执行：
[0196]
将列车的第一位置、速度和第一姿态信息输入卡第一卡尔曼滤波器，对预先建立的状态模型进行预测和更新，融合得到与列车的位姿数据的第一估计值；
[0197]
将列车的里程和第二姿态信息输入第二卡尔曼滤波器，对状态模型进行预测和更新，融合得到与列车的位姿数据的第二估计值；
[0198]
将列车的第二位置和第三姿态信息输入第三卡尔曼滤波器，对状态模型进行预测和更新，融合得到列车的位姿数据的第三估计值；
[0199]
将第一估计值、第二估计值和第三估计值输入第四卡尔曼滤波器，对状态模型进行预测得到列车的第四估计值，并将第四估计值分别反馈至第一滤波器和第三滤波器进行更新融合，直到收敛得到列车的定位结果。
[0200]
本实施例的定位装置和上述定位方法的原理和效果相同，在此不再赘述。
[0201]
应当理解，列车定位装置中记载的诸单元或模块与参考图2描述的方法中的各个步骤相对应。由此，上文针对方法描述的操作和特征同样适用于列车定位装置及其中包含的单元，在此不再赘述。列车定位装置可以预先实现在电子设备的浏览器或其他安全应用中，也可以通过下载等方式而加载到电子设备的浏览器或其安全应用中。列车定位装置中的相应单元可以与电子设备中的单元相互配合以实现本技术实施例的方案。在上文详细描述中提及的若干模块或者单元，这种划分并非强制性的。实际上，根据本公开的实施方式，上文描述的两个或更多模块或者单元的特征和功能可以在一个模块或者单元中具体化。反之，上文描述的一个模块或者单元的特征和功能可以进一步划分为由多个模块或者单元来具体化。
[0202]
另一方面，本技术的实施例提供一种列车定位系统400，如图8所示，定位系统包括：
[0203]
设置于列车上的gnss传感器401、imu传感器402、odo传感器403和lidar传感器404，gnss传感器401用于采集列车的第一位置和速度、imu传感器402用于采集列车的姿态信息、odo传感器403用于采集列车的里程以及lidar传感器404用于采集列车的第二位置；
[0204]
以及处理器405，处理器405分别与gnss传感器401、imu传感器402、odo传感器403和lidar传感器404信号连接，处理器405包括第一滤波器406、第二滤波器407、第三滤波器408和第四滤波器409；
[0205]
第一滤波器406，与gnss传感器401的输出端和imu传感器的输出端402连接，用于将第一位置、速度和姿态信息融合得到列车位姿数据的第一估计值；
[0206]
第二滤波器407，与odo传感器403的输出端和imu传感器402的输出端连接，用于将里程和姿态信息融合得到列车位姿数据的第二估计值；
[0207]
第三滤波器408，与lidar传感器404的输出端和imu传感器402的输出端连接，用于将第二位置和姿态信息融合得到列车位姿数据的第三估计值；
[0208]
第四滤波器409，分别于第一滤波器406、第二滤波器407、第三滤波器408的输出端连接，用于将第一估计值、第二估计值和第三估计值进行融合，并将融合得到的第四估计值分别反馈至第一滤波器和第三滤波器进行更新融合，直至收敛得到列车的定位结果。
[0209]
另一方面，本技术的实施例提供一种列车，该列车包括处理器，处理器用于执行第一方面的多传感器融合的列车定位方法。
[0210]
下面参考图9，图9为本技术实施例的列车的计算机系统的结构示意图。
[0211]
如图9所示，计算机系统500包括中央处理单元(cpu)501，其可以根据存储在只读存储器(rom)502中的程序或者从存储部分508加载到随机访问存储器(ram)503中的程序而执行各种适当的动作和处理。在ram 503中，还存储有系统500操作所需的各种程序和数据。cpu 501、rom 502以及ram 503通过总线504彼此相连。输入/输出(i/o)接口505也连接至总线504。
[0212]
以下部件连接至i/o接口505：包括键盘、鼠标等的输入部分506；包括诸如阴极射线管(crt)、液晶显示器(lcd)等以及扬声器等的输出部分507；包括硬盘等的存储部分508；以及包括诸如lan卡、调制解调器等的网络接口卡的通信部分509。通信部分509经由诸如因特网的网络执行通信处理。驱动器510也根据需要连接至i/o接口505。可拆卸介质511，诸如磁盘、光盘、磁光盘、半导体存储器等等，根据需要安装在驱动器510上，以便于从其上读出的计算机程序根据需要被安装入存储部分508。
[0213]
特别地，根据本公开的实施例，上文参考图2描述的过程可以被实现为计算机软件程序。例如，本公开的实施例包括一种计算机程序产品，其包括有形地包含在机器可读介质上的计算机程序，该计算机程序包含用于执行图1-2的方法的程序代码。在这样的实施例中，该计算机程序可以通过通信部分509从网络上被下载和安装，和/或从可拆卸介质511被安装。
[0214]
需要说明的是，本公开所示的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(ram)、只读存储器(rom)、可擦式可编程只读存储器(eprom或闪存)、光纤、便携式紧凑磁盘只读存储器(cd-rom)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本公开中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本公开中，计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：无线、电线、光缆、rf等等，或者上述的任意合适的组合。
[0215]
附图中的流程图和框图，图示了按照本发明各种实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代
表一个模块、程序段、或代码的一部分，前述模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
[0216]
描述于本技术实施例中所涉及到的单元或模块可以通过软件的方式实现，也可以通过硬件的方式来实现。所描述的单元或模块也可以设置在处理器中，例如，可以描述为：一种处理器包括获取模块以及融合模块。其中，这些单元或模块的名称在某种情况下并不构成对该单元或模块本身的限定，例如，融合模块还可以被描述为“用于将所述列车的位姿数据进行融合，得到列车的定位结果的单元”。
[0217]
作为另一方面，本技术还提供了一种计算机可读存储介质，该计算机可读存储介质可以是上述实施例中前述装置中所包含的计算机可读存储介质；也可以是单独存在，未装配入设备中的计算机可读存储介质。计算机可读存储介质存储有一个或者一个以上程序，前述程序被一个或者一个以上的处理器用来执行描述于本技术的应用于列车定位。
[0218]
以上描述仅为本技术的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解，本技术中所涉及的发明范围，并不限于上述技术特征的特定组合而成的技术方案，同时也应涵盖在不脱离所述发明构思的情况下，由上述技术特征或其等同特征进行任意组合而形成的其它技术方案。例如上述特征与本技术中公开的(但不限于)具有类似功能的技术特征进行互相替换而形成的技术方案。