手把手教用matlab做无人驾驶（十三）-- 后轮反馈控制算法（Rear wheel feedback）

后轮反馈控制算法：Rear wheel feedback 文章题目：A Survey of Motion Planning and Control Techniques for Self-driving Urban Vehicles

文章下载地址：http://xueshu.baidu.com/usercenter/paper/show?paperid=114c0fdaa1d0d5ed6f114fa66ffe4643&site=xueshu_se

后轮反馈式是利用后轮中心的跟踪偏差来进行转向控制量计算的方法，根据运动学方程(2-1)及车辆后轮与参考路径的几何关系，可推导出参考路径坐标系(?, ?)，Ferent 坐标系，下 (?, ?, ?_? ) 的变化率为:

̇其中，?表示参考路径上离本车最近点和本车的横向位置误差， ?_? 为参考路径上离本车最近点的切线角和本车的横摆角的误差值，?(?)为参考点?处的曲率， ?_? 为车辆后轮线速度，?为车身横摆角速度

对于二次连续可导的参考线，需要设计车身横摆角速度?保证在李亚普洛夫方程 ?( ?, ?_? ) = ?^2 + ?_?^2⁄?_2 下局部渐进收敛。李亚普洛夫稳定用数学的语言描述为:可控可微分的状态方程 ?̇ = ?(?,?) ，在给定的参考轨迹 $?_{??f}(?)$ 下，满足下列条件： ∀_?>0 t; ?_1<?_2 lt;?_2lt;?_2?_2，存在?>0:

对于李亚普洛夫稳定也分为渐进稳定和指数稳定。渐进稳定指对于时变系统，?在条件(1)下独立于时间 ?_1 ;指数稳定指收敛率是以指数下降。
对于后轮反馈式算法，为了保证车辆的李亚普洛夫稳定性，?可表示为式(2-10)

其中， ?_? 为横摆角偏差反馈控制增益， ?_? 为横向位置偏差反馈控制增益。因此，根据运动学方程(2-1)，可得到前轮转角?为:

这里大概讲解了一下原理，具体大家可以下载论文看看。

现在给出matlab代码，这里的代码是在我前面博客中stanley算法代码中改过来的：

% Editor:Robert
% Date:2019.6.11

function steercmd = fcn(RefPos,direction,CurPos,curvelocity,curvature)
KTH = 60;
KE=0.5;
gain=20;
wheelbase=0.15;
% Clip heading angle to be within [0, 360) and convert to radian

RefPos(3) =RefPos(3)*pi/180 ;
CurPos(3)= CurPos(3)*pi/180;

%to the interval [0 2*pi] such that zero maps to
%zero and 2*pi maps to 2*pi
twoPiRefPos = cast(2*pi, 'like', RefPos(3));
twoPiCurPos = cast(2*pi, 'like', CurPos(3));

positiveInputRefPos = RefPos(3) > 0;
positiveInputCurPos = CurPos(3) > 0;

thetaRef = mod(RefPos(3), twoPiRefPos);
thetaCur = mod(CurPos(3), twoPiRefPos);

positiveInputRefPos = (thetaRef == 0) & positiveInputRefPos;
thetaRef = thetaRef + twoPiRefPos*positiveInputRefPos;

positiveInputCurPos = (thetaCur == 0) & positiveInputCurPos;
thetaCur = thetaCur + twoPiCurPos*positiveInputCurPos;

RefPos(3)=thetaRef ;
CurPos(3)=thetaCur;

%This function ensures that angError is in the range [-pi,pi).

angError =CurPos(3)-RefPos(3);

piVal = cast(pi, 'like', angError);

twoPi = cast(2*pi, 'like', angError);

positiveInput = (angError+piVal)> 0;

theta = mod((angError+piVal), twoPi);

positiveInput = (theta == 0) & positiveInput;
theta = theta + twoPi*positiveInput;

theta=theta-piVal;

angError=theta;

d = norm(CurPos(1:2) - RefPos(1:2));

if angError==0
omega=0;
else
omega = curvelocity*curvature*cos(angError)/(1-curvature*d)-KTH*abs(curvelocity)*angError-KE*curvelocity*sin(angError)*d/angError
end

delta=atan2(wheelbase*omega/curvelocity,1);

delta=delta*180/pi;

delta = sign(delta) * min(abs(delta), 35);

steercmd = delta;
这里的代码跟前面的基本一样，不同的就是omega = curvelocity*curvature*cos(angError)/(1-curvature*d)-KTH*abs(curvelocity)*angError-KE*curvelocity*sin(angError)*d/angError，这个对于上面公式(2-10)和delta=atan2(wheelbase*omega/curvelocity,1)对应公式（2-11）。

仿真框架图：