基于Python开发的6自由度机械臂控制模型及代码

是 否 +2 论坛币

k人 参与回答

经管之家送您一份

应届毕业生专属福利!

求职就业群

赵安豆老师微信：zhaoandou666

经管之家联合CDA

送您一个全额奖学金名额~ !

立即领取

感谢您参与论坛问题回答

经管之家送您两个论坛币！

+2 论坛币

六自由度机械臂运动学建模与实现

本文介绍一个基于Python语言开发的6自由度机械臂控制模型，涵盖正运动学与逆运动学的核心算法实现。系统采用Denavit-Hartenberg（DH）参数法进行建模，适用于常见的舵机驱动型机械臂结构。

[此处为图片1]

1. DH参数定义与类初始化

在构建机械臂模型时，首先需要根据实际结构设定各关节的DH参数，包括连杆长度、扭转角、偏移距离以及初始角度偏移。以下为具体实现代码：

import math
import numpy as np
from scipy.spatial.transform import Rotation as R

class SixDOFRobotArm:
    def __init__(self):
        # DH参数表 (单位: mm, 度)
        # 每项格式为：[a, alpha, d, theta_offset]
        self.dh_params = [
            [0, 0, 89.2, 0],      # 关节1 - 基座旋转 (MG996)
            [0, -90, 0, -90],     # 关节2 - 肩部 (MG996)
            [135, 0, 0, 0],       # 关节3 - 肘部 (MG996)
            [0, -90, 147, 0],     # 关节4 - 腕部俯仰 (MG90S)
            [0, 90, 0, 0],        # 关节5 - 腕部旋转 (MG90S)
            [0, 0, 65, 0]         # 关节6 - 末端执行器 (MG90S)
        ]

        # 各关节舵机角度限制范围 (单位: 度)
        self.servo_limits = [
            (0, 180),  # 第1轴
            (0, 180),  # 第2轴
            (0, 180),  # 第3轴
            (0, 180),  # 第4轴
            (0, 180),  # 第5轴
            (0, 180)   # 第6轴
        ]

2. DH变换矩阵计算函数

DH变换是串联机器人运动学分析的基础工具。通过将每个关节的局部坐标系依次转换至基坐标系，最终获得末端执行器相对于基座的位姿关系。

    def dh_matrix(self, a, alpha, d, theta):
        """生成单个关节的DH变换矩阵"""
        alpha_rad = math.radians(alpha)
        theta_rad = math.radians(theta)
        return np.array([
            [math.cos(theta_rad), -math.sin(theta_rad)*math.cos(alpha_rad),
             math.sin(theta_rad)*math.sin(alpha_rad), a*math.cos(theta_rad)],
            [math.sin(theta_rad), math.cos(theta_rad)*math.cos(alpha_rad),
             -math.cos(theta_rad)*math.sin(alpha_rad), a*math.sin(theta_rad)],
            [0, math.sin(alpha_rad), math.cos(alpha_rad), d],
            [0, 0, 0, 1]
        ])

3. 正运动学求解方法

正运动学用于根据给定的六个关节角度，计算末端执行器的空间位置和姿态。该过程通过对各DH矩阵连续相乘实现。

    def forward_kinematics(self, joint_angles):
        """输入关节角度列表，输出末端位置和欧拉角"""
        T = np.eye(4)  # 初始化为单位矩阵
        for i in range(6):
            a, alpha, d, theta_offset = self.dh_params[i]
            theta = joint_angles[i] + theta_offset
            T_i = self.dh_matrix(a, alpha, d, theta)
            T = T @ T_i  # 矩阵累乘

        # 提取位置向量
        position = T[:3, 3]

        # 将旋转矩阵转换为XYZ顺序的欧拉角（单位：度）
        rotation = R.from_matrix(T[:3, :3])
        euler_angles = rotation.as_euler('xyz', degrees=True)

        return position, euler_angles

4. 逆运动学近似解法

