使用Matlab求解机器人五次多项式轨迹规划

模型构建

机器人每个关节是独立的，考虑某个关节在运动开始时刻ti的角度为 θ i \theta_i θi​，希望该关节在时刻tf运动到新的角度 θ f \theta_f θf​，使用五次多项式对轨迹进行规划：
θ ( t ) = c 0 + c 1 t + c 2 t 2 + c 3 t 3 + c 4 t 4 + c 5 t 5 θ ˙ ( t ) = c 1 + 2 c 2 t + 3 c 3 t 2 + 4 c 4 t 3 + 5 c 5 t 4 θ ¨ ( t ) = 2 c 2 + 6 c 3 t + 12 c 4 t 2 + 20 c 5 t 3 \theta(t) = c_0+c_1t+c_2 t^2+c_3 t^3+c_4 t^4+c_5 t^5\\ \dot{\theta}(t) = c_1+2c_2t+3c_3 t^2+4c_4 t^3+5c_5 t^4\\ \ddot{\theta}(t) = 2c_2+6c_3t+12c_4t^2+20c_5t^3\\ θ(t)=c0​+c1​t+c2​t2+c3​t3+c4​t4+c5​t5θ˙(t)=c1​+2c2​t+3c3​t2+4c4​t3+5c5​t4θ¨(t)=2c2​+6c3​t+12c4​t2+20c5​t3

给定参数

一般而言，机器人从ti=0时刻运动到tf=T时刻，初始位移为orgAngle，初始速度为orgSpeed，初始加速度为orgAcc，终点位移为tarAngle，终点速度为tarSpeed，终点加速度为tarAcc。那么，则有如下等式：
θ ( 0 ) = c 0 = o r g A n g l e θ ˙ ( 0 ) = c 1 = o r g S p e e d θ ¨ ( 0 ) = 2 c 2 = o r g A c c θ ( T ) = c 0 + c 1 T + c 2 T 2 + c 3 T 3 + c 4 T 4 + c 5 T 5 θ ˙ ( T ) = c 1 + 2 c 2 T + 3 c 3 T 2 + 4 c 4 T 3 + 5 c 5 T 4 θ ¨ ( T ) = 2 c 2 + 6 c 3 T + 12 c 4 T 2 + 20 c 5 T 3 \theta(0) = c_0=orgAngle\\ \dot{\theta}(0) = c_1=orgSpeed\\ \ddot{\theta}(0) = 2c_2=orgAcc\\ \theta(T) = c_0+c_1T+c_2 T^2+c_3T^3+c_4T^4+c_5T^5\\ \dot{\theta}(T) = c_1+2c_2T+3c_3T^2+4c_4T^3+5c_5T^4\\ \ddot{\theta}(T) = 2c_2+6c_3T+12c_4T^2+20c_5T^3\\ θ(0)=c0​=orgAngleθ˙(0)=c1​=orgSpeedθ¨(0)=2c2​=orgAccθ(T)=c0​+c1​T+c2​T2+c3​T3+c4​T4+c5​T5θ˙(T)=c1​+2c2​T+3c3​T2+4c4​T3+5c5​T4θ¨(T)=2c2​+6c3​T+12c4​T2+20c5​T3
[ 1 0 0 0 0 0 0 1 0 0 0 0 0 0 2 0 0 0 1 T T 2 T 3 T 4 T 5 0 1 2 T 3 T 2 4 T 3 5 T 4 0 0 2 6 T 12 T 2 20 T 3 ] 6 × 6 ∗ [ c 0 c 1 c 2 c 3 c 4 c 5 ] 6 × 1 = [ o r g A n g l e o r g S p e e d o r g A c c t a r A n g l e t a r S p e e d t a r A c c ] 6 × 1 \begin{bmatrix} 1 & 0 & 0 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 2 & 0 & 0 & 0 \\ 1 & T & T^2 & T^3 & T^4 & T^5\\ 0 & 1 & 2T & 3T^2 & 4T^3 & 5T^4\\ 0 & 0 & 2 & 6T & 12T^2 & 20T^3\\ \end{bmatrix}_{6 \times 6} * \begin{bmatrix} c_0 \\ c_1 \\ c_2 \\ c_3 \\ c_4 \\ c_5 \\ \end{bmatrix}_{6 \times 1} = \begin{bmatrix} orgAngle \\ orgSpeed \\ orgAcc \\ tarAngle \\ tarSpeed \\ tarAcc \\ \end{bmatrix}_{6 \times 1} ⎣⎢⎢⎢⎢⎢⎢⎡​100100​010T10​002T22T2​000T33T26T​000T44T312T2​000T55T420T3​⎦⎥⎥⎥⎥⎥⎥⎤​6×6​∗⎣⎢⎢⎢⎢⎢⎢⎡​c0​c1​c2​c3​c4​c5​​⎦⎥⎥⎥⎥⎥⎥⎤​6×1​=⎣⎢⎢⎢⎢⎢⎢⎡​orgAngleorgSpeedorgAcctarAngletarSpeedtarAcc​⎦⎥⎥⎥⎥⎥⎥⎤​6×1​

