机器人中的数值优化进阶|【一】三次样条曲线推导(上)

机器人中的数值优化进阶|【一】三次样条曲线推导(上)

三次样条曲线的定义

在三次样条曲线中,样条曲线通过一系列控制点 η = [ η 0 , η 1 , . . . η n ] \eta=[\eta_0, \eta_1,...\eta_n] η=[η0,η1,...ηn]来实现对样条曲线的生成。控制点意味着样条曲线必然要经过这几个点。对于每一段曲线,都可以由 s ∈ [ 0 , 1 ] s\in[0,1] s[0,1]来表征曲线,其定义为
p i ( s ) = a i + b i s + c i s 2 + d i s 3 p_i(s)=a_i+b_is+c_is^2+d_is^3 pi(s)=ai+bis+cis2+dis3
η = [ x , y ] ( n + 1 ) × 2 \eta=[x,y]_{(n+1)\times 2} η=[x,y](n+1)×2
考虑到位置连续性,速度连续性和加速度连续性,对位置连续性,有
p i ( 0 ) = p i − 1 ( 1 ) p_i(0)=p_{i-1}(1) pi(0)=pi1(1)
对于速度连续性,有
p i ( 1 ) ( 0 ) = p i − 1 ( 1 ) ( 1 ) p_i^{(1)}(0)=p_{i-1}^{(1)}(1) pi(1)(0)=pi1(1)(1)
考虑到加速度连续性,有
p i ( 2 ) ( 0 ) = p i − 1 ( 2 ) ( 1 ) p_i^{(2)}(0)=p_{i-1}^{(2)}(1) pi(2)(0)=pi1(2)(1)
因此,我们有
a i = a i − 1 + b i − 1 + c i − 1 + d i − 1 a_i = a_{i-1} + b_{i-1} + c_{i-1} + d_{i-1} ai=ai1+bi1+ci1+di1

b i = b i − 1 + 2 c i − 1 + 3 d i − 1 b_i = b_{i-1} + 2c_{i-1} + 3d_{i-1} bi=bi1+2ci1+3di1
2 c i = 2 c i − 1 + 6 d i − 1 2c_i = 2 c_{i-1} + 6d_{i-1} 2ci=2ci1+6di1
我们知道三次样条曲线必须过控制点,因此有
p i ( 0 ) = η i = a i p_i(0) = \eta_i=a_i pi(0)=ηi=ai
b i = D i b_i=D_i bi=Di,整理上面各式,有如下形式
a i = η i b i = D i c i = 3 ( η i + 1 − η i ) − 2 D i − D i + 1 d i = 2 ( η i − η i + 1 ) + D i + D i + 1 a_i = \eta_i \\ b_i = D_i \\ c_i = 3(\eta_{i+1} - \eta_i) - 2D_i - D_{i+1} \\ d_i = 2(\eta_i - \eta_{i+1}) + D_i + D_{i+1} ai=ηibi=Dici=3(ηi+1ηi)2DiDi+1di=2(ηiηi+1)+

你可能感兴趣的:(algorithm,机器人,线性代数)