导航与中央节点初步实现（未测试）

README.md

@@ -23,7 +23,7 @@
 
 ## 目标
 
-感谢同济 SuperPower 战队！自瞄算法参考同济的 sp_vision_26 。
+感谢同济 SuperPower 战队！自瞄算法参考同济的 sp_vision_26，整车决策和节点图系统为自有设计。
 
 ## 软件架构
 
@@ -44,7 +44,7 @@ ROS2 基本架构为节点。以每个节点的发布、解析、接收数据为
 - 订阅：中央节点 -> 控制指令
 - 发送：CH340.TX -> 控制指令
 - 发布：裁判系统数据（血量、比赛当前状态）
-        CH340 、IMU 连接 OK 标志
+        CH340 连接 OK 标志
 
 ### IMU 收发节点
 
@@ -87,3 +87,9 @@ stateDiagram-v2
 - 接收：收发节点 -> IMU.地磁角, IMU.速度, IMU.加速度, IMU.六轴陀螺仪
         中央节点 -> 状态
 - 发布：导航运动指令
+
+导航思想简单描述：
+0. IMU 被安装在哨兵云台的 YAW 转盘上，坐标轴系向前为 IMU 接口中对应 X 轴，向右为 Y 轴，向下为 Z 轴。由于底盘作动在代码中设置为跟随云台朝向，因此控制量可以直接同步到 IMU 的这个坐标系中观察作用。
+1. 将本届 RMUL 地图简化为一个 2D 平面地图，整体轮廓为矩形，面积 $(12\times 8)  m^2$。设置原点为矩形左下角，矩形长（X 轴） 12 米，高（Y 轴）8 米，基地范围为矩形 \[(0,6),(1.5,8)\]，有两墙在图中近似为矩形，一个位于 \[(2.5,1.5),(4.5,8)\] 另一个位于其相对场地的几何中心的中心对称位置，不可跨越，不可通过。“增益点”范围为矩形 \[(4.5,2.5),(7,9.5)\]。
+2. 将机器人简化为一个中心点圆，IMU 位于中心点，圆直径为 0.75 米。开局时，因为没有对地图的重定位手段，人为放置机器人，使得机器人中心点圆与基地矩形下、右边缘相切，使云台朝向场地的 Y 轴方向，至此机器人与场地的 X、Y 轴坐标系映射完成。
+3. 后续依据此映射和 IMU 的数据，计算出机器人的当前位置，并根据当前状态规划一个绕过墙，前往“增益点”内的路径。目前的方法是直接计算出能使机器人中心点圆通过的路径并一直按该路径行动（设置几个路径点，中心点圆覆盖到路径点时认为路径部分完成）