[发明专利]无人驾驶汽车的定位方法及装置

权利要求书

1.一种无人驾驶汽车的定位方法，其特征在于，包括：

获取当前控制量、历史观测位姿状态、历史预测位姿状态及历史预测协方差矩阵，并根据所述当前控制量、所述历史观测位姿状态、所述历史预测位姿状态及所述历史预测协方差矩阵，计算时间更新位姿状态及时间更新协方差矩阵；

获取当前观测定位结果；

获取当前观测矩阵及当前观测协方差矩阵，并将所述当前观测矩阵、所述当前观测协方差矩阵及所述时间更新协方差矩阵代入第一预设算法中，计算当前卡尔曼增益矩阵；

将所述时间更新位姿状态、所述当前卡尔曼增益矩阵及所述当前观测定位结果代入第二预设算法中，计算当前预测位姿状态；

所述根据所述当前控制量、所述历史观测位姿状态、所述历史预测位姿状态及所述历史预测协方差矩阵，计算时间更新位姿状态及时间更新协方差矩阵，包括：

使用预设函数对所述历史观测位姿状态进行偏导处理，以获得所述历史观测位姿状态对应的第一雅可比矩阵；

使用所述预设函数对所述当前控制量进行偏导处理，以获得所述当前控制量对应的第二雅可比矩阵；

将所述第一雅可比矩阵、所述第二雅可比矩阵、所述当前控制量及所述历史预测位姿状态代入第三预设算法中，计算所述时间更新位姿状态；

将所述第一雅可比矩阵、所述第二雅可比矩阵、所述历史预测协方差矩阵及自定义代数量代入第四预设算法中，计算所述时间更新协方差矩阵；

所述获取当前观测定位结果，包括：

将所述时间更新位姿状态代入第五预设算法中，计算当前预测定位结果；

获取第一当前定位结果，并计算所述第一当前定位结果与所述当前预测定位结果之间的第一差值；

若所述第一差值小于或等于预设阈值向量，则将所述第一当前定位结果确定为所述当前观测定位结果；

若所述第一差值大于所述预设阈值向量，则获取第二当前定位结果，并计算所述第二当前定位结果与所述当前预测定位结果之间的第二差值；

若所述第二差值小于或等于所述预设阈值向量，则将所述第二当前定位结果确定为所述当前观测定位结果；

若所述第二差值大于所述预设阈值向量，则获取第三当前定位结果，并计算所述第三当前定位结果与所述当前预测定位结果之间的第三差值；

若所述第三差值小于或等于所述预设阈值向量，则将所述第三当前定位结果确定为所述当前观测定位结果；

若所述第三差值大于所述预设阈值向量，则将所述第一差值、第二差值及第三差值中的最小值对应的当前定位结果确定为所述当前观测定位结果；其中，所述第一当前定位结果为基于差分GPS定位法获取得到的定位结果，所述第二当前定位结果为基于激光定位法获取得到的定位结果，所述第三当前定位结果为基于视觉定位法获取得到的定位结果。

2.根据权利要求1所述的方法，其特征在于，在所述将所述时间更新位姿状态、所述当前卡尔曼增益矩阵及所述当前观测定位结果代入第二预设算法中，计算当前预测位姿状态之后，所述方法还包括：

将所述当前观测矩阵、所述当前卡尔曼增益矩阵及所述时间更新协方差矩阵代入第六预设算法中，计算当前预测协方差矩阵；

将所述当前预测位姿状态、所述当前预测协方差矩阵及所述当前观测定位结果缓存至本地存储空间中。

3.根据权利要求1-2中任一项所述的方法，其特征在于，在所述将所述时间更新位姿状态、所述当前卡尔曼增益矩阵及所述当前观测定位结果代入第二预设算法中，计算当前预测位姿状态之后，所述方法还包括：

将所述当前预测位姿状态进行输出显示。