【Chrono Engine学习总结】4-vehicle-4.2-车辆轨迹跟踪

由于Chrono的官方教程在一些细节方面解释的并不清楚，自己做了一些尝试，做学习总结。

0、Vehicle的driver

driver在上一篇总结中有过介绍，【Chrono Engine学习总结】4-vehicle-4.1-vehicle的基本概念，这里进一步介绍。

对于一个具体的driver系统，控制的状态量即：油门、刹车、转向。这里重点介绍轨迹跟踪用到的driver：ChPathFollowerDriver

driver本质上就是驾驶这个车的东西，本文中的驾驶员、控制器等都可以是指这个driver。当然控制器严格来讲应该是controller。个人认为，上层一些的东西，就叫这个driver，包括转向、速度等多方面的handle，而具体到某个变量例如速度，就叫做controller。

1、闭环驾驶控制器 ChClosedLoopDriver

https://api.projectchrono.org/classchrono_1_1vehicle_1_1_ch_closed_loop_driver.html

顾名思义，这是“闭环”驾驶系统，因此肯定包括控制环节。具体的，一个ChClosedLoopDriver包括：转向控制器ChSteeringController和速度控制器ChSpeedController。
在初始化一个ChClosedLoopDriver时，会自动生成上述两个控制器。

速度控制器ChSpeedController
https://api.projectchrono.org/classchrono_1_1vehicle_1_1_ch_speed_controller.html
速度控制器包括：通过代码设定恒定速度、或通过JSON文件读取速度配置，设置PID参数等。
速度控制器只有一个，即不管是采用何种driver，都是同一个速度控制器。而下面的转向控制器稍微会复杂一些。

转向控制器ChSteeringController
https://api.projectchrono.org/classchrono_1_1vehicle_1_1_ch_steering_controller.html
在这里插入图片描述转向控制器是一个基类，不同路径跟踪driver会有不同的转向控制器，例如接下来要重点介绍的ChPathFollowerDriver采用的是ChPathSteeringController控制器。

对于转向控制器，理论上也存在PID参数等，但在这个ChSteeringController基类中没有实现，而是在后面继承的类中进行的实现。

有一个新概念，叫“哨兵点”，和“目标点”：

Sentinel（哨兵）: 通常用来监视或标记某个特定区域、路径或条件的存在。在仿真或游戏环境中，一个哨兵点可能用来指示玩家或系统需要特别注意的地方，或者作为触发某些事件的标记。
通过SetLookAheadDistance函数，给出在仿真时“哨兵点”的位置，即我关注车辆前方多远的具体，在这个点范围内的变化对仿真产生影响。这个距离是以底盘为坐标系的，车辆“前方”。
Target（目标）: 通常指一个要达到或影响的点。在仿真中，这可能是需要导航到的位置，或者是需要与之交互的对象。
进一步解释：在仿真时，vehicle的真实位置，是target真正要到的位置，而为了仿真系统高效的运行，设置一个“安全距离/提前注意”的位置，作为哨兵点，在哨兵点外的轨迹暂时不考虑。类似于碰撞检测，在短时间内不会发生碰撞时，避免复杂的碰撞条件计算。

Specify the look-ahead distance. This defines the location of the “sentinel” point (in front of the vehicle, at the given distance from the chassis reference frame).

2、轨迹跟踪控制器 ChPathFollowerDriver

https://api.projectchrono.org/classchrono_1_1vehicle_1_1_ch_path_follower_driver.html

这个控制器包括：速度控制器ChSpeedController和轨迹转向控制器ChPathSteeringController。
正如1.1 所说，速度控制器都是一样的，这里重点介绍轨迹转向控制器。

ChPathSteeringController
https://api.projectchrono.org/classchrono_1_1vehicle_1_1_ch_path_steering_controller.html

这个控制器继承了基类ChSteeringController，包括上面所说的所有功能。除此之外，包括：

设置PID参数 void SetGains (double Kp, double Ki, double Kd)
计算目标点位置：由于需要跟踪轨迹，因此哨兵点给的位置不一定落在目标轨迹上，因此需要通过计算出真正需要抵达的目标点。这个目标点就通过CalcTargetLocation函数计算哨兵点对应轨迹上的最近点是哪个，用于具体的控制。

轨迹跟踪控制器部分的关键代码如下：

