空间直线摆动自适应规划

算法思想:

  目前的空间摆动规划算法需要设置基准面和摆动角度自适应能力不强，需要人工设置摆动参数，该算法以机器人末端法兰盘(工具TCP)坐标系Z轴所垂直的平面为基准面，实现自适应摆动焊接

算法原理:

   该算法以正弦波为摆动规划模板,以空间直线插补算法确定插补点以及通过向量叉乘算法确定摆动焊接平面,结合摆动参数实现空间直线摆动自适应规划算法

   只要确定了直线的方向向量和TCP末端Z轴相对于基座标系的向量，我们就可以确定摆动的方向向量

   首先我们获取TCP相对于基座标系的齐次变换矩阵 ，我们是以Z轴为方向向量作为摆动平面的法向量,所以有： 

                                                                                                                        (1)

   齐次变换矩阵 可以写成如下形式:

                                                                                                          （2）

   其中：

                                     （3）

   根据齐次变换矩阵原理我们可知,矩阵 中的列向量( )就是TCP末端Z轴相对于基座标系的变换关系：

                                                                                                （4）

   我们记直线摆动起始点坐标 ,摆动终点坐标 ,则该直线的方向向量 为:

                                                                                             （5）

则根据式(1),(4),(5)我们可以确定摆动方向向量 为:

                                    （6）

定义摆动参数,频率 ,摆动振幅 ,插补点数 ,摆动次数 ,则根据正弦摆动原理有:

                                                                                                               （7）

   我们记插补点的集合为 ,插补点可以根据空间直线插补算法得出,在此不作具体说明

   则根据式(7)我们可以得到每个插补点的摆动偏移量 为为:

                               （8）

   根据式(6),(7),(8)我们得到更新后的最终点位 为:

                               （9）

  至此,我们得到该算法的数学模型

MatLab测试:

测试结果图:

摆动效果图(摆动幅度为50,摆动频率为0.785)

Gif图(c++,QT,opencascade)