LQR路径跟踪.rar

线性约束下的二次型规划控制算法实现路径跟踪实例，MATLAB源码分享，MATLAB2012a版本已经跑通了

function K=Func_Alpha_Pos（XbYbXnYn）
AngleY=Yn-Yb;
AngleX=Xn-Xb;
%***求Angle*******%
if Xb==Xn
    if Yn>Yb
        K=pi/2;
    else
        K=3*pi/2;
    end
else
    if Yb==Yn
        if Xn>Xb
            K=0;
        else
            K=pi;
        end
    else
        K=atan（AngleY/AngleX）;
    end    
end
%****修正K使之在0~360°之间*****%
   if （AngleY>0&&AngleX>0）%第一象限
        K=K;
    elseif （AngleY>0&&AngleX<0）||（AngleY<0&&AngleX<0）%第二、三象限
        K=K+pi;
    else if （AngleY<0&&AngleX>0）%第四象限
            K=K+2*pi;  
        else
            K=K;
        end
    end
end

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件        631  2014-05-30 10:20  Chapter_3_5_3\Func_Alpha_Pos.m

     文件        873  2014-05-30 10:20  Chapter_3_5_3\Func_CircularReferenceTrajGenerate.m

     文件        128  2014-05-30 10:20  Chapter_3_5_3\Func_Theta_Pos.m

     文件        838  2014-05-30 10:20  Chapter_3_5_3\Func_VehicleKineticModule_Euler.m

     文件       3502  2014-05-30 10:20  Chapter_3_5_3\LQR_1.m

     目录          0  2019-07-17 10:39  Chapter_3_5_3

----------- ---------  ---------- -----  ----

                 5972                    6