CN102799184B - 仿生机械恐龙爬行稳定性控制方法 - Google Patents

Abstract

本发明公开了一种仿生机械恐龙爬行稳定性控制方法，包括以下步骤：将各肢体简化为质点系；为每个刚体连杆建立D-H坐标系，再依次变换求得足底相对于机体坐标系的位姿以及机体坐标系与地面坐标系之间的转换矩阵将前进步态规划为下蹲、身体右倾并向右摆尾、迈左前腿、迈左后腿、身体左倾并向左摆尾、迈右后腿、迈右前腿、身体右倾；根据大腿、小腿以及髋关节的驱动函数，利用转换矩阵实时计算各肢体组成部分的重心及恐龙的整体质心，确保其垂直投影落在由立足点所构成的多边形区域内。本发明，通过机体坐标系与固定坐标系之间的转换获得总质心与各关节的函数关系，从而调节各关节变量，提高了仿生机械恐龙的行走稳定性。

Description

仿生机械恐龙爬行稳定性控制方法

技术领域

本发明涉及四足机器人，具体涉及仿生机械恐龙爬行稳定性控制方法。

背景技术

机器人技术是近几十年来迅速发展起来的一门高新技术，它综合了机械、微电子与计算机、自动控制、传感器与信息处理以及人工智能等多学科的最新研究成果，是机电一体化技术的典型载体。大型四足仿生机械恐龙是四足步行机器人的一种重要应用，广泛应用于娱乐、影视等领域。

众所周知，行走的稳定性是四足步行机器人的关键技术，为此，众多的研究机构都对四足步行机器人的步态规划展开了研究，然而，这些研究课题的研究对象都是小型四足步行机器人，在进行稳定性研究时，主要考虑的是腿部各关节变化对整体稳定性的影响。因此，这些研究成果应用到大型四足仿生机械恐龙时具有一定的局限性。主要原因在于：

（1）大型四足仿生机械恐龙尺寸大、质量重，且质量分布较为离散；

（2）大型四足仿生机械恐龙的腿部和首、尾质量相对较大，特别是现有针对小型四足机器人的步态规划并没有考虑首、尾质量的影响。

有鉴于此，对于大型四足仿生机械恐龙而言，必须结合其腿部和首尾的运动对质心的影响对其稳定性进行重新设计，以提高大型四足仿生机械恐龙的行走稳定性。

发明内容

本发明所要解决的技术问题是解决如何提高大型四足仿生机械恐龙的行走稳定性的问题。

为了解决上述技术问题，本发明所采用的技术方案是提供一种仿生机械恐龙爬行稳定性控制方法，包括以下步骤：

将仿生机械恐龙的各肢体组成部分视为刚体连杆，并简化为按一定装配和约束关系组合而成的质点系；

为每个刚体连杆建立D-H坐标系，利用4×4的齐次变换矩阵描述相邻两刚体连杆的空间关系，再依次变换求得足底相对于机体坐标系的位姿，以及机体坐标系B与地面坐标系G之间的转换矩阵

将一个前进爬行步态周期的步态规划如下：下蹲；身体右倾并向右摆尾；迈左前腿；迈左后腿；身体左倾并向左摆尾；迈右后腿；迈右前腿；身体右倾；

根据大腿关节、小腿关节以及髋关节的运动函数，利用机体坐标系B与地面坐标系G之间的转换矩阵实时计算各肢体组成部分的重心，并获得仿生机械恐龙的整体质心位置，确保其垂直投影落在由立足点所构成的多边形区域内。

在上述方法中，机体坐标系B与地面坐标系G之间的转换矩阵为：

$T_B^G=\left[\begin{array}{cccc}1& 0& 0& {\mathrm{Gx}}_{\mathrm{BO}}\\ 0& 1& 0& {\mathrm{Gy}}_{\mathrm{BO}}\\ 0& 0& 1& {\mathrm{Gz}}_{\mathrm{BO}}\\ 0& 0& 0& 1\end{array}\right];$

