机器人matlab仿真程序

入门级：2D移动机器人路径规划与仿真

这是最基础的机器人仿真,通常涉及一个在二维平面上移动的差速驱动机器人（如小轮车），我们将实现一个简单的“人工势场法”（Artificial Potential Field）来进行实时避障和路径规划。

核心概念

• 机器人模型：使用阿克曼转向模型或差速驱动模型，这里我们使用更简单的Dubins Car模型，即机器人有前进速度和转向角速度。
• 环境：一个2D平面，包含静态障碍物（圆形）和目标点。
• 控制算法：人工势场法，目标点对机器人产生“引力”，障碍物对机器人产生“斥力”，机器人朝着合力方向移动。

MATLAB仿真程序代码

% =========================================================================
% 2D移动机器人仿真：基于人工势场法的避障路径规划
% =========================================================================
clear; clc; close all;
%% 1. 环境初始化
% 机器人初始状态 [x, y, theta] - (x, y)位置, theta朝向角
robot_state = [0, 0, 0]; 
% 目标点位置
goal = [8, 8];
% 障碍物列表 [x, y, radius]
obstacles = [3, 4, 0.5; 6, 2, 0.8; 5, 6, 0.6];
%% 2. 仿真参数
dt = 0.1;            % 时间步长
sim_time = 30;       % 总仿真时间
steps = sim_time / dt;
robot_path = zeros(steps, 3); % 存储机器人路径
% 机器人物理参数
v_max = 1.0;         % 最大线速度
omega_max = 1.0;     % 最大角速度
k_att = 0.5;         % 引力增益系数
k_rep = 2.0;         % 斥力增益系数
d_safe = 1.0;        % 安全距离
%% 3. 仿真主循环
for i = 1:steps
    % --- 获取当前状态 ---
    x = robot_state(1);
    y = robot_state(2);
    theta = robot_state(3);
    % --- 记录路径 ---
    robot_path(i, :) = [x, y, theta];
    % --- 检查是否到达目标 ---
    if norm([x, y] - goal) < 0.3
        disp('机器人已到达目标点！');
        break;
    end
    % --- 计算引力 ---
    F_att = k_att * (goal - [x, y]);
    % --- 计算斥力 ---
    F_rep = [0, 0];
    for j = 1:size(obstacles, 1)
        obs = obstacles(j, :);
        obs_pos = obs(1:2);
        obs_radius = obs(3);
        % 计算到障碍物中心的向量
        d_vec = [x, y] - obs_pos;
        d = norm(d_vec);
        % 如果在障碍物的影响范围内
        if d < d_safe
            % 斥力大小与距离成反比
            F_rep_mag = k_rep * (1/d - 1/d_safe)^2 / (d^2);
            % 斥力方向为远离障碍物中心
            F_rep = F_rep + F_rep_mag * (d_vec / d);
        end
    end
    % --- 合力与控制 ---
    F_total = F_att + F_rep;
    % 将合力转换为期望的速度和角速度
    v_desired = norm(F_total); % 线速度大小
    if v_desired > v_max
        v_desired = v_max;
    end
    % 计算期望的朝向角
    theta_desired = atan2(F_total(2), F_total(1));
    % 计算角速度 (使用简单的P控制器)
    omega_desired = 5.0 * (theta_desired - theta);
    if abs(omega_desired) > omega_max
        omega_desired = sign(omega_desired) * omega_max;
    end
    % --- 更新机器人状态 (Dubins Car模型) ---
    robot_state(1) = x + v_desired * cos(theta) * dt;
    robot_state(2) = y + v_desired * sin(theta) * dt;
    robot_state(3) = theta + omega_desired * dt;
    % --- 可视化 ---
    drawrobot(x, y, theta, goal, obstacles);
    pause(dt);
end
%% 4. 绘制最终路径
figure;
hold on;
axis equal;
grid on;
% 绘制障碍物
for i = 1:size(obstacles, 1)
    pos = obstacles(i, 1:2);
    r = obstacles(i, 3);
    viscircles(pos, r, 'Color', 'r', 'LineStyle', '--');
