[发明专利]里程计累计推算误差的数据源纠正方法及装置

权利要求书

1.一种里程计累计推算误差的数据源纠正方法，包括如下步骤：

S1.获取里程计输出的第一位姿信息；

S2.采用第二定位系统获取第二位姿信息；

S3.进行第二位姿信息的置信度判定；

S4.在第二位姿信息的置信度满足设定的置信度的判定准则时进行第一位姿信息的误差等级判定；

S5.当第一位姿信息符合误差等级判断时，将第二位姿信息作为数据源，对数据源进行纠正。

2.根据权利要求1所述的里程计累计推算误差的数据源纠正方法，其特征在于步骤S1所述的第一位姿信息包括第一位置信息、第一角度信息和第一速度信息。

3.根据权利要求2所述的里程计累计推算误差的数据源纠正方法，其特征在于步骤S2，具体为第二定位系统提供位置输出，通过与里程坐标系进行角度换算，得到第二位姿信息；所述的第二位姿信息包括第二位置信息，或者第二位姿信息包括第二角度信息和第二速度信息；当第二位姿信息包括第二角度信息和第二速度信息时，直接输出第二位姿信息；当第二位姿信息不包括第二角度信息或第二角度信息由于第二位置信息的置信度低于设定值时，通过当前的第二位姿信息与在设定时间窗口内的若干组第二位姿信息历史值及间隔时间，计算当前移动载体的第二角度信息。

4.根据权利要求3所述的里程计累计推算误差的数据源纠正方法，其特征在于计算当前移动载体的第二角度信息，具体为GNSS天线安装到待测机器人的后部的中间位置，待测机器人在t_k-1时刻的第一GNSS位置及t_k时刻的第二GNSS位置获取GNSS定位信号；待测机器人在t_k-1时刻处于出发位置，此时待测机器人在里程坐标系中的位置定为(0,0)，里程坐标系采用通用横墨卡托格网系统坐标系；将t_k时刻的待测机器人在里程坐标系中的方向角记作ψ_k，将t_k时刻的在里程坐标系内的待测机器人中心位置记作(x_k,y_k)；设定待测机器人兼容的坐标系方向为，x轴向前，y轴向左，待测机器人在水平面上运动；待测机器人的天线在t_k-1时刻和t_k时刻的GNSS经纬度数值转换所得的通用横墨卡托格网系统位置分别记为utm_k-1和utm_k，其中t_k时刻的通用横墨卡托格网系统位置包括北向分量utmN_k和东向分量utmE_k；通用横墨卡托格网系统位置由GNSS经纬度数值转换；因此，根据待测机器人的天线在t_k-1时刻和t_k时刻在通用横墨卡托格网系统坐标系中的位置，采用逆正切法计算出t_k-1时刻到t_k时刻待测机器人的天线的里程位移相对于正东方向的角度，记为γ：

γ＝atan2(utmN_k-utmN_k-1,utmE_k-utmE_k-1)

将α_t设为t_k时刻待测机器人的中心相对于t_k-1时刻的待测机器人中心的方向角增量：

α_t＝atan2(y_k-y_k-1,x_k-x_k-1)

D_a为t_k时刻待测机器人的天线相对于t_k-1时刻待测机器人的中心的位移：

其中，D_c为待测机器人的中心在t_k-1时刻与t_k时刻之间发生的里程位移、L_a为待测机器人中心与待测机器人的天线的安装位置之间的距离；

δ为D_n与待测机器人在t_k-1时刻时中心与天线之间的方向的夹角：

其中，D_n为待测机器人的天线在t_k-1时刻与t_k时刻之间发生的里程位移、L_a为待测机器人的中心与机器人的天线的安装位置之间的距离、D_a为t_k时刻待测机器人的天线相对于t_k-1时刻待测机器人的中心的位移。