CN105938364A - 一种3d欠驱动双足机器人的动力学模型计算方法 - Google Patents

Abstract

本发明涉及机器人技术领域，提供了一种3D欠驱动双足机器人的动力学模型计算方法，该方法针对3D欠驱动双足机器人，对其在单足支撑相与双足支撑相都进行了动力学模型计算；其中，机器人的双足支撑相由碰撞过程以及坐标切换过程组成。相比于以往的动力学模型计算方法，本发明所提出的动力学模型计算方法使得3D欠驱动双足机器人动力学模型的编程计算过程变得简单清晰，从而便于编程应用。

Description

一种3D欠驱动双足机器人的动力学模型计算方法

技术领域

本发明属于机器人技术领域，具体涉及一种3D欠驱动双足机器人的动力学模型计算方法。

背景技术

双足机器人的动力学建模方法主要有牛顿-欧拉法以及拉格朗日法。当考虑更多自由度机器人的动力学建模问题时，利用牛顿-欧拉法需要关注各个关节细节，计算往往比较复杂，而拉格朗日法更受青睐。《机器人操作的数学导论》一书中基于拉格朗日方法获得了机器人的动力学模型，该模型计算方法为后来的学者所广泛接受。在该书第112页，机器人的动力学模型表示为

$D\left(q\right)\stackrel{\·\·}{q}+C(q,\stackrel{\·}{q})\stackrel{\·}{q}+G\left(q\right)=u.$

在该书中，作者相应的给出了惯性矩阵D以及科氏矩阵C的计算方法：

$C=\frac{1}{2}\underset{k=1}{\overset{n}{\Σ}}\{\frac{\∂D_{ij}}{\∂q_k}+\frac{\∂D_{ik}}{\∂q_j}-\frac{\∂D_{kj}}{\∂q_i}\}{\stackrel{\·}{q}}_k$

其中表示第i杆的广义惯性矩，J_i表示第i杆在世界坐标系下的雅可比矩阵。

但是，基于该方法对动力学模型进行编程计算时会导致程序看起来很复杂，不易理解，从而容易出现编程错误。

发明内容

本发明针对3D欠驱动双足机器人，对其在单足支撑相与双足支撑相都进行了动力学建模，并给出了相应的模型计算方法。其中，机器人的双足支撑相由碰撞过程以及坐标切换过程组成。

本发明的技术目的通过以下技术方案实现:

一种3D欠驱动双足机器人的动力学模型计算方法，包括以下步骤：

步骤一，基于DH方法，建立机器人的关节坐标系q＝(q₁,q₂,q₃,q₄,q₅,q₆,q₇,q₈)，并求出坐标系q_k在坐标系q_k-1下的齐次转换矩阵其中k＝1,2，..8；

步骤二，假设机器人各杆件的质量集中于质心，基于DH方法，求得各质心在世界坐标系下的位置向量p_i以及速度向量v_i；进而求得机器人系统的总的动能E和势能P：

$E=\underset{i=1}{\overset{5}{\Σ}}\frac{1}{2}{m}_i{\left(v_i\right)}^2,P=\underset{i=1}{\overset{5}{\Σ}}{m}_i{\mathrm{gp}}_i\left(3\right)$

其中，m_i表示第i个杆件的质量，i＝1,2，..5；p_i(3)表示向量p_i的第三个元素；

步骤三，计算双足机器人在单足支撑相的动力学模型，

$D\left(q\right)\stackrel{\·\·}{q}+H(q,\stackrel{\·}{q})=Bu=\left[\begin{array}{c}{0}_2\×6\\ I_2\×6\end{array}\right]u;$

其中D为惯性质量矩阵，H为耦合矩阵，B为常数矩阵，u＝[u₃,u₄,u₅,u₆,u₇,u₈]′为主动关节驱动力矩；

步骤四，计算双足机器人与地面碰撞的动力学模型，

$\left[\begin{array}{c}{\stackrel{\·}{q}}_e^{+}\\ F_2\end{array}\right]={\left[\begin{array}{cc}{D}_e& -E_2^T\\ E_2& 0_4\×4\end{array}\right]}^{-1}\left[\begin{array}{c}{D}_e{\stackrel{\·}{q}}_e^{-}\\ 0_{4\×4}\end{array}\right];$

其中，D_e表示增广的惯性质量矩阵，与表示机器人碰撞前后的广义角速度,F₂表示地面对摆动腿在接触点的反作用力，表示摆动腿的位置及其方向角对广义坐标系q_e的雅可比矩阵；

步骤五，计算机器人的坐标切换模型，其中，主动关节的切换模型为[q₃,q₄,q₅,q₆,q₇,q₈]→[q₈,q₇,q₆,q₅,q₄,q₃]，欠驱动关节的切换模型为[q₁,q₂]→[q_1sw,q_2sw]，q_1sw表示摆动腿俯仰角，q_2sw表示摆动腿滚动角。

进一步的，步骤三中，令则

