曲率多項式轉換爲直角座標系

對於形如 k(s) = a + bs + cs²+ds³的曲率多項式，我們將它轉換爲直角座標系表示的多項式。
推導如圖：

在每一步delta_s上面進行積分就可以獲得直角座標系下的曲線。

舉個例子，我們已經獲得了關於s的曲率表達式，如圖：

在橫軸上對s進行微分，並對每一步在直角座標系下做積分即可獲得直角座標系下的曲線關於s的表達式：如圖

測試使用的代碼如下：

clc
clear all
 
 
 Ti = 10;
 Di = 0.1;

 i = 1;
 for t = 0:0.1: Ti
p(i,:) = Bezierfrenet(Ti, Di,t);

i = i+1;
 end
 
 figure
 
plot(p(:,1),p(:,2),'LineWidth',2)
testa = Bezierfrenet(Ti, Di,10);


 d(1) = 0;
 theta = 0;
 for i = 2: 1 :length(p)
     delta_s = p(i,1)- p(i-1,1);
     theta = theta + delta_s * p(i,2)
     d(i) = d(i-1) + sin(theta) * (p(i,1)- p(i-1,1));
 end  
 
 figure
 plot(p(:,1),d','LineWidth',2)
function [p] = Bezierfrenet(Ti, Di,t)
 p0 = [ 0, 0];
 p1 = [Ti/2, 0];
 p2= [Ti/2, Di];
 p3 = [Ti, Di];
 %設置控制點
 p= (1-(t)/Ti)^3*p0 + 3*(1-(t)/Ti)^2*(t)/Ti*p1 + 3*(1-(t)/Ti)*((t)/Ti)^2*p2 + ((t)/Ti)^3*p3;
end
function [a0, a1, a2, a3, a4,a5] = quintic_polynomial(xs, vxs, axs, xe, vxe, axe,T)
% A = [0,0,0,0,0,1; T^5,T^4,T^3,T^2,T,1;...
%     0,0,0,0,1,0 ; 5*T^4  4*T^3 3*T^2 2*T 1 0 ; ...
%     0 0 0 2 0 0; 20*T^3 12*T^2 6*T 2 0 0];
% b = [xs, xe, vxs, vxe, axs, 0]';
% x = A\b;
% a5 = x(1);
% a4 = x(2);
% a3 = x(3);
% a2 = x(4);
% a1 = x(5);
% a0 = x(6);
A = [T^3 T^4 T^5; 3*T^2 4*T^3 5*T^4; 6*T 12*T^2 20*T^3];
b = [(xe - xs  - vxs*T - 0.5*axs*T^2); (vxe- vxs - axs*T ); (axe - axs)];
x = A\b;
a0 = xs;
a1 = vxs;
a2 = axs/2;

a3 = x(1);
a4 = x(2);
a5 = x(3);
end