解析apollo纵向控制标定表程序

百度apollo采用标定表描述车辆速度、加速度与油门/刹车之间的关系。该表可使无人车根据当前车速与期望加速度得到合适的油门/刹车开合度。除了文献《Baidu Apollo Auto-Calibration System - An Industry-Level Data-Driven and Learning based Vehicle Longitude Dynamic Calibrating Algorithm》给出的离线与在线标定方法，百度在apollo 3.0版本及更早版本的apollo/modules/tools/calibration中给出了人工标定的程序，可生成该标定表。现将代码进行解释说明。

一、工程目录

打开apollo/modules/tools/calibration，文件目录如下图所示：

其中，关键的几个程序为：data_collector.py、process_data.py与process.py。data_collector.py采集底盘的反馈信息，并保存文件。process_data.py与process.py对采信的数据进行处理得到最终的标定表。

二、数据采集

### data_collector.py ####
import os
import sys
import time
import signal

import rospy
from std_msgs.msg import String

from plot_data import Plotter

from modules.canbus.proto import chassis_pb2
from modules.control.proto import control_cmd_pb2
from modules.localization.proto import localization_pb2


class DataCollector(object):
    """
    DataCollector Class
    """

    def __init__(self):
        self.sequence_num = 0
        self.control_pub = rospy.Publisher(
            '/apollo/control', control_cmd_pb2.ControlCommand, queue_size=1)
        rospy.sleep(0.3)
        self.controlcmd = control_cmd_pb2.ControlCommand()

        self.canmsg_received = False
        self.localization_received = False

        self.case = 'a'
        self.in_session = False

        self.outfile = ""

    def run(self, cmd):
        signal.signal(signal.SIGINT, self.signal_handler)
		# 根据加速度、速度限制、减速度等信息得到将要保存的文件名
        self.in_session = True
        self.cmd = map(float, cmd)
        out = ''
        if self.cmd[0] > 0:
            out = out + 't'
        else:
            out = out + 'b'
        out = out + str(int(self.cmd[0]))
        if self.cmd[2] > 0:
            out = out + 't'
        else:
            out = out + 'b'
        out = out + str(int(self.cmd[2])) + 'r'
        i = 0
        self.outfile = out + str(i) + '_recorded.csv'
        # 得到一个未存在的新文件名
        while os.path.exists(self.outfile):
            i += 1
            self.outfile = out + str(i) + '_recorded.csv'
        self.file = open(self.outfile, 'w')
        self.file.write(
            "time,io,ctlmode,ctlbrake,ctlthrottle,ctlgear_location,vehicle_speed,"
            +
            "engine_rpm,driving_mode,throttle_percentage,brake_percentage,gear_location, imu\n"
        ) # 保存的数据头 

        print "Send Reset Command"
        self.controlcmd.header.module_name = "control"
        self.controlcmd.header.sequence_num = self.sequence_num
        self.sequence_num = self.sequence_num + 1
        self.controlcmd.header.timestamp_sec = rospy.get_time()
        self.controlcmd.pad_msg.action = 2
        self.control_pub.publish(self.controlcmd)

        rospy.sleep(0.2)
        # Set Default Message
        print "Send Default Command"
        self.controlcmd.pad_msg.action = 1
        self.controlcmd.throttle = 0
        self.controlcmd.brake = 0
        self.controlcmd.steering_rate = 100
        self.controlcmd.steering_target = 0
        self.controlcmd.gear_location = chassis_pb2.Chassis.GEAR_DRIVE

        self.canmsg_received = False

        rate = rospy.Rate(100)
        while self.in_session:
            self.publish_control() # 进入到发送控制命令函数
            rate.sleep()

    def signal_handler(self, signal, frame):
        self.in_session = False

    def callback_localization(self, data):
        """
        New Localization
        """
        self.acceleration = data.pose.linear_acceleration_vrf.y
        self.localization_received = True

    def callback_canbus(self, data):
        """
        New CANBUS
        """
        if not self.localization_received:
            print "No Localization Message Yet"
            return
        timenow = data.header.timestamp_sec
        self.vehicle_speed = data.speed_mps
        self.engine_rpm = data.engine_rpm
        self.throttle_percentage = data.throttle_percentage
        self.brake_percentage = data.brake_percentage
        self.gear_location = data.gear_location
        self.driving_mode = data.driving_mode

        self.canmsg_received = True
        if self.in_session:
            self.write_file(timenow, 0)  # 记录一组数据，该数据标记为0,在处理阶段被使用来生成标定表

    def publish_control(self):
        """
        New Control Command
        """
        if not self.canmsg_received:
            print "No CAN Message Yet"
            return

        self.controlcmd.header.sequence_num = self.sequence_num
        self.sequence_num = self.sequence_num + 1

        if self.case == 'a':
            if self.cmd[0] > 0:
                self.controlcmd.throttle = self.cmd[0]
                self.controlcmd.brake = 0
            else:
                self.controlcmd.throttle = 0
                self.controlcmd.brake = -self.cmd[0]
            if self.vehicle_speed >= self.cmd[1]:
                self.case = 'd'
        elif self.case == 'd':
            if self.cmd[2] > 0:
                self.controlcmd.throttle = self.cmd[0]
                self.controlcmd.brake = 0
            else:
                self.controlcmd.throttle = 0
                self.controlcmd.brake = -self.cmd[2]
            if self.vehicle_speed == 0:
                self.in_session = False

        self.controlcmd.header.timestamp_sec = rospy.get_time()
        self.control_pub.publish(self.controlcmd)
        self.write_file(self.controlcmd.header.timestamp_sec, 1)  # 此处记录的数据，标记为1,在处理阶段未使用
        if self.in_session == False:
            self.file.close()

    def write_file(self, time, io):
        """
        Write Message to File
        """
        self.file.write(
            "%.4f,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s\n" %
            (time, io, 1, self.controlcmd.brake, self.controlcmd.throttle,
             self.controlcmd.gear_location, self.vehicle_speed, self.engine_rpm,
             self.driving_mode, self.throttle_percentage, self.brake_percentage,
             self.gear_location, self.acceleration))  # 记录的数据