$D=\frac{\∂V}{\∂\stackrel{\·}{q}},H=\frac{\∂V}{\∂q}\stackrel{\·}{q}-\frac{\∂L}{\∂q}.$

进一步的，步骤五中，q_1sw与q_2sw的计算采用几何解析方法，包括以下步骤：

a)在机器人摆动腿末端焊接一根单位长度的虚拟杆一，所述虚拟杆一的方向与坐标系q₈的y轴负方向平行，利用所述虚拟杆一在世界坐标系z轴上的投影与虚拟杆一长度的比值来计算摆动腿俯仰角q_1sw的大小：

q_1sw＝-asin(l₁(3)/1)

其中l₁表示所述虚拟杆一在世界参考坐标系下的位置向量；

b)在机器人摆动腿末端焊接一根单位长度的虚拟杆二，所述虚拟杆二的方向与坐标系q₈的z轴正方向平行，利用所述虚拟杆二在世界坐标系z轴上的投影与虚拟杆二长度的比值来计算摆动腿滚动角q_2sw的大小：

q_2sw＝asin(l₂(3)/1)

其中l₂表示所述虚拟杆二在世界参考坐标系下的位置向量。

本发明的有益效果在于：相比于《机器人操作的数学导论》中的动力学模型计算方法，本发明所提出的动力学模型计算方法使得3D欠驱动双足机器人动力学模型各矩阵的编程计算变得简单清晰，便于编程人员的理解和应用，从而提高工作效率；另外，本发明所提出的解析法能够获得3D双足机器人摆动腿滚动角q_1sw以及俯仰角q_2sw的解析解，使得q_1sw以及q_2sw的计算结果更加精确，从而可以提高机器人步态的控制精度。

附图说明

图1是本发明中3D欠驱动双足机器人的模型示意图。

具体实施方式

下面结合附图和实施例对本发明作进一步说明。

本发明实施例所涉及的机器人如图1所示，为五杆欠驱动双足机器人，并假设杆件的质量集中于质心。该机器人具有8个独立自由度，q₁,q₂为机器人支撑腿踝关节角度，q₃为机器人支撑腿膝关节角度，q₄,q₅为支撑腿髋关节角度，q₆,q₇为摆动腿髋关节角度，q₈为摆动腿膝关节角度。其中，自由度q₃,q₄,q₅,q₆,q₇,q₈由力矩直接驱动，为主动关节，而q₁,q₂为欠驱动关节。

本发明针对上述双足机器人，对其在单足支撑相与双足支撑相都进行了动力学建模，并给出了相应的模型计算方法。其中，机器人的双足支撑相由碰撞过程以及坐标切换过程组成。具体的，计算方法包含以下步骤：

步骤一，基于DH方法，建立机器人的关节坐标系q＝(q₁,q₂,q₃,q₄,q₅,q₆,q₇,q₈)，并求出坐标系q_k在坐标系q_k-1下的齐次转换矩阵其中k＝1,2，..8；

步骤二，基于DH方法，求得各质心在世界坐标系下的位置向量p_i以及速度向量v_i；继而，可求得机器人系统的总的动能E和势能P：

$E=\underset{i=1}{\overset{5}{\Σ}}\frac{1}{2}{m}_i{\left(v_i\right)}^2,P=\underset{i=1}{\overset{5}{\Σ}}{m}_i{\mathrm{gp}}_i\left(3\right);$

其中，m_i表示第i个杆件的质量，i＝1,2，..5；p_i(3)表示向量p_i的第三个元素；

步骤三，计算双足机器人在单足支撑相的动力学模型，令L＝E-P，则动力学模型可表示为

$D\left(q\right)\stackrel{\·\·}{q}+H(q,\stackrel{\·}{q})=Bu=\left[\begin{array}{c}{0}_{2\×6}\\ I_{2\×6}\end{array}\right]u;$

其中惯性质量矩阵耦合矩阵B为常数矩阵，u＝[u₃,u₄,u₅,u₆,u₇,u₈]′为主动关节驱动力矩。

在进行计算编程时，上面所述的偏导数计算可通过matlab提供的jacobian函数来实现。比如计算其matlab程序可为：

D＝jacobian(V,dq)；％dq表示q的导数。

步骤四，计算双足机器人与地面碰撞的动力学模型，

$\left[\begin{array}{c}{\stackrel{\·}{q}}_e^{+}\\ F_2\end{array}\right]={\left[\begin{array}{cc}{D}_e& -E_2^T\\ E_2& 0_4\×4\end{array}\right]}^{-1}\left[\begin{array}{c}{D}_e{\stackrel{\·}{q}}_e^{-}\\ 0_{4\×4}\end{array}\right];$

其中，D_e表示增广的惯性质量矩阵，其计算方法与步骤三类似，与表示机器人碰撞前后的广义角速度,F₂表示地面对摆动腿在接触点的反作用力，表示摆动腿的位置及其方向角对广义坐标系q_e的雅可比矩阵；

