机械臂连续轨迹控制与管道外壁温度采集系统 —— 基于样条插值与过程控制

“那年石化装置检修，机械臂沿管道爬行采集温度，结果速度忽快忽慢，导致红外热像图拉伸变形，差点漏掉一处微泄漏热点。后来我们用五次B样条 + 弧长参数化 + 前馈PID，让末端沿管道外壁匀速扫掠，温度数据终于变得连续可信。”

—— 哈尔滨工程大学《工业过程控制》课程核心思想延伸

一、实际应用场景描述

在石化管道、核电蒸汽管线、锅炉受热面等场景，机械臂需要沿复杂空间管道外壁连续运动，同时搭载红外热像仪或接触式测温探头进行表面温度场扫描：

┌──────────────────────────────────────────────┐

│ 机械臂连续轨迹控制与温度采集系统 │

│ │

│ [上位机轨迹规划与监控层] │

│ │ 轨迹下发 / 温度上传 / 状态监控 │

│ ▼ │

│ ┌────────────────────────────┐ │

│ │ 轨迹规划层 │ │

│ │ ┌──────────────────────┐ │ │

│ │ │ 1. 管道CAD模型导入 │ │ │

│ │ │ (中心线提取) │ │ │

│ │ └──────────────────────┘ │ │

│ │ ┌──────────────────────┐ │ │

│ │ │ 2. 五次B样条拟合 │ │ │

│ │ │ (G²连续) │ │ │

│ │ └──────────────────────┘ │ │

│ │ ┌──────────────────────┐ │ │

│ │ │ 3. 弧长参数化 │ │ │

│ │ │ (匀速运动核心) │ │ │

│ │ └──────────────────────┘ │ │

│ └────────────┬───────────────┘ │

│ │ 位置/速度/加速度指令 │

│ ┌───────┴───────┐ │

│ ▼ ▼ │

│ ┌─────────┐ ┌─────────┐ │

│ │ 运动学解算 │ │ 力位混合控制 │ │

│ │ • 逆运动学 │ │ • 恒力接触 │ │

│ │ • 雅可比 │ │ • 法向跟随 │ │

│ │ • 奇异位形 │ │ • 切向匀速 │ │

│ └────┬────┘ └────┬────┘ │

│ │ 关节角度/力矩 │ 末端位姿/力反馈 │

│ ▼ ▼ │

│ ┌────────────────────────────┐ │

│ │ 机械臂执行层 │ │

│ │ • 伺服驱动器(1kHz) │ │

│ │ • 关节编码器反馈 │ │

│ │ • 六维力传感器 │ │

│ └────────────┬───────────────┘ │

│ │ 实时运动 + 力反馈 │

│ ▼ │

│ ┌────────────────────────────┐ │

│ │ 物理世界 (高危环境) │ │

│ │ 🌡️ 管道外壁 (Φ219~Φ530) │ │

│ │ 🔥 高温蒸汽 (350~540°C) │ │

│ │ ⚡ 红外热像仪 (640×512) │ │

│ │ 📊 温度场重建 │ │

│ └───────────────────────────┘ │

│ │

│ 核心: 样条轨迹规划 + 弧长参数化 + 力位混合控制 │

└──────────────────────────────────────────────┘

传统点位运动 vs 连续轨迹控制

维度 传统点位运动（PTP） 连续轨迹控制（CP）

运动方式 ❌ 点到点跳跃 ✅ 连续平滑扫掠

速度均匀性 ❌ 启停冲击大 ✅ 匀速扫描（±2%）

温度采集 ❌ 离散点，易漏检 ✅ 连续场，全覆盖

末端姿态 ❌ 频繁突变 ✅ 法向恒定对准

设备磨损 ❌ 冲击大，寿命短 ✅ 平滑运动，寿命长

成像质量 ❌ 热像图畸变 ✅ 等间距像素，无畸变

二、引入痛点

2.1 现场的真实困境

场景 现场发生了什么 根因

“热像图被拉长” “管道拐弯处温度被稀释” 无弧长参数化，角速度突变

“漏掉微泄漏” “0.5°C的温升异常被平均掉了” 速度不均匀，采样密度变化

“探头划伤管壁” “接触力过大刮伤防腐层” 无力位混合控制

“伺服频繁报警” “加加速度（Jerk）过大” 轨迹不够光滑（低于G²）

“奇异位形卡死” “机械臂在弯头处失稳” 未考虑姿态约束

2.2 核心矛盾

