4-3-4分段多项式插值(MATLAB实现)_4-3-3-3-4多项式插值函数

一、 通用的4-3-4分段多项式插值，支持三段或大于三段插值，起始速度和加速度、结束速度和加速度可设(MATLAB代码)

%{
Function: four_three_four_piecewise_polynomial
Description: 计算4-3-4分段多项式的系数
Input: 向量t(递增序列), 向量p, 起始速度vs, 结束速度ve, 起始加速度as, 结束加速度ae, 向量维数n
Output: 4-3-4分段多项式的系数a
Author: Marc Pony(marc_pony@163.com)
%}
function a = four_three_four_piecewise_polynomial(t, p, vs, ve, as, ae, n)

if n < 4
    error('向量维数小于4!');
end

segmentCount = n - 1;
parameterCount = 5 + 4 * (segmentCount - 2) + 5;
a = zeros(segmentCount, 5);
A = zeros(parameterCount, parameterCount);
B = zeros(parameterCount, 1);

%% （1）起点约束
%位置
A(1, 1) = 1.0;
A(1, 2) = t(1);
A(1, 3) = t(1)^2;
A(1, 4) = t(1)^3;
A(1, 5) = t(1)^4;
B(1) = p(1);

%速度
A(2, 1) = 0.0;
A(2, 2) = 1.0;
A(2, 3) = 2.0 * t(1);
A(2, 4) = 3.0 * t(1)^2;
A(2, 5) = 4.0 * t(1)^3;
B(2) = vs;

%加速度
A(3, 1) = 0.0;
A(3, 2) = 0.0;
A(3, 3) = 2.0;
A(3, 4) = 6.0 * t(1);
A(3, 5) = 12.0 * t(1)^2;
B(3) = as;

%% （2）第一二段连接处约束
%位置
A(4, 1) = 1.0;
A(4, 2) = t(2);
A(4, 3) = t(2)^2;
A(4, 4) = t(2)^3;
A(4, 5) = t(2)^4;
B(4) = p(2);

A(5, 6) = 1.0;
A(5, 7) = t(2);
A(5, 8) = t(2)^2;
A(5, 9) = t(2)^3;
B(5) = p(2);

%速度
A(6, 1) = 0.0;
A(6, 2) = 1.0;
A(6, 3) = 2.0 * t(2);
A(6, 4) = 3.0 * t(2)^2;
A(6, 5) = 4.0 * t(2)^3;
A(6, 6) = 0.0;
A(6, 7) = -1.0;
A(6, 8) = -2.0 * t(2);
A(6, 9) = -3.0 * t(2)^2;
B(6) = 0.0;

%加速度
A(7, 1) = 0.0;
A(7, 2) = 0.0;
A(7, 3) = 2.0;
A(7, 4) = 6.0 * t(2);
A(7, 5) = 12.0 * t(2)^2;
A(7, 6) = 0.0;
A(7, 7) = 0.0;
A(7, 8) = -2.0;
A(7, 9) = -6.0 * t(2);
B(7) = 0.0;

%% （3）中间三次多项式连接处约束
i = 8;
j = 6;
k = 3;
for m = 1 : segmentCount - 3
    %位置
    A(i, j) = 1.0;
    A(i, j + 1) = t(k);
    A(i, j + 2) = t(k)^2;
    A(i, j + 3) = t(k)^3;
    B(i) = p(k);
    
    A(i + 1, j + 4) = 1.0;
    A(i + 1, j + 5) = t(k);
    A(i + 1, j + 6) = t(k)^2;
    A(i + 1, j + 7) = t(k)^3;
    B(i + 1) = p(k);
    
    %速度
    A(i + 2, j) = 0.0;
    A(i + 2, j + 1) = 1.0;
    A(i + 2, j + 2) = 2.0 * t(k);
    A(i + 2, j + 3) = 3.0 * t(k)^2;
    A(i + 2, j + 4) = 0.0;
    A(i + 2, j + 5) = -1.0;
    A(i + 2, j + 6) = -2.0 * t(k);
    A(i + 2, j + 7) = -3.0 * t(k)^2;
    B(i + 2) = 0.0;
    
    %加速度
    A(i + 3, j) = 0.0;
    A(i + 3, j + 1) = 0.0;
    A(i + 3, j + 2) = 2.0;
    A(i + 3, j + 3) = 6.0 * t(k);
    A(i + 3, j + 4) = 0.0;
    A(i + 3, j + 5) = 0.0;
    A(i + 3, j + 6) = -2.0;
    A(i + 3, j + 7) = -6.0 * t(k);
    B(i + 3) = 0.0;
    
    k = k + 1;
    i = i + 4;
    j = j + 4;
end

%% （4）最后两段连接处约束
%位置
i = 8 + 4 * (segmentCount - 3);
j = 6 + 4 * (segmentCount - 3);
A(i, j) = 1.0;
A(i, j + 1) = t(n - 1);
A(i, j + 2) = t(n - 1)^2;
A(i, j + 3) = t(n - 1)^3;
B(i) = p(n - 1);

