Robotics simulation in MATLAB not working

Discussion in 'Programmer's Corner' started by orestes, May 1, 2016.

  1. orestes

    Thread Starter New Member

    May 1, 2016
    6
    0
    Hi, I am not sure if this belongs here, please if don't I would appreciate if a mod can move it. I am working in reproducing a robotics paper, first simulating it in MATLAB for applied to a real robot afterwards. The robot's model is:

    [​IMG]

    The idea is to apply an algorithm to avoid obstacles and reach a determines target. This algorithm uses a cone vision to measure the obstacle's properties. The information required to apply this system is:
    1) The minimum distance d(t) between the robot and the obstacle (this obstacle is modelled as a circle of know radius R).
    2) The obstacle's speed v_{obs}(t)
    3)The angles alpha_{1}(t) y \alpha_{2}(t) that form the robot's cone vision and
    4) the heading H(t) from the robot to the target

    [​IMG]

    First a safe distance d_{safe} between the robot and the obstacle is defined. The robot has to reach the target without being closer than d_safe to the obstacle.
    An extended angle \alpha_{0} >= arccos(R/(R+d_{safe}) is defined, where 0 <= \alpha_{0} <= \pi

    Then the following angles are calculated:

    \beta_{1}(t)=\alpha_{1}(t)-\alpha_{0}(t)
    \beta_{2}=\alpha_{2}(t)+\alpha_{0}(t)

    [​IMG]

    Then the following vectors are defined:

    l_{1}=(V_{max}-V)[cos(\beta_{1}(t)),sin(\beta_{1}(t))]
    l_{2}=(V_{max}-V)[cos(\beta_{2}(t)),sin(\beta_{1}(2))]

    here V_{max} is the maximum robot's speed and V a constant that fulfills ||v_{obs}(t)|| <= V <= V_{max}

    These vectors represent the boundaries of the cone vision of the vehicle

    Given the vectors l_{1} and l_{2}, the angle alpha(l_1,l_2) is the angle between l_{1} and l_{2} measured in counterclockwise direction, with \alpha element of (-\pi,\pi). The the function f is defined as


    f(l_1,l_2) = {

    0 & : \alpha(l_1,l_2) =0 \\
    1 &: 0 \le \alpha(l_1,l_2) \le \pi \\
    -1 & : -\pi \le \alpha(l_1,l_2) \le 0

    The evasion maneuver starts at time t_0. For that the robot find the index h:

    h = min|\alpha(v_{obs}(t_0)+l_j(t_0) , v_R(t_0))||

    where j={1,2} and v_R is the robot's velocity vector

    Then, from the two vectors v_{obs}(t_0)+l_j(t_0) we choose that one that forms the smallest angle with the robot's velocity vector. Once h is determinded, the control law is applied:

    u(t)=-U_{max} f(v_{obs}(t)+l_h(t),v_R(t))
    V(t)=||v_{obs}(t)+l_h(t)||


    This is a sliding mode type control law, that steers the robot's velocity v _R(t) towards a switching surface a una equal to the vector v_{obs}(t)+l_h(t). Ideally the robot avoids the obstacle by surrounding it a constant angle \alpha_0

    [​IMG]

    If the robot is not avoiding an obstacle it follows a control law:

    u(t)=0
    V(t)=V_{max}

    Hence the rules to switch between the two laws are:

    R10 Switching from (2) to (1) occurs whenthe distance to the obstacle is equal to a constant C, which means when d(t_0)=C and this distance is becoming smaller in time i.e. \dot{d(t)}<0

    R11Switching from (1) to (2) occurs when d(t_*)<1.1a_* and the vehicle is pointing towards the obstacle, i.e. \theta(t_*)=H(T_*)

    where a_*=R / (cos\alpha_0) - R

    Ideally the result should be similar to this

    [​IMG]

    But I'm getting this instead

    [​IMG]

    I've been trying to understand what's wrong with no result. In my opinion the robot manages to avoid the obstacle but at certain point, the robot turns to the wrong side, making impossible the condition H(t) = theta(t) to be achieved.

    After initializing the variables using a structure called ContxtSim, my main function called SimuPlatform is inside a loop using dt = 0.1s. First, I move the obstacle and the use an auxiliar variable to determine which of R10 o R11 is going to be used. After applying the deermined control law, the robot's movement is updated in the function RunProcessModel. In both R10 and R11 I first find H(t) y and calculate the required information from the obstacle, namely the angles \alpha_1 and \alpha_2, and the distance beween the robot and the obstacle

    Then u(t) and V(t) are calculated
    the relevant of my code is:

    function SimuPlatform(Dt)

    global ContextSim ;
    tk = ContextSim.tk ;
    % Simulate movement of the obstacle
    [ContextSim.OPreal, ContextSim.v1_t] = MovObst(ContextSim.OPreal,Dt) ;
    % Select current rule
    if tk == 1
    [ContextSim.u_t,ContextSim.v_t,ContextSim.tkn] = Rule11Stream(Dt) ;
    else
    [ContextSim.u_t,ContextSim.v_t,ContextSim.tkn] = Rule10Stream(ContextSim.r_1,ContextSim.alpha_0,ContextSim.theta) ;
    end
    tk = ContextSim.tkn ;
    % Simulate one step of the "real system": Xreal(time)
    [ContextSim.Xreal,ContextSim.cr_t,ContextSim.vr_t,ContextSim.theta] = RunProcessModel(ContextSim.Xreal,ContextSim.v_t,ContextSim.u_t,Dt) ;
    ContextSim.CurrSimulatedTime = ContextSim.CurrSimulatedTime+Dt ;

    return ;


    function [OPnext,v1_t] = MovObst(OP,dt)

    global ContextSim ;
    spdobs = ContextSim.spdobs ;
    spdobsx = spdobs*cos(3*pi/2) ;
    spdobsy = spdobs*sin(3*pi/2) ;
    v1_t = [spdobsx,spdobsy] ; % Vector velocity of the robot's centre of mass
    OPnext = OP + dt*[spdobsx ; spdobsy ] ; % Next obstacle's pose

    return ;

    function [u_t,v_t,tkn] = Rule11Stream(Dt)

    global ContextSim ;
    C = ContextSim.C ;
    % Measure distance and angle between robot and target
    [ContextSim.H_t] = MeasHeadTarget(ContextSim.Xreal) ;
    % Measure distance and angles between robot and moving obstacle
    [ContextSim.o_dist,ContextSim.d_dot,ContextSim.alpha_1,ContextSim.alpha_2] = MeasObstacle(ContextSim.Xreal,ContextSim.OPreal,ContextSim.r_1,ContextSim.theta,ContextSim.o_dist) ;
    ob_dist = ContextSim.o_dist ;
    d_dot = ContextSim.d_dot ;
    if (ob_dist <= C) && (d_dot < 0)
    [ContextSim.u_t,ContextSim.v_t,ContextSim.tk] = ApplyRule10(ContextSim.alpha_0,ContextSim.alpha_1,ContextSim.alpha_2,ContextSim.v1_t,ContextSim.vr_t,ContextSim.kt) ;
    u_t=ContextSim.u_t ;
    v_t=ContextSim.v_t ;
    else
    [ContextSim.u_t,ContextSim.v_t,ContextSim.tk] = ApplyRule11() ;
    u_t=ContextSim.u_t ;
    v_t=ContextSim.v_t ;
    end
    tkn = ContextSim.tk ;

    return ;

    function [u_t,v_t,tkn] = Rule10Stream(r_1,alpha_0,theta)

    global ContextSim ;
    [ContextSim.H_t]= MeasHeadTarget(ContextSim.Xreal) ;
    % Measure distance between robot and moving obstacle
    [ContextSim.o_dist,ContextSim.d_dot,ContextSim.alpha_1,ContextSim.alpha_2] = MeasObstacle(ContextSim.Xreal,ContextSim.OPreal,ContextSim.r_1,ContextSim.theta,ContextSim.o_dist) ;
    ob_dist = ContextSim.o_dist ;
    H_t = ContextSim.H_t ;
    a_1= r_1/cos(alpha_0)-r_1 ;
    if (ob_dist <= 1.1*a_1) && (abs(theta-H_t) < 0.01)
    [ContextSim.u_t,ContextSim.v_t,ContextSim.tk] = ApplyRule11() ;
    u_t=ContextSim.u_t ;
    v_t=ContextSim.v_t ;
    else
    [ContextSim.u_t,ContextSim.v_t,ContextSim.tk] = ApplyRule10(ContextSim.alpha_0,ContextSim.alpha_1,ContextSim.alpha_2, ContextSim.v1_t,ContextSim.vr_t,ContextSim.kt) ;
    u_t=ContextSim.u_t ;
    v_t=ContextSim.v_t ;
    end
    tkn = ContextSim.tk ;

    return ;

    function [u_t,v_t,tk] = ApplyRule10(alpha_0,alpha_1,alpha_2,v1_t,vr_t,kt)

    global ContextSim ;
    Vmax = ContextSim.Vmax ;
    V = ContextSim.V ;
    Umax = ContextSim.Umax ;
    beta_1 = alpha_1-alpha_0 ;
    beta_2 = alpha_2+alpha_0 ;
    l1 = (Vmax-V)*[cos(beta_1),sin(beta_1)] ; % Occlusion lines
    l2 = (Vmax-V)*[cos(beta_2),sin(beta_2)] ;
    aux_1 = v1_t+l1 ; % Auxiliary lines
    aux_2 = v1_t+l2 ;
    aux_angle_1 = atan(vr_t(2)/vr_t(1)) - atan(aux_1(2)/aux_1(1)) ; % MAYBE
    aux_angle_2 = atan(vr_t(2)/vr_t(1)) - atan(aux_2(2)/aux_2(1)) ;
    ContextSim.aux_1 = aux_1 ;
    ContextSim.aux_2 = aux_2 ;
    ContextSim.aux_angle_1 = aux_angle_1 ;
    ContextSim.aux_angle_2 = aux_angle_2 ;
    if kt == 1
    [h,kt] = choose_h(ContextSim.aux_angle_1,ContextSim.aux_angle_2) ;
    ContextSim.kt = kt ;
    ContextSim.h = h ;
    end
    h = ContextSim.h ;
    % Angle between l1 and l2:
    if h == 1
    aux_1 = ContextSim.aux_1 ;
    alpha_a = ContextSim.aux_angle_1 ; % Angle between l1 and l2
    f = findf(alpha_a) ;
    v_t = norm(aux_1) ;
    u_t = -Umax*f ;
    else
    aux_2 = ContextSim.aux_2 ;
    alpha_a = ContextSim.aux_angle_2 ;
    f = findf(alpha_a) ;
    v_t = norm(aux_2) ;
    u_t = -Umax*f ;
    end
    % Function and control law in(10). This law allows the robot "escape"
    % From a coming obstacle by "keeping it steady"
    ContextSim.alpha_a = alpha_a;
    tk = 2 ;

    return;

    function [u_t,v_t,tk] = ApplyRule11()

    global ContextSim ;
    Vmax = ContextSim.Vmax ;
    u_t = 0 ;
    v_t = Vmax ;
    tk = 1 ;
    kt = 1 ;
    ContextSim.kt = kt ;

    return;

    function [h,kt] = choose_h(aux_angle_1,aux_angle_2)

    if aux_angle_1 <= aux_angle_2
    h = 1;
    else
    h = 2;
    end
    kt = 2;

    return;

    function H_t = MeasHeadTarget(Xr)

    global landmark ;
    landmark = [0,5] ;
    EDx = landmark(1)-Xr(1) ;
    EDy = landmark(2)-Xr(2) ;
    H_t = atan2(EDy,EDx) ;

    return ;

    function [o_dist,d_dot,alpha_1, alpha_2] = MeasObstacle(X, OP, r_1, theta, o_dist_)

    o_dist_past = o_dist_ ;
    o_distx = sqrt((X(1)-OP(1))^2+(X(2)-OP(2))^2) ; % Distance between robot and obstacle's centre
    o_dist = o_distx-r_1 ; % Robot's distance to obstacle border
    d_dot = o_dist - o_dist_past ; % Finds the difference in the robots distance between time steps
    aux_t = atan2((OP(2)-X(2)),(OP(1)-X(1))) ; %-(pi/2-theta ); % Robot's angle to the obstacle center
    phi_c = asin(r_1/o_distx); % Auxiliary angle
    alpha_1 = aux_t-phi_c; % boundary angles of robot's cone vision
    alpha_2 = aux_t+phi_c;

    return;

    function f = findf(alpha_ang)

    if abs(alpha_ang) < 0.01
    f = 0 ;
    else if (0 < alpha_ang) && (alpha_ang < pi)
    f = 1 ;
    else
    f = -1 ;
    end
    end

    return;

    function [Xnext,cr_t,vr_t,theta] = RunProcessModel(X,v_t,u_t,dt)

    Xnext = X + dt*[ v_t*cos(X(3)) ; v_t*sin(X(3)) ; u_t ] ;
    cr_t = [Xnext(1),Xnext(2)] ;
    vr_t = [v_t*cos(Xnext(3)) v_t*sin(Xnext(3))] ;
    theta = Xnext(3) ;

    return;



    I've spent a lot of time trying to understand what is going wrong. I feel that I am not measuring properly the angle alpha between the v_{obs}(t)+l_h(t) and v_{obs}(t) , because in the debugging I can see that at certain point it stops switching between negative and positive values and become only positive, leading the robot's to the wrong side. I would gratly appreciate your help with this, any doubt please let me know.

    Best regards, Ander
     
  2. orestes

    Thread Starter New Member

    May 1, 2016
    6
    0
    Only testing something
     
Loading...