【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 file
    auto 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 initialize
    HMMWV_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 terrain
    RigidTerrain 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.0
    patch->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->ChPathFollowerDriver
    driver.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: latitute
    utils::ChRunningAverage fwd_acc_driver_filter(filter_window_size);
    utils::ChRunningAverage lat_acc_driver_filter(filter_window_size);

    // ---------------
    // Simulation loop
    // ---------------
    // Driver location in vehicle local frame
    ChVector<> driver_pos = hmmwv.GetChassis()->GetLocalDriverCoordsys().pos;

    // Number of simulation steps between miscellaneous events
    double render_step_size = 1 / fps;
    int render_steps = (int)std::ceil(render_step_size / step_size);

    // Initialize simulation frame counter and simulation time
    int sim_frame = 0;
    int render_frame = 0;

    hmmwv.GetVehicle().EnableRealtime(true);
    while (vis->Run()) {
        // Extract system state
        double 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 simulation
        if (time >= t_end)
            vis->Quit();

        // Driver inputs
        DriverInputs 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 modules
        driver.Advance(step_size);
        terrain.Advance(step_size);
        hmmwv.Advance(step_size);
        vis->Advance(step_size);

        // Increment simulation frame number
        sim_frame++;
    }
    return 0;
}