Python实现五项式插值法实现机械臂运动轨迹规划
在树莓派中使用Python实现五项式插值法，控制GPIO管脚输出PWM实现对机械臂的控制。
五项式插值法常用于机械臂运动轨迹规划中，可以实现对舵机角度、角速度和叫加速度进行约束，使舵机运行更加平稳，同时也减少了机械臂在运动过程中的抖动情况，可以延长机械臂使用寿命。
一、五项式插值法实现曲线图
二、五项式插值法推导公式
-
建立约束公式组
设五项式对角度 Θ ( t ) {\color{Red}\Theta_{(t)}} Θ(t)、角速 Θ ( t ) ′ {\color{Red}\Theta'_{(t)}} Θ(t)′和角加速度 Θ ( t ) ′ ′ {\color{Red}\Theta''_{(t)}} Θ(t)′′随时间变换的约束如公式组 A {\color{Red}A} A所示，如果需要通过五项式插值法来实现输出PWM控制舵机，最终实现机械臂运动轨迹控制，则需要求出式中的未知变量 a 0 、 a 1 、 a 2 、 a 3 、 a 4 、 a 5 {\color{Red}a_{0}、a_{1}、a_{2}、a_{3}、a_{4}、a_{5 }} a0、a1、a2、a3、a4、a5
A { Θ ( t ) = a 0 + a 1 t + a 2 t 2 + a 3 t 3 + a 4 t 4 + a 5 t 5 Θ ( t ) ′ = 0 + a 1 + 2 a 2 t + 3 a 3 t 2 + 4 a 4 t 3 + 5 a 5 t 4 Θ ( t ) ′ ′ = 0 + 0 + 2 a 2 + 6 a 3 t + 12 a 4 t 2 + 20 a 5 t 3 \\{\color{Red}A} \left\{\begin{matrix} \Theta _{(t)}=a_{0}+a_{1}t +a_{2}t^{2}+a_{3}t^{3}+a_{4}t^{4}+a_{5}t^{5}& & \\ \Theta' _{(t)}=0+a_{1}+2a_{2}t+3a_{3}t^{2}+4a_{4}t^{3}+5a_{5}t^{4}& & \\ \Theta'' _{(t)}=0+0+2a_{2}+6a_{3}t+12a_{4}t^{2}+20a_{5}t^{3} & & \end{matrix}\right. A⎩⎨⎧Θ(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5Θ(t)′=0+a1+2a2t+3a3t2+4a4t3+5a5t4Θ(t)′′=0+0+2a2+6a3t+12a4t2+20a5t3 -
求解未知变量
∙ \bullet ∙机械臂中舵机的旋转角度时随时间变换的，在时间线上从一个角度转到下一角度，设舵机从一个角度旋转到下一个角度，其中起始角度和起始时间为 Θ 0 和 t 0 {\color{Red}\Theta_{0}}和{\color{Red}t_{0}} Θ0和t0，停止角度和停止时间为 Θ f {\color{Red}\Theta_{f}} Θf和 t f {\color{Red}t_{f}} tf，将 [ Θ 0 、 t 0 ] {\color{Red}[\Theta_{0}、t_{0}]} [Θ0、t0]和 [ Θ f 、 t f ] {\color{Red}[\Theta_{f}、t_{f}]} [Θf、tf]分别带入公式组 A {\color{Red}A} A中的一式，由于 t 0 = 0 {\color{Red}t_{0}=0} t0=0，得到公式组 B {\color{Red}B} B中的一式和二式
∙ \bullet ∙对 [ Θ 0 、 Θ f ] {\color{Red}[\Theta_{0}、\Theta_{f}]} [Θ0、Θf]进行一介求导得到 [ Θ 0 ′ 、 Θ f ′ ] {\color{Red}[\Theta'_{0}、\Theta'_{f}]} [Θ0′、Θf′]，将 [ Θ 0 ′ 、 t 0 ] {\color{Red}[\Theta'_{0}、t_{0}]} [Θ0′、t0]和 [ Θ f ′ 、 t f ] {\color{Red}[\Theta'_{f}、t_{f}]} [Θf′、tf]分别带入公式组 A {\color{Red}A} A中的二式，由于 t 0 = 0 {\color{Red}t_{0}=0} t0=0，得到公式组 B {\color{Red}B} B中的三式和四式
∙ \bullet ∙对 [ Θ 0 、 Θ f ] {\color{Red}[\Theta_{0}、\Theta_{f}]} [Θ0、Θf]进行二阶求导得到 [ Θ 0 ′ ′ 、 Θ f ′ ′ ] {\color{Red}[\Theta''_{0}、\Theta''_{f}]} [Θ0′′、Θf′′]，将 [ Θ 0 ′ ′ 、 t 0 ] {\color{Red}[\Theta''_{0}、t_{0}]} [Θ0′′、t0]和 [ Θ f ′ ′ 、 t f ] {\color{Red}[\Theta''_{f}、t_{f}]} [Θf′′、tf]分别带入公式组 A {\color{Red}A} A中的三式，由于 t 0 = 0 {\color{Red}t_{0}=0} t0=0，得到公式组 B {\color{Red}B} B中的三式和四式
B { Θ ( t 0 ) = Θ 0 = a 0 Θ ( t f ) = Θ 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 Θ ( t 0 ) ′ = Θ 0 ′ = a 1 Θ ( t f ) ′ = Θ f ′ = a 1 + 2 a 2 t f + 3 a 3 t f 2 + 4 a 4 t f 3 + 5 a 5 t f 4 Θ ( t 0 ) ′ ′ = Θ 0 ′ ′ = 2 a 2 Θ ( t f ) ′ ′ = Θ f ′ ′ = 2 a 2 + 6 a 3 t f + 12 a 4 t f 2 + 20 a 5 t f 3 \\{\color{Red}B} \left\{\begin{matrix} \Theta _{(t_{0})}=\Theta _{0}=a_{0}& & \\ \Theta _{(t_{f})}=\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}& & \\ \Theta' _{(t_{0})}=\Theta' _{0}=a_{1}& & \\ \Theta' _{(t_{f})}=\Theta' _{f}=a_{1} +2a_{2}t_{f}+3a_{3}t_{f}^{2}+4a_{4}t_{f}^{3}+5a_{5}t_{f}^{4}& & \\ \Theta'' _{(t_{0})}=\Theta'' _{0}=2a_{2}& & \\ \Theta'' _{(t_{f})}=\Theta'' _{f}=2a_{2}+6a_{3}t_{f}+12a_{4}t_{f}^{2}+20a_{5}t_{f}^{3}& & \\ \end{matrix}\right. B⎩⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎧Θ(t0)=Θ0=a0Θ(tf)=Θf=a0+a1tf+a2tf2+a3tf3+a4tf4+a5tf5Θ(t0)′=Θ0′=a1Θ(tf)′=Θf′=a1+2a2tf+3a3tf2+4a4tf3+5a5tf4Θ(t0)′′=Θ0′′=2a2Θ(tf)′′=Θf′′=2a2+6a3tf+12a4tf2+20a5tf3
∙ \bullet ∙将求解得到的公式组 B {\color{Red}B} B带入到公式组 A {\color{Red}A} A中，求解得到未知变量 a 0 、 a 1 、 a 2 、 a 3 、 a 4 、 a 5 {\color{Red}a_{0}、a_{1}、a_{2}、a_{3}、a_{4}、a_{5 }} a0、a1、a2、a3、a4、a5如公式组 C {\color{Red}C} C所示
C { a 0 = Θ 0 a 1 = Θ 0 ′ a 2 = Θ 0 ′ ′ 2 a 3 = 20 Θ f − 20 Θ 0 − ( 8 Θ f ′ + 12 Θ 0 ′ ) t f − ( 3 Θ 0 ′ ′ − Θ f ′ ′ ) ) t f 2 2 t f 3 a 4 = 30 Θ 0 − 30 Θ f − ( 14 Θ f ′ + 16 Θ 0 ′ ) t f − ( 3 Θ 0 ′ ′ − 2 Θ f ′ ′ ) ) t f 2 2 t f 4 a 5 = 12 Θ f − 12 Θ 0 − ( 6 Θ f ′ + 6 Θ 0 ′ ) t f − ( Θ 0 ′ ′ − Θ f ′ ′ ) ) t f 2 2 t f 5 \\{\color{Red}C} \left\{\begin{matrix} a_{0}=\Theta _{0}& & \\ a_{1}=\Theta' _{0}& & \\ a_{2}=\frac{\Theta'' _{0}}{2}& & \\ a_{3}=\frac{20\Theta _{f}-20\Theta _{0}-(8\Theta' _{f}+12\Theta' _{0})t_{f}-(3\Theta'' _{0}-\Theta'' _{f}))t_{f}^{2}}{2t_{f}^{3}}& & \\ a_{4}=\frac{30\Theta _{0}-30\Theta _{f}-(14\Theta' _{f}+16\Theta' _{0})t_{f}-(3\Theta'' _{0}-2\Theta''_{f}))t_{f}^{2}}{2t_{f}^{4}}& & \\ a_{5}=\frac{12\Theta _{f}-12\Theta _{0}-(6\Theta' _{f}+6\Theta' _{0})t_{f}-(\Theta'' _{0}-\Theta'' _{f}))t_{f}^{2}}{2t_{f}^{5}}& & \end{matrix}\right. C⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧a0=Θ0a1=Θ0′a2=2Θ0′′a3=2tf320Θf−20Θ0−(8Θf′+12Θ0′)tf−(3Θ0′′−Θf′′))tf2a4=2tf430Θ0−30Θf−(14Θf′+16Θ0′)tf−(3Θ0′′−2Θf′′))tf2a5=2tf512Θf−12Θ0−(6Θf′+6Θ0′)tf−(Θ0′′−Θf′′))tf2
五项式插值法的公式推导到这里就结束了，之后就可以结合代码来实现五项式插值法，最终通过树莓派的GPIO管脚输出PWM对机械臂进行控制
三、代码实现
import matplotlib.pyplot as plt
import numpy as np
import math
#q_array=[[0,50,150,100,40],[0,90,150,180,0],[0,20,80,100,150],[180,100,150,50,100]]#指定起止位置
q_array=[[0,90,150,180,0],[0,20,80,100,150]]#指定起止位置
t_array=[0,3,6,12,14]#指定起止时间
v_array=[0,10,20,-15,0]#指定起止速度
a_array=[0,20,30,-20,0]#指定起止加速度
#初始状态
t=[t_array[0]]
q=[q_array[0][0]]
v=[v_array[0]]
a=[a_array[0]]
for i in range(len(q_array)):
for j in range(len(q_array[i])-1):
T = t_array[j + 1] - t_array[j] # 时间差
a0 = q_array[i][j]
a1 = v_array[j]
a2 = a_array[j] / 2
a3 = (20 * q_array[i][j + 1] - 20 * q_array[i][j] - (8 * v_array[j + 1] + 12 * v_array[j]) * T - (3 * a_array[j] - a_array[j + 1]) * math.pow(T, 2)) / (2 * math.pow(T, 3))
a4 = (30 * q_array[i][j] - 30 * q_array[i][j + 1] + (14 * v_array[j + 1] + 16 * v_array[j]) * T + (3 * a_array[j] - 2 * a_array[j + 1]) * math.pow(T, 2)) / (2 * math.pow(T, 4))
a5 = (12 * q_array[i][j + 1] - 12 * q_array[i][j] - (6 * v_array[j + 1] + 6 * v_array[j]) * T - ( a_array[j] - a_array[j + 1]) * math.pow(T, 2)) / (2 * math.pow(T, 5))
ti = np.arange(t_array[j], t_array[j + 1], .001)
'''
求解出角度，角速度，角加速度随某个时间区间随时间变换的曲线
'''
qi = a0 + a1 * (ti - t_array[j]) + a2 * np.power((ti - t_array[j]), 2) + a3 * np.power((ti - t_array[j]), 3) + a4 * np.power( (ti - t_array[j]), 4) + a5 * np.power((ti - t_array[j]), 5)
vi = a1 + 2 * a2 * (ti - t_array[j]) + 3 * a3 * np.power((ti - t_array[j]), 2) + 4 * a4 * np.power( (ti - t_array[j]), 3) + 5 * a5 * np.power((ti - t_array[j]), 4)
ai = 2 * a2 + 6 * a3 * (ti - t_array[j]) + 12 * a4 * np.power((ti - t_array[j]), 2) + 20 * a5 * np.power((ti - t_array[j]), 3)
ti = ti.tolist() # 将矩阵转换为list，否则进行数据整合会报错
qi = qi.tolist() # 将矩阵转换为list，否则进行数据整合会报错
vi = vi.tolist() # 将矩阵转换为list，否则进行数据整合会报错
ai = ai.tolist() # 将矩阵转换为List，否则进行数据整合会报错
t = t + ti[1:] #进行数据整合，用来绘制函数图像
q = q + qi[1:] #进行数据整合，用来绘制函数图像
v = v + vi[1:] #进行数据整合，用来绘制函数图像
a = a + ai[1:] #进行数据整合，用来绘制函数图像
plt.figure(i)
plt.plot(t, q, linestyle=":", color=(1, 0, 0, 1.0), label="angle curve")
plt.scatter(t_array, q_array[i], marker='*', s=70, label="angle spot", zorder=3)
plt.plot(t, v, linestyle="-", color=(1, 0.5, 1, 1.0), label="speed curve")
plt.scatter(t_array, v_array, marker='^', s=70, label="speed spot", zorder=3)
plt.plot(t, a, linestyle="-.", color=(0, 0, 0, 1.0), label="acceleration curve")
plt.scatter(t_array, a_array, marker='o', s=70, label="acceleration spot", zorder=3)
plt.grid()#显示网格
plt.legend()#显示图例
#保存图片
figpath = 'E:/Python代码/机械臂运动轨迹规划/servo'+i.__str__()
plt.savefig(figpath)
t.clear()
q.clear()
v.clear()
a.clear()
plt.show()
四、结语
写这篇博客收获很多，最大的收获就是学会来在MarkDwon编辑器中使用LaTeX插入数学公式。
下面使用LaTeX插入的小图案，谢谢观看
♠ ♠ ♣ ♠ ♣ ♣ ∈ ∋ ♣ ⋆ ⋆ ⋆ ∪ ⋆ ⋆ {\color{Green}\spadesuit} \qquad \qquad {\color{Green}\spadesuit}\\ {\color{Blue}\clubsuit} \qquad \quad {\color{Green}\spadesuit} \qquad \quad {\color{Blue}\clubsuit} \\ {\color{Blue}\clubsuit} \quad {\color{Red}\in} \qquad \; \; \; \; {\color{Red}\ni} \; \; \, {\color{Blue}\clubsuit} \\ \star \qquad \qquad \; \; \; \; \star \\ \star \quad \, \, {\color{Red}\cup} \; \; \; \; \star \\ \star ♠♠♣♠♣♣∈∋♣⋆⋆⋆∪⋆⋆
$$
{\color{Green}\spadesuit} \qquad \qquad {\color{Green}\spadesuit}\\
{\color{Blue}\clubsuit} \qquad \quad {\color{Green}\spadesuit} \qquad \quad {\color{Blue}\clubsuit} \\
{\color{Blue}\clubsuit} \quad {\color{Red}\in} \qquad \; \; \; \; {\color{Red}\ni} \; \; \, {\color{Blue}\clubsuit} \\
\star \qquad \qquad \; \; \; \; \star \\
\star \quad \, \, {\color{Red}\cup} \; \; \; \; \star \\
\star
$$
⋆ ⋆ ⋆ ⋆ ⋆ ⋆ ⋆ ⋆ ⋆ ⋆ ⋆ ⋆ \star \qquad \qquad \star\\ \star \qquad \quad \star \qquad \quad \star\\ \star \qquad \quad \qquad \; \; \; \; \; \; \, \star \\ \star \qquad \qquad \; \; \; \; \star \\ \star \qquad \; \; \; \; \star \\ \star ⋆⋆⋆⋆⋆⋆⋆⋆⋆⋆⋆⋆
$$
\star \qquad \qquad \star\\
\star \qquad \quad \star \qquad \quad \star\\
\star \qquad \quad \qquad \; \; \; \; \; \; \, \star \\
\star \qquad \qquad \; \; \; \; \star \\
\star \qquad \; \; \; \; \star \\
\star
$$
♣ ♣ ♣ ♣ ♣ ♣ ♣ ♣ ♣ ♣ ♣ ♣ \clubsuit \qquad \qquad \clubsuit\\ \clubsuit \qquad \quad \clubsuit \qquad \quad \clubsuit\\ \clubsuit \qquad \quad \qquad \; \; \; \; \; \; \, \clubsuit \\ \clubsuit \qquad \qquad \; \; \; \; \clubsuit \\ \clubsuit \qquad \; \; \; \; \clubsuit \\ \clubsuit ♣♣♣♣♣♣♣♣♣♣♣♣
$$
\clubsuit \qquad \qquad \clubsuit\\
\clubsuit \qquad \quad \clubsuit \qquad \quad \clubsuit\\
\clubsuit \qquad \quad \qquad \; \; \; \; \; \; \, \clubsuit \\
\clubsuit \qquad \qquad \; \; \; \; \clubsuit \\
\clubsuit \qquad \; \; \; \; \clubsuit \\
\clubsuit
$$
♠ ♠ ♠ ♠ ♠ ♠ ♠ ♠ ♠ ♠ ♠ ♠ \spadesuit \qquad \qquad \spadesuit\\ \spadesuit \qquad \quad \spadesuit \qquad \quad \spadesuit\\ \spadesuit\qquad \quad \qquad \; \; \; \; \; \; \, \spadesuit \\ \spadesuit \qquad \qquad \; \; \; \; \spadesuit \\ \spadesuit \qquad \; \; \; \; \spadesuit \\ \spadesuit ♠♠♠♠♠♠♠♠♠♠♠♠
$$
\spadesuit \qquad \qquad \spadesuit\\
\spadesuit \qquad \quad \spadesuit \qquad \quad \spadesuit\\
\spadesuit\qquad \quad \qquad \; \; \; \; \; \; \, \spadesuit \\
\spadesuit \qquad \qquad \; \; \; \; \spadesuit \\
\spadesuit \qquad \; \; \; \; \spadesuit \\
\spadesuit
$$