def main():
    """
    Main function
    """
    rospy.init_node('data_collector', anonymous=True)

    data_collector = DataCollector()
    plotter = Plotter()
    localizationsub = rospy.Subscriber('/apollo/localization/pose',
                                       localization_pb2.LocalizationEstimate,
                                       data_collector.callback_localization)
    canbussub = rospy.Subscriber('/apollo/canbus/chassis', chassis_pb2.Chassis,
                                 data_collector.callback_canbus)

    print "Enter q to quit"
    print "Enter p to plot result from last run"
    print "Enter x to remove result from last run"
    print "Enter x y z, where x is acceleration command, y is speed limit, z is decceleration command"
    print "Positive number for throttle and negative number for brake"
	# 命令行输入指定，程序按指定执行特定操作，当输入的个数为3时，也即包含{加速度，速度限制，减速度}等信息
    while True:
        cmd = raw_input("Enter commands: ").split()
        if len(cmd) == 0:
            print "Quiting"
            break
        elif len(cmd) == 1:
            if cmd[0] == "q":
                break
            elif cmd[0] == "p":
                print "Plotting result"
                if os.path.exists(data_collector.outfile):
                    plotter.process_data(data_collector.outfile)
                    plotter.plot_result()
                else:
                    print "File does not exist"
            elif cmd[0] == "x":
                print "Removing last result"
                if os.path.exists(data_collector.outfile):
                    os.remove(data_collector.outfile)
                else:
                    print "File does not exist"
        elif len(cmd) == 3:
            data_collector.run(cmd) # 进入数据采集主要程序


if __name__ == '__main__':
    main()

三、数据处理，生成标定表