式中：

Gx_BO＝[L_Tcos(θ_T)+L_Scos(θ_T+θ_S)]sin(θ_H)；

Gy_BO＝-L_Tsin(θ_T)-L_Ssin(θ_T+θ_S)；

Gz_BO＝Δ_Z1-Δ_Z2；

Δ_Z1＝L_T[1-cos(θ_T)]+L_S[1-cos(θ_T+θ_S)]；

Δ_Z2＝L[1-cos(θ_H)]；

L＝L_H+L_T+L_S；

L_T：大腿长度；

L_S：小腿长度；

L_H：髋关节回转中心到大腿关节回转中心的距离；

θ_T：大腿与竖直方向的夹角；

θ_S：小腿与大腿延长线之间的夹角。

在上述方法中，在前进爬行步态中，任一条腿的足底运动轨迹满足以下条件：

x＝-L_Tsin(θ_T)-L_Ssin(θ_T+θ_S)；

y＝-L_Tcos(θ_T)-L_Scos(θ_T+θ_S)；

式中：

L_T：大腿长度；

L_S：小腿长度；

θ_T：大腿与竖直方向的夹角；

θ_S：小腿与大腿延长线之间的夹角。

在上述方法中，任一条腿的迈腿过程中，

大腿关节的转动角度θ_T满足以下条件：

t=0.00～0.15秒时，θ_T＝-1.39t^2-0.17；

t=0.15～1.17秒时，θ_T＝-0.42t-0.14；

t=1.17～1.32秒时，θ_T＝1.39t²-3.67t+1.75；

t=1.32～1.47秒时，θ_T＝-0.66；

t=1.47～1.98秒时，θ_T＝-0.66；

t=1.98～2.13秒时，θ_T＝1.39t²-5.53t+4.83；

t=2.13～3.15秒时，θ_T＝0.42t-1.52；

t=3.15～3.30秒时，θ_T＝-1.39t²+9.21t-15.37；

小腿关节的转动角度θ_S满足以下条件：

t=0.00～0.15秒时，θ_S＝1.74t²；

t=0.15～1.17秒时，θ_S＝0.52t＋0.31；

t=1.17～1.32秒时，θ_S＝－1.74t²＋4.59t－2.06；

t=1.32～1.47秒时，θ_S＝－1.74t²＋4.59t－2.06；

t=1.47～1.98秒时，θ_S＝0.52t＋1.69；

t=1.98～2.13秒时，θ_S＝0.52t＋1.69；

t=2.13～3.15秒时，θ_S＝0.52t＋1.69；

t=3.15～3.30秒时，θ_S＝1.74t²－11.51t＋18.99；

θ_T、θ_S的单位为弧度。

在上述方法中，在一个前进爬行步态周期内髋关节的转动角度θ_H满足以下条件：

t=0.97～1.67秒时，θ_H＝-0.17t²＋0.34t-0.16；

t=1.67～2.38秒时，θ_H＝0.17t²-0.83t＋0.81；

t=8.98～9.98秒时，θ_H＝0.17t²－3.13t＋13.89；

t=9.98～10.98秒时，θ_H＝－0.17t²＋3.93t－20.86；

t=17.58～18.58秒时，θ_H＝－0.52t＋1.69；

t=18.58～19.58秒时，θ_H＝－0.52t＋1.69；

θ_H的单位为弧度。

在上述方法中，头部摆动角度最佳值θ_n为颈部质心在立足点支撑平面的投影到对角支撑足连线的距离最大时的摆角，尾部摆动角度最佳值θ_m为尾部质心在立足点支撑平面的投影到对角支撑足连线的距离最大时的摆角。

在上述方法中，θ_n＝θ_m＝51°。

在上述方法中，髋关节倾角θ_H介于lim1和lim2之间，θ_H＝Lim1时，整体质心落在三个立足点形成的支撑三角形的斜边上，θ_H＝Lim2时，整体质心落在三个立足点形成的支撑三角形的右边上，lim1=-6.191536295°，lim2=-30.2715°。

