机器人定位中的航迹推算

?? ? ??? ? ???? ??(4.9) 其中, ' AA 表示 A 点到'A 点的距离。当机器人走直线时 sin( ) / 0 2 2 ? ?? ??,则式(4.8),(4.9) 可写成: cos( ) 2 sin( ) 2 x S y S ??????? ?????????? ??????(4.10) 由于时间间隔 t?很短, 线段' AA 的长度近似等于圆弧长度, 设t?时间内编码盘走过的距离为l ,则 l S ??。设 1?为上次采样的角度, 2?为当前的角度,则 2 ? ??? ??。通过计算 t?时间内机器人的位置变化量, x y ? ?,进行累加就可以求出机器人在整个场地上的位置。 1 2 ' ' cos( ) 2 ( ) cos( ) 2 cos( ) 2 x x x x AA x l x l ??? ??? ??? ?????? ??? ???? ??(4.11 ) 1 2 ' ' sin( ) 2 ( ) sin( ) 2 sin( ) 2 y y y y AA y l y l ??? ??? ??? ?????? ??? ???? ??(4.12 ) 这样,在初始坐标(x,y) 的基础上,每个采样周期都计算一次更新坐标( ', ') x y , 这个( ', ') x y 相当于下一个周期的初始坐标( , ) x y ,如此循环,就可以知道任意时刻机器人所位置在的坐标。