【从kitti开始自动驾驶】--9.1 利用IMUGPS测距并比较效果

“义勇添青史几段”

• 1 基础知识
• • 1.1 角度计算
  • 1.2 距离计算
• 2 python程序编写
• • 2.1 大圆距离公式函数
  • 2.2-a GPS测距离
  • 2.2-b IMU测距离
  • 2.3 绘图
  • 2.4 效果展示
  • 2.5 结论
• 3. Measure_distance源码

在我们的数据集中,有IMU和GPS的资料,利用其中的信息可以计算的到每一帧(0.1s)之间的距离和方向的数据

本届jupyter工程存放在est3_autodrive_ws/src/jupyter_prj/Mesure_distance.ipynb

1 基础知识

要得到距离,需要知道两个因素:角度偏移和移动的距离

1.1 角度计算

角度计算较为简单, 在GPS/IMU数据中,有yaw航向角数据,两帧做差即可得到角度偏移

1.2 距离计算

得到距离有两种计算方法:

1. 利用GPS的精度和维度,上一帧的lantitude和lontitude已知,则利用大圆距离公式可以得到其在地球上的距离,是利用卫星测量;
2. 利用车上IMU的瞬时前向速度和横向速度(vf, vl),当前0.1秒,则速度合成后*0.1则可近似看做是该帧到下一帧的距离.

2 python程序编写 2.1 大圆距离公式函数

• 输入这一帧的经纬度, 上一帧的经纬度
• 地球半径6371000
• clip限幅函数,对delta_sigma限幅

def compute_great_circle_distance(lat1, lon1, lat2, lon2):
    '''
    Compute the great circle distance from two gps data
    Input: latitudes and longitudes in degree 
    Output : distance in meter
    '''
    delta_sigma = float(np.sin(lat1*np.pi/180)*np.sin(lat2*np.pi/180) + 
                       np.cos(lat1*np.pi/180)*np.cos(lat2*np.pi/180)*np.cos(lon1*np.pi/180 - lon2*np.pi/180))
    
    return 6371000.0*np.arccos(np.clip(delta_sigma, -1, 1))

2.2-a GPS测距离

• 利用大圆距离公式/经纬度数据测距离
• 建立这一帧和上一帧数据
• 循环迭代

prev_imu_data = None
gps_distance = []
imu_distance = []
for frame in range(150):
    imu_dat = read_imu('/home/qinsir/kitti_folder/2011_09_26/2011_09_26_drive_0005_sync/oxts/data/%010d.txt'%frame)
    if prev_imu_data is not None:
        #法1
        gps_distance += [compute_great_circle_distance(imu_dat.lat, imu_dat.lon, prev_imu_data.lat, prev_imu_data.lon)]
    #迭代
    prev_imu_data = imu_dat

2.2-b IMU测距离

• 利用vl和vf数据测距离
• linalg.onrm函数用于计算一个向量的模
• 速度转距离乘以时间0.1即可

# 保留前一帧IMU的数据
prev_imu_data = None
gps_distance = []
imu_distance = []
for frame in range(150):
    imu_dat = read_imu('/home/qinsir/kitti_folder/2011_09_26/2011_09_26_drive_0005_sync/oxts/data/%010d.txt'%frame)
    if prev_imu_data is not None:
        imu_distance += [0.1 * np.linalg.norm(imu_dat[['vf', 'vl']])]

    prev_imu_data = imu_dat

2.3 绘图

• 在同一张画布上展示,将会更加直观:

import matplotlib.pyplot as plt

plt.figure(figsize=(20, 10))
plt.plot(gps_distance, label = 'gps_distance')
plt.plot(imu_distance, label = 'imu_distance')
plt.legend()
plt.show()

2.4 效果展示

• 蓝色为GPS的效果
• 红色为IMU的效果
  

2.5 结论

通过下=上图看出来,GPS速度非常不平滑,只能反映大概的走势, 反观IMU则速度平滑

• 在短时间,短距离内使用IMU会更符合实际情况
• 但在长时间,IMU的误差会随着时间的累积会越来越多,长时间GPS精准度高

3. Measure_distance源码

源码上传至资源,建议自己编写,资源与如下代码无异

import numpy as np
import pandas as pd

# 提前标注每一行IMU数据的名称
IMU_COLUMN_NAMES = ['lat', 'lon', 'alt', 'roll', 'pitch', 'yaw', 'vn', 've', 'vf', 'vl', 'vu', 'ax', 'ay', 'az', 'af',
                    'al', 'au', 'wx', 'wy', 'wz', 'wf', 'wl', 'wu', 'posacc', 'velacc', 'navstat', 'numsats', 'posmode', 
                    'velmode', 'orimode']

def read_imu(path):
    df = pd.read_csv(path, header=None, sep=" ")
    df.columns = IMU_COLUMN_NAMES
    return df

def compute_great_circle_distance(lat1, lon1, lat2, lon2):
    '''
    Compute the great circle distance from two gps data
    Input: latitudes and longitudes in degree 
    Output : distance in meter
    '''
    delta_sigma = float(np.sin(lat1*np.pi/180)*np.sin(lat2*np.pi/180) + 
                       np.cos(lat1*np.pi/180)*np.cos(lat2*np.pi/180)*np.cos(lon1*np.pi/180 - lon2*np.pi/180))
    
    return 6371000.0*np.arccos(np.clip(delta_sigma, -1, 1))

# 保留前一帧IMU的数据
prev_imu_data = None
gps_distance = []
imu_distance = []
for frame in range(150):
    imu_dat = read_imu('/home/qinsir/kitti_folder/2011_09_26/2011_09_26_drive_0005_sync/oxts/data/%010d.txt'%frame)
    if prev_imu_data is not None:
        #法1
        gps_distance += [compute_great_circle_distance(imu_dat.lat, imu_dat.lon, prev_imu_data.lat, prev_imu_data.lon)]
        #法2  linalg.norm函数计算向量长度, 0.1系数长度 
        #当前速度矢量的长度,再转为距离
        imu_distance += [0.1 * np.linalg.norm(imu_dat[['vf', 'vl']])]
    #迭代
    prev_imu_data = imu_dat
    
gps_distance
    
    
import matplotlib.pyplot as plt

plt.figure(figsize=(20, 10))
plt.plot(gps_distance, label = 'gps_distance')
plt.plot(imu_distance, label = 'imu_distance')
plt.legend()
plt.show()

#通过下图看出来,GPS速度非常不平滑,只能反映大概的走势
#IMU速度平滑
# 结论:在短时间,短距离内使用IMU会更符合实际情况
# 结论:在长时间,IMU的误差会随着时间的累积会越来越多,长时间GPS精准度高

#地球半径6371KM
#clip 限幅函数