本发明，重新优化了仿生机械恐龙的前进爬行步态，根据大腿关节、小腿关节以及髋关节的驱动函数，利用机体坐标系B与地面坐标系G之间的转换矩阵实时计算各肢体组成部分的重心，并获得仿生机械恐龙的整体质心位置，确保其垂直投影落在由立足点所构成的多边形区域内，提高了仿生机械恐龙的行走稳定性。

附图说明

图1为本发明中仿生机械恐龙的质点简化示意图；

图2为本发明中D－H坐标系建立示意图；

图3为本发明中一个前进步态的时序图；

图4为足底运动分析示意图；

图5为足底迹细化示意图；

图6为髋关节侧倾时整体质心的两极限位置示意图。

具体实施方式

本发明提供的仿生机械恐龙爬行稳定性控制方法，充分考虑了大型四足仿生机械恐龙尺寸大、质量重且分布较为离散，加之腿部和首、尾质量相对较大的因素，将仿生机械恐龙的各肢体组成部分简化为按一定装配和约束关系组合而成的质点系，然后建立D-H坐标系，根据大腿关节、小腿关节以及髋关节的运动函数，利用机体坐标系B与地面坐标系G之间的转换矩阵实时计算各肢体组成部分的重心，并获得仿生机械恐龙的整体质心位置，本发明重新优化了仿生机械恐龙的前进爬行步态，使仿生机械恐龙在行进的过程中通过左倾、右倾、左右摇头、左右摆尾实时进行整体质心调整，确保其垂直投影落在由立足点所构成的多边形区域内，提高了仿生机械恐龙的行走稳定性。

下面结合附图和一个具体实施例对本发明作出详细的说明。

本发明所述的具体实施例中，大型四足仿生机械恐龙的长度为3.5米左右、高度为2米左右、宽度为1.5米左右，总重量约为400kg，具有头部、水平颈部、垂直颈部、左前大腿、左前小腿、左后髋部、左后前大腿、左后小腿、左后髋部、右前大腿、右前小腿、右前髋部、右后大腿、右后小腿、右后髋部、水平尾部、垂直尾部17个组件。

针对上述实施例的控制方法，包括以下步骤：

步骤1：简化模型。

由于本实施例中的大型四足仿生机械恐龙包括上述17个组件，给其运动学、动力学及强度和整体刚度分析带来诸多不便。为了方便开展各种分析，将上述17个组件分别视为一个刚体连杆，再将各刚体连杆的质量简化到刚体连杆的几何中心处，从而简化为按一定装配和约束关系组合而成的质点系，如图1所示。图中，表示质心。

步骤2：在每个刚体连杆上固接一个D-H坐标系并利用4×4的齐次变换矩阵描述这些D-H坐标系之间的关系。

大型四足仿生机械恐龙可以看作由机体向外发散的一个复合机构，包括颈部、尾部和四肢六条开式运动链。开式运动链的一端固定在机体上，另一端是自由的，由驱动器（例如电机）驱动关节运动，关节的相对运动导致刚体连杆的运动，从而使足底按照规划好的轨迹运动。

为此，在图1的基础上，建立如图2所示D-H(Denavit和Hartenberg)坐标系，图中下标符号的含意参见表1所示。

表1大型四足仿生机械恐龙基本参数表

建立了D-H坐标系后，用4×4的齐次变换矩阵描述相邻两刚体连杆的空间关系，再依次变换最终推导出足底相对于机体坐标系的位姿以及机体坐标系B与地面坐标系G之间的转换矩阵下面以左前腿为例加以说明，表2给出了组成左前腿的刚体连杆参数。

表2左前腿参数

连杆序号	ai-1	αi-1	di	θi
					1	0	0	0	θLFH
2	LLFH	π/2	0	θLFT
					3	LLFT	0	0	θLFS

