如何让小车自己识别车道线自动前行？

%%setup driving scenario;s = drivingScenario;R = 50;theta = 0:90;%degreeRoadCenter = [R*sind(theta)',R*cosd(theta)'];RoadWidth = 5;vel = 100 ;road(s,RoadCenter,RoadWidth);plot(s);%figure(1)%%f = figure(1);v = vehicle(s);initPosition = [-3,50,0];sensorLocation = [v.Length - v.FrontOverhang,0];v.Position = initPosition;s.updatePlots();%figure(1)

%%figure(1)ax = gca;height = 2;chasePlot(v,'Centerline','on','Viewlocation',sensorLocation,'ViewHeight',height,'Parent',ax);%figure(2)%drawnow();%refresh the plot.%%c = getframe(gcf);currentImage = c.cdata;%figure();%imshow(currentImage);

%% camera calibration;focalLength = [309,344];imageSize = [480,640];principalPoint = fliplr(imageSize)/2;%exchange x and y.f.Position = [1,1,fliplr(imageSize)];pitch = 0; % pitch ange of camera.% to set the intrinsic parameters of a camera.camIntrinsics = cameraIntrinsics(focalLength,principalPoint,imageSize);% to create a camera.sensor = monoCamera(camIntrinsics,height,'Pitch',pitch,'sensorLocation',sensorLocation);distAheadOfSensor = 20; SpaceToOneSide = 6;bottomOffset = sensorLocation(1);outView = [bottomOffset, distAheadOfSensor, -SpaceToOneSide,SpaceToOneSide];outImageSize = [500,NaN];% to set the bird eye view to be output.birdsEyeConfigure = birdsEyeView(sensor,outView,outImageSize);%%I = imresize(currentImage,birdsEyeConfigure.Sensor.Intrinsics.ImageSize);birdsEyeImage = transformImage(birdsEyeConfigure,I);%bird's-eye-view image%

%Lane extraction.approxBoundaryWidth = 0.25;% to get segment lane marker features using a ridge filter.birdsEyeBW = segmentLaneMarkerRidge(rgb2gray(birdsEyeImage),birdsEyeConfigure,approxBoundaryWidth);[imageX,imageY] = find(birdsEyeBW);% to convert bird's-eye-view image locations to vehicle coordinates.xyBoundaryPoints = imageToVehicle(birdsEyeConfigure,[imageY,imageX]);% to fit the curve with function: Ax2+Bx+C=0.boundaries = findParabolicLaneBoundaries(xyBoundaryPoints,approxBoundaryWidth,'MaxNumBoundaries',1,'MaxSamplingAttempts',100);%

%%load_system('unicycle');set_param('unicycle','FastRestart','on');plot(s);while advance(s)    % force update plots    updatePlots(s);%    % set figure location    f = figure(1);    ax = f.CurrentAxes;    ax.Position = [0,0,1,1];    c = getframe(gcf);    % Resize the image to fit sensor configuration;    I = imresize(c.cdata,birdsEyeConfigure.Sensor.Intrinsics.ImageSize);    birdsEyeImage = transformImage(birdsEyeConfigure,I);    %Lane extraction.    approxBoundaryWidth = 0.25;    birdsEyeBW = segmentLaneMarkerRidge(rgb2gray(birdsEyeImage),birdsEyeConfigure,approxBoundaryWidth);    [imageX,imageY] = find(birdsEyeBW);    xyBoundaryPoints = imageToVehicle(birdsEyeConfigure,[imageY,imageX]);    boundaries = findParabolicLaneBoundaries(xyBoundaryPoints,approxBoundaryWidth,'MaxNumBoundaries',1,'MaxSamplingAttempts',100);    currentPosition = v.Position;    forward = 10;    ref_yaw = [0,atan2(boundaries.computeBoundaryModel(forward),forward)];    options = simset('SrcWorkspace','Current');    stoptime = s.SampleTime;    simout = sim('unicycle',[],options);    yaw = simout.yaw;    dx = vel.*cos(yaw(end)).*stoptime;    dy = vel.*sin(yaw(end)).*stoptime;    currentPosition = v.Position;    v.Position = [currentPosition(1)+dx(end),currentPosition(2)+dy(end),0];    v.Yaw = rad2deg(yaw(end));

%%setup driving scenario;s = drivingScenario;R = 50;theta = 0:90;%degreeRoadCenter = [R*sind(theta)',R*cosd(theta)'];RoadWidth = 5 ;vel = 200 ;road(s,RoadCenter,RoadWidth);plot(s);%% add a vehicle;v = vehicle(s);initPosition = [-2,50,0];sensorLocation = [v.Length - v.FrontOverhang,0];v.Position = initPosition;s.updatePlots();%% add a plot for debug;f = figure(1);ax = gca;height = 2;chasePlot(v,'Centerline','on','Viewlocation',sensorLocation,'ViewHeight',height,'Parent',ax);drawnow();%% camera calibration;focalLength = [309,344];imageSize =  [640,480];principalPoint = fliplr(imageSize)/2;f.Position = [1,1,fliplr(imageSize)];c = getframe(gcf);pitch = 0;camIntrinsics = cameraIntrinsics(focalLength,principalPoint,imageSize);sensor = monoCamera(camIntrinsics,height,'Pitch',pitch,'sensorLocation',sensorLocation);distAheadOfSensor = 20 ; SpaceToOneSide = 6;bottomOffset = sensorLocation(1);outView = [bottomOffset, distAheadOfSensor, -SpaceToOneSide,SpaceToOneSide];outImageSize = [640,NaN];birdsEyeConfigure = birdsEyeView(sensor,outView,outImageSize);%%load_system('unicycle');set_param('unicycle','FastRestart','on');plot(s);while advance(s)    % force update plots    updatePlots(s);%    % set figure location    f = figure(1);    ax = f.CurrentAxes;    ax.Position = [0,0,1,1];    c = getframe(gcf);    % Resize the image to fit sensor configuration;    I = imresize(c.cdata,birdsEyeConfigure.Sensor.Intrinsics.ImageSize);    birdsEyeImage = transformImage(birdsEyeConfigure,I);    %Lane extraction.    approxBoundaryWidth = 0.25;    birdsEyeBW = segmentLaneMarkerRidge(rgb2gray(birdsEyeImage),birdsEyeConfigure,approxBoundaryWidth);    [imageX,imageY] = find(birdsEyeBW);    xyBoundaryPoints = imageToVehicle(birdsEyeConfigure,[imageY,imageX]);    boundaries = findParabolicLaneBoundaries(xyBoundaryPoints,approxBoundaryWidth,'MaxNumBoundaries',1,'MaxSamplingAttempts',100);    currentPosition = v.Position;    forward = 10;    ref_yaw = [0,atan2(boundaries.computeBoundaryModel(forward),forward)];    options = simset('SrcWorkspace','Current');    stoptime = s.SampleTime;    simout = sim('unicycle',[],options);    yaw = simout.yaw;    dx = vel.*cos(yaw(end)).*stoptime;    dy = vel.*sin(yaw(end)).*stoptime;    currentPosition = v.Position;    v.Position = [currentPosition(1)+dx(end),currentPosition(2)+dy(end),0];    v.Yaw = rad2deg(yaw(end));end
set_param('unicycle','FastRestart','off');unicycle([],[],[],'term');