正交多项式曲线拟合(MATLAB代码)

一、 正交多项式曲线拟合

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