python实现6自由度机械臂的五次多项式插值轨迹规划

提示：已经熟悉多项式插值法的可以直接跳转代码部分多项式插值（polynomial interopolation）是机械臂常见的轨迹规划方法,适用于关节空间内的规划，在机械臂规划过程中能够有效的减少抖动，增加机械臂使用寿命。但是我搜索CSDN上寻找用python实现的代码较少，MATLAB中的库又过于集成，并且在实际项目中无法应用，故自己研究了一下产生笔记如下，供各位参考。正文开始,代码可供参考,转

Jeni.

3010人浏览 · 2023-07-06 14:40:34

Jeni. · 2023-07-06 14:40:34 发布

前言

提示：已经熟悉多项式插值法的可以直接跳转代码部分

多项式插值（polynomial interopolation）是机械臂常见的轨迹规划方法,适用于关节空间内的规划，在机械臂规划过程中能够有效的减少抖动，增加机械臂使用寿命。

但是我搜索CSDN上寻找用python实现的代码较少，MATLAB中的库又过于集成，并且在实际项目中无法应用，故自己研究了一下产生笔记如下，供各位参考。

正文开始,代码可供参考,转载需注明

一、五次多项式插值法理论分析

如果要确定在路径曲线段的起始点和终止点的位置、速度和加速度，需要用一个五次多项式进行插值。

关于三次多项式插值和五次多项式的插值法在《机器人学导论》这本书中有系统的推导过程,在此只给出五次多项式的推导过程。

给出五次多项式如下：
$\theta(t) = a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5$
始末状态对应的是 $\theta_0$ 与 $\theta_f$ ，可以得到约束条件见下：
$\theta_0= a_0\\ \theta_f= a_0+a_1t_f+a_2t_f^2+a_3t_f^3+a_4t_f^4+a_5t_f^5\\ \dot{\theta_0}= a_1\\ \dot{\theta_f}= a_1+2a_2t_f+3a_3t_f^2+4a_4t_f^3+5a_5t_f^4\\ \ddot{\theta_0}= 2a_2\\ \ddot{\theta_f}= 2a_2+6a_3t_f+12a_4t_f^2+20a_5t_f^3\\$
这些约束条件确定了一个具有6个方程和6个未知数的线性方程组，其解如下（通过矩阵求解求出）：
$a_0=\theta_0\\ a_1=\dot{\theta_0}\\ a_2=\frac{\ddot{\theta_0}}{2}\\ \begin{array}{l} a_{3}=\frac{20 \theta_{f}-20 \theta_{0}-\left(8 \dot{\theta}_{f}+12 \dot{\theta}_{0}\right) t_{f}-\left(3 \ddot{\theta}_{0}-\ddot{\theta}_{f}\right) t_{f}^{2}}{2 t_{f}^{3}} \\ a_{4}=\frac{30 \theta_{0}-30 \theta_{f}+\left(14 \dot{\theta}_{f}+16 \dot{\theta}_{0}\right) t_{f}+\left(3 \ddot{\theta}_{0}-2 \theta_{f}\right) t_{f}^{2}}{2 t_{f}^{4}} \\ a_{5}=\frac{12 \theta_{f}-12 \theta_{0}-\left(6 \dot{\theta}_{f}+6 \dot{\theta}_{0}\right)-\left(\ddot{\theta}_{0}-\ddot{\theta}_{f}\right) t_{f}^{2}}{2 t_{f}^{5}} \end{array}$

二、python代码实现

话不多说，直接上代码：

1. 代码

# encoding: utf-8

import numpy as np
from scipy.interpolate import CubicHermiteSpline
import matplotlib as mpl
import matplotlib.pyplot as plt
import math


# 给定关节起始位置 
start_angle = np.array([15, 34, 56, 34, 66, 90])
end_angle = np.array([75, 45, 45, 56, 43, 70])
# 我用的角度，如果需要弧度可以放开下面的两行代码
# start_angle = np.deg2rad(start_angle1)
# end_angle = np.deg2rad(end_angle1)

