CN108227493A - 一种机器人轨迹跟踪方法 - Google Patents

Abstract

本发明公开了一种机器人轨迹跟踪方法，属于机器人控制技术领域，根据机器人各关节变量及连杆之间的几何参数构建机器人的运动学模型，并根据运动学模型获得机器人的末端位置模型；建立具有驱动系统参数的机器人动力学模型；根据末端位置模型和机器人动力学模型，建立预测机器人的实际末端位置的预测模型；根据预测模型、关节变量的第一约束条件、驱动系统的输入电压的第二约束条件建立非线性预测控制器。上述技术方案的有益效果是：根据机器人的运动学模型、动力学模型，以及机器人的驱动系统的动态性和机器人所在工作环境的约束问题，建立机器人轨迹跟踪控制的非线性预测控制器，以使机器人的轨迹跟踪控制更为精确，并且更符合实际应用需求。

Description

一种机器人轨迹跟踪方法

技术领域

本发明涉及机器人控制技术领域，尤其涉及一种机器人轨迹跟踪方法。

背景技术

机器人技术集机械、电子、计算机、自动控制、人工智能等基础和高新学科领域的理论和技术于一体，机器人本体的设计需要考虑材料选择、质量分配、尺寸优化等问题，需要应用三维造型、有限元分析、运动学和动力学分析、最优化理论等手段。机器人的控制系统涉及到伺服驱动、运动控制、计算机软件等。机器人的人机交互系统需要采用高性能嵌入式系统，需要考虑安全性和易操作性。机器人轨迹跟踪控制是指通过给定各关节的驱动力矩，使机器人的位置、速度等状态变量跟踪给定的理想轨迹，对于整个轨迹来说，都需要严格控制，是工业生产中应用最为广泛的控制方式。但是由于机器人系统是一个复杂的多输入多输出的非线性系统，具有时变、强耦合和非线性的动力学特性，因此，轨迹跟踪控制十分复杂与困难。研究机器人轨迹跟踪控制以及提高轨迹跟踪控制的精度对机器人技术有着重要的意义。

现有技术中，针对机器人轨迹跟踪控制，一般是根据机器人的运动学模型和动力学模型对机器人进行控制，都没有考虑机器人在相应的工作环境中的运动范围，以及机器人的驱动系统的动态特性，使得机器人的轨迹跟踪控制在实际控制中很难达到期望的控制效果。

发明内容

根据现有技术中存在的上述问题，现提供一种机器人轨迹跟踪控制方法，根据机器人的运动学模型、动力学模型，以及机器人的驱动系统的动态性和机器人所在工作环境的约束问题，建立机器人轨迹跟踪控制的非线性预测控制器，旨在实现更为精确地轨迹跟踪控制，使其更符合实际应用需求。本发明采用如下技术方案：

一种机器人轨迹跟踪方法，所述机器人通过一驱动系统控制运行在一实际运行轨迹中，所述实际运行轨迹中包括多个轨迹节点，包括以下步骤：

预先设定机器人的目标跟踪轨迹，所述目标跟踪轨迹包括多个跟踪轨迹节点；

步聚S1、根据所述机器人的各关节变量及连杆之间的几何参数，构建所述机器人的运动学模型，并根据所述运动学模型获得所述机器人的末端位置模型；

步聚S2、建立具有驱动系统参数的机器人动力学模型；

步骤S3、根据所述末端位置模型和所述机器人动力学模型，建立预测所述机器人的实际末端位置的预测模型；

步骤S4、根据所述预测模型、所述关节变量的第一约束条件、所述驱动系统的输入电压的第二约束条件建立非线性预测控制器；

步骤S5、根据所述运动学模型处理得到所述机器人运行到当前的所述轨迹节点时的实际末端位置；

步骤S6、所述非线性预测控制器根据当前的所述实际末端位置和所述目标跟踪轨迹处理获得下一所述轨迹节点的控制序列，所述控制序列中包括所述机器人运行到下一所述轨迹节点时的各关节变量；

