二维位置计算原理之地图旋转平移

问题是这样产生的：给室内定位机器人上传完整地图后，小车无法识别自己在当前上传地图中的位置。

解决方法(四步走)：

1、记录原地图中机器人当前位置（Pre_Position）及两个显著目标位置（Pre_GoalPosition1 和 Pre_GoalPosition2）；

2、记录当前地图中与第一步中对应的两个显著目标位置（Current_GoalPosition1 和 Current_GoalPosition2）；

3、计算原地图与当前地图的平移量(Map_Translation)与旋转角度（Map_AngleRotation）。

     （1）平移量(Map_Translation)的计算方法：Map_Translation = Current_GoalPosition1 - Pre_GoalPosition1 ；

     （2）计算旋转角度

       首先找到两个地图的两个向量值：Pre_vector  和 Current_vector （Pre_vector = Pre_GoalPosition2 -Pre_GoalPosition1 ；Current_vector = Current_GoalPosition2 -Current_GoalPosition1）

  然后计算两向量Pre_vector 与Current_vector夹角，利用公式

cos=a*b/[|a|*|b|]

=(x1x2+y1y2)/[√[x1^2+y1^2]*√[x2^2+y2^2]]

4、坐标系变换

(1)、坐标系变换原理

我们从平面直角坐标系出发，坐标系O-XY和坐标系O'-X'Y'的关系如下图所示：

对于以上坐标关系，我们可以作如下描述：坐标系O-XY逆时针旋转theta（或顺时针旋转-theta）后与坐标系O'-X'Y'重合，或者坐标系O‘-X’Y‘顺时针旋转theta（或逆时针旋转-theta）后与坐标系O-XY重合。利用简单的之间三角形的余弦定理和正玄定理就可以得到以下公式（格式编辑不太熟，故以图像代替）：

（2）、坐标变换方法

当前地图的位置向量Current_Position_vector 可以由 Pre_Position_vector 计算出来，其中Pre_Position_vector = Pre_Position - Pre_GoalPosition1;Current_Position_vector = Current_Position - Current_GoalPosition1。