end
% 绘制目标点
plot(goal(1), goal(2), 'g*', 'MarkerSize', 15, 'LineWidth', 2);
% 绘制机器人轨迹
plot(robot_path(:, 1), robot_path(:, 2), 'b-', 'LineWidth', 2);'机器人路径规划仿真结果');
xlabel('X (m)');
ylabel('Y (m)');
legend('机器人轨迹', '目标点', '障碍物', 'Location', 'best');
hold off;
%% =========================================================================
% 辅作函数：绘制机器人
% =========================================================================
function drawrobot(x, y, theta, goal, obstacles)
    persistent h_robot h_goal h_obs h_path h_arrow;
    if isempty(h_robot)
        % 第一次调用，创建图形窗口和所有图形对象
        figure;
        hold on;
        axis equal;
        grid on;
        xlim([-2, 10]);
        ylim([-2, 10]);
        title('机器人实时仿真');
        xlabel('X (m)');
        ylabel('Y (m)');
        h_goal = plot(goal(1), goal(2), 'g*', 'MarkerSize', 15, 'LineWidth', 2);
        for i = 1:size(obstacles, 1)
            pos = obstacles(i, 1:2);
            r = obstacles(i, 3);
            h_obs(i) = viscircles(pos, r, 'Color', 'r', 'LineStyle', '--');
        end
        h_path = plot(x, y, 'b-', 'LineWidth', 1.5);
        h_robot = plot(x, y, 'ko', 'MarkerSize', 8, 'MarkerFaceColor', 'b');
        h_arrow = quiver(x, y, 0, 0, 0, 'k', 'LineWidth', 2);
    else
        % 更新现有图形对象
        set(h_path, 'XData', [get(h_path, 'XData'), x], 'YData', [get(h_path, 'YData'), y]);
        set(h_robot, 'XData', x, 'YData', y);
        % 更新方向箭头
        arrow_length = 0.5;
        dx = arrow_length * cos(theta);
        dy = arrow_length * sin(theta);
        set(h_arrow, 'XData', x, 'YData', y, 'UData', dx, 'VData', dy);
    end
    drawnow;
end

如何运行

1. 将以上代码复制到MATLAB脚本文件（.m文件）中。
2. 点击“运行”按钮。
3. 你会看到一个弹出窗口,实时显示机器人（蓝色圆圈+黑色箭头）如何避开红色障碍物，最终到达绿色目标点，仿真结束后，另一个窗口会显示完整的路径轨迹。

进阶级：使用Robotics System Toolbox - 2D移动机器人

MATLAB官方的Robotics System Toolbox提供了更强大、更标准化的工具，可以让你用更少的代码实现更复杂的仿真。

核心概念

• robotics.DifferentialDriveRobot类：专门用于创建和仿真差分驱动机器人。
• unicycleMotionModel：一种更通用的运动学模型。
• plannerAStar：A*路径规划算法，可以找到最优路径。
• controllerPurePursuit：纯路径跟随控制器，用于沿着规划好的路径行驶。

MATLAB仿真程序代码

