search

LiJiangnanBit / path_optimizer_ilqr

Path planning for autonomous vehicles using constrained iLQR.
102 stars 41 forks source link

如果有多段轨迹是否需要进行分段优化？ #9

Open Castle-Boy opened 2 months ago

Castle-Boy commented 2 months ago

我没有ros环境，移植了您的代码，使用混合A*的搜索结果为其提供了初始点、终止点击reference point, 可是平滑后的结果很奇怪，只有曲率能满足要求，x,y,theta都与目标值相差很大。

LiJiangnanBit commented 2 months ago

可以贴一下具体的结果看看哈

Castle-Boy commented 2 months ago

raw_path.csv smoothed_trajectory0429.csv

Castle-Boy commented 2 months ago

raw path是混合A*规划的轨迹 分为前后两段， smoothed_trajectory是移植的您的算法，我修改了您的demo文件，直接给出了起始点和终止点，然后从csv文件中读取中间的参考点，求解器返回状态未2也就也是coveraged，修改后demo代码如下：

include

include

include

include

include

include

include <sys/stat.h>

// #include <nav_msgs/OccupancyGrid.h> // #include "modules/planning/open_space/pb_rosmsg-main/common_msgs/nav_msgs/OccupancyGrid.pb.h"

// #include <geometry_msgs/PointStamped.h>

include "modules/planning/open_space/pb_rosmsg-main/common_msgs/geometry_msgs/PointStamped.pb.h"

// #include <geometry_msgs/PoseWithCovarianceStamped.h>

include "modules/planning/open_space/pb_rosmsg-main/common_msgs/geometry_msgs/PoseWithCovariance.pb.h"

// #include <tf/transform_datatypes.h> // #include <tf/transform_broadcaster.h> // #include <tf/tf.h> // #include <ros_viz_tools/ros_viz_tools.h>

include <grid_map_core/grid_map_core.hpp>

// #include <grid_map_cv/grid_map_cv.hpp> // #include <grid_map_ros/grid_map_ros.hpp>

include "glog/logging.h"

include "eigen3/Eigen/Dense"

include "opencv2/core/core.hpp"

include "opencv2/core/eigen.hpp"

include "opencv2/opencv.hpp"

include "modules/planning/open_space/path_optimizer_ilqr/src/test/eigen2cv.h"

include "modules/planning/open_space/path_optimizer_ilqr/src/test/reference_line_processor.h"

include "modules/planning/open_space/path_optimizer_ilqr/src/path/data_structure.h"

include "modules/planning/open_space/path_optimizer_ilqr/src/path/path_problem_manager.h"

include "modules/planning/open_space/path_optimizer_ilqr/src/solver/solver.h"

include "modules/planning/open_space/path_optimizer_ilqr/src/path/gflags.h"

include "modules/planning/open_space/path_optimizer_ilqr/src/path/tool.h"

using namespace PathPlanning; using namespace grid_map; // 定义起点终点数据类型 PathPlanning::PathPoint start_state, end_state; // 定义参考点数据类型 std::vector reference_points;

// 从CSV文件读取数据并存储到vector中 std::vector readPathPointsFromCSV() {

const char *filename = "/apollo/modules/planning/open_space/path_optimizer_ilqr/src/test/raw_path.csv";
std::ifstream file(filename);
std::vector<PathPoint> pathPoints;
if (!file.is_open()) {
    LOG(ERROR)<< "Failed to open file"<< std::endl;
}
std::string line;
while (std::getline(file, line)) {
    std::istringstream iss(line);
    std::string token;
    PathPoint point;

    // 逐行读取数据，假设每行数据用逗号分隔
    if (std::getline(iss, token, ',')) {
        point.x = std::stod(token);
    }
    if (std::getline(iss, token, ',')) {
        point.y = std::stod(token);
    }
    if (std::getline(iss, token, ',')) {
        point.theta = std::stod(token);
    }

    LOG(INFO)<<point.x<<","<<point.y<<","<<point.theta<<std::endl;

    // 将读取到的数据存入vector
    pathPoints.push_back(point);
}
// LOG(INFO)<<"point size: "<< pathPoints.size();

file.close();
return pathPoints;

}

