假定移动机器人的位姿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章。