// 控制器创建与设置
ChPathFollowerDriver driver(hmmwv.GetVehicle(), path, "my_path", 5.0);	// 目标速度是5.0m/s，轨迹是path
driver.SetColor(ChColor(0.0f, 0.0f, 0.8f));
driver.GetSteeringController().SetLookAheadDistance(5);	// 设置转向控制器的哨兵点距离
driver.GetSteeringController().SetGains(0.8, 0, 0);      // 设置转向控制器的PID参数，这里P=0.8
driver.GetSpeedController().SetGains(0.4, 0, 0);			// 设置速度控制器的PID
driver.Initialize();// 循环仿真部分。只需要进行子系统的时间同步，然后动力学仿真一步即可。
driver.Synchronize(time);
driver.Advance(step_size);

到这里已经介绍了，轨迹跟踪的控制器是如何工作的。但还没有介绍上面跟踪的轨迹是怎么来。接下来进行介绍。

3、轨迹

轨迹本质上是一个贝塞尔曲线BezierCurve，即：ChBezierCurve。
https://api.projectchrono.org/classchrono_1_1_ch_bezier_curve.html

通过：

auto path = ChBezierCurve::read(vehicle::GetDataFile(path_file), closed_loop);

从文件读取一个轨迹，closed_loop表示，这个轨迹是否是一个闭环，即首尾相连？

3.1 轨迹文件

轨迹文件内包含的就是节点，第一行有两个数字，N和数字3或9，表示共有N个节点点，3或9表示两种不同的节点类型。从第二行开始，就是每个节点的具体坐标。例如，下图是一个200x200米的正方形环形的轨迹。贝塞尔曲线采用3次插值。
在这里插入图片描述

上面的是3列，即需要经过这写节点。如果是9列，则分别是：节点、进入控制点、离开控制点。更多关于样条曲线的内容参考之前我在知乎整理的一篇文章：【学习总结】连续时间SLAM（二）——B样条曲线

3.2 轨迹读取与传递给控制器

// 读取轨迹文件
auto path = ChBezierCurve::read(vehicle::GetDataFile(path_file), closed_loop);
// 将轨迹给轨迹跟踪控制器
ChPathFollowerDriver driver(hmmwv.GetVehicle(), path, "my_path", target_speed);
// ...
// 在仿真循环中
DriverInputs driver_inputs = driver.GetInputs();	// 获取沿轨迹运行时的控制量
hmmwv.Synchronize(time, driver_inputs, terrain);	// 将控制量同步给悍马车系统
hmmwv.Advance(step_size);			// 悍马车仿真一步。

4、完整代码

