动态向量场路径规划算法

动态向量场轨迹生成，是一种完全不同传统规划的方法，不预先计算一系列路径点，而是在整个工作空间定义一个连续的速度向量场，让机器人末端顺着场流自然流动，生成平滑、稳定的覆盖轨迹。

一、核心原理

1. 概念解释 想象在工作空间中，每一个点都关联一个速度向量，如果把所有点的速度向量画出来，就形成了一个速度场，就像气象图上的风向图。当机械臂末端进入这个场，它就会像一片被水流携带的树叶，沿着场的方向自动运动。

2. 数学模型 一个动态系统可以简单表示为：

x˙=f(x)

其中：

• x 是末端工具的当前位置（三维空间坐标）。
• x˙ 是该点的期望速度向量。
• f(x)是向量场函数，定义了空间中每一点的速度。

执行时，每个控制周期：

1. 根据当前工具位置 x计算期望速度 x˙。
2. 将该速度指令转换为关节速度，驱动机械臂运动。
3. 工具到达新位置 x+x˙Δt，循环往复。

3. 关键特性

• 轨迹在线生成：无需预存路径，轨迹是实时“流”出来的。
• 平滑连续：只要 f 是光滑函数，生成的轨迹自然平滑。
• 天然鲁棒：即使工具受外力偏离理论路径，场会立刻将它“拉回”正确的流动方向，不需重新规划。
• 易于组合：不同任务（覆盖、避障、趋向目标）可设计不同的子向量场，然后加权叠加。

二、极限环吸引子

这是实现重复性、周期性轨迹的核心工具，尤其适合内壁的螺旋式清洁。

1.概念解释 一个闭合的、孤立的周期轨道，周围的轨迹都会收敛到这个环上，无论从环内还是环外出发，最终都会绕圈稳定运行。

2. 构造一个绕圆柱的极限环 目标：让工具绕中心轴（Z轴）做半径为 R 的螺旋运动。

• 定义系统状态：在柱坐标系下，描述工具位置 (r,θ,z)。
• 设计径向动态：让半径收敛到目标值 R

若 r<R，r˙ 为正，工具向外移动；若 r>R，r˙ 为负，工具向内移动，最终稳定在 r=R 上。

• 设计角度动态：设置恒定角速度， θ˙=ω
• 设计高度动态：设置恒定下降速度， z˙=vz

组合起来，在笛卡尔坐标系中的向量场为：

这个场会使工具无论从哪里开始，都会被吸引到一个半径为 R的圆柱面上，并绕其做稳定的螺旋下降运动。

三、清洁步骤

1. 场函数设计：

示例A：内壁清洁场（极限环螺旋）

• 目标：形成稳定螺旋，覆盖内壁。
• 场分解：
  • 吸引子：r˙=α(R_est−r)，将工具拉向预估的曲面半径。
  • 旋转项：θ˙=ω，驱动绕中心轴旋转。
  • 下降项：z˙=v_z，控制清洁覆盖高度。
  • 安全排斥（可选）：若探测到过近障碍，加入一个反向速度场 x˙_rep。
• 合成：将所有项向量求和得到最终 x˙。

示例B：座圈清洁场（弓字形）

• 目标：形成平面往复扫描。
• 场分解：
  • 主方向切换：定义一条“引导线”（座圈长轴），工具被吸引到该线上，并沿线移动。
  • 横向偏移：到达边界后，横向跳跃一个行距，反向引导。
  • 可用两个一维极限环或分段的吸引场实现。

2. 参数在线粗估计（与场x`函数关联）

在执行前，通过粗感知获得场函数的关键参数：

• 内壁：中心轴 (P0,a)，半径 R，速度 ω,v_z，引力强度 α。
• 座圈：平面方程，边界角点，扫描速度。 这些参数直接注入到向量场的公式中，使其适配。

3. 伪代码流程：

# 设置场参数
R = estimate_radius()      # 通过基座相机或触觉探测
center = estimate_center()
alpha, omega, v_z = set_dynamics_params()

# 控制循环
while cleaning:
    x_current = get_tool_position()
    r = distance_to_axis(x_current, center)
    # 计算速度场
    dx_attract = alpha * (R - r) * radial_direction(x_current, center)
    dx_rotate = omega * tangential_direction(x_current, center)
    dx_z = v_z * z_axis
    dx_des = dx_attract + dx_rotate + dx_z

    # 力控修正（将力误差转换为速度修正量）
    F_err = F_des - F_contact
    dx_admittance = AdmittanceControl(F_err)   # 这是导纳输出的末端速度

    # 最终指令速度
    dx_cmd = dx_des + dx_admittance

    # 通过逆雅可比变换为关节速度并发送
    joint_vel = pinv(jacobian) * dx_cmd
    send_joint_velocity(joint_vel)

4. 粗补偿与场更新

• 半径 R 的动态更新：如果导纳控制器的平均径向修正持续偏正（工具一直在向外压），可缓慢增大 R，反之减小。

• 中心轴微调：类似地，可根据长期侧向力偏差缓慢平移中心点。

四、方案优势

• 极度简化路径规划：无需复杂的全覆盖算法，清洁轨迹由场的动力学自然生成。
• 在线适应性强：对初始定位误差、型号差异有很强的容忍度，场会主动将工具“拉”向正确的覆盖模式。
• 与控制无缝结合：向量场输出的速度指令，可直接与导纳控制器的力反馈速度指令线性叠加，框架极其优雅。
• 代码实现清晰：核心只是一个函数，输入位置，输出速度，易于调试和扩展。

这个方案可以让清洁轨迹规划部分，变成一个高度智能、自适应且鲁棒的运动行为生成器。