Dubins path生成matlab程序

Dubins path生成matlab程序

function [tpq] = produce_dubinspaths（alphabetadflag）

%========单位化状态下的程序=========%

% alphabeta单位为弧度；从x轴逆时针旋转为正，在取值值保持始终为正
%d为无量纲的相对长度
%t是第一次转弯角度（单位为弧度）
%p是直线距离
%q是第二次转弯角度（单位为弧度）

%flag代表路径类型
%lsl flag=1
%lsr flag=2
%rsr flag=3
%rsl flag=4

switch flag
    case 1
        %LSL路径计算
        t_lsl=-alpha+atan（（cos（beta）-cos（alpha））/（d+sin（alpha）-sin（beta）））;
        t_lsl=mod2pi（t_lsl）;
        p_lsl=sqrt（2+d^2-2*cos（alpha-beta）+2*d*（sin（alpha）-sin（beta）））;
        q_lsl=beta-atan（（cos（beta）-cos（alpha））/（d+sin（alpha）-sin（beta）））;
        q_lsl=mod2pi（q_lsl）;
        t=t_lsl;
        p=p_lsl;
        q=q_lsl;
        
    case 2
        %LSR路径计算
        p_lsr=sqrt（-2+d^2+2*cos（alpha-beta）+2*d*（sin（alpha）+sin（beta）））;
        t_lsr=-alpha+atan（（-cos（beta）-cos（alpha））/（d+sin（alpha）+sin（beta）））-atan（-2/p_lsr）;
        t_lsr=mod2pi（t_lsr）;
        q_lsr=-beta+atan（（-cos（beta）-cos（alpha））/（d+sin（alpha）+sin（beta）））-atan（-2/p_lsr）;
        q_lsr=mod2pi（q_lsr）;
        t=t_lsr;
        p=p_lsr;
        q=q_lsr;

    case 3
        %RSR路径计算
        t_rsr=alpha-atan（（cos（alpha）-cos（beta））/（d-sin（alpha）+sin（beta）））;
        t_rsr=mod2pi（t_rsr）;
        p_rsr=sqrt（2+d^2-2*cos（alpha-beta）+2*d*（sin（beta）-sin（alpha）））;
        q_rsr=-beta+atan（（cos（alpha）-cos（beta））/（d-sin（alpha）+sin（beta）））;
        q_rsr=mod2pi（q_rsr）;
        t=t_rsr;
        p=p_rsr;
        q=q_rsr;
        
    case 4 
        %RSL路径计算
        p_rsl=sqrt（-2+d^2+2*cos（alpha-beta）-2*d*（sin（alpha）+sin（beta）））;
        t_rsl=alpha-atan（（cos（beta）+cos（alpha））/（d-sin（alpha）-sin（beta）））+atan（2/p_rsl）;
        t_rsl=mod2pi（t_rsl）;
        q_rsl=beta-atan（（cos（beta）+cos（alpha））/（d-sin（alpha）-sin（beta）））+atan（2/p_rsl）;
        q_rsl=mod2pi（q_rsl）;
        t=t_rsl;
        p=p_rsl;
        q=q_rsl;
end

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        1892  2016-10-31 21:12  cal_dubinspaths.m
     文件      500019  2016-10-30 19:24  Classification of the Dubins set.pdf
     目录           0  2017-02-16 20:21  __MACOSX\
     文件         177  2016-10-30 19:24  __MACOSX\._Classification of the Dubins set.pdf
     文件         182  2016-10-28 23:16  mod2pi.m
     文件        1454  2016-10-30 19:20  plot_path.m
     文件        1234  2016-11-06 11:52  produce_dubinspaths.m