课堂作业讨论：智能驾驶技术

湘江人工智能学院作业题目参考

作业1路径规划

直接贴代码

这是一个机器人自动规划路径的问题
卡尔曼滤波原理不记得了
example_test.m

% This script is to help run your algorithm and visualize the result from it.
% 
% Please see example_lidar first to understand the lidar measurements, 
% and see example_bresenham to understand how to use it.
clear all;
close all;

load practice.mat 
% This will load four variables: ranges, scanAngles, t, pose
% [1] t is K-by-1 array containing time in second. (K=3701)
%     You may not use time info for implementation.
% [2] ranges is 1081-by-K lidar sensor readings. 
%     e.g. ranges(:,k) is the lidar measurement (in meter) at time index k.
% [3] scanAngles is 1081-by-1 array containing at what angles (in radian) the 1081-by-1 lidar
%     values ranges(:,k) were measured. This holds for any time index k. The
%     angles are with respect to the body coordinate frame.
% [4] pose is 3-by-K array containing the pose of the mobile robot over time. 
%     e.g. pose(:,k) is the [x(meter),y(meter),theta(in radian)] at time index k.

% 1. Decide map resolution, i.e., the number of grids for 1 meter.
param.resol = 25;

% 2. Decide the initial map size in pixels
param.size = [900, 900];

% 3. Indicate where you will put the origin in pixels
param.origin = [700,600]'; 

% 4. Log-odd parameters 
param.lo_occ = 1;
param.lo_free = 0.5; 
param.lo_max = 100;
param.lo_min = -100;


% Call your mapping function here.
% Running time could take long depending on the efficiency of your code.
% For a quicker test, you may take some hundreds frames as input arguments as
% shown.
figure
hold on
axis equal;
colormap('gray');
h = waitbar(0,'mapping');
for i = 1:10
    myMap = occGridMapping(ranges(:,1:floor((i)*end/10)), scanAngles, pose(:,1:floor((i)*end/10)), param);
    % The final grid map: 
    imagesc(myMap); 
    waitbar(i/9,h);
end
% colormap('gray'); 

#这是建图的程序，也就是具体实现
occGridMapping.m

% Complete this function  
function myMap = occGridMapping(ranges, scanAngles, pose, param)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5
% Parameters 
% 
% the number of grids for 1 meter.
myResol = param.resol;
% the initial map size in pixels
myMap = zeros(param.size);
% the origin of the map in pixels
myorigin = param.origin; 
% 
% % 4. Log-odd parameters 
lo_occ = param.lo_occ;
lo_free = param.lo_free; 
lo_max = param.lo_max;
lo_min = param.lo_min;

lidarn = size(scanAngles,1);
N = size(ranges,2);

for i = 1:N
%     % Find occupied-measurement cells and free-measurement cells
%       distances/ranges
    theta = pose(3,i);
    x = pose(1,i);
    y = pose(2,i);
    local_occs = [ranges(:,i).*cos(scanAngles+theta),-ranges(:,i).*sin(scanAngles+theta)];
    grid_rob = ceil(myResol * [x;y]);
%     % Update the log-odds
%      -calculate
    for j = 1:lidarn
        real_occ = local_occs(j,:) + [x y];
        grid_occ = ceil(myResol * real_occ);
         %     % Saturate the log-odd values
        [freex,freey] = bresenham(grid_rob(1),grid_rob(2),grid_occ(1),grid_occ(2));
         free = sub2ind(size(myMap),freey+myorigin(2),freex+myorigin(1));
         occ = sub2ind(size(myMap), grid_occ(2)+myorigin(2), grid_occ(1)+myorigin(1));
         %     % Visualize the map as needed
         myMap(free) = myMap(free) - lo_free;
         myMap(occ) = myMap(occ) + lo_occ;
    end
end
myMap(myMap < lo_min) = lo_min;
myMap(myMap > lo_max) = lo_max;

end

结果如图

作业2 EKF定位

ShowErrorEllipse.m

