机械臂轨迹规划方法综述

更新：2025.3.17，补充了关节轨迹规划部分（点到点五次多项式插值）

一、机械臂轨迹规划概述

机械臂的运动规划包括路径规划和轨迹规划两个方面。

1.路径规划：

定义：路径规划是指在给定的起点和终点之间，寻找一条无碰撞的路径。路径可以是直线、曲线或者其他形状，主要关注的是路径的几何形状。

目标：避障、路径尽可能短、平滑

常用方法：直线插补、圆弧插补、B样条、多项式插值、RRT、基于图搜索的方法、人工势场法等

2.轨迹规划

定义：轨迹规划是在路径规划的基础上，为路径赋予时间信息，包括速度、加速度等，是的机械臂能够按照预定的时间和运动特性完成运动

目标：时间优化、能量优化、运动平滑、动态约束

常用方法：笛卡尔轨迹规划（对末端执行器位姿有严格要求的场合）、关节空间轨迹规划（对关节运动由严格要求的场合）

关节空间轨迹规划：将关节变量表示成时间的函数，并规划它的一阶和二阶导数。

二、插补与拟合

1.直线插补

1）本质上就是在起始位置和目标位置之间生成一段直线轨迹。

2）其求解思路如下：

确定起始位置==》确定单位步长（dx，dy，dz），也就是（终点坐标-起点坐标）/（插值点个数-1）==》确定插值点坐标，其计算公式如下

def linear_interpolation_3d(start_point, end_point, num_points):
    """
    生成从start_point到end_point的三维直线插补点。
    参数:
    start_point (tuple): 起始点坐标，格式为(x, y, z)。
    end_point (tuple): 终止点坐标，格式为(x, y, z)。
    num_points (int): 要生成的插补点数量，包括起始点和终止点。
    返回:
    list: 包含插补点坐标的列表。
    """
    points = []
    x_start, y_start, z_start = start_point
    x_end, y_end, z_end = end_point
    
    # 计算x、y和z方向上的增量
    dx = (x_end - x_start) / (num_points - 1)
    dy = (y_end - y_start) / (num_points - 1)
    dz = (z_end - z_start) / (num_points - 1)
    # 生成插补点, 这里的dx，dy，dz就是单位步长，也就是每个插补点之间的间距是恒定的
    for i in range(num_points):
        x = x_start + i * dx
        y = y_start + i * dy
        z = z_start + i * dz
        points.append((x, y, z))
    return points

# 使用示例
start_point = (0, 0, 0)  # 起始点坐标
end_point = (10, 10, 10)  # 终止点坐标
num_points = 5  # 生成5个插补点，包括起始点和终止点

interpolated_points = linear_interpolation_3d(start_point, end_point, num_points)
print(interpolated_points)

2.圆弧插补

1）圆弧插补可以用于生成起点和终点之间的一段圆弧路径。

2）其求解思路如下：

给定起点、终点、圆心坐标==》计算起点与终点间对应圆弧的夹角，其计算公式如下：

==》计算单位步长（角度）==》计算插值点位置

import numpy as np

def circular_interpolation_3d_with_z(start_point, end_point, center_point, num_points):
    """
    生成从start_point到end_point的三维圆弧插补点，同时考虑Z坐标的线性变化。
    参数:
    start_point (tuple): 起始点坐标，格式为(x, y, z)。
    end_point (tuple): 终止点坐标，格式为(x, y, z)。
    center_point (tuple): 圆心点坐标，格式为(x, y, z)。
    num_points (int): 要生成的插补点数量，包括起始点和终止点。
    返回:
    list: 包含插补点坐标的列表。
    """
    points = []
    x_start, y_start, z_start = start_point
    x_end, y_end, z_end = end_point
    x_center, y_center, z_center = center_point
    # 计算从圆心到起点和终点的向量
    vector_start = np.array([x_start - x_center, y_start - y_center])
    vector_end = np.array([x_end - x_center, y_end - y_center])
    # 计算圆弧的角度
    angle = np.arccos(np.dot(vector_start, vector_end) / (np.linalg.norm(vector_start) * np.linalg.norm(vector_end)))
    # 计算插补点的步长
    angle_step = angle / (num_points - 1)
    # 生成插补点，Z采用直线插补，x，y采用圆弧插补
    for i in range(num_points):
        t = i * angle_step
        point = (center_point[0] + np.cos(t) * np.linalg.norm(vector_start),
                 center_point[1] + np.sin(t) * np.linalg.norm(vector_start),
                 z_start + i * (z_end - z_start) / (num_points - 1))
        points.append(point)
    return points

# 使用示例
start_point = (0, 0, 0)  # 起始点坐标
end_point = (10, 0, 5)  # 终止点坐标
center_point = (5, 0, 2.5)  # 圆心点坐标
num_points = 100  # 生成100个插补点，包括起始点和终止点

interpolated_points = circular_interpolation_3d_with_z(start_point, end_point, center_point, num_points)

#