左前腿各关节到仿生机械恐龙的整体质心（机体坐标系B）之间的坐标系转换矩阵，可遵照常规的D-H转换矩阵建立，这是本领域的公知技术，在此不再赘述。本发明的重点在于如何确定机体坐标系B与地面坐标系G（固定坐标系）之间的关系，只有转换为地面坐标系G，才能够根据足底运动轨迹，计算出整体质心与各关节转角之间的函数关系，从而使仿生机械恐龙在行进的过程中通过左倾、右倾、左右摇头、左右摆尾实时进行整体质心调整，使整体质心的垂直投影落在由立足点所构成的多边形区域内。

为简化计算，忽略由于前后腿少量的高度差在侧倾时产生的扭转，则：

Gx_BO＝[L_Tcos(θ_T)+L_Scos(θ_T+θ_S)]sin(θ_H)；公式（3）

Gy_BO＝-L_Tsin(θ_T)-L_Ssin(θ_T+θ_S)；公式（4）

Gz_BO＝Δ_Z1-Δ_Z2；公式（5）

Δ_Z1＝L_T[1-cos(θ_T)]+L_S[1-cos(θ_T+θ_S)]；公式（6）

Δ_Z2＝L[1-cos(θ_H)]；公式（7）

L＝L_H+L_T+L_S；公式（8）

$T_B^G=\left[\begin{array}{cccc}1& 0& 0& {\mathrm{Gx}}_{\mathrm{BO}}\\ 0& 1& 0& {\mathrm{Gy}}_{\mathrm{BO}}\\ 0& 0& 1& {\mathrm{Gz}}_{\mathrm{BO}}\\ 0& 0& 0& 1\end{array}\right];$ 公式（9）

L_T：大腿长度；

L_S：小腿长度；

L_H：髋关节回转中心到大腿关节回转中心的距离；

θ_T：大腿与竖直方向的夹角；

θ_S：小腿与大腿延长线之间的夹角。

由公式(6)(7)(8)可求得公式(3)(4)(5)，即得到机体坐标系B原点在固定坐标系G中的坐标，进而可得到机身坐标系B到固定坐标系G之间的转换矩阵，即公式(9)。其它坐标变换矩阵可按照表1和表2中参数来计算，限于篇幅所限，下面仅给出左前腿坐标系间的转换矩阵即公式(10)、(11)、(12)、(13)。

$T_{\mathrm{LF}0}^G=\left[\begin{array}{cccc}0& -1& 0& W/2\\ 0& 0& 1& L/2\\ -1& 0& 0& H/2\\ 0& 0& 0& 1\end{array}\right];$ 公式(10)

$T_{\mathrm{LFH}}^{\mathrm{LF}0}=\left[\begin{array}{cccc}\cos \left({\mathrm{\θ}}_{\mathrm{LFH}}\right)& -\sin \left({\mathrm{\θ}}_{\mathrm{LFH}}\right)& 0& 0\\ \sin \left({\mathrm{\θ}}_{\mathrm{LFH}}\right)& \cos \left({\mathrm{\θ}}_{\mathrm{LFH}}\right)& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right];$ 公式(11)

$T_{\mathrm{LFT}}^{\mathrm{LFH}}=\left[\begin{array}{cccc}\cos \left({\mathrm{\θ}}_{\mathrm{LFT}}\right)& -\sin \left({\mathrm{\θ}}_{\mathrm{LFT}}\right)& 0& L_{\mathrm{LFH}}\\ \sin \left({\mathrm{\θ}}_{\mathrm{LFT}}\right)& \cos \left({\mathrm{\θ}}_{\mathrm{LFT}}\right)& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right];$ 公式(12)

