本申请涉及机动车技术领域，具体涉及无人驾驶车辆技术领域，尤其涉及用于确定无人驾驶车辆的速度和位姿的方法和装置。
背景技术
目前在确定无人驾驶车辆的速度和位姿时，通常采用在车辆中设置sins(strap-downinertialnavigationsystem，捷联惯性导航系统)和gnss(globalnavigationsatellitesystem，全球导航卫星系统)接收机解算的数据来确定无人驾驶车辆的速度和位姿。
在gnss接收机接收到信号的时候，采用sins和gnss接收机输出的信息共同确定无人驾驶车辆的速度和位姿，在gnss接收机接收不到信号的时候，只能采用sins输出的信息确定无人驾驶车辆的速度和位姿，在sins的精度较低的情况下，降低了确定无人驾驶车辆的速度和位姿的精度。从而现有的确定无人驾驶车辆的速度和位姿的方法存在着精度较低的问题。
技术实现要素：
本申请的目的在于提出一种改进的用于确定无人驾驶车辆的速度和位姿的方法和装置，来解决以上背景技术部分提到的技术问题。
第一方面，本申请实施例提供了一种用于确定无人驾驶车辆的速度和位姿的方法，上述无人驾驶车辆设置有gnss接收机、sins以及轮速传感器，上述sins包括imu(inertialmeasurementunit，惯性测量单元)该方法包括：获取上述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、sins1014和轮速传感器1015。网络1012用以在驾驶控制设备1011和gnss接收机1013之间、驾驶控制设备1011与sins1014之间以及驾驶控制设备1011与轮速传感器1015之间提供通信链路的介质。网络1012可以包括各种连接类型，例如有线、无线通信链路或者光纤电缆等等。
gnss接收机1013用于与gnss通信，并解算得到无人驾驶车辆101的速度和位置信息。
sins1014用于根据解算得到无人驾驶车辆101的速度和位姿信息。sins1014可以包括imu(惯性测量单元)。作为示例，imu可以包括三个单轴的加速度计和三个单轴的陀螺仪，加速度计可以检测无人驾驶车辆101在沿车体坐标系的三个相互独立的坐标轴方向上的加速度信号，而陀螺仪可以检测无人驾驶车辆101相对于导航坐标系的角速度信号。有了imu测量得到的无人驾驶车辆101在三维空间中的角速度和加速度，sins可以解算出无人驾驶车辆的速度和位姿(即，位置和姿态)。可以理解的是，为了提高可靠性，还可以配备更多的传感器以测量无人驾驶车辆101沿车体坐标系的三个坐标轴方向的信号。一般而言，imu可以安装在无人驾驶车辆101的重心处。
驾驶控制设备(又称为车载大脑)1011负责无人驾驶车辆的智能控制。驾驶控制设备1011可以是单独设置的控制器，例如可编程逻辑控制器(programmablelogiccontroller，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。
这里，车速传感器的尺度因子是用于表征无人驾驶车辆的真实速度与轮速传感器采集的轮速之间的比值。
作为示例，下面给出一种具体实现方式：
其中：
v′od是计算所得的调整后的轮速，包括东向速度、北向速度和天向速度的3×1的矩阵；
δθ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的零偏误差为状态变量，构建第一卡尔曼滤波的状态方程：
其中：
δλ、δl和δh分别表示sins的位姿误差中的位置误差中的经度误差、纬度误差和高度误差；
δve、δvn和δvu分别表示sins的速度误差中的东向速度误差、北向速度误差和天向速度误差；
εx、εy、εz分别表示imu中陀螺仪在x轴方向的零偏、y轴方向的零偏和z轴方向的零偏；
l是sins解算的第一位姿中的纬度；
rn是地球卯酉圈半径；
h是sins解算的第一位姿中的高度；
rm是地球子午圈半径；
wie表示地球自转角速度；
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可以如下表示：
这样，观测量z1可以如下计算得到：
其中：
vsins是由sins解算的第一速度，包括东向速度、北向速度和天向速度的3×1的矩阵；
δθz是第二滤波操作所得到的轮速传感器与imu的z轴之间的安装误差角；
δθx是第二滤波操作所得到的轮速传感器与imu的x轴之间的安装误差角；
δk是第二滤波操作所得到的轮速传感器的尺度因子误差；
vod是轮速传感器采集的轮速，为一维的标量；
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)
其中：
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的零偏误差。
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的零偏误差为状态变量，构建第一卡尔曼滤波的状态方程：
其中关于各个参数的具体解释可以参见步骤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×606×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)
其中：
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的零偏误差。
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，realtimekinematic)定位模式下的情况下，表明从gnss接收机获取的第二速度和第二位置的精度较高，因此可以执行以下第二滤波操作：以轮速传感器的尺度因子误差和轮速传感器与imu之间的安装角误差作为状态变量构建第二卡尔曼滤波的状态方程，以轮速与第二速度之差作为观测量构建第二卡尔曼滤波的观测方程，采用卡尔曼滤波算法得到轮速传感器的尺度因子误差和轮速传感器与imu之间的安装角误差。执行完上述第二滤波操作后，转到步骤405。
这里，轮速传感器的尺度因子用于表征车辆速度与轮速传感器所采集的轮速之间的比值。尺度因子误差经常与车辆轮胎的胎压有关。轮速传感器与imu之间的安装角用于表征从轮速传感器所采集的轮速标量转换成东向速度、北向速度和天向速度所需要的系数。为了得到准确的尺度因子误差以及轮速传感器与imu之间的安装角误差，步骤406’中利用卡尔曼滤波算法对尺度因子误差和轮速传感器与imu之间的安装角误差进行估计，可以得到相对准确的尺度因子误差和轮速传感器与imu之间的安装角误差，作为当前周期的尺度因子误差和轮速传感器与imu之间的安装角误差，这样，上述电子设备可以在下一周期确定gnss接收机接收不到信号执行步骤403的过程中，步骤403中可以根据步骤406’在当前周期执行第二滤波操作所得到的尺度因子误差和轮速传感器与imu之间的安装角误差，对下一周期的轮速进行调整，得到调整后的轮速。
作为示例，下面给出第二滤波操作的一种具体实现方式：
首先，按照如下公式以轮速传感器的尺度因子误差和轮速传感器与imu之间的安装角误差为状态变量构建第二卡尔曼滤波的状态方程：
f2＝03×3(36)
g2＝i3×3(37)
其中：
x2表示状态变量(轮速传感器的尺度因子误差和轮速传感器与imu之间的安装角误差)。实践中，在imu的坐标为imu的坐标系为右—前—上的情况下，由于轮速传感器与imu的x轴和z轴之间的安装角误差对后续sins解算具有意义，而轮速传感器与imu的y轴之间的安装角误差对后续sins解算意义不大，因此，这里只考虑轮速传感器与imu的x轴之间的安装角误差
03×3为3×3的全0矩阵；
i3×3为3×3的单位矩阵；
wk为轮速传感器的尺度因子误差的系统噪声；
再，将上述状态方程按照如下公式，表示成离散化形式：
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可以如下表示：
这样，观测量z2可以如下计算得到：
其中：
vod表示由轮速传感器采集的轮速，为一维标量；
vgnss表示由gnss接收机解算的第二速度，包括东向速度、北向速度和天向速度，为3×1的矩阵；
z2表示观测量：轮速与第二速度之差，包括东向速度差、北向速度差和天向速度差，为3×1的矩阵；
接着，可以按照如下公式对第二卡尔曼滤波进行时间更新：
x2(k,k-1)＝φ2(k,k-1)x2(k-1)(48)
其中：
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之间的安装角误差。
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，centralprocessingunit)601，其可以根据存储在只读存储器(rom，readonlymemory)602中的程序或者从存储部分606加载到随机访问存储器(ram，randomaccessmemory)603中的程序而执行各种适当的动作和处理。在ram603中，还存储有系统600操作所需的各种程序和数据。cpu601、rom602以及ram603通过总线604彼此相连。输入/输出(i/o，input/output)接口605也连接至总线604。
以下部件连接至i/o接口605：包括硬盘等的存储部分606；以及包括诸如lan(局域网，localareanetwork)卡、调制解调器等的网络接口卡的通信部分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的零偏误差；根据速度误差和位姿误差，对第一速度和第一位姿进行调整，得到无人驾驶车辆的速度和位姿。
以上描述仅为本申请的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解，本申请中所涉及的发明范围，并不限于上述技术特征的特定组合而成的技术方案，同时也应涵盖在不脱离上述发明构思的情况下，由上述技术特征或其等同特征进行任意组合而形成的其它技术方案。例如上述特征与本申请中公开的(但不限于)具有类似功能的技术特征进行互相替换而形成的技术方案。