本文由 linuxROS 整理发布，首发于 linuxros.cn，转载请注明出处。

机器人动作自然过渡：运动图谱+QP优化

导读：BVH动捕数据怎么变成机器人关节角？本文拆解三个核心模块——运动分割器用速度/曲率剖面切分动作片段，运动图谱用SLERP四元数混合让过渡自然，QP求解器用约束优化让关节轨迹平滑不抖动。300行C++代码覆盖完整管线。

一、整体数据流

从 BVH 文件到机器人关节角，完整管线分为四步：

flowchart TB A["BVH动捕文件"] --> B["BVHReader 解析骨架+帧数据"] B --> C["SpatioTemporalSegmenter 速度/曲率剖面分割"] C --> D["MotionGraph 构建动作图+SLERP混合"] D --> E["分段IK求解"] E --> F["QPSolver 关节平滑优化"] F --> G["机器人关节角 JointAngles"] style A fill:#E3F2FD,stroke:#1976D2 style B fill:#E3F2FD,stroke:#1976D2 style C fill:#FFF8E1,stroke:#F57C00 style D fill:#F3E5F5,stroke:#7B1FA2 style E fill:#FFF8E1,stroke:#F57C00 style F fill:#E8F5E9,stroke:#388E3C style G fill:#E8F5E9,stroke:#388E3C

二、运动分割：SpatioTemporalSegmenter

2.1 核心思想

一段连续的人体动作（如"走路→转身→挥手"），需要自动切成独立的片段。切分的依据是两个特征：速度剖面和曲率剖面。

// SpatioTemporalSegmenter.h · 运动分割器
class SpatioTemporalSegmenter {
public:
    std::vector<MotionClip> segment(const std::vector<MotionFrame>& frames) const;
    std::vector<int> detectBreakPoints(const std::vector<MotionFrame>& frames) const;

private:
    std::vector<double> computeVelocityProfile(const std::vector<MotionFrame>& frames) const;
    std::vector<double> computeCurvatureProfile(const std::vector<MotionFrame>& frames) const;
};

2.2 速度剖面：识别静止↔运动切换

速度剖面记录每一帧所有关节的平均位移量。静止时速度接近0，运动时速度突然增大：

// SpatioTemporalSegmenter.cpp · 计算速度剖面
std::vector<double> SpatioTemporalSegmenter::computeVelocityProfile(
    const std::vector<motion::MotionFrame>& frames) const
{
    std::vector<double> profile(frames.size() - 1, 0.0);

    for (size_t i = 1; i < frames.size(); ++i) {
        double sumVel = 0.0;
        int count = 0;
        for (size_t j = 0; j < frames[i].joints.size(); ++j) {
            // 帧i与帧i-1的关节位置差
            common::Vec3 delta = frames[i].joints[j].position - frames[i - 1].joints[j].position;
            sumVel += delta.length();
            count++;
        }
        // 所有关节的平均位移
        profile[i - 1] = (count > 0) ? sumVel / count : 0.0;
    }
    return profile;
}

2.3 曲率剖面：识别转向/急停

曲率剖面记录相邻三帧的方向变化量。当关节从直线运动突然转弯，曲率会突然增大：

// SpatioTemporalSegmenter.cpp · 计算曲率剖面
std::vector<double> SpatioTemporalSegmenter::computeCurvatureProfile(
    const std::vector<motion::MotionFrame>& frames) const
{
    std::vector<double> profile(frames.size() - 2, 0.0);

    for (size_t i = 1; i < frames.size() - 1; ++i) {
        double maxCurvature = 0.0;
        for (size_t j = 0; j < frames[i].joints.size(); ++j) {
            common::Vec3 prev = frames[i - 1].joints[j].position;
            common::Vec3 curr = frames[i].joints[j].position;
            common::Vec3 next = frames[i + 1].joints[j].position;

            // 两次位移向量
            common::Vec3 d1 = curr - prev;
            common::Vec3 d2 = next - curr;
            double len1 = d1.length();
            double len2 = d2.length();
            if (len1 < 1e-9 || len2 < 1e-9) continue;

            // cosθ 反映方向变化
            double cosAngle = d1.dot(d2) / (len1 * len2);
            cosAngle = std::max(-1.0, std::min(1.0, cosAngle));
            double curvature = 1.0 - cosAngle;  // cos越接近-1（180°转弯），curvature越大
            maxCurvature = std::max(maxCurvature, curvature);
        }
        profile[i - 1] = maxCurvature;
    }
    return profile;
}

2.4 双门限融合找断点