步骤五，计算机器人的坐标切换模型；其中，主动关节的切换模型为[q₃,q₄,q₅,q₆,q₇,q₈]→[q₈,q₇,q₆,q₅,q₄,q₃]，欠驱动关节的切换模型为[q₁,q₂]→[q_1sw,q_2sw]。其中，q_1sw与q_2sw的计算是采用几何解析方法，具体过程为：

a)在机器人摆动腿末端焊接一根单位长度的虚拟杆，该杆的方向与坐标系q₈的y轴负方向平行，那么，可以利用这根虚拟杆在世界坐标系z轴上的投影与虚拟杆长度的比值来计算摆动腿俯仰角q_1sw的大小：

q_1sw＝-asin(l₁(3)/1)

其中l₁表示虚拟杆在世界参考坐标系下的位置向量；

b)在机器人摆动腿末端焊接一根单位长度的虚拟杆，该杆的方向与q₈坐标系的z轴正方向平行，那么，可以利用这根虚拟杆在世界坐标系z轴上的投影与虚拟杆长度的比值来计算摆动腿滚动角q_2sw的大小：

q_2sw＝asin(l₂(3)/1)

其中l₂表示虚拟杆在世界参考坐标系下的位置向量。

至此，本实施例完成了3D欠驱动双足机器人的单足支撑相以及双足支撑相的动力学模型计算。

相比于《机器人操作的数学导论》中的动力学模型计算方法，上述方法使得3D欠驱动双足机器人动力学模型各矩阵的编程计算变得简单清晰，便于编程人员的理解和应用，从而提高工作效率；另外，本发明所提出的解析法能够获得3D双足机器人摆动腿滚动角q_1sw以及俯仰角q_2sw的解析解，使得q_1sw以及q_2sw的计算结果更加精确，从而可以提高机器人步态的控制精度。

上述实施例仅仅是本发明技术构思实现形式的列举，本发明的保护范围不仅限于上述实施例，本发明的保护范围可延伸至本领域技术人员根据本发明的技术构思所能想到的等同技术手段。

Claims (3)

1.一种3D欠驱动双足机器人的动力学模型计算方法，其特征在于，包括以下步骤：

步骤一，基于DH方法，建立机器人的关节坐标系q＝(q₁,q₂,q₃,q₄,q₅,q₆,q₇,q₈)，并求出坐标系q_k在坐标系q_k-1下的齐次转换矩阵 其中k＝1,2，..8；

步骤二，假设机器人各杆件的质量集中于质心，基于DH方法，求得各质心在世界坐标系下的位置向量p_i以及速度向量v_i；进而求得机器人系统的总的动能E和势能P：

其中，m_i表示第i个杆件的质量，i＝1,2,…5；p_i(3)表示向量p_i的第三个元素；

步骤三，计算双足机器人在单足支撑相的动力学模型，

其中D为惯性质量矩阵，H为耦合矩阵，B为常数矩阵，u＝[u₃,u₄,u₅,u₆,u₇,u₈]′为主动关节驱动力矩；

步骤四，计算双足机器人与地面碰撞的动力学模型，

其中，D_e表示增广的惯性质量矩阵，与表示机器人碰撞前后的广义角速度,F₂表示地面对摆动腿在接触点的反作用力，表示摆动腿的位置及其方向角对广义坐标系q_e的雅可比矩阵；

步骤五，计算机器人的坐标切换模型，其中，主动关节的切换模型为 [q₃,q₄,q₅,q₆,q₇,q₈]→[q₈,q₇,q₆,q₅,q₄,q₃]，欠驱动关节的切换模型为[q₁,q₂]→[q_1sw,q_2sw]，q_1sw表示摆动腿俯仰角，q_2sw表示摆动腿滚动角。

2.如权利要求1所述的3D欠驱动双足机器人的动力学模型计算方法，其特征在于，步骤三中，令L＝E-P，则

3.如权利要求1所述的3D欠驱动双足机器人的动力学模型计算方法，其特征在于，步骤五中，q_1sw与q_2sw的计算采用几何解析方法，包括以下步骤：

a.在机器人摆动腿末端焊接一根单位长度的虚拟杆一，所述虚拟杆一的方向与坐标系q₈的y轴负方向平行，利用所述虚拟杆一在世界坐标系z轴上的投影与虚拟杆一长度的比值来计算摆动腿俯仰角q_1sw的大小：

q_1sw＝-asin(l₁(3)/1)

其中l₁表示所述虚拟杆一在世界参考坐标系下的位置向量；

b.在机器人摆动腿末端焊接一根单位长度的虚拟杆二，所述虚拟杆二的方向与坐标系q₈的z轴正方向平行，利用所述虚拟杆二在世界坐标系z轴上的投影与虚拟杆二长度的比值来计算摆动腿滚动角q_2sw的大小：

q_2sw＝asin(l₂(3)/1)

其中l₂表示所述虚拟杆二在世界参考坐标系下的位置向量。