[西门子] 机器人路径平滑——贝塞尔曲线

[复制链接]
查看46959 | 回复0 | 2024-3-21 17:09:48 | 显示全部楼层 |阅读模式
“贝塞尔曲线 (/bɛz.i.eɪ/ BEH-zee-ay) 是一种用于计算机图形学和相关领域的参数化曲线。一组离散的控制点通过公式定义平滑、连续的曲线。
本文首先叙述一阶/二阶/三阶贝塞尔曲线的基本原理,然后针对路径不平滑问题给出C++解决方案。”

01



一次Bézier曲线





02



二次Bézier曲线







画出图如下:





03



三次Bézier曲线





画出来:





   

04



C++实现

//以下是一个使用C++实现的贝塞尔曲线平滑路径曲线的示例代码:#include<iostream>#include<vector>#include<fstream>#include<iomanip>#include<string>

structPoint {float x;float y;};
Point calculateBezierPoint(float t, const Point& p0, const Point& p1, const Point& p2, const Point& p3){    Point point;float u = 1 - t;float tt = t * t;float uu = u * u;float uuu = uu * u;float ttt = tt * t;
    point.x = uuu * p0.x + 3 * uu * t * p1.x + 3 * u * tt * p2.x + ttt * p3.x;    point.y = uuu * p0.y + 3 * uu * t * p1.y + 3 * u * tt * p2.y + ttt * p3.y;
return point;}
std::vector<Point> smoothPath(conststd::vector<Point>& path, int numSegments){std::vector<Point> smoothedPath;    smoothedPath.reserve((path.size() - 2) * numSegments + 2);
for (int i = 0; i < path.size() - 3; i++) {const Point& p0 = path;const Point& p1 = path[i + 1];const Point& p2 = path[i + 2];const Point& p3 = path[i + 3];
for (int j = 0; j < numSegments; j++) {float t = static_cast<float>(j) / numSegments;            Point point = calculateBezierPoint(t, p0, p1, p2, p3);            smoothedPath.push_back(point);        }        i = i + 3;    }
    smoothedPath.push_back(path[path.size() - 2]);    smoothedPath.push_back(path[path.size() - 1]);
return smoothedPath;}
intmain(){double x[69] = {-3.7961,-3.7461,-3.6961,-3.6461,-3.5961,-3.5461,-3.4961,-3.4461,-3.3961,-3.3461,-3.2961,-3.2461,-3.1961,-3.1461,-3.0961,-3.0461,-2.9961,-2.9461,-2.8961,-2.8461,-2.7961,-2.7461,-2.6961,-2.6461,-2.5961,-2.5461,-2.4961,-2.4461,-2.3961,-2.3461,-2.2961,-2.2461,-2.1961,-2.1461,-2.0961,-2.0461,-1.9961,-1.9461,-1.8961,-1.8461,-1.7961,-1.7461,-1.6961,-1.6461,-1.5961,-1.5461,-1.4961,-1.4461,-1.3961,-1.3461,-1.2961,-1.2461,-1.1961,-1.1461,-1.0961,-1.0461,-0.9961,-0.9461,-0.8961,-0.8461,-0.7961,-0.7461,-0.6961,-0.6461,-0.5961,-0.5461,-0.4961,-0.4961,-0.4961}; double y[69]= {-0.5775,-0.5775,-0.5775,-0.5775,-0.5775,-0.5775,-0.5775,-0.5775,-0.5775,-0.5775,-0.5775,-0.6275,-0.6775,-0.7275,-0.7775,-0.8275,-0.8775,-0.9275,-0.9775,-1.0275,-1.0775,-1.1275,-1.1775,-1.2275,-1.2275,-1.2775,-1.2775,-1.2775,-1.3275,-1.3275,-1.3275,-1.3275,-1.3275,-1.3275,-1.3275,-1.3275,-1.3275,-1.3275,-1.3275,-1.3275,-1.3275,-1.3275,-1.3275,-1.3275,-1.2775,-1.2775,-1.2775,-1.2275,-1.2275,-1.2275,-1.2275,-1.2275,-1.1775,-1.1275,-1.0775,-1.0275,-0.9775,-0.9275,-0.8775,-0.8275,-0.8275,-0.7775,-0.7775,-0.7275,-0.6775,-0.6275,-0.5775,-0.5775,-0.5775};   
std::vector<Point> path;// 添加30个路径点的x和y坐标for(int i = 0;i < 69;i++){    path.push_back({ x, y });}//path.push_back({ 1, 2 });// path.push_back({ 3, 5 });// 添加更多路径点...// path.push_back({ 10, 10 });
int numSegments = 10; // 控制平滑路径的细分数
std::vector<Point> smoothedPath = smoothPath(path, numSegments);
// 输出平滑路径的坐标for (const Point& point : smoothedPath) {std::cout << point.x << "  " << point.y << std::endl;//std::ofstream file;//         file.open("/home/juchunyu/20231013/240218/log.txt",std::ios::app);//        if(file.is_open()){//                file<<   point.x << " " << point.y << std::endl;//        }//        file.close();    }

return0;}

以上代码实现的贝塞尔曲线平滑路径曲线的原理如下:

定义了一个Point结构体,包含了一个点的x和y坐标。

实现了calculateBezierPoint函数,用于计算贝塞尔曲线上的某个点的坐标。该函数使用了三阶贝塞尔曲线的公式,根据参数t(范围为0到1)、起始点p0、控制点p1和终止点p2 p3 计算出贝塞尔曲线上的相应点的坐标。

实现了smoothPath函数,用于平滑给定的路径曲线。该函数通过遍历路径上的相邻三个点,将每两个点之间的线段使用贝塞尔曲线进行平滑。在每个线段上,等间隔地取若干个t值,然后通过调用calculateBezierPoint函数计算出每个t值对应的贝塞尔曲线上的点,并将这些点存入一个新的向量中。

在main函数中,定义了一个包含30个路径点的path向量。你可以自行修改和添加路径点的坐标。

调用smoothPath函数,将给定的路径曲线进行平滑处理。numSegments参数用于控制每两个路径点之间贝塞尔曲线的细分数,即将每段线段平滑成多少个小曲线段。

输出smoothedPath向量中的路径点,即平滑后的路径曲线的坐标。

通过以上步骤,代码实现了使用贝塞尔曲线对给定的路径曲线进行平滑处理,得到了一条平滑的路径曲线。

05



效果



蓝色代表原始路径,红色代表平滑后路径。

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有账号?注册哦

x
您需要登录后才可以回帖 登录 | 注册哦

本版积分规则