$T_{\mathrm{LFS}}^{\mathrm{LFT}}=\left[\begin{array}{cccc}\cos \left({\mathrm{\θ}}_{\mathrm{LFS}}\right)& -\sin \left({\mathrm{\θ}}_{\mathrm{LFS}}\right)& 0& L_{\mathrm{LFT}}\\ \sin \left({\mathrm{\θ}}_{\mathrm{LFS}}\right)& \cos \left({\mathrm{\θ}}_{\mathrm{LFS}}\right)& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right];$ 公式(13)

其中W为机身宽度，L为机身长度，H机身等效高度，其余角标符号参见表1。按照以上方法，可得到所有刚体杆件的D-H坐标系到固定坐标系G的转换矩阵，下面以左前腿的三个坐标系到固定坐标系G的转换矩阵来说明到G坐标系的计算方法。

$T_{\mathrm{LFH}}^G=T_B^G\×T_{\mathrm{LF}0}^B\×T_{\mathrm{LFH}}^{\mathrm{LF}0};$ 公式(14)

$T_{\mathrm{LFT}}^G=T_B^G\×T_{\mathrm{LF}0}^B\×T_{\mathrm{LFH}}^{\mathrm{LF}0}\×T_{\mathrm{LFT}}^{\mathrm{LFH}};$ 公式(15)

$T_{\mathrm{LFS}}^G=T_B^G\×T_{\mathrm{LF}0}^B\×T_{\mathrm{LFH}}^{\mathrm{LF}0}\×T_{\mathrm{LFT}}^{\mathrm{LFH}}\×T_{\mathrm{LFS}}^{\mathrm{LFT}};$ 公式(16)

公式(14)、(15)、(16)即为左前腿各坐标系B到固定坐标系G的转换矩阵，按照同样的方式可得到，左后腿，右后腿，右前腿以及头部和尾部的坐标转换矩阵，不再列出。

步骤3：将一个前进爬行步态周期的步态规划如下：

（1）下蹲，四腿弯曲形成所述特定的腿部初始姿态；

（2）身体右倾，并向右摆尾，重心右移至左后腿、右前腿、右后腿立足点所构成的三角形区域内；

（3）迈左前腿，重心前移并维持在左前腿、右前腿、右后腿立足点所构成的三角形区域内；

（4）迈左后腿，重心前移至左前腿、左后腿、右前腿、右后腿立足点所构成的四边形区域内；

（5）身体左倾，并向左摆尾，重心左移至左前腿、左后腿、右前腿、立足点所构成的三角形区域内；

（6）迈右后腿，重心前移至左前腿、左后腿、右后腿立足点所构成的三角形区域内；

（7）迈右前腿，重心前移至左前腿、左后腿、右前腿、右后腿立足点所构成的四边形区域内；

（8）身体右倾，重心恢复至左前腿、左后腿、右前腿、右后腿立足点所构成的四边形区域的几何中心位置。

图3示出了迈步动作的时序图。

步骤4、根据大腿关节、小腿关节以及髋关节的驱动函数，利用机体坐标系B与地面坐标系G之间的转换矩阵实时计算各肢体组成部分的重心，并获得仿生机械恐龙的整体质心位置，确保其垂直投影落在由立足点所构成的多边形区域内。

如图4、图5所示，本发明中迈腿前后足底的运动轨迹由AB、BC和CD三段曲线构成，AB端为抬腿运动段。大腿关节和小腿关节同时运动，BC段为迈步阶段，主要运动效果是产生前进方向上的位移，CD段为落腿阶段，使腿部回落到地面。为防止足底与地面的干涉，足底端点运动方程须满足以下条件：

x＝-L_Tsin(θ_T)-L_Ssin(θ_T+θ_S)；公式（1）

y＝-L_Tcos(θ_T)-L_Scos(θ_T+θ_S)。公式（2）

式中：

x：任意单腿足底端点在一个步态周期内水平方向位移；

y：任意单腿足底端点在一个步态周期内竖直方向位移；

L_T：大腿长度；

L_S：小腿长度；

θ_T：大腿与竖直方向夹角；

θ_S：小腿与大腿延长线之间的夹角。

