自动驾驶车辆控制算法之如何利用查表算法--获取目标车速和转角下的控制扭矩（C++开发）

自动驾驶小卡

已于 2023-03-02 15:06:30 修改

阅读量774

点赞数

分类专栏：控制文章标签：自动驾驶人工智能机器学习 c++ 算法

于 2022-10-17 11:49:32 首次发布

本文链接：https://blog.csdn.net/zxy_ZXY123/article/details/127359980

版权

一、开发思路

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) /
  //