基于fmincon的逆运动学关节角偏置迭代收敛问题求助

问题分析 #

当前逆运动学求解中，fmincon因默认TolFun提前停止迭代，导致θ₅无法接近期望的连杆平行地面目标值（±0.17弧度）。即使调小TolFun或添加while循环，收敛结果仍未达标，核心原因是目标函数权重设计、约束逻辑和优化选项配置存在不合理之处。

具体优化方案 #

1. 重构目标函数权重与逻辑 #

当前目标函数中，位置误差和θ₅偏置的权重比例（1:1）可能导致位置精度优先压制偏置需求。需调整权重并优化误差项形式：

• 明确θ₅的目标值为-(θ₃+θ₄)，将偏置项改为平方误差（比绝对值更利于优化器收敛）
• 提升偏置项权重，若优先满足平行需求，可设为5~10，同时保留位置误差的基础权重

修改后的目标函数：

function error = objective_function(joint_angles, target, initial_guess, is_first_run)
    theta3 = joint_angles(2);
    theta4 = joint_angles(3);
    theta5_target = -(theta3 + theta4); % 明确θ₅的平行目标值
    theta5_current = joint_angles(4);

    % 位置误差计算
    position = forward_kinematics(joint_angles);
    pos_error = norm(position - target);
    
    % θ₅偏置误差（平方误差替代绝对值，优化器更易处理）
    theta5_error = (theta5_current - theta5_target)^2;
    
    % 权重调整：提升偏置项权重，优先满足平行要求
    error = pos_error + 5 * theta5_error; 

    if ~is_first_run
        error += 0.1 * norm(joint_angles - initial_guess); % 保留初始猜测的惩罚项
    end
end

2. 修正优化约束逻辑 #

当前非首次运行时的上下限计算错误，导致约束范围异常，直接影响收敛效果：

% 原错误代码
lb = initial_lb - initial_guess(1:4);
ub = initial_ub - initial_guess(1:4);

% 修改为：基于初始猜测的邻域约束，同时不超出关节硬限制
if ~is_first_run
    delta = 0.5; % 可根据实际关节活动范围调整
    lb = initial_guess(1:4) - delta;
    ub = initial_guess(1:4) + delta;
    % 确保不突破关节硬限制
    lb = max(lb, initial_lb);
    ub = min(ub, initial_ub);
end

3. 优化fmincon配置 #

调整优化选项，给迭代足够空间，同时提升收敛指向性：

• 降低TolFun和TolX阈值，提升MaxFunEvals和MaxIter
• 启用数值梯度计算，加快收敛效率

修改后的选项配置：

options = optimset(...
    'Display', 'iter', % 显示迭代过程，便于观察θ₅误差变化
    'MaxIter', 5000,
    'MaxFunEvals', 10000,
    'TolFun', 1e-8,
    'TolX', 1e-8,
    'GradObj', 'on' % 自动计算数值梯度，提升收敛稳定性
);

4. 替换while循环为多阶段优化 #

原while循环因fmincon内置停止准则优先级更高，无法有效触发循环。改为分两阶段优化：

1. 第一阶段：优先保证位置精度，得到可行解
2. 第二阶段：固定位置误差波动范围，重点优化θ₅偏置

示例代码片段：

function [joint_angles, exitflag] = inverse_kinematics(target_position, initial_guess, is_first_run)
    % 第一阶段：优先保证位置精度
    options_stage1 = optimset('Display', 'final', 'MaxIter', 2000, 'TolFun', 1e-6);
    [joint_angles_stage1, ~] = fmincon(@(j) objective_function_stage1([j;0], target_position), ...
        initial_guess(1:4), [], [], [], [], initial_lb, initial_ub, [], options_stage1);
    
    % 计算允许的位置误差波动范围（不超过初始精度的1.1倍）
    pos_error_base = norm(forward_kinematics([joint_angles_stage1;0]) - target_position);
    pos_error_allowed = pos_error_base * 1.1;

    % 第二阶段：重点优化θ₅偏置，限制位置误差不超限
    [joint_angles_stage2, ~] = fmincon(@(j) objective_function_stage2([j;0], target_position), ...
        joint_angles_stage1, [], [], [], [], initial_lb, initial_ub, ...
        @(j) constraint_position_error(j, target_position, pos_error_allowed), options);
    
    joint_angles = [joint_angles_stage2; 0];
end

% 约束函数：限制位置误差不超过允许值
function [c, ceq] = constraint_position_error(joint_angles, target, allowed_error)
    pos = forward_kinematics(joint_angles);
    c = norm(pos - target) - allowed_error; % 不等式约束：c ≤ 0
    ceq = [];
end

% 阶段1目标函数：优先位置精度
function error = objective_function_stage1(joint_angles, target)
    pos = forward_kinematics(joint_angles);
    error = norm(pos - target);
end

% 阶段2目标函数：优先θ₅偏置
function error = objective_function_stage2(joint_angles, target)
    theta3 = joint_angles(2);
    theta4 = joint_angles(3);
    theta5_target = -(theta3 + theta4);
    theta5_current = joint_angles(4);
    error = (theta5_current - theta5_target)^2;
end

关键注意事项 #

• 关节硬限制：所有约束必须在机械臂实际关节活动范围内，避免优化陷入不可行域
• 权重调试：根据实际需求调整位置误差与偏置项的权重，若位置精度要求不高，可进一步提升偏置项权重
• 梯度优化：若能手动推导目标函数的梯度，替换数值梯度可大幅提升收敛速度与精度

内容的提问来源于stack exchange，提问作者ravenswatch