考虑到四足大型仿生机械恐龙迈腿运作的协调性，本实施例中，设置迈步高度为10cm，即y=10cm，公式(1)和(2)中选择θ_T＝-38°；θ_S＝55°作为抬腿运动参数，则大腿关节的转动角度θ_T满足以下条件：

t=0.00～0.15秒时，θ_T＝-1.39t^2-0.17；

t=0.15～1.17秒时，θ_T＝-0.42t-0.14；

t=1.17～1.32秒时，θ_T＝1.39t²-3.67t+1.75；

t=1.32～1.47秒时，θ_T＝-0.66；

t=1.47～1.98秒时，θ_T＝-0.66；

t=1.98～2.13秒时，θ_T＝1.39t²-5.53t+4.83；

t=2.13～3.15秒时，θ_T＝0.42t-1.52；

t=3.15～3.30秒时，θ_T＝-1.39t²+9.21t-15.37；

小腿关节的转动角度θ_S满足以下条件：

t=0.00～0.15秒时，θ_S＝1.74t²；

t=0.15～1.17秒时，θ_S＝0.52t＋0.31；

t=1.17～1.32秒时，θ_S＝－1.74t²＋4.59t－2.06；

t=1.32～1.47秒时，θ_S＝－1.74t²＋4.59t－2.06；

t=1.47～1.98秒时，θ_S＝0.52t＋1.69；

t=1.98～2.13秒时，θ_S＝0.52t＋1.69；

t=2.13～3.15秒时，θ_S＝0.52t＋1.69；

t=3.15～3.30秒时，θ_S＝1.74t²－11.51t＋18.99；

θ_T、θ_S的单位为弧度。

根据以上条件，可得到精确的足底轨迹运动曲线，如图4所示足底的运动曲线由C1、C2、…C8共8段组成。

另外，在一个前进爬行步态周期内髋关节的转动角度θ_H满足以下条件：

t=0.97～1.67秒时，θ_H＝－0.17t²＋0.34t－0.16；

t=1.67～2.38秒时，θ_H＝0.17t²－0.83t＋0.81；

t=8.98～9.98秒时，θ_H＝0.17t²－3.13t＋13.89；

t=9.98～10.98秒时，θ_H＝－0.17t²＋3.93t－20.86；

t=17.58～18.58秒时，θ_H＝－0.52t＋1.69；

t=18.58～19.58秒时，θ_H＝-0.52t＋1.69；

θ_H的单位为弧度。

本发明中还给出了颈部摆动角θ_n、尾部摆动角θ_m以及髋关节倾角θ_H的极限值确定方法。

颈部摆动角θ_n与尾部摆角θ_m的极限值确定方法如下：

对于颈部摆动角θ_n与质心的关系，可转换成颈部质心对两对角支撑足底连线的力矩来分析，仿生机械恐龙的颈部由三个质量块mS、mN、mJ组成，即颈部上下摆动部件，颈部左右摆动部件和嘴巴部件，因此，颈部总质量：

m_neck＝m_S+m_N+m_J；公式(17)

按照坐标转换公式可得颈部三个坐标系到固定坐标系G的转换矩阵。颈部上下部件质心在S坐标系下的坐标的向量表示为式(18)，在G坐标系下的表示为式(19)，颈部质心在固定坐标系下的表示为式(20)(21)(22)。

^Sp_s＝[^Sx_s,^Sx_s,^Sx_s,]^T；公式(18)

^Gp_s＝^GT×^Sp_s；(19)

$x_{\mathrm{neck}}^G=\frac{x_s^G\×m_s+x_N^G\×m_N+x_J^G\×m_J}{m_{\mathrm{neck}}};$ 公式(20)

$y_{\mathrm{neck}}^G=\frac{y_s^G\×m_s+y_N^G\×m_N+y_J^G\×m_J}{m_{\mathrm{neck}}};$ 公式(21)

