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

已知上一时刻机器人位置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)$