移动机器人运动模型

最新推荐文章于 2025-05-18 12:58:35 发布

原创最新推荐文章于 2025-05-18 12:58:35 发布 · 6.4k 阅读

19 ·

CC 4.0 BY-SA版权

文章标签：

#移动机器人 #模型 #matlab

Mobile Robotics 专栏收录该内容

4 篇文章

订阅专栏

假定移动机器人的位姿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仿真结果如下：