###  process_data.py  ####
import math
import sys

import numpy as np
import tkFileDialog

from process import get_start_index
from process import preprocess
from process import process


class Plotter(object):
    """
    plot the speed info
    """

    def __init__(self, filename):
        """
        init the speed info
        """

        np.set_printoptions(precision=3)
        self.file = open('result.csv', 'a')
        self.file_one = open(filename + ".result", 'w')

    def process_data(self, filename):
        """
        load the file and preprocess th data
        """

        self.data = preprocess(filename)  # 预处理

        self.tablecmd, self.tablespeed, self.tableacc, self.speedsection, self.accsection, self.timesection = process(
            self.data) #核心处理程序

    def save_data(self):
        """
        """
        for i in range(len(self.tablecmd)):
            for j in range(len(self.tablespeed[i])):
                self.file.write("%s, %s, %s\n" %
                                (self.tablecmd[i], self.tablespeed[i][j],
                                 self.tableacc[i][j]))
                self.file_one.write("%s, %s, %s\n" %
                                    (self.tablecmd[i], self.tablespeed[i][j],
                                     self.tableacc[i][j]))


def main():
    """
    demo
    """
    if len(sys.argv) == 2:
        # get the latest file
        file_path = sys.argv[1]
    else:
        file_path = tkFileDialog.askopenfilename(
            initialdir="/home/caros/.ros",
            filetypes=(("csv files", ".csv"), ("all files", "*.*")))
    plotter = Plotter(file_path)
    plotter.process_data(file_path)
    plotter.save_data()
    print 'save result to:', file_path + ".result"


if __name__ == '__main__':
    main()

3.1 预处理

import math
import warnings

import numpy as np
import scipy.signal as signal

warnings.simplefilter('ignore', np.RankWarning)

SPEED_INTERVAL = 0.2
SPEED_DELAY = 130  #Speed report delay relative to IMU information


def preprocess(filename):
    data = np.genfromtxt(filename, delimiter=',', names=True)
    data = data[np.where(data['io'] == 0)[0]]
    data = data[np.argsort(data['time'])]
    data['time'] = data['time'] - data['time'][get_start_index(data)]

    b, a = signal.butter(6, 0.05, 'low')  # 低通滤波，去除数据中的噪声，由于采集频率为100HZ，此处表示留下频率为10HZ的信号，去除高频噪声。
    data['imu'] = signal.filtfilt(b, a, data['imu'])

    data['imu'] = np.append(data['imu'][-SPEED_DELAY / 10:],
                            data['imu'][0:-SPEED_DELAY / 10]) 
    return data

def get_start_index(data):
    if np.all(data['vehicle_speed'] == 0):
        return 0

    start_ind = np.where(data['vehicle_speed'] == 0)[0][0]

    ind = start_ind
    while ind < len(data):
        if data['vehicle_speed'][ind] == 0:  
            ind = ind + 1
            # begin from vehicle_speed > 0 
        else:
            break
    return ind

• 数据对齐说明

    data['imu'] = np.append(data['imu'][-SPEED_DELAY / 10:],
                            data['imu'][0:-SPEED_DELAY / 10]) 

有两种理解，分别为apollo的理解和我的理解。

apollo里的理解：

在给定油门/刹车开度得到加速度，但是速度是加速度与时间共同作用的结果。换句话说，与加速度对应的速度在未来。要把速度与加速度对齐，需要将加速度整体向后偏移一个时间常量，此处为$\frac{13}{100HZ}s=130ms$，与决策周期$100ms$非常接近。

我的理解与apollo的作法正好相反：

由于采集时，速度、加速度、油门/刹车的数据的时间戳是相同的。标定表能够工作的前提是，在速度一定下，给定确定的油门量或刹车量，能够得到确定的加速度。但是当前速度下，给定油门/刹车量，得到的加速度应该反应在未来时刻。因此，需要将加速度数据整体向前偏移一个时间常量。

谁对谁错呢？

