双轮差速模型机器人通过线速度、角速度计算机器人位姿

已知上一时刻机器人位置P_OLD (x,y,\theta),机器人当前时刻的线速度和角速度(v,\omega),短时间内t内,机器人在线性部分和非线性部分的增量为

线性部分:

 dx =v*t*\cos(\theta )

dy = v*t*\sin(\theta)

d\theta = t*\omega

非线性部分: 

dx{}' = t*(v/\omega )*(-\sin(\theta+\omega*t)+\sin(\theta))dy' = t*(v/\omega )*(\cos(\theta+\omega*t )-\cos(\theta))

由于\omega可能非常小,导致非线性部分数值不稳定, 因此可以对以上部分做泰勒级数展开,我们只做一阶泰勒展开  f(x+dx) = f(x)+f'(x)*dx

所以-\sin(\theta+\omega*t) = -(sin(\theta)+cos(\theta)*\omega*t) ,所以

dx{}' =-t*(v/\omega )*(\cos(\theta)*\omega*t )=-v*t^2*\cos(theta)

dy' = t*(v/\omega )*(-\sin(\theta)*\omega *t)=-v*t^2*\sin(\theta)

你可能感兴趣的:(机器人,人工智能)