逆运动学问题更为复杂，目标是根据期望的末端位姿反推所需关节角度。此处提供一种简化的几何解析方法作为示例，适用于特定构型的机械臂。

    def inverse_kinematics(self, target_position, target_euler=None):
        """根据目标位置和姿态求解关节角度（简化版）"""
        if target_euler is None:
            target_euler = [0, 0, 0]
        x, y, z = target_position
        roll, pitch, yaw = target_euler

        # 第一关节：基座水平旋转角
        theta1 = math.degrees(math.atan2(y, x))

        # 计算XY平面投影距离
        r = math.sqrt(x**2 + y**2)
        D = math.sqrt(r**2 + (z - self.dh_params[0][2])**2)

        # 获取第二、三关节的连杆长度
        a2, a3 = self.dh_params[1][0], self.dh_params[2][0]

        # 使用余弦定理求解第三关节角
        cos_theta3 = (a2**2 + a3**2 - D**2) / (2 * a2 * a3)

上述代码实现了六自由度机械臂的基本运动学框架。实际工程应用中，逆运动学通常需结合数值迭代或完整解析解以提高精度与稳定性。此外，还可加入轨迹规划、碰撞检测等高级功能扩展系统能力。

# 限制theta3的余弦值在有效范围内，避免反余弦计算出错
cos_theta3 = np.clip(cos_theta3, -1, 1)
# 将余弦值转换为角度（单位：度）
theta3 = math.degrees(math.acos(cos_theta3))

# 计算alpha角：使用z轴偏移与r构成的直角三角形的反正切
alpha = math.atan2(z - self.dh_params[0][2], r)

# 计算beta角：根据连杆长度a2、a3及theta3的角度关系求解
beta = math.atan2(
    a3 * math.sin(math.radians(theta3)),
    a2 + a3 * math.cos(math.radians(theta3))
)

# 第二个关节角度由alpha与beta之差得到，并转为度数
theta2 = math.degrees(alpha - beta)

# 假设腕部运动简化处理：
# theta4设为负的(theta2 + theta3)，用于维持末端执行器水平姿态
theta4 = - (theta2 + theta3)  # 确保末端保持水平

# theta5直接取俯仰角（pitch）
theta5 = pitch

# theta6取偏航角（yaw）
theta6 = yaw

# 汇总所有六个关节的目标角度
joint_angles = [theta1, theta2, theta3, theta4, theta5, theta6]

# 对计算出的各关节角度进行限幅，确保不超出舵机物理范围
return self.limit_joint_angles(joint_angles)


def limit_joint_angles(self, joint_angles):
    """对输入的关节角度进行限制，使其处于舵机允许的工作区间内"""
    limited_angles = []
    for i, angle in enumerate(joint_angles):
        min_angle, max_angle = self.servo_limits[i]
        # 将角度限制在[min_angle, max_angle]区间内
        limited_angle = max(min_angle, min(max_angle, angle))
        limited_angles.append(limited_angle)
    return limited_angles

2. 舵机控制模块实现

import time
import serial
import struct

class ServoController:
    def __init__(self, port='/dev/ttyUSB0', baudrate=9600):
        self.port = port
        self.baudrate = baudrate
        self.ser = None

        # 定义各机械臂关节对应的舵机通道编号
        self.servo_channels = {
            'base': 0,         # MG996R，对应关节1（底座旋转）
            'shoulder': 1,     # MG996R，对应关节2（肩部）
            'elbow': 2,        # MG996R，对应关节3（肘部）
            'wrist_pitch': 3,  # MG90S，对应关节4（手腕俯仰）
            'wrist_roll': 4,   # MG90S，对应关节5（手腕翻转）
            'gripper': 5       # MG90S，对应关节6（夹爪）
        }

        # 校准参数配置，便于后期微调不同舵机的零点和比例
        self.servo_calibration = {
            0: {'offset': 0, 'scale': 1.0},  # 关节1校准参数
            1: {'offset': 0, 'scale': 1.0},  # 关节2校准参数
            2: {'offset': 0, 'scale': 1.0},  # 关节3校准参数
            3: {'offset': 0, 'scale': 1.0},  # 关节4校准参数
            4: {'offset': 0, 'scale': 1.0},  # 关节5校准参数
            5: {'offset': 0, 'scale': 1.0}   # 关节6校准参数
        }

    def connect(self):
        """建立与舵机控制板的串口连接"""
        try:
            self.ser = serial.Serial(self.port, self.baudrate, timeout=1)
            print(f"Connected to servo controller on {self.port}")
            return True
        except Exception as e:
            print(f"Connection failed: {e}")
            return False

    def disconnect(self):
        """关闭当前的串口连接"""
        if self.ser and self.ser.is_open:
            self.ser.close()

    def angle_to_pulse(self, angle, channel):
        """将目标角度转换为对应舵机的脉冲宽度（微秒）"""
        min_pulse = 500    # 最小脉宽：500μs
        max_pulse = 2500   # 最大脉宽：2500μs

        # 应用该通道的校准参数（偏移和缩放）
        calib = self.servo_calibration[channel]
        calibrated_angle = angle * calib['scale'] + calib['offset']

        # 线性映射角度(0-180°)到脉宽(500-2500μs)
        pulse = min_pulse + (calibrated_angle / 180.0) * (max_pulse - min_pulse)
        return int(pulse)