// SpatioTemporalSegmenter.cpp · 双门限融合检测断点
std::vector<int> SpatioTemporalSegmenter::detectBreakPoints(
    const std::vector<motion::MotionFrame>& frames) const
{
    auto velProfile = computeVelocityProfile(frames);
    auto curvProfile = computeCurvatureProfile(frames);

    std::vector<int> breakPoints;
    breakPoints.push_back(0);  // 片段起点

    for (size_t i = 1; i < velProfile.size() - 1; ++i) {
        // 速度接近0 → 可能是动作间隙
        double velScore = velProfile[i] < config_.velocityThreshold ? 1.0 : 0.0;
        // 曲率突然增大 → 可能是转向点
        double curvScore = curvProfile[i] > config_.curvatureThreshold ? 1.0 : 0.0;

        // 两者满足其一即标记为断点
        double score = 0.5 * velScore + 0.5 * curvScore;
        if (score > 0.5) {
            breakPoints.push_back(static_cast<int>(i));
        }
    }

    breakPoints.push_back(static_cast<int>(frames.size()) - 1);  // 片段终点
    return mergeBreakPoints(breakPoints, static_cast<int>(frames.size()));
}

2.5 分割结果

// 使用示例
SpatioTemporalSegmenter segmenter;
std::vector<MotionClip> clips = segmenter.segment(frames);
// clips[0] = {label="walk", frames=[0~30]}
// clips[1] = {label="turn", frames=[31~50]}
// clips[2] = {label="wave", frames=[51~80]}

三、运动图谱：MotionGraph

3.1 什么是运动图谱

运动图谱把动作片段变成"图"：节点=动作片段，边=可过渡连接。通过选择不同的节点序列，可以合成无限长的自然动作：

flowchart LR subgraph Nodes["🎬 动作节点"] N1["walk 走路"] N2["turn 转身"] N3["wave 挥手"] end N1 -- "SLERP walk→turn" --> N2 N2 -- "SLERP turn→wave" --> N3 N1 -. "SLERP walk→wave" .-> N3 style N1 fill:#E3F2FD,stroke:#1976D2 style N2 fill:#FFF8E1,stroke:#F57C00 style N3 fill:#F3E5F5,stroke:#7B1FA2

3.2 节点与边的数据结构

// MotionGraph.h · 运动图谱
struct MotionGraphNode {
    int id;
    std::string label;
    MotionClip clip;  // 动作片段
};

struct MotionGraphEdge {
    int fromNode;
    int toNode;
    int blendFrames;  // 过渡帧数
    std::vector<MotionFrame> blendedFrames;  // SLERP混合后的过渡帧
};

class MotionGraph {
    std::vector<MotionGraphNode> nodes_;
    std::vector<MotionGraphEdge> edges_;
    int nextNodeId_ = 0;

public:
    int addNode(const MotionClip& clip, const std::string& label);
    void buildEdges(int blendFrames = 10);
    void addManualEdge(int fromId, int toId, int blendFrames);
    MotionClip synthesize(const std::vector<int>& nodeSequence) const;
    MotionClip blendClips(const MotionClip& from, const MotionClip& to, int blendFrames) const;
};

3.3 自动建边算法

任意两个节点之间都建立边（简化版），混合帧数由参数控制：

// MotionGraph.cpp · 自动构建所有边
void MotionGraph::buildEdges(int blendFrames) {
    edges_.clear();
    for (size_t i = 0; i < nodes_.size(); ++i) {
        for (size_t j = 0; j < nodes_.size(); ++j) {
            if (i == j) continue;  // 不自连
            // 边 = 前一片段的最后一帧 + 后一片段的前N帧，SLERP混合
            addManualEdge(static_cast<int>(i), static_cast<int>(j), blendFrames);
        }
    }
}

void MotionGraph::addManualEdge(int fromId, int toId, int blendFrames) {
    const auto& fromClip = nodes_[fromId].clip;
    const auto& toClip = nodes_[toId].clip;

    // 混合from的最后1帧 + to的前N帧
    MotionClip fromTail = {fromClip.label, {fromClip.frames.back()}, 0, 0, 0};
    MotionGraphEdge edge;
    edge.fromNode = fromId;
    edge.toNode = toId;
    edge.blendFrames = blendFrames;
    edge.blendedFrames = blendClips(fromTail, toClip, blendFrames).frames;
    edges_.push_back(edge);
}

3.4 SLERP 四元数姿态混合（核心）

两个关键帧之间的旋转不能直接线性插值（会产生"被门夹住"的中间态），必须用球面线性插值（Slerp）：

来自 linuxros.cn · linuxROS

