机器人中的轨迹规划（Trajectory Planning ）

最新推荐文章于 2024-06-30 15:22:08 发布

weixin_33924312

最新推荐文章于 2024-06-30 15:22:08 发布

阅读量3.3k

点赞数 2

文章标签： matlab

本文介绍了机器人轨迹规划中的两种方法：五次多项式曲线和梯形速度曲线。五次多项式曲线通过边界条件求解线性方程组，保证了连续光滑性，但在速度利用率上存在不足。而梯形速度曲线采用抛物线拼接直线段，形成速度梯形，被广泛应用于电机驱动。MATLAB机器人工具箱提供了tpoly和lspb函数来生成这两种轨迹。

摘要由CSDN通过智能技术生成

Figure. Several possible path shapes for a single joint

五次多项式曲线(quintic polynomial)

$$\theta(t)=a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5$$

　　考虑边界条件：

$$\begin{align*}
\theta_0&=a_0\\
\theta_f&=a_0+a_1t+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_2t_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{align*}$$

　　这6组约束构成了一个6个未知数的线性方程组，可以求出系数为：

$$\begin{align*}
a_0&=\theta_0\\
a_1&=\dot{\theta_0}\\
a_2&=\frac{\ddot{\theta_0}}{2}\\
a_3&=\frac{20\theta_f-20\theta_0-(8\dot{\theta_f}+12\dot{\theta_0})t_f-(3\ddot{\theta_0}-\ddot{\theta_f}){t_f}^2}{2{t_f}^3}\\
a_4&=\frac{30\theta_0-30\theta_f+(14\dot{\theta_f}+16\dot{\theta_0})t_f+(3\ddot{\theta_0}-2\ddot{\theta_f}){t_f}^2}{2{t_f}^4}\\
a_5&=\frac{12\theta_f-12\theta_0-(6\dot{\theta_f}+6\dot{\theta_0})t_f-(\ddot{\theta_0}-\ddot{\theta_f}){t_f}^2}{2{t_f}^5}
\end{align*}$$

　　在MATLAB机器人工具箱中函数tpoly可以用于计算并生成机器人单轴的五次多项式轨迹曲线。当$t \in [0,T]$时，五次多项式曲线以及其一阶导数、二阶导数都是连续光滑的多项式曲线：

$$\begin{align*}
S(t)&=At^5+Bt^4+Ct^3+Dt^2+Et+F\\
\dot{S}(t)&=5At^4+4Bt^3+3Ct^2+2Dt+E\\
\ddot{S}(t)&=20At^3+12Bt^2+6Ct+2D
\end{align*}$$

　　根据约束条件

　　可以写出矩阵方程如下：

　　利用MATLAB提供的左除（反除）操作符，可以方便的求解线性方程组：Ax=b → x=A\b（表示矩阵A的逆乘以b)

　　 tpoly.m主要内容如下：

%TPOLY Generate scalar polynomial trajectory

% [S,SD,SDD] = TPOLY(S0, SF, T, SD0, SDF) as above but specifies initial 
% and final joint velocity for the trajectory and time vector T.

function [s,sd,sdd] = tpoly(q0, qf, t, qd0, qdf)

    if isscalar(t)
        t = (0:t-1)';
    else
        t = t(:);
    end
    if nargin < 4
        qd0 = 0;
    end
    if nar