def angle_to_pulse(self, angle, channel):
    """将角度转换为脉冲宽度"""
    calibrated_angle = max(0, min(180, angle))  # 限制角度在0-180范围内
    pulse = min_pulse + (calibrated_angle / 180.0) * (max_pulse - min_pulse)
    return int(pulse)

def set_servo_angle(self, channel, angle):
    """控制指定通道舵机转动到目标角度"""
    if not self.ser:
        print("未连接至舵机控制器")
        return False

    pulse = self.angle_to_pulse(angle, channel)

    # 组装指令数据包（依据实际控制板通信协议）
    # 示例格式: [0xFF, 0xFF, 通道号, 脉冲高8位, 脉冲低8位]
    command = bytearray()
    command.append(0xFF)           # 起始标志字节1
    command.append(0xFF)           # 起始标志字节2
    command.append(channel)        # 指定舵机通道
    command.append((pulse >> 8) & 0xFF)  # 高位脉宽数据
    command.append(pulse & 0xFF)         # 低位脉宽数据

    try:
        self.ser.write(command)
        return True
    except Exception as e:
        print(f"设置舵机角度失败: {e}")
        return False

def set_all_servos(self, joint_angles):
    """批量设置所有舵机的角度值"""
    for channel, angle in enumerate(joint_angles):
        success = self.set_servo_angle(channel, angle)
        if not success:
            return False
        time.sleep(0.02)  # 添加短暂延时以减少瞬时电流负载
    return True

def move_to_position(self, target_position, target_euler=None, duration=2.0):
    """平滑地移动机械臂至目标位姿"""
    robot_arm = SixDOFRobotArm()

    # 执行逆运动学求解，获取目标关节角
    target_angles = robot_arm.inverse_kinematics(target_position, target_euler)

    # 获取当前各关节角度（需硬件支持反馈或状态记录）
    current_angles = self.get_current_angles()

    # 使用线性插值实现平滑过渡
    steps = int(duration * 50)  # 按50Hz计算总步数
    for i in range(steps):
        t = i / steps
        intermediate_angles = [
            current + t * (target - current)
            for current, target in zip(current_angles, target_angles)
        ]
        self.set_all_servos(intermediate_angles)
        time.sleep(0.02)

def get_current_angles(self):
    """读取当前各舵机的实际角度（依赖反馈机制）"""
    # 若控制板具备角度反馈功能，则应在此实现数据读取
    # 否则可返回估计值或通过外部编码器获取
    return [90, 90, 90, 90, 90, 90]  # 默认返回居中位置作为初始假设

3. 轨迹规划模块

class TrajectoryPlanner:
    def __init__(self):
        self.robot_arm = SixDOFRobotArm()

    def linear_trajectory(self, start_pos, end_pos, steps=50):
        """生成从起点到终点的直线路径点序列"""
        trajectory = []
        for i in range(steps):
            t = i / (steps - 1)  # 归一化插值参数
            current_pos = [
                start + t * (end - start)
                for start, end in zip(start_pos, end_pos)
            ]
            trajectory.append(current_pos)
        return trajectory

    def circular_trajectory(self, center, radius, steps=50):
        """生成绕指定中心的圆周运动轨迹"""
        trajectory = []
        for i in range(steps):
            angle = 2 * math.pi * i / steps

[此处为图片1]

            x = center[0] + radius * math.cos(angle)
            y = center[1] + radius * math.sin(angle)
            z = center[2]  # 假设圆周运动在XY平面内
            trajectory.append([x, y, z])
        return trajectory

def joint_space_trajectory(self, start_angles, end_angles, steps=50):
    ""关节空间轨迹规划""
    trajectory = []
    for i in range(steps):
        t = i / (steps - 1)
        current_angles = [
            start + t * (end - start)
            for start, end in zip(start_angles, end_angles)
        ]
        trajectory.append(current_angles)
    return trajectory

