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

室内定位机器人地图匹配
本文介绍了一种解决室内定位机器人地图匹配问题的方法。通过记录原地图与新地图中机器人的位置及显著目标位置,计算两者之间的平移量与旋转角度,并进行坐标系变换,使机器人能够在新地图中正确定位自身。

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

解决方法(四步走):

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。


评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值