步骤S7、根据所述控制序列控制所述机器人动作，并当所述机器人运行到下一所述轨迹节点时返回所述步骤S5。

较佳的，上述机器人轨迹跟踪方法中，所述步骤S2包括：

步骤S21、通过拉格朗日方法建立所述机器人的第一动力学模型；

步骤S22、建立所述驱动系统的动态模型；

步骤S23、根据所述第一动力学模型和所述动态模型处理得到所述机器人动力学模型。

较佳的，上述机器人轨迹跟踪方法中，所述第一动力学模型为：

其中，τ∈Rⁿ为关节的驱动力项，n为自由项，θ∈Rⁿ为所述关节变量，M(θ)∈R^n×n为对称正定的惯性矩阵，为向心力和哥氏力项，G(θ)∈Rⁿ为重力项，为关节摩擦项。

较佳的，上述机器人轨迹跟踪方法中，所述关节摩擦项采用库伦摩擦力和粘性摩擦力，所述关节摩擦项为：

其中，D_v为粘性摩擦系数，D_d为库伦摩擦力。

较佳的，上述机器人轨迹跟踪方法中，所述动态模型为：

其中，J_m∈R^n×n惯量矩阵，K_T∈R^n×n为电机转矩常量，B_m∈R^n×n为扭转阻尼系数，θ_m∈R^n×n为电机转动位置向量，τ_m∈Rⁿ为负载力矩，V_t∈Rⁿ为电枢输入电压，K_E∈R^n×n为反电动式系数，R_a∈R^n×n为电枢电阻。

较佳的，上述机器人轨迹跟踪方法中，所述步骤S23中根据变换关系处理所述第一动力学模型和所述动态模型得到所述机器人动力学模型；

所述机器人动力学模型为：

较佳的，上述机器人轨迹跟踪方法中，所述步骤S3包括：

步骤S31、根据一变换公式，处理所述机器人动力学模型获得预测所述机器人的所述关节变量的关节变量预测模型；

步骤S32、根据所述关节变量预测模型和所述末端位置模型，获得所述预测模型。

较佳的，上述机器人轨迹跟踪方法中，所述非线性预测控制器包括一优化函数，所述步骤S6中，通过对所述优化函数进行最小化处理获得所述控制序列；

所述优化函数为：

V_tmin≤V_t≤V_tmax；

其中，V_tmin≤V_t≤V_tmax为所述第二约束条件，为第一约束条件，Q为误差权矩阵参数，R为控制权矩阵参数，θ为所述关节变量，D_p为跟踪误差，所述跟踪误差为所述机器人的末端位置与被跟踪轨迹之间的距离。

较佳的，上述机器人轨迹跟踪方法中，通过以下公式获得所述跟踪误差：

D_p＝W_r(k+j|k)-D_r(k+j|k)；

其中，W_r为所述目标跟踪轨迹，D_r为通过所述预测模型获得的所述机器人的末端的实际位置。

上述技术方案的有益效果是：根据机器人的运动学模型、动力学模型，以及机器人的驱动系统的动态性和机器人所在工作环境的约束问题，建立机器人轨迹跟踪控制的非线性预测控制器，以使机器人的轨迹跟踪控制更为精确，并且更符合实际应用需求。

附图说明

图1-3是本发明的较佳的实施例中，一种机器人轨迹跟踪方法的流程图。

具体实施方式

下面结合附图和具体实施例对本发明作进一步说明，但不作为本发明的限定。

本发明的较佳的实施例中，如图1-3所示，提供一种机器人轨迹跟踪方法，机器人通过一驱动系统控制运行在一实际运行轨迹中，实际运行轨迹中包括多个轨迹节点，如图1所示，包括以下步骤：

预先设定机器人的目标跟踪轨迹，目标跟踪轨迹包括多个跟踪轨迹节点；

步聚S1、根据机器人的各关节变量及连杆之间的几何参数，构建机器人的运动学模型，并根据运动学模型获得机器人的末端位置模型；