3.2 后处理

def process(data):
    """
    process data
    """
    np.set_printoptions(precision=3)

    if np.all(data['vehicle_speed'] == 0):
        print "All Speed = 0"
        return [], [], [], [], [], []

    start_index = get_start_index(data)

    #print "Start index: ", start_index
    data = data[start_index:]
    data['time'] = data['time'] - data['time'][0]
	# 得到单调加速段与单调减速段，因为在单调加速段，油门量相同，单调减速段，刹车量相同，便于批量处理。
    transition = np.where(
        np.logical_or(
            np.diff(data['ctlbrake']) != 0, np.diff(data['ctlthrottle']) != 0))[
                0]
    transition = np.insert(np.append(transition, len(data) - 1), 0, 0)
    #print "Transition indexes: ", transition

    speedsegments = []
    timesegments = []
    accsegments = []
    tablespeed = []
    tableacc = []
    tablecmd = []

    for i in range(len(transition) - 1):
        #print "process transition index:", data['time'][transition[i]], ":", data['time'][transition[i + 1]]
        speedsection = data['vehicle_speed'][transition[i]:transition[i +
                                                                      1] + 1]
        timesection = data['time'][transition[i]:transition[i + 1] + 1]
        brake = data['ctlbrake'][transition[i] + 1]
        throttle = data['ctlthrottle'][transition[i] + 1]
        imusection = data['imu'][transition[i]:transition[i + 1] + 1]
        if brake == 0 and throttle == 0:
            continue
        #print "Brake CMD: ", brake, " Throttle CMD: ", throttle
        firstindex = 0

        while speedsection[firstindex] == 0:
            firstindex = firstindex + 1
        firstindex = max(firstindex - 2, 0)
        speedsection = speedsection[firstindex:]
        timesection = timesection[firstindex:]
        imusection = imusection[firstindex:]

        if speedsection[0] < speedsection[-1]:
            is_increase = True
            lastindex = np.argmax(speedsection)
        else:
            is_increase = False
            lastindex = np.argmin(speedsection)

        speedsection = speedsection[0:lastindex + 1]
        timesection = timesection[0:lastindex + 1]
        imusection = imusection[0:lastindex + 1]

        speedmin = np.min(speedsection)
        speedmax = np.max(speedsection)
        speedrange = np.arange(
            max(0, round(speedmin / SPEED_INTERVAL) * SPEED_INTERVAL),
            min(speedmax, 10.01), SPEED_INTERVAL)
        #print "Speed min, max", speedmin, speedmax, is_increase, firstindex, lastindex, speedsection[-1]
        accvalue = []
		# 对于给定速度，查询或插值得到对应的加速度数据。
        for value in speedrange:
            val_ind = 0
            if is_increase:
                while val_ind < len(
                        speedsection) - 1 and value > speedsection[val_ind]:
                    val_ind = val_ind + 1
            else:
                while val_ind < len(
                        speedsection) - 1 and value < speedsection[val_ind]:
                    val_ind = val_ind + 1
            if val_ind == 0:
                imu_value = imusection[val_ind]
            else:
                slope = (imusection[val_ind] - imusection[val_ind - 1]) / (
                    speedsection[val_ind] - speedsection[val_ind - 1])
                imu_value = imusection[val_ind - 1] + slope * (
                    value - speedsection[val_ind - 1])
            accvalue.append(imu_value)  

        if brake == 0:
            cmd = throttle
        else:
            cmd = -brake
        #print "Overall CMD: ", cmd
        #print "Time: ", timesection
        #print "Speed: ", speedrange
        #print "Acc: ", accvalue
        #print cmd
        tablecmd.append(cmd)
        tablespeed.append(speedrange)
        tableacc.append(accvalue)

        speedsegments.append(speedsection)
        accsegments.append(imusection)
        timesegments.append(timesection)

    return tablecmd, tablespeed, tableacc, speedsegments, accsegments, timesegments