求解

matlab模型建立

使用matlab进行建模求解矩阵方程：

clear;
clc;
syms T;
syms orgAngle orgSpeed orgAcc tarAngle tarSpeed tarAcc;
b=[orgAngle;orgSpeed;orgAcc;tarAngle;tarSpeed;tarAcc];
A=[1 0 0 0 0 0;
    0 1 0 0 0 0;
    0 0 2 0 0 0;
    1 T T.^2 T.^3 T.^4 T.^5;
    0 1 2*T 3*T.^2 4*T.^3 5*T.^4;
    0 0 2 6*T 12*T.^2 20*T.^3];

r = pinv(A)*b
simple_r = simplify(r)

matlab求解

运行如上程序，得出结果：

 
r =
 
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                 -orgAngle*((100*T^2*conj(T)^2 + 225*T*conj(T) + 36)/(T^5*conj(T)^5) - (100*T^2*conj(T)^2 + T^5*conj(T)^5 + 225*T*conj(T) + 36)/(T^5*conj(T)^5))
                                                                                                                                                                                                                                                                                                                                                              orgAcc*((3*(4*T^2*conj(T)^2 + 7*T*conj(T) + 1))/(2*T^3*conj(T)^4) + (3*(6*T^2*conj(T)^2 + 8*T*conj(T) + 1))/(2*T^3*conj(T)^4) - (3*(10*T^2*conj(T)^2 + 15*T*conj(T) + 2))/(2*T^3*conj(T)^4)) + orgAngle*((6*(10*T^2*conj(T)^2 + 20*T*conj(T) + 3))/(T^5*conj(T)^4) + (40*T^2*conj(T)^2 + 105*T*conj(T) + 18)/(T^5*conj(T)^4) - (100*T^2*conj(T)^2 + 225*T*conj(T) + 36)/(T^5*conj(T)^4)) - tarAcc*((4*T^2*conj(T)^2 + 14*T*conj(T) + 3)/(2*T^3*conj(T)^4) - (5*T^2*conj(T)^2 + 15*T*conj(T) + 3)/(T^3*conj(T)^4) + (6*T^2*conj(T)^2 + 16*T*conj(T) + 3)/(2*T^3*conj(T)^4)) + tarSpeed*((16*T^2*conj(T)^2 + 49*T*conj(T) + 9)/(T^4*conj(T)^4) + (24*T^2*conj(T)^2 + 56*T*conj(T) + 9)/(T^4*conj(T)^4) - (40*T^2*conj(T)^2 + 105*T*conj(T) + 18)/(T^4*conj(T)^4)) - tarAngle*((6*(10*T^2*conj(T)^2 + 20*T*conj(T) + 3))/(T^5*conj(T)^4) + (40*T^2*conj(T)^2 + 105*T*conj(T) + 18)/(T^5*conj(T)^4) - (100*T^2*conj(T)^2 + 225*T*conj(T) + 36)/(T^5*conj(T)^4)) + orgSpeed*((24*T^2*conj(T)^2 + 56*T*conj(T) + 9)/(T^4*conj(T)^4) - (6*(10*T^2*conj(T)^2 + 20*T*conj(T) + 3))/(T^4*conj(T)^4) + (36*T^2*conj(T)^2 + T^4*conj(T)^4 + 64*T*conj(T) + 9)/(T^4*conj(T)^4))
 orgSpeed*((3*(6*T^2*conj(T)^2 + 8*T*conj(T) + 1))/(T^4*conj(T)^3) - (6*T^2*conj(T)^2 + 16*T*conj(T) + 3)/(T^4*conj(T)^3) - (6*(10*T^2*conj(T)^2 + 20*T*conj(T) + 3))/(T^4*conj(T)^3) + (2*(24*T^2*conj(T)^2 + 56*T*conj(T) + 9))/(T^4*conj(T)^3)) - orgAngle*((2*(5*T^2*conj(T)^2 + 15*T*conj(T) + 3))/(T^5*conj(T)^3) - (3*(10*T^2*conj(T)^2 + 15*T*conj(T) + 2))/(T^5*conj(T)^3) - (2*(40*T^2*conj(T)^2 + 105*T*conj(T) + 18))/(T^5*conj(T)^3) + (100*T^2*conj(T)^2 + 225*T*conj(T) + 36)/(T^5*conj(T)^3)) + tarAcc*((T^2*conj(T)^2 + 4*T*conj(T) + 1)/(2*T^3*conj(T)^3) - (3*T^2*conj(T)^2 + 6*T*conj(T) + 1)/(2*T^3*conj(T)^3) - (4*T^2*conj(T)^2 + 14*T*conj(T) + 3)/(T^3*conj(T)^3) + (5*T^2*conj(T)^2 + 15*T*conj(T) + 3)/(T^3*conj(T)^3)) + tarSpeed*((3*(4*T^2*conj(T)^2 + 7*T*conj(T) + 1))/(T^4*conj(T)^3) - (4*T^2*conj(T)^2 + 14*T*conj(T) + 3)/(T^4*conj(T)^3) + (2*(16*T^2*conj(T)^2 + 49*T*conj(T) + 9))/(T^4*conj(T)^3) - (40*T^2*conj(T)^2 + 105*T*conj(T) + 18)/(T^4*conj(T)^3)) + tarAngle*((2*(5*T^2*conj(T)^2 + 15*T*conj(T) + 3))/(T^5*conj(T)^3) - (3*(10*T^2*conj(T)^2 + 15*T*conj(T) + 2))/(T^5*conj(T)^3) - (2*(40*T^2*conj(T)^2 + 105*T*conj(T) + 18))/(T^5*conj(T)^3) + (100*T^2*conj(T)^2 + 225*T*conj(T) + 36)/(T^5*conj(T)^3)) - orgAcc*((3*T^2*conj(T)^2 + 6*T*conj(T) + 1)/(2*T^3*conj(T)^3) - (3*(4*T^2*conj(T)^2 + 7*T*conj(T) + 1))/(T^3*conj(T)^3) + (3*(10*T^2*conj(T)^2 + 15*T*conj(T) + 2))/(2*T^3*conj(T)^3) - (9*T^2*conj(T)^2 + T^3*conj(T)^3 + 9*T*conj(T) + 1)/(2*T^3*conj(T)^3))
                                                                                                                                                                                                                                                                                                                                                     tarAcc*((3*(T^2*conj(T)^2 + 4*T*conj(T) + 1))/(2*T^3*conj(T)^2) - (3*(4*T^2*conj(T)^2 + 14*T*conj(T) + 3))/(2*T^3*conj(T)^2) + (5*T^2*conj(T)^2 + 15*T*conj(T) + 3)/(T^3*conj(T)^2)) - orgSpeed*((3*(6*T^2*conj(T)^2 + 16*T*conj(T) + 3))/(T^4*conj(T)^2) + (6*(10*T^2*conj(T)^2 + 20*T*conj(T) + 3))/(T^4*conj(T)^2) - (3*(24*T^2*conj(T)^2 + 56*T*conj(T) + 9))/(T^4*conj(T)^2)) - orgAngle*((6*(5*T^2*conj(T)^2 + 15*T*conj(T) + 3))/(T^5*conj(T)^2) - (3*(40*T^2*conj(T)^2 + 105*T*conj(T) + 18))/(T^5*conj(T)^2) + (100*T^2*conj(T)^2 + 225*T*conj(T) + 36)/(T^5*conj(T)^2)) - orgAcc*((3*(3*T^2*conj(T)^2 + 6*T*conj(T) + 1))/(2*T^3*conj(T)^2) - (9*(4*T^2*conj(T)^2 + 7*T*conj(T) + 1))/(2*T^3*conj(T)^2) + (3*(10*T^2*conj(T)^2 + 15*T*conj(T) + 2))/(2*T^3*conj(T)^2)) - tarSpeed*((3*(4*T^2*conj(T)^2 + 14*T*conj(T) + 3))/(T^4*conj(T)^2) - (3*(16*T^2*conj(T)^2 + 49*T*conj(T) + 9))/(T^4*conj(T)^2) + (40*T^2*conj(T)^2 + 105*T*conj(T) + 18)/(T^4*conj(T)^2)) + tarAngle*((6*(5*T^2*conj(T)^2 + 15*T*conj(T) + 3))/(T^5*conj(T)^2) - (3*(40*T^2*conj(T)^2 + 105*T*conj(T) + 18))/(T^5*conj(T)^2) + (100*T^2*conj(T)^2 + 225*T*conj(T) + 36)/(T^5*conj(T)^2))
                                                                                                                                                                                                                                                                                                                                                                                               tarAcc*((3*(T^2*conj(T)^2 + 4*T*conj(T) + 1))/(T^3*conj(T)) - (2*(4*T^2*conj(T)^2 + 14*T*conj(T) + 3))/(T^3*conj(T)) + (5*T^2*conj(T)^2 + 15*T*conj(T) + 3)/(T^3*conj(T))) - orgSpeed*((6*(6*T^2*conj(T)^2 + 16*T*conj(T) + 3))/(T^4*conj(T)) + (6*(10*T^2*conj(T)^2 + 20*T*conj(T) + 3))/(T^4*conj(T)) - (4*(24*T^2*conj(T)^2 + 56*T*conj(T) + 9))/(T^4*conj(T))) - orgAngle*((12*(5*T^2*conj(T)^2 + 15*T*conj(T) + 3))/(T^5*conj(T)) - (4*(40*T^2*conj(T)^2 + 105*T*conj(T) + 18))/(T^5*conj(T)) + (100*T^2*conj(T)^2 + 225*T*conj(T) + 36)/(T^5*conj(T))) - orgAcc*((3*(3*T^2*conj(T)^2 + 6*T*conj(T) + 1))/(T^3*conj(T)) - (6*(4*T^2*conj(T)^2 + 7*T*conj(T) + 1))/(T^3*conj(T)) + (3*(10*T^2*conj(T)^2 + 15*T*conj(T) + 2))/(2*T^3*conj(T))) - tarSpeed*((6*(4*T^2*conj(T)^2 + 14*T*conj(T) + 3))/(T^4*conj(T)) - (4*(16*T^2*conj(T)^2 + 49*T*conj(T) + 9))/(T^4*conj(T)) + (40*T^2*conj(T)^2 + 105*T*conj(T) + 18)/(T^4*conj(T))) + tarAngle*((12*(5*T^2*conj(T)^2 + 15*T*conj(T) + 3))/(T^5*conj(T)) - (4*(40*T^2*conj(T)^2 + 105*T*conj(T) + 18))/(T^5*conj(T)) + (100*T^2*conj(T)^2 + 225*T*conj(T) + 36)/(T^5*conj(T)))
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      tarAcc*((5*(T^2*conj(T)^2 + 4*T*conj(T) + 1))/T^3 - (5*(4*T^2*conj(T)^2 + 14*T*conj(T) + 3))/(2*T^3) + (5*T^2*conj(T)^2 + 15*T*conj(T) + 3)/T^3) - orgSpeed*((10*(6*T^2*conj(T)^2 + 16*T*conj(T) + 3))/T^4 + (6*(10*T^2*conj(T)^2 + 20*T*conj(T) + 3))/T^4 - (5*(24*T^2*conj(T)^2 + 56*T*conj(T) + 9))/T^4) - orgAngle*((20*(5*T^2*conj(T)^2 + 15*T*conj(T) + 3))/T^5 - (5*(40*T^2*conj(T)^2 + 105*T*conj(T) + 18))/T^5 + (100*T^2*conj(T)^2 + 225*T*conj(T) + 36)/T^5) - orgAcc*((5*(3*T^2*conj(T)^2 + 6*T*conj(T) + 1))/T^3 - (15*(4*T^2*conj(T)^2 + 7*T*conj(T) + 1))/(2*T^3) + (3*(10*T^2*conj(T)^2 + 15*T*conj(T) + 2))/(2*T^3)) - tarSpeed*((10*(4*T^2*conj(T)^2 + 14*T*conj(T) + 3))/T^4 - (5*(16*T^2*conj(T)^2 + 49*T*conj(T) + 9))/T^4 + (40*T^2*conj(T)^2 + 105*T*conj(T) + 18)/T^4) + tarAngle*((20*(5*T^2*conj(T)^2 + 15*T*conj(T) + 3))/T^5 - (5*(40*T^2*conj(T)^2 + 105*T*conj(T) + 18))/T^5 + (100*T^2*conj(T)^2 + 225*T*conj(T) + 36)/T^5)
 
 

simple_r =
 
                                                                                          orgAngle
                                                                                          orgSpeed
                                                                                          orgAcc/2
   -(20*orgAngle - 20*tarAngle + 12*T*orgSpeed + 8*T*tarSpeed + 3*T^2*orgAcc - T^2*tarAcc)/(2*T^3)
 (30*orgAngle - 30*tarAngle + 16*T*orgSpeed + 14*T*tarSpeed + 3*T^2*orgAcc - 2*T^2*tarAcc)/(2*T^4)
      -(12*orgAngle - 12*tarAngle + 6*T*orgSpeed + 6*T*tarSpeed + T^2*orgAcc - T^2*tarAcc)/(2*T^5)

整理可得：
[ c 0 c 1 c 2 c 3 c 4 c 5 ] 6 × 1 = [ o r g A n g l e o r g S p e e d o r g A c c / 2 [ 10 ∗ ( t a r A n g l e − o r g A n g l e ) − ( 4 ∗ t a r S p e e d + 6 ∗ o r g S p e e d ) ∗ T + ( t a r A c c − 3 ∗ o r g A c c ) ∗ T 2 / 2 ] / T 3 ( 15 ∗ ( o r g A n g l e − t a r A n g l e ) + ( 7 ∗ t a r S p e e d + 8 ∗ o r g S p e e d ) ∗ T + ( 3 ∗ o r g A c c − 2 ∗ t a r A c c ) ∗ T 2 / 2 ) / T 4 ( 6 ∗ ( t a r A n g l e − o r g A n g l e ) − 3 ∗ ( t a r S p e e d + o r g S p e e d ) ∗ T + ( t a r A c c − o r g A c c ) ∗ T 2 / 2 ) / T 5 ] 6 × 1 \begin{bmatrix} c_0 \\ c_1 \\ c_2 \\ c_3 \\ c_4 \\ c_5 \\ \end{bmatrix}_{6 \times 1} = \begin{bmatrix} orgAngle \\ orgSpeed \\ orgAcc/2 \\ [10*(tarAngle-orgAngle) - (4*tarSpeed+6*orgSpeed)*T + (tarAcc-3*orgAcc)*T^2/2]/T^3 \\ (15*(orgAngle-tarAngle) + (7*tarSpeed+8*orgSpeed)*T + (3*orgAcc-2*tarAcc)*T^2/2)/T^4 \\ (6*(tarAngle-orgAngle) - 3*(tarSpeed+orgSpeed)*T + (tarAcc-orgAcc)*T^2/2)/T^5 \\ \end{bmatrix}_{6 \times 1} ⎣⎢⎢⎢⎢⎢⎢⎡​c0​c1​c2​c3​c4​c5​​⎦⎥⎥⎥⎥⎥⎥⎤​6×1​=⎣⎢⎢⎢⎢⎢⎢⎡​orgAngleorgSpeedorgAcc/2[10∗(tarAngle−orgAngle)−(4∗tarSpeed+6∗orgSpeed)∗T+(tarAcc−3∗orgAcc)∗T2/2]/T3(15∗(orgAngle−tarAngle)+(7∗tarSpeed+8∗orgSpeed)∗T+(3∗orgAcc−2∗tarAcc)∗T2/2)/T4(6∗(tarAngle−orgAngle)−3∗(tarSpeed+orgSpeed)∗T+(tarAcc−orgAcc)∗T2/2)/T5​⎦⎥⎥⎥⎥⎥⎥⎤​6×1​

# 机器人
Logo
天工造物局

北京人形旗下天工造物具身智能开源社区，聚焦具身天工与慧思开物两大平台

更多推荐

Understand Anything 多智能体知识图谱管线：从代码扫描到可交互 Dashboard 的实现解析

本文以 Understand Anything 的官方文档为主线，系统拆解它如何用一条「Tree-sitter 静态分析 + LLM 语义理解」的多智能体管线，把任意代码库 / 知识库 / 文档转换成可探索、可检索、可问答的知识图谱，并落到一个可交互的 Web Dashboard。读完你应能掌握：如何安装并在 Claude Code / Codex / Cursor / Copilot / Gem

avatar 天工造物局

graphify 视频音视频转写全流程解析：从 detect 结果到知识图谱文档的 Step 2.5

graphify 的核心能力是把任意语料（代码、文档、PDF、图片）转化为可查询的知识图谱，而视频与音频文件无法被直接阅读，必须先把它们转写成文本。本文围绕 `graphify` 运行手册中的转写参考文档 [transcribe.md](https://link.gitcode.com/i/d3285781588e0f1b84886dfdbc4fe7f6)，完整讲解 "Step 2.5 - Tra

avatar 天工造物局

ML-For-Beginners 终章：经典机器学习在金融、医疗、零售等八大行业中的真实落地场景

本篇技术指南基于开源课程仓库 ML-For-Beginners 的收尾课程《Postscript: Machine learning in the real world》（对应仓库目录 [9-Real-World/1-Applications](https://link.gitcode.com/i/6b23002df80861c20abf34d7db63ef41)）编写。该课此前 11 周的教学带

avatar 天工造物局