回旋曲线的计算

%% 调用 clothoid函数，并将终点位置的曲率半径设置为 0.1:0.2:3
for i = 0.1:0.2:3
    clothoid(i)
end

%%%% 计算回旋曲线，并绘图
function [] = clothoid(Rmin)

N = 100; %% 
sk = 2; %% 曲线长
c = 1 / Rmin / sk; %% 曲率变化率，即上文中的曲率变化率 a
x = zeros(1,N);
y = zeros(1,N);

for sk_i = sk / N : sk / N : sk
    result_x = 0; %% 初始化x坐标点的值
    result_y = 0; %% 初始化y坐标点的值
    
    for i = 0 : N %% 计算x
        result_x = result_x + ((-1) ^ i) * (c ^ (2 * i)) * (sk_i ^ (4 * i + 1)) / ((2 ^ (2 * i)) * (4 * i + 1) * fctorial(2 * i));
    end
    x(int32(sk_i / sk * N)) = result_x;
    
    for i = 0 : N %% 计算y
        result_y = result_y + ((-1) ^ i) * (c ^ (2 * i + 1)) * (sk_i ^ (4 * i + 3)) / ((2 ^ (2 * i + 1)) * (4 * i + 3) * fctorial(2 * i + 1));
    end
    y(int32(sk_i / sk * N)) = result_y;
    
end

hold on;
plot(-x,-y);
hold on;
plot(-x(end),-y(end),'*'); %%回旋曲线的最后一个点
axis equal;
end