温度场的“空间分辨率”取决于“末端运动的均匀性”。 如果机械臂沿管道运动时速度忽快忽慢，红外热像仪采集的像素间距就会忽大忽小，导致温度场重建失真。解决方案是：使用五次B样条生成 G² 连续的轨迹，并通过弧长参数化实现末端沿路径的匀速运动，同时利用六维力传感器实现法向恒力接触。

2.3 我们要解决什么

用一段精简的 Python 程序，构建一个 机械臂连续轨迹控制与管道温度采集仿真系统，实现：

1. 管道建模 —— 空间圆弧 + 直线组合的管道中心线

2. 轨迹规划 —— 五次 B 样条拟合，保证 G² 连续

3. 弧长参数化 —— 牛顿迭代求逆，实现匀速运动

4. 力位混合 —— 法向恒力 + 切向匀速的 Hybrid Control

5. 温度采集 —— 模拟沿路径的温度场采样

6. 可视化 —— 轨迹、速度剖面、温度场分布

三、核心逻辑讲解

3.1 理论基础：样条插值与弧长参数化

本工具基于哈工程《工业过程控制》第二章“系统数学模型”、第五章“状态空间分析”和第六章“PID 控制”：

① 五次 B 样条曲线

给定控制点 \mathbf{P}_i ，五次 B 样条基函数为：

N_{i,5}(u) = \sum_{j=i}^{i+5} \mathbf{P}_j \cdot B_{j,5}(u)

其中 B_{j,5}(u) 是五次 Bernstein 多项式。

优点：达到 G² 连续（曲率连续），加加速度（Jerk）有限，适合高速扫描。

② 弧长参数化（Arc-Length Parameterization）

定义弧长函数：

s(u) = \int_{0}^{u} \left\| \frac{d\mathbf{C}(\tau)}{d\tau} \right\| d\tau

其中 \mathbf{C}(u) 是样条曲线。

目标：找到映射 u = u(s) ，使得末端速度恒定：

\left\| \frac{d\mathbf{C}(u(s))}{ds} \right\| = 1

实现方法：牛顿迭代求解 s(u) - s_{target} = 0 。

③ 力位混合控制（Hybrid Position/Force Control）

将任务空间分解为：

- 位置控制子空间（切向）：控制沿管道的位移和速度

- 力控制子空间（法向）：控制探头对管壁的接触压力

控制律：

\begin{cases}

\mathbf{F}_{pos} = K_{p,p}\tilde{\mathbf{x}}_t + K_{d,p}\dot{\tilde{\mathbf{x}}}_t \\

\mathbf{F}_{force} = K_{p,f}(\mathbf{f}_d - \mathbf{f}) + K_{d,f}(\dot{\mathbf{f}}_d - \dot{\mathbf{f}})

\end{cases}

3.2 控制架构总览

┌─────────────┐

│ 管道CAD模型 │

│ (中心线点云) │

└──────┬──────┘

│

┌─────────▼─────────┐

│ 样条轨迹生成 │

│ • 控制点选取 │

│ • 五次B样条拟合 │

│ • G²连续性验证 │

└─────────┬─────────┘

│ 参数曲线 C(u)

┌─────────▼─────────┐

│ 弧长参数化 │

│ • 数值积分 s(u) │

│ • 牛顿迭代 u(s) │

│ • 速度规划 v(s) │

└─────────┬─────────┘

│ 弧长参数 s(t)

┌─────────▼─────────┐

│ 轨迹生成器 │

│ • 位置指令 x_d(t) │

│ • 速度指令 v_d(t) │

│ • 加速度指令 a_d(t) │

└─────────┬─────────┘

│ 笛卡尔空间指令

┌─────────▼─────────┐

│ 力位混合控制器 │

│ • 选择矩阵 S │

│ • 位置PID (切向) │

│ • 力PID (法向) │

└─────────┬─────────┘

│ 关节力矩指令

▼

┌─────────────┐

│ 机械臂执行层 │

└─────────────┘

四、代码讲解（面向对象设计）

4.1 类结构总览

类名 职责 设计模式

"Vector3" 三维向量（dataclass） 值对象

"PipeSegment" 管道段基类 抽象基类

"StraightPipe" 直管段 继承

"ElbowPipe" 弯头段 继承

"BSplineTrajectory" 五次B样条轨迹 策略模式

"ArcLengthParameterizer" 弧长参数化器 模板方法

"HybridController" 力位混合控制器 复合模式

"TemperatureSensor" 温度传感器模拟 工厂模式

"RobotArmSimulator" 机械臂仿真器（聚合根） 聚合根

"VisualizationEngine" 可视化引擎 封装

4.2 核心代码（完整可运行）

完整源码约 580 行，包含 9 个类、轨迹规划、力位控制、温度采集、可视化。

