在机器人参数辨识中使用傅里叶级数，主要是为了生成一种激励轨迹，具有良好的数学特性，同时符合物理要求。傅里叶级数的特性使其在机器人动力学辨识的激励轨迹设计中非常常用。
1. 为什么需要激励轨迹？
激励轨迹的作用是通过驱动机器人运动，产生尽可能丰富的数据，帮助准确辨识动力学模型参数。理想的激励轨迹需要满足：
- 覆盖丰富的动态特性： 激励轨迹需要让系统尽可能激活更多的自由度（关节），产生多样的运动状态（位置、速度、加速度）。
- 避免高频噪声： 激励轨迹需要避免激发机器人结构的高频柔性振动，导致噪声放大。
- 控制复杂度： 激励轨迹应易于生成和分析，同时适合实际硬件执行。
2. 为什么用傅里叶级数？
傅里叶级数在激励轨迹设计中的优势包括以下几点：
(1) 数学简洁性
傅里叶级数能够将任何周期性函数表示为正弦和余弦函数的组合：
q(t)=a0+∑n=1N(ancos(nωt)+bnsin(nωt))q(t) = a_0 + \sum_{n=1}^N \left( a_n \cos(n \omega t) + b_n \sin(n \omega t) \right)
这种形式非常适合生成平滑、周期性的激励轨迹。
(2) 可控的频率特性
- 低频分量为主： 通过选择较低阶的傅里叶项，可以限制轨迹的频率范围，避免激发高频振动。
- 可调的频率和幅值： 傅里叶级数的参数（系数）可以直接调整轨迹的幅值和频率特性，满足特定动态要求。
(3) 灵活性
- 可以轻松调整轨迹的复杂性（通过控制傅里叶级数的阶数 NN）。
- 可以通过优化傅里叶系数（如本代码中使用的
fmincon），设计特定性质的轨迹，如最小化条件数或减少能量消耗。
(4) 周期性要求
傅里叶级数天然适用于周期性轨迹。对于许多机器人系统（如关节型机械臂），周期性运动是自然的选择，因为它可以让系统循环激励而无需频繁切换模式。
3. 具体应用中的目的
在您的代码中，傅里叶级数用于生成激励轨迹，并通过优化傅里叶系数来满足以下目标：
- 提高辨识矩阵的条件数：
- 条件数是系统数值稳定性的指标。优化傅里叶系数可以确保辨识矩阵具有较好的数值稳定性，从而提高参数辨识的精度。
- 确保轨迹平滑：
- 傅里叶级数生成的轨迹天然是连续且光滑的，避免了不连续或非光滑轨迹对机器人运动的负面影响。
- 覆盖丰富动态范围：
- 不同阶的傅里叶分量提供了轨迹的不同频率特性，使得运动数据能够覆盖更广的动态范围。
4. 其他常见激励轨迹设计方法的对比
| 方法 | 优点 | 缺点 |
|---|---|---|
| 傅里叶级数 | 平滑、易调控、数学简单，适合周期性激励。 | 不适合非周期性轨迹。 |
| 多项式轨迹 | 非周期性轨迹设计方便，适合局部运动。 | 高阶多项式可能导致高频振动。 |
| 阶跃信号或方波信号 | 易实现、激励强度大。 | 对硬件要求高，可能引入高频噪声。 |
| 随机信号（如伪随机序列） | 激励范围最广，适用于随机系统。 | 难以控制硬件执行，信号可能过于复杂。 |
| 预设轨迹（如标准三角波） | 简单易用，适合某些特定场景。 | 灵活性不足，可能无法覆盖全部动态特性。 |
5. 总结
傅里叶级数是激励轨迹设计中的一种理想选择，特别适合周期性运动的机器人参数辨识。使用傅里叶级数有助于生成平滑、低噪声、动态特性丰富的轨迹，同时允许通过优化调整参数以满足特定需求。
如果您对傅里叶系数优化或者其他激励轨迹设计方法有进一步问题，可以随时讨论！
6. 总结个人小结以及代码实现：
傅里叶级数，直接映射了频域的响应。直接对应了不同omega的响应。 实际操作是常用 3阶 5阶，就可以了。根据物理情况，选择roi omega .e.g. 10 hz
%% Excited Trajectory Generation
clear
clc
% 设置傅里叶级数的阶数
N = 3; % 使用三阶傅里叶级数
% 设置优化参数和限制条件
OPTIONS = optimoptions('fmincon', 'Algorithm', 'active-set'); % 使用激活集算法进行约束优化
A = []; b = []; % 无线性不等式约束
Aeq = []; beq = []; % 无线性等式约束
% 设置傅里叶系数的上下界（例如，振幅限制在 [-3, 3]）
lb = [-3 * ones(1, 2), -3, -3 * ones(1, 2 * (N - 1))]; % 下界
ub = [3 * ones(1, 2), 3, 3 * ones(1, 2 * (N - 1))]; % 上界
% 优化傅里叶系数，目标函数和约束分别定义在 myfun2 和 mycon
x = fmincon(@(x) myfun2(x), ... % 目标函数：由 myfun2 定义
1 * ones(1, 2 * N + 1), ... % 初始值：所有系数为 1
[], [], [], [], ... % 无额外的线性约束
lb, ub, ... % 系数的上下界
@(x) mycon(x), ... % 非线性约束：由 mycon 定义
OPTIONS); % 优化选项
% 轨迹生成的时间参数
Tf = 10; % 总时间周期 (秒)
omega = 2 * pi / Tf; % 基频 (rad/s)
dt = 0.007; % 时间步长 (秒)
t = 0:dt:Tf; % 时间向量
%% 生成傅里叶级数轨迹
% 初始化第一个傅里叶级数的各阶分量
l = 1; % 傅里叶级数的阶次
q{1} = x(1) / (omega * l) * sin(omega * l * t) ...
- x(2) / (omega * l) * cos(omega * l * t) + x(3); % 位移
dq{1} = x(1) * cos(omega * l * t) + x(2) * sin(omega * l * t); % 速度
ddq{1} = -x(1) * omega * l * sin(omega * l * t) ...
+ x(2) * omega * l * cos(omega * l * t); % 加速度
% 生成其他傅里叶级数的分量
for i = 2:N
l = i; % 当前傅里叶级数的阶次
q{i} = x(2 + (i - 2) * 2 + 2) / (omega * l) * sin(omega * l * t) ...
- x(2 + (i - 2) * 2 + 3) / (omega * l) * cos(omega * l * t); % 位移
dq{i} = x(2 + (i - 2) * 2 + 2) * cos(omega * l * t) ...
+ x(2 + (i - 2) * 2 + 3) * sin(omega * l * t); % 速度
ddq{i} = -x(2 + (i - 2) * 2 + 2) * omega * l * sin(omega * l * t) ...
+ x(2 + (i - 2) * 2 + 3) * omega * l * cos(omega * l * t); % 加速度
end
%% 合并所有傅里叶分量
% 初始化合成轨迹
qF = q{1}; % 合成位置
dqF = dq{1}; % 合成速度
ddqF = ddq{1}; % 合成加速度
% 将每阶傅里叶分量累加
for i = 2:N
qF = qF + q{i};
dqF = dqF + dq{i};
ddqF = ddqF + ddq{i};
end
%% 生成动力学特征矩阵
% 初始化第一时间点的动力学特征矩阵
F = [ddqF(1), dqF(1), sign(dqF(1)), cos(qF(1)), sin(qF(1))];
for i = 2:length(t)
F = [F; ddqF(i), dqF(i), sign(dqF(i)), cos(qF(i)), sin(qF(i))];
end
% 计算特征矩阵的条件数（数值稳定性评价）
f = cond(F);
%% 保存轨迹数据
save('tra', 'qF', 'dqF', 'ddqF');
此代码用于生成激励轨迹，并计算相应的动力学特征矩阵的条件数。
代码的目的
- 通过傅里叶级数生成一个周期性激励轨迹 q(t)q(t)，其包含位置、速度和加速度。
- 优化傅里叶系数，以确保动力学辨识矩阵具有良好的条件数（数值稳定性）。
- 保存生成的轨迹和对应的动力学数据。
/
代码主要部分总结
-
优化傅里叶系数：
- 使用
fmincon优化傅里叶级数系数，以确保生成的轨迹符合约束条件。 myfun2和mycon分别定义了优化的目标函数和非线性约束。
- 使用
-
生成轨迹：
- 基于傅里叶级数生成机器人激励轨迹，包括位置 qFqF、速度 dqFdqF 和加速度 ddqFddqF。
-
动力学特征矩阵：
- 通过轨迹生成对应的动力学特征矩阵 FF，每一行包括加速度、速度、速度符号、位置的余弦和正弦。
- 计算矩阵的条件数 cond(F)\text{cond}(F)，评估辨识的数值稳定性。
-
保存数据：
- 将合成轨迹 qFqF、速度 dqFdqF 和加速度 ddqFddqF 保存到文件
tra.mat，供后续分析使用。
- 将合成轨迹 qFqF、速度 dqFdqF 和加速度 ddqFddqF 保存到文件
附加建议
- 优化目标：进一步明确
myfun2的具体目标（可能是条件数或轨迹平滑性）。 - 约束定义：确保
mycon合理定义非线性约束，避免优化结果不符合实际物理要求。 - 性能提升：如果轨迹生成计算量较大，可以尝试优化循环结构，减少重复计算。
