二维平面目标点跟踪

一.移动机器人运动学

        这部分内容非常简单，而且参考书籍在西交大出版社有中文版，翻译更加没有意义。详细内容参见《Introduction to Autonomous Mobile Robots》第3章。下面针对具体的问题做一些分析。

二.matlab验证

       假定当前环境不存在障碍，给定起始点和终止点，计算应给给移动机器人多大的速度和角速度使其顺利从起始点运动到终止点，其中起始点和终止点中包含位姿信息。

       matlab源码如下：

clc; clear;

StartPoint=[180,60,pi/2]'; %[xl,yl,theta]
GoalPoint=[0,0,0]';
StartPoint(3)=atan2(GoalPoint(2)-StartPoint(2),GoalPoint(1)-StartPoint(1));
K=[0.3,0.13,0.13];  %gain of rho,alpha and beta

T=100;
State=zeros(T,3);
PosOrient=zeros(T,3);
PosOrient(1,:)=StartPoint;

for t=1:T-1
    deltaX=GoalPoint(1)-PosOrient(t,1);
    deltaY=GoalPoint(2)-PosOrient(t,2);

    rho=sqrt(deltaX^2+deltaY^2);
    alpha=-PosOrient(t,3)+atan2(deltaY,deltaX);
    beta=-PosOrient(t,3)-alpha;
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    if alpha>pi
        alpha=alpha-2*pi;
    else if alpha<-pi
            alpha=alpha+2*pi;
       end
    end
    if beta>pi
        beta=beta-2*pi;
    else if beta<-pi
            beta=beta+2*pi;
        end
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    if alpha>-pi/2 && alpha<=pi/2
        v=K(1)*rho;
    else
        v=-K(1)*rho;
    end
    w=K(2)*alpha+K(3)*beta;
    
    deltaRho=-v*cos(alpha);
    deltaAlpha=v*sin(alpha)/rho-w;
    deltaBeta=-v*sin(alpha)/rho;
    
    rho=rho+deltaRho;
    alpha=alpha+deltaAlpha;
    beta=beta+deltaBeta;
    
    State(t,1)=rho;
    State(t,2)=alpha*180/pi;
    State(t,3)=beta*180/pi;
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
    %forward kinematics
    PosOrient(t+1,1)=PosOrient(t,1)+v*cos(PosOrient(t,3));
    PosOrient(t+1,2)=PosOrient(t,2)+v*sin(PosOrient(t,3));
    PosOrient(t+1,3)=PosOrient(t,3)+w; 
    
    if PosOrient(t+1,3)>pi
        PosOrient(t+1,3)=PosOrient(t+1,3)-2*pi;
    else if PosOrient(t+1,3)<-pi
            PosOrient(t+1,3)=PosOrient(t+1,3)+2*pi;
        end
    end
end

figure;
set(gcf,'Color','white');
t=0:T-1;
subplot(2,1,1);
xlabel('time'); ylabel('state');
plot(t,PosOrient(t+1,1),t,PosOrient(t+1,2),t,PosOrient(t+1,3)*180/pi,'r-');
legend('x','y','theta');
grid on;
subplot(2,1,2);
xlabel('x'); ylabel('y');
plot(PosOrient(t+1,1),PosOrient(t+1,2));
plot(PosOrient(t+1,1),PosOrient(t+1,2),'o-');
legend('route');
grid on;

PosOrient(:,3)=PosOrient(:,3)*180/pi;

       matlab仿真的结果如下：

       图中的曲线通过调整rho,alpha和beta参数可以使得机器人能够较好地跟踪至目标位置。由于alpha和beta的方向相反，所以计算角速度w的时候两个分量会出现相互抵消的现象。如果K(1)过小，接近目标点的速度会很慢；如果K(2)过大，前进时会左右摇摆得很厉害；如果K(3)过小，则向全局坐标系x轴接近的速度会降低。问题是在参数那儿，经过调试发现K(2)和K(3)的大小接近时效果相对较好。角速度w设置为0时，轨迹为非常平稳的直线，但是目标点的位姿一直不变。说明在机器人前进过程中，实时调整姿态会影响到从目标点到终止点的轨迹，越是希望在过程中调整到准确的位姿，得到的轨迹越不理想。当然这种控制方式的局限性是它是基于线性系统的比例控制，如果希望实时地调整位姿，可以使用曲线拟合的方式效果更显而易见。

       注意：正向运动学的计算是基于全局坐标系的原点为(0,0,0)的情况，否则需要额外的坐标系转换。