以下为精简核心版，完整代码可直接复制运行。

<details><summary>🔧 完整源码（点击展开/折叠）</summary>

"""

机械臂连续轨迹控制与管道外壁温度采集系统

参考哈尔滨工程大学《工业过程控制》第五章状态空间分析与第六章PID控制

"""

from dataclasses import dataclass, field

from typing import List, Tuple, Optional, Dict, Any

import numpy as np

import matplotlib.pyplot as plt

from abc import ABC, abstractmethod

import math

from collections import deque

import warnings

# ============================================================

# 1. 基础数据结构

# ============================================================

@dataclass

class Vector3:

"""三维向量 —— 值对象"""

x: float = 0.0

y: float = 0.0

z: float = 0.0

def __add__(self, other):

return Vector3(self.x + other.x, self.y + other.y, self.z + other.z)

def __sub__(self, other):

return Vector3(self.x - other.x, self.y - other.y, self.z - other.z)

def __mul__(self, scalar: float):

return Vector3(self.x * scalar, self.y * scalar, self.z * scalar)

def dot(self, other):

return self.x * other.x + self.y * other.y + self.z * other.z

def cross(self, other):

return Vector3(

self.y * other.z - self.z * other.y,

self.z * other.x - self.x * other.z,

self.x * other.y - self.y * other.x

)

def norm(self):

return math.sqrt(self.x**2 + self.y**2 + self.z**2)

def normalized(self):

n = self.norm()

if n < 1e-10:

return Vector3()

return Vector3(self.x/n, self.y/n, self.z/n)

def to_array(self):

return np.array([self.x, self.y, self.z])

def from_array(self, arr):

self.x, self.y, self.z = arr[0], arr[1], arr[2]

return self

# ============================================================

# 2. 管道几何模型

# ============================================================

class PipeSegment(ABC):

"""管道段抽象基类"""

@abstractmethod

def point_at(self, t: float) -> Vector3:

"""参数t对应的点 (t∈[0,1])"""

pass

@abstractmethod

def tangent_at(self, t: float) -> Vector3:

"""切向量"""

pass

@abstractmethod

def normal_at(self, t: float) -> Vector3:

"""法向量（指向外侧）"""

pass

@abstractmethod

def length(self) -> float:

"""近似长度"""

pass

class StraightPipe(PipeSegment):

"""直管段"""

def __init__(self, start: Vector3, end: Vector3, radius: float):

self.start = start

self.end = end

self.radius = radius

self.length_val = (end - start).norm()

def point_at(self, t: float) -> Vector3:

return self.start + (self.end - self.start) * t

def tangent_at(self, t: float) -> Vector3:

return (self.end - self.start).normalized()

def normal_at(self, t: float) -> Vector3:

# 默认向上为法向（指向外侧）

tangent = self.tangent_at(t)

up = Vector3(0, 0, 1)

# 如果切线接近竖直，改用x轴

if abs(tangent.dot(up)) > 0.9:

up = Vector3(1, 0, 0)

normal = up.cross(tangent).cross(tangent).normalized()

return normal * self.radius + self.point_at(t)

def length(self) -> float:

return self.length_val

class ElbowPipe(PipeSegment):

"""弯头（圆弧）"""

def __init__(self, center: Vector3, radius: float,

start_angle: float, end_angle: float,

normal: Vector3, clockwise: bool = False):

self.center = center

self.radius = radius

self.start_angle = start_angle

self.end_angle = end_angle

self.normal = normal.normalized()

self.clockwise = clockwise

self.angle_span = abs(end_angle - start_angle)

if clockwise and self.angle_span > 0:

self.angle_span = 2*math.pi - self.angle_span

def point_at(self, t: float) -> Vector3:

angle = self.start_angle + (self.end_angle - self.start_angle) * t

if self.clockwise:

angle = self.start_angle - (self.end_angle - self.start_angle) * t

# 构建局部坐标系

u = Vector3(1, 0, 0)

if abs(self.normal.dot(u)) > 0.9:

u = Vector3(0, 1, 0)

v = self.normal.cross(u).normalized()

u = v.cross(self.normal).normalized()

# 圆弧上的点

x = self.center.x + self.radius * (math.cos(angle) * u.x + math.sin(angle) * v.x)

y = self.center.y + self.radius * (math.cos(angle) * u.y + math.sin(angle) * v.y)

z = self.center.z + self.radius * (math.cos(angle) * u.z + math.sin(angle) * v.z)

return Vector3(x, y, z)

def tangent_at(self, t: float) -> Vector3:

angle = self.start_angle + (self.end_angle - self.start_angle) * t

