Python基于Kitti数据集实现自动驾驶车辆轨迹绘制

唐可

发布时间：2026-10-15 | 5人浏览过

原创

本文介绍如何使用Python处理Kitti数据集，结合GPS与IMU数据计算车辆位移和旋转角度，进而绘制单车及多车行驶轨迹。通过对比两种传感器数据，展示利用IMU yaw值进行坐标变换的方法，实现自动驾驶场景下的物体轨迹可视化。

本文是前期内容的延续，重点展示如何利用Kitti数据集绘制车辆的行驶轨迹。

首先，我们查看最终实现的可视化效果。

第一步，利用IMU和GPS数据计算汽车的移动距离与旋转角度。

具体涉及移动距离的计算方法。
其中一种方式是通过GPS数据进行计算。

#定义计算GPS距离方法
def computer_great_circle_distance(lat1,lon1,lat2,lon2):
    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))

#使用GPS计算距离
 gps_distance += [computer_great_circle_distance(imu_data.lat,imu_data.lon,prev_imu_data.lat,prev_imu_data.lon)]

另一种方式则是通过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']
#获取IMU数据
imu_data = read_imu('/home/wsj/data/kitty/RawData/2011_09_26/2011_09_26_drive_0005_sync/oxts/data/%010d.txt'%frame)
#使用IMU计算距离
imu_distance += [0.1*np.linalg.norm(imu_data[['vf','vl']])]

随后对两种计算方式得出的距离结果进行比较（GPS与IMU）。

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()

从结果可以看出，通过IMU计算得到的距离曲线更为平滑。

关于旋转角度的计算相对简单，只需根据IMU获取的yaw值进行前后两帧图像的yaw值相减即可得出。

第二步，绘制Kitti数据集中车辆的行驶轨迹。

prev_imu_data = None
locations = []
for frame in range(150):
    imu_data = read_imu('/home/wsj/data/kitty/RawData/2011_09_26/2011_09_26_drive_0005_sync/oxts/data/%010d.txt'%frame)
    
    if prev_imu_data is not None:
        displacement = 0.1*np.linalg.norm(imu_data[['vf','vl']])
        yaw_change = float(imu_data.yaw-prev_imu_data.yaw)
        for i in range(len(locations)):
            x0, y0 = locations[i]
            x1 = x0 * np.cos(yaw_change) + y0 * np.sin(yaw_change) - displacement
            y1 = -x0 * np.sin(yaw_change) + y0 * np.cos(yaw_change)
            locations[i] = np.array([x1,y1])
            
    locations += [np.array([0,0])]           
    prev_imu_data =imu_data
plt.figure(figsize=(20,10))
plt.plot(np.array(locations)[:, 0],np.array(locations)[:, 1])

第三步，进一步绘制所有检测到的车辆轨迹。

class Object():
    def __init__(self, center):
        self.locations = deque(maxlen=20)
        self.locations.appendleft(center)
    def update(self, center, displacement, yaw):
        for i in range(len(self.locations)):
            x0, y0 = self.locations[i]
            x1 = x0 * np.cos(yaw_change) + y0 * np.sin(yaw_change) - displacement
            y1 = -x0 * np.sin(yaw_change) + y0 * np.cos(yaw_change)
            self.locations[i] = np.array([x1,y1])
        if center is not None:    
            self.locations.appendleft(center)
    def reset(self):
        self.locations = deque(maxlen=20)
#创建发布者        
loc_pub = rospy.Publisher('kitti_loc', MarkerArray, queue_size=10)

  #获取距离和旋转角度
        imu_data =  read_imu('/home/wsj/data/kitty/RawData/2011_09_26/2011_09_26_drive_0005_sync/oxts/data/%010d.txt'%frame)
        
        if prev_imu_data is None:
            for track_id in centers:
                tracker[track_id] = Object(centers[track_id])
        else:
            displacement = 0.1*np.linalg.norm(imu_data[['vf','vl']])
            yaw_change = float(imu_data.yaw - prev_imu_data.yaw)
            for track_id in centers: # for one frame id 
                if track_id in tracker:
                    tracker[track_id].update(centers[track_id], displacement, yaw_change)
                else:
                    tracker[track_id] = Object(centers[track_id])
            for track_id in tracker:# for whole ids tracked by prev frame,but current frame did not
                if track_id not in centers: # dont know its center pos
                    tracker[track_id].update(None, displacement, yaw_change)
        
        prev_imu_data = imu_data
        
def publish_loc(loc_pub, tracker, centers):
    marker_array = MarkerArray()
    for track_id in centers:
        marker = Marker()
        marker.header.frame_id = FRAME_ID
        marker.header.stamp = rospy.Time.now()
 
        marker.action = marker.ADD
        marker.lifetime = rospy.Duration(LIFETIME)
        marker.type = Marker.LINE_STRIP
        marker.id = track_id
        marker.color.r = 1.0
        marker.color.g = 1.0
        marker.color.b = 0.0
        marker.color.a = 1.0
        marker.scale.x = 0.2
    
        marker.points = []
        for p in tracker[track_id].locations:
            marker.points.append(Point(p[0], p[1], 0))
        marker_array.markers.append(marker)
    loc_pub.publish(marker_array)