计算机控制系统-Stanley车道保持控制算法_基于神经网络车道保持控制代码-优快云博客

本文链接：https://blog.youkuaiyun.com/jaz/article/details/146485973

计算机控制系统课程，为了让同学们能动起来，我把CU-ICAR课程学习内容搬过来，希望Dr. Jia 看到后要鼓励我们借鉴哦。

像素到车辆的坐标转换关系没弄清楚，我咨询了Dr. Jia，他让我参看他和Longxiang的论文。

文献1：Teaching Autonomous Vehicles How to Drive under Sensing Exceptions by Human Driving Demonstrations （但我无法下载）

但我只能下载另一篇论文：IEEE Xplore Full-Text PDF:

Automatic Sensor Correction of Autonomous Vehicles by Human-Vehicle Teaching-and-Learning

1 控制思路

车辆组成如下图，摄像头进行车道识别（需要标定摄像头），MCU 控制转向和直行。

如果车辆两个电机分别控制转向和行驶，则比较简单；

如果是差速电机，只需要调整一个合适的转向速度差即可；直行的速度可设定为定值。

控制思路： 1 前置摄像头：识别车道线（看前面blog-车道线识别）；2 Stanley算法进行车道保持控制；3 Arduino控制左右电机

类似这个车。

2 车道保持控制器

可参看：自动驾驶控制算法——Stanley 算法（Stanley）_stanley控制-优快云博客

Stanley算法是一种基于几何路径的轨迹追踪控制器，其核心思想是将参考点设置在前轮中心，而非传统的重心、质心或后轮重心。通过控制车辆前轮转角来减小横向和航向误差，实现对目标路径的跟踪。具体来说，Stanley算法的前轮转角由两部分构成：一部分是航向误差引起的转角，即当前车身方向与参考轨迹最近点的切线方向的夹角；另一部分是横向误差引起的转角，即前轮速度方向与参考轨迹最近点的切线方向所成的夹角。

算法实现

确定车辆初始位置和目标路径：在车辆控制系统中，首先需要明确车辆的初始位置和目标路径。目标路径可以是预先规划好的车道中心线或其他期望的行驶轨迹。
实时获取车辆位姿信息：通过车辆上的传感器（如GPS、IMU， camera等），实时获取车辆的位置、速度、航向角等位姿信息。（课程中用camera）
计算横向误差和航向误差：根据车辆当前位姿和目标路径，计算横向误差（即车辆到目标路径的垂直距离）和航向误差（即车辆航向与目标路径切线方向的夹角）。
计算前轮转角控制量：根据横向误差和航向误差，利用Stanley算法的公式计算出前轮转角控制量。具体公式为：δ=θe+arcsin(k∗ey/v)，其中δ为前轮转角，θe为航向误差，ey为横向误差，v为车速，k为控制增益系数（需要调整）。
执行控制和循环迭代：将计算得到的前轮转角控制量发送给车辆的转向系统，控制车辆前轮的转动。随后继续实时获取车辆位姿信息，重复上述计算和控制过程，使车辆不断调整自身姿态，保持在目标路径上行驶

Stanley 方法是一种基于横向跟踪误差（前轴中心 B 到规划轨迹最近点 C 的距离）的非线性反馈函数，并且能实现横向跟踪误差收敛到 0。

需要做的就是：1 左右车道线；2 计算航向角误差和横向距离， 3 进行转向控制。

车道识别可实现上述参数提取，计算公式如下。

3 摄像头标定

stanley需要横向距离和航行偏差，则需要真实的信息，则需要将摄像头拍摄的相机坐标与车辆坐标系进行转换。

可参考：相机标定（Camera calibration）原理、步骤_视觉相机示教目的-优快云博客

可直接用matlab的相机标定工具来获得相机转换矩阵。

最好将摄像头置于前轮转向中心，否则需要进行坐标偏移。

相机标定矩阵，matlab自带相机标定工具，打印10*10的棋盘格即可。

%% load camera parameters
    load('down.mat');%摄像头的标定参数
    rot = down.RotationMatrices(:,:,2);
    trans = down.TranslationVectors(2,:)';

4 代码实现

手中没有小车，用之前代码和采集的图片进行展示。

采集的现场图

车道保持，中心线

相机坐标变换

lineLeft = pointsToWorld(cameraParams,rotationMatrix,translationVector,1/picscale*lineLeft); %%[xf,yf;xr,yr];
lineRight = pointsToWorld(cameraParams,rotationMatrix,translationVector,1/picscale*lineRight); %%[xf,yf;xr,yr];
  [r,c]=size(lineLeft');
  %disp('tempL')
tempL=[lineLeft';ones(1,c)];%[xf,yf;xn,yn]'; notice the its transform
tempR=[lineRight';ones(1,c)];%[xf,yf;xn,yn]';
% result is like this
%xf,  xn
%yf,  yn


%% the constant matrices is the change vector of image zero point
  T=[1 0 -70;0 -1 300];%% zero point of checkerboad by vehicle coordinate
 %word2veh=[1 0 -100;0 -1 220]*temp % change` to vehilce coordinate far [xf,yf;xr,yr] near, of center line
%% change to vehilce coordinate
 LineL2veh=T*tempL; %[xf,yf;xn,yn]';
LineR2veh=T*tempR;%[xf,yf;xn,yn]';
%xf,  xn
%yf,  yn
 CenLine=(LineL2veh+LineR2veh)/2;%[xf,yf;xn,yn]';
 %xf,  xn
%yf,  yn

 %% caculate the parameters
 % efa=xcn+(xcf-xcn)/(zcf-zcn)*(zcn-cpos)
 %  theta = atan2(zcf-zcn,xcf-xcn);
 the1=atan2((CenLine(2,1)-CenLine(2,2)),(CenLine(1,1)-CenLine(1,2)))-pi/2; %-90; %% angle

计算了航行偏差theta和横向距离efa

%% if the centerline is vertical to camera, efa will be inf. 
 % and efa should be the distance from original coordinate to centerline
 % NF. so we can use triangle to caculate the efa.
 % use vector to caculate the efa
 O=[0 0];
 N=CenLine(:,1)'; %% chagne to xn,yn
 F=CenLine(:,2)';
 efa=(det([N-F;O-F]))/norm(N-F);

控制结果：

将上述结果，按照Stanley算法进行合成，编写代码，需要把控制值映射到0~1范围，因为用的是arduino的servo模块。

 %% efa is from -150~200; theta is [-1.12,0.5]
    %% turn left :
    phi = theta_e + atan(kg*efa/speed1);  
        %% store the value
           storev(1,num)=phi;
           storev(2,num)=efa;    
    % mapping phi to control signal
    phi = max(min(phi,pi/4),-pi/4);
    temp1=phi;
    phi = (2*(pi/4-phi)/pi)*0.6+0.2;