步聚S2、建立具有驱动系统参数的机器人动力学模型；

步骤S3、根据末端位置模型和机器人动力学模型，建立预测机器人的实际末端位置的预测模型；

步骤S4、根据预测模型、关节变量的第一约束条件、驱动系统的输入电压的第二约束条件建立非线性预测控制器；

步骤S5、根据运动学模型处理得到机器人运行到当前的轨迹节点时的实际末端位置；

步骤S6、非线性预测控制器根据当前的实际末端位置和目标跟踪轨迹处理获得下一轨迹节点的控制序列，控制序列中包括机器人运行到下一轨迹节点时的各关节变量；

步骤S7、根据控制序列控制机器人动作，并当机器人运行到下一轨迹节点时返回步骤S5。

本实施例中，根据机器人的各关节变量及连杆之间的几何参数，构建机器人的运动学模型；再根据机器人的驱动系统的动态特性建立具有驱动系统参数的机器人动力学模型，并通过处理机器人的运动学模型和机器人动力学模型建立预测机器人实际末端位置的预测模型；然后再根据预测模型和关节变量的第一约束条件、驱动系统的输入电压的第二约束条件建立非线性预测控制器。然后再使用非线性预测控制器对机器人的跟踪轨迹进行预测，非线性预测控制器根据当前的实际末端位置和目标跟踪轨迹处理获得下一轨迹节点的控制序列，控制序列中包括机器人运行到下一轨迹节点时的各关节变量的具体值，然后再根据控制序列控制机器人运行。

上述技术方案中，根据机器人的运动学模型、动力学模型，以及机器人的驱动系统的动态性和机器人所在工作环境的约束问题，建立机器人轨迹跟踪控制的非线性预测控制器，以使机器人的轨迹跟踪控制更为精确，并且更符合实际应用需求。

本发明的较佳的实施例中，以六关节机器人为例，如下表所示，通过D-H参数方法构建机器人的运动学模型：

连杆	θi	di/mm	ai/mm	αi/mm
					1	θ1	250	150	90
2	θ2	0	550	0
					3	θ3	0	160	90
4	θ4	580	0	-90
					5	θ5	0	0	90
6	θ6	0	0	0

其中，θ_i为关节转角，即上述的关节变量，d_i为连杆间距，a_i为连杆长度，α_i为连杆扭角。

再通过关节变换矩阵：

对上表所示的运动学模型进行处理得到机器人的末端位置(P_x，P_y，P_z)方程：

P_x＝[a₆c₂c₃c₄s₅+(a₄-a₆c₅)s₂s₃+a₂c₂]c₁+a₆c₁s₅s₄；

P_y＝[a₆c₂c₃c₄s₅+(a₄-a₆c₅)s₂s₃+a₂c₂]s₁+a₆c₁s₅s₄；

P_z＝[a₆c₂c₃c₄s₅+(a₄-a₆c₅)s₂s₃+a₂c₂]s₁+1； (1)

其中，c_i＝cosθ_i，s_i＝sinθ_i。

再根据机器人的末端位置(P_x，P_y，P_z)方程(公式(1))建立机器人的末端位置模型：

[P_x，P_y，P_z]＝f(θ₁，θ₂，...，θ_n) (2)

本发明的较佳的实施例中，如图2所示，步骤S2包括：

步骤S21、通过拉格朗日方法建立机器人的第一动力学模型；

本实施例中，第一动力学模型为：

其中，τ∈Rⁿ为关节的驱动力项，n为自由项，θ∈Rⁿ为关节变量，M(θ)∈R^n×n为对称正定的惯性矩阵，为向心力和哥氏力项，G(θ)∈Rⁿ为重力项，为关节摩擦项；

进一步地，本实施例中，关节摩擦项采用库伦摩擦力和粘性摩擦力，关节摩擦项为：

其中，D_v为粘性摩擦系数，D_d为库伦摩擦力。

步骤S22、建立驱动系统的动态模型；

