正交多项式曲线拟合(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');

正交多项式曲线拟合(MATLAB代码)_第1张图片

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');

正交多项式曲线拟合(MATLAB代码)_第2张图片

二、参考文献

Trajectory Planning for Automatic Machines and Robots中章节:4.2 Orthogonal Polynomials

你可能感兴趣的:(插值与拟合)