#include "chrono/utils/ChFilters.h"
#include "chrono_vehicle/ChVehicleModelData.h"
#include "chrono_vehicle/terrain/RigidTerrain.h"
#include "chrono_vehicle/driver/ChPathFollowerDriver.h"
#include "chrono_vehicle/utils/ChVehiclePath.h"
#include "chrono_vehicle/wheeled_vehicle/ChWheeledVehicleVisualSystemIrrlicht.h"
#include "chrono_models/vehicle/hmmwv/HMMWV.h"
#include "chrono_thirdparty/filesystem/path.h"using namespace chrono;
using namespace chrono::geometry;
using namespace chrono::vehicle;
using namespace chrono::vehicle::hmmwv;// 设定一些车辆参数
// Contact method type
ChContactMethod contact_method = ChContactMethod::SMC;
// Type of tire model (RIGID, FIALA, PAC89, PAC02, or TMEASY)
TireModelType tire_model = TireModelType::TMEASY;
// Type of engine model (SHAFTS, SIMPLE, SIMPLE_MAP)
EngineModelType engine_model = EngineModelType::SHAFTS;
// Type of transmission model (SHAFTS, SIMPLE_MAP)
TransmissionModelType transmission_model = TransmissionModelType::SHAFTS;
// Drive type (FWD, RWD, or AWD)
DrivelineTypeWV drive_type = DrivelineTypeWV::RWD;
// Steering type (PITMAN_ARM or PITMAN_ARM_SHAFTS)
// Note: Compliant steering requires higher PID gains.
SteeringTypeWV steering_type = SteeringTypeWV::PITMAN_ARM;
// Visualization type for vehicle parts (PRIMITIVES, MESH, or NONE)
VisualizationType chassis_vis_type = VisualizationType::PRIMITIVES;
VisualizationType suspension_vis_type = VisualizationType::PRIMITIVES;
VisualizationType steering_vis_type = VisualizationType::PRIMITIVES;
VisualizationType wheel_vis_type = VisualizationType::MESH;
VisualizationType tire_vis_type = VisualizationType::MESH;// 车辆跟踪轨迹、速度的设定
// Input file names for the path-follower driver model
std::string path_file("paths/my_path.txt");
// Set to true for a closed-loop path and false for an open-loop
bool closed_loop = false;
// Desired vehicle speed (m/s)
double target_speed = 12;// 地型设置
// Rigid terrain dimensions
double terrainHeight = 0;
double terrainLength = 300.0;  // size in X direction
double terrainWidth = 300.0;   // size in Y direction// 可视化点，跟踪车辆底盘
// Point on chassis tracked by the chase camera
ChVector<> trackPoint(0.0, 0.0, 1.75);// Simulation step size 仿真参数
double step_size = 2e-3;
double tire_step_size = 1e-3;
double t_end = 100;
// Render FPS
double fps = 60;// =============================================================================int main(int argc, char* argv[]) {GetLog() << "Copyright (c) 2017 projectchrono.org\nChrono version: " << CHRONO_VERSION << "\n\n";chrono::SetChronoDataPath("E:/codeGit/chrono/chrono/build/data/");              // change the default data loading path.chrono::vehicle::SetDataPath("E:/codeGit/chrono/chrono/build/data/vehicle/");              // change the vehicle data path// ----------------------// Create the Bezier path// ----------------------// From data fileauto path = ChBezierCurve::read(vehicle::GetDataFile(path_file), closed_loop);// Bezier曲线文件：x行 y列，y=3/9. 3：节点坐标；9：节点，incoming控制点，outcoming控制点// read是一个静态公有成员函数。即使没有创建类的对象，也可以调用这些函数。// 计算初始车头朝向auto point0 = path->getPoint(0);auto point1 = path->getPoint(1);ChVector<> initLoc = point0;initLoc.z() = 0.5;ChQuaternion<> initRot = Q_from_AngZ(std::atan2(point1.y() - point0.y(), point1.x() - point0.x()));// ------------------------------// Create the vehicle and terrain// ------------------------------// Create the HMMWV vehicle, set parameters, and initializeHMMWV_Full hmmwv;hmmwv.SetCollisionSystemType(ChCollisionSystem::Type::BULLET);hmmwv.SetContactMethod(contact_method);hmmwv.SetChassisFixed(false);hmmwv.SetInitPosition(ChCoordsys<>(initLoc, initRot));hmmwv.SetEngineType(engine_model);hmmwv.SetTransmissionType(transmission_model);hmmwv.SetDriveType(drive_type);hmmwv.SetSteeringType(steering_type);hmmwv.SetTireType(tire_model);hmmwv.SetTireStepSize(tire_step_size);hmmwv.Initialize();hmmwv.SetChassisVisualizationType(chassis_vis_type);hmmwv.SetSuspensionVisualizationType(suspension_vis_type);hmmwv.SetSteeringVisualizationType(steering_vis_type);hmmwv.SetWheelVisualizationType(wheel_vis_type);hmmwv.SetTireVisualizationType(tire_vis_type);// 创建地形。// Create the terrainRigidTerrain terrain(hmmwv.GetSystem());ChContactMaterialData minfo;minfo.mu = 0.8f;minfo.cr = 0.01f;minfo.Y = 2e7f;auto patch_mat = minfo.CreateMaterial(contact_method);auto patch = terrain.AddPatch(patch_mat, CSYSNORM, terrainLength, terrainWidth);    // thinkness默认1.0patch->SetColor(ChColor(1, 0.5, 0.5));patch->SetTexture(vehicle::GetDataFile("terrain/textures/tile4.jpg"), 200, 200);terrain.Initialize();// ------------------------// Create the driver system// ------------------------ChPathFollowerDriver driver(hmmwv.GetVehicle(), path, "my_path", target_speed);     // ChDriver->ChClosedLoopDriver->ChPathFollowerDriverdriver.SetColor(ChColor(0.0f, 0.0f, 0.8f));driver.GetSteeringController().SetLookAheadDistance(5);driver.GetSteeringController().SetGains(0.8, 0, 0);     // SetGains (double Kp, double Ki, double Kd)driver.GetSpeedController().SetGains(0.4, 0, 0);driver.Initialize();// 创建车辆可视化内容// ---------------------------------------// Create the vehicle Irrlicht application// ---------------------------------------auto vis = chrono_types::make_shared<ChWheeledVehicleVisualSystemIrrlicht>();vis->SetLogLevel(irr::ELL_NONE);vis->AttachVehicle(&hmmwv.GetVehicle());vis->SetWindowTitle("Steering PID Controller Demo");vis->SetHUDLocation(500, 20);       // HUD: 平视显示系统vis->SetChaseCamera(trackPoint, 6.0, 0.5);vis->Initialize();vis->AddSkyBox();vis->AddLogo();vis->AddLight(ChVector<>(-150, -150, 200), 300, ChColor(0.7f, 0.7f, 0.7f));vis->AddLight(ChVector<>(-150, +150, 200), 300, ChColor(0.7f, 0.7f, 0.7f));vis->AddLight(ChVector<>(+150, -150, 200), 300, ChColor(0.7f, 0.7f, 0.7f));vis->AddLight(ChVector<>(+150, +150, 200), 300, ChColor(0.7f, 0.7f, 0.7f));// 可视化哨兵点和目标点auto ballS = chrono_types::make_shared<ChVisualShapeSphere>(0.1);           // sentinel 哨兵auto ballT = chrono_types::make_shared<ChVisualShapeSphere>(0.1);ballS->SetColor(ChColor(1, 0, 0));ballT->SetColor(ChColor(0, 1, 0));int iballS = vis->AddVisualModel(ballS, ChFrame<>());int iballT = vis->AddVisualModel(ballT, ChFrame<>());// 记录地盘和驾驶员位置的加速度参数？// GC: gravity center，质心/底盘的加速度；  driver：驾驶员位置的加速度utils::ChRunningAverage fwd_acc_GC_filter(filter_window_size);      // fwd: forward,utils::ChRunningAverage lat_acc_GC_filter(filter_window_size);      // lat: latituteutils::ChRunningAverage fwd_acc_driver_filter(filter_window_size);utils::ChRunningAverage lat_acc_driver_filter(filter_window_size);// ---------------// Simulation loop// ---------------// Driver location in vehicle local frameChVector<> driver_pos = hmmwv.GetChassis()->GetLocalDriverCoordsys().pos;// Number of simulation steps between miscellaneous eventsdouble render_step_size = 1 / fps;int render_steps = (int)std::ceil(render_step_size / step_size);// Initialize simulation frame counter and simulation timeint sim_frame = 0;int render_frame = 0;hmmwv.GetVehicle().EnableRealtime(true);while (vis->Run()) {// Extract system statedouble time = hmmwv.GetSystem()->GetChTime();ChVector<> acc_CG = hmmwv.GetVehicle().GetChassisBody()->GetPos_dtdt();         // 获取车体地盘质心的加速度ChVector<> acc_driver = hmmwv.GetVehicle().GetPointAcceleration(driver_pos);    // 获取驾驶员所在位置点的加速度double fwd_acc_CG = fwd_acc_GC_filter.Add(acc_CG.x());          // 这是一步滤波，获取窗口范围内的平均加速度double lat_acc_CG = lat_acc_GC_filter.Add(acc_CG.y());double fwd_acc_driver = fwd_acc_driver_filter.Add(acc_driver.x());double lat_acc_driver = lat_acc_driver_filter.Add(acc_driver.y());// End simulationif (time >= t_end)vis->Quit();// Driver inputsDriverInputs driver_inputs = driver.GetInputs();// Update sentinel and target location markers for the path-follower controller.vis->UpdateVisualModel(iballS, ChFrame<>(driver.GetSteeringController().GetSentinelLocation()));vis->UpdateVisualModel(iballT, ChFrame<>(driver.GetSteeringController().GetTargetLocation()));vis->BeginScene();vis->Render();vis->EndScene();// Update modules (process inputs from other modules)driver.Synchronize(time);terrain.Synchronize(time);hmmwv.Synchronize(time, driver_inputs, terrain);vis->Synchronize(time, driver_inputs);// Advance simulation for one timestep for all modulesdriver.Advance(step_size);terrain.Advance(step_size);hmmwv.Advance(step_size);vis->Advance(step_size);// Increment simulation frame numbersim_frame++;}return 0;
}