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

本文主要是介绍解析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 signalimport rospy
from std_msgs.msg import Stringfrom plot_data import Plotterfrom modules.canbus.proto import chassis_pb2
from modules.control.proto import control_cmd_pb2
from modules.localization.proto import localization_pb2class DataCollector(object):"""DataCollector Class"""def __init__(self):self.sequence_num = 0self.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 = Falseself.localization_received = Falseself.case = 'a'self.in_session = Falseself.outfile = ""def run(self, cmd):signal.signal(signal.SIGINT, self.signal_handler)# 根据加速度、速度限制、减速度等信息得到将要保存的文件名self.in_session = Trueself.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 = 0self.outfile = out + str(i) + '_recorded.csv'# 得到一个未存在的新文件名while os.path.exists(self.outfile):i += 1self.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_numself.sequence_num = self.sequence_num + 1self.controlcmd.header.timestamp_sec = rospy.get_time()self.controlcmd.pad_msg.action = 2self.control_pub.publish(self.controlcmd)rospy.sleep(0.2)# Set Default Messageprint "Send Default Command"self.controlcmd.pad_msg.action = 1self.controlcmd.throttle = 0self.controlcmd.brake = 0self.controlcmd.steering_rate = 100self.controlcmd.steering_target = 0self.controlcmd.gear_location = chassis_pb2.Chassis.GEAR_DRIVEself.canmsg_received = Falserate = rospy.Rate(100)while self.in_session:self.publish_control() # 进入到发送控制命令函数rate.sleep()def signal_handler(self, signal, frame):self.in_session = Falsedef callback_localization(self, data):"""New Localization"""self.acceleration = data.pose.linear_acceleration_vrf.yself.localization_received = Truedef callback_canbus(self, data):"""New CANBUS"""if not self.localization_received:print "No Localization Message Yet"returntimenow = data.header.timestamp_secself.vehicle_speed = data.speed_mpsself.engine_rpm = data.engine_rpmself.throttle_percentage = data.throttle_percentageself.brake_percentage = data.brake_percentageself.gear_location = data.gear_locationself.driving_mode = data.driving_modeself.canmsg_received = Trueif 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"returnself.controlcmd.header.sequence_num = self.sequence_numself.sequence_num = self.sequence_num + 1if self.case == 'a':if self.cmd[0] > 0:self.controlcmd.throttle = self.cmd[0]self.controlcmd.brake = 0else:self.controlcmd.throttle = 0self.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 = 0else:self.controlcmd.throttle = 0self.controlcmd.brake = -self.cmd[2]if self.vehicle_speed == 0:self.in_session = Falseself.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"breakelif len(cmd) == 1:if cmd[0] == "q":breakelif 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 sysimport numpy as np
import tkFileDialogfrom process import get_start_index
from process import preprocess
from process import processclass 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 filefile_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 warningsimport numpy as np
import scipy.signal as signalwarnings.simplefilter('ignore', np.RankWarning)SPEED_INTERVAL = 0.2
SPEED_DELAY = 130  #Speed report delay relative to IMU informationdef 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 datadef get_start_index(data):if np.all(data['vehicle_speed'] == 0):return 0start_ind = np.where(data['vehicle_speed'] == 0)[0][0]ind = start_indwhile ind < len(data):if data['vehicle_speed'][ind] == 0:  ind = ind + 1# begin from vehicle_speed > 0 else:breakreturn ind

数据对齐说明

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

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

apollo里的理解：

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

我的理解与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_indexdata = 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: ", transitionspeedsegments = []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: ", throttlefirstindex = 0while speedsection[firstindex] == 0:firstindex = firstindex + 1firstindex = max(firstindex - 2, 0)speedsection = speedsection[firstindex:]timesection = timesection[firstindex:]imusection = imusection[firstindex:]if speedsection[0] < speedsection[-1]:is_increase = Truelastindex = np.argmax(speedsection)else:is_increase = Falselastindex = 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 = 0if is_increase:while val_ind < len(speedsection) - 1 and value > speedsection[val_ind]:val_ind = val_ind + 1else:while val_ind < len(speedsection) - 1 and value < speedsection[val_ind]:val_ind = val_ind + 1if 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 = throttleelse:cmd = -brake#print "Overall CMD: ", cmd#print "Time: ", timesection#print "Speed: ", speedrange#print "Acc: ", accvalue#print cmdtablecmd.append(cmd)tablespeed.append(speedrange)tableacc.append(accvalue)speedsegments.append(speedsection)accsegments.append(imusection)timesegments.append(timesection)return tablecmd, tablespeed, tableacc, speedsegments, accsegments, timesegments