账单
奇亿娱乐-奇艺账号注册登录官网
销量0件 评价0条 品质0分

奇亿娱乐注册三种不同端点约束下的三次样条拟合

奇艺平台登录 奇亿,奇亿娱乐注册

价格:¥26.00

卖家: 奇亿娱乐-奇艺账号注册登录官网

类型:虚拟

运费:¥0.00

规格: 正在加载...

库存:565

数量:

商品详情 用户评价
奇亿娱乐注册三种不同端点约束下的三次样条拟合
一、推导过程
  给定n + 1 n+1n+1个点( t i , q i ) , i = 0 , 1 , . . . , n (t_i,q_i),i=0,1,...,n(ti,qi),i=0,1,...,n,求解n nn段三次多项式q k ( t ) q_k(t)qk(t),使得拼接而成的三次样条曲线s ( t ) s(t)s(t)尽或许通过一切给定点(起点与结尾一般要求通过),曲线尽或许滑润(曲率或加速度尽或许小),并且二阶接连。三次样条曲线s ( t ) s(t)s(t)界说如下:
{ s ( t ) = { q k ( t ) , t ∈ [ t k , t k + 1 ] , k = 0 , 1 , . . . , n − 1 } q k ( t ) = a k 0 + a k 1 ( t − t k ) + a k 2 ( t − t k ) 2 + a k 3 ( t − t k ) 3 (1)

{s(t)={qk(t),t∈[tk,tk+1],k=0,1,...,n−1}qk(t)=ak0+ak1(t−tk)+ak2(t−tk)2+ak3(t−tk)3(1)(1){s(t)={qk(t),t∈[tk,tk+1],k=0,1,...,n−1}qk(t)=ak0+ak1(t−tk)+ak2(t−tk)2+ak3(t−tk)3
{s(t)={qk(t),t∈[tk,tk+1],k=0,1,...,n−1}qk(t)=ak0+ak1(t−tk)+ak2(t−tk)2+ak3(t−tk)3(1)
  求解三次样条的系数则通过最小化以下方针函数:
L = μ ∑ k = 0 n w k ( s ( t k ) − q k ) 2 + ( 1 − μ ) ∫ t 0 t n s ¨ ( t ) 2 d t (2) L = \mu\sum_{k=0}^{n}w_k(s(t_k)-q_k)^2+(1-\mu) \int_{t_0}^{t_n}\ddot s(t)^2dt \tag 2L=μk=0∑nwk(s(tk)−qk)2+(1−μ)∫t0tns¨(t)2dt(2)
  其间,参数μ ∈ ( 0 , 1 ] \mu\in(0,1]μ∈(0,1],用于衡量两个不同的优化方针(曲线更好地接近给定点、曲线的滑润性)的重要程度。w k w_kwk为第k kk个点的权重,用于调整样条曲线在该点的拟合精度。
  式(2)方针函数能够进一步推导得到:
L = ∑ k = 0 n w k ( s ( t k ) − q k ) 2 + λ ∑ k = 0 n − 1 2 T k ( ω k 2 + ω k ω k + 1 + ω k + 1 2 ) (3) L = \sum_{k=0}^{n}w_k(s(t_k)-q_k)^2+ \lambda \sum_{k=0}^{n-1}2T_k(\omega_k^2+\omega_k\omega_{k+1}+\omega_{k+1}^2) \tag 3L=k=0∑nwk(s(tk)−qk)2+λk=0∑n−12Tk(ωk2+ωkωk+1+ωk+12)(3)
  其间,λ = ( 1 − μ ) / ( 6 μ ) \lambda=(1-\mu)/(6\mu)λ=(1−μ)/(6μ)。
  式(3)写成:
L = ( q − s ) T W ( q − s ) + λ ω T A ω (4) L = (q-s)^TW(q-s)+\lambda \omega^TA \omega \tag 4L=(q−s)TW(q−s)+λωTAω(4)
  其间,s ss为待优化求解的向量,ω = [ ω 0 , . . . , ω n ] T \omega=[\omega_0,...,\omega_n]^Tω=[ω0,...,ωn]T为给定点处的加速度向量(不知道),W = d i a g { w 0 , . . . , w n } W=diag\{ w_0,...,w_n\}W=diag{w0,...,wn}为对角线为权向量的矩阵,A AA为矩阵,界说为:
A = [ 2 T 0 T 0 0 ⋯ 0 T 0 2 ( T 0 + T 1 ) T 1 0 ⋮ 0 ⋱ 0 ⋮ 0 T n − 2 2 ( T n − 2 + T n − 1 ) T n − 1 0 ⋯ 0 T n − 1 2 T n − 1 ) ] (5) A=\left[
2T0T00⋮0T02(T0+T1)0⋯0T1⋱Tn−20⋯02(Tn−2+Tn−1)Tn−10⋮0Tn−12Tn−1)2T0T00⋯0T02(T0+T1)T10⋮0⋱0⋮0Tn−22(Tn−2+Tn−1)Tn−10⋯0Tn−12Tn−1)
\right] \tag {5}A=⎣⎢⎢⎢⎢⎢⎢⎡2T0T00⋮0T02(T0+T1)0⋯0T1⋱Tn−20⋯02(Tn−2+Tn−1)Tn−10⋮0Tn−12Tn−1)⎦⎥⎥⎥⎥⎥⎥⎤(5)
  式(4)包括不知道量s ss与ω \omegaω,有必要找到两者联系,并将其一代换掉。关于v 0 = 0 , v n = 0 v_0=0,v_n=0v0=0,vn=0的状况,有联系:
