多智能体编队怎样随领队转向？用固定偏移计算目标位置并核对距离

多移动智能体要随领队转向并保持指定队形，先要把“各自站在哪里”写成几何目标。固定的队形偏移属于领队的局部坐标；领队旋转后，偏移也必须旋转，再加上领队的世界位置。只给每个成员的世界坐标加同一个常量，可以平移，却不能实现队形随朝向转动。

本文演示二维虚拟领队和三个点智能体的编队基础。已在Python 3.11.15执行坐标映射、有限步跟踪和一个交叉轨迹拒绝案例；没有运行机器人、训练强化学习策略或证明现实系统稳定性。输入是人工构造的有限数字，全部坐标采用同一长度单位。

把固定槽位写在领队坐标系里

虚拟领队位姿设为(2,1,90度)，三个槽位偏移为(0,0)、(−1,−1)、(−1,1)。第一个槽位位于领队参考点，后两个位于其后侧。这是教学队形，不对应某种真实机器人间距规范。虚拟领队只是参考位姿，不是代码里另一个需要积分的实体。

局部偏移(dx,dy)转换为世界目标：x目标=x领队+cosθ×dx−sinθ×dy，y目标=y领队+sinθ×dx+cosθ×dy。θ在计算时用弧度。90度旋转将(−1,−1)变成(1,−1)，所以第二目标是(3,0)；第三目标是(1,0)，第一目标是(2,1)。旋转和平移保留槽位之间的距离，不能把三个成员都拉向同一个平均位置。

这一目标位置关系依据Petrinić与Petrović的2013年编队论文第III节式(4)。论文还讨论非完整机器人运动学与跟踪控制；本文仅使用目标几何关系，自写简单点位置步进，没有复现其控制律或稳定性证明。资料于2026-10-03北京时间读取。

位置跟踪先算候选，再核对整步距离

为便于核算，领队保持不动，三个点的初始位置都在各自目标下方2个单位。每轮朝自己的目标移动最多0.25单位，因此这一共同平移在8轮后完成。程序先从全部旧位置计算候选位置，再一次性返回；原列表不会被边算边修改。

端点距离够大，途中仍可能相撞。例如(−1,0)与(1,0)交换位置，两端距离都是2，线性运动中点却重合。因此检查相对位移r(t)=r0+t×d在0到1之间的最小长度。最接近时刻为−r0·d/|d|²，截到[0,1]；相对位移不变时直接查原距离。这个检查假设所有点在同一轮内按相同归一化进度线性移动。

运行最小编队核对

保存为formation_demo.py，执行python -X utf8 formation_demo.py。示例无需第三方库。可以让AI按上述坐标关系编写实现，但应要求它输出旋转后的三个目标、每步移动上限、队形距离和交叉失败断言，再由本地程序验收。

from math import cos, sin, radians, hypot, isclose
from itertools import combinations

def targets(leader, offsets):
    x, y, degrees = leader
    theta = radians(degrees)
    return [(x + cos(theta)*dx - sin(theta)*dy,
             y + sin(theta)*dx + cos(theta)*dy) for dx, dy in offsets]

def swept_distance(a, b, na, nb):
    # Relative position r(t)=r0+t*d, 0<=t<=1; minimize its norm.
    r = (a[0]-b[0], a[1]-b[1])
    d = ((na[0]-a[0])-(nb[0]-b[0]),
         (na[1]-a[1])-(nb[1]-b[1]))
    d2 = d[0]**2 + d[1]**2
    t = max(0.0, min(1.0, -(r[0]*d[0]+r[1]*d[1])/d2)) if d2 else 0.0
    return hypot(r[0]+t*d[0], r[1]+t*d[1])

def checked_step(current, desired, max_step=0.25, min_distance=1.0):
    if len(current) != len(desired) or max_step <= 0 or min_distance <= 0:
        raise ValueError('invalid step parameters')
    nxt = []
    for p, q in zip(current, desired):
        dx, dy = q[0]-p[0], q[1]-p[1]
        length = hypot(dx, dy)
        scale = min(1.0, max_step/length) if length else 0.0
        nxt.append((p[0]+scale*dx, p[1]+scale*dy))
    for i, j in combinations(range(len(current)), 2):
        if swept_distance(current[i], current[j], nxt[i], nxt[j]) < min_distance-1e-12:
            raise ValueError('distance violated during linear step')
    return nxt  # input list is unchanged, including on failure

offsets = [(0,0), (-1,-1), (-1,1)]
goal = targets((2,1,90), offsets)
expected = [(2,1), (3,0), (1,0)]
assert all(isclose(a,b,abs_tol=1e-12) for p,q in zip(goal,expected) for a,b in zip(p,q))
print('targets', [tuple(round(v,6) for v in p) for p in goal])
pos = [(x,y-2) for x,y in goal]
for _ in range(8):
    old = pos[:]
    pos = checked_step(old, goal)
    assert all(hypot(a[0]-b[0],a[1]-b[1]) <= .25+1e-12 for a,b in zip(old,pos))
error = max(hypot(p[0]-q[0],p[1]-q[1]) for p,q in zip(pos,goal))
assert error < 1e-12
print('eight_step_error', round(error,12))
for i,j in combinations(range(3),2):
    assert isclose(hypot(goal[i][0]-goal[j][0],goal[i][1]-goal[j][1]),
                   hypot(offsets[i][0]-offsets[j][0],offsets[i][1]-offsets[j][1]))
print('shape_distances_preserved', True)

start = [(-1,0), (1,0)]
before = start[:]
try:
    checked_step(start, [(1,0),(-1,0)], max_step=2)
except ValueError:
    pass
else:
    raise AssertionError('crossing accepted')
assert start == before
print('crossing_rejected_unchanged', True)

输出对应哪一项要求

targets [(2.0, 1.0), (3.0, -0.0), (1.0, 0.0)]
eight_step_error 0.0
shape_distances_preserved True
crossing_rejected_unchanged True

targets应与(2,1)、(3,0)、(1,0)一致，显示中的−0.0是浮点零的符号表现，不是目标偏到负坐标。eight_step_error按12位小数显示为0.0，实际断言检查误差小于10⁻¹²。形状检查分别比较三对槽位距离，未把“到达一个共同位置”当保持队形。

crossing的True说明两点交换被拒绝，调用前的位置列表仍然原样。拒绝只给出失败结果，没有自动规划绕行路线。若希望完成该交换，需要另外设计可行轨迹；删掉距离检查只能掩盖冲突，不能让任务完成。

哪些条件一变就需要重新分析

本文输入坐标已确认有限，程序没有完整的外部输入验证；不要直接向它传入NaN、无穷或错误维度。max_step是每轮位移上限，不是未指定时间单位的速度。min_distance=1.0是教学阈值，也不是现实安全间距。浮点容差只用于本例数值误差。

本例不考虑障碍物、实体半径、加速度、朝向约束或网络延迟。真实机器人可能无法侧移，轮速也不等于位置直接改变；必须采用合适的运动学与控制器。领队持续运动时，成员可能追不上，8轮完成结论只适用于这里静止目标下的共同2单位偏移。

下一次只改变一个条件

先把领队朝向改为0度，手算三目标应为(2,1)、(1,0)、(1,2)，再让AI解释为什么距离不变。之后再改一个领队位置，检查平移关系。若目标间距本身小于设定阈值，应先改队形设计；若候选路径被拒绝，保留原位置并报告阻塞，不声称已经到达。

Python 3.11 math官方文档支持radians、sin、cos、hypot及isclose的用法；具体槽位、步长、距离阈值与轨迹检查由本文教学规则给定。