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

[Castle-Boy]

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

[LiJiangnanBit]

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

[Castle-Boy]

raw_path.csv
smoothed_trajectory0429.csv

[Castle-Boy]

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

#include
#include
#include
#include <unistd.h>
#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::vectorPathPlanning::PathPoint 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::is_same<Type_, float>::value || std::is_same<Type_, double>::value) {
maxImageValue = 1.0;
} else if (std::is_same<Type_, unsigned short>::value || std::is_same<Type_, unsigned char>::value) {
maxImageValue = (float)std::numeric_limits<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]

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

[Castle-Boy]

raw_path.csv
smoothed_trajectory1.csv

[Castle-Boy]

raw_path2.csv
smoothed_trajectory2.csv

[Castle-Boy]

我分别试了两组混合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]

我分别试了两组混合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