%将x估计的协方差以分布画出来，椭圆形
function ShowErrorEllipse(xEst,PEst)
Pxy=PEst(1:2,1:2);%x,y处的
[eigvec, eigval]=eig(Pxy);
if eigval(1,1)>=eigval(2,2)
    bigind=1;
    smallind=2;
else
    bigind=2;
    smallind=1;
end
chi=9.21;
t=0:10:360;
a=sqrt(eigval(bigind,bigind)*chi);
b=sqrt(eigval(smallind,smallind)*chi);
x=[a*cosd(t);
   b*sind(t)];
angle = atan2(eigvec(bigind,2),eigvec(bigind,1));
if(angle < 0)
    angle = angle + 2*pi;
end
R=[cos(angle) sin(angle);
   -sin(angle) cos(angle)];
x=R*x;
plot(x(1,:)+xEst(1),x(2,:)+xEst(2))

ExtendedKalmanFilterLocalization.m

% -------------------------------------------------------------------------
% Homework2
% File : ExtendedKalmanFilterLocalization.m
%
% Discription : Mobible robot localization sample code with
% Extended Kalman Filter (EKF)
% -------------------------------------------------------------------------
 
function [] = ExtendedKalmanFilterLocalization()
 
close all;
clear all;

%设定仿真时间及步长
time = 0;
endtime = 60; % [sec]
global dt;
dt = 0.1; % [sec]
nSteps = ceil((endtime - time)/dt);

%结果初始化
result.time=[];
result.xTrue=[]; % 机器人真实位姿
result.xd=[];
result.xEst=[];  %机器人估计位姿
result.z=[];     %观测值
result.PEst=[];  %估计协方差
result.u=[];     %控制量
  
% State Vector [x y yaw v]'
xEst=[0 0 0 0]'; 
PEst = eye(4);
% True State
xTrue=xEst; 
% Dead Reckoning
xd=xTrue; 
% Observation vector [x y yaw v]'
z=[0 0 0 0]'; 
% Covariance Matrix for motion
Q=diag([0.1 0.1 toRadian(1) 0.05]).^2; 
% Covariance Matrix for observation
R=diag([1.5 1.5 toRadian(3) 0.05]).^2;
 
% Simulation parameter
global Qsigma
Qsigma=diag([0.1 toRadian(20)]).^2; %[v yawrate] 
global Rsigma
Rsigma=diag([1.5 1.5 toRadian(3) 0.05]).^2;%[x y z yaw v]
 
tic; %开始计时

