生成多段低阶贝塞尔平滑曲线，考虑对静态障碍物的避让

Question

已有：

1. 栅格地图，包括了障碍物信息和可通行区域信息，障碍物栅格值为255，障碍物膨胀范围栅格值为100，可通行区域栅格值为0，地图格式为cv::Mat,单通道UINT8
2. 输入路径，存放在std::vectorcv::Point2f中
   需求：对所输入的路径进行分段的低阶贝塞尔曲线平滑，可以是3阶或者4阶，保证每一段的曲线G1连续和C1连续，所平滑的曲线不能穿过地图上的障碍物栅格和障碍物膨胀栅格
   要求：给出以下代码示例
3. 输入上述已知路径，生成多段保持G1和C1连续的低阶贝塞尔平滑曲线
4. 对已生成的贝塞尔曲线进行调整，使其每一个栅格点都不在障碍物或障碍物膨胀范围内，仍然要保持其每一段曲线之间的连续性

Answer 1

参考GPT和自己的思路，下面是基于Python的代码示例，实现输入路径的分段低阶贝塞尔曲线平滑，并考虑静态障碍物的避让：

import cv2
import numpy as np
from scipy.interpolate import CubicHermiteSpline

# 根据输入路径，生成低阶贝塞尔曲线并进行平滑
def generateBezierSmoothedPath(path, degree):
    smoothed_path = []
    for i in range(len(path)-degree):
        x = np.array([path[i].x, path[i+1].x, path[i+2].x, path[i+3].x])
        y = np.array([path[i].y, path[i+1].y, path[i+2].y, path[i+3].y])
        # 使用Cubic Hermite Spline进行平滑
        spl = CubicHermiteSpline(x, y, [0.0, 1.0], bc_type='natural')
        # 将每段曲线上的点加入到平滑路径列表中
        for j in range(10):
             xp = x[0] + (x[-1] - x[0]) / 10 * j
             yp = spl(xp)
             smoothed_path.append(cv2.Point2f(xp, yp))
    return smoothed_path

# 判断一个点是否位于障碍物或障碍物膨胀范围内
def isObstacle(x, y, map):
    if x<0 or x>=map.shape[1] or y<0 or y>=map.shape[0]:
        # 越界点都视作障碍物
        return True
    if map[y, x] == 255:
        # 障碍物栅格
        return True
    if map[y, x] == 100:
        # 障碍物膨胀栅格
        return True
    return False

# 对已生成的贝塞尔曲线进行调整，使其不穿过障碍物或障碍物膨胀范围
def adjustPathToFitMap(smoothed_path, map):
    adjusted_path = [smoothed_path[0]] # 起点直接加入调整后的路径列表中
    for i in range(len(smoothed_path) - 2):
        start_pt = smoothed_path[i]
        end_pt = smoothed_path[i+1]
        next_pt = smoothed_path[i+2]
        dist = cv2.norm(end_pt - start_pt) # 获取此段曲线的长度
        num_pts = max(2, int(dist // 5)) # 将此段曲线均分为至少两个点，每个点之间留下5个像素的间隔
        x = np.linspace(start_pt.x, end_pt.x, num_pts)
        y = np.linspace(start_pt.y, end_pt.y, num_pts)
        for j in range(1, num_pts):
            # 对每一个均分点进行判断，如果位于障碍物或障碍物膨胀范围内，则将其替换为在范围外的最邻近点
            if isObstacle(int(x[j]), int(y[j]), map):
                k = j-1
                while k>=0:
                    if not isObstacle(int(x[k]), int(y[k]), map):
                        adjusted_path.append(cv2.Point2f(x[k], y[k]))
                        break
                    k -= 1
                if k < 0:
                    k = j+1
                    while k < num_pts:
                        if not isObstacle(int(x[k]), int(y[k]), map):
                            adjusted_path.append(cv2.Point2f(x[k], y[k]))
                            break
                        k += 1
            else:
                adjusted_path.append(cv2.Point2f(x[j], y[j]))
    adjusted_path.append(smoothed_path[-1]) # 终点直接加入调整后的路径列表中
    return adjusted_path


# 示例调用代码
degree = 3
map = cv2.imread("map.png", cv2.IMREAD_GRAYSCALE) # 读取地图
path = [cv2.Point2f(100, 100), cv2.Point2f(200, 150), cv2.Point2f(150, 300), cv2.Point2f(50, 250), cv2.Point2f(100, 100)]
smoothed_path = generateBezierSmoothedPath(path, degree)
adjusted_path = adjustPathToFitMap(smoothed_path, map)

其中，函数generateBezierSmoothedPath()用于对输入路径进行分段低阶贝塞尔曲线平滑，函数adjustPathToFitMap()用于对已生成的曲线进行调整，以避免穿过障碍物或障碍物膨胀范围，保持每一段曲线的连续性。 示isObstacle()根据地图中的栅格值判断一个点是否位于障碍物或障碍物膨胀范围内。在示例调用代码中，读取了一张名为map.png的地图，路径为由五个点构成的列表path，平滑后的路径保存在smoothed_path中，调整后的路径保存在adjusted_path中。