一、开发思路
1.基于横向控制算法获取目标方向盘转角,基于纵向控制算法获取目标速度
2.创建二维数组,存放目标扭矩,行为目标速度,列为目标转角
3.计算获取的目标速度所在行的索引号x_index,目标转角所在列的索引号y_index
4.通过线性插值对x,y,z数组进行插值(一维和二维插值)
5.查表获取扭矩Torque[x_index][y_index]
二、基于C++的代码开发
测试数据:
#ifndef STRUCT_
#define STRUCT_
struct Vehicle_Parameters {
float Speed[5] = {1.1, 2.2, 3.6, 7.2, 10.5}; // X
float newSpeed[9] = {0.f};
float Steer[5] = {1, 2, 3, 4, 5}; // Y
float newSteer[9] = {0.f};
float Torque[5][5] = {
{0.2, 0.3, 0.4, 0.6, 0.8},
{0.3, 0.5, 0.6, 0.7, 0.8},
{0.4, 0.6, 0.7, 0.8, 0.9},
{1.1, 1.2, 1.3, 1.4, 1.5},
{1.2, 1.3, 1.5, 1.6, 1.9}}; // Z
float newTorque[9][9] = {
{0.f}};
float v_target_value;
float s_target_value;
int v_length;
int s_length;
int v_newLength;
int s_newLength;
int v_index;
int s_index;
};
#endif
接口 声明:
lookup_table.hpp
#ifndef LOOKUP_TABLE_
#define LOOKUP_TABLE_
#include "struct.hpp"
#include <cmath>
#include <iostream>
class Lookup_Table {
private:
float v_target;
float s_target;
// int newWidth;
// int newHeight;
// float newTorque[][];
Vehicle_Parameters vehicle_parameters_;
public:
Lookup_Table();
~Lookup_Table();
int CalculateIndex_v();
int CalculateIndex_s();
// Vehicle_Parameters vehicle_parameters_;
int searchIndex(float *nums, int length, float target);
void SetSpeedInfo(float v);
void SetSteerINfo(float s);
float ReadSpeedInfo();
float ReadSteerInfo();
float CalculateTorque(int x, int y);
void InterSpeed(Vehicle_Parameters &vehicle_parameters_);
void InterTarget(Vehicle_Parameters &vehicle_parameters_);
void InterTorque(Vehicle_Parameters &vehicle_parameters_);
};
#endif
接口实现:
lookup_table.cpp
#include "lookup_table.hpp"
Lookup_Table::Lookup_Table() {
// vehicle_parameters_.v_length =
// sizeof(vehicle_parameters_.Speed) /
// sizeof(vehicle_parameters_.Speed[0]);
// std::cout << "vehicle_parameters_.v_length = " <<
// vehicle_parameters_.v_length
// << std::endl;
// vehicle_parameters_.s_length =
// sizeof(vehicle_parameters_.Steer) /
//