轨迹规划——带抛物线过渡的直线插补

唐豆豆*

于 2025-03-23 18:28:59 发布

阅读量1k

点赞数 30

分类专栏：双臂机器人项目文章标签：人工智能

本文链接：https://blog.csdn.net/doudou2weiwei/article/details/146457411

版权

双臂机器人项目专栏收录该内容

8 篇文章

订阅专栏

一、引言

笛卡尔轨迹规划的大体流程如下：

所以，我们规划的关键内容就是如何在起始位置和姿态中进行插值。

常用的方法有直线插补、圆弧插补、样条插补、多项式插补等。

1）常规直线插补

由于机械臂在实际应用中会存在需要它走直线完成指令的过程，所以直线插补必不可少。常规的直线插补就是直接按照时间进行插值，具体流程如下：

首先算出起点和终点的直线距离——然后计算插补步数——再算出各个插值点的位姿——最后计算各点的逆运动学进行检查

这个插补一般是按照时间进行插补的，常用的插补时间为0.25ms~0.4ms之间。利用时间插补时，插补步数。用这个公式可以快速计算出中间需要多少个插值点。

具体插补点坐标计算公式如下：

这样插值是非常简单的，也很好理解，但会有一个显著的不足。如果机械臂的轨迹由多条直线组成，那就会造成速度突变，速度突变某种意义上就意味着加速度在这里是接近无限大的。所以一般工程不会直接用这种直线插补。而会采用带二次多项式过渡的直线插补策略。

2）添加过渡的优势

上图的最左边就是只用直线插补的方法，这个时候速度突然就做出改变，加速度接近无穷。从电机的使用角度上来看，我们一般是希望避免这种情况的。中间这个图，在起点和终点处，分别插入了一段圆弧，这个时候，速度经历了一小段时间才达到直线段的速度，所以这里加速度虽然发生了突变，但是，加速度是个定值，相对而言运动轨迹就会更加平滑。右边这个图对应的是采用最小加速度进行插补的结果，我们可以看到最小加速度对应的加速时间就是整个行程时间的一半。

二、过渡方法

那具体怎么做过渡呢？其实就是在两段直线段处，添加一段抛物线（二次多项式），这个时候，速度就可以连续变化了。

我们可以一起看一下这个抛物线的设定，这个抛物线段，本质就是做一个等加速运动，将速度从v（0）变成v（end），所以可以用到以下的通用表达式。

等加速运动的位移表达式如下：

其中，加速度可以采用如下方式计算：

那具体放到我们的路径规划里面，就需要进一步做一些设定，假定我们现在已知4个点的坐标，那插值的时候就有4段抛物线，3段直线段。如下图所示：

这里可以用以下数组表示：

位置点：[θ1，θ2，θ3，θ4]
时间点：[0, t1, t2, t3, t4]
速度：[0, v1, v2,v3, 0] 这里补充了起始速度和结束速度为0。
v1=（θ2-θ1）/(t2-t1)；v2=（θ3-θ2）/(t3-t2)；v3=（θ4-θ3）/(t4-t3)

我们要的是这个插值位置与时间的关系，那就需要分直线段和抛物线段来讨论。

直线段：

抛物线段：

起始段和最后段的抛物线表达式与上述表达式一致，但是定义域略有差别。但从上述的图像可以看出，这样插值会有一个特点，就是路径点不在轨迹上，如果机械臂没有要求必须经过路径点，这个不影响，但是如果机械臂要求必须经过路径点，那么就需要略加修改。

在路径点的附近再添加上两个点作为路径点，然后让设定的路径点位于直线路径上就可以了。此时路径点肯定会位于轨迹上。因为直线上的点是必然会通过的。

三、代码

import numpy as np
import matplotlib.pyplot as plt

def count_vec(t0, t1, x0, x1):
    V = (x1-x0) / (t1-t0)
    return V

def count_a(v0, v1, detat):
    a = (v1-v0)/detat
    return a

def linear( t, x, v, deltat, num_point1,numpoint2):
    q_total=[]
    t_total=[]
    for i in range(len(x)):
        if i == 0:
            # 第一段只有二次段
            t_dot = np.linspace(t[i],t[i+1]+deltat/2,num_point1)
            a=(v[i+1]-v[i])/deltat
            print(a)
            q = x[i] + 0.5 *a *(t_dot-t[i+1]+deltat/2)**2
            q_total.append(q)
            t_total.append(t_dot)
        else:
            # 其余段是一个直线段+一个二次段组合而成
            # 直线段插值
            t_dot1 = np.linspace(t[i]+deltat/2,t[i+1]-deltat/2,num_point2)
            q1=x[i-1] + v[i] * (t_dot1 - t[i]) # 直线段
            q_total.append(q1)
            t_total.append(t_dot1)
            # 二次段插值
            if i==len(x)-1:
                t_dot2 = np.linspace(t[i+1]-deltat/2,t[i+1],num_point1) # 最后一段
            else:
                t_dot2 = np.linspace(t[i+1]-deltat/2,t[i+1]+deltat/2,num_point1) # 中间段
            a2=(v[i+1]-v[i])/deltat
            q2 = x[i-1]+v[i]*(t_dot2-t[i])+0.5 * a2 *(t_dot2 - t[i+1] + 0.5*deltat)**2
            q_total.append(q2)
            t_total.append(t_dot2)

    q_total= np.concatenate(q_total)
    t_total=np.concatenate(t_total)
    return q_total, t_total
            

t = np.array([0, 1, 15, 20])
x = np.array([2, 4, 6])
# 计算速度
v=[0]

for i in range(len(x)-1):
    v1 = count_vec(t[i+1],t[i+2],x[i],x[i+1])
    v.append(v1)
v.append(0)
print(v)

deltat = 2
num_point1 = 100
num_point2 = 500

# 调用函数
q_total, t_total= linear(t, x, v, deltat, num_point1, num_point2)

#print(len(q_total))
#print(len(t_total))
# 打印结果
#for q in q_total:
#    print(q)

plt.figure(figsize=(10, 6))
plt.plot(t_total, q_total, label='Interpolated Points')
plt.scatter(t[1:], x, color='red', label='Data Points')
plt.xlabel('Time')
plt.ylabel('Position')
plt.title('Linear Interpolation with Quadratic Transition')
plt.legend()
plt.grid(True)
plt.show()