机器人局部地图：基于角度与单元格数求数组索引问题求助

核心坐标转换逻辑 #

先明确两个坐标系的映射关系：

• 机器人视角：中心为(0,0)，范围(-100,-100)到(100,100)
• C/C++数组：左下角为(0,0)，范围(0,0)到(200,200)
  映射公式：

数组x索引 = 机器人视角x坐标 + 100
数组y索引 = 机器人视角y坐标 + 100

角度与坐标计算的正确实现 #

非特殊角度失效的核心原因通常是取整方式错误或角度坐标系定义不匹配，以下是标准实现步骤：

1. 统一角度坐标系 #

假设机器人角度定义为：

• 0°：x轴正方向（向右，对应数组x增大）
• 90°：y轴正方向（向上，对应数组y增大）
• 角度逆时针递增（符合数学坐标系）
  如果你的机器人用顺时针递增角度（比如0°向前、90°向右），需要调整三角函数参数，见下文示例。

2. 计算机器人视角坐标 #

给定角度theta（度）和单元格数n，执行以下步骤：

• 将角度转为弧度：double rad = theta * M_PI / 180.0;
• 计算连续坐标：double robot_x = n * cos(rad); double robot_y = n * sin(rad);
• 四舍五入到最近单元格：int rx = round(robot_x); int ry = round(robot_y);

  注意：不能用强制类型转换（如int(robot_x)）截断小数，否则非特殊角度会出现坐标偏移。

3. 转换为数组索引并做边界处理 #

将机器人视角坐标转为数组索引，同时确保索引在0~200范围内：

int arr_x = rx + 100;
int arr_y = ry + 100;
// 边界钳位
arr_x = std::clamp(arr_x, 0, 200);
arr_y = std::clamp(arr_y, 0, 200);

完整C++示例代码 #

#include <cmath>
#include <algorithm>

// 输入：角度theta（度），单元格数n
// 输出：数组的x、y索引
void getArrayIndex(double theta, int n, int& arr_x, int& arr_y) {
    // 逆时针角度转弧度
    double rad = theta * M_PI / 180.0;
    
    // 计算机器人视角连续坐标
    double robot_x = n * cos(rad);
    double robot_y = n * sin(rad);
    
    // 四舍五入到整数单元格
    int rx = static_cast<int>(round(robot_x));
    int ry = static_cast<int>(round(robot_y));
    
    // 转换为数组索引并做边界处理
    arr_x = std::clamp(rx + 100, 0, 200);
    arr_y = std::clamp(ry + 100, 0, 200);
}

// 如果机器人角度是顺时针递增（0°向前，90°向右），使用此版本
void getArrayIndexClockwise(double theta, int n, int& arr_x, int& arr_y) {
    double rad = theta * M_PI / 180.0;
    // 顺时针角度的坐标分量转换
    double robot_x = n * sin(rad);
    double robot_y = n * cos(rad);
    
    int rx = static_cast<int>(round(robot_x));
    int ry = static_cast<int>(round(robot_y));
    
    arr_x = std::clamp(rx + 100, 0, 200);
    arr_y = std::clamp(ry + 100, 0, 200);
}

常见错误排查 #

1. 取整方式错误：用截断替代四舍五入，导致非特殊角度的坐标偏差
2. 角度方向颠倒：机器人角度是顺时针递增，但代码用了逆时针的三角函数逻辑
3. y轴映射错误：误将机器人视角y正方向对应数组y减小方向（数组(0,0)是左下角，y正方向对应数组y增大）
4. 浮点数精度问题：cos/sin计算的微小误差导致四舍五入错误，可添加微小偏移修正：round(robot_x + 1e-6)

内容的提问来源于stack exchange，提问作者user18455025