% =========================================================================
% 使用Robotics System Toolbox进行2D移动机器人仿真
% =========================================================================
clear; clc; close all;
%% 1. 创建机器人模型
% 创建一个差分驱动机器人对象
robot = robotics.DifferentialDriveRobot;
% 设置机器人尺寸 (半径, 轮距)
robot.WheelRadius = 0.1;
robot.WheelBase = 0.2;
% 设置运动学约束
robot.MaxAngularVelocity = 2.0; % rad/s
robot.MaxLinearVelocity = 1.0;  % m/s
%% 2. 创建环境地图
% 创建一个10x10米的栅格地图
map = robotics.BinaryOccupancyGrid(10, 10, 20);
% 添加一些障碍物 (1表示障碍物)
setOccupancy(map, 3:4, 3:4, 1);
setOccupancy(map, 6:7, 2:3, 1);
setOccupancy(map, 5:6, 6:7, 1);
%% 3. 路径规划
% 使用A*算法进行路径规划
startPose = [1, 1, 0]; % [x, y, theta]
goalPose = [9, 9, 0];  % [x, y, theta]
planner = plannerAStar(map);
planner.ValidationDistance = 0.5; % 验证路径的距离
path = planner(startPose(1:2), goalPose(1:2));
% 将路径点转换为机器人状态 [x, y, theta]
refPath = path;
refPath(:, 3) = atan2(diff(path(:, 2)), diff(path(:, 1))); % 计算每个路径点的朝向
refPath = [path(1, :); refPath]; % 添加第一个点的朝向 (设为0)
%% 4. 创建路径跟随控制器
controller = controllerPurePursuit;
controller.Waypoints = refPath(:, 1:2);
controller.LookaheadDistance = 0.5; % 前视距离
%% 5. 仿真设置
dt = 0.05;
simTime = 30;
numSteps = simTime / dt;
% 存储机器人状态
actualPose = startPose;
actualPoses = zeros(numSteps, 3);
%% 6. 仿真主循环
for i = 1:numSteps
    % 获取控制指令 [v, omega]
    [v, omega] = controller(actualPose(1:2));
    % 应用控制指令到机器人模型
    [actualPose, ~] = robot.step(actualPose, [v, omega], dt);
    % 存储状态
    actualPoses(i, :) = actualPose;
    % 可视化
    show(map);
    hold on;
    plot(actualPoses(1:i, 1), actualPoses(1:i, 2), 'b-o', 'MarkerSize', 3);
    plot(refPath(:, 1), refPath(:, 2), 'g--');
    plot(actualPose(1), actualPose(2), 'ro', 'MarkerSize', 8, 'MarkerFaceColor', 'r');
    drawnow;
    pause(dt);
end
disp('仿真完成！');

如何运行

1. 确保你已经安装了Robotics System Toolbox。
2. 将代码复制到.m文件并运行。
3. 你会看到机器人沿着绿色的规划路径（虚线）行驶，实际轨迹用蓝色实线表示。

高级应用：6自由度(6-DOF)机械臂仿真

对于机械臂,仿真通常涉及运动学（正解和逆解）和动力学（力、力矩分析）。

核心概念

• rigidBodyTree类：这是Robotics System Toolbox中用于表示和操作机器人（尤其是机械臂）的核心数据结构。
• addBody和fixedJoint：用于构建机械臂的树状结构。
• inverseKinematics：求解逆运动学，即给定末端执行器的位姿，求解各个关节的角度。
• forwardKinematics：求解正运动学，即给定各个关节的角度，求解末端执行器的位姿。

MATLAB仿真程序代码

