二维矩形机器人绕中心旋转避障的最大转角计算方案

1. 推导机器人矩形的顶点坐标 #

已知机器人中心M(Mx, My)、前点F(Fx, Fy)、后点B(Bx, By)，先提取矩形关键参数：

• 中线方向单位向量：u_x = (Fx - Mx)/d, u_y = (Fy - My)/d，其中d = sqrt((Fx-Mx)^2 + (Fy-My)^2)（机器人半长，中心到前/后端的距离）
• 宽度方向单位向量：v_x = -u_y, v_y = u_x（顺时针垂直中线方向）；v'_x = u_y, v'_y = -u_x（逆时针垂直中线方向）
• 设机器人半宽为w（中心到侧边的距离，由机器人实际尺寸确定），则矩形四个顶点坐标为：
  • 前左顶点：P1 = (Mx + d*u_x + w*v_x, My + d*u_y + w*v_y)
  • 前右顶点：P2 = (Mx + d*u_x - w*v_x, My + d*u_y - w*v_y)
  • 后左顶点：P3 = (Mx - d*u_x + w*v_x, My - d*u_y + w*v_y)
  • 后右顶点：P4 = (Mx - d*u_x - w*v_x, My - d*u_y - w*v_y)

2. 单个障碍物的临界转角计算 #

机器人绕中心M旋转时，每个顶点做圆周运动。对单个障碍物O(O_x, O_y)，计算临界转角（顶点刚好接触障碍物时的角度）：

变量定义 #

• 障碍物相对中心的偏移：Ox' = O_x - Mx, Oy' = O_y - My，偏移距离D = sqrt(Ox'^2 + Oy'^2)
• 顶点Pi相对中心的偏移：dx = Pi.x - Mx, dy = Pi.y - My，旋转半径r_i = sqrt(dx^2 + dy^2)
• 碰撞阈值ε（一般取0，即刚好接触）

核心方程 #

旋转后顶点Pi与障碍物O的距离等于ε时，满足：

A*cosθ + B*sinθ = C

其中：

• A = Ox'*dx + Oy'*dy
• B = Oy'*dx - Ox'*dy
• C = (r_i^2 + D^2 - ε^2)/2

求解转角 #

用辅助角公式将方程转化为：

cos(θ - φ) = C/K

其中K = sqrt(A^2 + B^2)，φ = arctan2(B, A)。当|C| ≤ K时，方程有解：

θ = φ ± arccos(C/K)

将弧度转换为角度：θ_deg = θ * 180 / π

3. 确定最大可转动角度 #

• 对所有障碍物和所有顶点，计算所有临界转角θ_deg（逆时针为正，顺时针为负）
• 排除初始状态（θ=0）已碰撞的情况
• 逆时针方向：取所有正临界角度中的最小值，即为逆时针最大无碰撞转角
• 顺时针方向：取所有负临界角度中的最大值（最接近0的负数），即为顺时针最大无碰撞转角
• 按1度精度取整（四舍五入或截断均可）

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