MATLAB实现四足机器人足端轨迹规划：手把手教你编写摆线轨迹代码-CSDN博客

MATLAB实现四足机器人足端轨迹规划：从摆线到实战的完整代码指南

在探索四足机器人运动控制的奇妙世界里，足端轨迹规划是让机器“活”起来的第一步。想象一下，你面前的机器人模型还只是一堆静止的连杆和关节，如何让它像真实的动物一样，协调地抬起、落下四条腿，平稳地跨越地面甚至障碍？这个问题的核心钥匙，就握在轨迹规划算法的手中。对于许多刚接触机器人学的学生、工程师或爱好者来说，理论公式往往令人望而生畏，而网上零散的代码片段又难以直接集成到自己的项目中。本文的目标，就是充当你的实战向导，抛开复杂的数学推导，直接深入到MATLAB的代码层面，手把手带你构建一个可运行、可调整、可观察的摆线轨迹规划器。我们将从最基础的摆线方程开始，逐步扩展到完整的四足步态协调，并对比其他几种常见轨迹的代码实现差异，最终让你获得一套能直接用于学术研究或教学实验验证的“工具箱”。

1. 理解摆线轨迹：为什么它是四足机器人的理想起点

在众多轨迹规划方法中，摆线轨迹以其平滑的速度和加速度特性脱颖而出，尤其适合对运动平稳性要求较高的位置控制模式。其核心思想，是模仿自然界中许多生物肢体摆动的运动曲线——起步缓慢加速，中间段匀速，落地前缓慢减速。这种“慢-快-慢”的模式，能有效减少足端与地面接触时的冲击力，对于保护机器人机械结构和实现稳定步态至关重要。

从数学上看，一个标准的摆线方程描述了一个圆沿直线滚动时，圆上一点所经过的路径。在足端轨迹规划中，我们通常只取其一个周期的上升和下降段。在水平方向（X轴，即机器人前进方向），我们期望足端能匀速地从后向前移动，以提供稳定的推进力；在垂直方向（Z轴），则需要完成抬起、前移、放下的过程。摆线公式巧妙地将这两个维度的运动耦合起来。

摆线轨迹的核心参数决定了机器人的步态特征：

步态周期 Ts：一条腿完成一次“摆动-支撑”循环的总时间。周期越短，机器人迈步频率越高，理论速度越快，但对电机和控制的动态响应要求也越高。
占空比 faai (或 beta)：支撑相时间占整个步态周期的比例。例如，占空比为0.75意味着腿在75%的时间里处于支撑地面状态，25%的时间处于摆动状态。占空比大于0.5是静态稳定步态的基础。
抬腿高度 h：足端在摆动相中离地的最大垂直距离。这需要根据地面平整度和越障需求来设定，过高会浪费能量并可能引起机身晃动，过低则容易绊倒。
步长 S：足端在一个摆动相期间，在水平方向（X轴）上移动的距离，即 xf - xs。它直接决定了机器人的步幅。

对于四足机器人，我们通常采用对角步态，即将1、3号腿分为一组，2、4号腿为另一组。两组腿交替担任摆动腿和支撑腿的角色。当一组腿处于摆动相时，另一组的三条腿（实际上是对角的两条加另一侧的一条支撑腿）共同支撑身体并向后蹬地，推动身体前进。理解这种分组和交替的时序，是编写协调控制代码的前提。

2. 摆线轨迹的MATLAB代码实现与逐行解析

理论清晰后，我们进入实战环节。下面将构建一个完整的MATLAB函数，用于计算单腿在任意时刻的足端目标位置。我们将采用自顶向下的方式，先定义函数框架，再填充核心计算逻辑。

首先，创建一个名为 cycloid_trajectory.m 的脚本文件。我们将在这个文件里编写一个可被主程序调用的函数。

function [x, z] = cycloid_trajectory(t, Ts, beta, h, xs, xf)
% CYCLOID_TRAJECTORY 计算摆线轨迹的足端坐标
%   输入参数：
%       t   : 当前时间，相对于当前步态周期起点的时刻 (0 <= t <= Ts)
%       Ts  : 完整的步态周期时长 (秒)
%       beta: 占空比，支撑相时间占比 (0.5 < beta < 1)
%       h   : 最大抬腿高度 (米)
%       xs  : 足端在X轴的起始坐标 (米)
%       xf  : 足端在X轴的目标坐标 (米)
%   输出参数：
%       x   : 当前时刻足端在X轴的目标坐标
%       z   : 当前时刻足端在Z轴的目标坐标 (地面为0，向上为正)

    % 计算摆动相时长
    T_swing = Ts * (1 - beta);
    
    % 判断当前腿处于摆动相还是支撑相
    if t < T_swing
        % --- 摆动相计算 ---
        % 归一化的摆动相时间，范围[0, 1]
        t_normalized = t / T_swing;
        
        % 摆线公式的核心部分：角度参数
        theta = 2 * pi * t_normalized;
        
        % X轴坐标：从xs线性移动到xf
        x = xs + (xf - xs) * t_normalized;
        
        % Z轴坐标：摆线抬升轨迹
        % 公式：h * (1 - cos(theta)) / 2
        % 当theta从0到2pi， (1-cos(theta))/2 从0到1再到0，实现平滑抬起和放下
        z = h * (1 - cos(theta)) / 2;
        
    else
        % --- 支撑相计算 ---
        % 在支撑相，足端紧贴地面（z=0），并相对于机身向后滑动（X坐标回退）
        % 注意：这里假设身体匀速前进，因此支撑相足端在地面的轨迹是身体运动的逆向
        % 更精确的模型中，需要考虑身体实际位移。此处简化为线性回退。
        
        % 计算支撑相已过去的时间
        t_support = t - T_swing;
        % 支撑相总时长
        T_support = Ts * beta;
        % 支撑相归一化时间
        t_support_normalized = t_support / T_support;
        
        % X轴坐标：从xf线性