A(i + 1, j + 4) = 1.0;
A(i + 1, j + 5) = t(n - 1);
A(i + 1, j + 6) = t(n - 1)^2;
A(i + 1, j + 7) = t(n - 1)^3;
A(i + 1, j + 8) = t(n - 1)^4;
B(i + 1) = p(n - 1);

%速度
A(i + 2, j) = 0.0;
A(i + 2, j + 1) = 1.0;
A(i + 2, j + 2) = 2.0 * t(n - 1);
A(i + 2, j + 3) = 3.0 * t(n - 1)^2;
A(i + 2, j + 4) = 0.0;
A(i + 2, j + 5) = -1.0;
A(i + 2, j + 6) = -2.0 * t(n - 1);
A(i + 2, j + 7) = -3.0 * t(n - 1)^2;
A(i + 2, j + 8) = -4.0 * t(n - 1)^3;
B(i + 2) = 0.0;

%加速度
A(i + 3, j) = 0.0;
A(i + 3, j + 1) = 0.0;
A(i + 3, j + 2) = 2.0;
A(i + 3, j + 3) = 6.0 * t(n - 1);
A(i + 3, j + 4) = 0.0;
A(i + 3, j + 5) = 0.0;
A(i + 3, j + 6) = -2.0;
A(i + 3, j + 7) = -6.0 * t(n - 1);
A(i + 3, j + 8) = -12.0 * t(n - 1)^2;
B(i + 3) = 0.0;


%% （5）终点约束
%位置
A(parameterCount - 2, parameterCount - 4) = 1.0;
A(parameterCount - 2, parameterCount - 3) = t(n);
A(parameterCount - 2, parameterCount - 2) = t(n)^2;
A(parameterCount - 2, parameterCount - 1) = t(n)^3;
A(parameterCount - 2, parameterCount) = t(n)^4;
B(parameterCount - 2) = p(n);

%速度
A(parameterCount - 1, parameterCount - 4) = 0.0;
A(parameterCount - 1, parameterCount - 3) = 1.0;
A(parameterCount - 1, parameterCount - 2) = 2.0 * t(n);
A(parameterCount - 1, parameterCount - 1) = 3.0 * t(n)^2;
A(parameterCount - 1, parameterCount) = 4.0 * t(n)^3;
B(parameterCount - 1) = ve;

%加速度
A(parameterCount, parameterCount - 4) = 0.0;
A(parameterCount, parameterCount - 3) = 0.0;
A(parameterCount, parameterCount - 2) = 2.0;
A(parameterCount, parameterCount - 1) = 6.0 * t(n);
A(parameterCount, parameterCount) = 12.0 * t(n)^2;
B(parameterCount) = ae;

%% 计算4-3-4分段多项式的系数
x = A \ B;
a(1, :) = x(1 : 5);
k = 6;
for i = 2 : segmentCount - 1
    a(i, 1 : 4) = x(k : k + 3);
    k = k + 4;
end
a(segmentCount, :) = x(parameterCount - 4 : parameterCount);

end
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206

clc;
clear;
close all;

n = 10; %插值点数(n >= 4)
t = linspace(0, 1, n); %递增时间序列
p = 10.0 * t.^3 - 15.0 * t.^4 + 6.0 * t.^5;
vs = 0; %起始速度
ve = 0; %结束速度
as = 0; %起始加速度
ae = 0; %结束加速度
deltaT = 0.001; %插补周期

a = four_three_four_piecewise_polynomial(t, p, vs, ve, as,ae, n);

time = [];
pos = [];
vel = [];
acc = [];
time = [time; t(1)];
pos = [pos; p(1)];
vel = [vel; vs];
acc = [acc; as];
for i = 1 : n - 1
    tt = (t(i) + deltaT : deltaT : t(i + 1))';
    s = a(i, 1) + tt .* (a(i, 2) + tt .* (a(i, 3) + tt .* (a(i, 4) + a(i, 5) .* tt)));
    ds = a(i, 2) + tt .* (2.0 * a(i, 3) + tt .* (3.0 * a(i, 4) + 4.0 * a(i, 5) .* tt));
    dds = 2.0 * a(i, 3) + tt .* (6.0 * a(i, 4) + 12.0 * a(i, 5) .* tt);
    time = [time; tt];
    pos = [pos; s];
    vel = [vel; ds];
    acc = [acc; dds];
end
if abs(time(end) - t(n)) > 1.0e-8
    time = [time; t(n)];
    pos = [pos; p(n)];
    vel = [vel; ve];
    acc = [acc; ae];
end

figure(1)
plot(time, pos);
hold on
plot(t, p, 'ro');
xlabel('t')
ylabel('pos')

figure(2)
plot(time, vel);
hold on
plot([t(1), t(n)], [vs, ve], 'ro');
xlabel('t')
ylabel('vel')

figure(3)
plot(time, acc);
hold on
plot([t(1), t(n)], [as, ae], 'ro');
xlabel('t')
ylabel('acc')
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60

二、 参考文献

Trajectory Planning for Automatic Machines and Robots中章节：3.6 Piecewise Polynomial Trajectory