曲率多项式转换为直角坐标系

对于形如 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