A ω = C s (6) A \omega=Cs \tag 6Aω=Cs(6)
  其间,
C = [ − 6 / T 0 6 / T 0 0 ⋯ 0 6 / T 0 − ( 6 / T 0 + 6 / T 1 ) 6 / T 1 ⋮ 0 6 / T 1 − ( 6 / T 1 + 6 / T 2 ) 6 / T 2 ⋮ ⋱ 0 6 / T n − 2 − ( 6 / T n − 2 + 6 / T n − 1 ) 6 / T n − 1 0 ⋯ 0 6 / T n − 1 − 6 / T n − 1 ) ] (7) C=\left[
−6/T06/T00⋮06/T0−(6/T0+6/T1)6/T1⋯06/T1−(6/T1+6/T2)6/Tn−20⋯6/T2⋱−(6/Tn−2+6/Tn−1)6/Tn−10⋮06/Tn−1−6/Tn−1)−6/T06/T00⋯06/T0−(6/T0+6/T1)6/T1⋮06/T1−(6/T1+6/T2)6/T2⋮⋱06/Tn−2−(6/Tn−2+6/Tn−1)6/Tn−10⋯06/Tn−1−6/Tn−1)
\right] \tag {7}C=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡−6/T06/T00⋮06/T0−(6/T0+6/T1)6/T1⋯06/T1−(6/T1+6/T2)6/Tn−20⋯6/T2⋱−(6/Tn−2+6/Tn−1)6/Tn−10⋮06/Tn−1−6/Tn−1)⎦⎥⎥⎥⎥⎥⎥⎥⎥⎤(7)
  由式(6)得:
ω = A − 1 C s (8) \omega=A^{-1}Cs \tag 8ω=A−1Cs(8)
  上式代入式(4),因为矩阵A AA为实对称矩阵,( A T ) − 1 = A − 1 (A^T)^{-1}=A^{-1}(AT)−1=A−1,化简得到:
L ( s ) = ( q − s ) T W ( q − s ) + λ s T C T A − 1 C s (9) L(s) = (q-s)^TW(q-s)+\lambda s^TC^TA^{-1}Cs \tag 9L(s)=(q−s)TW(q−s)+λsTCTA−1Cs(9)
  极值点在一阶导数为0处获得,对式(9)求导并令其为0:
