贝塞尔曲线平滑后的轨迹怎么规避静态障碍物？

Question

如题，现考虑使用贝塞尔曲线对原路径进行平滑，但是现在不知道怎么考虑对静态障碍物的规避。需要让平滑后的路径根据已知的障碍物进行一定的调整，但仍保持整体的曲率连续。
所使用的是栅格地图，C++开发。

Answer 1

对于栅格地图中的贝塞尔曲线平滑路径，需要进行静态障碍物的规避，可以考虑以下步骤：

确定障碍物的位置：在栅格地图中，可以通过遍历地图，找到障碍物所在的栅格，记录下来。

在贝塞尔曲线上插入避障点：对于每个障碍物，可以在贝塞尔曲线上插入一个避障点，使得路径绕开障碍物。避障点的位置可以根据贝塞尔曲线的参数方程计算得到。具体来说，可以通过计算每个障碍物与贝塞尔曲线的最短距离，然后将避障点插入到该距离对应的曲线参数位置上。

调整贝塞尔曲线：在插入避障点后，需要重新计算贝塞尔曲线的控制点，使得路径保持曲率连续。具体来说，可以采用贝塞尔曲线的拟合方法，通过已知的路径点和避障点，计算出新的控制点。

根据调整后的贝塞尔曲线生成路径：根据调整后的贝塞尔曲线，可以生成新的平滑路径，用于避开障碍物。
需要注意的是，避障点的位置和数量需要根据具体情况进行调整，以保证路径能够避开障碍物并且不会离开可行区域。同时，对于动态障碍物，还需要实时更新避障点的位置，以保证路径的有效性。
以下是一个简单的C++代码示例，用于在贝塞尔曲线上插入避障点：

// 假设已经得到了贝塞尔曲线的参数方程
// p0, p1, p2, p3 分别表示控制点和起点终点
// obs_x, obs_y 分别表示障碍物的坐标
// threshold 表示插入避障点的阈值
void insert_obstacle_point(double p0, double p1, double p2, double p3, 
                           double obs_x, double obs_y, double threshold,
                           std::vector<double>& new_params) {
    double min_dist = std::numeric_limits<double>::max();
    double min_t = 0.0;
    for (double t = 0.0; t <= 1.0; t += 0.01) {
        double x = pow(1 - t, 3) * p0 + 3 * pow(1 - t, 2) * t * p1 
                 + 3 * (1 - t) * pow(t, 2) * p2 + pow(t, 3) * p3;
        double y = pow(1 - t, 3) * p0 + 3 * pow(1 - t, 2) * t * p1 
                 + 3 * (1 - t) * pow(t, 2) * p2 + pow(t, 3) * p3;
        double dist = std::sqrt(std::pow(x - obs_x, 2) + std::pow(y - obs_y, 2));
        if (dist < min_dist) {
            min_dist = dist;
            min_t = t;
        }
    }
    if (min_dist < threshold) {
        double new_t = std::max(0.0, std::min(1.0, min_t + (threshold - min_dist) / 10.0));
        new_params.push_back(p0);
        new_params.push_back(p1);
        new_params.push_back(p2);
        new_params.push_back(p3);
        new_params.push_back(new_t);
    }
    return;
}

其中，new_params 表示新的贝塞尔曲线参数，threshold 表示插入避障点的阈值，如果障碍物与曲线距离小于该值，则插入避障点。