Apollo和Carsim/TruckSim联合仿真

1. 背景

仿真在自动驾驶的研发中起到重要作用，能大大提高研发效率，也为算法的可靠性提供保障。百度Apollo系统是优秀开源的平台，适合想学习自动驾驶的朋友研究。Carsim/Trucksim是经典的汽车动力学仿真工具。
今天给大家分享的是通过Apollo和Trucksim的联合，实现本地的实时仿真。本文档适合于初学者搭建仿真平台，研究Apollo系统。

2. 架构设计

Apollo工程核心代码是C++实现，Trucksim的常用接口有simulink, python和C。本文先给出Apollo, simulink和Trucksim联合仿真的架构以及本套仿真存在的弊病。再讲Apollo和Trucksim的联合仿真。

2.1 Apollo, simulink和Trucksim方案

simulink和Apollo可通过ros通信，由于Apollo的消息数据格式是protobuf，而simulink的ros工具只支持标准的ros msg，因此可在Apollo中添加一个格式转换的节点。simulink调用Trucksim的方案网上资源很多，本人不再赘述。

2.2 Apollo，rosbridge和Trucksim方案

建立QT工程，和Apollo工程通过websocket通信. Apollo工程里增加rosbridge(Apollo 5.0后版本使用cyber_bridge), 实现ros消息(或cyber消息)和websocket的互转。QT工程发挥websocket Client作用。QT用C++实现，并调用Trucksim的动态库，达到实时运行Trucksim的作用。
暂时无法在飞书文档外展示此内容

3. 功能实现

3.1 Apollo, simulink和Trucksim方案

3.1.1 Simulink配置

Simulink的工具包中有ROS的支持包，配置ROS网络地址如图2所示。Hostname/IP Address和Port分别为ROS_MASTER_URI的地址和端口号，在上面的通信机制中有说明。

图1 配置ROS网络地址
ROS subscriber接收interface的消息，因此Topic, Message type, Sample time要与interface中程序相对应。

图2 配置ros subcribe

为了便于调试验证，现在MATLAB端启动ROS。配置过程如下：
MATLAB设置的指令：

>> setenv('ROS_MASTER_URI','http://192.168.103.122:11311')
>> setenv('ROS_IP','192.168.103.198')
>> rosinit('192.168.103.122')

3.1.2 TruckSim配置

1. 配置界面
   Trucksim是向导式编程，参数配置界面：卡车选择5A Tractor(SS_SSS)，具体参数见图3.控制界面：Models选择simulink。
   
   图3 主界面配置图
   
   
   图4 拖头拖挂参数配置
   
   图5 拖头具体参数
   
   图6 拖挂具体参数
   
   图7 trucksim输入输出界面
   
   图8 trucksim输出参数设置

2. 输入输出
   输入：
   IMP_THROTTLE_ENGINE: 油门值，单位：百分比
   IMP_BK_STAT: 刹车，单位：百分比
   IMP_DETEER_SW: 方向盘转角速率，单位：度每秒
   IMP_STEER_SW: 方向盘转角，单位：度
   IMP_GEAR_STATUS: 挡位状态
   输出：
   X0: 拖头的全局x坐标，单位：m
   Y0：拖头的全局y坐标，单位：m
   Z0: 拖头的全局z坐标，单位：m
   Yaw: 拖头的全局坐标系下艏向角,单位：°（度）
   Vx: 拖头径向速度，单位：km/h
   Steer_SW：方向盘转角，单位：°（度）
   AV_Y: 拖头的全局坐标系下艏向角速率,单位：°/s（度每秒）
   GearStart: 挡位信息
   X0_2: 拖挂的全局x坐标，单位：m
   Y0_2: 拖挂的全局x坐标，单位：m
   Z0_2: 拖挂的全局x坐标，单位：m
   Yaw_2: 拖挂的全局坐标系下艏向角,单位：°（度）
   Vy: 拖头横向向速度，单位：km/h
   注释：Trucksim的配置可以保存为.cpar文件格式，其他人想配置时，只需加载这些.cpar文件即可，不需要在界面上重新配置。

3.2 Apollo, rosbridge和Trucksim方案

3.2.1 apollo中配置rosbridge

rosbridge安装网上教程较多，本文不再赘述。
使用方法如下：

1. cd ros_pkgs_ws
   catkin_make
2. 启动rosbridge
   source /apollo/ros_pkgs_ws/devel/setup.bash
   PATH=/usr/local/miniconda2/bin:$PATH
   roslaunch rosbridge_server rosbridge_websocket.launch

3.2.1 Qt工程配置说明

3.2.1.1 Qt和CMake版本信息

工具	版本
websocket库	Qt 5.13.1
Qt Creator	4.10.0
CMake	3.15.4

3.2.1.2 环境变量

在系统环境变量path中添加：
C:\Program Files\CMake\bin
C:\Qt\Qt5.13.1\5.13.1\mingw73_64\bin
在用户环境变量中添加：
C:\Qt\Qt5.13.1\Tools\QtCreator\share\qtcreator\cplusplus\wrappedQtHeaders\QtCore
在Qt Creator中构建环境中，选择使用系统环境变量。
说明：环境变量也可以直接在Qt Creator的构建设置中配置，不需要到系统的环境变量中设置。

