函数组:QF05(随机数值生成器)

本文介绍了一个名为QF05的随机数值生成器函数组,它包括了生成随机数、整数随机数及保存初始值等功能模块。与F052函数组类似,QF05为开发者提供了灵活的随机数生成解决方案。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

函数组:QF05(随机数值生成器)

函数组:QF05;随机数值生成器,跟函数组 F052 类似。


QF05_RANDOM                    Random number generator
QF05_RANDOM_INTEGER            Random (whole) number
QF05_RANDOM_SAVE_SEED          Save the initial value for the function modules of this group

%% 机械臂PTP轨迹控制 clear; clc; close all; % =========================================== % 机械臂参数定义 (与之前一致) % =========================================== a = [0, 0, 500, 500, 0, 0]; % 连杆长度(mm) alpha = [0, -pi/2, 0, -pi/2, pi/2, -pi/2]; % 连杆扭角(rad) d = [400, 0, 0, 0, 0, 100]; % 连杆偏移(mm) joint_limits = [ -pi, pi; % 关节1 -pi/2, pi/2; % 关节2 -pi/2, pi/2; % 关节3 -pi, pi; % 关节4 0, pi; % 关节5 -pi, pi % 关节6 ]; % =========================================== % 轨迹参数设置 % =========================================== % 工作空间中随机选择两点 P_start = [randi([-800, 800]); randi([-800, 800]); randi([0, 1000])]; P_end = [randi([-800, 800]); randi([-800, 800]); randi([0, 1000])]; % 目标方向矩阵 (保持垂直向下) R = [1, 0, 0; 0, 1, 0; 0, 0, 1]; % 创建目标位姿矩阵 T_start = [R, P_start; 0 0 0 1]; T_end = [R, P_end; 0 0 0 1]; % 运动参数 total_time = 5; % 总运动时间() time_step = 0.05; % 时间步长() time_vector = 0:time_step:total_time; num_points = length(time_vector); % =========================================== % 逆运动学求解起点和终点关节角度 % =========================================== q_start = inverse_kinematics(T_start, a, alpha, d, joint_limits); q_end = inverse_kinematics(T_end, a, alpha, d, joint_limits); % 选择最优解 if ~isempty(q_start) && ~isempty(q_end) q_start = select_solution(q_start, [0,0,0,0,0,0], joint_limits); q_end = select_solution(q_end, q_start, joint_limits); fprintf('起点位置: [%.1f, %.1f, %.1f] mm\n', P_start(1), P_start(2), P_start(3)); fprintf('终点位置: [%.1f, %.1f, %.1f] mm\n', P_end(1), P_end(2), P_end(3)); fprintf('起点关节角度 (): %s\n', num2str(rad2deg(q_start))); fprintf('终点关节角度 (): %s\n', num2str(rad2deg(q_end))); else error('无法找到有效的逆运动学解'); end % =========================================== % PTP轨迹规划 (关节空间五次多项式插) % =========================================== % 初始化轨迹矩阵: 6关节 x 时间点 q_trajectory = zeros(6, num_points); dq_trajectory = zeros(6, num_points); % 关节速度 ddq_trajectory = zeros(6, num_points); % 关节加速度 % 计算五次多项式系数 (保证起点和终点的位置、速度、加速度连续) for j = 1:6 q0 = q_start(j); qf = q_end(j); % 边界条件: 起点和终点速度为0,加速度为0 v0 = 0; vf = 0; a0 = 0; af = 0; % 求解五次多项式系数: q(t) = a0 + a1*t + a2*t^2 + a3*t^3 + a4*t^4 + a5*t^5 A = [1, 0, 0, 0, 0, 0; 0, 1, 0, 0, 0, 0; 0, 0, 2, 0, 0, 0; 1, total_time, total_time^2, total_time^3, total_time^4, total_time^5; 0, 1, 2*total_time, 3*total_time^2, 4*total_time^3, 5*total_time^4; 0, 0, 2, 6*total_time, 12*total_time^2, 20*total_time^3]; b = [q0; v0; a0; qf; vf; af]; coeffs = A \ b; % 计算轨迹点 for i = 1:num_points t = time_vector(i); q_trajectory(j, i) = coeffs(1) + coeffs(2)*t + coeffs(3)*t^2 + ... coeffs(4)*t^3 + coeffs(5)*t^4 + coeffs(6)*t^5; dq_trajectory(j, i) = coeffs(2) + 2*coeffs(3)*t + 3*coeffs(4)*t^2 + ... 4*coeffs(5)*t^3 + 5*coeffs(6)*t^4; ddq_trajectory(j, i) = 2*coeffs(3) + 6*coeffs(4)*t + ... 12*coeffs(5)*t^2 + 20*coeffs(6)*t^3; end end % =========================================== % 轨迹可视化 % =========================================== % 1. 关节角度轨迹 figure('Name', '关节角度轨迹', 'Position', [100, 100, 1200, 800]); titles = {'关节1', '关节2', '关节3', '关节4', '关节5', '关节6'}; for j = 1:6 subplot(2, 3, j); plot(time_vector, rad2deg(q_trajectory(j, :)), 'LineWidth', 2); hold on; % 绘制关节限位 ylim_min = min(joint_limits(j, 1), joint_limits(j, 2)) * 180/pi; ylim_max = max(joint_limits(j, 1), joint_limits(j, 2)) * 180/pi; ylim_range = ylim_max - ylim_min; plot([0, total_time], [rad2deg(joint_limits(j, 1)), rad2deg(joint_limits(j, 1))], 'r--'); plot([0, total_time], [rad2deg(joint_limits(j, 2)), rad2deg(joint_limits(j, 2))], 'r--'); title(titles{j}); xlabel('时间 ()'); ylabel('角度 ()'); grid on; legend('关节角度', '关节限位', 'Location', 'best'); % 设置合理的Y轴范围 y_min = min(rad2deg(q_trajectory(j, :))) - 0.1*ylim_range; y_max = max(rad2deg(q_trajectory(j, :))) + 0.1*ylim_range; ylim([y_min, y_max]); end % 2. 关节速度和加速度 figure('Name', '关节速度和加速度', 'Position', [100, 100, 1200, 800]); for j = 1:6 subplot(2, 3, j); yyaxis left; plot(time_vector, rad2deg(dq_trajectory(j, :)), 'b-', 'LineWidth', 2); ylabel('速度 (度/秒)'); yyaxis right; plot(time_vector, rad2deg(ddq_trajectory(j, :)), 'r-', 'LineWidth', 2); ylabel('加速度 (度/秒²)'); title([titles{j} ' 运动状态']); xlabel('时间 ()'); grid on; legend('速度', '加速度', 'Location', 'best'); end % 3. 末端轨迹动画 figure('Name', '机械臂运动轨迹动画', 'Position', [100, 100, 800, 600]); hold on; grid on; axis equal; view(3); xlabel('X (mm)'); ylabel('Y (mm)'); zlabel('Z (mm)'); title('机械臂PTP运动轨迹'); % 绘制起点和终点 plot3(P_start(1), P_start(2), P_start(3), 'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g'); plot3(P_end(1), P_end(2), P_end(3), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r'); % 绘制轨迹 traj_points = zeros(3, num_points); for i = 1:num_points T = forward_kinematics(q_trajectory(:, i), a, alpha, d); traj_points(:, i) = T(1:3,4); end plot3(traj_points(1,:), traj_points(2,:), traj_points(3,:), 'b-', 'LineWidth', 1.5); % 动画 h_robot = plot_robot(q_trajectory(:, 1), a, alpha, d, 'k', 2); legend('起点', '终点', '轨迹', '机械臂', 'Location', 'best'); for i = 1:10:num_points delete(h_robot); h_robot = plot_robot(q_trajectory(:, i), a, alpha, d, 'k', 2); drawnow; pause(0.05); end % =========================================== % 核心函数定义 % =========================================== %% 正运动学函数 (Modified D-H方法) function T = forward_kinematics(q, a, alpha, d) T = eye(4); for i = 1:6 ct = cos(q(i)); st = sin(q(i)); ca = cos(alpha(i)); sa = sin(alpha(i)); Ti = [ct, -st*ca, st*sa, a(i)*ct; st, ct*ca, -ct*sa, a(i)*st; 0, sa, ca, d(i); 0, 0, 0, 1 ]; T = T * Ti; end end %% 逆运动学函数 (解法) function solutions = inverse_kinematics(T_target, a, alpha, d, joint_limits) lb = joint_limits(:, 1); ub = joint_limits(:, 2); options = optimoptions('fmincon', 'Display', 'off', ... 'Algorithm', 'sqp', ... 'MaxIterations', 1000, ... 'MaxFunctionEvaluations', 5000); solutions = []; num_attempts = 10; for attempt = 1:num_attempts q0 = lb + (ub - lb) .* rand(6,1); [q_opt, ~, exitflag] = fmincon(@(q)ik_objective(q, T_target, a, alpha, d), ... q0, [], [], [], [], lb, ub, [], options); if exitflag > 0 T_achieved = forward_kinematics(q_opt, a, alpha, d); pos_error = norm(T_achieved(1:3,4) - T_target(1:3,4)); if pos_error < 10 % 位置误差小于10mm solutions = [solutions; q_opt']; end end end if isempty(solutions) fprintf('警告: 未找到有效的逆运动学解\n'); end end %% 逆运动学目标函数 function cost = ik_objective(q, T_target, a, alpha, d) T_achieved = forward_kinematics(q, a, alpha, d); % 位置误差 pos_error = norm(T_achieved(1:3,4) - T_target(1:3,4)); % 方向误差 (简化处理) rot_error = norm(T_achieved(1:3,1:3) - T_target(1:3,1:3), 'fro'); % 总成本 cost = pos_error + 0.1 * rot_error; end %% 最优解选择函数 function best_solution = select_solution(solutions, reference, joint_limits) if size(solutions, 1) == 1 best_solution = solutions(1, :); return; end min_cost = Inf; best_idx = 1; for i = 1:size(solutions, 1) % 1. 关节角度变化最小化 joint_change = norm(solutions(i, :) - reference); % 2. 关节限位惩罚 joint_penalty = 0; for j = 1:6 margin = min(solutions(i,j) - joint_limits(j,1), ... joint_limits(j,2) - solutions(i,j)); if margin < deg2rad(5) joint_penalty = joint_penalty + 10 * (deg2rad(5) - margin); end end % 3. 总成本 total_cost = joint_change + joint_penalty; if total_cost < min_cost min_cost = total_cost; best_idx = i; end end best_solution = solutions(best_idx, :); end %% 机械臂可视化函数 function h = plot_robot(q, a, alpha, d, color, linewidth) if nargin < 5, color = 'k'; end if nargin < 6, linewidth = 2; end % 计算各关节位置 T = cell(1, 7); T{1} = eye(4); for i = 1:6 T{i+1} = T{i} * dh_transform(a(i), alpha(i), d(i), q(i)); end % 提取位置 positions = zeros(3, 7); for i = 1:7 positions(:, i) = T{i}(1:3, 4); end % 绘制连杆 h = plot3(positions(1,:), positions(2,:), positions(3,:), ... 'o-', 'Color', color, 'LineWidth', linewidth, ... 'MarkerFaceColor', color, 'MarkerSize', 6); end %% DH变换矩阵 function T = dh_transform(a, alpha, d, theta) T = [cos(theta), -sin(theta)*cos(alpha), sin(theta)*sin(alpha), a*cos(theta); sin(theta), cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta); 0, sin(alpha), cos(alpha), d; 0, 0, 0, 1]; end (阅读代码,不修改)
06-27
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值