2024年华数杯全国大学生数学建模竞赛A题-考虑动力学和外部干扰的关节角路径优化问题4详细求解过程

建模忠哥大师兄

于 2024-08-04 23:36:01 发布

阅读量959

点赞数 34

文章标签：数学建模

本文链接：https://blog.csdn.net/jianmozhongge/article/details/140914181

版权

4.1 问题描述

在前三个问题的基础上，假设机器臂在实际操作过程中会受到外部干扰力的影响。外部干扰力可能来源于环境的变化、负载的波动等。目标是设计一种鲁棒的关节角路径，使得在外部干扰力作用下，机器臂末端的运动误差最小。

4.2 建立数学模型

为了在有外部干扰的情况下优化机器臂的关节角路径，我们需要考虑以下几方面：

动力学建模：机器臂的动力学方程，包括质量、惯性和关节力矩。
干扰力模型：描述外部干扰力的模型。
鲁棒性优化：在干扰力作用下，优化关节角路径以最小化末端误差。

4.3 目标函数

我们的目标是最小化以下两个方面：

末端误差：表示末端执行器实际位置与目标位置之间的欧氏距离。
关节力矩消耗：表示各关节为了抵抗干扰力所消耗的能量。

目标函数定义为：

其中，Eerror是末端误差，Etorque是关节力矩消耗，Lpath是底座路径长度，w1,w2, w3是权重因子，用于平衡三个目标。 θ表示关节角度向量，τ表示关节力矩向量，p 表示底座移动路径的坐标序列。

4.4 约束条件

4.5 决策变量

决策变量包括：

4.6 动力学建模

机器臂的动力学方程可以表示为：

4.7 干扰力模型

假设干扰力矩为高斯白噪声模型：

4.8 优化算法

可以使用鲁棒优化方法，例如基于随机扰动的遗传算法，来求解这个优化问题。

具体步骤

定义动力学方程：计算关节力矩在不同角度、速度和加速度下的值。
定义干扰力模型：使用高斯白噪声模型模拟干扰力矩。
定义联合目标函数：结合末端误差、关节力矩消耗和底座路径长度。
优化算法：使用基于随机扰动的遗传算法优化关节角度路径、力矩和底座路径。

设定约束条件：关节角度范围、末端误差允许范围、关节力矩限制和底座路径长度。

优化算法

动力学方程计算：

1. 根据当前关节角度和速度，计算动力学方程中的质量矩阵、科里奥利力矩和重力矩。

干扰力模型：

1. 使用高斯白噪声模拟干扰力矩。

目标函数评估：

1. 在每次迭代中，计算在给定关节角度、力矩和底座路径下的末端误差、力矩消耗和路径长度。

遗传算法优化：

1. 使用遗传算法优化关节角度路径、力矩和底座路径，使目标函数最小化。

通过上述步骤，可以设计出在干扰力作用下具有鲁棒性的最优关节角路径和底座路径。

完整代码：

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.font_manager import FontProperties
from deap import base, creator, tools, algorithms
from heapq import heappop, heappush
import pandas as pd

# 设置中文字体
font = FontProperties(fname='C:\\Windows\\Fonts\\simsun.ttc', size=14)

# 读取附件1的数据
file_path = r"F:\2024年第五届“华数杯”全国大学生数学建模竞赛赛题\2024年第五届“华数杯”全国大学生数学建模竞赛赛题\A题\附件.xlsx"
data = pd.read_excel(file_path, sheet_name='Sheet2', header=None)
# 定义初始位置和目标位置
start = None
targets = []

# 提取障碍物和目标点的位置
obstacles = []

for i in range(data.shape[0]):
    for j in range(data.shape[1]):
        cell_value = data.iloc[i, j]
        if cell_value == 'Start':
            start = (i, j)
        elif 'target' in str(cell_value):
            targets.append((i, j))
        elif cell_value == 1:
            obstacles.append((i, j))