3.2.1.3 Apollo工程和QT工程的接口定义

发送端	消息	接收端
QT工程	/apollo/trucksim/pose	Apollo工程
QT工程	/apollo/sensor/gnss/odometry	Apollo工程
QT工程	/apollo/sensor/gnss/corrected_imu	Apollo工程
QT工程	/apollo/canbus/chassis	Apollo工程
Apollo工程	/apollo/control	QT工程

/apollo/trucksim/pose

{"op":"publish",
"topic":"/apollo/tucksim/pose",
"msg":
 {
  "header":
 {"timestamp_sec":1572253610.76292,
 "sequence_num":77},
 "trucksimpose":
 {"XCG_TM":30.9964522249, // 单位：m
 "YCG_TM":0.657853758823, // 单位：m
 "ZCG_TM":1.00644079555, // 单位：m
 "YAW":-0.015505948987, // 单位：rad
 "VX":7.81497285565, // 单位：m/s
 "STEER_SW":2.84450684087, // 单位：rad
 "AV_Y":0.133153549217, // 单位：rad/s
 "GEARSTAT":5.0, // 无单位
 "XCG_TM2":22.5890979801, // 单位：m
 "YCG_TM2":-0.471483304991, // 单位：m
 "ZCG_TM2":2.08466406388, // 单位：m
 "YAW_2":-0.0253130178796, // 单位：rad
 "VY":0.326368169782, // 单位：m/s
 "DISTANCE":31.0034324244, // 单位：m
 "DELTA_YAW":-0.015505948987, // 单位：rad
 "DISTANCE_2":22.5940178822, // 单位：m
 "DELTA_YAW_2":-0.0253130178796 // 单位：rad/s
 }
 }
}

/apollo/sensor/gnss/odometry

header {
timestamp_sec: 1574731650.49
sequence_num: 4495
}
localization {
position {
 x: 1137.99194336
 y: 346.311798096
 z: 0.966913342476
}
orientation {
 qx: 0.000129811684019
 qy: 0.00239154277369
 qz: 0.998526215553
 qw: 0.0542182847857
}
linear_velocity {
 x: 3.81316094717e-05
 y: 0.000499870278873
 z: -0.00303626712412
}
heading: -186.216018677
}
/apollo/sensor/gnss/corrected_imu
header {
timestamp_sec: 1574731724.4
}
imu {
linear_acceleration {
 x: 3.08257440338e-05
 y: 2.36972823586e-06
 z: -7.97993634478e-05
}
angular_velocity {
 x: 2.78042625723e-06
 y: -4.34010331674e-07
 z: 6.27197778158e-08
}
heading: -186.190139771
euler_angles {
 x: -6.28290615923
 y: -5.50735356707e-08
 z: -3.24963094857
}
}

/apollo/canbus/chassis

engine_started: true
engine_rpm: 2000.0
*speed_mps: 1.17951503853e-06
odometer_m: 0.0
fuel_range_m: 0
*throttle_percentage: 0.0
*brake_percentage: 100.0
*steering_percentage: 0.0
parking_brake: false
high_beam_signal: false
low_beam_signal: false
left_turn_signal: false
right_turn_signal: false
wiper: false
*driving_mode: COMPLETE_AUTO_DRIVE
*gear_location: GEAR_DRIVE
header {
timestamp_sec: 1574731781.0
module_name: "chassis"
sequence_num: 0
}
chassis_gps {
latitude: 0.0031245521439
longitude: -4.47857173552
gps_valid: true
year: 2019
month: 11
day: 26
hours: 9
minutes: 29
seconds: 41
compass_direction: 180.0
pdop: 0.1
is_gps_fault: false
is_inferred: false
altitude: 0.0
heading: 186.189834595
hdop: 0.1
vdop: 0.1
quality: FIX_3D
num_satellites: 15
gps_speed: 1.1795152659e-06
}
state_info {
truck_control_mode: AUTO
}

4. 使用心得

1.TruckSim模型离散时间补偿设置为0.001s，即模型更新频率为1000hz，选择每个步长更新两次的积分方法（如：AM-2, RK-2等）。
注释：1）真车是一个高阶非线性连续系统，TruckSim通过固定时间步长离散系统来模拟真车，当模型步长选择较大时（如之前设置的0.01s），模型较不准；TruckSim模型是由悬架系统-动力系统-转向系统-制动系统-轮胎模型-空气动力学等系统构成的复杂系统，当其中一个或多个系统因为时间步长太大而很不准时，就会出现车抖动比较明显的现象。
2）模型更新频率设为1000hz是TruckSim官网推荐的，经验证，这个频率能解决车抖动问题。
4.接口线程加载频率用定时器控制，加载调用simfile.sim，license和DLL的频率设置为1000hz，与Trucksim模型离散步长一致。
注释：受3中积分方法决定，当选择每个步长更新两次的积分方法，VS_EXT_EQ_IN和VS_EXT_EQ_OUT更新频率是加载频率的2倍，为2000hz。