移动机器人运动模型

       假定移动机器人的位姿X=[x,y,theta]',其中x为机器人相对于世界坐标系的横坐标,y为机器人相对于世界坐标系的纵坐标,theta为机器人前进方向与世界坐标系横轴正方向的夹角,Xt为t时刻的位姿。控制指令U=[v,w],其中v表示机器人相对于机器人坐标系的前进速度,w表示机器人相对于该坐标系的角速度,逆时针为正方向。Ut为t时刻的控制输入。

       已知上一时刻和当前时刻的位姿以及当前时刻的控制输入,motion_model_velocity算法计算由上一时刻位姿和控制输入得到的后验概率。sample_motion_model_velocity算法根据motion_model_velocity算法得到的概率在固定输入ut和位姿xt-1的基础上产生随机样本。

matlab仿真代码如下:

clc; clear;

T=200;                                      %length of motion time
X=zeros(T,3);                               %motion state
r=25;                                       %assume that our robot moves along a circle which radius^2 equals 25
U=[ones(T,1)*r*pi/180 ones(T,1)*pi/180];    %expected motion command,including transitional and rotational velocity

Alpha=[6 6 6 6 5 5];                        %motion error parameters used to calculate the posteriors
AlphaOdom=[0.1 0.1 0.05 0.05]; 

for t=1:T                                   %calculte robot state every time it moves according to our route
    X(t,1)=r*cos(t*pi/180);                 
    X(t,2)=r*sin(t*pi/180);
    X(t,3)=pi-atan2(X(t,1),X(t,2));
end


X1=zeros(T,3);                              %robot state sampled from the velocity motion model
Prob=zeros(T,1);                            %motion error
X1(1,:)=X(1,:);

Xodom=zeros(T,3);                           %robot state=[rot1,trans,rot2]
Xcalc=zeros(T,3);
ProbOdom=zeros(T,1);
X2(1,:)=X(1,:);

rng(1);                                     %for reproductivity
for t=2:T

    %algorithm_motion_model_velocity
    mu=((X(t-1,1)-X(t,1))*cos(X(t,3))+(X(t-1,2)-X(t,2))*sin(X(t,3)))/((X(t-1,2)-X(t,2))*cos(X(t,3))-(X(t-1,1)-X(t,1))*sin(X(t,3)))*0.5;
    x=(X(t-1,1)+X(t,1))/2+mu*(X(t-1,2)-X(t,2));
    y=(X(t-1,2)+X(t,2))/2+mu*(X(t,1)-X(t-1,1)); 
    r=sqrt((X(t-1,1)-x)^2+(X(t-1,2)-y)^2);
    dTheta=atan2(X(t,2)-y,X(t,1)-x)-atan2(X(t-1,2)-y,X(t-1,1)-x);
    v=dTheta*r;
    w=dTheta;
    g=(X(t,3)-X(t-1,3))-dTheta;
    Prob(t,1)=ProbNormalDistribution(U(t-1,1)-v,Alpha(1)*abs(v)+Alpha(2)*abs(w))...
        *ProbNormalDistribution(U(t-1,2)-w,Alpha(3)*abs(v)+Alpha(4)*abs(w))...
        *ProbNormalDistribution(g,Alpha(5)*abs(v)+Alpha(6)*abs(w));

    %algorithm_sample_motion_model_velocity
    v=U(t,1)+SampleNormalDistribution(Alpha(1)*abs(U(t,1))+Alpha(2)*abs(U(t,2)));
    w=U(t,2)+SampleNormalDistribution(Alpha(3)*abs(U(t,1))+Alpha(4)*abs(U(t,2)));  
    g=Alpha(5)*abs(U(t,1))+Alpha(6)*abs(U(t,2));
    
    X1(t,1)=X(t-1,1)-v/w*sin(X(t-1,3))+v/w*sin(X(t-1,3)+w);
    X1(t,2)=X(t-1,2)+v/w*cos(X(t-1,3))-v/w*cos(X(t-1,3)+w);   
    X1(t,3)=X(t-1,3)+w+g;
    
    %algorithm_motion_model_odometry
    Xodom(t,1)=atan2(X1(t,1)-X1(t-1,1),X1(t,2)-X1(t-1,2))-X1(t-1,3);
    Xodom(t,2)=sqrt((X1(t-1,1)-X1(t,1))^2+(X1(t,2)-X1(t-1,2))^2);
    Xodom(t,3)=X1(t,3)-X1(t-1,3)-Xodom(t,1);
    
    Xcalc(t,1)=atan2(X(t,1)-X(t-1,1),X(t,2)-X(t-1,2))-X(t-1,3);
    Xcalc(t,2)=sqrt((X(t-1,1)-X(t,1))^2+(X(t,2)-X(t-1,2))^2);
    Xcalc(t,3)=X(t,3)-X(t-1,3)-Xcalc(t,1);
    
    ProbOdom(t,1)=ProbNormalDistribution(Xodom(t,1)-Xcalc(t,1),abs(AlphaOdom(1)*Xcalc(t,1)+AlphaOdom(2)*Xcalc(t,2)))...
                 *ProbNormalDistribution(Xodom(t,2)-Xcalc(t,2),abs(AlphaOdom(3)*Xcalc(t,2)+AlphaOdom(4)*(Xcalc(t,1)+Xcalc(t,3))))...
                 *ProbNormalDistribution(Xodom(t,3)-Xcalc(t,3),abs(AlphaOdom(1)*Xcalc(t,3)+AlphaOdom(2)*Xcalc(t,2)));
             
    %algorithm_sample_motion_odometry
    Xodom(t,1)=atan2(X1(t,1)-X1(t-1,1),X1(t,2)-X1(t-1,2))-X1(t-1,3);
    Xodom(t,2)=sqrt((X1(t-1,1)-X1(t,1))^2+(X1(t,2)-X1(t-1,2))^2);
    Xodom(t,3)=X1(t,3)-X1(t-1,3)-Xodom(t,1);
    
    Xcalc(t,1)=Xodom(t,1)-SampleNormalDistribution(abs(AlphaOdom(1)*Xcalc(t,1)+AlphaOdom(2)*Xcalc(t,2)));
    Xcalc(t,2)=Xodom(t,2)-SampleNormalDistribution(abs(AlphaOdom(3)*Xcalc(t,2)+AlphaOdom(4)*(Xcalc(t,1)+Xcalc(t,3))));
    Xcalc(t,3)=Xodom(t,3)-SampleNormalDistribution(abs(AlphaOdom(1)*Xcalc(t,3)+AlphaOdom(2)*Xcalc(t,2))); 
    
    X2(t,1)=X(t,1)+Xcalc(t,2)*cos(X(t,3)+Xcalc(t,1));
    X2(t,2)=X(t,2)+Xcalc(t,2)*sin(X(t,3)+Xcalc(t,1));
    X2(t,3)=X(t,3)+Xcalc(t,1)+Xcalc(t,3);