dangle = 1e-6

p1 = self.point_at(t)

p2 = self.point_at(t + dangle)

return (p2 - p1).normalized()

def normal_at(self, t: float) -> Vector3:

# 指向圆心方向 + 半径偏移

point = self.point_at(t)

to_center = (self.center - point).normalized()

return to_center * self.radius + point

def length(self) -> float:

return self.radius * self.angle_span

# ============================================================

# 3. 五次B样条轨迹

# ============================================================

class BSplineTrajectory:

"""

五次B样条轨迹 —— 策略模式

保证G²连续，适合高速连续运动

"""

def __init__(self, control_points: List[Vector3], closed: bool = False):

self.control_points = control_points

self.closed = closed

self.n = len(control_points)

self.degree = 5

self.knots = self._generate_knots()

def _generate_knots(self) -> List[float]:

"""生成五次B样条的节点向量"""

m = self.n + self.degree + 1

knots = [0.0] * (self.degree + 1)

if self.closed:

# 闭合曲线

for i in range(1, self.n - self.degree):

knots.append(float(i) / (self.n - self.degree))

else:

# 开放曲线

for i in range(1, self.n - self.degree):

knots.append(float(i) / (self.n - self.degree))

knots.extend([1.0] * (self.degree + 1))

return knots

def basis_function(self, i: int, p: int, u: float) -> float:

"""递归计算B样条基函数"""

if p == 0:

return 1.0 if self.knots[i] <= u < self.knots[i+1] else 0.0

left = 0.0

if self.knots[i+p] != self.knots[i]:

left = (u - self.knots[i]) / (self.knots[i+p] - self.knots[i]) * \

self.basis_function(i, p-1, u)

right = 0.0

if self.knots[i+p+1] != self.knots[i+1]:

right = (self.knots[i+p+1] - u) / (self.knots[i+p+1] - self.knots[i+1]) * \

self.basis_function(i+1, p-1, u)

return left + right

def point_at(self, u: float) -> Vector3:

"""计算样条曲线上的点"""

u = max(0.0, min(1.0, u))

result = Vector3()

for i in range(self.n):

coeff = self.basis_function(i, self.degree, u)

result.x += self.control_points[i].x * coeff

result.y += self.control_points[i].y * coeff

result.z += self.control_points[i].z * coeff

return result

def derivative_at(self, u: float, order: int = 1) -> Vector3:

"""计算导数（数值差分）"""

du = 1e-6

if order == 1:

p1 = self.point_at(u - du)

p2 = self.point_at(u + du)

return (p2 - p1) * (0.5 / du)

elif order == 2:

p0 = self.point_at(u - du)

p1 = self.point_at(u)

p2 = self.point_at(u + du)

return (p0 - p1*2 + p2) * (1.0 / (du*du))

return Vector3()

def tangent_at(self, u: float) -> Vector3:

return self.derivative_at(u, 1).normalized()

def curvature_at(self, u: float) -> float:

"""计算曲率"""

d1 = self.derivative_at(u, 1)

d2 = self.derivative_at(u, 2)

cross = d1.cross(d2)

norm_d1 = d1.norm()

if norm_d1 < 1e-10:

return 0.0

return cross.norm() / (norm_d1 ** 3)

# ============================================================

# 4. 弧长参数化器

# ============================================================

class ArcLengthParameterizer:

"""

弧长参数化器 —— 模板方法

通过牛顿迭代实现 u = u(s) 的映射

"""

def __init__(self, trajectory: BSplineTrajectory, samples: int = 1000):

self.trajectory = trajectory

self.samples = samples

self.arc_length_table = []

self.u_table = []

self.total_length = 0.0

self._build_lookup_table()

def _build_lookup_table(self):

"""构建弧长查找表"""

self.arc_length_table = [0.0]

self.u_table = [0.0]

prev_point = self.trajectory.point_at(0.0)

total = 0.0

for i in range(1, self.samples + 1):

u = i / self.samples

curr_point = self.trajectory.point_at(u)

segment_len = (curr_point - prev_point).norm()

total += segment_len

self.arc_length_table.append(total)

self.u_table.append(u)

prev_point = curr_point

self.total_length = total

def arc_length(self, u: float) -> float:

"""计算从0到u的弧长"""

if u <= 0:

return 0.0

if u >= 1.0:

return self.total_length

# 线性插值查找

idx = int(u * self.samples)

if idx >= len(self.u_table) - 1:

return self.total_length

u1, u2 = self.u_table[idx], self.u_table[idx+1]

s1, s2 = self.arc_length_table[idx], self.arc_length_table[idx+1]

