matlab——车道线识别跟踪

最新推荐文章于 2025-05-14 13:13:46 发布
原创最新推荐文章于 2025-05-14 13:13:46 发布 · 2.8k 阅读
29 ·
CC 4.0 BY-SA版权
matlab 专栏收录该内容
4 篇文章
订阅专栏
该博客主要介绍了车道线检测的流程，包括初始帧RANSAC算法、跟踪帧的ROI处理以及卡尔曼滤波的应用。在初始帧中，通过RANSAC算法找到车道线，然后使用最小二乘法进行跟踪。当跟踪失效时，系统会重新进入初始帧。在跟踪过程中，采用卡尔曼滤波进行参数平滑，确保在车道线消失或参数突变时仍能稳定追踪。此外，还涉及ROI区域的更新和调整，以提高检测的准确性。
摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >
1.初始帧RANSAC
2.跟踪帧降低感兴趣区域，使用最小二乘。
3.如果跟踪失效，重新进入初始帧。
%% 初始化代码
%-----FLAGL=1;FLAGR=1;在跟踪状态下未检测到车道线，以上一帧的位置打印
clc;close all;
clear all
pic_name='Tsd76.gif';
VideoFile = VideoReader('E:\MATLAB\遍历图片保存为mat\shangqi\lane_detection\test.avi');
nFrames =VideoFile.NumberOfFrames;   %得到帧数
%% 初始化calman参数
 %X=[rho,theta,dr,dtheta]选取的初值分别为距离rho、角度加入量测
%速度dr、角速度的平均值vb 
X_left=[-1 100 0.2 5];
R=[(X_left(1)*0.2)^2 0;0 (X_left(2)*0.1)^2];%测量噪声协方差矩阵 已知
Q=[(X_left(1)*0.2)^2 0 0 0;0 (X_left(2)*0.1)^2 0 0;0 0 (X_left(3)*0.2)^2 0;0 0 0 (X_left(4)*0.1)^2]; %过程噪声协方差矩阵 已知
A=[1 0 1 0;0 1 0 1;0 0 1 0;0 0 0 1];%系统矩阵 已知
H=[1 0 0 0;0 1 0 0]; %观测状态转移矩阵
P_left=[10 0 0 0; 0 25 0 0; 0 0 4 0; 0 0 0 4];%误差协方差矩阵 任意给的初值 待定
P_right=[10 0 0 0; 0 25 0 0; 0 0 4 0; 0 0 0 4];%误差协方差矩阵 任意给的初值 待定
xL=zeros(10,4);%用于存储中间状态值，按行存储，用时得转置变为列向量
xR=zeros(10,4);%用于存储中间状态值，按行存储，用时得转置变为列向量
countL=5;
countR=5;%车道线初始帧的标志
I_H=480;
I_W=360;
offset=10;
flag=1;
for times=1:nFrames
     Frame= read(VideoFile,times);    %从VideoFile视频文件中读取视频帧  
        I=imresize( Frame ,[360 60]);
  tic;
  gray= rgb2gray(I);
   %% ROI点的确定与ROI的提取 
  %------------------------从边框中提取感兴趣区域-----------------------
  if countL==5 | countR==5 %roi更新
    %   m=[1,480,480,1];n=[220,220,360,360]
    load roi;
      roi= roipoly( gray,m,n);%原始roi
  else
       roi_l = roipoly( gray,roi_Pl(:,1) ,roi_Pl(:,2)); %感兴趣区域为白色
      roi_r= roipoly( gray,roi_Pr(:,1) ,roi_Pr(:,2)); %感兴趣区域为白色
      roi=bitor( roi_l , roi_r );
  end
  gray(:,1:70)=0;
   gray(180:240,360:480)=0;
gz = medfilt2( gray);                       %对躁声中值滤波
SE = strel('square',20);
Fs=imtophat(gz,SE);%图像
F=im2bw(Fs,graythresh(Fs(180:360,:)));                 %大津法局部二值化
% figure;imshow(F(141:end,:));