$z_{\mathrm{neck}}^G=\frac{z_s^G\×m_s+z_N^G\×m_N+z_J^G\×m_J}{m_{\mathrm{neck}}};$ 公式(22)

假设由于颈部重力作用仿生机械恐龙会倾覆，则倾覆时必然以对角支撑足连线为轴翻转，即颈部重力对两对角支撑足底连线的力矩最大，由此关系可知，颈部摆动角度θ_n最佳值为颈部重力对两对角支撑足底连线的力矩最大时所对应的摆角，即颈部质心在支撑平面投影N’到对角支撑足连线的距离d最大时的摆角。由此计算得到左前腿质心到足底线距离的为式(23)，左前腿质心到足底线距离的导数为式(24)，d取得最大值时颈部摆角为式(25)。

d＝995sin(θ_H)+364sin(θ_n)+290cos(θ_n)+780；公式(23)

d＝364cos(θ_n)-290sin(θ_n)；公式(24)

θ_n＝arctan(644/533)；公式(25)

θ_H为髋关节的倾角。

无论髋关节倾角θ_H取何值，d都会在arctan(644/533)时取得极大值，因此为取得调整质心的最佳效果，头部摆角在左倾时取值约为51°，而在右倾时摆动角越大越好，为取得行走动作的协调性在左倾时摆角也取为同值51°。尾部摆动角θ_m与此相似，取值也为51°。

髋关节倾角θ_H的确定方法如下：

髋关节倾角θ_H的选取主要考虑其与重心的关系。研究髋关节倾角θ_H与质心的关系是为了找出在调整重心的运动过程中，确保重心在稳定范围内髋关节倾角θ_H的取值范围。

图6示出了髋关节侧倾时整体质心的两极限位置Lim1、Lim2，θ_H＝Lim1时，整体质心落在三个立足点形成的支撑三角形的斜边上，θ_H＝Lim2时，整体质心落在三个立足点形成的支撑三角形的右边上，依据上面两个边界条件可求得θ_H的两个边界值，在求得Lim1时需找出迈腿过程中，腿部质量对支撑三角形斜边最大力矩时腿部的位姿状态，参照头部质心对支撑斜边线的最大力矩方法计算。此过程通过在Matlab中编程计算，经过多次取θ_H值，d在t＝2.05s取得最大值，大腿摆角为-37.6430°，小腿摆角为35.2480°，继而可得lim1=-6.191536295°，lim2=-30.2715°。

本发明不局限于上述最佳实施方式，任何人应该得知在本发明的启示下作出的结构变化，凡是与本发明具有相同或相近的技术方案，均落入本发明的保护范围之内。

Claims (4)

1.仿生机械恐龙爬行稳定性控制方法，其特征在于，包括以下步骤：

将仿生机械恐龙的各肢体组成部分视为刚体连杆，并简化为按一定装配和约束关系组合而成的质点系；

为每个刚体连杆建立D-H坐标系，利用4×4的齐次变换矩阵描述相邻两刚体连杆的空间关系，再依次变换求得足底相对于机体坐标系的位姿，以及机体坐标系B与地面坐标系G之间的转换矩阵

将一个前进爬行步态周期的步态规划如下：下蹲；身体右倾并向右摆尾；迈左前腿；迈左后腿；身体左倾并向左摆尾；迈右后腿；迈右前腿；身体右倾；

根据大腿关节、小腿关节以及髋关节的运动函数，利用机体坐标系B与地面坐标系G之间的转换矩阵实时计算各肢体组成部分的重心，并获得仿生机械恐龙的整体质心位置，确保其垂直投影落在由立足点所构成的多边形区域内；

在前进爬行步态中，任一条腿的足底运动轨迹满足以下条件：

x＝-L_Tsin(θ_T)-Lssin(θ_T+θ_S)；

y＝-L_Tcos(θ_T)-Lscos(θ_T+θ_S)；

式中：

L_T：大腿长度；

Ls：小腿长度；

θ_T：大腿与竖直方向的夹角；