static bool initializeFromImage(const cv::Mat& image, const double resolution, grid_map::GridMap& gridMap, const grid_map::Position& position) { const double lengthX = resolution image.rows; const double lengthY = resolution image.cols; Length length(lengthX, lengthY); gridMap.setGeometry(length, resolution, position); return true; }

template<typename Type, int NChannels>

static bool addLayerFromImage(const cv::Mat& image, const std::string& layer, grid_map::GridMap& gridMap, const float lowerValue = 0.0, const float upperValue = 1.0, const double alphaThreshold = 0.5) { if (gridMap.getSize()(0) != image.rows || gridMap.getSize()(1) != image.cols) { std::cerr << "Image size does not correspond to grid map size!" << std::endl; return false; }

bool isColor = false; if (image.channels() >= 3) isColor = true; bool hasAlpha = false; if (image.channels() >= 4) hasAlpha = true;

cv::Mat imageMono; if (isColor && !hasAlpha) { cv::cvtColor(image, imageMono, CV_BGR2GRAY); } else if (isColor && hasAlpha) { cv::cvtColor(image, imageMono, CV_BGRA2GRAY); } else if (!isColor && !hasAlpha){ imageMono = image; } else { std::cerr << "Something went wrong when adding grid map layer form image!" << std::endl; return false; }

const float mapValueDifference = upperValue - lowerValue;

float maxImageValue; if (std::issame<Type, float>::value || std::issame<Type, double>::value) { maxImageValue = 1.0; } else if (std::issame<Type, unsigned short>::value || std::issame<Type, unsigned char>::value) { maxImageValue = (float)std::numericlimits<Type>::max(); } else { std::cerr << "This image type is not supported." << std::endl; return false; }

const Type alphaTreshold = (Type)(alphaThreshold * maxImageValue);

gridMap.add(layer); grid_map::Matrix& data = gridMap[layer];

for (grid_map::GridMapIterator iterator(gridMap); !iterator.isPastEnd(); ++iterator) { const grid_map::Index gridMapIndex = *iterator; const grid_map::Index imageIndex = iterator.getUnwrappedIndex();

// Check for alpha layer.
if (hasAlpha) {
const Type_ alpha = image.at<cv::Vec<Type_, NChannels_>>(imageIndex(0), imageIndex(1))[NChannels_ - 1];                 
if (alpha < alphaTreshold) continue;
}

// Compute value.
const Type_ imageValue = imageMono.at<Type_>(imageIndex(0), imageIndex(1));
const float mapValue = lowerValue + mapValueDifference * ((float) imageValue / maxImageValue);
data(gridMapIndex(0), gridMapIndex(1)) = mapValue;

}

return true;
}

