在RoboDK路径中识别90度角点并生成对应垂直向量

1. 核心逻辑：用点积判断90度转角 #

要找到序列中的90度转角点，核心是利用向量点积为0则夹角90度的性质，步骤如下：

• 按顺序取连续三个点：前点P_prev、候选转角点P_curr、后点P_next
• 计算向量 v1 = P_curr - P_prev（前点指向转角点的向量）
• 计算向量 v2 = P_next - P_curr（转角点指向后点的向量）
• 计算点积：dot = v1.x*v2.x + v1.y*v2.y + v1.z*v2.z
• 由于浮点精度误差，判断abs(dot) < 1e-6即可认为夹角是90度

2. 整理你的点位数据 #

先清理重复符号，整理为每行一个标准点位（xyz为坐标，ijk暂不参与本次转角判断）：

125.15790, 50.00000, 139.30003, 0.00028, -0.02356, -0.99972
89.54415, 50.00000, 103.68629, 0.70711, 0.00000, -0.70711
139.30003, 50.00000, 125.15790, -0.99972, -0.02356, 0.00028
125.15790, 50.00000, 139.30003, 0.00028, -0.02356, -0.99972
103.68629, 50.00000, 89.54415, -0.70711, 0.00000, 0.70711
139.30003, 50.00000, 125.15790, -0.99972, -0.02356, 0.00028

3. 针对你的数据的计算示例 #

遍历所有连续三点组合后，发现当前数据中没有符合90度转角的点，举其中一组计算验证：

• 取第2、3、4个点：
  • P_prev = (89.54415, 50.00000, 103.68629)
  • P_curr = (139.30003, 50.00000, 125.15790)
  • P_next = (125.15790, 50.00000, 139.30003)
  • 向量v1 = (49.75588, 0, 21.47161)，向量v2 = (-14.14213, 0, 14.14213)
  • 点积 = 49.75588*(-14.14213) + 0 + 21.47161*14.14213 ≈ -400，远大于1e-6，不符合条件

4. 生成90度向量的方法 #

如果找到符合条件的转角点，可通过两种方式生成目标向量：

• 直接用判断时的v1和v2：这两个向量本身就是夹角90度的向量
• 若需要垂直于转角平面的第三个正交向量，计算叉积 v3 = v1 × v2，v3会与v1、v2都垂直

5. 注意事项 #

• 浮点精度：不要直接判断点积等于0，用极小阈值（如1e-6）判断绝对值
• 序列顺序：必须严格按点位的原始顺序取连续三点，不能打乱
• 重复数据：你的数据中有重复点位（如第1和第4个点），处理时需确认是否属于序列中的重复节点

内容的提问来源于stack exchange，提问作者joonas