// MotionGraph.cpp · SLERP球面线性插值
common::Quaternion MotionGraph::slerpQuat(
    const common::Quaternion& a,
    const common::Quaternion& b,
    double t) const
{
    // 点积判断是否反向（取较短弧）
    double dot = a.w * b.w + a.x * b.x + a.y * b.y + a.z * b.z;
    common::Quaternion qb = b;
    if (dot < 0.0) {
        dot = -dot;
        qb = common::Quaternion(-b.w, -b.x, -b.y, -b.z);
    }

    // 非常接近时用线性近似（避免除零）
    const double threshold = 0.9995;
    if (dot > threshold) {
        common::Quaternion result(
            a.w + t * (qb.w - a.w),
            a.x + t * (qb.x - a.x),
            a.y + t * (qb.y - a.y),
            a.z + t * (qb.z - a.z));
        return result.normalized();
    }

    // SLERP标准公式
    double theta0 = std::acos(dot);      // 夹角
    double theta = theta0 * t;            // 插值位置
    double sinTheta = std::sin(theta);
    double sinTheta0 = std::sin(theta0);
    double s0 = std::cos(theta) - dot * sinTheta / sinTheta0;  // 权重0
    double s1 = sinTheta / sinTheta0;                             // 权重1

    return common::Quaternion(
        s0 * a.w + s1 * qb.w,
        s0 * a.x + s1 * qb.x,
        s0 * a.y + s1 * qb.y,
        s0 * a.z + s1 * qb.z);
}

3.5 混合两个动作片段

对关节位置线性插值，对旋转四元数球面插值：

// MotionGraph.cpp · 混合两个片段
MotionClip MotionGraph::blendClips(const MotionClip& from, const MotionClip& to, int blendFrames) const {
    MotionClip result;
    result.label = "blend";
    result.frames.reserve(blendFrames);

    const auto& lastFrame = from.frames.back();
    const auto& firstFrame = to.frames.front();

    for (int b = 0; b < blendFrames; ++b) {
        double t = static_cast<double>(b) / static_cast<double>(blendFrames);  // 0→1插值因子
        MotionFrame mf;
        mf.frameId = static_cast<uint32_t>(b);
        mf.timestamp = t;

        size_t numJoints = std::min(lastFrame.joints.size(), firstFrame.joints.size());
        for (size_t ji = 0; ji < numJoints; ++ji) {
            JointState js;
            js.name = lastFrame.joints[ji].name;
            // 位置：线性插值
            js.position = lastFrame.joints[ji].position * (1.0 - t)
                        + firstFrame.joints[ji].position * t;
            // 旋转：SLERP插值（核心）
            js.rotation = slerpQuat(lastFrame.joints[ji].rotation, firstFrame.joints[ji].rotation, t);
            mf.joints.push_back(js);
            mf.jointIndexMap[js.name] = ji;
        }
        result.frames.push_back(mf);
    }
    return result;
}

3.6 按节点序列合成动作

给定节点序列，自动拼接片段并插入过渡帧：

// MotionGraph.cpp · 按序列合成完整动作
MotionClip MotionGraph::synthesize(const std::vector<int>& nodeSequence) const {
    MotionClip result;
    result.label = "synthesized";

    for (size_t si = 0; si < nodeSequence.size(); ++si) {
        int nodeId = nodeSequence[si];
        const auto& node = nodes_[nodeId];

        // 1. 插入过渡帧（如果非首节点）
        if (si > 0) {
            int prevId = nodeSequence[si - 1];
            auto edgeIt = std::find_if(edges_.begin(), edges_.end(),
                [prevId, nodeId](const MotionGraphEdge& e) {
                    return e.fromNode == prevId && e.toNode == nodeId;
                });
            if (edgeIt != edges_.end()) {
                for (auto f : edgeIt->blendedFrames) {
                    f.frameId = static_cast<uint32_t>(result.frames.size());
                    f.timestamp = result.durationSec;
                    result.frames.push_back(f);
                }
            }
        }

        // 2. 插入完整节点帧
        for (size_t fi = 0; fi < node.clip.frames.size(); ++fi) {
            MotionFrame f = node.clip.frames[fi];
            f.frameId = static_cast<uint32_t>(result.frames.size());
            f.timestamp = result.durationSec;
            result.frames.push_back(f);
        }
    }
    result.durationSec = result.frames.empty() ? 0.0 : result.frames.back().timestamp;
    return result;
}

四、QP二次规划关节平滑

4.1 为什么需要QP优化

IK输出的关节角有两个问题：
1. 限位边界振荡：关节接近限位时被钳制，下一帧又弹回
2. 相邻帧突变：帧间关节角变化过大导致机器人抖动
QP优化的目标是一次求解两个相互矛盾的需求：既要跟上人体的角度（跟踪精度），又不能变得太快（运动平滑）。给两个目标各分配一个权重想更精准就提高跟踪权重，想更丝滑就提高平滑权重QP自动算出最优折中。

4.2 QP求解器实现

简化的 Active Set 方法（约束边界逐个激活）：

