点云轨迹平滑处理

背景：目前有一组按特定算法生成机械臂点云坐标数据，使用六轴协作机器人去跑这些点位。但是考虑到机械臂运动空间有限，我们在设定了机械臂的运动区间，只有满足这些点位的数据才保留。这会导致机械臂点云中部分数据会被去截去，从某一个点到另一个之间直接是直线运动，那么如何补充这些数据呢？

因为是企业项目，只讲述原理方法，代码和仿真效果没法展示！

三种方法

1、采用直线插值的方式，需先滤波处理。

（1）读取点云文档数据文档'test1.txt'

（2）按照x、y、z、Rx、Ry、Rz分别存储在6个数组中

（3）根据欧式距离阈值，判断那些点被删除了，阈值判断可采用两种方法。（1）取固定值 （2）取所有阈值的均值，然后乘以一定的倍数，后续需要调参

（4）初始化插值后向量

（5）遍历所有欧式距离，找到需要插值的点及下一点

（6）根据间隔距离的远近，按一定数量分配插值点

（7）计算x、y、z、Rx、Ry的插值点，对Rz不做处理，但需要保持和x同维数。

（8）将插值点加入到当前点中

（9） 同时生成两份txt文件，一份是排序的阈值数据表，一份是按对应点位生成的插值排序表，此表用来直观显示出欧式距离的变化，也可以找到明显被删除的点，但是一旦数据过多，及不太容易找打，因此采用一个排序的数据文档，可以看到欧式距离的变化。

（10）合并成新的n行6列的6d空间姿态坐标向量

（11）输出新的文本文件”testafter.txt”

校验插值数据

方法一、根据生成的txt文件，将数据拷贝到excel文件中，利用excel的数据对比功能，比较前后两个6D空间姿态的数据变化，找到插值点位插入的数据。

方案二、MATLAB，根据读取的前后两个点云数据中x、y、z的坐标，生成点云图，拟合平面，尤其是生成机械臂运动轨迹图，可根据轨迹的变化清楚的看到前后插值处理后的轨迹变化。

2、贝塞尔曲线插值+均值滤波方案

编程思路

1、贝塞尔函数模块：采用三阶贝塞尔曲线，接受4个控制点和一个时间参数t，返回时间t时刻的贝塞尔曲线。

2、滤波模块：移动均值滤波+加权移动均值滤波

3、定义Point结构体，用来表示三维空间中的一个点

4、主函数：

① 读文件、提取x、y、z。

② 将x、y、z存入vector容器中

③ 滤波，可选移动均值或加权移动均值

④将四个相邻的控制点视为一组，计算出连续的贝塞尔曲线上的点，将其存入new_points中。

⑤、计算第一个片段最后一个点和第二个片段第一个点，确定两个切线向量，对向量长度进行归一化。

⑥、计算两个控制点control_point1和control_point2*，它将第一个控制点 (control_point1) 设置为当前控制点 (p3) 向后偏移 tangent_length2 * 0.33 的位置。这个长度是第二个向量的长度，所以它确保了两个连接贝塞尔曲线之间的距离相等。同样地，它将第二个控制点 (control_point2) 设置为下一个控制点 (p4) 向前偏移 tangent_length1 * 0.33 的位置。

⑦最后，在新创建的两个控制点 (control_point1 和 control_point2) 之间计算连续贝塞尔曲线上的点，并将它们添加到 new_points 数组中。

⑧输出新的轨迹点

3、B样条曲线