function [joint_angles, success] = robot_inverse_kinematics_minimum_variation(initial_theta,target_pose)
%   ROBOT_INVERSE_KINEMATICS 求解六关节机器人逆运动学（基于DH参数法，选择与当前关节角度最近的解）
%   输入参数：
%       initial_theta: 1x6向量，关节角度初始值（单位：弧度），用于选择最优解的参考基准
%       target_pose: 4x4矩阵，期望末端执行器位姿（齐次变换矩阵，包含位置和姿态）
%   输出参数：
%       joint_angles: 1x6向量，求解得到的关节角度（单位：弧度），为与初始角度最近的有效解
%       success: 逻辑值，true表示求解成功（正向验算通过），false表示求解失败

    % 1. 验证输入合法性：确保输入参数的维度符合要求
    if length(initial_theta) ~= 6
        error('关节角度初始值必须为1x6向量');
    end
    if size(target_pose) ~= [4,4]
        error('期望末端位姿必须为4x4齐次变换矩阵');
    end
    
    % 提取当前各关节角度（用于后续选择最优解）
    robot_current_joint_1 = initial_theta(1);
    robot_current_joint_2 = initial_theta(2);
    robot_current_joint_3 = initial_theta(3);
    robot_current_joint_4 = initial_theta(4);
    robot_current_joint_5 = initial_theta(5);
    robot_current_joint_6 = initial_theta(6);

    % 2. 机器人DH结构参数（固定参数，根据实际机器人型号配置）
    % DH参数定义：d(连杆偏距), a(连杆长度), alfa(连杆扭角), seta_offset(关节角度偏移)
    d1 = 0;      a1 = 0.00;   alfa1 = -pi/2;  seta1_offset = 0.00;
    d2 = 305;    a2 = 0;      alfa2 = pi/2;   seta2_offset = pi/2;
    d3 = 0;      a3 = 785;    alfa3 = 0;      seta3_offset = pi/2;
    d4 = 0;      a4 = 774;    alfa4 = 0;      seta4_offset = 0;
    d5 = 0;      a5 = 0;      alfa5 = pi/2;   seta5_offset = pi/2;
    d6 = 485;    a6 = 0;      alfa6 = 0;      seta6_offset = 0;        
    
    % 3. 处理目标位姿：去除末端执行器沿Z轴的偏距d6的影响，得到关节5末端的目标位姿
    TransZ_d6 = [1 0 0 0; 0 1 0 0; 0 0 1 d6; 0 0 0 1];  % 沿Z轴平移d6的齐次矩阵
    NPOS_Mod = target_pose * inv(TransZ_d6);  % 修正后的目标位姿（关节5末端）

    % 4. 提取修正后目标位姿的关键参数（旋转矩阵列向量和位置坐标）
    % 法线向量(nx, ny, nz) - 旋转矩阵第一列（末端执行器x轴方向）
    nx = NPOS_Mod(1,1);
    ny = NPOS_Mod(2,1);
    nz = NPOS_Mod(3,1);
    % 方向向量(ox, oy, oz) - 旋转矩阵第二列（末端执行器y轴方向）
    ox = NPOS_Mod(1,2);
    oy = NPOS_Mod(2,2);
    oz = NPOS_Mod(3,2);
    % 接近向量(ax, ay, az) - 旋转矩阵第三列（末端执行器z轴方向）
    ax = NPOS_Mod(1,3);
    ay = NPOS_Mod(2,3);
    az = NPOS_Mod(3,3);
    % 位置坐标(p_x, p_y, p_z) - 齐次矩阵第四列（关节5末端的目标位置）
    px = NPOS_Mod(1,4);
    py = NPOS_Mod(2,4);
    pz = NPOS_Mod(3,4);
    
    %% 求解逆运动学：逐关节计算可能解，并选择与当前角度最近的解
    %% 1. 第一关节角度求解（2个可能解，基于接近向量和位置坐标推导）
    seat1_Iv_1 = atan((ax*NPOS_Mod(3,4) - az*NPOS_Mod(1,4))/(-ay*NPOS_Mod(3,4)+az*NPOS_Mod(2,4)));
    seat1_Iv_2 = seat1_Iv_1 + pi;  % 第二个解（与第一个解相差pi）
    % 确保解落在[-π, π]范围内
    if seat1_Iv_2 > pi
        seat1_Iv_2 = seat1_Iv_2 - 2*pi;
    end
    seat1_Iv_temp = [seat1_Iv_1, seat1_Iv_2];  % 存储第一关节的两个可能解
    
    % 选择与当前第一关节角度最近的解
    if abs(robot_current_joint_1 - seat1_Iv_1) < abs(robot_current_joint_1 - seat1_Iv_2)
        seat1_Iv = seat1_Iv_1;
    else 
        seat1_Iv = seat1_Iv_2;
    end
    
    %% 2. 第二关节角度求解（基于第一关节的选定解，2个可能解）
    seat2_Iv_1 = atan((NPOS_Mod(1,4)*cos(seat1_Iv) + NPOS_Mod(2,4)*sin(seat1_Iv))/NPOS_Mod(3,4));
    seat2_Iv_2 = seat2_Iv_1 + pi;  % 第二个解（与第一个解相差pi）
    % 确保解落在[-π, π]范围内
    if seat2_Iv_2 > pi
        seat2_Iv_2 = seat2_Iv_2 - 2*pi;
    end
    seat2_Iv_temp = [seat2_Iv_1, seat2_Iv_2];  % 存储第二关节的两个可能解
    
    % 选择与当前第二关节角度最近的解
    if abs(robot_current_joint_2 - seat2_Iv_1) < abs(robot_current_joint_2 - seat2_Iv_2)
        seat2_Iv = seat2_Iv_1;
    else 
        seat2_Iv = seat2_Iv_2;
    end
    
    %% 3. 第六关节角度求解（基于第一关节的选定解，2个可能解）
    seat6_Iv_1 = atan((NPOS_Mod(2,2)*cos(seat1_Iv) - NPOS_Mod(1,2)*sin(seat1_Iv)) / (NPOS_Mod(1,1)*sin(seat1_Iv) - NPOS_Mod(2,1)*cos(seat1_Iv)));
    seat6_Iv_2 = seat6_Iv_1 + pi;  % 第二个解（与第一个解相差pi）
    % 确保解落在[-π, π]范围内
    if seat6_Iv_2 > pi
        seat6_Iv_2 = seat6_Iv_2 - 2*pi;
    end
    seat6_Iv_temp = [seat6_Iv_1, seat6_Iv_2];  % 存储第六关节的两个可能解
    
    % 选择与当前第六关节角度最近的解
    if abs(robot_current_joint_6 - seat6_Iv_1) < abs(robot_current_joint_6 - seat6_Iv_2)
        seat6_Iv = seat6_Iv_1;
    else 
        seat6_Iv = seat6_Iv_2;
    end

    %% 4. 第四关节角度求解（基于第一、二关节的选定解，2个可能解）
    % 计算辅助参数m和n（由目标位置和已选关节角度推导）
    m = NPOS_Mod(3,4)*cos(seat2_Iv) + NPOS_Mod(1,4)*cos(seat1_Iv)*sin(seat2_Iv) + NPOS_Mod(2,4)*sin(seat1_Iv)*sin(seat2_Iv);
    n = NPOS_Mod(2,4)*cos(seat1_Iv) - NPOS_Mod(1,4)*sin(seat1_Iv) - d2;
    % 基于余弦定理求解第四关节角度
    seat4_Iv_1 = acos((m^2 + n^2 - a3^2 - a4^2)/(2*a3*a4));
    seat4_Iv_2 = -seat4_Iv_1;  % 第二个解（与第一个解关于x轴对称）
    seat4_Iv_temp = [seat4_Iv_1, seat4_Iv_2];  % 存储第四关节的两个可能解

    % 选择与当前第四关节角度最近的解
    if abs(robot_current_joint_4 - seat4_Iv_1) < abs(robot_current_joint_4 - seat4_Iv_2)
        seat4_Iv = seat4_Iv_1;
    else 
        seat4_Iv = seat4_Iv_2;
    end

    %% 5. 第三关节角度求解（基于第一、二、四关节的选定解，2个可能解）
    seat3_Iv_1 = atan(((a3 + a4*cos(seat4_Iv))*m - a4*n*sin(seat4_Iv)) / ((a3 + a4*cos(seat4_Iv))*n + a4*m*sin(seat4_Iv)));
    seat3_Iv_2 = seat3_Iv_1 + pi;  % 第二个解（与第一个解相差pi）
    % 确保解落在[-π, π]范围内
    if seat3_Iv_2 > pi
        seat3_Iv_2 = seat3_Iv_2 - 2*pi;
    end
    seat3_Iv_temp = [seat3_Iv_1, seat3_Iv_2];  % 存储第三关节的两个可能解
    
    % 选择与当前第三关节角度最近的解
    if abs(robot_current_joint_3 - seat3_Iv_1) < abs(robot_current_joint_3 - seat3_Iv_2)
        seat3_Iv = seat3_Iv_1;
    else 
        seat3_Iv = seat3_Iv_2;
    end

    %% 6. 第五关节角度求解（基于前面选定的关节解，考虑角度周期性）
    % 计算两个基础解（由姿态约束推导）
    temp_5_1 = asin(-NPOS_Mod(3,3)/sin(seat2_Iv + seta2_offset));
    temp_5_2 = pi - temp_5_1;  % 第二个基础解（反正弦函数的对称解）

    % 将第二个基础解调整到[-π, π]范围内
    if temp_5_2 > pi
        temp_5_2 = temp_5_2 - 2*pi;
    end
    
    % 计算第五关节的两个原始解（扣除相关偏移量和已选关节角度）
    seat5_Iv_1 = temp_5_1 - seat3_Iv - seta3_offset - seat4_Iv - seta5_offset;
    seat5_Iv_2 = temp_5_2 - seat3_Iv - seta3_offset - seat4_Iv - seta5_offset;

    % 生成[-2π, 2π]范围内的所有周期解（考虑角度2π周期性）
    solutions = [];
    for base_sol = [seat5_Iv_1, seat5_Iv_2]
        % 向上下扩展2个周期，确保覆盖[-2π, 2π]区间
        for k = -2:2
            candidate = base_sol + k * 2*pi;
            % 筛选出在[-2π, 4π]范围内的有效解
            if candidate >= -2*pi && candidate <= 4*pi  %在[-2pi,4pi]内寻找最小变动量的解
                solutions = [solutions, candidate];
            end
        end
    end
    % 去重并排序，确保解的唯一性和有序性
    solutions = unique(solutions);
    solutions = sort(solutions);

    % 寻找与当前第五关节角度距离最近的解（基于绝对差值）
    % 计算每个解与当前关节5角度的绝对差值
    diffs = abs(solutions - robot_current_joint_5);    
    % 对差值排序，获取从小到大的索引（支持多次替换次小解）
    [sorted_diffs, sorted_idx] = sort(diffs);
    % 初始化最优解索引（从最小差值开始）
    opt_idx = 1;
    seat5_Iv = solutions(sorted_idx(opt_idx));

    %% 正向运动学验算：验证求解结果的正确性，失败则seat5_Iv替换为次小差值解重试
    while true
        % 组合所有选定的关节角度
        seta_Iv = [seat1_Iv, seat2_Iv, seat3_Iv, seat4_Iv, seat5_Iv, seat6_Iv];   
        % 调用正向运动学函数，计算实际末端位姿
        Joint_End_Pose = robot_forward_kinematics(seta_Iv);
        
        % 对目标位姿和实际位姿进行5位小数四舍五入后比较
        if isequal(round(target_pose, 5), round(Joint_End_Pose, 5))
            disp("求解成功");
            % 整合关节角度为输出向量
            joint_angles = [seat1_Iv, seat2_Iv, seat3_Iv, seat4_Iv, seat5_Iv, seat6_Iv];
            % 设置成功标志
            success = true;
            break;
        else 
            % 检查是否还有次小解可替换
            if opt_idx < length(sorted_idx)
                opt_idx = opt_idx + 1;
                seat5_Iv = solutions(sorted_idx(opt_idx));
                fprintf('当前解（差值排序第%d位）验算失败，尝试次小差值解（%.4f弧度）\n', ...
                    opt_idx-1, seat5_Iv);
            else
                % 所有解均验算失败，抛出错误
                error('所有第五关节可能解均验算失败，求解失败');
            end
        end
    end
    
end