概述

• 为全局定位添加校准功能
• 在关键位置校准机器人位姿，修正全局定位数据

• 防止难以避免的累计误差带来影响

  基本思路

• 利用激光测距在二维平面内定位机器人以及此时的自旋角，补充反馈机器人姿态信息。

计算方法

• 如图将激光固定于小车底盘边缘
• 需保证三条激光直线反向会交于底盘一点
• 同时测出中心点分别与三个激光发射器之间的距离bia1，bia2，bia3
• 分别与激光测距所得结果求和，测得distant1，distant2，distant3

解三角函数公式

• 如图当前机器人位置取第三象限

• 得出了小车边缘距墙壁的距离，以及自己的自旋角度

  注意

• 利用math.h文件求三角函数时，三角函数后面的数值应为弧度制

• 添加角度与弧度的换算的宏定义：角度 = 弧度*180/3.1415926