基于FPGA时间同步的里程计快速标定方法及系统技术方案

本发明专利技术公开了一种基于FPGA时间同步的里程计快速标定方法。包括初始静对准步骤，由GNSS/IMU组合导航系统完成初始静对准；行车路程划分步骤，利用轨迹相似性原理，将行车路程划分，分别计算里程计分段时间内脉冲当量和GNSS/IMU组合导航系统分段时间内的位移增量；标定步骤，通过递推最小二乘法，对里程计刻度系数进行标定。本发明专利技术可以实现快速准确的估计结果。

【技术实现步骤摘要】
基于FPGA时间同步的里程计快速标定方法及系统
本申请涉及自动驾驶领域，更具体地涉及一种基于FPGA时间同步的里程计快速标定方法。
技术介绍
随着自动驾驶技术的发展与日臻成熟，自动驾驶将成为未来出行的一种重要手段。现阶段的自动驾驶系统功能模块主要包括感知模块、定位模块、规划决策模块、控制模块、高精地图模块及云计算模块。定位模块作为自动驾驶系统的核心模块，一方面用于路径规划和车辆的精确控制；同时还辅助感知系统，得到更加准确的检测和跟踪结果。在现有自动驾驶车辆定位技术方案中，多数采用多传感器融合定位技术来确定车辆运动状态如车辆位置、速度和姿态等。通常采用的传感器包括惯性测量单元(InertialMeasurementUnit,IMU)、全球卫星导航系统(GlobalNavigationSatelliteSystem,GNSS)、里程计(Odometry,ODO)、激光雷达(Lidar)等。里程计可以用来测量自动驾驶车辆的行驶速度/行驶路程，其测量误差不随行驶时间积累，测速范围宽、动态性能好，自主性和抗干扰能力强，与惯性测量单元(InertialMeasurementUnit,IMU)、全球卫星导航系统(GlobalNavigationSatelliteSystem,GNSS)、激光雷达(Lidar)等通过数据融合技术，能够有效提升自动驾驶车辆在城市峡谷、GNSS信号遮挡、应用环境特征退化等典型城市工况下短时定位精度。通常，里程计传感器输出与车辆行驶路程成比例的脉冲数，受限于路况特征(坑洼、崎岖不平、胎压)等因素，刻度系数的准确程度直接影响里程计测得的速度精度，因此需要对里程计标度因数进行精确标定。专利号：CN106595715B，《基于捷联惯导与卫星组合导航系统里程计标定方法与装置》，利用捷联惯导与卫星组合导航系统初始对准后的速度输出序列与里程计速度序列进行时间对齐，以此来标定里程计的刻度系数和安装误差角，该方案标定精度高，适用于陆基通信指挥、测绘车辆，但捷联惯导初始对准精度收敛需要较长时间，操作复杂度高，价格高昂(30万以上)，不利于自动驾驶车辆系统快速部署。且受限于车载里程计增量输出采样间隔不稳定，直接利用里程计位移增量求取速度序列存在噪声放大问题，影响里程计刻度系数准确标定。专利号：CN111098335A，《一种双轮差速驱动机器人里程计标定方法》利用连续多个满足收敛条件时位姿测量时刻修正后的状态向量确定差动机器人的里程计标定结果，但鲜有提及里程计及机器人位姿测量系统时间同步的约束，限于差速里程计上报运动增量信息无时间指标，仅利用里程计左右轮运动增量与位姿测量系统的输出进行时间匹配插值，难以准确标定差速里程计参数。专利号：CN111324848A，《移动激光雷达测量系统车载轨迹数据优化》，通过GNSS/IMU组合导航系统输出的导航参数(位置、速度、姿态)以及IMU加速度计信息，对测量轨迹进行粗分段、细分段，利用SG局部特征的多项式拟合方法对轨迹数据进行平滑处理，有助于消除GNSS/IMU组合导航系统的局部位置跳跃点，提高轨迹数据、点云地图精度。该专利属于轨迹分段后应用领域，分段及平滑结果依赖于GNSS/IMU组合导航系统的输出精度，忽略了自动驾驶车辆应用场景复杂(城市峡谷、工业园区、隧道等)，GNSS/IMU组合导航系统不能提供长时间的精确的位置、速度、姿态(俯仰角、横滚角)及方位信息。同时，一种基于FPGA时间同步模块的里程计快速标定方案，在初始静对准和标定阶段依赖于GNSS信号环境，标定车辆需在空旷环境下行驶，确保分段时间内GNSS/IMU组合导航系统输出的位置精度足够高。同时，GNSS/IMU组合导航系统组合输出频率高(典型值：100～200Hz)，进行0.1s～0.5s分段，轨迹相对较平滑，无需局部特殊处理。相应的，标定后的车辆可降低对GNSS信号环境的依赖，能提供较高精度的位置信息给点云地图以及PNC(PlanningandControl)模块，在复杂场景使用。
技术实现思路
1、本专利技术的目的本专利技术为了解决里程计标度因数精确标定的问题，而提出了一种基于FPGA时间同步的里程计快速标定方法。2、本专利技术所采用的技术方案本专利技术公开了一种基于FPGA时间同步的里程计快速标定方法，包括：初始静对准步骤，由GNSS/IMU组合导航系统完成初始静对准；行车路程划分步骤，利用轨迹相似性原理，将行车路程划分，分别计算里程计分段时间内脉冲当量和GNSS/IMU组合导航系统分段时间内的位移增量；标定步骤，通过递推最小二乘法，对里程计刻度系数进行标定。优选的，行车路程划分步骤，将里程计采样和组合导航系统输出进行精确的时间对齐，并把行车路程分段，分别计算里程计分段的脉冲当量和GNSS/IMU组合导航系统分段位移增量。优选的，行车路程分成N小段，计算里程计N小段脉冲当量：假设每小段从A点到B点里程计k个采样时间点上报脉冲数为εi，i＝1,2,…,k，则里程计每小段的脉冲当量为：相应地，行车路程N小段的脉冲当量分别为(t1,n1)、(t2,n2)、…、(tN,nN)。优选的，从A点到B点GNSS/IMU组合导航系统j个采样时间点输出位置信息，即纬度L、经度λ、高程h，则组合导航系统每小段位移增量计算方法如下：RM＝Re(1-2f+3fsin2Li)RN＝Re(1+fsin2Li)liE＝(RN+h)cosLjsin(λi+1-λi)liN＝(RM+h)sin(λi+1-λi)liU＝hi+1-hi其中，Li、λi、hi为第i个采样时间点，GNSS/IMU组合导航系统输出的车辆纬度、经度、海拔高度信息；liE、liN、liU为车辆在第i至第i+1采样时刻点的东向、北向、天向位移增量；pi,i+1为每小分段内第i个采样点到第i+1采样点的位移增量；pl为第l个时间分段内的位移增量；RM、RN为地球子午圈和卯酉圈主曲率半径，Re＝6378137m，为地球半长轴，为地球扁率，行车N小段轨迹对应的GNSS/IMU的位移增量分别为(t1,p1)、(t2,p2)、…、(tN,pN)。优选的，标定步骤：根据轨迹相似性原理，自动驾驶车辆N个小段里程计脉冲当量和GNSS/IMU组合导航系统N个小段的位移增量具有线性对应关系，利用递推最小二乘法，可得到：本专利技术提出了一种基于FPGA时间同步的里程计快速标定系统，包括：初始静对准模块，由GNSS/IMU组合导航系统完成初始静对准；行车路程划分模块，利用轨迹相似性原理，将行车路程划分，分别计算里程计分段时间内脉冲当量和GNSS/IMU组合导航系统分段时间内的位移增量；标定模块，通过递推最小二乘法，对里程计刻度系数进行标定。优选的，行车路程划分模块，将里程计采样和组合导航系统输出进行精确的时间对齐，并把行车路程分段，分别计算里程计分段的脉冲当量和GNSS/IMU组合导航系统分段位移增量。<本文档来自技高网...