f=bitand(F,roi);%位操作，取出车道线
%figure;imshow(f);
% subplot 121;imshow(Fs)
% subplot 122;imshow(f);
SE = strel('square',5);
 f=imclose(f,SE);%图像膨胀后腐蚀处理，消除空洞
% figure,imshow(f);
if flag==1
[B,L1] = bwboundaries(f,'noholes');
width=30;%车道宽度极大值
for i=1:length(B)
     X=[];
     X=unique(B{i}(:,1));%所有的不重复X坐标
     Y=mean(B{i}(:,2));
      if abs(Y-240)>155 &&mean(X)<200%检测平均X是否距中轴线（80/240)
        for j=1:length(X)
        IndexY=find(B{i}(:,1)==X(j));
        f(X(j),min(B{i}(IndexY,2)):max(B{i}(IndexY,2)))=0;
        end
      else
           for j=1:length(X)
                IndexY=find(B{i}(:,1)==X(j));
                wide=max(B{i}(IndexY,2))-min(B{i}(IndexY,2));
                if wide>width
                  f(X(j),min(B{i}(IndexY,2)):max(B{i}(IndexY,2)))=0;
                end
           end
      end
end
end
imshow(f)
L = bwlabeln(f,8);
S = regionprops(L, 'Area');
 area=sort([S.Area],'descend');
 G= ismember(L, find([S.Area] >=20));
 imshow(G(171:end,:));
%% 检测是否为初始帧
%加入扫描起始点,图像并非严格的以中轴线分为左右两区域
if flag==1
    middle=240;
else
    middle=floor(0.5*roi_Pr(1,1)+0.5*roi_Pl(1,1));
    if middle >350|middle<100
        middle=240;
    end
end
[X,X1,Y,Y1]=lineGet(G,240);%
data=[X;Y];
data1=[X1;Y1];
 % 系统状态判断，flag=1进入初始帧，flag=2进入跟踪。flag=3正常跟踪
  if countL==5 | countR==5
   flag=1;
  elseif countL==0 & countR==0
      flag=2;
  else
      flag=3
  end

     if  flag==1
      if length(data) > 10
           [inliers1, bestParameter1, bestParameter2]=ransac(data);
           countL=0;%countL作为车道线提取的标志 
           slopeL = bestParameter1;
            xAxis1 = min(inliers1):max(inliers1);
            yAxis1 = bestParameter1*xAxis1 + bestParameter2;
            bL=  bestParameter2;
            FLAG=1;%初始帧标志
             xL(times,1:2)=[slopeL,bL];%初始帧的测量值作为输入
      else
          countL=5;
      end
      if length(data1) > 10
          [inliers2, bestParameter3, bestParameter4]=ransac(data1);
          countR=0; slopeR = bestParameter3;
             bR=  bestParameter4;
              xR(times,1:2)=[slopeR,bR];%以上一帧的校正值作为输入。
                xAxis2 = min(inliers2):max(inliers2);
           yAxis2 = bestParameter3*xAxis2 + bestParameter4;       
             else
          countR=5;
        end
  else%kalman有效则进行最小二乘拟合
       if length(data) > 20
            pL=polyfit(X,Y,1);
            slopeL = pL(1);
            bL=  pL(2);
            flag=3;
             FLAGL=1;
       else%未检测到车道线
            slopeL=0;
            bL=0;
             FLAGL=0;
       end
    if length(data1) > 20
      pR=polyfit(X1,Y1,1);
           slopeR=pR(1)
         bR=pR(2);  
         flag=3;
          FLAGR=1;
      else
        slopeR=0
         bR=0;
         FLAGR=0;
    end
      FLAG=0;
  end
% 初始帧拟合结果检测 flag=1进入初始帧，flag=2进入跟踪。flag=3正常跟踪
  if countL==5 | countR==5
   flag=1;
  elseif countL==0 & countR==0
      flag=2;
  else
      flag=3;
  end
