二次规划（Qp）在机械臂（二维平面）轨迹规划中的应用Matlab仿真实例

摘要

本技术报告介绍了如何使用MATLAB来控制和模拟一个两自由度的机械臂。我们详细讨论了机械臂的建模、控制策略、轨迹规划、数值优化以及可视化等关键方面。通过MATLAB的强大功能，我们能够实现高效的机械臂运动控制和仿真。

引言

机械臂是工业自动化和机器人技术的重要组成部分，具有广泛的应用领域，包括制造业、医疗、航空航天等。本报告旨在介绍如何使用MATLAB来控制和模拟一个两自由度的机械臂。我们将通过建模、控制策略、数值优化和可视化来展示机械臂的运动和轨迹跟踪。

• 机械臂模型

本文选取了一个二连杆的机械臂模型，因为用二连杆机械臂建立动力学和运动学模型都非常简单。本文连杆长度为：L=1.0m,机械臂各关节初始角度均为theta=0.5（弧度制）。其中机械臂末端位置为

利用正向运动学描述为：

其雅可比矩阵为：

• 控制策略

我们给定机械臂初始角度值为：theta=[0.5,0.5] 并通过正向运动学方程计算其末端执行器初始位置，也就是说给定不同的初始关节角度末端执行器会有不同的初始位置。同时设置末端执行器目标位置，通过利用二次规划（Qp）求解其需要的关节角度增量以控制机械臂达到目标位置，通过Qp求解的优点在于不用求解机械臂的逆运动学函数，这在拥有更高自由度的机械臂上应用时具备适用性。下面会详细介绍如何设置Qp求解函数。

• 轨迹生成

利用matlab里面的线性插值函数，对初始位置和目标位置进行N次插值，生成目标轨迹x_target,y_target为N×1的矩阵。

接下来就是让末端执行器跟踪x_target,y_target。将其转化为二规划形式并Matlab里面的Qp求解器进行求解最优控制输入。

• 数值优化与Qp求解

数值优化是一种数学方法，旨在找到一个函数的最优解或最优值，满足一组约束条件。在数值优化中，我们通常需要最小化（或最大化）一个目标函数，同时满足一系列等式约束和不等式约束。这些问题可以在许多领域中找到应用，包括工程、经济学、物理学、机器学习和计算机科学等。

具体来说，数值优化的目标是找到使目标函数达到最小（或最大）值的一组决策变量的取值。这些决策变量可以表示设计参数、控制策略、投资组合或任何需要优化的量。这些算法可以是梯度下降、牛顿法、拟牛顿法、线性规划、二次规划等等。

二次规划的一般形式可以表示为：

其中：

Qp求解器的设计

本文Qp求解器设计如下：

首先定义定义二次型函数：

相应的：

为关节角度，J为雅可比矩阵

我们设置目标位置为x_target,y_target，利用当前机械臂末端执行器的位置x_current,y_current可以求出x,y位置跟踪误差

同时可以利用雅可比矩阵求出末端执行器速度：

我们将上述作为控制目标

关节速度作为约束：

b = [dtheta1_ub; -dtheta1_lb; dtheta2_ub; -dtheta2_lb];

构建Qp求解器：

利用matlab里面的quadprog函数

五、结果展示

机械臂轨迹跟踪效果（初始位置：[1.4,1.3]  目标位置：[0.6,0.4] T=100 dt=0.01）

机械臂初始位置

机械臂运动到目标位置，绿色线段为机械臂移动轨迹

机械臂关节角度随时间变化曲线

末端执行位置随时间变化曲线

   

末端执行器位置与目标位置跟踪误差曲线

Qp求解的最优关节角度变化量曲线

六、结论：

本文，我们成功通过二次规划（Qp）的方法实现了机械臂的轨迹跟踪控制。机械臂能够按照目标轨迹移动，并且在运动过程中实时调整关节角度以减小位置和速度误差。雅可比矩阵在轨迹跟踪和控制过程中发挥了关键作用，将关节空间和笛卡尔空间联系起来，实现了控制目标。

总的来说，这个代码示例提供了一个简单而有效的方法，用于控制和轨迹跟踪机械臂。这对于机械臂控制的初学者和研究人员来说是一个有用的起点，可以进一步扩展和优化以满足更复杂的应用需求。通过不断改进和调整控制算法和参数，可以实现更高精度的轨迹跟踪和控制性能。