% =========================================================================
% 6-DOF机械臂运动学仿真
% =========================================================================
clear; clc; close all;
%% 1. 创建机械臂的刚体树模型
robot = rigidBodyTree;
% --- 添加连杆和关节 ---
% 基座 (固定)
baseBody = rigidBody('base');
baseBody.Joint.Fixed = true;
addBody(robot, baseBody, 'base');
% 连杆1 (绕Z轴旋转)
body1 = rigidBody('link1');
body1.Joint.Name = 'joint1';
body1.Joint.LimitBounds = [-pi, pi];
body1.Joint.HomeTransform = trvec2tform([0 0 0.1]) * axang2tform([0 0 1 0]); % 平移+旋转
body1.CenterOfMass = [0 0 0.05];
body1.Mass = 2;
addBody(robot, body1, 'base');
% 连杆2 (绕Y轴旋转)
body2 = rigidBody('link2');
body2.Joint.Name = 'joint2';
body2.Joint.LimitBounds = [-pi/2, pi/2];
body2.Joint.HomeTransform = trvec2tform([0 0 0.3]);
body2.CenterOfMass = [0 0 0.15];
body2.Mass = 1.5;
addBody(robot, body2, 'link1');
% 连杆3 (绕Y轴旋转)
body3 = rigidBody('link3');
body3.Joint.Name = 'joint3';
body3.Joint.LimitBounds = [-pi/2, pi/2];
body3.Joint.HomeTransform = trvec2tform([0 0 0.25]);
body3.CenterOfMass = [0 0 0.125];
body3.Mass = 1;
addBody(robot, body3, 'link2');
% 末端执行器 (末端连杆)
body4 = rigidBody('endEffector');
body4.Joint.Name = 'joint4';
body4.Joint.LimitBounds = [-pi/2, pi/2];
body4.Joint.HomeTransform = trvec2tform([0 0 0.1]);
body4.CenterOfMass = [0 0 0.05];
body4.Mass = 0.5;
addBody(robot, body4, 'link3');
% 显示机器人模型
show(robot);'机械臂初始模型');
view(3);
%% 2. 设置目标位姿并求解逆运动学
% 定义末端执行器的目标位姿 (4x4齐次变换矩阵)
% 目标位置在 [0.4, 0.2, 0.6]，姿态为绕Y轴旋转90度
targetPose = trvec2tform([0.4, 0.2, 0.6]) * axang2tform([0 1 0 pi/2]);
% 创建逆运动学求解器
ik = inverseKinematics('RigidBodyTree', robot);
weights = [1, 1, 1, 1, 1, 1]; % 位置和姿态的权重
ik.MaxIterations = 100;
% 初始关节角度
initialConfig = homeConfiguration(robot);
% 求解逆运动学
[config, solutionInfo] = ik(initialConfig, targetPose, weights);
% 检查求解是否成功
if solutionInfo.Status == 0
    disp('逆运动学求解成功！');
    disp('求解出的关节角度 (弧度):');
    disp(config);
else
    error('逆运动学求解失败: %s', solutionInfo.InfoMessage);
end
%% 3. 仿真运动过程
% 创建一个简单的轨迹 (从初始位置到目标位置)
numSteps = 50;
jointTraj = zeros(numSteps, length(config));
for i = 1:numSteps
    jointTraj(i, :) = initialConfig + (config - initialConfig) * (i-1) / (numSteps-1);
end
% 动画显示
figure;
show(robot, 'Frames', 'on', 'Visuals', 'on');'机械臂运动仿真');
view(3);
axis equal;
grid on;
% 逐帧更新
for i = 1:numSteps
    % 更新机器人构型
    show(robot, jointTraj(i, :), 'Frames', 'on', 'Visuals', 'on');
    % 显示目标位姿
    hold on;
    plot3(targetPose(1,4), targetPose(2,4), targetPose(3,4), 'r*');
    hold off;
    drawnow;
    pause(0.05);
end
disp('机械臂运动仿真完成！');

如何运行

1. 同样需要Robotics System Toolbox。
2. 运行代码,首先会看到机械臂的静态模型。
3. 一个新的窗口会弹出,显示机械臂从初始位置平滑地运动到红色星号标记的目标位置。

总结与建议

学习建议：

1. 从入门级开始：先自己动手写一遍2D移动机器人的仿真，这能让你深刻理解机器人运动学和算法的细节。
2. 学习进阶级：然后尝试使用Robotics System Toolbox重写入门级的例子，你会发现代码量大大减少，功能却更强大，这是从“自己造轮子”到“使用标准工具库”的过渡。
3. 挑战高级应用：当你对2D仿真感到得心应手后，再开始学习机械臂的仿真。rigidBodyTree是现代机器人仿真的标准，掌握它非常有价值。
4. 探索更多：Robotics System Toolbox还支持ROS (Robot Operating System) 集成、视觉识别、无人机仿真等，你可以根据自己的兴趣进一步探索。

希望这些示例和解释能帮助你顺利开启机器人MATLAB仿真的学习之旅！

标签： 机器人matlab仿真程序设计 机器人运动学matlab仿真程序 机器人路径规划matlab仿真程序