机器人的运动模型

机器人的运动模型

这篇文章主要介绍两轮机器人如何根据:
机器人轮子的编码信息和机器人当先位姿 (x1,y1,θ) ( x 1 , y 1 , θ ) 计算出下一时刻机器人的位姿 (x2,y2,θ2) ( x 2 , y 2 , θ 2 ) 。
即：
输入： (x1,y2,θ1) ( x 1 , y 2 , θ 1 ) 、 机器人左右轮子的间距 w w ，码盘的距离信息 l l 和 r r
输出： (x2,y2,θ2) ( x 2 , y 2 , θ 2 )
机器人在平面上运动时有两种情况，一种是转弯的情况，另一种是不转弯的情况；
下面分别进行介绍：

1. 转弯的情况

机器人在转弯的时候，其运动的示意图如下所示，其中角 ∠P1P1′P2′ ∠ P 1 P 1 ′ P 2 ′ 表示机器人P1点的头朝向，角 ∠P2P2′基线 ∠ P 2 P 2 ′ 基 线 表示机器人在P2点的头朝向。需要注意的是，头朝向 θ θ 是以 O O 为圆心， R+w/2 R + w / 2 为半径的圆的边界引出一条切线和基线之间的形成的夹角的大小。在图中，通过相似三角形的性质可以知道， ∠P1P1′P2′ ∠ P 1 P 1 ′ P 2 ′ = ∠O′OP1 ∠ O ′ O P 1 = θ1 θ 1
那么角度 θ2 θ 2 可以通过四边形OO’P2P2’的内角和为360度得到：

∠O′OP2+∠OO′P2′+∠O′P2′P2+∠OP2P2′=360 ∠ O ′ O P 2 + ∠ O O ′ P 2 ′ + ∠ O ′ P 2 ′ P 2 + ∠ O P 2 P 2 ′ = 360

即

(θ1+α)+90+(180−θ2)+90=360 ( θ 1 + α ) + 90 + ( 180 − θ 2 ) + 90 = 360

整理之后，得到，

θ2=θ1+α θ 2 = θ 1 + α

在中小学的时期，我们学到如下计算周长的公式：

C=2πr=r∫2π0dθ C = 2 π r = r ∫ 0 2 π d θ

由此可知，当 θ θ 的值很小的时候，周长=角度*半径。利用这种思想，我们可以构建如下的公式：

αR=l α R = l

α(R+w)=r α ( R + w ) = r

在上述公式中，只有 l l 和 r r 是已经知道的，但是我们可以根据该公式计算出 α α 和 R R :

α=r−lw α = r − l w

R=lwr−l R = l w r − l

现在已知 P1(x1,y1,θ1) P 1 ( x 1 , y 1 , θ 1 ) 、 α α 、 R R 和轮子之间的间距 w w ，如何求 P2 P 2 呢?

假设机器人在转弯的时候是以圆心 O O 为中心从 P1(x1,y1,θ1) P 1 ( x 1 , y 1 , θ 1 ) 运动到 P2(x2,y2,θ2) P 2 ( x 2 , y 2 , θ 2 ) ,
那么圆心的计算公式如下：

x0+(R+w2)sin(θ1)=x1 x 0 + ( R + w 2 ) s i n ( θ 1 ) = x 1

y0−(R+w2)cos(θ1)=y1 y 0 − ( R + w 2 ) c o s ( θ 1 ) = y 1

[x0y0]=[x1y1]+(R+w2)[−sin(θ1)cos(θ1)] [ x 0 y 0 ] = [ x 1 y 1 ] + ( R + w 2 ) [ − s i n ( θ 1 ) c o s ( θ 1 ) ]

知道圆心和从 P1 P 1 到 P2 P 2 之间移动角度大小为 θ1+α θ 1 + α ,那么 P2 P 2 的计算方法为：

[x0y0]+(R+w2)[cos(θ1+α)−sin(θ1+α)]=[x2y2] [ x 0 y 0 ] + ( R + w 2 ) [ c o s ( θ 1 + α ) − s i n ( θ 1 + α ) ] = [ x 2 y 2 ]

所以此时 P2(x2,y2,θ2) P 2 ( x 2 , y 2 , θ 2 ) 的坐标为

⎡⎣⎢x2y2θ2⎤⎦⎥=⎡⎣⎢⎢(R+w2)cos(θ1+α)−(R+w2)sin(θ1+α)θ1+α⎤⎦⎥⎥ [ x 2 y 2 θ 2 ] = [ ( R + w 2 ) c o s ( θ 1 + α ) − ( R + w 2 ) s i n ( θ 1 + α ) θ 1 + α ]

2. 不转弯的情况

当机器人不转弯的时候，机器人位姿中 θ1 θ 1 是不会发生变化的，且 l=r l = r ，所以这时

θ2=θ1 θ 2 = θ 1

而机器人的位姿会发生的变化如下图所示，

[x2y2]=[x1y1]+[lcos(θ1)lsin(θ1)] [ x 2 y 2 ] = [ x 1 y 1 ] + [ l c o s ( θ 1 ) l s i n ( θ 1 ) ]