print("Start position:", start)
print("Target positions:", targets)
print("Obstacles:", obstacles)

# 机器臂的 Denavit-Hartenberg 参数
dh_params = [
    [0, 0, 600, 0],
    [300, -90, 0, -90],
    [1200, 0, 0, 0],
    [300, -90, 1200, 180],
    [0, -90, 0, -90],
    [0, -90, 0, 0]
]

# 关节的转动惯量和平均角速度
inertia = [0.5, 0.3, 0.4, 0.6, 0.2, 0.4]
avg_angular_velocity = [2.0, 1.5, 1.0, 2.5, 3.0, 2.0]

# 定义关节角度范围
theta_bounds = [(-np.pi, np.pi) for _ in range(6)]


def dh_transform(a, alpha, d, theta):
    """根据D-H参数计算变换矩阵"""
    alpha = np.radians(alpha)
    return np.array([
        [np.cos(theta), -np.sin(theta) * np.cos(alpha), np.sin(theta) * np.sin(alpha), a * np.cos(theta)],
        [np.sin(theta), np.cos(theta) * np.cos(alpha), -np.cos(theta) * np.sin(alpha), a * np.sin(theta)],
        [0, np.sin(alpha), np.cos(alpha), d],
        [0, 0, 0, 1]
    ])


def forward_kinematics(dh_params, thetas):
    """计算前向运动学，返回末端执行器的位置"""
    T = np.eye(4)
    for i, param in enumerate(dh_params):
        T = T @ dh_transform(param[0], param[1], param[2], thetas[i])
    return T[:3, 3]


def energy_consumption(thetas):
    """计算能耗"""
    energy = 0.0
    for i in range(len(thetas)):
        energy += 0.5 * inertia[i] * (avg_angular_velocity[i] ** 2)
    return energy


def path_length(path):
    """计算路径长度"""
    length = 0
    for i in range(1, len(path)):
        length += np.linalg.norm(np.array(path[i]) - np.array(path[i - 1]))
    return length


def objective_function(thetas, path, F_ext, target_pos):
    """联合目标函数"""
    end_effector_pos = forward_kinematics(dh_params, thetas)
    error = np.linalg.norm(target_pos - end_effector_pos)
    energy = energy_consumption(thetas)
    length = path_length(path)

    # 干扰力模型
    torque = np.array([np.random.normal(0, 1) for _ in range(6)]) + F_ext

    # 计算总能耗
    total_energy = energy + np.sum(torque ** 2)

    # 权重因子
    w1, w2, w3 = 1.0, 1.0, 1.0

    return w1 * error + w2 * total_energy + w3 * length


def is_valid_move(x, y):
    """检查移动是否有效（未碰撞障碍物）"""
    return (x, y) not in obstacles


def a_star_search(start, goal):
    """A*算法搜索路径"""
    open_set = []
    heappush(open_set, (0, start))
    came_from = {}
    g_score = {start: 0}
    f_score = {start: np.linalg.norm(np.array(start) - np.array(goal))}

    while open_set:
        _, current = heappop(open_set)

        if current == goal:
            path = []
            while current in came_from:
                path.append(current)
                current = came_from[current]
            path.append(start)
            path.reverse()
            return path

        for dx, dy in [(-1, 0), (1, 0), (0, -1), (0, 1)]:
            neighbor = (current[0] + dx, current[1] + dy)
            if not is_valid_move(*neighbor):
                continue

            tentative_g_score = g_score[current] + 1
            if neighbor not in g_score or tentative_g_score < g_score[neighbor]:
                came_from[neighbor] = current
                g_score[neighbor] = tentative_g_score
                f_score[neighbor] = g_score[neighbor] + np.linalg.norm(np.array(neighbor) - np.array(goal))
                heappush(open_set, (f_score[neighbor], neighbor))

    return []


# 生成初始个体时考虑关节角度范围
def create_individual():
    return [np.random.uniform(low, high) for low, high in theta_bounds]