− ( q − s ) T W + λ s T C T A − 1 C = 0 (10) -(q-s)^TW+\lambda s^TC^TA^{-1}C=0 \tag {10}−(q−s)TW+λsTCTA−1C=0(10)
  解得:
s = ( W + λ C T A − 1 C ) − 1 W q (11) s =(W+\lambda C^TA^{-1}C)^{-1}Wq \tag {11}s=(W+λCTA−1C)−1Wq(11)
  能够看到式(11)需要求逆矩阵两次,使用矩阵求逆引理(matrix inversion lemma),上式能够写成:
s = q − λ W − 1 C T ( A + λ C W − 1 C T ) − 1 C q (12) s =q-\lambda W^{-1}C^T(A+\lambda C W^{-1}C^T)^{-1}Cq \tag {12}s=q−λW−1CT(A+λCW−1CT)−1Cq(12)
  因为W WW为对角阵,很简单求得逆矩阵,因此式(12)只需求解矩阵A + λ C W − 1 C T A+\lambda C W^{-1}C^TA+λCW−1CT的逆一次。
  进一步,能够将式(12)分步进行,求解矩阵方程:
( A + λ C W − 1 C T ) ω = C q (13) (A+\lambda C W^{-1}C^T) \omega =Cq \tag {13}(A+λCW−1CT)ω=Cq(13)
  然后代入下式:
s = q − λ W − 1 C T ω (14) s =q-\lambda W^{-1}C^T\omega \tag {14}s=q−λW−1CTω(14)
  更一般地,给定的端点条件或许并非v 0 = 0 v_0=0v0=0与v n = 0 v_n=0vn=0,这时依据式(13)(14)求得的新方位s ss,修正其端点束缚,再使用三次样条进行插值即可(选用新的端点束缚后,曲线形状会有一点改变)。拜见博文:三次样条插值(附完好代码)。
  这儿完成了三种不同给定束缚的三次样条拟合:
  (1) 给定开始速度v 0 v_0v0与完毕速度v n v_nvn
  (2) 开始方位q 0 q_0q0与完毕方位q n q_nqn持平(数据需满意的特性,非束缚条件),开始速度v 0 v_0v0与完毕速度v n v_nvn持平,开始加速度a 0 a_0a0与完毕加速度a n a_nan持平(周期三次样条)
  (3) 给定开始速度v 0 v_0v0、完毕速度v n v_nvn、开始加速度a 0 a_0a0、完毕加速度a n a_nan
  当指定拟合精度时,使用二分法能够迭代得到参数μ \muμ,再进行以上三种不同给定束缚的三次样条拟合。
