[发明专利]一种冗余度机械臂加速度层容噪控制方法

摘要

本发明提供了一种冗余度机械臂加速度层容噪控制方法，包括：根据机械臂加速度层雅可比矩阵等式，通过引入位置误差和速度误差反馈，设计可容噪的新型雅可比矩阵等式；结合所需要优化的性能指标，建立加速度层容噪控制方案，所述的控制方案受约束于新型雅可比矩阵等式、机械臂动力学方程、关节角度极限、关节速度极限、关节加速度极限和关节力矩极限；将上述的控制方案转化为一个标准的二次型优化问题，并通过数值算法求解器对其进行求解；下位机控制器根据求解结果，驱动机械臂使其完成给定的末端任务。本发明的加速度层容噪控制方法，不仅能使机械臂在具有噪声的情况下仍可完成给定的末端任务，而且使机械臂各关节的末态速度接近为零。

主权项

1.一种冗余度机械臂加速度层容噪控制方法，其特征在于，包括如下步骤：根据机械臂加速度层雅可比矩阵等式，通过引入位置误差和速度误差反馈，设计可容噪的雅可比矩阵等式；结合所需要优化的性能指标，建立加速度层容噪控制方案，所述的控制方案受约束于可容噪的雅可比矩阵等式、机械臂动力学方程、关节角度极限、关节速度极限、关节加速度极限和关节力矩极限；将加速度层容噪控制方案转化为一个标准的二次型优化问题，并通过数值算法求解器对其进行求解；下位机控制器根据二次型优化问题的求解结果，驱动机械臂使其完成给定的末端任务；可容噪的加速度层雅可比矩阵等式设计为：其中，设计参数α>0，β>0，且两者满足α²>β；表示机械臂关节速度，表示机械臂关节加速度，J表示机械臂的雅可比矩阵，表示J的时间导数；e(t)表示机械臂末端的位置误差且e(t)＝f(θ)‑r_d，t表示时间，θ表示机械臂关节角度，f表示一个非线性映射函数，r_d表示机械臂末端期望的运动轨迹；ε(t)表示机械臂末端的速度误差且表示r_d的时间导数；另外，表示的时间导数，δ表示积分变量；加速度层容噪控制方案设计为：最小化受约束于θ^‑≤θ≤θ⁺，τ^‑≤τ≤τ⁺；其中，设计参数λ>0，上标^T表示矩阵或向量的转置；等式约束对应于机械臂末端在加速度层的运动轨迹；等式约束表示机械臂的动力学方程，H表示机械臂的惯性矩阵，c表示离心力变量，g表示重力变量，τ表示机械臂关节力矩；θ^±、和τ^±分别表示关节角度极限、关节速度极限、关节加速度极限和关节力矩极限；将加速度层容噪控制方案转化为一个标准的二次型优化问题，其性能指标为x^Tx/2+p^Tx，约束条件为Cx＝d，Ax≤b，x^‑≤x≤x⁺，其中，C＝J，A＝[‑H^T H^T]^T，b＝[(c+g‑τ^‑)^T (τ⁺‑c‑g)^T]^T，x^±表示x的上下限。