deadreckontimevelocity.m

来自「该程序用PID系统实现差动转向车辆控制」· M 代码 · 共 24 行

%% Author: epokh
%% Website: www.epokh.org/drupy
%% This software is under GPL


function new_position=deadReckonTimeVelocity(position,vr,vl,t,bw)
%% This function calculate the new position of the robot
%% with some approssimation on the direct kinematic
%% from the left wheel speed, right wheel speed and simulation time
%% bw is the axe's length

x0=position(1);
y0=position(2);
theta0=position(3);

sr=vr*t;
sl=vl*t;

theta=theta0+(sr-sl)/bw;
x=x0+(sr+sl)/2*(cos(theta));
y=y0+(sr+sl)/2*(sin(theta));
new_position=[x,y,theta];

end