% imshow(G)
%  hold on
%  plot(xAxis1,yAxis1,xAxis2,yAxis2,'b-','LineWidth',4)
% plot([X,X1],[Y,Y1],'.')
 %  tic calman程序 时间已过 0.017254 秒。
 
 %% 进入卡尔曼
 if flag~=1
            xp_left=A*xL(times,:)'; %先验状态估计方程
            PP_left = A*P_left*A' + Q ; %PP 先验误差协方差矩阵 P是上一时刻的误差协方差阵 Q 过程噪声协方差矩阵 A、Q已知
            K_left = PP_left*H'*inv(H*PP_left*H'+R); %K增益矩阵 PP先验误差协方差阵 R 测量噪声协方差阵 H 观测状态转移矩阵 H、R已知
            xL(times+1,:) = (xp_left + K_left*([slopeL,bL]' - H*xp_left))';%k时刻的状态估计值 xp是上一时刻的先验状态估计
            P_left = (eye(4)-K_left*H)*PP_left ;%k时刻的误差协方差阵
            %卡尔曼预测值和测量值作比较，如果符合误差，则测量值有效。在theta1-5量测值有效。超过5则使用预测值，则认为参数突变，系统初始化
               if  abs(atan(xL(times,1))-atan(slopeL)) < 3*3.14/180 &slopeL~=0
                    xL(times+1,1)=slopeL;
                    xL(times+1,2)= bL;
                  %  disp('测量值有效')
                elseif abs(atan(xL(times,1))-atan(slopeL)) < 7*3.14/180 
                    slopeL= xL(times+1,1);
                    bL=xL(times+1,2);
                    disp('修正值有效')
                    elseif slopeL==0
              % 若未检测到车道线，则以上一帧的输出，作为当前帧的车道参数。并更改卡尔曼滤波器本次的预测值
            
               slopeL= xL(times,1);%修正当前帧
                bL=xL(times,2);
                xL(times+1,1)=slopeL;%更改预测
                xL(times+1,2)= bL;
              countL=countL+1;%未检测出车道线，标志位加1
                end   
 end
 
   if flag~=1
      
                   %% right卡尔曼观测
        xp_right=A*xR(times,:)'; %先验状态估计方程
        PP_right = A*P_right*A' + Q ; %PP 先验误差协方差矩阵 P是上一时刻的误差协方差阵 Q 过程噪声协方差矩阵 A、Q已知
        K_right = PP_right*H'*inv(H*PP_right*H'+R); %K增益矩阵 PP先验误差协方差阵 R 测量噪声协方差阵 H 观测状态转移矩阵 H、R已知
        xR(times+1,:) = (xp_right +  K_right *([slopeR,bR]' - H*xp_right))';%k时刻的状态估计值 xp是上一时刻的先验状态估计
        P_right = (eye(4)-K_right*H)*PP_right ;%k时刻的误差协方差阵 
        if  abs(atan(xR(times,1))-atan(slopeR)) < 3*3.14/180  &slopeR~=0
            xR(times+1,1)=slopeR;
            xR(times+1,2)= bR;
           % disp('测量值有效')
        elseif abs(atan(xR(times,1))-atan(slopeR)) < 5*3.14/180  &slopeR~=0
           slopeR= xR(times+1,1);
             bR=xR(times+1,2);
             disp('修正值有效')
          elseif slopeR==0
              % 若未检测到车道线，则以上一帧的输出，作为当前帧的车道参数。并更改卡尔曼滤波器本次的预测值
            
               slopeR= xR(times,1);%修正当前帧
                bR=xR(times,2);
                xR(times+1,1)=slopeR;%更改预测
                xR(times+1,2)= bR;
               countR=countR+1
        end
       
         
        
   end 
   
   % 系统状态判断，flag=1进入初始帧，flag=2进入跟踪。flag=3正常跟踪
  if countL==5 | countR==5
   flag=1
     countR
       
  elseif countL==0 & countR==0
      flag=2;
  else
      flag=3;
  end
  
  %----------------------calman跟踪车道线-------------------------------- 
    %%left卡尔曼观测,如果参数突变，或者连续5帧没有车道线。则认为跟踪无效。
   % theta变化超过阈值，则上一帧的预测值作为输入，
   %% 5次连续没有检测到车道线，calman系数初始化
   if flag==1 | (abs(atan(xL(times,1))-atan(slopeL)) >10*3.14/180 & slopeL~=0 )| (abs(atan(xR(times,1))-atan(slopeR)) > 10*3.14/180 & slopeR~=0 )
       R=[(X_left(1)*0.2)^2 0;0 (X_left(2)*0.1)^2];%测量噪声协方差矩阵
       Q=[(X_left(1)*0.2)^2 0 0 0;0 (X_left(2)*0.1)^2 0 0;0 0 (X_left(3)*0.2)^2 0;0 0 0 (X_left(4)*0.1)^2]; %过程噪声协方差矩阵 已知
       P_left=[10 0 0 0; 0 25 0 0; 0 0 4 0; 0 0 0 4];%误差协方差矩阵 任意给的初值
       P_right=[10 0 0 0; 0 25 0 0; 0 0 4 0; 0 0 0 4];%误差协方差矩阵 任意给的初值
       sprintf('第%dcalman帧照片初始化',times+1)
       if flag==1
           disp('参数突变')
       end
      countL=5;countR=5;%进入初始化
   end
  t(times)=toc;
     %-------------------------更新roi区域--------------------------------
  if flag~=1
    roi_Pl(1,:)= [(I_W/2- bL)/slopeL-1.5*offset,I_W/2];
    roi_Pl(2,:)= [(I_W/2- bL)/slopeL+1.5*offset,I_W/2];
    roi_Pl(3,:)= [(I_W- bL)/slopeL+1.5*offset,I_W];
    roi_Pl(4,:)= [(I_W- bL)/slopeL-1.5*offset,I_W];
  
    roi_Pr(1,:)=[(I_W/2- bR)/slopeR-1.5*offset,I_W/2];
    roi_Pr(2,:)=[(I_W/2- bR)/slopeR+1.5*offset,I_W/2];
    roi_Pr(3,:)= [(I_W- bR)/slopeR+1.5*offset,I_W];
    roi_Pr(4,:)= [(I_W- bR)/slopeR-1.5*offset,I_W];
  end
    %% 在图像上画出所有的东西 
    
    %------------------在每个框架上绘制外推线、梯形图和方向图-----------------
    %tic
  % figure(times)
  imshow(G);
   hold on
 % plot(roi_Pl([1:4,1],1) ,roi_Pl([1:4,1],2),roi_Pr([1:4,1],1) ,roi_Pr([1:4,1],2), 'LineWidth',2,'Color','red')
  %检测是否为初始帧，初始帧为蓝色标记。
  if ~FLAG
    %如果这一帧左侧的车道线为空,则打印上组信息
    if FLAGL
   xAxis1 =min(X):max(X);
   yAxis1 =  slopeL*xAxis1+bL;
    end
    if FLAGR
   xAxis2 =min(X1):max(X1);
   yAxis2 =slopeR*xAxis2+bR;
    end
   plot(xAxis1,yAxis1,xAxis2,yAxis2, 'LineWidth',3,'Color','R');
  else
      plot(xAxis1,yAxis1,xAxis2,yAxis2, 'LineWidth',5,'Color','B'); 
  end
%     plot(xAxis1,yAxis1,xAxis2,yAxis2, 'LineWidth',3,'Color','red');
% imshow(strain_image,'border','tight','initialmagnification','fit');
% set (gcf,'Position',[0,0,500,500]);
axis normal;
%    title(awarn);    
  %  saveas(gca,['test' num2str(times) '.bmp']);
frame = getframe(gcf);
%    writeVideo(writerObj,frame); 
   hold off
% %%转换为可以直接输出的格式(这会使图像丢失)
% %如果要制作彩色的图像,你只能把生成的彩色图像单独制作(使用其他软件)
%  im=frame2im(frame);
% [I,map]=rgb2ind(im,256);
% if times==1
% imwrite(I,map,pic_name,'gif','loopcount',inf)
% else
% imwrite(I,map,pic_name,'gif','writemode','append')
% end
 
end
%close(writerObj);