本实施例中，驱动系统的动态模型为：

其中，J_m∈R^n×n惯量矩阵，K_T∈R^n×n为电机转矩常量，B_m∈R^n×n为扭转阻尼系数，θ_m∈R^n×n为电机转动位置向量，τ_m∈Rⁿ为负载力矩，V_t∈Rⁿ为电枢输入电压，K_E∈R^n×n为反电动式系数，R_a∈R^n×n为电枢电阻。

步骤S23、根据第一动力学模型和动态模型处理得到机器人动力学模型。

本实施例中，步骤S23中利用电机转动位置与关节变量的变换关系，即结合第一动力学模型(公式(3))和动态模型(公式(4))联合得到包括驱动系统参数的机器人动力学模型，进一步地，机器人动力学模型为：

本发明的较佳的实施例中，如图3所示，步骤S3包括：

步骤S31、根据一变换公式，处理机器人动力学模型获得预测机器人的关节变量的关节变量预测模型；

本实施例中，使用公式处理机器人动力学模型，即公式(5)，得到预测机器人的关节变量的关节变量预测模型：

θ(k+1)＝f_s(θ(k)，V_t(k))。 (6)

步骤S32、根据关节变量预测模型和末端位置模型，获得预测模型。

本实施例中，将关节变量预测模型(公式(6))代入到末端位置模型(公式(2))中，得到预测机器人的实际末端位置的预测模型：

D_r＝f_r(θ₁，θ₂，...，θ_n)。 (7)

本发明的较佳的实施例中，非线性预测控制器包括一优化函数，步骤S6中，通过对优化函数进行最小化处理获得控制序列；

优化函数为：

其中，V_tmin≤V_t≤V_tmax为第二约束条件，为第一约束条件，Q为误差权矩阵参数，R为控制权矩阵参数，θ为关节变量，D_p为跟踪误差，跟踪误差为机器人的末端位置与被跟踪轨迹之间的距离。

进一步地，本发明的较佳的实施例中，通过以下公式获得跟踪误差：

D_p＝W_r(k+j|k)-D_r(k+j|k)；

其中，W_r为目标跟踪轨迹，D_r为通过预测模型获得的机器人的末端的实际位置。

本实施例中，通过于机器人运行到当前轨迹节点时，根据机器人上配置的检测装置检测机器人的在当前轨迹节点下各关节变量的具体值，将当前轨迹节点下各关节变量的具体值代入末端位置模型(公式(2))，可得到机器人在当前轨迹节点下的实际末端位置；再通过预测模型(公式(6))根据机器人在当前轨迹节点下的实际末端位置预测机器人运行到下一轨迹节点时的末端位置D_r(k+j|k)，预测的末端位置D_r(k+j|k)与目标跟踪轨迹的相应的跟踪轨迹节点W_r(k+j|k)的差值为跟踪误差D_p。本实施例中，优化函数(公式(8))中Δθ(k+j-1|k)用于表示机器人在下一轨迹节点下的关节变量与机器人在当前轨迹节点下的关节变量之间的差值，对优化函数进行最小化处理，即使机器人轨迹跟踪过程中跟踪误差D_p最小，以及各关节变量的角度变化尽可能小，进一步地，本实施例中，Δθ(k+j-1|k)可根据关节变量预测模型(公式(6))获得。

以上所述仅为本发明较佳的实施例，并非因此限制本发明的实施方式及保护范围，对于本领域技术人员而言，应当能够意识到凡运用本发明说明书及图示内容所作出的等同替换和显而易见的变化所得到的方案，均应当包含在本发明的保护范围内。

Claims (9)

1.一种机器人轨迹跟踪方法，其特征在于，所述机器人通过一驱动系统控制运行在一实际运行轨迹中，所述实际运行轨迹中包括多个轨迹节点，包括以下步骤：

预先设定机器人的目标跟踪轨迹，所述目标跟踪轨迹包括多个跟踪轨迹节点；