// QPSolver.cpp · QP求解：跟踪 + 平滑双目标
std::vector<double> QPSolver::solve(
    const std::vector<double>& humanRefAngles,   // 跟踪目标（人体关节角）
    const std::vector<double>& prevRobotAngles,   // 上一帧机器人关节角
    const kinematics::DHChain& chain)              // 用于获取关节限位
{
    size_t n = chain.dof();
    std::vector<double> q(n, 0.0);      // 目标函数梯度
    std::vector<double> diagW(n, 0.0);  // Hessian对角
    std::vector<double> lower(n, 0.0);   // 下界（关节限位+margin）
    std::vector<double> upper(n, 0.0);   // 上界（关节限位-margin）

    for (size_t i = 0; i < n; ++i) {
        // 梯度 = -2*w_track*θ_human - 2*w_smooth*θ_prev
        q[i] = -2.0 * config_.trackingWeight * humanRefAngles[i]
               - 2.0 * config_.smoothnessWeight * prevRobotAngles[i];

        // Hessian对角 = 2*(w_track + w_smooth)
        diagW[i] = 2.0 * (config_.smoothnessWeight + config_.trackingWeight);

        // 关节限位加margin，防止频繁触发
        double margin = config_.jointLimitMargin;
        lower[i] = chain.jointMin(i) + margin;
        upper[i] = chain.jointMax(i) - margin;
    }

    return solveActiveSet(q, diagW, lower, upper);
}

4.3 对角Hessian的解析求解

对角 Hessian 的 QP 可以解析求解：每个变量独立优化，先求无约束最优解，再钳制到边界：

// QPSolver.cpp · 对角QP解析求解（每个变量独立优化）
std::vector<double> QPSolver::solveActiveSet(
    const std::vector<double>& q,
    const std::vector<double>& diagW,
    const std::vector<double>& lowerBounds,
    const std::vector<double>& upperBounds)
{
    size_t n = q.size();
    std::vector<double> x(n, 0.0);

    for (size_t i = 0; i < n; ++i) {
        if (diagW[i] < 1e-12) {
            x[i] = 0.0;
            continue;
        }

        // 无约束最优解：x* = -q / diagW（二次函数顶点）
        double unconstrained = -q[i] / diagW[i];

        // 钳制到边界（Box Constraint）
        x[i] = std::max(lowerBounds[i], std::min(upperBounds[i], unconstrained));
    }
    return x;
}

4.4 参数配置建议

// Config.h · QP参数
struct QPConfig {
    double trackingWeight = 1.0;    // 跟踪权重：越大越接近人体角度
    double smoothnessWeight = 0.5;  // 平滑权重：越大越不会抖动
    double jointLimitMargin = 0.05; // 限位缓冲：防止频繁触发边界
};

// 调试经验值
QPConfig config;
config.trackingWeight = 1.0;    // 默认
config.smoothnessWeight = 0.5;   // 默认
// 关节限位被频繁触发时 → 增大margin
// 机器人动作滞后于人体时 → 增大trackingWeight
// 机器人运动抖动时 → 增大smoothnessWeight

五、完整管线集成

// RetargetingSolver.cpp · 端到端重定位
MotionClip MotionRetargetingSolver::retarget(
    const MotionClip& clip,
    const RobotModel& robot) const
{
    // 1. 分段
    auto segmenter = SpatioTemporalSegmenter();
    auto segments = segmenter.segment(clip.frames);

    // 2. 建图谱
    auto graph = MotionGraph();
    for (const auto& seg : segments) {
        graph.addNode(seg, seg.label);
    }
    graph.buildEdges(blendFrames_);

    // 3. 合成
    auto synthesized = graph.synthesize(segmentIds);

    // 4. 每帧IK + QP
    MotionClip result;
    result.label = "retargeted";
    std::vector<double> prevAngles(robot.totalDof(), 0.0);

    for (const auto& frame : synthesized.frames) {
        // 分段IK求解
        auto robotAngles = solvePerLimb(frame, robot);

        // QP平滑
        auto qpSolver = QPSolver();
        auto smoothed = qpSolver.solve(robotAngles, prevAngles, robot.leftArmChain());
        prevAngles = smoothed;

        result.frames.push_back(buildRobotFrame(frame, smoothed));
    }
    return result;
}

六、总结

运动重定位的三个核心模块各有分工：

模块	职责	核心算法
SpatioTemporalSegmenter	切分动作片段	速度+曲率双门限
MotionGraph	拼接片段 + 自然过渡	SLERP四元数混合
QPSolver	关节轨迹平滑	跟踪+平滑双目标QP

SLERP 是姿态插值的正确方式，线性插值四元数会产生非旋转的"畸变"。QP 平滑解决 IK 输出在限位边界的振荡问题，让机器人动作既跟随人体又不会抖动。

代码仓库：D:\code\test\motion_retargeting
源码路径：src/motion/MotionGraph.cpp, src/retargeting/QPSolver.cpp, src/segmentation/SpatioTemporalSegmenter.cpp