自动驾驶实测：贝塞尔曲线静态障碍物避障

clc
clear all

% load('D:\ccs\DaischTest\CurveCurve.mat')
load('D:\ccs\DaischTest\TraceC10.mat')
 spacelimit = 1;    
 start =10;
 finish =1;
x= PosLat(start : spacelimit:length(PosLat)-finish);
y = PosLon(start: spacelimit:length(PosLat)-finish);
ha = AngleHeading(start: spacelimit:length(PosLat)-finish);
time = Time(start: spacelimit:length(PosLat)-finish);

% sw = SAS_SteerWheelAngle(start: spacelimit:length(PosLat)-finish);

x = roundn(x, -8);
y =  roundn(y,-8);
B0 = x(1);
L0 = y(1);
HA= roundn(ha, -2);
% SW = vpa(sw,3);
% plot(x,y, 'LineWidth',10)

%%

for  i = 1: length(x)
     [X(i),Y(i)]=transformation1(x(i),y(i),B0,L0);
     i
end

X = X';
Y = Y';

%%
figure 
for i = 1: 1:length(X)
% plot(X(3:i),Y(3:i),'-o')
% i
plot(X(i), Y(i), 'o')
hold on

%  drawnow
end
%%
xs = 0; ys = 0;
Ldis = sqrt((X(200)-X(350)).^2 + (Y(200)-Y(350)).^2);
xe =Ldis;
ye = 0;

p = BezierGeneration(xs,ys, xe,ye,2.5);
% p = vpa(p,3);
% p1 = vpa(p(:,1),3);
% p2 =vpa( p(:,2),3);
% 
% figure
% plot(p1,p2,'-*')
% hold on
p = FrenetToHuace(p);
% p1 = vpa(p(:,1),3);
% p2 =vpa( p(:,2),3);
% plot(p1,p2,'-*')

p = [p, ones(size(p,1),1)];
xc = X(200); yc = Y(200); heading_current= HA(200);
[ trans] =  BackTransfer(xc,yc,heading_current); %3*3

P = trans * p'; % （3*3） * （3*92）
P = P(1:2,:)';    % (92 * 2)
P(:,1) = roundn(P(:,1),-2);
P(:,2) = roundn(P(:,2),-2);

plot(P(:,1),P(:,2),'*r')

在原有轨迹上，根据障碍物规划出新的轨迹：

xs = 0; ys = 0;
Ldis = sqrt((X(200)-X(350)).^2 + (Y(200)-Y(350)).^2);
xe =Ldis;
ye = 0;

p = BezierGeneration(xs,ys, xe,ye,2.5);
% p = vpa(p,3);
% p1 = vpa(p(:,1),3);
% p2 =vpa( p(:,2),3);
% 
% figure
% plot(p1,p2,'-*')
% hold on
p = FrenetToHuace(p);
% p1 = vpa(p(:,1),3);
% p2 =vpa( p(:,2),3);
% plot(p1,p2,'-*')

p = [p, ones(size(p,1),1)];
xc = X(200); yc = Y(200); heading_current= HA(200);
[ trans] =  BackTransfer(xc,yc,heading_current); %3*3

P = trans * p'; % （3*3） * （3*92）
P = P(1:2,:)';    % (92 * 2)
P(:,1) = roundn(P(:,1),-2);
P(:,2) = roundn(P(:,2),-2);

plot(P(:,1),P(:,2),'*r')

效果如图：