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

其他 2020-10-30 11:25:34 阅读次数: 0

文章目录

一、 正交多项式曲线拟合

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

猜你喜欢

转载自blog.csdn.net/maple_2014/article/details/106753260
今日推荐
美国拟限制 AI 大模型出口中国和俄罗斯
苹果将与 OpenAI 达成协议，将 ChatGPT 应用于 iPhone
openKylin 社区生态委员会第六次会议圆满召开
阿里云正式发布通义千问 2.5
Python 3.13 发布首个 Beta：实验性自由线程模式和 JIT、改进交互式解释器
Stack Overflow 拿我的代码去训练 AI 大模型，还封了我的账号
Pop!_OS 的 COSMIC 桌面完成 App Store 上架工作
报告：Django 仍然是 74% 开发者的首选
《2024 年一季度互联网投融资运行情况》研究报告
15 年前上了“FFmpeg 耻辱柱”，今天他还得谢谢咱——腾讯QQPlayer一雪前耻？
TIOBE 5 月榜单：Fortran “复活”进入 Top 10
GCC 14.1 发布
周排行
curl的POST请求，封装方法
8.1.1. Integer Types
Java基础 Day05(个人复习整理)
Python - Django - 中间件 process_exception
小L的试卷
【Shell编程】 （函数）判断用户是否存在
python(css样式)
spring ant path 匹配原则 - 【笔记】
《JavaScript与JScript从入门到精通》(美)James.Jaworski.中译本.扫描版.pdf
Eclipse运行带参数的java程序
每日归档
更多
2024-05-12(0)
2024-05-11(38)
2024-05-10(38)
2024-05-09(35)
2024-05-08(42)
2024-05-07(14)
2024-05-06(40)
2024-05-05(0)
2024-05-04(7)
2024-05-03(19)

代码天地 © 2018 codetd.com

境外服务器   最新电视剧2022