Python实现五项式插值法实现机械臂运动轨迹规划

最新推荐文章于 2024-04-22 10:25:03 发布

Cherishalfy

最新推荐文章于 2024-04-22 10:25:03 发布

阅读量4.1w

点赞数 14

分类专栏：机械臂运动控制文章标签： 1024程序员节几何学 python

本文链接：https://blog.csdn.net/weixin_44223883/article/details/109115235

版权

机械臂运动控制专栏收录该内容

1 篇文章 3 订阅

订阅专栏

在树莓派中使用Python实现五项式插值法，控制GPIO管脚输出PWM实现对机械臂的控制。
五项式插值法常用于机械臂运动轨迹规划中，可以实现对舵机角度、角速度和叫加速度进行约束，使舵机运行更加平稳，同时也减少了机械臂在运动过程中的抖动情况，可以延长机械臂使用寿命。

一、五项式插值法实现曲线图

在这里插入图片描述

二、五项式插值法推导公式

建立约束公式组
设五项式对角度 ${\color{Red}\Theta_{(t)}}$ 、角速 ${\color{Red}\Theta'_{(t)}}$ 和角加速度 ${\color{Red}\Theta''_{(t)}}$ 随时间变换的约束如公式组 ${\color{Red}A}$ 所示，如果需要通过五项式插值法来实现输出PWM控制舵机，最终实现机械臂运动轨迹控制，则需要求出式中的未知变量 ${\color{Red}a_{0}、a_{1}、a_{2}、a_{3}、a_{4}、a_{5 }}$
$\\{\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.$
求解未知变量
$\bullet$ 机械臂中舵机的旋转角度时随时间变换的，在时间线上从一个角度转到下一角度，设舵机从一个角度旋转到下一个角度，其中起始角度和起始时间为 ${\color{Red}\Theta_{0}}和{\color{Red}t_{0}}$ ，停止角度和停止时间为 ${\color{Red}\Theta_{f}}$ 和 ${\color{Red}t_{f}}$ ，将 ${\color{Red}[\Theta_{0}、t_{0}]}$ 和 ${\color{Red}[\Theta_{f}、t_{f}]}$ 分别带入公式组 ${\color{Red}A}$ 中的一式，由于 ${\color{Red}t_{0}=0}$ ，得到公式组 ${\color{Red}B}$ 中的一式和二式
$\bullet$ 对 ${\color{Red}[\Theta_{0}、\Theta_{f}]}$ 进行一介求导得到 ${\color{Red}[\Theta'_{0}、\Theta'_{f}]}$ ，将 ${\color{Red}[\Theta'_{0}、t_{0}]}$ 和 ${\color{Red}[\Theta'_{f}、t_{f}]}$ 分别带入公式组 ${\color{Red}A}$ 中的二式，由于 ${\color{Red}t_{0}=0}$ ，得到公式组 ${\color{Red}B}$ 中的三式和四式
$\bullet$ 对 ${\color{Red}[\Theta_{0}、\Theta_{f}]}$ 进行二阶求导得到 ${\color{Red}[\Theta''_{0}、\Theta''_{f}]}$ ，将 ${\color{Red}[\Theta''_{0}、t_{0}]}$ 和 ${\color{Red}[\Theta''_{f}、t_{f}]}$ 分别带入公式组 ${\color{Red}A}$ 中的三式，由于 ${\color{Red}t_{0}=0}$ ，得到公式组 ${\color{Red}B}$ 中的三式和四式
$\\{\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.$
$\bullet$ 将求解得到的公式组 ${\color{Red}B}$ 带入到公式组 ${\color{Red}A}$ 中，求解得到未知变量 ${\color{Red}a_{0}、a_{1}、a_{2}、a_{3}、a_{4}、a_{5 }}$ 如公式组 ${\color{Red}C}$ 所示
$\\{\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.$
五项式插值法的公式推导到这里就结束了，之后就可以结合代码来实现五项式插值法，最终通过树莓派的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
$$

Cherishalfy

关注

14
点赞
踩
94

收藏

觉得还不错? 一键收藏
5
评论
Python实现五项式插值法实现机械臂运动轨迹规划

在树莓派中使用Python实现五项式插值法，控制GPIO管脚输出PWM实现对机械臂的控制。五项式插值法常用于机械臂运动轨迹规划中，可以实现对舵机角度、角速度和叫加速度进行约束，使舵机运行更加平稳，同时也减少了机械臂在运动过程中的抖动情况，可以延长机械臂使用寿命。一、实现效果图二、五项式插值法推导公式三、代码实现import matplotlib.pyplot as pltimport numpy as npimport math#q_array=[[0,50,150,100,40
复制链接

扫一扫