x = center[0] + radius * math.cos(angle)
y = center[1] + radius * math.sin(angle)
z = center[2]
trajectory.append([x, y, z])
return trajectory

[此处为图片1]

class RobotArmController:
    def __init__(self):
        self.servo_ctrl = ServoController()
        self.robot_arm = SixDOFRobotArm()
        self.trajectory_planner = TrajectoryPlanner()
        # 机械臂工作空间限制
        self.workspace_limits = {
            'x': [-300, 300],  # mm
            'y': [-300, 300],  # mm
            'z': [0, 400]       # mm
        }

def initialize(self):
    ""初始化机械臂""
    if not self.servo_ctrl.connect():
        return False
    # 移动到初始位置
    home_position = [200, 0, 200]  # x, y, z
    home_angles = self.robot_arm.inverse_kinematics(home_position)
    self.servo_ctrl.set_all_servos(home_angles)
    time.sleep(2)
    print("Robot arm initialized")
    return True

def move_to_point(self, x, y, z, roll=0, pitch=0, yaw=0):
    ""移动到指定坐标点""
    target_position = [x, y, z]
    target_euler = [roll, pitch, yaw]
    # 检查工作空间限制
    if not self.check_workspace(target_position):
        print("Target position out of workspace")
        return False
    joint_angles = self.robot_arm.inverse_kinematics(target_position, target_euler)
    return self.servo_ctrl.set_all_servos(joint_angles)

def execute_trajectory(self, trajectory):
    ""执行轨迹""
    for point in trajectory:
        if not self.move_to_point(*point):
            print("Trajectory execution failed")
            return False
        time.sleep(0.1)
    return True

def check_workspace(self, position):
    ""检查位置是否在工作空间内""
    x, y, z = position
    return (self.workspace_limits['x'][0] <= x <= self.workspace_limits['x'][1] and
            self.workspace_limits['y'][0] <= y <= self.workspace_limits['y'][1] and
            self.workspace_limits['z'][0] <= z <= self.workspace_limits['z'][1])

def demo(self):
    ""演示程序""
    print("Starting demo...")
    # 移动到几个预设点
    points = [
        [200, 0, 200],    # 点1
        [200, 150, 150],  # 点2
        [0, 200, 200],    # 点3
    ]

# 定义目标点坐标
points = [
    [200, 0, 200],     # 点1
    [200, 150, 150],   # 点2
    [0, 150, 150],     # 点3
    [-200, 150, 150],  # 点4
    [-200, 0, 200]     # 点5
]

# 遍历所有目标点并移动到每个位置
for point in points:
    print(f"Moving to {point}")
    self.move_to_point(*point)
    time.sleep(1)

# 执行完毕后返回初始位置
self.move_to_point(200, 0, 200)
print("Demo completed")

# 主程序入口
if __name__ == "__main__":
    robot = RobotArmController()
    if robot.initialize():
        # 启动演示流程
        robot.demo()
        
        # 可选：手动控制示例（已注释）
        # robot.move_to_point(250, 100, 150)
        
        # 断开舵机控制器连接
        robot.servo_ctrl.disconnect()

依赖安装命令

pip install numpy scipy pyserial

硬件连接说明

1. 舵机分配：
   • MG996R（高扭矩型号）：用于驱动前三个关节——基座、肩部与肘部。
   • MG90S（标准扭矩）：负责腕部转动及末端执行器的控制，对应第4至第6个关节。
2. 控制板接线方式：
   • 将六个舵机依次接入控制板上的0到5号通道。
   • 为确保稳定运行，建议使用外部电源供电，避免因电流不足导致动作异常。
3. 校准操作步骤：
   • 在首次运行或更换舵机后，需进行角度校准。
   • 通过调整代码中的 servo_calibration 参数，使各关节实际角度与理论值一致。

该实现构建了一个完整的机械臂控制系统，涵盖正逆运动学计算、轨迹插值规划以及底层硬件通信功能。用户可根据自身设备的实际结构参数（如连杆长度）和所用舵机特性，修改DH参数表和校准偏移量以适配具体硬件配置。

[此处为图片1]

扫码加我 拉你入群

请注明：姓名-公司-职位

以便审核进群资格，未注明则拒绝

分享0 收藏0 回帖

关键词：python 自由度 intermediate intermediat calibration