步聚S1、根据所述机器人的各关节变量及连杆之间的几何参数，构建所述机器人的运动学模型，并根据所述运动学模型获得所述机器人的末端位置模型；

步聚S2、建立具有驱动系统参数的机器人动力学模型；

步骤S3、根据所述末端位置模型和所述机器人动力学模型，建立预测所述机器人的实际末端位置的预测模型；

步骤S4、根据所述预测模型、所述关节变量的第一约束条件、所述驱动系统的输入电压的第二约束条件建立非线性预测控制器；

步骤S5、根据所述运动学模型处理得到所述机器人运行到当前的所述轨迹节点时的实际末端位置；

步骤S6、所述非线性预测控制器根据当前的所述实际末端位置和所述目标跟踪轨迹处理获得下一所述轨迹节点的控制序列，所述控制序列中包括所述机器人运行到下一所述轨迹节点时的各关节变量；

步骤S7、根据所述控制序列控制所述机器人动作，并当所述机器人运行到下一所述轨迹节点时返回所述步骤S6。

2.如权利要求1所述的机器人轨迹跟踪方法，其特征在于，所述步骤S2包括：

步骤S21、通过拉格朗日方法建立所述机器人的第一动力学模型；

步骤S22、建立所述驱动系统的动态模型；

步骤23、根据所述第一动力学模型和所述动态模型处理得到所述机器人动力学模型。

3.如权利要求2所述的机器人轨迹跟踪方法，其特征在于，所述第一动力学模型为：

其中，τ∈Rⁿ为关节的驱动力项，n为自由项，θ∈Rⁿ为所述关节变量，M(θ)∈R^n×n为对称正定的惯性矩阵，为向心力和哥氏力项，G(θ)∈Rⁿ为重力项，为关节摩擦项。

4.如权利要求3所述的机器人轨迹跟踪方法，其特征在于，所述关节摩擦项采用库伦摩擦力和粘性摩擦力，所述关节摩擦项为：

其中，D_v为粘性摩擦系数，D_d为库伦摩擦力。

5.如权利要求3所述的机器人轨迹跟踪方法，其特征在于，所述动态模型为：

其中，J_m∈R^n×n惯量矩阵，K_T∈R^n×n为电机转矩常量，B_m∈R^n×n为扭转阻尼系数，θ_m∈R^n×n为电机转动位置向量，τ_m∈Rⁿ为负载力矩，V_t∈Rⁿ为电枢输入电压，K_E∈R^n×n为反电动式系数，R_a∈R^n×n为电枢电阻。

6.如权利要求5所述的机器人轨迹跟踪方法，其特征在于，所述步骤S23中根据变换关系处理所述第一动力学模型和所述动态模型得到所述机器人动力学模型；

所述机器人动力学模型为：

7.如权利要求1所述的机器人轨迹跟踪方法，其特征在于，所述步骤S3包括：

步骤S31、根据一变换公式，处理所述机器人动力学模型获得预测所述机器人的所述关节变量的关节变量预测模型；

步骤S32、根据所述关节变量预测模型和所述末端位置模型，获得所述预测模型。

8.如权利要求1所述的机器人轨迹跟踪方法，其特征在于，所述非线性预测控制器包括一优化函数，所述步骤S6中，通过对所述优化函数进行最小化处理获得所述控制序列；

所述优化函数为：

V_tmin≤V_t≤V_tmax；

其中，V_tmin≤V_t≤V_tmax为所述第二约束条件，为第一约束条件，Q为误差权矩阵参数，R为控制权矩阵参数，θ为所述关节变量，D_p为跟踪误差，所述跟踪误差为所述机器人的末端位置与被跟踪轨迹之间的距离。

9.如权利要求8所述的机器人轨迹跟踪方法，其特征在于，通过以下公式获得所述跟踪误差：

D_p＝W_r(k+j|k)-D_r(k+j|k)；

其中，W_r为所述目标跟踪轨迹，D_r为通过所述预测模型获得的所述机器人的末端的实际位置。