CN108731667B - 用于确定无人驾驶车辆的速度和位姿的方法和装置 - Google Patents

用于确定无人驾驶车辆的速度和位姿的方法和装置 Download PDF

Info

Publication number
CN108731667B
CN108731667B CN201710245249.2A CN201710245249A CN108731667B CN 108731667 B CN108731667 B CN 108731667B CN 201710245249 A CN201710245249 A CN 201710245249A CN 108731667 B CN108731667 B CN 108731667B
Authority
CN
China
Prior art keywords
error
sins
speed
pose
imu
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201710245249.2A
Other languages
English (en)
Other versions
CN108731667A (zh
Inventor
杨晓龙
宋适宇
徐宝强
蔡仁澜
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Baidu Netcom Science and Technology Co Ltd
Original Assignee
Beijing Baidu Netcom Science and Technology Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Baidu Netcom Science and Technology Co Ltd filed Critical Beijing Baidu Netcom Science and Technology Co Ltd
Priority to CN201710245249.2A priority Critical patent/CN108731667B/zh
Publication of CN108731667A publication Critical patent/CN108731667A/zh
Application granted granted Critical
Publication of CN108731667B publication Critical patent/CN108731667B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本申请公开了用于确定无人驾驶车辆的速度和位姿的方法和装置。该无人驾驶车辆设置有全球导航卫星系统GNSS接收机、捷联惯性导航系统SINS以及轮速传感器，该SINS包括惯性测量单元IMU，该方法的一具体实施方式包括：获取SINS解算的无人驾驶车辆的第一速度、第一位姿和轮速传感器采集的无人驾驶车辆的轮速；确定GNSS接收机是否接收到信号；响应于确定GNSS接收机接收不到信号，根据轮速、第一速度和第一位姿，采用卡尔曼滤波算法确定SINS的速度误差、SINS的位姿误差以及IMU的零偏误差；根据速度误差、位姿误差和零偏误差，对第一速度和第一位姿进行调整，得到无人驾驶车辆的速度和位姿。

Description

用于确定无人驾驶车辆的速度和位姿的方法和装置
技术领域
本申请涉及机动车技术领域，具体涉及无人驾驶车辆技术领域，尤其涉及用于确定无人驾驶车辆的速度和位姿的方法和装置。
背景技术
目前在确定无人驾驶车辆的速度和位姿时，通常采用在车辆中设置SINS(Strap-down Inertial Navigation System，捷联惯性导航系统)和GNSS(Global NavigationSatellite System，全球导航卫星系统)接收机解算的数据来确定无人驾驶车辆的速度和位姿。
在GNSS接收机接收到信号的时候，采用SINS和GNSS接收机输出的信息共同确定无人驾驶车辆的速度和位姿，在GNSS接收机接收不到信号的时候，只能采用SINS输出的信息确定无人驾驶车辆的速度和位姿，在SINS的精度较低的情况下，降低了确定无人驾驶车辆的速度和位姿的精度。从而现有的确定无人驾驶车辆的速度和位姿的方法存在着精度较低的问题。
发明内容
本申请的目的在于提出一种改进的用于确定无人驾驶车辆的速度和位姿的方法和装置，来解决以上背景技术部分提到的技术问题。
第一方面，本申请实施例提供了一种用于确定无人驾驶车辆的速度和位姿的方法，上述无人驾驶车辆设置有GNSS接收机、SINS以及轮速传感器，上述SINS包括IMU(Inertial Measurement Unit，惯性测量单元)该方法包括：获取上述SINS解算的上述无人驾驶车辆的第一速度、第一位姿和上述轮速传感器采集的上述无人驾驶车辆的轮速；确定上述GNSS接收机是否接收到信号；响应于确定上述GNSS接收机接收不到信号，根据上述轮速和上述第一速度，采用卡尔曼滤波算法确定上述SINS的速度误差、上述SINS的位姿误差以及上述IMU的零偏误差；根据上述速度误差和上述位姿误差，对上述第一速度和上述第一位姿进行调整，得到上述无人驾驶车辆的速度和位姿。
在一些实施例中，上述根据上述速度误差和上述位姿误差，对上述第一速度和上述第一位姿进行调整，得到上述无人驾驶车辆的速度和位姿之前，上述方法还包括：响应于确定上述GNSS接收机接收到信号，获取上述GNSS接收机解算的上述无人驾驶车辆的第二速度和第二位置，并根据上述第一速度、上述第一位姿、上述第二速度和上述第二位置，采用卡尔曼滤波算法确定上述SINS的速度误差、上述SINS的位姿误差以及上述IMU的零偏误差。
在一些实施例中，上述第一位姿包括第一位置和第一姿态；以及上述根据上述第一速度、上述第一位姿、上述第二速度和上述第二位置，采用卡尔曼滤波算法确定上述SINS的速度误差、上述SINS的位姿误差以及上述IMU的零偏误差，包括：执行以下第一滤波操作：以上述SINS的速度误差、上述SINS的位姿误差和上述IMU的零偏误差为状态变量构建第一卡尔曼滤波的状态方程，以上述第一速度与上述第二速度之差以及上述第一位置与上述第二位置之差作为观测量构建上述第一卡尔曼滤波的观测方程，采用卡尔曼滤波算法得到上述SINS的速度误差、上述SINS的位姿误差和上述IMU的零偏误差。
在一些实施例中，在上述执行以下第一滤波操作之后，上述方法还包括：确定上述GNSS接收机是否工作在实时动态差分定位模式下；响应于确定上述GNSS接收机工作在实时动态差分定位模式下，执行以下第二滤波操作：以上述轮速传感器的尺度因子误差和上述轮速传感器与上述IMU之间的安装角误差作为状态变量构建第二卡尔曼滤波的状态方程，以上述轮速与上述第二速度之差作为观测量构建上述第二卡尔曼滤波的观测方程，采用卡尔曼滤波算法得到上述尺度因子误差和上述安装角误差。
在一些实施例中，上述根据上述轮速和上述第一速度，采用卡尔曼滤波算法确定上述SINS的速度误差、上述SINS的位姿误差以及上述IMU的零偏误差，包括：以上述SINS的速度误差、上述SINS的位姿误差和上述IMU的零偏误差为状态变量构建第一卡尔曼滤波的状态方程，以上述第一速度与上述轮速之差作为观测量构建上述第一卡尔曼滤波的观测方程，采用卡尔曼滤波算法得到上述SINS的速度误差、上述SINS的位姿误差和上述IMU的零偏误差。
在一些实施例中，上述根据上述轮速和上述第一速度，采用卡尔曼滤波算法确定上述SINS的速度误差、上述SINS的位姿误差以及上述IMU的零偏误差，包括：根据上述尺度因子误差和上述安装角误差对上述轮速进行调整，得到调整后的轮速；以上述SINS的速度误差、上述SINS的位姿误差和上述IMU的零偏误差为状态变量构建上述第一卡尔曼滤波的状态方程，以上述第一速度与上述调整后的轮速之差作为观测量构建上述第一卡尔曼滤波的观测方程，采用卡尔曼滤波算法得到上述SINS的速度误差、上述SINS的位姿误差和上述IMU的零偏误差。
第二方面，本申请实施例提供了一种用于确定无人驾驶车辆的速度和位姿的装置，上述无人驾驶车辆设置有GNSS接收机、SINS以及轮速传感器，上述SINS包括IMU，该装置包括：获取单元，配置用于获取上述SINS解算的上述无人驾驶车辆的第一速度、第一位姿和上述轮速传感器采集的上述无人驾驶车辆的轮速；信号确定单元，配置用于确定上述GNSS接收机是否接收到信号；第一误差确定单元，配置用于响应于确定上述GNSS接收机接收不到信号，根据上述轮速和上述第一速度，采用卡尔曼滤波算法确定上述SINS的速度误差、上述SINS的位姿误差以及上述IMU的零偏误差；调整单元，配置用于根据上述速度误差和上述位姿误差，对上述第一速度和上述第一位姿进行调整，得到上述无人驾驶车辆的速度和位姿。
在一些实施例中，上述装置还包括：第二误差确定单元，配置用于响应于确定上述GNSS接收机接收到信号，获取上述GNSS接收机解算的上述无人驾驶车辆的第二速度和第二位置，并根据上述第一速度、上述第一位姿、上述第二速度和上述第二位置，采用卡尔曼滤波算法确定上述SINS的速度误差、上述SINS的位姿误差以及上述IMU的零偏误差。
在一些实施例中，上述第一位姿包括第一位置和第一姿态；以及上述第二误差确定单元，包括：第一滤波模块，配置用于执行以下第一滤波操作：以上述SINS的速度误差、上述SINS的位姿误差和上述IMU的零偏误差为状态变量构建第一卡尔曼滤波的状态方程，以上述第一速度与上述第二速度之差以及上述第一位置与上述第二位置之差作为观测量构建上述第一卡尔曼滤波的观测方程，采用卡尔曼滤波算法得到上述SINS的速度误差、上述SINS的位姿误差和上述IMU的零偏误差。
在一些实施例中，上述第二误差确定单元，还包括：定位模式确定模块，配置用于确定上述GNSS接收机是否工作在实时动态差分定位模式下；第二滤波模块，配置用于响应于确定上述GNSS接收机工作在实时动态差分定位模式下，执行以下第二滤波操作：以上述轮速传感器的尺度因子误差和上述轮速传感器与上述IMU之间的安装角误差作为状态变量构建第二卡尔曼滤波的状态方程，以上述轮速与上述第二速度之差作为观测量构建上述第二卡尔曼滤波的观测方程，采用卡尔曼滤波算法得到上述尺度因子误差和上述安装角误差。
在一些实施例中，上述第一误差确定单元进一步配置用于：以上述SINS的速度误差、上述SINS的位姿误差和上述IMU的零偏误差为状态变量构建第一卡尔曼滤波的状态方程，以上述第一速度与上述轮速之差作为观测量构建上述第一卡尔曼滤波的观测方程，采用卡尔曼滤波算法得到上述SINS的速度误差、上述SINS的位姿误差和上述IMU的零偏误差。
在一些实施例中，上述第一误差确定单元进一步配置用于：根据上述尺度因子误差和上述安装角误差对上述轮速进行调整，得到调整后的轮速；以上述SINS的速度误差、上述SINS的位姿误差和上述IMU的零偏误差为状态变量构建上述第一卡尔曼滤波的状态方程，以上述第一速度与上述调整后的轮速之差作为观测量构建上述第一卡尔曼滤波的观测方程，采用卡尔曼滤波算法得到上述SINS的速度误差、上述SINS的位姿误差和上述IMU的零偏误差。
第三方面，本申请实施例提供了一种无人驾驶车辆，该无人驾驶车辆包括：全球导航卫星系统接收机，用于解算无人驾驶车辆的速度和位置；捷联惯性导航系统，用于解算无人驾驶车辆的速度和位姿；轮速传感器，用于采集无人驾驶车辆的轮速；一个或多个处理器；存储装置，用于存储一个或多个程序，当上述一个或多个程序被上述一个或多个处理器执行时，使得上述一个或多个处理器实现如第一方面中任一实现方式描述的方法。
第四方面，本申请实施例提供了一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该计算机程序被处理器执行时实现如第一方面中任一实现方式描述的方法。
本申请实施例提供的用于确定无人驾驶车辆的速度和位姿的方法和装置，通过在无人驾驶车辆中设置GNSS接收机、SINS以及轮速传感器，并在GNSS接收机接收不到信号的时候，根据从SINS获取的速度和从轮速传感器获取的轮速，采用卡尔曼滤波算法确定SINS的速度误差、SINS的位姿误差以及SINS中的IMU的零偏误差，并根据所确定的各个误差最终确定无人驾驶车辆的速度和位姿。从而提高了确定无人驾驶车辆的速度和位姿的精度，并继而提高了无人驾驶车辆的定位精度。
附图说明
通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本申请的其它特征、目的和优点将会变得更明显：
图1是本申请可以应用于其中的示例性系统架构图；
图2是根据本申请的用于确定无人驾驶车辆的速度和位姿的方法的一个实施例的流程图；
图3是根据本申请的用于确定无人驾驶车辆的速度和位姿的方法的一个应用场景的示意图；
图4是根据本申请的用于确定无人驾驶车辆的速度和位姿的方法的又一个实施例的流程图；
图5是根据本申请的用于确定无人驾驶车辆的速度和位姿的装置的一个实施例的结构示意图；
图6是适于用来实现本申请实施例的电子设备的计算机系统的结构示意图。
具体实施方式
下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与有关发明相关的部分。
需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。
图1示出了可以应用本申请的用于确定无人驾驶车辆的速度和位姿的方法或用于确定无人驾驶车辆的速度和位姿的装置的实施例的示例性系统架构100。
如图1所示，系统架构100可以包括无人驾驶车辆101。无人驾驶车辆101上可以安装有驾驶控制设备1011、网络1012、GNSS接收机1013、SINS 1014和轮速传感器1015。网络1012用以在驾驶控制设备1011和GNSS接收机1013之间、驾驶控制设备1011与SINS 1014之间以及驾驶控制设备1011与轮速传感器1015之间提供通信链路的介质。网络1012可以包括各种连接类型，例如有线、无线通信链路或者光纤电缆等等。
GNSS接收机1013用于与GNSS通信，并解算得到无人驾驶车辆101的速度和位置信息。
SINS 1014用于根据解算得到无人驾驶车辆101的速度和位姿信息。SINS 1014可以包括IMU(惯性测量单元)。作为示例，IMU可以包括三个单轴的加速度计和三个单轴的陀螺仪，加速度计可以检测无人驾驶车辆101在沿车体坐标系的三个相互独立的坐标轴方向上的加速度信号，而陀螺仪可以检测无人驾驶车辆101相对于导航坐标系的角速度信号。有了IMU测量得到的无人驾驶车辆101在三维空间中的角速度和加速度，SINS可以解算出无人驾驶车辆的速度和位姿(即，位置和姿态)。可以理解的是，为了提高可靠性，还可以配备更多的传感器以测量无人驾驶车辆101沿车体坐标系的三个坐标轴方向的信号。一般而言，IMU可以安装在无人驾驶车辆101的重心处。
驾驶控制设备(又称为车载大脑)1011负责无人驾驶车辆的智能控制。驾驶控制设备1011可以是单独设置的控制器，例如可编程逻辑控制器(Programmable LogicController，PLC)、单片机、工业控制机等；也可以是由其他具有输入/输出端口，并具有运算控制功能的电子器件组成的设备；还可以是安装有车辆驾驶控制类应用的计算机设备。
需要说明的是，本申请实施例所提供的用于确定无人驾驶车辆的速度和位姿的方法一般由驾驶控制设备1011执行，相应地，用于确定无人驾驶车辆的速度和位姿的装置一般设置于驾驶控制设备1011中。
应该理解，图1中的驾驶控制设备、GNSS接收机、SINS和轮速传感器的数目仅仅是示意性的。根据实现需要，可以具有任意数目的驾驶控制设备、GNSS接收机、SINS和轮速传感器。
继续参考图2，其示出了根据本申请的用于确定无人驾驶车辆的速度和位姿的方法的一个实施例的流程200。该用于确定无人驾驶车辆的速度和位姿的方法，包括以下步骤：
步骤201，获取SINS解算的无人驾驶车辆的第一速度、第一位姿和轮速传感器采集的无人驾驶车辆的轮速。
在本实施例中，用于确定无人驾驶车辆的速度和位姿的方法运行于其上的电子设备(例如图1所示的驾驶控制设备)可以通过有线连接方式或者无线连接方式获取SINS解算的无人驾驶车辆的第一速度、第一位姿，以及获取轮速传感器采集的无人驾驶车辆的轮速。其中，SINS中可以设置有IMU，SINS可以根据IMU采集的加速度信息和角速度信息进行解算，得到无人驾驶车辆的第一速度和第一位姿。需要说明的是，具体如何解算是目前广泛研究和应用的现有技术，在此不再赘述。
在本实施例的一些可选的实现方式中，位姿可以包括位置和姿态，其中，姿态可以包括东向姿态角、北向姿态角和天向姿态角。
步骤202，确定GNSS接收机是否接收到信号。
在本实施例中，GNSS接收机用于接收GNSS发来的信号，当无人驾驶车辆上设置的GNSS接收机处于一些特定地点(例如，隧道)时，GNSS接收机接收不到GNSS信号。上述电子设备(例如图1所示的驾驶控制设备)可以确定GNSS接收机是否接收到信号。具体地，这里的信号可以为全球导航卫星信号。
步骤203，响应于确定GNSS接收机接收不到信号，根据轮速和第一速度，采用卡尔曼滤波算法确定SINS的速度误差、SINS的位姿误差以及IMU的零偏误差。
在本实施例中，用于确定无人驾驶车辆的速度和位姿的方法运行于其上的电子设备可以在步骤202中确定GNSS接收机接收不到信号的情况下，根据轮速传感器采集的无人驾驶车辆的轮速和SINS解算的无人驾驶车辆的第一速度，采用卡尔曼滤波算法确定SINS的速度误差、SINS的位姿误差以及IMU的零偏误差。
具体地，上述电子设备可以以SINS的速度误差、SINS的位姿误差和IMU的零偏误差为状态变量构建第一卡尔曼滤波的状态方程，以第一速度与轮速之差作为观测量构建第一卡尔曼滤波的观测方程，采用卡尔曼滤波算法得到SINS的速度误差、SINS的位姿误差和IMU的零偏误差。需要说明的是，卡尔曼滤波算法是本领域广泛研究和应用的现有技术，在此不再赘述。
步骤204，根据SINS的速度误差和SINS的位姿误差，对第一速度和第一位姿进行调整，得到无人驾驶车辆的速度和位姿。
在本实施例中，上述电子设备可以根据步骤203中确定的SINS的速度误差、SINS的位姿误差和IMU的零偏误差，对第一速度和第一位姿进行调整，得到无人驾驶车辆的速度和位姿。
作为示例，上述电子设备可以将第一速度减去SINS的速度误差得到的速度确定为无人驾驶车辆的速度，将第一位姿减去SINS的位姿误差得到的位姿确定为无人驾驶车辆的位姿。
继续参见图3，图3是根据本实施例的用于确定无人驾驶车辆的速度和位姿的方法的应用场景的一个示意图。在图3的应用场景中，无人驾驶车辆的驾驶控制设备从SINS获取第一速度和第一位姿，并从轮速传感器获取轮速，如图3中附图标记301所示。然后，驾驶控制设备在确定GNSS接收机接收不到信号的情况下，根据所获取的第一速度和轮速确定SINS的速度误差和位姿误差以及IMU的零偏误差，并最终确定无人驾驶车辆的速度和位姿以及输出无人驾驶车辆的速度和位姿给定位系统，如图3中附图标记302所示。其中，定位系统可以是无人驾驶车辆中的其他电子设备，定位系统也可以是驾驶控制设备中安装的定位类应用。
本申请的上述实施例提供的方法通过在GNSS接收机接收不到信号的时候，根据SINS解算的速度和位姿以及轮速传感器采集的轮速确定无人驾驶车辆的速度和位姿，从而提高了确定无人驾驶车辆的速度和位姿的精度。
进一步参考图4，其示出了用于确定无人驾驶车辆的速度和位姿的方法的又一个实施例的流程400。该用于确定无人驾驶车辆的速度和位姿的方法的流程400，包括以下步骤：
步骤401，获取SINS解算的无人驾驶车辆的第一速度、第一位姿和轮速传感器采集的无人驾驶车辆的轮速。
在本实施例中，步骤401的具体操作与图2所示的实施例中步骤201的操作基本相同，在此不再赘述。
步骤402，确定GNSS接收机是否接收到信号，如果接收不到接收不到信号，转到步骤403，如果接收到信号，转到步骤403’。
在本实施例中，上述电子设备(例如图1所示的驾驶控制设备)可以确定GNSS接收机是否接收到信号，并在确定接收不到信号的情况下，转到步骤403，在确定接收到信号的情况下，转到步骤403’。
步骤403，根据车速传感器的尺度因子误差和车速传感器与IMU之间的安装角误差对轮速进行调整，得到调整后的轮速。
在本实施例中，上述电子设备(例如图1所示的驾驶控制设备)可以在步骤402中判断GNSS接收机接收不到信号的情况下，首先根据步骤406’中在上一周期执行第二滤波操作所得到的车速传感器的尺度因子误差和车速传感器与IMU之间的安装角误差对轮速进行调整，得到调整后的轮速。执行完步骤403后，转到步骤404。
这里，车速传感器的尺度因子是用于表征无人驾驶车辆的真实速度与轮速传感器采集的轮速之间的比值。
作为示例，下面给出一种具体实现方式：
Figure BDA0001270468490000101
其中：
V′od是计算所得的调整后的轮速，包括东向速度、北向速度和天向速度的3×1的矩阵；
Figure BDA0001270468490000102
是由SINS给出的3×3的姿态矩阵，该姿态矩阵可以由SINS解算的姿态信息得到，用于将车体坐标系的坐标变换到导航坐标系的坐标；
δθZ是第二滤波操作所得到的轮速传感器与IMU的Z轴之间的安装误差角；
δθX是第二滤波操作所得到的轮速传感器与IMU的X轴之间的安装误差角；
δk是第二滤波操作所得到的轮速传感器的尺度因子误差；
Vod是轮速传感器采集的轮速，为一维标量。
步骤404，以SINS的速度误差、SINS的位姿误差和IMU的零偏误差为状态变量构建第一卡尔曼滤波的状态方程，以第一速度与调整后的轮速之差作为观测量构建第一卡尔曼滤波的观测方程，采用卡尔曼滤波算法得到SINS的速度误差、SINS的位姿误差和IMU的零偏误差。
在本实施例中，上述电子设备可以在步骤403中对轮速进行调整，得到调整后的轮速后，以SINS的速度误差、SINS的位姿误差和IMU的零偏误差为状态变量构建第一卡尔曼滤波的状态方程，以第一速度与调整后的轮速之差作为观测量构建第一卡尔曼滤波的观测方程，采用卡尔曼滤波算法得到SINS的速度误差、SINS的位姿误差和IMU的零偏误差。执行完步骤404后，转到步骤405。
作为示例，这里给出一种具体实现方式：
首先，可以按照如下公式以SINS的速度误差、SINS的位姿误差和IMU的零偏误差为状态变量，构建第一卡尔曼滤波的状态方程：
Figure BDA0001270468490000111
Figure BDA0001270468490000112
Figure BDA0001270468490000113
Figure BDA0001270468490000114
Figure BDA0001270468490000115
Figure BDA0001270468490000116
Figure BDA0001270468490000117
Figure BDA0001270468490000121
Figure BDA0001270468490000122
Figure BDA0001270468490000123
Figure BDA0001270468490000124
Figure BDA0001270468490000125
其中：
δλ、δL和δh分别表示SINS的位姿误差中的位置误差中的经度误差、纬度误差和高度误差；
δve、δvn和δvu分别表示SINS的速度误差中的东向速度误差、北向速度误差和天向速度误差；
Figure BDA0001270468490000126
和
Figure BDA0001270468490000127
分别表示SINS的位姿误差中的姿态误差中东向姿态角误差、北向姿态角误差和天向姿态角误差；
Figure BDA0001270468490000131
分别表示IMU中加速度计在X轴方向的零偏、Y轴方向的零偏和Z轴方向的零偏；
εx、εy、εz分别表示IMU中陀螺仪在X轴方向的零偏、Y轴方向的零偏和Z轴方向的零偏；
Figure BDA0001270468490000132
是由SINS给出的3×3的姿态矩阵，该姿态矩阵可以由SINS解算的姿态信息得到，用于将车体坐标系的坐标变换到导航坐标系的坐标；
L是SINS解算的第一位姿中的纬度；
Rn是地球卯酉圈半径；
h是SINS解算的第一位姿中的高度；
Rm是地球子午圈半径；
wie表示地球自转角速度；
Figure BDA0001270468490000133
是SINS解算的第一速度中的东向速度；
Figure BDA0001270468490000134
是SINS解算的第一速度中的天向速度；
Figure BDA0001270468490000135
是SINS解算的第一速度中的北向速度；
Figure BDA0001270468490000136
为IMU中的加速度计的Z轴输出比力；
Figure BDA0001270468490000137
为IMU中的加速度计的Y轴输出比力；
Figure BDA0001270468490000138
为IMU中的加速度计X轴输出比力；
W1为系统噪声，为12×1的矩阵，包括IMU的加速度计输出噪声、陀螺仪输出噪声、加速度计零偏噪声和陀螺仪零偏噪声。
再，将上述状态方程按照如下公式，表示成离散化形式：
X1(k)＝Φ1(k,k-1)·X1(k-1)+Γ1(k)·W1(k) (14)
其中：
k表示第一卡尔曼滤波的当前周期数，k为正整数；
Φ1(k,k-1)为第一卡尔曼滤波在第k个周期的F1的离散形式，为15×15的矩阵；
Γ1(k)为第一卡尔曼滤波在第k个周期的G1的离散形式，为15×12的矩阵。
然后，按照如下公式以第一速度与调整后的轮速之差作为观测量构建第一卡尔曼滤波的观测方程：
Z1＝H1X1+V1 (15)
其中：
X1表示状态变量：SINS的速度误差、SINS的位姿误差和IMU的零偏误差，为15×1的矩阵；
H1表示3×15的观测矩阵；
V1表示系统量测噪声矩阵，为3×1的矩阵；
Z1表示观测量：第一速度与调整后的轮速之差，为3×1的矩阵。
具体的，观测矩阵H1和系统量测噪声Z1可以如下表示：
Figure BDA0001270468490000141
Figure BDA0001270468490000142
这样，观测量Z1可以如下计算得到：
Figure BDA0001270468490000143
Figure BDA0001270468490000144
其中：
Vsins是由SINS解算的第一速度，包括东向速度、北向速度和天向速度的3×1的矩阵；
Figure BDA0001270468490000145
是由SINS给出的3×3的姿态矩阵，该姿态矩阵可以由SINS解算的姿态信息得到，用于将车体坐标系的坐标变换到导航坐标系的坐标；
δθZ是第二滤波操作所得到的轮速传感器与IMU的Z轴之间的安装误差角；
δθX是第二滤波操作所得到的轮速传感器与IMU的X轴之间的安装误差角；
δk是第二滤波操作所得到的轮速传感器的尺度因子误差；
Vod是轮速传感器采集的轮速，为一维的标量；
Figure BDA0001270468490000146
表示由SINS给出的地球自转角速度在导航坐标系上的投影，为3×1的矩阵；
Figure BDA0001270468490000147
表示由SINS给出的导航坐标系相对于地球坐标系的旋转在导航坐标系上的投影，为3×1的矩阵；
Figure BDA0001270468490000148
为IMU中陀螺仪输出的角速度，为3×1的矩阵；
Figure BDA0001270468490000149
表示为轮速传感器与IMU之间的杆臂值，为3×1的矩阵；
Figure BDA0001270468490000151
是调整后的轮速；
03×3为3×3的全0矩阵；
I3×3为3×3的单位矩阵；
03×6为3×6的全0矩阵。
接着，可以按照如下公式对第一卡尔曼滤波进行时间更新：
X1(k,k-1)＝Φ1(k,k-1)X1(k-1) (20)
Figure BDA0001270468490000152
其中：
k表示第一卡尔曼滤波的当前周期数，k为正整数；
X1(k-1)是第一卡尔曼滤波对状态变量X1(即，SINS的速度误差、SINS的位姿误差和IMU的零偏误差)在第k-1个周期进行量测更新后的结果；
X1(k,k-1)是计算所得的第一卡尔曼滤波对状态变量X1在第k个周期进行时间更新后的结果，也是第一卡尔曼滤波对状态变量X1在第k个周期进行预测的结果；
P1(k,k-1)是计算所得的X1(k,k-1)的协方差矩阵，为15×15的矩阵；
Q1是第一卡尔曼滤波的系统噪声矩阵,为12×12的矩阵。
最后，按照如下公式对第一卡尔曼滤波进行量测更新，得到SINS的速度误差、SINS的位姿误差和IMU的零偏误差。
Figure BDA0001270468490000153
P1(k)＝(I15×15-K1(k)H1(k))P1(k,k-1) (23)
X1(k)＝X1(k,k-1)+K1(k)(Z1(k)-H1(K)X1(k,k-1)) (24)
其中：
k表示第一卡尔曼滤波的当前周期数，k为正整数；
H1(k)是第一卡尔曼滤波在第k个周期的观测矩阵，为3×15的矩阵；
R1(k)是第一卡尔曼滤波在第k个周期的观测噪声协方差矩阵，为3×3的矩阵；
K1(k)是计算所得的第一卡尔曼滤波在第k个周期的滤波增益矩阵为3×3的矩阵；
P1(k)是计算所得的第一卡尔曼滤波在第k个周期的状态变量X1的协方差矩阵，为15×15的矩阵；
I15×15是15×15的单位矩阵；
Z1(k)是第一卡尔曼滤波在第k个周期的观测量(即，第一速度与调整后的轮速之差)，为3×1的矩阵；
X1(k)是计算所得的第一卡尔曼滤波在第k个周期进行量测更新后的状态变量X1的取值，即，X1(k)是对第一卡尔曼滤波进行量测更新后所得到的SINS的速度误差、SINS的位姿误差和IMU的零偏误差，为15×1的矩阵。
步骤403’，获取GNSS接收机解算的无人驾驶车辆的第二速度和第二位置。
在本实施例中，上述电子设备可以在步骤402中确定GNSS接收机接收到信号的情况下，获取GNSS接收机解算的无人驾驶车辆的第二速度和第二位置。执行完步骤403’后转到步骤404’。
步骤404’，根据第一速度、第一位姿、第二速度和第二位置，采用卡尔曼滤波算法确定SINS的速度误差、SINS的位姿误差以及IMU的零偏误差。
在本实施例中，上述电子设备可以在步骤403’获取到GNSS接收机解算的无人驾驶车辆的第二速度和第二位置后，根据第一速度、第一位姿、第二速度和第二位置，采用卡尔曼滤波算法确定SINS的速度误差、SINS的位姿误差以及IMU的零偏误差。在执行完步骤404’后，转到步骤405’。
在一些实现方式中，上述第一位姿可以包括第一位置和第一姿态。这样，步骤404’可以如下进行：执行以下第一滤波操作：以SINS的速度误差、SINS的位姿误差和IMU的零偏误差为状态变量构建第一卡尔曼滤波的状态方程，以第一速度与第二速度之差以及第一位置与第二位置之差作为观测量构建第一卡尔曼滤波的观测方程，采用卡尔曼滤波算法得到SINS的速度误差、SINS的位姿误差和IMU的零偏误差。
作为示例，下面给出上述第一滤波操作的一种具体实现方式：
首先，按照如下公式以SINS的速度误差、SINS的位姿误差和IMU的零偏误差为状态变量，构建第一卡尔曼滤波的状态方程：
Figure BDA0001270468490000171
Figure BDA0001270468490000172
其中关于各个参数的具体解释可以参见步骤404中的相关描述，在此不再赘述。
再，将上述状态方程按照如下公式，表示成离散化形式：
X1(k)＝Φ1(k,k-1)·X1(k-1)+Γ1(k)·Q1(k) (27)
其中：
k表示第一卡尔曼滤波的当前周期数，k为正整数；
Φ1(k,k-1)为第一卡尔曼滤波在第k个周期的F1的离散形式，为15×15的矩阵；
Γ1(k)为第一卡尔曼滤波在第k个周期的G1的离散形式，为15×12的矩阵。
这里，SINS的速度误差可以包括东向速度误差、北向速度误差和天向速度误差，SINS的位姿误差可以包括SINS的经度误差、纬度误差、高度误差、东向姿态角误差、北向姿态角误差和天向姿态角误差，IMU的零偏误差可以包括IMU的三个加速度误差和三个角速度误差，也就是说，这里，第一卡尔曼滤波的状态变量是15维的向量。
然后，按照如下公式以第一速度与第二速度之差以及第一位置与第二位置之差作为观测量构建第一卡尔曼滤波的观测方程：
Z1＝H1X1+V1 (28)
其中：
X1表示状态变量：SINS的速度误差、SINS的位姿误差和IMU的零偏误差，X1为15×1的矩阵；
H1表示从状态变量X1到观测量Z1的观测矩阵，具体地，H1可以表示如下：
H1＝[I6×6 06×9] (29)
这样，X1的前六维分量可以包括：SINS的东向速度误差、北向速度误差和天向速度误差以及SINS的经度误差、纬度误差和高度误差；
I6×6表示6×6的单位矩阵；
06×9表示6×9的全零矩阵；
V1表示系统量测噪声矩阵，为15×1的矩阵；
Z1表示观测量：第一速度与第二速度之差，以及第一位置与第二位置之差，为6×1的矩阵。
接着，按照如下公式对第一卡尔曼滤波进行时间更新：
X1(k,k-1)＝Φ1(k,k-1)X1(k-1) (30)
Figure BDA0001270468490000181
其中：
k表示第一卡尔曼滤波的当前周期数，k为正整数；
X1(k-1)是第一卡尔曼滤波对状态变量X1(即，SINS的速度误差、SINS的位姿误差和IMU的零偏误差)在第k-1个周期进行量测更新后的结果；
X1(k,k-1)是计算所得的第一卡尔曼滤波对状态变量X1在第k个周期进行时间更新后的结果，也是第一卡尔曼滤波对状态变量X1在第k个周期进行预测的结果；
P1(k,k-1)是计算所得的X1(k,k-1)的协方差矩阵，为15×15的矩阵；
Q1是第一卡尔曼滤波的系统噪声矩阵,为12×12的矩阵。
最后，按照如下公式对第一卡尔曼滤波进行量测更新，得到SINS的速度误差、SINS的位姿误差和IMU的零偏误差。
Figure BDA0001270468490000182
P1(k)＝(I15×15-K1(k)H1(k))P1(k,k-1) (33)
X1(k)＝X1(k,k-1)+K1(k)(Z1(k)-H1(K)X1(k,k-1)) (34)
其中：
k表示第一卡尔曼滤波的当前周期数，k为正整数；
H1(k)是第一卡尔曼滤波在第k个周期的观测矩阵，为6×15的矩阵；
R1(k)是第一卡尔曼滤波在第k个周期的观测噪声协方差矩阵，为6×6的矩阵；
K1(k)是计算所得的第一卡尔曼滤波在第k个周期的滤波增益矩阵，为15×6的矩阵；
P1(k)是计算所得的第一卡尔曼滤波在第k个周期的状态变量X1的协方差矩阵，为15×15的矩阵；
I15×15是15×15的单位矩阵；
Z1(k)是第一卡尔曼滤波在第k个周期的观测量(即，第一速度与第二速度之差，以及第一位置与第二位置之差)，为6×1的矩阵；
X1(k)是计算所得的第一卡尔曼滤波在第k个周期进行量测更新后的状态变量X1的取值，即，X1(k)是对第一卡尔曼滤波进行量测更新后所得到的SINS的速度误差、SINS的位姿误差和IMU的零偏误差。
步骤405’，确定GNSS接收机是否工作在实时动态差分定位模式下，如果是，转到步骤406’，如果不是，转到步骤405。
在本实施例中，上述电子设备可以在执行完步骤404’后，确定GNSS接收机是否工作在实时动态差分定位模式下，如果是，表明从GNSS接收机获取的第二速度和第二位置的精度较高，因此转到步骤406’，执行第二滤波操作。如果不是，表明从GNSS接收机获取的第二速度和第二位置的精度较低，因此直接转到步骤405。
步骤406’，执行第二滤波操作。
在本实施例中，上述电子设备可以在步骤405’中确定GNSS接收机工作在实时动态差分(RTK，Real Time Kinematic)定位模式下的情况下，表明从GNSS接收机获取的第二速度和第二位置的精度较高，因此可以执行以下第二滤波操作：以轮速传感器的尺度因子误差和轮速传感器与IMU之间的安装角误差作为状态变量构建第二卡尔曼滤波的状态方程，以轮速与第二速度之差作为观测量构建第二卡尔曼滤波的观测方程，采用卡尔曼滤波算法得到轮速传感器的尺度因子误差和轮速传感器与IMU之间的安装角误差。执行完上述第二滤波操作后，转到步骤405。
这里，轮速传感器的尺度因子用于表征车辆速度与轮速传感器所采集的轮速之间的比值。尺度因子误差经常与车辆轮胎的胎压有关。轮速传感器与IMU之间的安装角用于表征从轮速传感器所采集的轮速标量转换成东向速度、北向速度和天向速度所需要的系数。为了得到准确的尺度因子误差以及轮速传感器与IMU之间的安装角误差，步骤406’中利用卡尔曼滤波算法对尺度因子误差和轮速传感器与IMU之间的安装角误差进行估计，可以得到相对准确的尺度因子误差和轮速传感器与IMU之间的安装角误差，作为当前周期的尺度因子误差和轮速传感器与IMU之间的安装角误差，这样，上述电子设备可以在下一周期确定GNSS接收机接收不到信号执行步骤403的过程中，步骤403中可以根据步骤406’在当前周期执行第二滤波操作所得到的尺度因子误差和轮速传感器与IMU之间的安装角误差，对下一周期的轮速进行调整，得到调整后的轮速。
作为示例，下面给出第二滤波操作的一种具体实现方式：
首先，按照如下公式以轮速传感器的尺度因子误差和轮速传感器与IMU之间的安装角误差为状态变量构建第二卡尔曼滤波的状态方程：
Figure BDA0001270468490000201
F2＝03×3 (36)
G2＝I3×3 (37)
Figure BDA0001270468490000202
Figure BDA0001270468490000203
其中：
X2表示状态变量(轮速传感器的尺度因子误差和轮速传感器与IMU之间的安装角误差)。实践中，在IMU的坐标为IMU的坐标系为右—前—上的情况下，由于轮速传感器与IMU的X轴和Z轴之间的安装角误差对后续SINS解算具有意义，而轮速传感器与IMU的Y轴之间的安装角误差对后续SINS解算意义不大，因此，这里只考虑轮速传感器与IMU的X轴之间的安装角误差
Figure BDA0001270468490000204
和轮速传感器与IMU的Z轴之间的安装角误差
Figure BDA0001270468490000205
不考虑轮速传感器与IMU的Y轴之间的安装角误差。由此，第二卡尔曼滤波的状态变量包括轮速传感器的尺度因子误差δk、轮速传感器与IMU的X轴之间的安装角误差
Figure BDA0001270468490000206
和轮速传感器与IMU的Z轴之间的安装角误差
Figure BDA0001270468490000211
Figure BDA0001270468490000212
为状态变量X2(轮速传感器的尺度因子误差和轮速传感器与IMU之间的安装角误差)的微分；
03×3为3×3的全0矩阵；
I3×3为3×3的单位矩阵；
Figure BDA0001270468490000213
为轮速传感器与IMU的X轴的安装角误差的系统噪声；
wk为轮速传感器的尺度因子误差的系统噪声；
Figure BDA0001270468490000214
为轮速传感器与IMU的Z轴的安装角误差的系统噪声。
再，将上述状态方程按照如下公式，表示成离散化形式：
X2(k)＝Φ2(k,k-1)·X2(k-1)+Γ2(k)·W2(k) (40)
Φ2(k,k-1)＝I3×3 (41)
Γ2(k,k-1)＝I3×3 (42)
其中：
k表示第二卡尔曼滤波的当前周期数，k为正整数；
Φ2(k,k-1)为第二卡尔曼滤波中F2的离散形式，为3×3的单位矩阵；
Γ2(k)为第二卡尔曼滤波中G2的离散形式，为3×3的单位矩阵；
然后，按照如下公式以轮速与第二速度之差作为观测量构建第二卡尔曼滤波的观测方程：
Z2＝H2X2+V2 (43)
其中：
X2表示状态变量：轮速传感器的尺度因子误差和轮速传感器与IMU之间的安装角误差，为3×1的矩阵；
H2表示从状态变量X2到观测量Z2的观测矩阵，为3×3的矩阵；
V2表示系统量测噪声矩阵，为3×1的矩阵；
Z2表示观测量：轮速与第二速度之差，为3×1的矩阵。
具体的，观测矩阵H2和系统量测噪声V2可以如下表示：
Figure BDA0001270468490000215
Figure BDA0001270468490000216
这样，观测量Z2可以如下计算得到：
Figure BDA0001270468490000221
Figure BDA0001270468490000222
其中：
Vod表示由轮速传感器采集的轮速，为一维标量；
Vgnss表示由GNSS接收机解算的第二速度，包括东向速度、北向速度和天向速度，为3×1的矩阵；
Z2表示观测量：轮速与第二速度之差，包括东向速度差、北向速度差和天向速度差，为3×1的矩阵；
Figure BDA0001270468490000223
是由SINS给出的3×3的姿态矩阵，该姿态矩阵可以由SINS解算的姿态信息得到，用于将车体坐标系的坐标变换到导航坐标系的坐标；
Figure BDA0001270468490000224
表示由SINS给出的地球自转角速度在导航坐标系上的投影，为3×1的矩阵；
Figure BDA0001270468490000225
表示由SINS给出的为导航坐标系相对于地球坐标系的旋转在导航坐标系上的投影，为3×1的矩阵；
Figure BDA0001270468490000226
为IMU中陀螺仪输出的角速度，为3×1的矩阵；
Figure BDA0001270468490000227
表示为轮速传感器与IMU之间的杆臂值，为3×1的矩阵。
接着，可以按照如下公式对第二卡尔曼滤波进行时间更新：
X2(k,k-1)＝Φ2(k,k-1)X2(k-1) (48)
Figure BDA0001270468490000228
其中：
k表示第二卡尔曼滤波的当前周期数，k为正整数；
X2(k-1)是第二卡尔曼滤波对状态变量X2(即，轮速传感器的尺度因子误差和轮速传感器与IMU之间的安装角误差)在第k-1个周期进行量测更新后的结果；
X2(k,k-1)是计算所得的第二卡尔曼滤波对状态变量X2在第k个周期进行时间更新后的结果，也是第二卡尔曼滤波对状态变量X2在第k个周期进行预测的结果；
P2(k,k-1)是计算所得的X2(k,k-1)的协方差矩阵，为3×3的矩阵；
Q2是第二卡尔曼滤波的系统噪声矩阵,为3×3的矩阵。
最后，按照如下公式对第二卡尔曼滤波进行量测更新，得到轮速传感器的尺度因子误差和轮速传感器与IMU之间的安装角误差。
Figure BDA0001270468490000231
P2(k)＝(I3×3-K2(k)H2(k))P2(k,k-1) (51)
X2(k)＝X2(k,k-1)+K2(k)(Z2(k)-H2(K)X2(k,k-1)) (52)
其中：
k表示第二卡尔曼滤波的当前周期数，k为正整数；
H2(k)是第二卡尔曼滤波在第k个周期的观测矩阵，为3×3的矩阵；
R2(k)是第二卡尔曼滤波在第k个周期的观测噪声协方差矩阵，为3×3的矩阵；
K2(k)是计算所得的第二卡尔曼滤波在第k个周期的滤波增益矩阵，为3×3的矩阵；
P2(k)是计算所得的第二卡尔曼滤波在第k个周期的状态变量X2的协方差矩阵，为3×3的矩阵；
I3×3是3×3的单位矩阵；
Z2(k)是第二卡尔曼滤波在第k个周期的观测量(即，轮速与第二速度之差)，为3×1的矩阵；
X2(k)是计算所得的第二卡尔曼滤波在第k个周期进行量测更新后的状态变量X2的取值，即，X2(k)是对第二卡尔曼滤波进行量测更新后所得到的轮速传感器的尺度因子误差和轮速传感器与IMU之间的安装角误差，为3×1的矩阵。
步骤405，根据SINS的速度误差、SINS的位姿误差和IMU的零偏误差，对第一速度和第一位姿进行调整，得到无人驾驶车辆的速度和位姿。
在本实施例中，步骤405的具体操作与图2所示的实施例中步骤204的操作基本相同，在此不再赘述。
从图4中可以看出，与图2对应的实施例相比，本实施例中的用于确定无人驾驶车辆的速度和位姿的方法的流程400多出了在GNSS接收机接收到信号的情况下根据第一速度、第一位姿和GNSS接收机解算的无人驾驶车辆的第二速度和第二位置，采用卡尔曼滤波算法确定SINS的速度误差、SINS的位姿误差以及IMU的零偏误差，并在GNSS接收机工作在实时动态差分定位模式下执行第二滤波操作以对轮速传感器的尺度因子误差和轮速传感器与IMU之间的安装角误差进行估计，以及在GNSS接收机接收不到信号的情况下，使用第二滤波操作估计得到的尺度因子误差和安装角误差对轮速进行修正，并根据修正后的轮速和SINS解算的第一速度和第一位姿确定SINS的速度误差和位姿误差以及IMU的零偏误差。由此，本实施例描述的方案可以进一步提高确定无人驾驶车辆的速度和位姿的精度，并从而提高无人驾驶车辆的定位精度。
进一步参考图5，作为对上述各图所示方法的实现，本申请提供了一种用于确定无人驾驶车辆的速度和位姿的装置的一个实施例，该无人驾驶车辆设置有GNSS接收机、SINS以及轮速传感器，该SINS包括IMU，该装置实施例与图2所示的方法实施例相对应，该装置具体可以应用于各种电子设备中。
如图5所示，本实施例的用于确定无人驾驶车辆的速度和位姿的装置500包括：获取单元501、信号确定单元502、第一误差确定单元503和调整单元504。其中，获取单元501，配置用于获取上述SINS解算的上述无人驾驶车辆的第一速度、第一位姿和上述轮速传感器采集的上述无人驾驶车辆的轮速；信号确定单元502，配置用于确定上述GNSS接收机是否接收到信号；第一误差确定单元503，配置用于响应于确定上述GNSS接收机接收不到信号，根据上述轮速和上述第一速度，采用卡尔曼滤波算法确定上述SINS的速度误差、上述SINS的位姿误差以及上述IMU的零偏误差；调整单元504，配置用于根据上述速度误差和上述位姿误差，对上述第一速度和上述第一位姿进行调整，得到上述无人驾驶车辆的速度和位姿。
在本实施例中，用于确定无人驾驶车辆的速度和位姿的装置500的获取单元501、信号确定单元502、第一误差确定单元503和调整单元504的具体处理及其所带来的技术效果可分别参考图2对应实施例中步骤201、步骤202、步骤203和步骤204的相关说明，在此不再赘述。
在本实施例的一些可选的实现方式中，上述装置500还可以包括：第二误差确定单元505，配置用于响应于确定上述GNSS接收机接收到信号，获取上述GNSS接收机解算的上述无人驾驶车辆的第二速度和第二位置，并根据上述第一速度、上述第一位姿、上述第二速度和上述第二位置，采用卡尔曼滤波算法确定上述SINS的速度误差、上述SINS的位姿误差以及上述IMU的零偏误差。第二误差确定单元505的具体处理及其所带来的技术效果可分别参考图4对应实施例中步骤403’和步骤404’的相关说明，在此不再赘述。
在本实施例的一些可选的实现方式中，上述第一位姿可以包括第一位置和第一姿态；以及上述第二误差确定单元505可以包括：第一滤波模块5051，配置用于执行以下第一滤波操作：以上述SINS的速度误差、上述SINS的位姿误差和上述IMU的零偏误差为状态变量构建第一卡尔曼滤波的状态方程，以上述第一速度与上述第二速度之差以及上述第一位置与上述第二位置之差作为观测量构建上述第一卡尔曼滤波的观测方程，采用卡尔曼滤波算法得到上述SINS的速度误差、上述SINS的位姿误差和上述IMU的零偏误差。第一滤波模块5051的具体处理及其所带来的技术效果可参考图4对应实施例中步骤404’的相关说明，在此不再赘述。
在本实施例的一些可选的实现方式中，上述第二误差确定单元505还可以包括：定位模式确定模块5052，配置用于确定上述GNSS接收机是否工作在实时动态差分定位模式下；第二滤波模块5053，配置用于响应于确定上述GNSS接收机工作在实时动态差分定位模式下，执行以下第二滤波操作：以上述轮速传感器的尺度因子误差和上述轮速传感器与上述IMU之间的安装角误差作为状态变量构建第二卡尔曼滤波的状态方程，以上述轮速与上述第二速度之差作为观测量构建上述第二卡尔曼滤波的观测方程，采用卡尔曼滤波算法得到上述尺度因子误差和上述安装角误差。定位模式确定模块5052和第二滤波模块5053的具体处理及其所带来的技术效果可分别参考图4对应实施例中步骤405’和步骤406’的相关说明，在此不再赘述。
在本实施例的一些可选的实现方式中，上述第一误差确定单元503可以进一步配置用于：以上述SINS的速度误差、上述SINS的位姿误差和上述IMU的零偏误差为状态变量构建第一卡尔曼滤波的状态方程，以上述第一速度与上述轮速之差作为观测量构建上述第一卡尔曼滤波的观测方程，采用卡尔曼滤波算法得到上述SINS的速度误差、上述SINS的位姿误差和上述IMU的零偏误差。第一误差确定单元503的具体处理及其所带来的技术效果可参考图2对应实施例中步骤203的相关说明，在此不再赘述。
在本实施例的一些可选的实现方式中，上述第一误差确定单元503可以进一步配置用于：根据上述尺度因子误差和上述安装角误差对上述轮速进行调整，得到调整后的轮速；以上述SINS的速度误差、上述SINS的位姿误差和上述IMU的零偏误差为状态变量构建上述第一卡尔曼滤波的状态方程，以上述第一速度与上述调整后的轮速之差作为观测量构建上述第一卡尔曼滤波的观测方程，采用卡尔曼滤波算法得到上述SINS的速度误差、上述SINS的位姿误差和上述IMU的零偏误差。第一误差确定单元503的具体处理及其所带来的技术效果可参考图4对应实施例中步骤403的相关说明，在此不再赘述。
下面参考图6，其示出了适于用来实现本申请实施例的电子设备的计算机系统600的结构示意图。图6示出的电子设备仅仅是一个示例，不应对本申请实施例的功能和使用范围带来任何限制。
如图6所示，计算机系统600包括中央处理单元(CPU，Central Processing Unit)601，其可以根据存储在只读存储器(ROM，Read Only Memory)602中的程序或者从存储部分606加载到随机访问存储器(RAM，Random Access Memory)603中的程序而执行各种适当的动作和处理。在RAM 603中，还存储有系统600操作所需的各种程序和数据。CPU 601、ROM602以及RAM 603通过总线604彼此相连。输入/输出(I/O，Input/Output)接口605也连接至总线604。
以下部件连接至I/O接口605：包括硬盘等的存储部分606；以及包括诸如LAN(局域网，Local Area Network)卡、调制解调器等的网络接口卡的通信部分607。通信部分607经由诸如因特网的网络执行通信处理。驱动器608也根据需要连接至I/O接口605。可拆卸介质609，诸如磁盘、光盘、磁光盘、半导体存储器等等，根据需要安装在驱动器608上，以便于从其上读出的计算机程序根据需要被安装入存储部分606。
特别地，根据本公开的实施例，上文参考流程图描述的过程可以被实现为计算机软件程序。例如，本公开的实施例包括一种计算机程序产品，其包括承载在计算机可读介质上的计算机程序，该计算机程序包含用于执行流程图所示的方法的程序代码。在这样的实施例中，该计算机程序可以通过通信部分607从网络上被下载和安装，和/或从可拆卸介质609被安装。在该计算机程序被中央处理单元(CPU)601执行时，执行本申请的方法中限定的上述功能。需要说明的是，本申请所述的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本申请中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本申请中，计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：无线、电线、光缆、RF等等，或者上述的任意合适的组合。
附图中的流程图和框图，图示了按照本申请各种实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分，该模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
描述于本申请实施例中所涉及到的单元可以通过软件的方式实现，也可以通过硬件的方式来实现。所描述的单元也可以设置在处理器中，例如，可以描述为：一种处理器包括获取单元、信号确定单元、第一误差确定单元和调整单元。其中，这些单元的名称在某种情况下并不构成对该单元本身的限定，例如，信号确定单元还可以被描述为“确定GNSS接收机是否接收到信号的单元”。
作为另一方面，本申请还提供了一种计算机可读介质，该计算机可读介质可以是上述实施例中描述的装置中所包含的；也可以是单独存在，而未装配入该装置中。上述计算机可读介质承载有一个或者多个程序，当上述一个或者多个程序被该装置执行时，使得该装置：获取SINS解算的无人驾驶车辆的第一速度、第一位姿和轮速传感器采集的述无人驾驶车辆的轮速；确定GNSS接收机是否接收到信号；响应于确定GNSS接收机接收不到信号，根据轮速和第一速度，采用卡尔曼滤波算法确定SINS的速度误差、SINS的位姿误差以及IMU的零偏误差；根据速度误差和位姿误差，对第一速度和第一位姿进行调整，得到无人驾驶车辆的速度和位姿。
以上描述仅为本申请的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解，本申请中所涉及的发明范围，并不限于上述技术特征的特定组合而成的技术方案，同时也应涵盖在不脱离上述发明构思的情况下，由上述技术特征或其等同特征进行任意组合而形成的其它技术方案。例如上述特征与本申请中公开的(但不限于)具有类似功能的技术特征进行互相替换而形成的技术方案。

Claims (12)

1.一种用于确定无人驾驶车辆的速度和位姿的方法，其特征在于，所述无人驾驶车辆设置有全球导航卫星系统GNSS接收机、捷联惯性导航系统SINS以及轮速传感器，所述SINS包括惯性测量单元IMU，所述方法包括：
获取所述SINS解算的所述无人驾驶车辆的第一速度、第一位姿和所述轮速传感器采集的所述无人驾驶车辆的轮速；
确定所述GNSS接收机是否接收到信号；
响应于确定所述GNSS接收机接收不到信号，根据所述轮速和所述第一速度，采用卡尔曼滤波算法确定所述SINS的速度误差、所述SINS的位姿误差以及所述IMU的零偏误差；
响应于确定所述GNSS接收机接收到信号，获取第二速度；
确定所述GNSS接收机是否工作在实时动态差分定位模式下；
响应于确定所述GNSS接收机工作在实时动态差分定位模式下，执行以下第二滤波操作：以所述轮速传感器的尺度因子误差和所述轮速传感器与所述IMU之间的安装角误差作为状态变量构建第二卡尔曼滤波的状态方程，以所述轮速与所述第二速度之差作为观测量构建所述第二卡尔曼滤波的观测方程，采用卡尔曼滤波算法得到所述尺度因子误差和所述安装角误差；
根据所述速度误差和所述位姿误差，对所述第一速度和所述第一位姿进行调整，得到所述无人驾驶车辆的速度和位姿。
2.根据权利要求1所述的方法，其特征在于，所述根据所述速度误差和所述位姿误差，对所述第一速度和所述第一位姿进行调整，得到所述无人驾驶车辆的速度和位姿之前，所述方法还包括：
响应于确定所述GNSS接收机接收到信号，获取所述GNSS接收机解算的所述无人驾驶车辆的第二速度和第二位置，并根据所述第一速度、所述第一位姿、所述第二速度和所述第二位置，采用卡尔曼滤波算法确定所述SINS的速度误差、所述SINS的位姿误差以及所述IMU的零偏误差。
3.根据权利要求2所述的方法，其特征在于，所述第一位姿包括第一位置和第一姿态；以及
所述根据所述第一速度、所述第一位姿、所述第二速度和所述第二位置，采用卡尔曼滤波算法确定所述SINS的速度误差、所述SINS的位姿误差以及所述IMU的零偏误差，包括：
执行以下第一滤波操作：以所述SINS的速度误差、所述SINS的位姿误差和所述IMU的零偏误差为状态变量构建第一卡尔曼滤波的状态方程，以所述第一速度与所述第二速度之差以及所述第一位置与所述第二位置之差作为观测量构建所述第一卡尔曼滤波的观测方程，采用卡尔曼滤波算法得到所述SINS的速度误差、所述SINS的位姿误差和所述IMU的零偏误差。
4.根据权利要求1所述的方法，其特征在于，所述根据所述轮速和所述第一速度，采用卡尔曼滤波算法确定所述SINS的速度误差、所述SINS的位姿误差以及所述IMU的零偏误差，包括：
以所述SINS的速度误差、所述SINS的位姿误差和所述IMU的零偏误差为状态变量构建第一卡尔曼滤波的状态方程，以所述第一速度与所述轮速之差作为观测量构建所述第一卡尔曼滤波的观测方程，采用卡尔曼滤波算法得到所述SINS的速度误差、所述SINS的位姿误差和所述IMU的零偏误差。
5.根据权利要求3所述的方法，其特征在于，所述根据所述轮速和所述第一速度，采用卡尔曼滤波算法确定所述SINS的速度误差、所述SINS的位姿误差以及所述IMU的零偏误差，包括：
根据所述尺度因子误差和所述安装角误差对所述轮速进行调整，得到调整后的轮速；
以所述SINS的速度误差、所述SINS的位姿误差和所述IMU的零偏误差为状态变量构建所述第一卡尔曼滤波的状态方程，以所述第一速度与所述调整后的轮速之差作为观测量构建所述第一卡尔曼滤波的观测方程，采用卡尔曼滤波算法得到所述SINS的速度误差、所述SINS的位姿误差和所述IMU的零偏误差。
6.一种用于确定无人驾驶车辆的速度和位姿的装置，其特征在于，所述无人驾驶车辆设置有全球导航卫星系统GNSS接收机、捷联惯性导航系统SINS以及轮速传感器，所述SINS包括惯性测量单元IMU，所述装置包括：
获取单元，配置用于获取所述SINS解算的所述无人驾驶车辆的第一速度、第一位姿和所述轮速传感器采集的所述无人驾驶车辆的轮速；
信号确定单元，配置用于确定所述GNSS接收机是否接收到信号；
第一误差确定单元，配置用于响应于确定所述GNSS接收机接收不到信号，根据所述轮速和所述第一速度，采用卡尔曼滤波算法确定所述SINS的速度误差、所述SINS的位姿误差以及所述IMU的零偏误差；
第二误差确定单元，配置用于响应于确定所述GNSS接收机接收到信号，获取所述GNSS接收机解算的所述无人驾驶车辆的第二速度；
定位模式确定模块，配置用于确定所述GNSS接收机是否工作在实时动态差分定位模式下；
第二滤波模块，配置用于响应于确定所述GNSS接收机工作在实时动态差分定位模式下，执行以下第二滤波操作：以所述轮速传感器的尺度因子误差和所述轮速传感器与所述IMU之间的安装角误差作为状态变量构建第二卡尔曼滤波的状态方程，以所述轮速与所述第二速度之差作为观测量构建所述第二卡尔曼滤波的观测方程，采用卡尔曼滤波算法得到所述尺度因子误差和所述安装角误差；
调整单元，配置用于根据所述速度误差和所述位姿误差，对所述第一速度和所述第一位姿进行调整，得到所述无人驾驶车辆的速度和位姿。
7.根据权利要求6所述的装置，其特征在于，
第二误差确定单元，还被配置用于响应于确定所述GNSS接收机接收到信号，获取所述GNSS接收机解算的所述无人驾驶车辆的第二速度和第二位置，并根据所述第一速度、所述第一位姿、所述第二速度和所述第二位置，采用卡尔曼滤波算法确定所述SINS的速度误差、所述SINS的位姿误差以及所述IMU的零偏误差。
8.根据权利要求7所述的装置，其特征在于，所述第一位姿包括第一位置和第一姿态；以及
所述第二误差确定单元，包括：
第一滤波模块，配置用于执行以下第一滤波操作：以所述SINS的速度误差、所述SINS的位姿误差和所述IMU的零偏误差为状态变量构建第一卡尔曼滤波的状态方程，以所述第一速度与所述第二速度之差以及所述第一位置与所述第二位置之差作为观测量构建所述第一卡尔曼滤波的观测方程，采用卡尔曼滤波算法得到所述SINS的速度误差、所述SINS的位姿误差和所述IMU的零偏误差。
9.根据权利要求6所述的装置，其特征在于，所述第一误差确定单元进一步配置用于：
以所述SINS的速度误差、所述SINS的位姿误差和所述IMU的零偏误差为状态变量构建第一卡尔曼滤波的状态方程，以所述第一速度与所述轮速之差作为观测量构建所述第一卡尔曼滤波的观测方程，采用卡尔曼滤波算法得到所述SINS的速度误差、所述SINS的位姿误差和所述IMU的零偏误差。
10.根据权利要求8所述的装置，其特征在于，所述第一误差确定单元进一步配置用于：
根据所述尺度因子误差和所述安装角误差对所述轮速进行调整，得到调整后的轮速；
以所述SINS的速度误差、所述SINS的位姿误差和所述IMU的零偏误差为状态变量构建所述第一卡尔曼滤波的状态方程，以所述第一速度与所述调整后的轮速之差作为观测量构建所述第一卡尔曼滤波的观测方程，采用卡尔曼滤波算法得到所述SINS的速度误差、所述SINS的位姿误差和所述IMU的零偏误差。
11.一种无人驾驶车辆，包括：
全球导航卫星系统接收机，用于解算无人驾驶车辆的速度和位置；
捷联惯性导航系统，用于解算无人驾驶车辆的速度和位姿；
轮速传感器，用于采集无人驾驶车辆的轮速；
一个或多个处理器；
存储装置，用于存储一个或多个程序，
当所述一个或多个程序被所述一个或多个处理器执行时，使得所述一个或多个处理器实现如权利要求1-5中任一所述的方法。
12.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1-5中任一所述的方法。
CN201710245249.2A 2017-04-14 2017-04-14 用于确定无人驾驶车辆的速度和位姿的方法和装置 Active CN108731667B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710245249.2A CN108731667B (zh) 2017-04-14 2017-04-14 用于确定无人驾驶车辆的速度和位姿的方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710245249.2A CN108731667B (zh) 2017-04-14 2017-04-14 用于确定无人驾驶车辆的速度和位姿的方法和装置

Publications (2)

Publication Number Publication Date
CN108731667A CN108731667A (zh) 2018-11-02
CN108731667B true CN108731667B (zh) 2020-09-29

Family

ID=63923970

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710245249.2A Active CN108731667B (zh) 2017-04-14 2017-04-14 用于确定无人驾驶车辆的速度和位姿的方法和装置

Country Status (1)

Country Link
CN (1) CN108731667B (zh)

Families Citing this family (18)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109556634A (zh) * 2018-12-14 2019-04-02 北京百度网讯科技有限公司 车辆定位能力的检测方法、装置、设备及可读存储介质
EP3827286A4 (en) * 2018-12-20 2021-10-27 SZ DJI Technology Co., Ltd. METHOD AND DEVICE FOR DETERMINATION OF POSITION ON THE BASIS OF LAYING DATA
CN109798890B (zh) * 2019-02-01 2021-09-17 上海戴世智能科技有限公司 用于车用组合惯性导航系统在无观测值条件下的启动方法、车用组合惯性导航系统和车辆
WO2020237495A1 (zh) * 2019-05-28 2020-12-03 深圳市大疆创新科技有限公司 地面可移动平台与其运动信息检测方法、系统
CN110307836B (zh) * 2019-07-10 2021-05-07 北京智行者科技有限公司 一种用于无人清扫车辆贴边清扫的精确定位方法
CN112525143B (zh) * 2019-09-19 2022-09-27 北京魔门塔科技有限公司 一种设备安装角的确定方法及车载终端
CN110887494A (zh) * 2019-11-29 2020-03-17 腾讯科技（深圳）有限公司 车辆定位方法、装置
TWI724686B (zh) * 2019-12-13 2021-04-11 國立成功大學 應用高精地圖之定位定向系統與定位定向方法
CN111856062A (zh) * 2020-06-30 2020-10-30 安徽海博智能科技有限责任公司 一种轮速传感器系统、无人驾驶矿车及控制方法
CN111879309A (zh) * 2020-06-30 2020-11-03 同济大学 一种车载定位数据集成装置及其方法
CN111982106A (zh) * 2020-08-28 2020-11-24 北京信息科技大学 导航方法、装置、存储介质及电子装置
CN112268557B (zh) * 2020-09-22 2024-03-05 宽凳(湖州)科技有限公司 一种城市场景实时高精定位方法
CN114445490A (zh) * 2020-10-31 2022-05-06 华为技术有限公司 一种位姿确定方法及其相关设备
CN112526978A (zh) * 2020-12-09 2021-03-19 智道网联科技(北京)有限公司 车辆驾驶性能检测方法及系统、电子设备和存储介质
CN113008244B (zh) * 2021-02-25 2023-07-14 广州导远电子科技有限公司 导航信息的处理方法、装置、电子设备及存储介质
CN113188539A (zh) * 2021-04-27 2021-07-30 深圳亿嘉和科技研发有限公司 一种巡检机器人的组合定位方法
CN113790731B (zh) * 2021-08-06 2022-12-02 荣耀终端有限公司 速度信息的生成方法及装置
CN114683871B (zh) * 2021-11-05 2024-02-06 中国北方车辆研究所 滑动转向电驱动无人车驱动防滑控制方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1687709A (zh) * 2005-05-12 2005-10-26 吉林大学 汽车运动状态测量系统
CN102914294A (zh) * 2012-09-10 2013-02-06 中国南方电网有限责任公司超高压输电公司天生桥局 一种基于图像的无人机电力巡线测量系统及方法
CN103454665A (zh) * 2013-08-26 2013-12-18 哈尔滨工程大学 一种双差gps/sins组合导航姿态测量方法
CN103528587A (zh) * 2013-10-15 2014-01-22 西北工业大学 自主组合导航系统
CN105424040A (zh) * 2016-01-15 2016-03-23 极翼机器人(上海)有限公司 一种新型mems惯性传感器阵列余度配置方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN1687709A (zh) * 2005-05-12 2005-10-26 吉林大学 汽车运动状态测量系统
CN102914294A (zh) * 2012-09-10 2013-02-06 中国南方电网有限责任公司超高压输电公司天生桥局 一种基于图像的无人机电力巡线测量系统及方法
CN103454665A (zh) * 2013-08-26 2013-12-18 哈尔滨工程大学 一种双差gps/sins组合导航姿态测量方法
CN103528587A (zh) * 2013-10-15 2014-01-22 西北工业大学 自主组合导航系统
CN105424040A (zh) * 2016-01-15 2016-03-23 极翼机器人(上海)有限公司 一种新型mems惯性传感器阵列余度配置方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
面向无人驾驶汽车的车道级导航研究;李陆浩;《中国优秀硕士学位论文全文数据库 工程科技II辑》;20141015;第29-42页 *

Also Published As

Publication number Publication date
CN108731667A (zh) 2018-11-02

Similar Documents

Publication Publication Date Title
CN108731667B (zh) 用于确定无人驾驶车辆的速度和位姿的方法和装置
EP3364153B1 (en) Method for updating all attitude angles of agricultural machine on the basis of nine-axis mems sensor
EP1585939B1 (en) Attitude change kalman filter measurement apparatus and method
CN109143304B (zh) 用于确定无人驾驶车辆位姿的方法和装置
US7979231B2 (en) Method and system for estimation of inertial sensor errors in remote inertial measurement unit
CN106990426B (zh) 一种导航方法和导航装置
RU2591018C2 (ru) Способ калибровки инерционного датчика, установленного в произвольной позиции на борту транспортного средства, и система датчиков динамических параметров транспортного средства, выполненная с возможностью установки на борту в произвольной позиции
CN106969783B (zh) 一种基于光纤陀螺惯性导航的单轴旋转快速标定技术
De Agostino et al. Performances comparison of different MEMS-based IMUs
KR20170104623A (ko) 관성 항법 장치의 초기 정렬 방법
CN109186597B (zh) 一种基于双mems-imu的室内轮式机器人的定位方法
CN109870173A (zh) 一种基于校验点的海底管道惯性导航系统的轨迹修正方法
Chen et al. Improving inertial sensor by reducing errors using deep learning methodology
CN110849360A (zh) 面向多机协同编队飞行的分布式相对导航方法
CN111256708A (zh) 一种基于射频识别的车载组合导航方法
CN116559923A (zh) 用于gnss天线杆臂和里程计杆臂的在线估计方法、设备及介质
CN113917512B (zh) 自动驾驶车辆的定位方法、装置、电子设备及存储介质
US20230349699A1 (en) Absolute heading estimation with constrained motion
CN114877886A (zh) 一种车载组合导航设备姿态自适应估计方法
KR101941009B1 (ko) 자세추정 시스템 및 자세 추정 시스템을 포함하는 무인 이동 장치
CN112595330B (zh) 车辆定位方法、装置、电子设备和计算机可读介质
Shaukat et al. Robust vehicle localization with gps dropouts
CN113985466A (zh) 一种基于模式识别的组合导航方法及系统
CN116380119A (zh) 组合导航的校准方法、装置和系统
CN113093239A (zh) 用于提供导航信息的方法、对应系统和程序产品

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant