使用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+c1t+c2t2+c3t3+c4t4+c5t5θ˙(t)=c1+2c2t+3c3t2+4c4t3+5c5t4θ¨(t)=2c2+6c3t+12c4t2+20c5t3
给定参数
一般而言，机器人从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+c1T+c2T2+c3T3+c4T4+c5T5θ˙(T)=c1+2c2T+3c3T2+4c4T3+5c5T4θ¨(T)=2c2+6c3T+12c4T2+20c5T3
[
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}
⎣⎢⎢⎢⎢⎢⎢⎡100100010T10002T22T2000T33T26T000T44T312T2000T55T420T3⎦⎥⎥⎥⎥⎥⎥⎤6×6∗⎣⎢⎢⎢⎢⎢⎢⎡c0c1c2c3c4c5⎦⎥⎥⎥⎥⎥⎥⎤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}
⎣⎢⎢⎢⎢⎢⎢⎡c0c1c2c3c4c5⎦⎥⎥⎥⎥⎥⎥⎤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
2558
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
