[发明专利]获得车辆定位信息的方法及装置

权利要求书

1.一种获得车辆定位信息的方法，其特征在于，包括：

构建用于表示车辆在多个时刻的导航状态以及所述多个时刻的导航状态之间的约束关系的因子图，其中，每个时刻的导航状态均包括所述车辆在该时刻的位姿、速度以及惯性测量单元IMU的零偏量，所述多个时刻包括目标时刻；

确定所述因子图中所述多个时刻的导航状态的初始值；

基于所述多个时刻的导航状态的初始值，以及所述多个时刻的导航状态之间的约束关系对所述因子图进行优化求解；

将所述目标时刻的导航状态的优化值确定为车辆在所述目标时刻的定位信息；

其中，所述因子图包括：

多个节点组，用于表示所述多个时刻的导航状态，其中，每个节点组均包括X节点、V节点以及B节点，分别对应所述多个时刻的导航状态中一个时刻的导航状态的位姿、速度以及IMU的零偏量，所述多个节点组在所述因子图中按照所述多个时刻的时间先后顺序排列；

多个约束因子，用于表示所述多个时刻的导航状态之间的约束关系对应，每个约束因子均与所述因子图中的至少一个节点连接，用于表示存在于所述至少一个节点之间的约束关系；所述多个约束因子包括先验因子、全球定位系统GPS因子、绝对位姿因子、相对位姿因子、IMU因子以及IMU偏移量因子中的至少一种约束因子，其中，

所述先验因子能够与所述因子图的第一个节点组的X节点、V节点或B节点连接，所述先验因子为基于所述多个时刻中的最早时刻之前的导航状态或观测量构建的约束方程；

所述GPS因子能够与所述因子图的任意一个X节点连接，所述GPS因子为基于GPS的观测量构建的约束方程；

所述绝对位姿因子能够与所述因子图的任意一个X节点连接，所述绝对位姿因子为基于视觉定位系统的观测量构建的约束方程；

所述相对位姿因子能够分别与所述因子图的任意两个不同的X节点连接，所述相对位姿因子为基于轮速里程计的观测量构建的约束方程；

所述IMU因子能够分别与所述因子图的任意一个节点组的X节点、V节点以及B节点连接，以及分别与和该节点组相邻的下一个节点组的X节点以及V节点连接，所述IMU因子为基于IMU的观测量构建的约束方程；

所述IMU偏移量因子能够分别与所述因子图的任意两个相邻的B节点连接，所述IMU偏移量因子为基于IMU的零偏量的偏移量构建的约束方程。

2.根据权利要求1所述的获得车辆定位信息的方法，其特征在于，所述构建用于表示车辆在多个时刻的导航状态以及所述多个时刻的导航状态之间的约束关系的因子图，包括：

判断所述目标时刻是否为所述多个时刻中的最晚时刻；

若是，获得包括所述多个时刻中除所述目标时刻外的其他时刻的其他节点组，以及与所述其他节点组中的节点连接的其他约束因子的部分因子图；

在所述部分因子图的最后一个节点组之后构建所述目标时刻的目标节点组；

基于所述目标时刻的观测量构建与所述目标节点组中的节点连接的目标约束因子；

获得包括所述部分因子图、所述目标节点组以及所述目标约束因子的所述因子图。

3.根据权利要求2所述的获得车辆定位信息的方法，其特征在于，所述判断所述目标时刻是否为所述多个时刻中的最晚时刻之后，所述方法还包括：

若否，获得包括所述多个时刻中除所述目标时刻外的其他时刻的其他节点组的部分因子图；

在所述部分因子图的与所述目标时刻相邻的两个时刻的节点组之间构建所述目标时刻的目标节点组；

基于所述目标时刻的观测量构建与所述目标节点组中的节点连接的目标约束因子；

基于所述其他时刻的观测量构建与所述其他节点组中的节点连接的其他约束因子；

获得包括所述部分因子图、所述其他约束因子、所述目标节点组以及所述目标约束因子的所述因子图。

4.根据权利要求1－3中任一项所述的获得车辆定位信息的方法，其特征在于，所述确定所述因子图中所述多个时刻的导航状态的初始值，包括：

将所述多个时刻中除所述目标时刻外的其他时刻的导航状态的历史优化值确定为所述其他时刻的导航状态的初始值；

基于所述其他时刻中与所述目标时刻相邻的前一时刻的导航状态的历史优化值，以及所述前一时刻之后的IMU的观测量，积分获得所述目标时刻的导航状态的初始值。