【技术保护点】
1.一种基于FPGA时间同步的里程计快速标定方法，其特征在于，包括：/n初始静对准步骤，由GNSS/IMU组合导航系统完成初始静对准；/n行车路程划分步骤，利用轨迹相似性原理，将行车路程划分，分别计算里程计分段时间内脉冲当量和GNSS/IMU组合导航系统分段时间内的位移增量；/n标定步骤，通过递推最小二乘法，对里程计刻度系数进行标定。/n

【技术特征摘要】
1.一种基于FPGA时间同步的里程计快速标定方法，其特征在于，包括：
初始静对准步骤，由GNSS/IMU组合导航系统完成初始静对准；
行车路程划分步骤，利用轨迹相似性原理，将行车路程划分，分别计算里程计分段时间内脉冲当量和GNSS/IMU组合导航系统分段时间内的位移增量；
标定步骤，通过递推最小二乘法，对里程计刻度系数进行标定。


2.根据权利要求1所述的基于FPGA时间同步的里程计快速标定方法，其特征在于：行车路程划分步骤，将里程计采样和组合导航系统输出进行精确的时间对齐，并把行车路程分段，分别计算里程计分段时间内的脉冲当量和GNSS/IMU组合导航系统分段时间内的位移增量。


3.根据权利要求2所述的基于FPGA时间同步的里程计快速标定方法，其特征在于：行车路程分成N小段，计算里程计N小段脉冲当量：假设每小段从A点到B点里程计k个采样时间点上报脉冲数为εi，i＝1,2,…,k，则里程计每小段的脉冲当量为：



相应地，行车路程N小段的脉冲当量分别为(t1,n1)、(t2,n2)、…、(tN,nN)。


4.根据权利要求3所述的基于FPGA时间同步的里程计快速标定方法，其特征在于：从A点到B点GNSS/IMU组合导航系统j个采样时间点输出位置信息，即纬度L、经度λ、高程h，则组合导航系统每小段位移增量计算方法如下：
RM＝Re(1-2f+3fsin2Li)
RN＝Re(1+fsin2Li)
liE＝(RN+h)cosLjsin(λi+1-λi)
liN＝(RM+h)sin(λi+1-λi)
liU＝hi+1-hi






其中，Li、λi、hi为第i个采样时间点，GNSS/IMU组合导航系统输出的车辆纬度、经度、海拔高度信息；liE、liN、liU为车辆在第i至第i+1采样时刻点的东向、北向、天向位移增量；pi,i+1为每小分段内第i个采样点到第i+1采样点的位移增量；pl为第l个时间分段内的位移增量；RM、RN为地球子午圈和卯酉圈主曲率半径，Re＝6378137m，为地球半长轴，为地球扁率，行车N小段轨迹对应的GNSS/IMU的位移增量分别为(t1,p1)、(t2,p2)、…、(tN,pN)。


5.根据权利要求1所述的基于FPGA时间同步的里程计快速标定方法，其特征在于，标定步骤：根据轨迹相似性原理，自动驾驶车辆N个小段里程计脉冲当量和GNSS/IMU组合导航系统N个小段的位移增量具有线性对应关系，利用递推最小二乘法，可得到：





6.一种基于FPGA时间同步的里程计快速标定系统，其特征在于，包括：
初始静对准模块，由GNSS/IMU组合导航系统完成初始静对准；
行车路程划分模块...

【专利技术属性】
技术研发人员：陈海波，褚之恒，张保群，
申请(专利权)人：深兰人工智能深圳有限公司，
类型：发明
国别省市：广东;44