% Main loop
for i=1 : nSteps
    time = time + dt;
    % 仿真控制输入Input
    u=doControl(time);
    % 仿真机器人运动及观察，获得Observation 
    [z,xTrue,xd,u]=Observation(xTrue, xd, u);
    %-------待补充如下代码---------------
    %-----------------------------------
    % -------- Kalman Filter -----------
    % step1：预测
    % （1）计算出当前时刻机器人位姿的预测值，xPred
    % （2）计算上述预测值的协方差，PPred
    xEst = f(xTrue,u);
    jf = jacobF(xEst,u);
    PPred = jf*PEst*(jf')+Q;
    
    % step2：观测
    % （1）计算真实观测和预测观测间的误差，即新息，y
    % （2）计算新息的协方差，S
    y = z-h(xEst);
    jh = jacobH(xEst);
    S = jh*PPred*(jh')+R;
    % step3：更新
    % （1）更新机器人位姿预测为估计值，xEst
    % （2）更新估计值的协方差，PEst
    wt = PPred*(jh')/S;
    xEst = xEst+wt*y;
    PEst = PPred-wt*S*(wt');
    
    % Simulation Result
    result.time=[result.time; time];
    result.xTrue=[result.xTrue; xTrue'];
    result.xd=[result.xd; xd'];
    result.xEst=[result.xEst;xEst'];
    result.z=[result.z; z'];
    result.PEst=[result.PEst; diag(PEst)'];
    result.u=[result.u; u'];
    
    %Animation (remove some flames)
    if rem(i,5)==0
        plot(result.xTrue(:,1),result.xTrue(:,2),'.b');hold on;
        plot(result.z(:,1),result.z(:,2),'.g');hold on;
        plot(result.xd(:,1),result.xd(:,2),'.k');hold on;
        plot(result.xEst(:,1),result.xEst(:,2),'.r');hold on;
        ShowErrorEllipse(xEst,PEst);
        axis equal;
        grid on;
        drawnow;
    end 
end
toc %计时结束
%movie2avi(mov,'movie.avi'); %把仿真动画存为视频文件。 
DrawGraph(result);

function x = f(x, u)
% 理想运动模型Motion Model
global dt; 
F = [1 0 0 0
    0 1 0 0
    0 0 1 0
    0 0 0 0]; 
B = [
    dt*cos(x(3)) 0
    dt*sin(x(3)) 0
    0 dt
    1 0]; 
x= F*x+B*u;
 
function jF = jacobF(x, u)
% 运动模型的雅克比矩阵 Jacobian of Motion Model
global dt; 
jF=[
    1 0 0 0
    0 1 0 0
    -dt*u(1)*sin(x(3)) dt*u(1)*cos(x(3)) 1 0
     dt*cos(x(3)) dt*sin(x(3)) 0 1];

function z = h(x)
%理想观测模型Observation Model
H = [1 0 0 0
    0 1 0 0
    0 0 1 0
    0 0 0 1 ]; 
z=H*x;

function jH = jacobH(x)
%观测模型的雅克比矩阵Jacobian of Observation Model
jH =[1 0 0 0
    0 1 0 0
    0 0 1 0
    0 0 0 1];

%运动控制仿真，根据时间来仿真运动控制量u
function u = doControl(time)
%Calc Input Parameter
T=10; % [sec] 
% [V yawrate]
V=1.0; % [m/s]
yawrate = 5; % [deg/s] 
u =[ V*(1-exp(-time/T)) toRadian(yawrate)*(1-exp(-time/T))]';
 
%观测，真实的观测结果，带有噪声的。
function [z, x, xd, u] = Observation(x, xd, u)
%Calc Observation from noise prameter
global Qsigma;
global Rsigma;
x=f(x, u);% 没有噪声的情况，利用运动模型来计算机器人的位姿x，即真实值
u=u+Qsigma*randn(2,1);%add Process Noise
xd=f(xd, u);% 仿真Dead Reckoning，带有噪声，利用运动模型来预测机器人位姿
z=h(x+Rsigma*randn(4,1));%仿真观察Simulate Observation


function []=DrawGraph(result)
%Plot Result
figure(1);
x=[ result.xTrue(:,1:2) result.xEst(:,1:2) result.z(:,1:2)];
set(gca, 'fontsize', 16, 'fontname', 'times');
plot(x(:,5), x(:,6),'.g','linewidth', 4); hold on;
plot(x(:,1), x(:,2),'-.b','linewidth', 4); hold on;
plot(x(:,3), x(:,4),'r','linewidth', 4); hold on;
plot(result.xd(:,1), result.xd(:,2),'--k','linewidth', 4); hold on;
 
title('EKF Localization Result', 'fontsize', 16, 'fontname', 'times');
xlabel('X (m)', 'fontsize', 16, 'fontname', 'times');
ylabel('Y (m)', 'fontsize', 16, 'fontname', 'times');
legend('Ground Truth','GPS','Dead Reckoning','EKF','Error Ellipse');
grid on;
axis equal;

function angle=Pi2Pi(angle)
angle = mod(angle, 2*pi);
i = find(angle>pi);
angle(i) = angle(i) - 2*pi;
i = find(angle<-pi);
angle(i) = angle(i) + 2*pi;

function radian = toRadian(degree)
% degree to radian
radian = degree/180*pi;

function degree = toDegree(radian)
% radian to degree
degree = radian/pi*180;

结果

感想

虽然做出来结果，但我对整个过程仍然不太了解。抱着实力划水的精神，不明所以地完成了两个作业，强烈diss湘江人工智能学院，有认识的朋友评论一下，大家一起吐槽。