买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：东风商用车有限公司

摘要：本发明提供一种高精度定位方法、装置、设备及可读存储介质。该方法包括：获取自车当前时刻的观测状态向量以及自车上一时刻的融合优化后的状态向量，其中，所述状态向量包括自车的坐标、航向角和或速度；根据自车上一时刻的融合优化后的状态向量得到自车当前时刻的预测状态向量；将自车当前时刻的观测状态向量与自车当前时刻的预测状态向量进行融合，得到自车当前时刻的融合优化后的状态向量。通过本发明，解决了高精度惯导实现车道级定位的成本高且稳定性和精度较低的问题。

主权项：1.一种高精度定位方法，其特征在于，所述高精度定位方法包括：获取自车当前时刻的观测状态向量以及自车上一时刻的融合优化后的状态向量，其中，所述状态向量包括自车的坐标、航向角和或速度；根据自车上一时刻的融合优化后的状态向量得到自车当前时刻的预测状态向量；将自车当前时刻的观测状态向量与自车当前时刻的预测状态向量进行融合，得到自车当前时刻的融合优化后的状态向量；所述根据自车上一时刻的融合优化后的状态向量得到自车当前时刻的预测状态向量的步骤包括：根据自车上一时刻的融合优化后的坐标、航向角和或速度通过扩展卡尔曼滤波算法第一递推公式得到自车当前时刻的预测状态向量，第一递推公式如下： 其中，为自车上一时刻的融合优化后的速度，（，）为自车上一时刻的融合优化后的坐标，其中，为自车上一时刻x方向融合优化后的坐标，为自车上一时刻y方向融合优化后的坐标，为自车上一时的融合优化后的航向角，为当前时刻与上一时刻的间隔时长，为自车轴距，为自车方向盘转角，为自车当前时刻的预测速度，为自车当前时刻的预测航向角，（，）为自车当前时刻的预测坐标，其中，为自车当前时刻x方向预测坐标，为自车当前时刻y方向预测坐标；所述将自车当前时刻的观测状态向量与自车当前时刻的预测状态向量进行融合，得到自车当前时刻的融合优化后的状态向量的步骤包括：在上一时刻通过扩展卡尔曼滤波算法第二递推公式递推当前时刻的协方差矩阵，其中，为当前时刻的协方差矩阵，J为雅克比矩阵，为上一时刻的协方差矩阵，T为转置矩阵，Q为过程噪声；将当前时刻的协方差矩阵代入第一预设公式计算得到扩展卡尔曼增益，第一预设公式如下：，其中，R为观测噪声，H为观测矩阵，K为扩展卡尔曼增益；将扩展卡尔曼增益、自车当前时刻的观测状态向量以及自车当前时刻的预测状态向量代入第二预设公式将自车当前时刻的观测状态向量与自车当前时刻的预测状态向量进行融合，得到自车当前时刻的融合优化后的状态向量，第二预设公式如下：，其中，为自车当前时刻的预测状态向量，为自车当前时刻的观测状态向量，为自车当前时刻的融合优化后的状态向量。

全文数据：

权利要求：

百度查询： 东风商用车有限公司 高精度定位方法、装置、设备及可读存储介质