[发明专利]一种抗噪与动力学约束的机器人实时运动规划方法及装置

权利要求书

1.一种抗噪与动力学约束的机器人实时运动规划方法，其特征在于，所述方法包括：

读取当前时刻机器人的关节角度、关节角速度的反馈，获得机器人的关节角度和关节角加速度；

基于当前的关节角和关节角加速度与所述机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩推导当前角加速度的上下限；同时，

获得所述机器人的期望轨迹，基于所述期望轨迹计算所述机器人的跟踪误差及其对应的导数；

在所述机器人的控制系统存在噪声情况下，基于所述跟踪误差及其对应的导数设置所述机器人在实现轨迹跟踪时的加速度等式约束条件；

基于所述当前角加速度的上下限和所述加速度等式约束条件计算机器人角加速度以及辅助变量的变化率；

基于机器人角加速度以及辅助变量的变化率更新所述机器人的角加速度指令。

2.根据权利要求1所述的机器人实时运动规划方法，其特征在于，所述方法还包括：

建立机器人数学模型，并基于机器人当前的关节角、关节角速度、关节角加速度以及关节力矩的约束建立机器人的物理约束模型；

基于所述机器人数学模型及所述物理约束模型建立与角加速度相关的综合物理不等式模型。

3.根据权利要求2所述的机器人实时运动规划方法，其特征在于，所述机器人数学模型如下：

所述物理约束模型如下：

其中，r为机器人末端执行器的坐标；θ为机器人关节角度；f(θ)为机器人的前向运动学模型；为r的一阶导数，为r的二阶导数；为θ的一阶导数，表示机器人关节角速度，为θ的二阶导数，表示机器人关节角加速度；J表示机器人的雅克比矩阵；为J的一阶导数；τ为机器人的关节力矩；M、C、G分别为机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩；θ^-表示θ下限，θ⁺表示θ上限；表示下限，表示上限；表示下限，表示上限；τ^-表示τ下限，τ⁺表示τ上限。

4.根据权利要求2所述的机器人实时运动规划方法，其特征在于，所述综合物理不等式模型如下：

其中，M、C、G分别为机器人的惯性矩阵、哥矢力与离心力矩阵和重力矩；κ₁、k₂、均为正控制常数；θ为机器人关节角度，为θ的一阶导数，表示机器人关节角速度，为θ的二阶导数，表示机器人关节角加速度；τ为机器人的关节力矩；θ^-表示θ下限，θ⁺表示θ上限；表示下限，表示上限；表示下限，表示上限；τ^-表示τ下限，τ⁺表示τ上限。

5.根据权利要求1所述的机器人实时运动规划方法，其特征在于，所述加速度等式约束条件如下：

其中，θ为机器人关节角度，为θ的一阶导数，表示机器人关节角速度，为θ的二阶导数，表示机器人关节角加速度；；J表示机器人的雅克比矩阵；为J的一阶导数；α、β、γ均为正控制常数；e为机器人对期望轨迹的跟踪误差，e＝r-r_d，为e的一阶导数，r为机器人的实际轨迹，r_d为机器人的期望轨迹；t表示时间。

6.根据权利要求1所述的机器人实时运动规划方法，其特征在于，所述基于所述当前角加速度的上下限和所述加速度等式约束条件计算机器人角加速度以及辅助变量的变化率，还包括：

建立所述加速度等式约束条件和综合物理不等式模型与加速度优化指标的问题描述模型；

基于所述问题描述模型在迭代的机器人控制器求解模型中进行方案求解，获得机器人角加速度以及辅助变量的变化率。