因子避牆法的核心在於將「預期移動方向」與「基於牆壁距離計算的安全方向」作為加權因子,動態計算出最優移動角度。此法可無縫整合至現有移動演算法中。
首先建立角度正規化與方位計算的基礎工具:
private static final double DOUBLE_PI = Math.PI * 2;
private static final double HALF_PI = Math.PI / 2;
/** 計算從 (sourceX,sourceY) 到 (targetX,targetY) 的相對方位(弧度) */
public double calculateBearingToXYRadians(double sourceX, double sourceY,
double sourceHeading, double targetX, double targetY) {
return normalizeRelativeAngleRadians(
Math.atan2(targetX - sourceX, targetY - sourceY) - sourceHeading);
}
/** 將角度正規化至 [0, 2π) */
public double normalizeAbsoluteAngleRadians(double angle) {
if (angle < 0) return DOUBLE_PI + (angle % DOUBLE_PI);
return angle % DOUBLE_PI;
}
/** 將角度正規化至 (-π, π] */
public static double normalizeRelativeAngleRadians(double angle) {
double trimmed = angle % DOUBLE_PI;
if (trimmed > Math.PI) return -(Math.PI - (trimmed % Math.PI));
if (trimmed < -Math.PI) return Math.PI + (trimmed % Math.PI);
return trimmed;
}
為了靈活切換前進/後退,需覆寫移動與轉向方法,並引入 direction 實例變數(1 為前進,-1 為後退):
private int direction = 1; // 1=前進, -1=後退
/** 取得相對於當前移動方向的車頭絕對角度 */
public double getRelativeHeadingRadians() {
double h = getHeadingRadians();
if (direction < 1) h = normalizeAbsoluteAngleRadians(h + Math.PI);
return h;
}
/** 反轉移動方向 */
public void reverseDirection() {
double dist = getDistanceRemaining() * direction;
direction *= -1;
setAhead(dist);
}
/** 覆寫 setAhead:依 direction 調整實際移動距離 */
public void setAhead(double distance) {
double relDist = distance * direction;
super.setAhead(relDist);
if (distance < 0) direction *= -1;
}
public void setBack(double distance) {
double relDist = distance * direction;
super.setBack(relDist);
if (distance > 0) direction *= -1;
}
/** 最佳化左轉:若角度超過 90° 則改為反向右轉 */
public void setTurnLeftRadiansOptimal(double angle) {
double turn = normalizeRelativeAngleRadians(angle);
if (Math.abs(turn) > HALF_PI) {
reverseDirection();
turn = turn < 0 ? HALF_PI + (turn % HALF_PI) : -(HALF_PI - (turn % HALF_PI));
}
setTurnLeftRadians(turn);
}
public void setTurnRightRadiansOptimal(double angle) {
double turn = normalizeRelativeAngleRadians(angle);
if (Math.abs(turn) > HALF_PI) {
reverseDirection();
turn = turn < 0 ? HALF_PI + (turn % HALF_PI) : -(HALF_PI - (turn % HALF_PI));
}
setTurnRightRadians(turn);
}
此方法根據與牆壁的距離計算「安全點」(靠近牆壁時指向戰場中心),並將安全方位與預期方位按比例加權:
private static final double WALL_AVOID_INTERVAL = 10;
private static final double WALL_AVOID_FACTORS = 20;
private static final double WALL_AVOID_DISTANCE = WALL_AVOID_INTERVAL * WALL_AVOID_FACTORS;
private double adjustHeadingForWalls(double heading) {
double fieldH = getBattleFieldHeight();
double fieldW = getBattleFieldWidth();
double centerX = fieldW / 2, centerY = fieldH / 2;
double currHeading = getRelativeHeadingRadians();
double x = getX(), y = getY();
boolean nearWall = false;
double desiredX = x, desiredY = y;
// 判斷是否靠近牆壁,決定安全點
if (y < WALL_AVOID_DISTANCE || (fieldH - y) < WALL_AVOID_DISTANCE) {
desiredY = centerY; nearWall = true;
}
if (x < WALL_AVOID_DISTANCE || (fieldW - x) < WALL_AVOID_DISTANCE) {
desiredX = centerX; nearWall = true;
}
if (nearWall) {
double safeBearing = calculateBearingToXYRadians(x, y, currHeading, desiredX, desiredY);
double distToWall = Math.min(Math.min(x, fieldW - x), Math.min(y, fieldH - y));
int wallFactor = (int) Math.min(distToWall / WALL_AVOID_INTERVAL, WALL_AVOID_FACTORS);
// 加權平均:愈靠近牆壁,安全方位權重愈大
return (((WALL_AVOID_FACTORS - wallFactor) * safeBearing) + (wallFactor * heading))
/ WALL_AVOID_FACTORS;
}
return heading;
}
只需將導航演算法產生的角度送入 adjustHeadingForWalls 即可:
public void run() {
while (true) {
setTurnRightRadiansOptimal(adjustHeadingForWalls(0)); // 0 為直線前進
setAhead(100);
execute();
}
}
調整 WALL_AVOID_INTERVAL 與 WALL_AVOID_FACTORS 可改變機器人對牆壁的敏感度。此演算法簡單高效,是許多頂尖機器人生存能力的關鍵基石。