二、三种不同端点束缚下的三次样条拟合
%{ Function: estimate_new_position
Description: 估量新的方位
Input: 系数lambda, 对角矩阵inverseW, 本来方位向量q, 时刻距离向量T, 数据点个数count
Output: 新的方位向量s
Author: Marc Pony(marc_pony@163.com) %} function s = estimate_new_position(lambda, inverseW, q, T, count) a = zeros(count, 1); b = zeros(count, 1); c = zeros(count, 1); for i = 1 : count - 1 a(i + 1) = T(i); end b(1) = 2.0 * T(1); for i = 2 : count - 1 b(i) = 2.0 * (T(i - 1) + T(i)); end b(count) = 2.0 * T(count - 1); for i = 1 : count - 1 c(i) = T(i); end
A = diag(a(2 : count), -1) + diag(b, 0) + diag(c(1 : count - 1), 1); aa = zeros(count, 1); bb = zeros(count, 1); cc = zeros(count, 1); for i = 1 : count - 1 aa(i + 1) = 6.0 / T(i); end bb(1) = -6.0 / T(1); for i = 2 : count - 1 bb(i) = -(6.0 / T(i - 1) + 6.0 / T(i)); end bb(count) = -6.0 / T(count - 1); for i = 1 : count - 1 cc(i) = 6.0 / T(i); end
C = diag(aa(2 : count), -1) + diag(bb, 0) + diag(cc(1 : count - 1), 1); omegaTemp = (A + lambda * C * inverseW * C') \ (C * q); s = q - lambda * inverseW * C' * omegaTemp; end

1、给定开始速度v 0 v_0v0与完毕速度v n v_nvn
clc; clear; close all; t = [0, 5, 7, 8, 10, 15, 18]'; %递加时刻序列
q = [3, -2, -5, 0, 6, 12, 8]'; inverseW = diag([0, 1, 1, 1, 1, 1, 0]); %严厉通过起点与结尾(值越接近于0,曲线越接近给定点) mu = 0.6; %在(0, 1]之间取值,用于衡量两个不同的优化方针(曲线更好地接近给定点、曲线的滑润性)的重要程度
lambda = (1.0 - mu) / (6.0 * mu); qNew = estimate_new_position(lambda, inverseW, q, diff(t), length(t)); v0 = 2; %开始速度
vn = -3; %完毕速度
deltaT = 0.001; %插补周期

n = length(t); % n >= 4 T = diff(t); a = zeros(n, 1); b = zeros(n, 1); c = zeros(n, 1); d = zeros(n, 1); for i = 1 : n - 1 a(i + 1) = T(i); end b(1) = 2.0 * T(1); for i = 2 : n - 1 b(i) = 2.0 * (T(i - 1) + T(i)); end b(n) = 2.0 * T(n - 1); for i = 1 : n - 1 c(i) = T(i); end d(1) = 6.0 * ((qNew(2) - qNew(1)) / T(1) - v0); for i = 2 : n - 1 d(i) = 6.0 * ((qNew(i + 1) - qNew(i)) / T(i) - (qNew(i) - qNew(i - 1)) / T(i - 1)); end d(n) = 6.0 * (vn - (qNew(n) - qNew(n - 1)) / T(n - 1)); [omega, sta] = solve_tridiagonal_matrix_equation(a, b, c, d, n); if sta == 0 error('三对角矩阵方程求解犯错!'); end

time = []; pos = []; vel = []; acc = []; time = [time; t(1)]; pos = [pos; qNew(1)]; vel = [vel; v0]; acc = [acc; omega(1)]; for i = 1 : n - 1 tt = (t(i) + deltaT : deltaT : t(i + 1))'; a0 = qNew(i); a1 = (qNew(i + 1) - qNew(i)) / T(i) - T(i) * (omega(i + 1) + 2.0 * omega(i)) / 6.0; a2 = 0.5 * omega(i); a3 = (omega(i + 1) - omega(i)) / (6.0 * T(i)); time = [time; tt]; pos = [pos; a0 + (tt - t(i)) .* (a1 + (tt - t(i)) .* (a2 + a3 .* (tt - t(i))))]; vel = [vel; a1 + (tt - t(i)) .* (2.0 * a2 + 3.0 * a3 .* (tt - t(i)))]; acc = [acc; 2.0 * a2 + 6.0 * a3 .* (tt - t(i))]; end if abs(time(end) - t(n)) > 1.0e-8 time = [time; t(n)]; pos = [pos; qNew(n)]; vel = [vel; vn]; acc = [acc; omega(n)]; end figure(1) subplot(3, 1, 1) plot(time, pos); hold on plot(t, q, 'ro'); xlabel('t') ylabel('pos') subplot(3, 1, 2) plot(time, vel); xlabel('t') ylabel('vel') subplot(3, 1, 3) plot(time, acc); xlabel('t') ylabel('acc')
关注了解更多奇亿娱乐58techan.com

¥

库存

已选:""

数量:
×

©2018 - 2020 58techan.com 版权所有

本页面由聚货网提供,在线发卡上聚货网!

Powered by JvHuo.Com

首页
购物车
充值
客服
顶部