if abs(u2 - u1) < 1e-10:

return s1

return s1 + (s2 - s1) * (u - u1) / (u2 - u1)

def parameter_at_length(self, s_target: float, tolerance: float = 1e-6) -> float:

"""牛顿迭代求 u(s)"""

if s_target <= 0:

return 0.0

if s_target >= self.total_length:

return 1.0

# 二分查找初始值

left, right = 0.0, 1.0

for _ in range(20):

mid = (left + right) / 2

s_mid = self.arc_length(mid)

if s_mid < s_target:

left = mid

else:

right = mid

u_guess = (left + right) / 2

# 牛顿迭代精化

for _ in range(10):

s_u = self.arc_length(u_guess)

ds_du = self.trajectory.derivative_at(u_guess, 1).norm()

if ds_du < 1e-10:

break

delta = (s_target - s_u) / ds_du

u_guess += delta

if abs(delta) < tolerance:

break

return max(0.0, min(1.0, u_guess))

# ============================================================

# 5. 力位混合控制器

# ============================================================

class HybridController:

"""

力位混合控制器 —— 复合模式

切向位置控制 + 法向力控制

"""

def __init__(self):

# 位置PID参数（切向）

self.kp_pos = 50.0

self.ki_pos = 5.0

self.kd_pos = 10.0

# 力PID参数（法向）

self.kp_force = 0.5

self.ki_force = 0.05

self.kd_force = 0.1

# 状态

self.pos_integral = Vector3()

self.force_integral = 0.0

self.prev_pos_error = Vector3()

self.prev_force_error = 0.0

# 期望力（法向接触力，单位：N）

self.desired_normal_force = 5.0

# 选择矩阵（1=位置控制，0=力控制）

self.selection_matrix = np.array([

[1, 0, 0], # X轴：位置控制（切向）

[0, 1, 0], # Y轴：位置控制

[0, 0, 0] # Z轴：力控制（法向）

])

def compute(self, desired_pos: Vector3, current_pos: Vector3,

current_force: Vector3, dt: float) -> Vector3:

"""

计算控制输出

desired_pos: 期望位置（切向）

current_pos: 当前位置

current_force: 当前六维力传感器读数

dt: 控制周期

"""

# 位置误差（切向）

pos_error = desired_pos - current_pos

self.pos_integral = self.pos_integral + pos_error * dt

pos_derivative = (pos_error - self.prev_pos_error) / max(dt, 1e-6)

# 位置PID输出

pos_output = (

pos_error * self.kp_pos +

self.pos_integral * self.ki_pos +

pos_derivative * self.kd_pos

)

# 法向力误差（假设Z轴为法向）

normal_force = current_force.z

force_error = self.desired_normal_force - normal_force

self.force_integral += force_error * dt

force_derivative = (force_error - self.prev_force_error) / max(dt, 1e-6)

# 力PID输出

force_output = (

force_error * self.kp_force +

self.force_integral * self.ki_force +

force_derivative * self.kd_force

)

# 力位混合

output = Vector3()

output.x = self.selection_matrix[0, 0] * pos_output.x

output.y = self.selection_matrix[1, 1] * pos_output.y

output.z = (1 - self.selection_matrix[2, 2]) * force_output

# 保存状态

self.prev_pos_error = pos_error

self.prev_force_error = force_error

return output

# ============================================================

# 6. 温度传感器模拟

# ============================================================

class TemperatureSensor:

"""温度传感器模拟 —— 工厂模式"""

def __init__(self, noise_std: float = 0.5, drift_rate: float = 0.01):

self.noise_std = noise_std

self.drift_rate = drift_rate

self.drift = 0.0

self.last_temp = 25.0

def sample(self, position: Vector3, true_temp_field) -> float:

"""采样温度"""

# 真实温度场

true_temp = true_temp_field(position)

# 添加噪声

noise = np.random.normal(0, self.noise_std)

# 添加漂移

self.drift += np.random.normal(0, self.drift_rate)

# 模拟热惯性（一阶滞后）

alpha = 0.3

measured_temp = alpha * true_temp + (1 - alpha) * self.last_temp

measured_temp += noise + self.drift

self.last_temp = measured_temp

return measured_temp

def create_temperature_field(pipe_segments: List[PipeSegment]):

"""创建管道温度场（模拟泄漏热点）"""

def temp_field(pos: Vector3) -> float:

base_temp = 150.0 # 基础温度150°C

# 模拟一个热点（微泄漏）

hotspot_center = Vector3(1.5, 0.5, 0.3)

di

利用AI解决实际问题，如果你觉得这个工具好用，欢迎关注长安牧笛！