MATLAB空间机器人动力学与路径规划
一、空间机器人动力学建模
1.1 空间机器人运动学
% space_robot_kinematics.m
% 空间机器人运动学建模
classdef SpaceRobotKinematics < handle
properties
n_joints % 关节数量
link_lengths % 连杆长度
link_masses % 连杆质量
com_positions % 连杆质心位置（相对于连杆坐标系）
inertia_tensors % 连杆惯量张量
base_mass % 基座质量
base_inertia % 基座惯量
% DH参数 [alpha, a, d, theta]
dh_params
end
methods
function obj = SpaceRobotKinematics(n_joints, link_lengths, link_masses)
obj.n_joints = n_joints;
obj.link_lengths = link_lengths;
obj.link_masses = link_masses;
% 默认DH参数（平面机器人）
obj.dh_params = zeros(n_joints, 4);
for i = 1:n_joints
obj.dh_params(i, :) = [0, link_lengths(i), 0, 0];
end
% 默认质心位置（在连杆中心）
obj.com_positions = zeros(n_joints, 3);
for i = 1:n_joints
obj.com_positions(i, :) = [link_lengths(i)/2, 0, 0];
end
% 默认惯量（简化为均匀杆）
obj.inertia_tensors = cell(1, n_joints);
for i = 1:n_joints
m = link_masses(i);
L = link_lengths(i);
I_xx = m * L^2 / 12;
I_yy = m * L^2 / 12;
I_zz = 0;
obj.inertia_tensors{i} = diag([I_xx, I_yy, I_zz]);
end
end
function T = forward_kinematics(obj, q, joint_index)
% 计算正向运动学
% q: 关节角度向量
% joint_index: 计算到第几个关节的变换矩阵
T = eye(4);
for i = 1:joint_index
alpha = obj.dh_params(i, 1);
a = obj.dh_params(i, 2);
d = obj.dh_params(i, 3);
theta = obj.dh_params(i, 4) + q(i);
% DH变换矩阵
Ti = [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];
T = T * Ti;
end
end
function [pos, R] = get_end_effector_pose(obj, q)
% 获取末端执行器位姿
T = obj.forward_kinematics(q, obj.n_joints);
pos = T(1:3, 4);
R = T(1:3, 1:3);
end
function J = jacobian(obj, q)
% 计算雅可比矩阵（几何法）
J = zeros(6, obj.n_joints);
% 获取各关节位置和z轴方向
joint_positions = zeros(3, obj.n_joints);
z_axes = zeros(3, obj.n_joints);
T_prev = eye(4);
for i = 1:obj.n_joints
T_i = obj.forward_kinematics(q, i);
joint_positions(:, i) = T_i(1:3, 4);
z_axes(:, i) = T_prev(1:3, 3);
T_prev = T_i;
end
% 末端执行器位置
T_end = obj.forward_kinematics(q, obj.n_joints);
p_end = T_end(1:3, 4);
% 计算雅可比矩阵的每一列
for i = 1:obj.n_joints
% 线性速度部分
J(1:3, i) = cross(z_axes(:, i), p_end - joint_positions(:, i));
% 角速度部分
J(4:6, i) = z_axes(:, i);
end
end
function [q_solutions, success] = inverse_kinematics(obj, target_pos, target_R, initial_guess)
% 数值逆运动学求解
% 使用牛顿-拉夫逊方法
max_iterations = 100;
tolerance = 1e-6;
alpha = 0.1; % 步长
q = initial_guess;
success = false;
for iter = 1:max_iterations
% 当前末端位姿
[current_pos, current_R] = obj.get_end_effector_pose(q);
% 计算位置误差
pos_error = target_pos - current_pos;
% 计算姿态误差（使用旋转矩阵的误差）
R_error = target_R * current_R';
angle_error = rotationMatrixToVector(R_error);
% 总误差
error = [pos_error; angle_error];
% 检查收敛
if norm(error) < tolerance
success = true;
break;
end
% 计算雅可比矩阵
J = obj.jacobian(q);
% 使用伪逆求解关节角度增量
dq = alpha * pinv(J) * error;
% 更新关节角度
q = q + dq;
% 关节限位
q = max(min(q, pi), -pi);
end
q_solutions = q;
if ~success
warning('逆运动学未收敛，误差: %f', norm(error));
end
end
function q_analytic = analytic_inverse_kinematics(obj, target_pos)
% 解析逆运动学（针对2连杆平面机器人）
if obj.n_joints ~= 2
error('解析逆运动学仅支持2自由度机器人');
end
L1 = obj.link_lengths(1);
L2 = obj.link_lengths(2);
x = target_pos(1);
y = target_pos(2);
% 计算距离
d = norm([x, y]);
if d > (L1 + L2) || d < abs(L1 - L2)
error('目标点不可达');
end
% 余弦定理求q2
cos_q2 = (x^2 + y^2 - L1^2 - L2^2) / (2 * L1 * L2);
q2 = acos(cos_q2);
% 两个解（肘部向上/向下）
q2_solutions = [q2; -q2];
% 求q1
q1_solutions = zeros(2, 1);
for i = 1:2
q2_i = q2_solutions(i);
beta = atan2(y, x);
alpha = atan2(L2 * sin(q2_i), L1 + L2 * cos(q2_i));
q1_solutions(i) = beta - alpha;
end
q_analytic = [q1_solutions, q2_solutions];
end
end
methods (Static)
function v = rotationMatrixToVector(R)
% 旋转矩阵转换为旋转向量
theta = acos((trace(R) - 1) / 2);
if abs(theta) < 1e-10
v = zeros(3, 1);
else
v = theta / (2 * sin(theta)) * [R(3,2)-R(2,3);
R(1,3)-R(3,1);
R(2,1)-R(1,2)];
end
end
end
end
1.2 空间机器人动力学
% space_robot_dynamics.m
% 空间机器人动力学建模
classdef SpaceRobotDynamics < SpaceRobotKinematics
properties
gravity % 重力向量（对于空间机器人通常为[0;0;0]）
friction_coeffs % 关节摩擦系数
end
methods
function obj = SpaceRobotDynamics(n_joints, link_lengths, link_masses)
obj = obj@SpaceRobotKinematics(n_joints, link_lengths, link_masses);
obj.gravity = [0; 0; 0]; % 空间环境无重力
obj.friction_coeffs = 0.01 * ones(1, n_joints); % 小摩擦系数
end
function M = mass_matrix(obj, q)
% 计算质量矩阵（惯性矩阵）
n = obj.n_joints;
M = zeros(n, n);
% 计算各连杆雅可比矩阵（质心）
J_com = cell(1, n);
for i = 1:n
% 计算到第i个连杆质心的雅可比
T_i = obj.forward_kinematics(q, i);
p_com_i = T_i(1:3, 1:3) * obj.com_positions(i, :)' + T_i(1:3, 4);
% 简化计算质心雅可比
J_com_i = zeros(6, n);
T_prev = eye(4);
for j = 1:i
if j <= i
z_axis = T_prev(1:3, 3);
T_j = obj.forward_kinematics(q, j);
p_joint = T_j(1:3, 4);
% 线性部分
J_com_i(1:3, j) = cross(z_axis, p_com_i - p_joint);
% 角度部分
J_com_i(4:6, j) = z_axis;
else
J_com_i(:, j) = zeros(6, 1);
end
T_prev = T_j;
end
J_com{i} = J_com_i;
end
% 计算质量矩阵 M = Σ(J_i^T * M_i * J_i)
for i = 1:n
m_i = obj.link_masses(i);
I_i = obj.inertia_tensors{i};
% 连杆的空间惯性矩阵
M_i = [m_i * eye(3), zeros(3, 3);
zeros(3, 3), I_i];
J_i = J_com{i};
M = M + J_i' * M_i * J_i;
end
end
function C = coriolis_matrix(obj, q, q_dot)
% 计算科里奥利和离心力矩阵
n = obj.n_joints;
M = obj.mass_matrix(q);
% 使用Christoffel符号计算C矩阵
C = zeros(n, n);
for i = 1:n
for j = 1:n
for k = 1:n
c_ijk = 0.5 * (diff(M(i, j), q(k)) + ...
diff(M(i, k), q(j)) - ...
diff(M(j, k), q(i)));
% 数值微分近似
epsilon = 1e-6;
q_perturbed = q;
q_perturbed(k) = q_perturbed(k) + epsilon;
M_perturbed = obj.mass_matrix(q_perturbed);
dM_dqk = (M_perturbed(i, j) - M(i, j)) / epsilon;
q_perturbed = q;
q_perturbed(j) = q_perturbed(j) + epsilon;
M_perturbed = obj.mass_matrix(q_perturbed);
dM_dqj = (M_perturbed(i, k) - M(i, k)) / epsilon;
q_perturbed = q;
q_perturbed(i) = q_perturbed(i) + epsilon;
M_perturbed = obj.mass_matrix(q_perturbed);
dM_dqi = (M_perturbed(j, k) - M(j, k)) / epsilon;
c_ijk = 0.5 * (dM_dqk + dM_dqj - dM_dqi);
C(i, j) = C(i, j) + c_ijk * q_dot(k);
end
end
end
end
function G = gravity_vector(obj, q)
% 计算重力向量（对于空间机器人，通常为0）
n = obj.n_joints;
G = zeros(n, 1);
if norm(obj.gravity) > 0
% 如果有重力，计算重力项
for i = 1:n
% 计算第i个连杆质心位置
T_i = obj.forward_kinematics(q, i);
p_com_i = T_i(1:3, 1:3) * obj.com_positions(i, :)' + T_i(1:3, 4);
% 计算雅可比矩阵的转置
J_i = obj.jacobian_com(q, i);
% 重力在基座坐标系中的表示
G_i = obj.link_masses(i) * obj.gravity;
G = G + J_i(1:3, :)' * G_i;
end
end
end
function F = friction_vector(obj, q_dot)
% 计算摩擦向量
F = -obj.friction_coeffs' .* q_dot;
end
function tau = inverse_dynamics(obj, q, q_dot, q_ddot)
% 逆动力学：计算所需的关节力矩
M = obj.mass_matrix(q);
C = obj.coriolis_matrix(q, q_dot);
G = obj.gravity_vector(q);
F = obj.friction_vector(q_dot);
tau = M * q_ddot + C * q_dot + G + F;
end
function [q_ddot, tau] = forward_dynamics(obj, q, q_dot, tau_applied)
% 正动力学：计算关节加速度
M = obj.mass_matrix(q);
C = obj.coriolis_matrix(q, q_dot);
G = obj.gravity_vector(q);
F = obj.friction_vector(q_dot);
q_ddot = M \ (tau_applied - C * q_dot - G - F);
end
function trajectory = simulate_dynamics(obj, q0, q_dot0, tau_func, tspan, dt)
% 动力学仿真
t = tspan(1):dt:tspan(2);
n_steps = length(t);
% 初始化
q = zeros(obj.n_joints, n_steps);
q_dot = zeros(obj.n_joints, n_steps);
q_ddot = zeros(obj.n_joints, n_steps);
q(:, 1) = q0;
q_dot(:, 1) = q_dot0;
% 时间积分
for k = 1:n_steps-1
% 当前状态
q_k = q(:, k);
q_dot_k = q_dot(:, k);
% 计算施加的力矩
tau_k = tau_func(t(k), q_k, q_dot_k);
% 计算加速度
q_ddot(:, k) = obj.forward_dynamics(q_k, q_dot_k, tau_k);
% 欧拉积分
q_dot(:, k+1) = q_dot_k + q_ddot(:, k) * dt;
q(:, k+1) = q_k + q_dot_k * dt + 0.5 * q_ddot(:, k) * dt^2;
% 关节限位
q(:, k+1) = max(min(q(:, k+1), pi), -pi);
end
trajectory.t = t;
trajectory.q = q;
trajectory.q_dot = q_dot;
trajectory.q_ddot = q_ddot;
end
function J_com = jacobian_com(obj, q, link_index)
% 计算指定连杆质心的雅可比矩阵
% 简化的雅可比计算
T_i = obj.forward_kinematics(q, link_index);
p_com_i = T_i(1:3, 1:3) * obj.com_positions(link_index, :)' + T_i(1:3, 4);
J_com = zeros(6, obj.n_joints);
T_prev = eye(4);
for j = 1:link_index
if j <= link_index
z_axis = T_prev(1:3, 3);
T_j = obj.forward_kinematics(q, j);
p_joint = T_j(1:3, 4);
% 线性部分
J_com(1:3, j) = cross(z_axis, p_com_i - p_joint);
% 角度部分
J_com(4:6, j) = z_axis;
end
T_prev = T_j;
end
end
end
end
二、遗传算法路径规划
2.1 基本遗传算法实现
% genetic_algorithm_path_planning.m
% 遗传算法路径规划
classdef GAPathPlanning < handle
properties
robot % 机器人对象
obstacles % 障碍物列表
start_point % 起始点
goal_point % 目标点
% 遗传算法参数
pop_size % 种群大小
num_generations % 迭代代数
mutation_rate % 变异率
crossover_rate % 交叉率
elitism_count % 精英保留数量
% 路径参数
num_waypoints % 路径点数量
joint_limits % 关节限位
end
methods
function obj = GAPathPlanning(robot, start_point, goal_point)
obj.robot = robot;
obj.start_point = start_point;
obj.goal_point = goal_point;
% 默认参数
obj.pop_size = 50;
obj.num_generations = 100;
obj.mutation_rate = 0.1;
obj.crossover_rate = 0.8;
obj.elitism_count = 5;
obj.num_waypoints = 10;
obj.joint_limits = [-pi, pi; -pi, pi]; % 2自由度示例
% 默认障碍物
obj.obstacles = [];
end
function add_obstacle(obj, type, center, radius)
% 添加障碍物
obstacle.type = type;
obstacle.center = center;
obstacle.radius = radius;
obj.obstacles = [obj.obstacles; obstacle];
end
function population = initialize_population(obj)
% 初始化种群
population = cell(obj.pop_size, 1);
for i = 1:obj.pop_size
% 随机生成路径（关节空间）
path = zeros(obj.robot.n_joints, obj.num_waypoints);
for j = 1:obj.num_waypoints
for k = 1:obj.robot.n_joints
path(k, j) = obj.joint_limits(k, 1) + ...
rand() * (obj.joint_limits(k, 2) - obj.joint_limits(k, 1));
end
end
% 确保起点和终点正确
[q_start, ~] = obj.robot.inverse_kinematics(obj.start_point, eye(3), zeros(obj.robot.n_joints, 1));
[q_goal, ~] = obj.robot.inverse_kinematics(obj.goal_point, eye(3), zeros(obj.robot.n_joints, 1));
path(:, 1) = q_start;
path(:, end) = q_goal;
population{i} = path;
end
end
function fitness = evaluate_fitness(obj, path)
% 评估路径适应度
% 适应度由多个因素组成
% 1. 路径长度（关节空间）
path_length = 0;
for i = 1:size(path, 2)-1
segment_length = norm(path(:, i+1) - path(:, i));
path_length = path_length + segment_length;
end
% 2. 障碍物碰撞检测
collision_penalty = 0;
for i = 1:size(path, 2)
% 获取末端执行器位置
[pos, ~] = obj.robot.get_end_effector_pose(path(:, i));
% 检查与障碍物的碰撞
for j = 1:length(obj.obstacles)
obstacle = obj.obstacles(j);
if strcmp(obstacle.type, 'sphere')
distance = norm(pos - obstacle.center);
if distance < obstacle.radius
collision_penalty = collision_penalty + 100;
end
end
end
end
% 3. 关节速度平滑性
smoothness_penalty = 0;
for i = 1:size(path, 2)-2
acc = norm(path(:, i+2) - 2*path(:, i+1) + path(:, i));
smoothness_penalty = smoothness_penalty + acc^2;
end
% 4. 终点精度
[final_pos, ~] = obj.robot.get_end_effector_pose(path(:, end));
goal_error = norm(final_pos - obj.goal_point);
% 总适应度（越小越好）
fitness = 0.3 * path_length + ...
0.4 * collision_penalty + ...
0.2 * smoothness_penalty + ...
0.1 * goal_error;
end
function new_population = evolve(obj, population)
% 执行一代进化
% 计算适应度
fitness = zeros(length(population), 1);
for i = 1:length(population)
fitness(i) = obj.evaluate_fitness(population{i});
end
% 排序
[sorted_fitness, indices] = sort(fitness);
% 精英选择
new_population = cell(obj.pop_size, 1);
for i = 1:obj.elitism_count
new_population{i} = population{indices(i)};
end
% 轮盘赌选择
fitness_sum = sum(1./sorted_fitness); % 适应度越大，选择概率越小
probs = (1./sorted_fitness) / fitness_sum;
cum_probs = cumsum(probs);
% 生成新个体（交叉和变异）
for i = obj.elitism_count+1:obj.pop_size
% 选择父代
r1 = rand();
parent1_idx = find(cum_probs >= r1, 1);
parent1 = population{indices(parent1_idx)};
r2 = rand();
parent2_idx = find(cum_probs >= r2, 1);
parent2 = population{indices(parent2_idx)};
% 交叉
if rand() < obj.crossover_rate
child = obj.crossover(parent1, parent2);
else
child = parent1; % 无交叉
end
% 变异
if rand() < obj.mutation_rate
child = obj.mutate(child);
end
new_population{i} = child;
end
end
function child = crossover(obj, parent1, parent2)
% 单点交叉
crossover_point = randi([2, obj.num_waypoints-1]);
child = parent1;
child(:, crossover_point:end) = parent2(:, crossover_point:end);
% 确保起点和终点不变
[q_start, ~] = obj.robot.inverse_kinematics(obj.start_point, eye(3), zeros(obj.robot.n_joints, 1));
[q_goal, ~] = obj.robot.inverse_kinematics(obj.goal_point, eye(3), zeros(obj.robot.n_joints, 1));
child(:, 1) = q_start;
child(:, end) = q_goal;
end
function mutated = mutate(obj, individual)
% 随机变异
mutated = individual;
% 随机选择一个路径点进行变异
point_idx = randi([2, obj.num_waypoints-1]); % 不变异起点和终点
% 随机选择一个关节进行变异
joint_idx = randi(obj.robot.n_joints);
% 添加随机扰动
mutation_range = 0.5; % 变异幅度
perturbation = mutation_range * (2*rand() - 1);
mutated(joint_idx, point_idx) = mutated(joint_idx, point_idx) + perturbation;
% 关节限位
mutated(joint_idx, point_idx) = max(min(mutated(joint_idx, point_idx), ...
obj.joint_limits(joint_idx, 2)), obj.joint_limits(joint_idx, 1));
end
function [best_path, best_fitness, history] = optimize_path(obj)
% 主优化函数
% 初始化种群
population = obj.initialize_population();
% 记录历史
history.best_fitness = zeros(obj.num_generations, 1);
history.avg_fitness = zeros(obj.num_generations, 1);
% 进化循环
for gen = 1:obj.num_generations
% 计算当前代适应度
fitness = zeros(obj.pop_size, 1);
for i = 1:obj.pop_size
fitness(i) = obj.evaluate_fitness(population{i});
end
% 记录统计信息
[best_fitness_gen, best_idx] = min(fitness);
avg_fitness_gen = mean(fitness);
history.best_fitness(gen) = best_fitness_gen;
history.avg_fitness(gen) = avg_fitness_gen;
fprintf('Generation %d: Best Fitness = %.4f, Avg Fitness = %.4f\n', ...
gen, best_fitness_gen, avg_fitness_gen);
% 进化
population = obj.evolve(population);
end
% 返回最佳路径
final_fitness = zeros(obj.pop_size, 1);
for i = 1:obj.pop_size
final_fitness(i) = obj.evaluate_fitness(population{i});
end
[best_fitness, best_idx] = min(final_fitness);
best_path = population{best_idx};
end
function smoothed_path = smooth_path(obj, path, alpha, beta, tolerance, iterations)
% 路径平滑（梯度下降法）
% alpha: 数据项权重
% beta: 平滑项权重
if nargin < 3
alpha = 0.5;
beta = 0.5;
tolerance = 1e-4;
iterations = 100;
end
smoothed_path = path;
for iter = 1:iterations
new_path = smoothed_path;
for i = 2:size(path, 2)-1
% 数据项：保持接近原始路径
data_term = alpha * (path(:, i) - smoothed_path(:, i));
% 平滑项：减小曲率
smooth_term = beta * (smoothed_path(:, i-1) - 2*smoothed_path(:, i) + smoothed_path(:, i+1));
% 更新
new_path(:, i) = smoothed_path(:, i) + data_term - smooth_term;
end
% 检查收敛
if norm(new_path - smoothed_path) < tolerance
break;
end
smoothed_path = new_path;
end
end
function trajectory = generate_trajectory(obj, path, total_time)
% 生成平滑轨迹（五次多项式插值）
n_points = size(path, 2);
t_points = linspace(0, total_time, n_points);
% 为每个关节生成轨迹
trajectory.t = linspace(0, total_time, 100);
trajectory.q = zeros(obj.robot.n_joints, length(trajectory.t));
trajectory.q_dot = zeros(obj.robot.n_joints, length(trajectory.t));
trajectory.q_ddot = zeros(obj.robot.n_joints, length(trajectory.t));
for joint = 1:obj.robot.n_joints
% 提取关节路径
joint_path = path(joint, :);
% 计算五次多项式系数
coeffs = zeros(n_points-1, 6);
for segment = 1:n_points-1
t0 = t_points(segment);
t1 = t_points(segment+1);
dt = t1 - t0;
q0 = joint_path(segment);
q1 = joint_path(segment+1);
% 假设起点和终点速度为0
v0 = 0;
v1 = 0;
a0 = 0;
a1 = 0;
% 计算系数
coeffs(segment, :) = quintic_polynomial_coeffs(t0, q0, v0, a0, t1, q1, v1, a1);
end
% 计算轨迹点
for i = 1:length(trajectory.t)
t = trajectory.t(i);
% 找到对应的段
segment = find(t >= t_points, 1, 'last');
if segment == n_points
segment = n_points - 1;
end
if segment > 0 && segment < n_points
t_rel = t - t_points(segment);
c = coeffs(segment, :);
% 位置
trajectory.q(joint, i) = c(1) + c(2)*t_rel + c(3)*t_rel^2 + ...
c(4)*t_rel^3 + c(5)*t_rel^4 + c(6)*t_rel^5;
% 速度
trajectory.q_dot(joint, i) = c(2) + 2*c(3)*t_rel + 3*c(4)*t_rel^2 + ...
4*c(5)*t_rel^3 + 5*c(6)*t_rel^4;
% 加速度
trajectory.q_ddot(joint, i) = 2*c(3) + 6*c(4)*t_rel + ...
12*c(5)*t_rel^2 + 20*c(6)*t_rel^3;
end
end
end
end
end
end
function coeffs = quintic_polynomial_coeffs(t0, q0, v0, a0, t1, q1, v1, a1)
% 计算五次多项式系数
dt = t1 - t0;
% 边界条件矩阵
A = [1, t0, t0^2, t0^3, t0^4, t0^5;
0, 1, 2*t0, 3*t0^2, 4*t0^3, 5*t0^4;
0, 0, 2, 6*t0, 12*t0^2, 20*t0^3;
1, t1, t1^2, t1^3, t1^4, t1^5;
0, 1, 2*t1, 3*t1^2, 4*t1^3, 5*t1^4;
0, 0, 2, 6*t1, 12*t1^2, 20*t1^3];
b = [q0; v0; a0; q1; v1; a1];
coeffs = (A \ b)';
end
2.2 路径规划示例
% main_path_planning.m
% 主程序：空间机器人路径规划示例
function main_path_planning()
clear all; close all; clc;
% 创建空间机器人（2自由度）
link_lengths = [1.0, 0.8];
link_masses = [2.0, 1.5];
robot = SpaceRobotDynamics(2, link_lengths, link_masses);
% 设置路径规划参数
start_point = [0.5, 0.2, 0]';
goal_point = [1.2, 0.8, 0]';
planner = GAPathPlanning(robot, start_point, goal_point);
% 添加障碍物
planner.add_obstacle('sphere', [0.7, 0.5, 0]', 0.2);
planner.add_obstacle('sphere', [1.0, 0.3, 0]', 0.15);
% 设置遗传算法参数
planner.pop_size = 30;
planner.num_generations = 50;
planner.num_waypoints = 8;
% 运行优化
fprintf('开始路径优化...\n');
tic;
[best_path, best_fitness, history] = planner.optimize_path();
elapsed_time = toc;
fprintf('优化完成，用时: %.2f秒\n', elapsed_time);
fprintf('最佳适应度: %.4f\n', best_fitness);
% 路径平滑
fprintf('进行路径平滑...\n');
smoothed_path = planner.smooth_path(best_path, 0.3, 0.7, 1e-4, 50);
% 生成轨迹
trajectory = planner.generate_trajectory(smoothed_path, 10);
% 可视化
visualize_path_planning(robot, planner, best_path, smoothed_path, trajectory, history);
% 动力学仿真
simulate_robot_trajectory(robot, trajectory);
end
function visualize_path_planning(robot, planner, raw_path, smooth_path, trajectory, history)
% 创建可视化
% 图1：适应度演化
figure('Position', [100, 100, 1200, 800]);
subplot(2, 3, 1);
plot(1:planner.num_generations, history.best_fitness, 'b-', 'LineWidth', 2);
hold on;
plot(1:planner.num_generations, history.avg_fitness, 'r--', 'LineWidth', 1.5);
xlabel('Generation');
ylabel('Fitness');
title('Genetic Algorithm Evolution');
legend('Best Fitness', 'Average Fitness');
grid on;
% 图2：工作空间路径
subplot(2, 3, 2);
hold on;
% 绘制障碍物
for i = 1:length(planner.obstacles)
obstacle = planner.obstacles(i);
if strcmp(obstacle.type, 'sphere')
[X, Y, Z] = sphere(20);
surf(obstacle.radius*X + obstacle.center(1), ...
obstacle.radius*Y + obstacle.center(2), ...
obstacle.radius*Z + obstacle.center(3), ...
'FaceColor', 'red', 'FaceAlpha', 0.3, 'EdgeColor', 'none');
end
end
% 绘制起点和终点
plot3(planner.start_point(1), planner.start_point(2), planner.start_point(3), ...
'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g');
plot3(planner.goal_point(1), planner.goal_point(2), planner.goal_point(3), ...
'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');
% 绘制原始路径
for i = 1:size(raw_path, 2)
[pos, ~] = robot.get_end_effector_pose(raw_path(:, i));
if i == 1
plot3(pos(1), pos(2), pos(3), 'bo', 'MarkerSize', 6);
elseif i == size(raw_path, 2)
plot3(pos(1), pos(2), pos(3), 'bo', 'MarkerSize', 6);
else
plot3(pos(1), pos(2), pos(3), 'b.', 'MarkerSize', 10);
end
end
% 绘制平滑路径
for i = 1:size(smooth_path, 2)
[pos, ~] = robot.get_end_effector_pose(smooth_path(:, i));
if i == 1
plot3(pos(1), pos(2), pos(3), 'mo', 'MarkerSize', 6);
elseif i == size(smooth_path, 2)
plot3(pos(1), pos(2), pos(3), 'mo', 'MarkerSize', 6);
else
plot3(pos(1), pos(2), pos(3), 'm.', 'MarkerSize', 10);
end
end
% 连接点
for i = 1:size(raw_path, 2)-1
[pos1, ~] = robot.get_end_effector_pose(raw_path(:, i));
[pos2, ~] = robot.get_end_effector_pose(raw_path(:, i+1));
line([pos1(1), pos2(1)], [pos1(2), pos2(2)], [pos1(3), pos2(3)], ...
'Color', 'b', 'LineStyle', '--', 'LineWidth', 1);
end
for i = 1:size(smooth_path, 2)-1
[pos1, ~] = robot.get_end_effector_pose(smooth_path(:, i));
[pos2, ~] = robot.get_end_effector_pose(smooth_path(:, i+1));
line([pos1(1), pos2(1)], [pos1(2), pos2(2)], [pos1(3), pos2(3)], ...
'Color', 'm', 'LineStyle', '-', 'LineWidth', 2);
end
xlabel('X'); ylabel('Y'); zlabel('Z');
title('Workspace Path Planning');
legend('Obstacles', 'Start', 'Goal', 'Raw Path', 'Smoothed Path');
grid on; axis equal;
view(2);
% 图3：关节空间路径
subplot(2, 3, 3);
hold on;
plot(1:size(raw_path, 2), raw_path(1, :), 'b-o', 'LineWidth', 1.5);
plot(1:size(raw_path, 2), raw_path(2, :), 'r-o', 'LineWidth', 1.5);
plot(1:size(smooth_path, 2), smooth_path(1, :), 'm--s', 'LineWidth', 2);
plot(1:size(smooth_path, 2), smooth_path(2, :), 'c--s', 'LineWidth', 2);
xlabel('Waypoint Index');
ylabel('Joint Angle (rad)');
title('Joint Space Paths');
legend('Joint 1 (Raw)', 'Joint 2 (Raw)', 'Joint 1 (Smooth)', 'Joint 2 (Smooth)');
grid on;
% 图4：关节角度轨迹
subplot(2, 3, 4);
hold on;
plot(trajectory.t, trajectory.q(1, :), 'b-', 'LineWidth', 2);
plot(trajectory.t, trajectory.q(2, :), 'r-', 'LineWidth', 2);
xlabel('Time (s)');
ylabel('Joint Angle (rad)');
title('Joint Trajectories');
legend('Joint 1', 'Joint 2');
grid on;
% 图5：关节速度轨迹
subplot(2, 3, 5);
hold on;
plot(trajectory.t, trajectory.q_dot(1, :), 'b-', 'LineWidth', 2);
plot(trajectory.t, trajectory.q_dot(2, :), 'r-', 'LineWidth', 2);
xlabel('Time (s)');
ylabel('Joint Velocity (rad/s)');
title('Joint Velocities');
legend('Joint 1', 'Joint 2');
grid on;
% 图6：关节加速度轨迹
subplot(2, 3, 6);
hold on;
plot(trajectory.t, trajectory.q_ddot(1, :), 'b-', 'LineWidth', 2);
plot(trajectory.t, trajectory.q_ddot(2, :), 'r-', 'LineWidth', 2);
xlabel('Time (s)');
ylabel('Joint Acceleration (rad/s^2)');
title('Joint Accelerations');
legend('Joint 1', 'Joint 2');
grid on;
sgtitle('Space Robot Path Planning Results');
end
function simulate_robot_trajectory(robot, trajectory)
% 机器人轨迹仿真
% 计算所需力矩（逆动力学）
tau = zeros(robot.n_joints, length(trajectory.t));
for i = 1:length(trajectory.t)
q = trajectory.q(:, i);
q_dot = trajectory.q_dot(:, i);
q_ddot = trajectory.q_ddot(:, i);
tau(:, i) = robot.inverse_dynamics(q, q_dot, q_ddot);
end
% 创建动画
figure('Position', [100, 100, 800, 600]);
for i = 1:10:length(trajectory.t)
clf;
hold on;
% 绘制机器人
q_current = trajectory.q(:, i);
plot_robot(robot, q_current);
% 绘制轨迹
pos_history = zeros(3, floor(i/10)+1);
for j = 1:10:i
[pos, ~] = robot.get_end_effector_pose(trajectory.q(:, j));
pos_history(:, ceil(j/10)) = pos;
end
plot3(pos_history(1, :), pos_history(2, :), pos_history(3, :), 'g-', 'LineWidth', 1);
% 设置图形属性
xlabel('X (m)'); ylabel('Y (m)'); zlabel('Z (m)');
title(sprintf('Robot Motion Simulation (t = %.2f s)', trajectory.t(i)));
grid on; axis equal;
xlim([-0.5, 2]); ylim([-1, 1]); zlim([-0.5, 0.5]);
view(45, 30);
drawnow;
pause(0.01);
end
% 绘制力矩曲线
figure;
subplot(2, 1, 1);
plot(trajectory.t, tau(1, :), 'b-', 'LineWidth', 2);
xlabel('Time (s)');
ylabel('Torque (Nm)');
title('Joint 1 Torque');
grid on;
subplot(2, 1, 2);
plot(trajectory.t, tau(2, :), 'r-', 'LineWidth', 2);
xlabel('Time (s)');
ylabel('Torque (Nm)');
title('Joint 2 Torque');
grid on;
sgtitle('Required Joint Torques');
end
function plot_robot(robot, q)
% 绘制机器人
colors = {'b', 'r', 'g', 'm', 'c'};
% 绘制各连杆
for i = 1:robot.n_joints
T_i = robot.forward_kinematics(q, i);
if i == 1
p_prev = [0; 0; 0];
else
T_prev = robot.forward_kinematics(q, i-1);
p_prev = T_prev(1:3, 4);
end
p_current = T_i(1:3, 4);
% 绘制连杆
line([p_prev(1), p_current(1)], ...
[p_prev(2), p_current(2)], ...
[p_prev(3), p_current(3)], ...
'Color', colors{mod(i-1, length(colors))+1}, ...
'LineWidth', 3);
% 绘制关节
plot3(p_current(1), p_current(2), p_current(3), ...
'ko', 'MarkerSize', 8, 'MarkerFaceColor', 'k');
end
% 绘制基座
plot3(0, 0, 0, 'ks', 'MarkerSize', 12, 'MarkerFaceColor', 'k');
% 绘制末端执行器
T_end = robot.forward_kinematics(q, robot.n_joints);
p_end = T_end(1:3, 4);
plot3(p_end(1), p_end(2), p_end(3), ...
'go', 'MarkerSize', 10, 'MarkerFaceColor', 'g');
end
三、高级功能扩展
3.1 自适应遗传算法
% adaptive_ga_path_planning.m
% 自适应遗传算法
classdef AdaptiveGAPathPlanning < GAPathPlanning
methods
function obj = AdaptiveGAPathPlanning(robot, start_point, goal_point)
obj = obj@GAPathPlanning(robot, start_point, goal_point);
end
function [best_path, best_fitness, history] = optimize_path_adaptive(obj)
% 自适应遗传算法
population = obj.initialize_population();
history.best_fitness = zeros(obj.num_generations, 1);
history.avg_fitness = zeros(obj.num_generations, 1);
history.mutation_rate = zeros(obj.num_generations, 1);
history.crossover_rate = zeros(obj.num_generations, 1);
% 初始参数
current_mutation_rate = obj.mutation_rate;
current_crossover_rate = obj.crossover_rate;
for gen = 1:obj.num_generations
% 计算适应度
fitness = zeros(obj.pop_size, 1);
for i = 1:obj.pop_size
fitness(i) = obj.evaluate_fitness(population{i});
end
% 记录统计信息
[best_fitness_gen, best_idx] = min(fitness);
avg_fitness_gen = mean(fitness);
history.best_fitness(gen) = best_fitness_gen;
history.avg_fitness(gen) = avg_fitness_gen;
history.mutation_rate(gen) = current_mutation_rate;
history.crossover_rate(gen) = current_crossover_rate;
% 自适应调整参数
if gen > 1
% 如果适应度改善缓慢，增加变异率
improvement = history.best_fitness(gen-1) - best_fitness_gen;
if improvement < 0.01
current_mutation_rate = min(0.3, current_mutation_rate * 1.1);
current_crossover_rate = max(0.6, current_crossover_rate * 0.9);
else
current_mutation_rate = max(0.05, current_mutation_rate * 0.95);
current_crossover_rate = min(0.95, current_crossover_rate * 1.05);
end
end
fprintf('Generation %d: Fitness=%.4f, Mutation=%.3f, Crossover=%.3f\n', ...
gen, best_fitness_gen, current_mutation_rate, current_crossover_rate);
% 使用当前参数进化
obj.mutation_rate = current_mutation_rate;
obj.crossover_rate = current_crossover_rate;
population = obj.evolve(population);
end
% 返回最佳路径
final_fitness = zeros(obj.pop_size, 1);
for i = 1:obj.pop_size
final_fitness(i) = obj.evaluate_fitness(population{i});
end
[best_fitness, best_idx] = min(final_fitness);
best_path = population{best_idx};
end
end
end
3.2 多目标优化
% multi_objective_path_planning.m
% 多目标路径规划
classdef MultiObjectivePathPlanning < GAPathPlanning
methods
function obj = MultiObjectivePathPlanning(robot, start_point, goal_point)
obj = obj@GAPathPlanning(robot, start_point, goal_point);
end
function [objectives, constraints] = evaluate_multi_objective(obj, path)
% 评估多个目标函数
objectives = zeros(3, 1); % 3个目标
% 目标1：路径长度
path_length = 0;
for i = 1:size(path, 2)-1
segment_length = norm(path(:, i+1) - path(:, i));
path_length = path_length + segment_length;
end
objectives(1) = path_length;
% 目标2：能量消耗（近似为力矩平方积分）
energy = 0;
for i = 1:size(path, 2)-1
q = path(:, i);
q_dot = (path(:, i+1) - path(:, i)) / 1.0; % 假设单位时间
q_ddot = zeros(size(q_dot)); % 简化为0
tau = obj.robot.inverse_dynamics(q, q_dot, q_ddot);
energy = energy + norm(tau)^2;
end
objectives(2) = energy;
% 目标3：安全距离（与障碍物的最小距离）
min_distance = inf;
for i = 1:size(path, 2)
[pos, ~] = obj.robot.get_end_effector_pose(path(:, i));
for j = 1:length(obj.obstacles)
obstacle = obj.obstacles(j);
distance = norm(pos - obstacle.center) - obstacle.radius;
min_distance = min(min_distance, distance);
end
end
objectives(3) = -min_distance; % 负号表示越大越好
% 约束：碰撞检测
constraints = 0;
for i = 1:size(path, 2)
[pos, ~] = obj.robot.get_end_effector_pose(path(:, i));
for j = 1:length(obj.obstacles)
obstacle = obj.obstacles(j);
distance = norm(pos - obstacle.center);
if distance < obstacle.radius
constraints = constraints + (obstacle.radius - distance);
end
end
end
end
function [pareto_front, pareto_solutions] = nsga_ii(obj)
% NSGA-II多目标优化
population = obj.initialize_population();
pop_size = length(population);
% 初始化
objectives = zeros(pop_size, 3);
constraints = zeros(pop_size, 1);
for i = 1:pop_size
[obj_vec, constr] = obj.evaluate_multi_objective(population{i});
objectives(i, :) = obj_vec';
constraints(i) = constr;
end
% NSGA-II主循环
for gen = 1:obj.num_generations
% 非支配排序
[fronts, ranks] = non_dominated_sort(objectives, constraints);
% 计算拥挤度
crowding_distances = crowding_distance_assignment(objectives, fronts);
% 选择
selected_indices = tournament_selection(ranks, crowding_distances, pop_size);
% 交叉和变异
new_population = cell(pop_size, 1);
for i = 1:pop_size
parent1 = population{selected_indices(i)};
parent2 = population{selected_indices(mod(i, pop_size)+1)};
% 交叉
child = obj.crossover(parent1, parent2);
% 变异
if rand() < obj.mutation_rate
child = obj.mutate(child);
end
new_population{i} = child;
end
population = new_population;
% 重新评估
for i = 1:pop_size
[obj_vec, constr] = obj.evaluate_multi_objective(population{i});
objectives(i, :) = obj_vec';
constraints(i) = constr;
end
fprintf('Generation %d\n', gen);
end
% 提取帕累托前沿
[fronts, ~] = non_dominated_sort(objectives, constraints);
pareto_indices = fronts{1};
pareto_front = objectives(pareto_indices, :);
pareto_solutions = population(pareto_indices);
end
end
end
function [fronts, ranks] = non_dominated_sort(objectives, constraints)
% 非支配排序
n = size(objectives, 1);
S = cell(n, 1);
n_p = zeros(n, 1);
ranks = zeros(n, 1);
% 第一遍：计算支配关系
for i = 1:n
for j = 1:n
if i ~= j
% 检查i是否支配j
if dominates(objectives(i, :), constraints(i), ...
objectives(j, :), constraints(j))
S{i} = [S{i}, j];
elseif dominates(objectives(j, :), constraints(j), ...
objectives(i, :), constraints(i))
n_p(i) = n_p(i) + 1;
end
end
end
end
% 分层
fronts = {};
current_front = find(n_p == 0);
while ~isempty(current_front)
fronts{end+1} = current_front;
next_front = [];
for i = current_front
for j = S{i}
n_p(j) = n_p(j) - 1;
if n_p(j) == 0
next_front = [next_front, j];
ranks(j) = length(fronts);
end
end
end
current_front = next_front;
end
end
function d = dominates(obj1, constr1, obj2, constr2)
% 检查解1是否支配解2
% 考虑约束违反
if constr1 > 0 && constr2 > 0
% 两者都违反约束，选择违反较少的
d = constr1 < constr2;
elseif constr1 > 0
% 只有解1违反约束
d = false;
elseif constr2 > 0
% 只有解2违反约束
d = true;
else
% 两者都满足约束，比较目标函数
d = all(obj1 <= obj2) && any(obj1 < obj2);
end
end
function distances = crowding_distance_assignment(objectives, fronts)
% 计算拥挤度距离
n = size(objectives, 1);
m = size(objectives, 2);
distances = zeros(n, 1);
for f = 1:length(fronts)
front = fronts{f};
front_size = length(front);
if front_size > 0
front_distances = zeros(front_size, 1);
front_objectives = objectives(front, :);
for obj_idx = 1:m
% 按当前目标排序
[sorted_obj, sort_idx] = sort(front_objectives(:, obj_idx));
% 边界点的距离设为无穷大
front_distances(sort_idx(1)) = inf;
front_distances(sort_idx(end)) = inf;
% 计算中间点的距离
for i = 2:front_size-1
if isinf(front_distances(sort_idx(i)))
continue;
end
range = sorted_obj(end) - sorted_obj(1);
if range > 0
front_distances(sort_idx(i)) = front_distances(sort_idx(i)) + ...
(sorted_obj(i+1) - sorted_obj(i-1)) / range;
end
end
end
distances(front) = front_distances;
end
end
end
function selected = tournament_selection(ranks, distances, pop_size)
% 锦标赛选择
tournament_size = 2;
selected = zeros(pop_size, 1);
for i = 1:pop_size
% 随机选择参赛者
contestants = randperm(length(ranks), tournament_size);
% 选择优胜者
[~, best_idx] = min(ranks(contestants));
if ranks(contestants(1)) == ranks(contestants(2))
% 如果等级相同，选择拥挤度大的
if distances(contestants(1)) > distances(contestants(2))
best_idx = 1;
else
best_idx = 2;
end
end
selected(i) = contestants(best_idx);
end
end
参考代码 空间机器人动力学正逆解及遗传算法路径规划 www.3dddown.com/cna/113093.html
四、使用说明
- 基本使用：
% 创建机器人
robot = SpaceRobotDynamics(2, [1, 0.8], [2, 1.5]);
% 正向运动学
q = [0.5, 0.3]';
[pos, R] = robot.get_end_effector_pose(q);
% 逆运动学
target_pos = [1.2, 0.5, 0]';
target_R = eye(3);
[q_sol, success] = robot.inverse_kinematics(target_pos, target_R, [0, 0]');
% 动力学计算
q_dot = [0.1, 0.2]';
q_ddot = [0.05, 0.1]';
tau = robot.inverse_dynamics(q, q_dot, q_ddot);
- 路径规划：
% 创建规划器
planner = GAPathPlanning(robot, start_point, goal_point);
% 添加障碍物
planner.add_obstacle('sphere', [0.7, 0.5, 0]', 0.2);
% 优化路径
[best_path, best_fitness] = planner.optimize_path();
% 生成轨迹
trajectory = planner.generate_trajectory(best_path, 10);
- 高级优化：
% 自适应遗传算法
adaptive_planner = AdaptiveGAPathPlanning(robot, start_point, goal_point);
[path_adaptive, fitness_adaptive] = adaptive_planner.optimize_path_adaptive();
% 多目标优化
multi_planner = MultiObjectivePathPlanning(robot, start_point, goal_point);
[pareto_front, pareto_solutions] = multi_planner.nsga_ii();
浙公网安备 33010602011771号