int main() {

google::InitGoogleLogging("frenet_ilqr_test_demo");
FLAGS_colorlogtostderr = true;
FLAGS_stderrthreshold = google::INFO;
FLAGS_log_dir = "/apollo/modules/planning/open_space/path_optimizer_ilqr/log";
FLAGS_logbufsecs = 0;
FLAGS_max_log_size = 100;
FLAGS_stop_logging_if_full_disk = true;

// 初始化地图 生成占据网格图
std::string image_dir = "/apollo/modules/planning/open_space/path_optimizer_ilqr";
std::string image_file = "gridmap.png";
image_dir.append("/" + image_file);
cv::Mat img_src = cv::imread(image_dir, CV_8UC1);
double resolution = 0.2;  // in meter
grid_map::GridMap grid_map(std::vector<std::string>{"obstacle", "distance"});
initializeFromImage(
    img_src, resolution, grid_map, grid_map::Position::Zero());
// Add obstacle layer.
unsigned char OCCUPY = 0;
unsigned char FREE = 255;
addLayerFromImage<unsigned char, 1>(
    img_src, "obstacle", grid_map, OCCUPY, FREE, 0.5);
// Update distance layer.
Eigen::Matrix<unsigned char, Eigen::Dynamic, Eigen::Dynamic> binary =
    grid_map.get("obstacle").cast<unsigned char>();
cv::distanceTransform(eigen2cv(binary), eigen2cv(grid_map.get("distance")),
                      CV_DIST_L2, CV_DIST_MASK_PRECISE);
grid_map.get("distance") *= resolution;
grid_map.setFrameId("map");

// 获取起始点和终止点位置信息及 参考轨迹点信息
start_state.x = 3.81;
start_state.y = 8.0;
start_state.theta = 3.14;
LOG(ERROR)<< "起点信息 : x="<< start_state.x <<" y="<<start_state.y<<" theta="<<start_state.theta;

end_state.x = 1.25;
end_state.y = 1.345;
end_state.theta = 1.5708;
LOG(ERROR)<< "终点信息 : x="<< end_state.x <<" y="<<end_state.y<<" theta="<<end_state.theta;  
//TODO: 添加参考点信息 LZQ
reference_points = readPathPointsFromCSV();
// LOG(ERROR)<<reference_points.size()<<std::endl;
// LOG(ERROR)<<reference_points.at(2).x<<","<<reference_points.at(2).y<<","<<reference_points.at(2).theta;

// 开始路径优化
// if (reference_rcv && start_state_rcv && end_state_rcv) {    
Test::Map map(grid_map);
Test::ReferenceLineProcessor reference_line_processor(reference_points, map, start_state);
std::shared_ptr<PathPlanning::ReferenceLine> ref_line_ptr = std::make_shared<PathPlanning::ReferenceLine>();
std::shared_ptr<PathPlanning::FreeSpace> free_space_ptr = std::make_shared<PathPlanning::FreeSpace>();
reference_line_processor.solve(ref_line_ptr, free_space_ptr);

// solve
PathPlanning::PathProblemManager path_problem_manager;
path_problem_manager.formulate_path_problem(*free_space_ptr, *ref_line_ptr, start_state, end_state);
Solver::ILQRSolver<PathPlanning::N_PATH_STATE, PathPlanning::N_PATH_CONTROL> ilqr_solver(path_problem_manager);
const auto solve_status = ilqr_solver.solve();
std::cout<<"solve_status: " <<static_cast<int>(solve_status)<<std::endl;

const auto& opt_path_raw = ilqr_solver.final_trajectory();
const auto result = PathProblemManager::transform_to_path_points(*ref_line_ptr, opt_path_raw);
std::ofstream outfile;
outfile.open("smoothed_trajectory0429.csv",std::ios::out);
outfile <<"x,y,theta,theta_diff,kappa,dkappa"<<std::endl;
for (size_t i = 0; i != result.size(); ++i) {
    const auto path_point = result.at(i);
    outfile<<path_point.x<<","<<
            path_point.y<<","<<
            path_point.theta<<","<<
            path_point.theta_diff<<","<<
            path_point.kappa<<","<<
            path_point.dkappa<<std::endl;
}
outfile.close();

// }
return 1;

}

LiJiangnanBit commented 2 months ago

有没有可视化的结果呢，这么看有点费劲

Castle-Boy commented 2 months ago

raw_path.csv smoothed_trajectory1.csv IMG_3116 IMG_3115

Castle-Boy commented 2 months ago

raw_path2.csv smoothed_trajectory2.csv IMG_0001

Castle-Boy commented 2 months ago

我分别试了两组混合A数据，起始点都是 x=3.81 y= 8.0 theta =3.14， 第一组目标终点为 x=1.25 y= 1.345 theta =1.57， 第二组目标终点为 x=10.25 y= 1.345 theta =1.57。图中蓝色的点是混合A规划的初始路径，红色的是平滑后的路径，第一组平滑后的结果有很大突变，从x=8.7之后就不连续了，第二组好很多，但是theta和y还是有些偏差。

LiJiangnanBit commented 2 months ago

我分别试了两组混合A_数据，起始点都是 x=3.81 y= 8.0 theta =3.14， 第一组目标终点为 x=1.25 y= 1.345 theta =1.57， 第二组目标终点为 x=10.25 y= 1.345 theta =1.57。图中蓝色的点是混合A_规划的初始路径，红色的是平滑后的路径，第一组平滑后的结果有很大突变，从x=8.7之后就不连续了，第二组好很多，但是theta和y还是有些偏差。

是的，这种带折返的分段参考线是得分开优化的，而且这个代码因为是主要做一个行车场景的规划，对终点的位置和角度约束没有严格的约束，要是想做泊车，分段优化的话，需要改一下，对终点加很强的cost