# 将个体归一化到[-pi, pi]范围内
def normalize_individual(individual):
    return [((theta + np.pi) % (2 * np.pi)) - np.pi for theta in individual]


# 遍历每个目标点，进行路径规划和优化
for target in targets:
    # 计算底座移动路径
    path = a_star_search(start, target)

    # 使用遗传算法进行关节角度优化
    creator.create("FitnessMin", base.Fitness, weights=(-1.0,))
    creator.create("Individual", list, fitness=creator.FitnessMin)

    toolbox = base.Toolbox()
    toolbox.register("attr_float", create_individual)
    toolbox.register("individual", tools.initIterate, creator.Individual, toolbox.attr_float)
    toolbox.register("population", tools.initRepeat, list, toolbox.individual)


    def evaluate(individual):
        # 模拟外部干扰力，标准差设置为较小值
        F_ext = np.array([np.random.normal(0, 5) for _ in range(6)])
        target_pos = np.array([target[0] * 100, target[1] * 100, 200])
        individual = normalize_individual(individual)
        return objective_function(individual, path, F_ext, target_pos),


    toolbox.register("evaluate", evaluate)
    toolbox.register("mate", tools.cxBlend, alpha=0.5)
    toolbox.register("mutate", tools.mutGaussian, mu=0, sigma=10, indpb=0.2)
    toolbox.register("select", tools.selTournament, tournsize=3)

    toolbox.register("map", map)

    population = toolbox.population(n=300)
    ngen = 100
    cxpb = 0.7
    mutpb = 0.2

    stats = tools.Statistics(lambda ind: ind.fitness.values)
    stats.register("min", np.min)
    stats.register("avg", np.mean)

    result, log = algorithms.eaSimple(population, toolbox, cxpb, mutpb, ngen, stats=stats, verbose=False)

    # 找到最优个体
    best_individual = tools.selBest(population, 1)[0]
    optimal_thetas = normalize_individual(best_individual)

    print("Target position:", target)
    print("最优关节角度:", optimal_thetas)
    print("最小末端误差和能耗:", evaluate(optimal_thetas)[0])


    # 计算每个关节的坐标
    def calculate_joint_positions(dh_params, thetas):
        points = [np.array([0, 0, 0, 1])]
        T = np.eye(4)
        for i, param in enumerate(dh_params):
            T = T @ dh_transform(param[0], param[1], param[2], thetas[i])
            points.append(T @ np.array([0, 0, 0, 1]))
        return points


    points = calculate_joint_positions(dh_params, optimal_thetas)
    # 提取各个关节的x, y, z坐标
    x_coords = [point[0] for point in points]
    y_coords = [point[1] for point in points]
    z_coords = [point[2] for point in points]

    # 绘制底座移动路径和机械臂简图
    fig, ax = plt.subplots()
    ax.plot([p[0] for p in path], [p[1] for p in path], '-o', label='底座路径')
    ax.set_xlabel('X', fontproperties=font)
    ax.set_ylabel('Y', fontproperties=font)
    ax.set_title('底座移动路径', fontproperties=font)
    ax.legend(prop=font)
    plt.show()

    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')
    ax.plot(x_coords, y_coords, z_coords, '-o', label='机械臂')
    ax.set_xlabel('X (mm)', fontproperties=font)
    ax.set_ylabel('Y (mm)', fontproperties=font)
    ax.set_zlabel('Z (mm)', fontproperties=font)
    ax.set_title('六自由度机械臂优化后的状态简图', fontproperties=font)
    ax.legend(prop=font)

    # 设置显示范围
    ax.set_xlim([-500, 2000])
    ax.set_ylim([-500, 2000])
    ax.set_zlim([0, 2000])

    plt.show()

    # 生成遗传算法收敛示意图
    gen = log.select("gen")
    min_fitness_values = log.select("min")
    avg_fitness_values = log.select("avg")

    fig, ax1 = plt.subplots()
    line1 = ax1.plot(gen, min_fitness_values, "b-", label="最小适应度值")
    ax1.set_xlabel("世代数", fontproperties=font)
    ax1.set_ylabel("最小适应度值", color="b", fontproperties=font)
    for tl in ax1.get_yticklabels():
        tl.set_color("b")

    ax2 = ax1.twinx()
    line2 = ax2.plot(gen, avg_fitness_values, "r-", label="平均适应度值")
    ax2.set_ylabel("平均适应度值", color="r", fontproperties=font)
    for tl in ax2.get_yticklabels():
        tl.set_color("r")

    lines = line1 + line2
    labels = [l.get_label() for l in lines]
    ax1.legend(lines, labels, loc="center right", prop=font)

    plt.title("遗传算法收敛示意图", fontproperties=font)
    plt.show()