θ_S：小腿与大腿延长线之间的夹角；

任一条腿的迈腿过程中，

大腿关节的转动角度θ_T满足以下条件：

t＝0.00～0.15秒时，θ_T＝-1.39t^2-0.17；

t＝0.15～1.17秒时，θ_T＝-0.42t-0.14；

t＝1.17～1.32秒时，θ_T＝1.39t²-3.67t+1.75；

t＝1.32～1.47秒时，θ_T＝-0.66；

t＝1.47～1.98秒时，θ_T＝-0.66；

t＝1.98～2.13秒时，θ_T＝1.39t²-5.53t+4.83；

t＝2.13～3.15秒时，θ_T＝0.42t-1.52；

t＝3.15～3.30秒时，θ_T＝-1.39t²+9.21t-15.37；

小腿关节的转动角度θ_S满足以下条件：

t＝0.00～0.15秒时，θ_S＝1.74t²；

t＝0.15～1.17秒时，θ_S＝0.52t+0.31；

t＝1.17～1.32秒时，θ_S＝－1.74t²+4.59t－2.06；

t＝1.32～1.47秒时，θ_S＝－1.74t²+4.59t－2.06；

t＝1.47～1.98秒时，θ_S＝0.52t+1.69；

t＝1.98～2.13秒时，θ_S＝0.52t+1.69；

t＝2.13～3.15秒时，θ_S＝0.52t+1.69；

t＝3.15～3.30秒时，θ_S＝1.74t²－11.51t+18.99；

θ_T、θ_S的单位为弧度；

在一个前进爬行步态周期内髋关节的转动角度θ_H满足以下条件：

t＝0.97～1.67秒时，θ_H＝－0.17t²+0.34t－0.16；

t＝1.67～2.38秒时，θ_H＝0.17t²－0.83t+0.81；

t＝8.98～9.98秒时，θ_H＝0.17t²－3.13t+13.89；

t＝9.98～10.98秒时，θ_H＝－0.17t²+3.93t－20.86；

t＝17.58～18.58秒时，θ_H＝－0.52t+1.69；

t＝18.58～19.58秒时，θ_H＝－0.52t+1.69；

θ_H的单位为弧度。

颈部摆动角度θ_n的最佳值为颈部质心在立足点支撑平面的投影到对角支撑足连线的距离最大时的摆角，尾部摆动角度θ_m最佳值为尾部质心在立足点支撑平面的投影到对角支撑足连线的距离最大时的摆角。

2.如权利要求1所述的仿生机械恐龙爬行稳定性控制方法，其特征在于，机体坐标系B与地面坐标系G之间的转换矩阵为：

式中：

Gx_BO＝[L_Tcos(θ_T)+Lscos(θ_T+θ_S)]sin(θ_H)；

Gy_BO＝-L_Tsin(θ_T)-Lssin(θ_T+θ_S)；

Gz_BO＝Δ_Z1-Δ_Z2；

Δ_Z1＝L_T[1-cos(θ_T)]+Ls[1-cos(θ_T+θ_S)]；

Δ_Z2＝L[1-cos(θ_H)]；

L＝L_H+L_T+Ls；

L_T：大腿长度；

Ls：小腿长度；

L_H：髋关节回转中心到大腿关节回转中心的距离；

θ_T：大腿与竖直方向的夹角；

θ_S：小腿与大腿延长线之间的夹角。

3.如权利要求1所述的仿生机械恐龙爬行稳定性控制方法，其特征在于，θ_n＝θ_m＝51°。

4.如权利要求1所述的仿生机械恐龙爬行稳定性控制方法，其特征在于，髋关节倾角θ_H介于lim1和lim2之间，θ_H＝Lim1时，整体质心落在三个立足点形成的支撑三角形的斜边上，θ_H＝Lim2时，整体质心落在三个立足点形成的支撑三角形的右边上，lim1＝-6.191536295°，lim2＝-30.2715°。