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

---

前言

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

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

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

---

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

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

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

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

给出五次多项式如下：
$$\theta (t)=a_0+a_{1}t+a_2{t}^2+a_3{t}^3+a_4{t}^4+a_5{t}^5$$
始末状态对应的是${\theta }_0$与${\theta }_f$，可以得到约束条件见下：
$$\begin{gathered} {\theta }_0=a_0 \\ {\theta }_f=a_0+a_1{t}_f+a_2{t}_f^2+a_3{t}_f^3+a_4{t}_f^4+a_5{t}_f^5 \\ \dot{{\theta }_0}=a_1 \\ \dot{{\theta }_f}=a_1+2a_2{t}_f+3a_3{t}_f^2+4a_4{t}_f^3+5a_5{t}_f^4 \\ \ddot{{\theta }_0}=2a_2 \\ \ddot{{\theta }_f}=2a_2+6a_3{t}_f+12a_4{t}_f^2+20a_5{t}_f^3 \end{gathered}$$
这些约束条件确定了一个具有6个方程和6个未知数的线性方程组，其解如下（通过矩阵求解求出）：
$$\begin{gathered} 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}{2t_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}{2t_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}{2t_f^5}\end{array} \end{gathered}$$

二、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()

2. 结果

画出来的结果如下

OK！美滋滋结束 😉

总结

以上就是五次多项式插值法的python实现，本文只是简单给出了机械臂插值法的定义与应用代码，在实际应用中还需要根据具体情况进行修改，后续我会继续更新机械臂规划相关的学习心得，欢迎交流。

参考书籍

[1]机器人学导论，John J.Craig，斯坦福大学