输出结果：

Start position: (1, 0)

Target positions: [(0, 0), (1, 14), (8, 10), (16, 8), (18, 1), (20, 19)]

Obstacles: [(1, 1), (1, 2), (1, 3), (1, 5), (1, 19), (2, 7), (2, 8), (2, 9), (2, 10), (2, 16), (2, 18), (3, 3), (3, 4), (3, 5), (3, 15), (3, 18), (3, 19), (4, 1), (4, 3), (4, 6), (4, 15), (4, 16), (5, 0), (5, 4), (5, 7), (5, 15), (5, 16), (5, 17), (5, 19), (6, 0), (6, 2), (6, 3), (6, 5), (6, 7), (6, 8), (6, 11), (6, 12), (6, 16), (6, 17), (6, 18), (6, 19), (7, 5), (7, 19), (8, 2), (8, 4), (8, 6), (8, 18), (9, 3), (9, 9), (9, 11), (9, 13), (9, 15), (9, 16), (9, 17), (9, 18), (9, 19), (10, 1), (10, 3), (10, 9), (10, 11), (10, 18), (11, 1), (11, 2), (11, 10), (11, 11), (11, 14), (11, 17), (12, 3), (12, 4), (12, 6), (12, 10), (12, 12), (12, 13), (13, 1), (13, 5), (13, 9), (13, 13), (13, 16), (13, 17), (13, 19), (14, 10), (14, 11), (14, 17), (14, 18), (14, 19), (15, 1), (15, 5), (15, 6), (15, 13), (15, 17), (16, 5), (16, 11), (16, 12), (16, 19), (17, 6), (17, 9), (17, 13), (17, 15), (18, 5), (18, 6), (19, 1), (19, 2), (19, 4), (19, 5), (19, 6), (19, 8), (19, 12), (19, 16), (19, 17), (20, 3), (20, 4), (20, 8), (20, 12), (20, 13), (20, 16)]

Target position: (0, 0)

最优关节角度: [0.5343354455041922, 0.5485174258666099, 2.1800919460450823, 3.0323610408091213, -1.961273679626597, -2.385504305042666]

最小末端误差和能耗: 1373.437201893616

最优关节角度: [-0.9188638748398432, -0.17365900570842419, 2.5448334646474082, 2.71400375811492, 2.143045561621026, 2.0311795874899268]

最小末端误差和能耗: 262.3705582914117

解释：每个目标位置的最优关节角度和最小末端误差和能耗

对于每个目标位置，程序输出了最优关节角度和最小末端误差和能耗。例如：

Target position: (0, 0)

最优关节角度: [-2.6447988097179085, 2.6964917750280826, 2.2210057369864495, 3.0563796765387625, -0.6751770987927728, -2.8471618569677055]

最小末端误差和能耗: 1438.7997816980785

Target position: 当前处理的目标位置。
最优关节角度: 达到该目标位置的最优关节角度，已归一化到 [−π,π][-π, π][−π,π] 范围内。
最小末端误差和能耗: 该路径的最小末端误差和总能耗。