end

figure(1);
set(gcf,'Color','white');
t=1:T;
subplot(2,2,1);
xlabel('x'); ylabel('y');
plot(X(t,1),X(t,2),'r-',X1(t,1),X1(t,2),'bo-','MarkerSize',4,'MarkerFaceColor','b');
legend('True Route','Sample Route','Location','Southeast');
grid on;
subplot(2,2,2);
xlabel('t'); ylabel('motion error');
plot(t,X(t,1)-X1(t,1),t,X(t,2)-X1(t,2));
legend('x error','y error','Location','Southeast');
grid on;
subplot(2,2,3);
xlabel('x'); ylabel('y');
plot(X(t,1),X(t,2),'r-',X2(t,1),X2(t,2),'bo-','MarkerSize',4,'MarkerFaceColor','b');
legend('True Route','Sample Route','Location','Southeast');
grid on;
subplot(2,2,4);
xlabel('t'); ylabel('motion error');
plot(t,X(t,1)-X2(t,1),t,X(t,2)-X2(t,2));
legend('x error','y error','Location','Southeast');
grid on;

sum(X(:,1)-X1(:,1))
sum(X(:,1)-X2(:,1))

Alpha(1)和Alpha(2)反映机器人前进方向上的不确定性,Alpha(3)和Alpha(4)反映水平方向上的不确定性,Alpha(5)和Alpha(6)反映预测的重要性。这里假设将随机采样的状态值作为里程计的测量值,AlphaOdom(1)和AlphaOdom(2)反映里程计的平移误差,AlphaOdom(3)和AlphaOdom(4)反映里程计的旋转误差。

matlab仿真结果如下:

       栅格地图中有些部分会表示障碍,在计算后验概率时需要考虑是否遇到障碍,用p(x|m)表示在已知地图的前提下位姿出现在x处的概率。有障碍的地方将它设置成常数项(比如1),否则设置为0。最后将它乘以未加栅格地图时得到的后验概率,从而得到模型在当前时刻位姿为x的概率。

       注:模型的具体内容和推导参见《Probabilistic Robotics》第5章。

评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值