问题描述
具有单位长度连杆的二维关节机器人臂，关注其在
X
X
X方向上的末端执行器位置。
通过给定的前向运动学公式：
x
(
t
)
=
cos
(
q
1
(
t
)
)
+
cos
(
q
2
(
t
)
)
=
h
(
q
(
t
)
)
x(t) = \cos(q_1(t)) + \cos(q_2(t)) = h(\mathbf{q}(t))
x(t)=cos(q1(t))+cos(q2(t))=h(q(t))
其中 x ( t ) x(t) x(t)表示末端执行器的位置，而 q 1 ( t ) q_1(t) q1(t)和 q 2 ( t ) q_2(t) q2(t)是机器人臂的两个关节角度。
任务是找到一条轨迹，包含 N + 1 N + 1 N+1个路径点 { ( q 1 ( 0 ) , q 2 ( 0 ) ) , ( q 1 ( 1 ) , q 2 ( 1 ) ) , … , ( q 1 ( N ) , q 2 ( N ) ) } \{(q_1(0), q_2(0)), (q_1(1), q_2(1)), \dots, (q_1(N), q_2(N))\} {(q1(0),q2(0)),(q1(1),q2(1)),…,(q1(N),q2(N))}，以及相应的时间索引 { 0 , t ( 1 ) , … , t ( N ) } \{0, t(1), \dots, t(N)\} {0,t(1),…,t(N)}，使得机器人从初始位置 q ( 0 ) = ( q 1 ( 0 ) , q 2 ( 0 ) ) \mathbf{q}(0) = (q_1(0), q_2(0)) q(0)=(q1(0),q2(0)) 移动到目标位置 x ∗ x^* x∗。路径需要满足每秒关节最大增量 δ q max \delta q_{\text{max}} δqmax 的限制。
题目要求：
- 最小化达到 x ∗ x^* x∗所需的时间。
- 在笛卡尔空间中沿最短路径。
- 在关节空间中沿最短路径。
其中 δ t \delta t δt 是固定的时间步长，每个时间步间隔为 t ( n ) − t ( n − 1 ) = t ( N ) / N t(n) - t(n-1) = t(N)/N t(n)−t(n−1)=t(N)/N，对于所有 n ∈ [ 1 , … , N ] n \in [1, \dots, N] n∈[1,…,N]。
解题思路
问题描述回顾
- 机器人结构：二维平面内，两根单位长度连杆，通过关节角度 q 1 ( t ) q_1(t) q1(t) 和 q 2 ( t ) q_2(t) q2(t) 控制末端执行器的位置。
- 前向运动学：
x ( t ) = cos ( q 1 ( t ) ) + cos ( q 2 ( t ) ) = h ( q ( t ) ) x(t) = \cos(q_1(t)) + \cos(q_2(t)) = h(\mathbf{q}(t)) x(t)=cos(q1(t))+cos(q2(t))=h(q(t)) - 约束条件：
- 关节增量限制：
∣ q i ( n ) − q i ( n − 1 ) ∣ ≤ δ q max δ t ∀ i ∈ { 1 , 2 } , ∀ n ∈ { 1 , 2 , … , N } |q_i(n) - q_i(n-1)| \leq \delta q_{\text{max}} \delta t \quad \forall i \in \{1, 2\}, \forall n \in \{1, 2, \dots, N\} ∣qi(n)−qi(n−1)∣≤δqmaxδt∀i∈{1,2},∀n∈{1,2,…,N} - 初始关节角度固定：
q ( 0 ) = ( q 1 ( 0 ) , q 2 ( 0 ) ) = 初始配置 \mathbf{q}(0) = (q_1(0), q_2(0)) = \text{初始配置} q(0)=(q1(0),q2(0))=初始配置 - 末端执行器达到目标位置：
x ( N ) = x ∗ x(N) = x^* x(N)=x∗
- 关节增量限制：
- 轨迹表示：包含
N
+
1
N + 1
N+1 个路径点
{
(
q
1
(
0
)
,
q
2
(
0
)
)
,
(
q
1
(
1
)
,
q
2
(
1
)
)
,
…
,
(
q
1
(
N
)
,
q
2
(
N
)
)
}
\{(q_1(0), q_2(0)), (q_1(1), q_2(1)), \dots, (q_1(N), q_2(N))\}
{(q1(0),q2(0)),(q1(1),q2(1)),…,(q1(N),q2(N))} 和相应的时间索引
{
0
,
t
(
1
)
,
…
,
t
(
N
)
}
\{0, t(1), \dots, t(N)\}
{0,t(1),…,t(N)}，其中时间步长固定为：
δ t = t ( N ) N , t ( n ) − t ( n − 1 ) = δ t ∀ n ∈ { 1 , 2 , … , N } \delta t = \frac{t(N)}{N}, \quad t(n) - t(n-1) = \delta t \quad \forall n \in \{1, 2, \dots, N\} δt=Nt(N),t(n)−t(n−1)=δt∀n∈{1,2,…,N}
优化目标与对应方法
问题 1：最小化到达 x ∗ x^* x∗ 的时间
目标函数
最小化达到目标位置 x ∗ x^* x∗所需的最终时间 T f T_f Tf。
问题 2：最小化笛卡尔空间路径长度
目标函数
最小化从初始位置到目标位置的笛卡尔路径总长度：
∑
n
=
1
N
∣
(
cos
(
q
1
(
n
)
)
+
cos
(
q
2
(
n
)
)
)
−
(
cos
(
q
1
(
n
−
1
)
)
+
cos
(
q
2
(
n
−
1
)
)
)
∣
\sum_{n=1}^{N} \left| \left( \cos(q_1(n)) + \cos(q_2(n)) \right) - \left( \cos(q_1(n-1)) + \cos(q_2(n-1)) \right) \right|
n=1∑N∣(cos(q1(n))+cos(q2(n)))−(cos(q1(n−1))+cos(q2(n−1)))∣
问题 3：最小化关节空间路径长度
目标函数
最小化总关节空间路径长度：
∑
n
=
1
N
(
q
1
(
n
)
−
q
1
(
n
−
1
)
)
2
+
(
q
2
(
n
)
−
q
2
(
n
−
1
)
)
2
\sum_{n=1}^{N} \sqrt{(q_1(n) - q_1(n-1))^2 + (q_2(n) - q_2(n-1))^2}
n=1∑N(q1(n)−q1(n−1))2+(q2(n)−q2(n−1))2
总结
通过上述方法，可以系统地优化二维两关节机器人臂的运动轨迹，确保在满足物理约束的同时，实现时间最短、笛卡尔空间路径最短或关节空间路径最短的目标。具体实现时，可根据需求选择合适的优化策略和数值方法，以求得最优解。
下面代码不可用，只能找到局部极小值
% 优化二维两关节机器人臂的运动轨迹
% 目标：最小化时间、笛卡尔空间最短路径、关节空间最短路径
clear; clc; close all;
%% 参数定义
% 连杆长度
L1 = 1;
L2 = 1;
% 初始关节角度（弧度）
q1_initial = pi/3;
q2_initial = pi/2;
% 目标末端执行器位置
x_star = 2; % 可以根据需要调整
% 最大关节增量（每步）
delta_q_max = pi/30;
% 时间步长
delta_t = 0.1; % 秒
% 最大时间步数
N_max = 100;
% 优化目标选择
% 1 - 最小时间
% 2 - 笛卡尔空间最短路径
% 3 - 关节空间最短路径
optimization_goal = 2; % 用户可以修改此值以选择不同的目标
%% 优化过程
switch optimization_goal
case 1
% 最小化达到 x* 所需的时间
[q_opt, N_opt, success] = optimize_min_time(q1_initial, q2_initial, x_star, ...
delta_q_max, N_max, L1, L2);
if success
fprintf('最小时间优化成功，所需步数 N = %d\n', N_opt);
else
fprintf('最小时间优化失败。\n');
end
plot_results(q_opt, N_opt, L1, L2, '最小时间优化');
case 2
% 在笛卡尔空间中沿最短路径
N = 50; % 预设步数
[q_opt, success] = optimize_cartesian_shortest_path(q1_initial, q2_initial, x_star, ...
delta_q_max, N, L1, L2);
if success
fprintf('笛卡尔空间最短路径优化成功。\n');
else
fprintf('笛卡尔空间最短路径优化失败。\n');
end
plot_results(q_opt, N, L1, L2, '笛卡尔空间最短路径优化');
case 3
% 在关节空间中沿最短路径
N = 50; % 预设步数
[q_opt, success] = optimize_joint_shortest_path(q1_initial, q2_initial, x_star, ...
delta_q_max, N, L1, L2);
if success
fprintf('关节空间最短路径优化成功。\n');
else
fprintf('关节空间最短路径优化失败。\n');
end
plot_results(q_opt, N, L1, L2, '关节空间最短路径优化');
otherwise
error('无效的优化目标选择。');
end
%% 函数定义
function [q_opt, N_opt, success] = optimize_min_time(q1_initial, q2_initial, x_star, ...
delta_q_max, N_max, L1, L2)
% 最小化达到 x* 所需的时间
% 使用递增搜索，逐步增加 N 直到找到可行解
success = false;
q_opt = [];
N_opt = 0;
for N = 1:N_max
% 优化变量: [q1(1), q2(1), q1(2), q2(2), ..., q1(N), q2(N)]
% 初始猜测：保持关节角度不变
q1_guess = repmat(q1_initial, 1, N);
q2_guess = repmat(q2_initial, 1, N);
q0 = [q1_guess; q2_guess]';
q0 = q0(:);
% 设置优化选项
options = optimoptions('fmincon','Display','none','Algorithm','sqp');
% 定义目标函数为0，因为我们在搜索可行解
objective = @(q) 0;
% 非线性约束
nonlcon = @(q) min_time_constraints(q, q1_initial, q2_initial, x_star, ...
delta_q_max, N, L1, L2);
% 运行优化
[q_sol, ~, exitflag] = fmincon(objective, q0, [], [], [], [], [], [], nonlcon, options);
if exitflag > 0
% 重新构建完整的关节角度轨迹，包括初始配置
q_steps = reshape(q_sol, 2, N)';
q_opt = [[q1_initial, q2_initial]; q_steps];
N_opt = N;
success = true;
break;
end
end
end
function [q_opt, success] = optimize_cartesian_shortest_path(q1_initial, q2_initial, x_star, ...
delta_q_max, N, L1, L2)
% 在笛卡尔空间中沿最短路径
% 优化关节角度以使末端执行器沿直线移动
success = false;
% 优化变量: [q1(1), q2(1), q1(2), q2(2), ..., q1(N), q2(N)]
% 初始猜测：保持关节角度不变
q1_guess = repmat(q1_initial, 1, N);
q2_guess = repmat(q2_initial, 1, N);
q0 = [q1_guess; q2_guess]';
q0 = q0(:);
% 设置优化选项
options = optimoptions('fmincon','Display','iter','Algorithm','sqp');
% 定义目标函数：最小化关节角度的总变化
objective = @(q) sum((q(1:2:end) - q1_initial).^2 + (q(2:2:end) - q2_initial).^2);
% 非线性约束
nonlcon = @(q) cartesian_shortest_constraints(q, q1_initial, q2_initial, x_star, ...
delta_q_max, N, L1, L2);
% 运行优化
[q_sol, ~, exitflag] = fmincon(objective, q0, [], [], [], [], [], [], nonlcon, options);
if exitflag > 0
% 重新构建完整的关节角度轨迹，包括初始配置
q_steps = reshape(q_sol, 2, N)';
q_opt = [[q1_initial, q2_initial]; q_steps];
success = true;
end
end
function [q_opt, success] = optimize_joint_shortest_path(q1_initial, q2_initial, x_star, ...
delta_q_max, N, L1, L2)
% 在关节空间中沿最短路径
% 最小化关节角度变化总和
success = false;
% 优化变量: [q1(1), q2(1), q1(2), q2(2), ..., q1(N), q2(N)]
% 初始猜测：保持关节角度不变
q1_guess = repmat(q1_initial, 1, N);
q2_guess = repmat(q2_initial, 1, N);
q0 = [q1_guess; q2_guess]';
q0 = q0(:);
% 设置优化选项
options = optimoptions('fmincon','Display','iter','Algorithm','sqp');
% 定义目标函数：最小化关节角度的总变化
objective = @(q) sum((q(1:2:end) - q1_initial).^2 + (q(2:2:end) - q2_initial).^2);
% 非线性约束
nonlcon = @(q) joint_shortest_constraints(q, q1_initial, q2_initial, x_star, ...
delta_q_max, N, L1, L2);
% 运行优化
[q_sol, ~, exitflag] = fmincon(objective, q0, [], [], [], [], [], [], nonlcon, options);
if exitflag > 0
% 重新构建完整的关节角度轨迹，包括初始配置
q_steps = reshape(q_sol, 2, N)';
q_opt = [[q1_initial, q2_initial]; q_steps];
success = true;
end
end
function [c, ceq] = min_time_constraints(q, q1_initial, q2_initial, x_star, ...
delta_q_max, N, L1, L2)
% 非线性约束函数，用于最小时间优化
% c: 不等式约束
% ceq: 等式约束
ceq = [];
c = [];
% Reshape q
q = reshape(q, 2, N)';
% 前一步关节角度
q_prev = [q1_initial, q2_initial];
for i = 1:N
% 当前步关节角度
q_curr = q(i, :);
% 关节增量限制
dq1 = q_curr(1) - q_prev(1);
dq2 = q_curr(2) - q_prev(2);
c = [c; abs(dq1) - delta_q_max; abs(dq2) - delta_q_max];
% 更新前一步关节角度
q_prev = q_curr;
end
% 最终末端执行器位置
x_final = L1 * cos(q(end,1)) + L2 * cos(q(end,2));
ceq = [ceq; x_final - x_star];
end
function [c, ceq] = cartesian_shortest_constraints(q, q1_initial, q2_initial, x_star, ...
delta_q_max, N, L1, L2)
% 非线性约束函数，用于笛卡尔空间最短路径优化
% c: 不等式约束
% ceq: 等式约束
ceq = [];
c = [];
% Reshape q
q = reshape(q, 2, N)';
% 初始末端执行器位置
x0 = L1 * cos(q1_initial) + L2 * cos(q2_initial);
for i = 1:N
% 当前步关节角度
q_curr = q(i, :);
% 前一步关节角度
if i == 1
q_prev = [q1_initial, q2_initial];
else
q_prev = q(i-1, :);
end
% 关节增量限制
dq1 = q_curr(1) - q_prev(1);
dq2 = q_curr(2) - q_prev(2);
c = [c; abs(dq1) - delta_q_max; abs(dq2) - delta_q_max];
% 末端执行器应沿直线移动
xi = L1 * cos(q_curr(1)) + L2 * cos(q_curr(2));
desired_xi = x0 + (i / N) * (x_star - x0);
ceq = [ceq; xi - desired_xi];
end
end
function [c, ceq] = joint_shortest_constraints(q, q1_initial, q2_initial, x_star, ...
delta_q_max, N, L1, L2)
% 非线性约束函数，用于关节空间最短路径优化
% c: 不等式约束
% ceq: 等式约束
ceq = [];
c = [];
% Reshape q
q = reshape(q, 2, N)';
% 前一步关节角度
q_prev = [q1_initial, q2_initial];
for i = 1:N
% 当前步关节角度
q_curr = q(i, :);
% 关节增量限制
dq1 = q_curr(1) - q_prev(1);
dq2 = q_curr(2) - q_prev(2);
c = [c; abs(dq1) - delta_q_max; abs(dq2) - delta_q_max];
% 更新前一步关节角度
q_prev = q_curr;
end
% 最终末端执行器位置
x_final = L1 * cos(q(end,1)) + L2 * cos(q(end,2));
ceq = [ceq; x_final - x_star];
end
function plot_results(q_opt, N, L1, L2, title_str)
% 绘制优化结果
time = (0:N) * delta_t;
q1 = q_opt(:,1);
q2 = q_opt(:,2);
% 绘制关节角度随时间变化
figure;
subplot(2,1,1);
plot(time, q1, '-o', time, q2, '-x');
xlabel('时间 (s)');
ylabel('关节角度 (rad)');
legend('q1', 'q2');
title([title_str, ' - 关节角度轨迹']);
grid on;
% 计算末端执行器位置
x = L1 * cos(q1) + L2 * cos(q2);
y = L1 * sin(q1) + L2 * sin(q2);
% 绘制末端执行器轨迹
subplot(2,1,2);
plot(x, y, '-o');
hold on;
plot(x(1), y(1), 'go', 'MarkerSize',10,'MarkerFaceColor','g'); % 起点
plot(x(end), y(end), 'ro', 'MarkerSize',10,'MarkerFaceColor','r'); % 终点
xlabel('X 位置');
ylabel('Y 位置');
legend('末端执行器轨迹','起点','终点');
title([title_str, ' - 末端执行器轨迹']);
grid on;
axis equal;
end
1.2 补充练习：（可选）
将上述优化推广到一个具有 K K K个关节的机器人，末端执行器在三维笛卡尔空间中移动，给定正向运动学 h ( ⋅ ) : R K → R 3 h(\cdot) : \mathbb{R}^K \rightarrow \mathbb{R}^3 h(⋅):RK→R3，其中 x = h ( q ) \mathbf{x} = h(\mathbf{q}) x=h(q)。
解释
这个补充练习要求将之前的优化问题（针对 2 个关节的 2D 机械臂）推广到更通用的情况，即一个具有 K K K个关节的机器人，其末端执行器在 3D 笛卡尔空间中移动。
- K K K个关节：意味着该机器人可以有多个自由度（例如 6 自由度机器人），每个关节用一个关节变量 q i q_i qi表示，共 K K K 个关节变量。
- 三维笛卡尔空间：此时机器人的末端执行器可以在 x x x、 y y y、 z z z 方向上移动。
- 正向运动学 h ( q ) h(\mathbf{q}) h(q)：函数 h ( q ) h(\mathbf{q}) h(q)将关节空间中的 K K K个关节变量 q \mathbf{q} q转换为三维空间中的位置 x \mathbf{x} x，即末端执行器的位置。
实现时，主要需要修改正向运动学公式
h
(
q
)
h(\mathbf{q})
h(q)，并在约束条件和目标函数中使用 3D 空间中的目标位置。
对于UR5机械臂的三个不同优化问题，我们可以定义以下目标函数和约束条件：
问题描述
UR5机械臂的末端执行器需要从初始位置移动到目标位置，满足关节速度和角度范围的限制。根据不同的优化目标，以下是三个具体的优化问题：
- 最小化到达目标位置的时间。
- 最小化笛卡尔空间的路径长度。
- 最小化关节空间的路径长度。
1. 最小化时间
目标：找到一个路径，使UR5机械臂从初始位置到达目标位置的时间最短。
目标函数
最小化最终时间
T
f
T_f
Tf：
J
=
T
f
J = T_f
J=Tf
约束条件
-
位置约束：末端执行器需要到达目标位置 x ∗ \mathbf{x}^* x∗：
h ( q ( N ) ) = x ∗ h(\mathbf{q}(N)) = \mathbf{x}^* h(q(N))=x∗ -
关节角速度限制：每个关节的角速度在每个时间步上受到最大增量 δ q max \delta q_{\text{max}} δqmax的限制：
∣ q i ( n ) − q i ( n − 1 ) ∣ ≤ δ q max ⋅ δ t , ∀ i = 1 , … , 6 |q_i(n) - q_i(n-1)| \leq \delta q_{\text{max}} \cdot \delta t, \quad \forall i = 1, \dots, 6 ∣qi(n)−qi(n−1)∣≤δqmax⋅δt,∀i=1,…,6 -
关节角度范围限制：每个关节的角度有物理限制：
q min , i ≤ q i ( n ) ≤ q max , i , ∀ i = 1 , … , 6 q_{\text{min},i} \leq q_i(n) \leq q_{\text{max},i}, \quad \forall i = 1, \dots, 6 qmin,i≤qi(n)≤qmax,i,∀i=1,…,6
2. 最小化笛卡尔空间的路径长度
目标：找到一条路径，使末端执行器在三维笛卡尔空间中行驶的距离最短。
目标函数
在笛卡尔空间最小化路径长度：
J = ∑ n = 1 N ∥ h ( q ( n ) ) − h ( q ( n − 1 ) ) ∥ 2 J = \sum_{n=1}^{N} \| h(\mathbf{q}(n)) - h(\mathbf{q}(n-1)) \|^2 J=n=1∑N∥h(q(n))−h(q(n−1))∥2
约束条件
与第一个问题的约束条件相同。
3. 最小化关节空间的路径长度
目标：找到一条路径，使在关节空间中行驶的距离最短。
目标函数
在关节空间最小化路径长度：
J = ∑ n = 1 N ∑ i = 1 6 ∣ q i ( n ) − q i ( n − 1 ) ∣ 2 J = \sum_{n=1}^{N} \sum_{i=1}^{6} |q_i(n) - q_i(n-1)|^2 J=n=1∑Ni=1∑6∣qi(n)−qi(n−1)∣2
约束条件
与第一个问题的约束条件相同。
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