# 设定起始时间
start_time = 0
end_time = 5
t_time = np.array([0, 0, 0, 0, 0, 0])

# 给定关节始末角速度与角加速度
start_vel = np.array([0, 0, 0, 0, 0, 0])
end_vel = np.array([0, 0, 0, 0, 0, 0])

start_acc = np.array([0, 0, 0, 0, 0, 0])
end_acc = np.array([0, 0, 0, 0, 0, 0])

# 初始化保存每个关节角度、速度和加速度随时间变化的列表
qq = []
vv = []
aa = []

# 计算a0-a5，并带入多项式中求解曲线
for i in range(len(start_angle)):
    # i = 1
    # print(len(start_angle))
    #初始状态
    t=[t_time[0]]
    q=[start_angle[i]]
    v=[start_vel[i]]
    a=[start_acc[i]]

    a0 = start_angle[i]
    a1 = start_vel[i]
    a2 = start_acc[i]
    # print(30*start_angle[i]-30*start_angle[0])
    a3 = (20*end_angle[i]-20*start_angle[i]-(8*end_vel[i]+12*start_vel[i])*end_time-(3*end_acc[i]-start_acc[i])*math.pow(end_time, 2))/(2 * math.pow(end_time, 3))
    a4 = (30*start_angle[i]-30*end_angle[i]+(14*end_vel[i]+16*start_vel[i])*end_time+(3*end_acc[i]-2*start_acc[i])*math.pow(end_time, 2))/(2 * math.pow(end_time, 4))
    a5 = (12*end_angle[i]-12*start_angle[i]-(6*end_vel[i]+6*start_vel[i])*end_time-(end_acc[i]-start_acc[i])*math.pow(end_time, 2))/(2 * math.pow(end_time, 5))
    # print(a0,a1,a2,a3,a4,a5)
    ti = np.arange(start_time, end_time, .01)
    
    # 求解出角度，角速度，角加速度随某个时间区间随时间变换的曲线
    qi = a0 + a1 * ti + a2 * np.power(ti, 2) + a3 * np.power(ti, 3) + a4 * np.power(ti, 4) + a5 * np.power(ti, 5)
    vi = a1 + 2 * a2 * ti  + 3 * a3 * np.power(ti , 2) + 4 * a4 * np.power(ti, 3) + 5 * a5 * np.power(ti, 4)
    ai = 2 * a2 + 6 * a3 * ti + 12 * a4 * np.power(ti, 2) + 20 * a5 * np.power(ti, 3)

    # # 将矩阵转换为List，否则进行数据整合会报错
    ti = ti.tolist()  
    qi = qi.tolist()  
    vi = vi.tolist()  
    ai = ai.tolist()  
    # print(ai)

    #进行数据整合，用来绘制函数图像
    t = t + ti[1:] 
    q = q + qi[1:] 
    v = v + vi[1:] 
    a = a + ai[1:] 
    # print(type(q))

    qq.append(q)
    vv.append(v)
    aa.append(a)

# 绘制图像
plt.figure()
for j in range(len(start_angle)):

    plt.subplot(3,2,j+1)

    plt.plot(t, qq[j], linestyle=":", color=(1, 0, 0, 1.0), label="angle curve")  # 角度变化曲线
    plt.plot(t, vv[j], linestyle="-", color=(1, 0.5, 1, 1.0), label="speed curve")  # 角速度变化曲线
    plt.plot(t, aa[j], linestyle="-.", color=(0, 0, 0, 1.0), label="acceleration curve")  # 角加速度变化曲线
    plt.title("Angle {}".format(j+1))
    plt.xlabel("Time (s)", x=1)

    # plt.rcParams['font.size'] = 15   #设置字体大小
    plt.suptitle("5th Polynomial Interpolation (6-DOF-ROBOT)")
    plt.grid()  # 显示网格
    plt.legend()  # 显示图例

plt.tight_layout()  # 调整子图布局
plt.show()