一种基于北斗的智慧工地车辆定位方法及系统

专利主权项内容

1.一种基于北斗的智慧工地车辆定位方法，其特征在于，包括：获取北斗卫星观测数据，对所述北斗卫星观测数据进行误差矫正处理，以得到矫正观测数据；基于所述矫正观测数据确定第一伪距观测量，计算所述第一伪距观测量的第一检验值与第二检验值，基于所述第一检验值与所述第二检验值对所述伪距观测量进行噪声剔除，以得到第二伪距观测量；基于所述第二伪距观测量对智慧工地车辆进行坐标联合解算，以得到所述智慧工地车辆的第一定位数据；获取所述智慧工地车辆的惯性数据与里程数据，将所述第一定位数据、所述惯性数据与所述里程数据进行融合，以得到融合数据；将所述融合数据进行粒子转化，以得到位姿粒子集，对所述位姿粒子集进行权重更新与采样处理，以得到所述智慧工地车辆的第二定位数据；所述获取所述智慧工地车辆的惯性数据与里程数据，将所述第一定位数据、所述惯性数据与所述里程数据进行融合，以得到融合数据的步骤包括：获取所述智慧工地车辆的惯性数据与里程数据，并基于所述第一定位数据、所述惯性数据与所述里程数据确定第时刻的智慧工地车辆的初始位姿矩阵/>：
；式中，为第/>时刻的智慧工地车辆在全局坐标下的x轴坐标、y轴坐标、z轴坐标，/>为第/>时刻的智慧工地车辆在全局坐标下的角度，/>分别为第/>时刻的智慧工地车辆的全局线速度、全局角速度；对所述初始位姿矩阵进行运动转化，以得到转化位姿矩阵/>：
；式中，为第/>时刻的智慧工地车辆在全局坐标下的x轴坐标、y轴坐标、z轴坐标，/>为第/>时刻的智慧工地车辆在全局坐标下的角度，/>分别为第/>时刻的智慧工地车辆的全局线速度、全局角速度，/>为高斯分布噪声；将所述转化位姿矩阵、所述惯性数据与所述里程数据进行融合，以得到融合数据/>：
；式中，、/>分别为里程噪声、惯性噪声；所述对所述位姿粒子集进行权重更新与采样处理，以得到所述智慧工地车辆的第二定位数据的步骤包括：将第时刻的位姿粒子集/>均匀分布至全局空间中，其中，/>，/>为第/>个表示第/>时刻的位姿的粒子，/>为粒子/>对应的粒子权重；通过第时刻的位姿粒子集迭代估计第/>时刻的粒子分布，以得到估计位姿粒子/>：
；式中，分别为第/>时刻智慧工地车辆的x方向估计位移、y方向估计位移、z方向估计位移、角度估计值，/>、/>、/>、/>为智慧工地车辆在第/>时刻至第/>时刻的x方向位移变化量、y方向位移变化量、z方向位移变化量、角度变化量，/>分别为x方向位移噪声、y方向位移噪声、z方向位移噪声、角度噪声；基于第时刻的粒子分布对位姿粒子集/>中每个粒子权重进行评估更新，以得到更新权重/>：
；式中，为观测结果，/>为给定估计位姿粒子/>的条件下采样得到/>的理想概率密度；基于所述更新权重对所述位姿粒子集中的所有粒子进行重新采样，以得到采样权重与采样粒子/>，基于所述采样权重/>计算有效粒子数量/>；
；判断所述有效粒子数量是否大于粒子阈值，若所述有效粒子数量/>大于粒子阈值，则第二定位数据/>，若所述有效粒子数量/>不大于粒子阈值，则在所述位姿粒子集添加若干数量的随机粒子，以得到更新粒子集，重复对所述更新粒子集进行粒子分布估计、权重更新与重新采样直至有效粒子数量/>大于粒子阈值，输出对应的迭代采样权重/>与迭代采样粒子/>，则第二定位数据/>，其中，/>为更新粒子集中的粒子数量。