正交多项式曲线拟合(MATLAB代码)
文章目录
- 一、 正交多项式曲线拟合
-
- 1.曲线不经过起点与终点
- 2.曲线经过起点与终点
- 二、参考文献
一、 正交多项式曲线拟合
1.曲线不经过起点与终点
%{
Function: calculate_orthogonal_polynomial_params
Description: 利用递推关系计算正交多项式参数alpha，beta(曲线不经过起点与终点)
Input: 点序列(ti, qi)的横坐标序列t，多项式次数m
Output: 正交多项式参数alpha，beta
Author: Marc Pony(marc_pony@163.com)
%}
function [alpha, beta] = calculate_orthogonal_polynomial_params(t, m)
if m == 0
alpha = [];
beta = [];
return;
end
if m == 1
p = ones(size(t));
alpha = sum(t .* p.^2) / sum(p.^2);
beta = [];
return;
end
alpha = zeros(m, 1);
beta = zeros(m - 1, 1);
p0 = ones(size(t));
alpha(1) = sum(t .* p0.^2) / sum(p0.^2);
p1 = (t - alpha(1)) .* p0;
for i = 2 : m
alpha(i) = sum(t .* p1.^2) / sum(p1.^2);
beta(i - 1) = sum(p1.^2) / sum(p0.^2);
p2 = (t - alpha(i)) .* p1 - beta(i - 1) * p0;
p0 = p1;
p1 = p2;
end
end
%{
Function: evaluate_orthogonal_polynomial
Description: 利用递推关系计算正交多项式pm在t处自0阶到2阶导数值(曲线不经过起点与终点)
Input: 横坐标t，多项式次数m，正交多项式参数alpha与beta
Output: 正交多项式pm在t处自0阶到2阶导数值p, dp, ddp
Author: Marc Pony(marc_pony@163.com)
%}
function [p, dp, ddp] = evaluate_orthogonal_polynomial(t, m, alpha, beta)
p0 = 1.0;
dp0 = 0.0;
ddp0 = 0.0;
p = p0;
dp = dp0;
ddp = ddp0;
if m == 0
return;
end
p1 = (t - alpha(1)) * p0;
dp1 = p0 + (t - alpha(1)) * dp0;
ddp1 = 2.0 * dp0 + (t - alpha(1)) * ddp0;
p = p1;
dp = dp1;
ddp = ddp1;
if m == 1
return;
end
for i = 2 : m
p2 = (t - alpha(i)) * p1 - beta(i - 1) * p0;
dp2 = p1 + (t - alpha(i)) * dp1 - beta(i - 1) * dp0;
ddp2 = 2.0 * dp1 + (t - alpha(i)) * ddp1 - beta(i - 1) * ddp0;
p0 = p1;
dp0 = dp1;
ddp0 = ddp1;
p1 = p2;
dp1 = dp2;
ddp1 = ddp2;
end
p = p2;
dp = dp2;
ddp = ddp2;
end
%{
Function: evaluate_orthogonal_polynomial_and_derivatives
Description: 利用递推关系计算正交多项式p0~pm在t处自0阶到2阶导数值(曲线不经过起点与终点)
Input: 横坐标t，多项式次数m，正交多项式参数alpha与beta
Output: 正交多项式p0~pm在t处自0阶到2阶导数值p, dp, ddp
Author: Marc Pony(marc_pony@163.com)
%}
function [p, dp, ddp] = evaluate_orthogonal_polynomial_and_derivatives(t, m, alpha, beta)
p = zeros(m + 1, 1);
dp = zeros(m + 1, 1);
ddp = zeros(m + 1, 1);
p0 = 1.0;
dp0 = 0.0;
ddp0 = 0.0;
p(1) = p0;
dp(1) = dp0;
ddp(1) = ddp0;
if m == 0
return;
end
p1 = (t - alpha(1)) * p0;
dp1 = p0 + (t - alpha(1)) * dp0;
ddp1 = 2.0 * dp0 + (t - alpha(1)) * ddp0;
p(2) = p1;
dp(2) = dp1;
ddp(2) = ddp1;
if m == 1
return;
end
for i = 2 : m
p2 = (t - alpha(i)) * p1 - beta(i - 1) * p0;
dp2 = p1 + (t - alpha(i)) * dp1 - beta(i - 1) * dp0;
ddp2 = 2.0 * dp1 + (t - alpha(i)) * ddp1 - beta(i - 1) * ddp0;
p(i + 1) = p2;
dp(i + 1) = dp2;
ddp(i + 1) = ddp2;
p0 = p1;
dp0 = dp1;
ddp0 = ddp1;
p1 = p2;
dp1 = dp2;
ddp1 = ddp2;
end
end
clc;
clear;
close all;
t = [0, 1, 3, 7, 8, 10]'; %单调递增时间序列
q = [2, 3, 5, 6, 8, 9]';
m = 4; %多项式阶数（非负整数)
dt = 0.001;
n = length(t);
[alpha, beta] = calculate_orthogonal_polynomial_params(t, m);
a = zeros(m + 1, 1);
for j = 0 : m
sumValue = zeros(2, 1);
for k = 1 : n
[pj, ~, ~] = evaluate_orthogonal_polynomial(t(k), j, alpha, beta);
sumValue(1) = sumValue(1) + q(k) * pj;
sumValue(2) = sumValue(2) + pj^2;
end
if abs(sumValue(2)) > 1.0e-20
a(j + 1) = sumValue(1) / sumValue(2);
else
a(j + 1) = 0.0
end
end
posArray = [];
velArray = [];
accArray = [];
tArray = (t(1) : dt : t(end))';
for i = 1 : length(tArray)
tt = tArray(i);
[p, dp, ddp] = evaluate_orthogonal_polynomial_and_derivatives(tt, m, alpha, beta);
pos = dot(a, p);
vel = dot(a, dp);
acc = dot(a, ddp);
posArray = [posArray; pos];
velArray = [velArray; vel];
accArray = [accArray; acc];
end
if abs(tArray(end) - t(end)) > 1.0e-8
[p, dp, ddp] = evaluate_orthogonal_polynomial_and_derivatives(t(end), m, alpha, beta);
pos = dot(a, p);
vel = dot(a, dp);
acc = dot(a, ddp);
tArray = [tArray; t(end)];
posArray = [posArray; pos];
velArray = [velArray; vel];
accArray = [accArray; acc];
end
figure(1)
subplot(3, 1, 1)
plot(tArray, posArray);
hold on
plot(t, q, '+');
xlabel('t');
ylabel('Position');
subplot(3, 1, 2)
plot(tArray, velArray);
xlabel('t');
ylabel('Velocity');
subplot(3, 1, 3)
plot(tArray, accArray);
xlabel('t');
ylabel('Acceleration');
2.曲线经过起点与终点
%{
Function: calculate_orthogonal_polynomial_params2
Description: 利用递推关系计算正交多项式参数alpha，beta(曲线经过起点与终点)
Input: 归一化后的点序列横坐标tau，多项式次数m
Output: 正交多项式参数alpha，beta
Author: Marc Pony(marc_pony@163.com)
%}
function [alpha, beta] = calculate_orthogonal_polynomial_params2(tau, m)
if m == 0
alpha = [];
beta = [];
return;
end
if m == 1
p = tau .* (1.0 - tau);
alpha = sum(tau .* p.^2) / sum(p.^2);
beta = [];
return;
end
alpha = zeros(m, 1);
beta = zeros(m - 1, 1);
p0 = tau .* (1.0 - tau);
alpha(1) = sum(tau .* p0.^2) / sum(p0.^2);
p1 = (tau - alpha(1)) .* p0;
for i = 2 : m
alpha(i) = sum(tau .* p1.^2) / sum(p1.^2);
beta(i - 1) = sum(p1.^2) / sum(p0.^2);
p2 = (tau - alpha(i)) .* p1 - beta(i - 1) * p0;
p0 = p1;
p1 = p2;
end
end
%{
Function: evaluate_orthogonal_polynomial2
Description: 利用递推关系计算正交多项式pm在t处自0阶到2阶导数值(曲线经过起点与终点)
Input: 归一化横坐标tau，多项式次数m，正交多项式参数alpha与beta
Output: 正交多项式pm在t处自0阶到2阶导数值p, dp, ddp
Author: Marc Pony(marc_pony@163.com)
%}
function [p, dp, ddp] = evaluate_orthogonal_polynomial2(tau, m, alpha, beta)
p0 = tau * (1.0 - tau);
dp0 = 1.0 - 2.0 * tau;
ddp0 = -2.0;
p = p0;
dp = dp0;
ddp = ddp0;
if m == 0
return;
end
p1 = (tau - alpha(1)) * p0;
dp1 = p0 + (tau - alpha(1)) * dp0;
ddp1 = 2.0 * dp0 + (tau - alpha(1)) * ddp0;
p = p1;
dp = dp1;
ddp = ddp1;
if m == 1
return;
end
for i = 2 : m
p2 = (tau - alpha(i)) * p1 - beta(i - 1) * p0;
dp2 = p1 + (tau - alpha(i)) * dp1 - beta(i - 1) * dp0;
ddp2 = 2.0 * dp1 + (tau - alpha(i)) * ddp1 - beta(i - 1) * ddp0;
p0 = p1;
dp0 = dp1;
ddp0 = ddp1;
p1 = p2;
dp1 = dp2;
ddp1 = ddp2;
end
p = p2;
dp = dp2;
ddp = ddp2;
end
%{
Function: evaluate_orthogonal_polynomial_and_derivatives2
Description: 利用递推关系计算正交多项式p0~pm在t处自0阶到2阶导数值(曲线经过起点与终点)
Input: 归一化横坐标tau，多项式次数m，正交多项式参数alpha与beta
Output: 正交多项式p0~pm在t处自0阶到2阶导数值p, dp, ddp
Author: Marc Pony(marc_pony@163.com)
%}
function [p, dp, ddp] = evaluate_orthogonal_polynomial_and_derivatives2(tau, m, alpha, beta)
p = zeros(m + 1, 1);
dp = zeros(m + 1, 1);
ddp = zeros(m + 1, 1);
p0 = tau * (1.0 - tau);
dp0 = 1.0 - 2.0 * tau;
ddp0 = -2.0;
p(1) = p0;
dp(1) = dp0;
ddp(1) = ddp0;
if m == 0
return;
end
p1 = (tau - alpha(1)) * p0;
dp1 = p0 + (tau - alpha(1)) * dp0;
ddp1 = 2.0 * dp0 + (tau - alpha(1)) * ddp0;
p(2) = p1;
dp(2) = dp1;
ddp(2) = ddp1;
if m == 1
return;
end
for i = 2 : m
p2 = (tau - alpha(i)) * p1 - beta(i - 1) * p0;
dp2 = p1 + (tau - alpha(i)) * dp1 - beta(i - 1) * dp0;
ddp2 = 2.0 * dp1 + (tau - alpha(i)) * ddp1 - beta(i - 1) * ddp0;
p(i + 1) = p2;
dp(i + 1) = dp2;
ddp(i + 1) = ddp2;
p0 = p1;
dp0 = dp1;
ddp0 = ddp1;
p1 = p2;
dp1 = dp2;
ddp1 = ddp2;
end
end
clc;
clear;
close all;
t = [0, 1, 3, 7, 8, 10]'; %单调递增时间序列
q = [2, 3, 5, 6, 8, 9]';
m = 4; %多项式阶数（非负整数)
dt = 0.001;
n = length(t);
tau = (t - t(1)) / (t(n) - t(1));
q_ = q - q(1) * (1 - tau) - q(n) * tau;
[alpha, beta] = calculate_orthogonal_polynomial_params2(tau, m);
a = zeros(m + 1, 1);
for j = 0 : m
sumValue = zeros(2, 1);
for k = 1 : n
[pj, ~, ~] = evaluate_orthogonal_polynomial2(tau(k), j, alpha, beta);
sumValue(1) = sumValue(1) + q_(k) * pj;
sumValue(2) = sumValue(2) + pj^2;
end
if abs(sumValue(2)) > 1.0e-20
a(j + 1) = sumValue(1) / sumValue(2);
else
a(j + 1) = 0.0;
end
end
posArray = [];
velArray = [];
accArray = [];
tArray = (t(1) : dt : t(end))';
for i = 1 : length(tArray)
tau = (tArray(i) - t(1)) / (t(n) - t(1));
[p, dp, ddp] = evaluate_orthogonal_polynomial_and_derivatives2(tau, m, alpha, beta);
pos = dot(a, p) + q(1) * (1.0 - tau^2) + q(n) * tau^2;
vel = dot(a, dp) + 2.0 * tau * (q(n) - q(1));
acc = dot(a, ddp) + 2.0 * (q(n) - q(1));
posArray = [posArray; pos];
velArray = [velArray; vel];
accArray = [accArray; acc];
end
if abs(tArray(end) - t(end)) > 1.0e-8
[p, dp, ddp] = evaluate_orthogonal_polynomial_and_derivatives2(1.0, m, alpha, beta);
pos = dot(a, p) + q(n);
vel = dot(a, dp) + 2.0 * (q(n) - q(1));
acc = dot(a, ddp) + 2.0 * (q(n) - q(1));
tArray = [tArray; t(end)];
posArray = [posArray; pos];
velArray = [velArray; vel];
accArray = [accArray; acc];
end
figure(1)
subplot(3, 1, 1)
plot(tArray, posArray);
hold on
plot(t, q, '+');
xlabel('t');
ylabel('Position');
subplot(3, 1, 2)
plot(tArray, velArray);
xlabel('t');
ylabel('Velocity');
subplot(3, 1, 3)
plot(tArray, accArray);
xlabel('t');
ylabel('Acceleration');
二、参考文献
Trajectory Planning for Automatic Machines and Robots中章节：4.2 Orthogonal Polynomials