CN110083075B - 一种拖挂式自行车的稳定裕度估算方法以及控制方法 - Google Patents

Abstract

本发明公开一种拖挂式自行车的稳定裕度估算方法以及控制方法，设计了动态估算稳定裕度的方法，利用姿态检测数据计算ZMP以及三个接地点在大地坐标系下的坐标，经ZMP做各边平行线，在接地三角形内平行线与对应的边形成三个最大梯形，通过计算三个最大梯形的面积与当前接地三角形面积之比，便可获得动态下每条边的稳定裕度，并以动态下每条边的稳定裕度作为平衡控制的数据基础，该动态估算稳定裕度的方法，能实时估算动态运行下接地三角形每条边的稳定裕度，并以动态的接地三角形每条边的稳定裕度为数据基础，实时地、动态地、量化地控制拖挂式自行车的平衡，在估算和控制上更具针对性，也更准确。

Description

一种拖挂式自行车的稳定裕度估算方法以及控制方法

技术领域

本发明涉及自平衡自行车技术领域，具体涉及一种拖挂式自行车的稳定裕度估算方法以及控制方法。

背景技术

拖挂式自行车是由双轮牵引车和单轮拖挂车构成的新型自平衡自行车道路行走机构，其刚体结构主要包括牵引车车架、车把、牵引车前轮、牵引车后轮、拖挂车车架和拖挂车车轮。拖挂式自行车配置的是三个接地车轮，是一种三个接地点的移动机械***。对于这种动态***，稳定裕度是其首先需要考虑的问题。

现有技术中，拖挂式自行车的稳定裕度的估算方法，通常是将三个接地点(如图7中的P₁、P₂、P₃)连接形成的三角形里内接一个圆，再用该圆的圆心到零力矩点(ZMP)的距离来评判拖挂式自行车的稳定裕度，其稳定裕度计算的几何图如图7所示。该方法可以综合地、整体性地评判拖挂式自行车的稳定程度，但是估算出的稳定裕度误差较大，因而也不能针对性地进行稳定性评判，准确度相对较低，也会影响后续控制的准确性。

发明内容

本发明提供一种拖挂式自行车的稳定裕度估算方法以及控制方法，解决现有技术存在的稳定裕度估算不够准确以及控制不够准确的问题。

本发明通过以下技术方案解决技术问题：

一种拖挂式自行车的稳定裕度估算方法，包括动态稳定裕度的估算，所述动态稳定裕度的估算步骤为：

(1)利用所述拖挂式自行车的姿态检测数据计算出ZMP在大地坐标系下的坐标以及所述拖挂式自行车与地面接触的三个接地点在大地坐标系下的坐标；

(2)将所述三个接地点连线，得到动态接地三角形；

(3)过ZMP做所述动态接地三角形第一条边的平行线，得到第一平行线，在所述动态接地三角形范围内，所述第一平行线与所述第一条边形成的最大梯形为第一梯形；过ZMP做所述动态接地三角形第二条边的平行线，得到第二平行线，在所述动态接地三角形范围内，所述第二平行线与所述第二条边形成的最大梯形为第二梯形；过ZMP做所述动态接地三角形第三条边的平行线，得到第三平行线，在所述动态接地三角形范围内，所述第三平行线与所述第三条边形成的最大梯形为第三梯形；

(4)所述动态接地三角形第一条边的稳定裕度为所述第一梯形的面积与当前动态接地三角形的面积之比，得到第一条边的动态稳定裕度；所述动态接地三角形第二条边的稳定裕度为所述第二梯形的面积与当前动态接地三角形的面积之比，得到第二条边的动态稳定裕度；所述动态接地三角形第三条边的稳定裕度为所述第三梯形的面积与当前动态接地三角形的面积之比，得到第三条边的动态稳定裕度。

进一步地，在所述步骤(1)中，所述姿态检测数据包括所述拖挂式自行车的牵引车车架的三个欧拉角度和对应的三个欧拉角速度、所述拖挂式自行车的车把绕牵引车车架的转角和角速度、所述拖挂式自行车的牵引车前轮绕车把的角速度、所述拖挂式自行车的牵引车后轮绕牵引车车架的角速度、所述拖挂式自行车的拖挂车车架绕牵引车车架左右转动的转角和角速度、所述拖挂式自行车的拖挂车车架绕牵引车车架上下转动的转角和角速度、所述拖挂式自行车的拖挂车车轮绕拖挂车车架转动的角速度。

进一步地，步骤(1)中所述ZMP在大地坐标系下的坐标满足公式：

式中:M为所述拖挂式自行车的总质量；g为重力加速度；x_c、y_c为所述拖挂式自行车的质心在大地坐标系下的坐标；P_x、P_y、P_z为所述拖挂式自行车的线动量分别在大地坐标系的x、y、z轴上的分量，

为P_x的一阶导，

为P_y的一阶导，

为P_z的一阶导；L_x、L_y为绕大地坐标系圆心转动的角动量在x、y轴的分量，

为L_x的一阶导，

为L_y的一阶导；x_zmp、y_zmp、z_zmp为ZMP在大地坐标系下的坐标，所述拖挂式自行车在平地上运动时z_zmp＝0。

进一步地，步骤(1)中，所述拖挂式自行车与地面接触的三个接地点分别为第一接地点、第二接地点和第三接地点，所述第一接地点、第二接地点和第三接地点在大地坐标系下的坐标计算步骤为：

1)通过所述牵引车车架的三个欧拉角速度计算出所述牵引车车架在大地坐标系下的角速度；

2)通过所述牵引车车架的角速度计算出牵引车后轮在大地坐标系下的角速度；

3)通过所述牵引车后轮的角速度计算出牵引车后轮所在坐标系的原点在大地坐标系下的线速度；

4)对所述线速度进行积分运算，得到所述牵引车后轮所在坐标系的原点在大地坐标系下的坐标；

5)通过所述牵引车后轮所在坐标系的原点在大地坐标系下的坐标以及坐标变换，计算出所述第一接地点、第二接地点以及拖挂车车架所在坐标系的原点在大地坐标系下的坐标；

6)通过拖挂车车架所在坐标系的原点在大地坐标系下的坐标计算出第三接地点在大地坐标系下的坐标。

进一步地，还包括静态稳定裕度的估算；所述静态稳定裕度的估算步骤为：静态下的拖挂式自行车与地面接触的三个接地点形成静态接地三角形，所述静态接地三角形的面积与这三个接地点能形成的最大三角形的面积之比，得到静态稳定裕度。

一种基于上述拖挂式自行车的稳定裕度估算方法的控制方法，包括PID控制器；所述PID控制器以

作为误差值进行PID运算，输出电流，所述电流输入至所述拖挂式自行车车把的电机，以控制所述拖挂式自行车的平衡，其中，i＝1、2、3，K₁为第一条边的动态稳定裕度，K₂为第二条边的动态稳定裕度，K₃为第三条边的动态稳定裕度。

进一步地，所述PID运算表达式为：

式中，I为所述拖挂式自行车车把的电机电流，K_Pi、K_Di、K_Ii为PID调节参数，

为δ_i的一阶导。

与现有技术相比，具有如下特点：

1、设计了动态估算稳定裕度的方法，利用姿态检测数据计算ZMP以及三个接地点在大地坐标系下的坐标，经ZMP做各边平行线，在接地三角形内平行线与对应的边形成三个最大梯形，通过计算三个最大梯形的面积与当前接地三角形面积之比，便可获得动态下每条边的稳定裕度，并以动态下每条边的稳定裕度作为平衡控制的数据基础，该动态估算稳定裕度的方法，能实时估算动态运行下接地三角形每条边的稳定裕度，并以动态的接地三角形每条边的稳定裕度为数据基础，实时地、动态地、量化地控制拖挂式自行车的平衡，在估算和控制上更具针对性，也更准确；

2、动态估算稳定裕度时，计算ZMP和三个接地点的数据基础包括各个刚体动态运行下的姿态，涉及欧拉角度、欧拉角速度、转角以及角速度等，以上述姿态数据作为估算的数据基础，能充分反映拖挂式行车的动态运行状态，能保证稳定裕度估算的准确性；

3、对拖挂式自行车进行平衡控制时，以与接地三角形每条边的稳定裕度相关的误差值作为PID运算的输入，以车把电机电流作为输出，通过车把的运动改变接地三角形每条边的稳定裕度，进而控制拖挂式自行车的平衡，相当于将估算和控制形成闭环结构，实时地、准确地、动态地调整拖挂式自行车的平衡，使得控制更为准确。

附图说明

图1为本发明的稳定裕度的动态估算以及控制的流程图。

图2为PID控制器的流程图。

图3为构建S₁区域的几何图。

图4为构建S₂区域的几何图。

图5为构建S₃区域的几何图。

图6为坐标映射计算链式图。

图7为现有技术稳定裕度估算的几何图。

图8为现有技术拖挂式自行车的机械结构图。

图中标号为：1、牵引车车架；2、车把、3、牵引车前轮；4、牵引车后轮；5、拖挂车车架；6、拖挂车车轮；7、大地。

具体实施方式

以下结合实施例对本发明作进一步说明，但本发明并不局限于这些实施例。

拖挂式自行车包括如下刚体：牵引车车架1、车把2、牵引车前轮3、牵引车后轮4、拖挂车车架5和拖挂车车轮6。在车把2上安装有电机，电机通过齿轮传动机构与牵引车前轮3相配合，驱动牵引车前轮3转动；在牵引车车架1上安装有电机，该电机通过齿轮传动机构与车把2相配合，驱动车把2转动。牵引车车架1和拖挂车车架5是通过十字轴组件连接，拖挂车车架5可以绕牵引车车架1上下转动以及左右转动。拖挂式自行车的机械结构图如图8所示。

在牵引车车架1上安装陀螺仪，检测牵引车车架1的三个欧拉角度以及对应的三个欧拉角速度；在牵引车车架1上安装增量编码器和绝对编码器，检测车把2绕牵引车车架1的转角和角速度；在车把2上安装增量编码器，检测牵引车前轮3绕车把2的角速度；在牵引车车架1上设置增量编码器，检测牵引车后轮4绕牵引车车架1的角速度；在牵引车车架1上安装2个增量编码器和2个绝对编码器，检测拖挂车车架5绕牵引车车架1左右转动的转角和角速度，以及拖挂车车架5绕牵引车车架1上下转动的转角和角速度；在拖挂车车架5上安装增量编码器，检测拖挂车车轮6绕拖挂车车架5转动的角速度。

针对上述拖挂式自行车的机械结构，提出其稳定裕度的估算方法，包括动态稳定裕度的估算，所述动态稳定裕度的估算过程如图1所示，具体步骤为：

(1)利用所述拖挂式自行车的姿态检测数据计算出ZMP在大地坐标系下的坐标以及所述拖挂式自行车与地面接触的三个接地点在大地坐标系下的坐标；

(2)将所述三个接地点连线，得到动态接地三角形；

(3)过ZMP做所述动态接地三角形第一条边的平行线，得到第一平行线，在所述动态接地三角形范围内，所述第一平行线与所述第一条边形成的最大梯形为第一梯形，如图3中的S₁；过ZMP做所述动态接地三角形第二条边的平行线，得到第二平行线，在所述动态接地三角形范围内，所述第二平行线与所述第二条边形成的最大梯形为第二梯形，如图4中的S₂；过ZMP做所述动态接地三角形第三条边的平行线，得到第三平行线，在所述动态接地三角形范围内，所述第三平行线与所述第三条边形成的最大梯形为第三梯形，如图5中的S₃；

(4)所述动态接地三角形第一条边的稳定裕度为所述第一梯形的面积与当前动态接地三角形的面积之比，得到第一条边的动态稳定裕度；所述动态接地三角形第二条边的稳定裕度为所述第二梯形的面积与当前动态接地三角形的面积之比，得到第二条边的动态稳定裕度；所述动态接地三角形第三条边的稳定裕度为所述第三梯形的面积与当前动态接地三角形的面积之比，得到第三条边的动态稳定裕度。

在所述步骤(1)中，所述姿态检测数据包括上述陀螺仪、增量编码器以及绝对编码器的检测数据，具体包括：所述拖挂式自行车的牵引车车架1的三个欧拉角度q₁、q₂、q₃，对应的三个欧拉角速度

所述拖挂式自行车的车把2绕牵引车车架1的转角和角速度

所述拖挂式自行车的牵引车前轮3绕车把2的角速度

所述拖挂式自行车的牵引车后轮4绕牵引车车架1的角速度

所述拖挂式自行车的拖挂车车架5绕牵引车车架1左右转动的转角和角速度

所述拖挂式自行车的拖挂车车架5绕牵引车车架1上下转动的转角和角速度

所述拖挂式自行车的拖挂车车轮6绕拖挂车车架5转动的角速度

步骤(1)中，计算ZMP在大地7坐标系下的坐标的方法为：

ZMP在大地7坐标系下的坐标满足公式：

式中:M为所述拖挂式自行车的总质量；g为重力加速度；x_c、y_c为所述拖挂式自行车的质心在大地坐标系下的坐标；P_x、P_y、P_z为所述拖挂式自行车的线动量分别在大地坐标系的x、y、z轴上的分量，P_x、P_y、P_z为关于各个刚体线速度的函数，而各个刚体线速度由相应的检测到的角速度计算而得；

为P_x的一阶导，

为P_y的一阶导，

为P_z的一阶导；L_x、L_y为绕大地坐标系圆心转动的角动量在x、y轴的分量，L_x、L_y为角动量，为关于各个刚体角速度的函数，

为L_x的一阶导，

为L_y的一阶导；x_zmp、y_zmp、z_zmp为ZMP在大地坐标系下的坐标，所述拖挂式自行车在平地上运动时z_zmp＝0。

本发明涉及的坐标系有7个：固定在大地7上的全局坐标系，即大地7坐标系，坐标原点为O₀；牵引车车架1的坐标系，且坐标原点在牵引车前轮3的几何中心处，坐标原点为O₁；车把2的坐标系，且坐标原点在牵引车车架1与车把2轴线的交点处，坐标原点为O₂；牵引车前轮3的坐标系，且坐标原点在牵引车前轮,3的几何中心处；坐标原点为O₃；牵引车后轮4的坐标系，且坐标原点在牵引车后轮4的几何中心处，坐标原点为O₄；拖挂车车架5的坐标系，且坐标原点在十字轴的几何中心处，坐标原点为O₅；拖挂车车轮6的坐标系，且坐标原点在,拖挂车车轮6的几何中心处，坐标原点为O₆。

各个检测到的角速度均是在其安装位置所在的坐标系下进行的检测，使用角速度数据时，均将其进行坐标转换，换算为大地7坐标系下的角速度。因此，检测车把2绕牵引车车架1的转角、拖挂车车架5绕牵引车车架1左右转动的转角以及拖挂车车架5绕牵引车车架1上下转动的转角用于获取各个转角所在的坐标系与大地7坐标系之间的旋转矩阵，利用旋转矩阵将相应的角速度换算到大地7坐标系下的角速度，以便于获取大地7坐标系下的线速度，进而获取P_x、P_y、P_z、L_x、L_y的值，进而求取ZMP在大地7坐标系下的坐标。

步骤(1)中，所述拖挂式自行车与地面接触的三个接地点分别为第一接地点、第二接地点和第三接地点，即图8中的P1、P2、P3，所述第一接地点、第二接地点和第三接地点在大地坐标系下的坐标计算步骤为：

1)通过所述牵引车车架1的三个欧拉角速度计算出所述牵引车车架1在大地7坐标系O₀下的角速度；

2)通过所述牵引车车架1的角速度计算出牵引车后轮4在大地7坐标系O₀下的角速度；

3)通过所述牵引车后轮4的角速度计算出牵引车后轮4所在坐标系的原点O₄在大地7坐标系下的线速度；

4)对所述线速度进行积分运算，得到所述牵引车后轮4所在坐标系的原点O₄在大地7坐标系下的坐标；

5)通过所述牵引车后轮4所在坐标系的原点O₄在大地7坐标系下的坐标以及坐标变换，计算出所述第一接地点、第二接地点以及拖挂车车架5所在坐标系的原点O₅在大地7坐标系下的坐标；

6)通过拖挂车车架5所在坐标系的原点O₅在大地7坐标系下的坐标计算出第三接地点在大地7坐标系下的坐标。

步骤1)中，牵引车车架1在大地7坐标系下的角速度的表达式为：

式中，ω_B1为牵引车车架1在大地7坐标系下的角速度，s₁＝sin(q₁)，s₂＝sin(q₂)，c₁＝cos(q₁)，c₂＝cos(q₂)，c₃＝cos(q₃)。

步骤2)中，牵引车后轮4在大地7坐标系下的角速度表达式为：

式中，ω_B4牵引车后轮4在大地7坐标系下的角速度，s₁＝sin(q₁)，s₂＝sin(q₂)，c₁＝cos(q₁)，c₂＝cos(q₂)。

步骤3)中，牵引车后轮4所在坐标系的原点O₄在大地7坐标系下的线速度的表达式为：

式中，v_B4表示牵引车后轮4所在坐标系的原点O₄在大地7坐标系下的线速度，s₁＝sin(q₁)，s₂＝sin(q₂)，c₁＝cos(q₁)，c₂＝cos(q₂)，r为牵引车后轮4的半径。

步骤4)中，牵引车后轮4所在坐标系的原点O₄在大地7坐标系下的坐标表达式为：

式中，s₁＝sin(q₁)，s₂＝sin(q₂)，c₁＝cos(q₁)，c₂＝cos(q₂)，r为牵引车后轮4的半径。

步骤5)中，计算第一接地点P1在大地7坐标系下的坐标：在大地7坐标系下

可通过牵引车后轮4所在坐标系的向量

即[0 0 -l₁]^T旋转变换得到，其中l₁为O₄到O₃的距离。在大地7坐标系下

可通过牵引车前轮3所在坐标系的向量

即[0 0 -r]^T旋转变换得到，在大地7坐标系下

便可求出第一接地点P1在大地7坐标系下的坐标。

步骤5)中，计算第二接地点P2在大地7坐标系下的坐标：在大地7坐标系下得到向量

在大地7坐标系下

可通过牵引车后轮4所在坐标系的向量

即[0 0 -r]^T旋转变换得到，在大地7坐标系下，

便可求出第二接地点，即图8中的第二接地点P2在大地7坐标系下的坐标；

步骤5)中，计算第二接地点O₅在大地7坐标系下的坐标：通过牵引车后轮4所在坐标系的向量

即[-l_x1 0 l_z1]^T旋转变换得到，其中l_x1为O₄到O₅的沿牵引车后轮4所在坐标系x轴方向的距离，l_z为O₄到O₅的沿牵引车后轮4所在坐标系z轴方向的距离，在大地7坐标系下，

便可求出O₅点在大地7坐标系下的坐标。

步骤6)中，通过拖挂车车架5所在坐标系的向量

即[-l_x2 0 -l_z2]^T旋转变换得到，其中l_x2为O₅到O₆的沿拖挂车车架5所在坐标系x轴方向的距离，l_z为O₅到O₆的沿拖挂车车架5所在坐标系z轴方向的距离；在大地7坐标系下

可通过拖挂车车轮6所在坐标系的向量

即[0 0 -r]^T旋转变换得到，在大地7坐标系下，

便便可求出第三接地点在大地7坐标系下的坐标。

上述步骤5)和步骤6)涉及相关的坐标映射计算链式图如图6所示。

在步骤(3)中，三个梯形面积的计算方法相同，以S₁为例，具体的计算步骤为：

1)在大地7坐标系中，P1、P2、P3、ZMP的坐标可分别表示为P₁(x₁,y₁)、P₂(x₂,y₂)、P₃(x₃,y₃)、ZMP(x_zmp,y_zmp)，上述各坐标已经在步骤(1)中计算出，于是直线P₁P₂的方程为

直线P₂P₃的方程为

直线P₃P₁的方程为

过ZMP(x_zmp,y_zmp)点作P₁P₂平行线，即第一平行线，第一平行线的方程为

2)第一平行线与直线P₂P₃和直线P₃P₁分别有1个交点，这两个交点之间的可求出为d₁；通过点ZMP到直线的距离公式可求出ZMP(x_zmp,y_zmp)到直线P₁P₂的距离为d₂；通过已知的P₁(x₁,y₁)，P₂(x₂,y₂)可求出P₁点到P₂点的距离为d₃；于是，

便可求出S₁的面积大小。

S₂、S₃的计算方法与S₁的计算方法相同，S则利用三角形面积求法求得。拖挂式自行车相对于接地三角形的每条边的动态稳定裕度可表示为：

其中i＝1,2,3。K_i值越大，说明拖挂式自行车相对于所对应的边越稳定。

本发明还进行静态稳定裕度的估算，具体方法为：静态下的拖挂式自行车与地面接触的三个接地点形成静态接地三角形，所述静态接地三角形的面积与这三个接地点能形成的最大三角形的面积之比，得到静态稳定裕度。即，K＝S/S_max，其中，K为静态稳定裕度，S为静态接地三角形的面积，S_max为三个接地点所能形成的最大三角形面积。当K＝0时，拖挂式自行车三个车轮在一条直线上，拖挂式自行车稳定程度最差；当K＝1时，静态接地三角形面积最大，拖挂式自行车达到最稳定状态。静态稳定裕度用于综合评判整个拖挂式自行车的稳定程度，无需考虑ZMP在静态接地三角形内的位置。

针对步骤(4)的动态接地三角形的三条边的动态稳定裕度设置了PID控制器，用于控制拖挂式自行车的动态平衡，所述PID控制器以

作为误差值进行PID运算，输出电流，所述电流输入至所述拖挂式自行车车把2的电机，以控制所述拖挂式自行车的平衡。其中，i＝1、2、3，即，K₁为第一条边的动态稳定裕度，K₂为第二条边的动态稳定裕度，K₃为第三条边的动态稳定裕度；等式

说明动态稳定裕度K_i越大，误差δ_i越小。PID控制器的流程图如图2所示。

所述PID运算表达式为：

式中，I为所述拖挂式自行车车把2的电机电流，通过调节I便可达到对车把2的调节，进而实现对拖挂式自行车动态稳定进行控制，K_Pi、K_Di、K_Ii为PID调节参数，

为δ_i的一阶导，i＝1、2、3。K_i的值越大时，K_Pi、K_Di、K_Ii的值越小。例如，当S₁＞S₂＞S₃时，K_P3＞K_P2＞K_P1、K_D3＞K_D2＞K_D1、K_I3＞K_I2＞K_I1，即S_i越大调节越慢，以至于动态接地三角形的三个梯形面积快速的调至大小接近，从而使拖挂式自行车达到最稳定状态。调节K_Pi、K_Di、K_Ii到合适的数值，和通过每一时刻各个传感器测量的数据进行反馈，便可以达到控制拖挂式自行车的稳定的目的。PID控制器的流程图如图2所示。

Claims (4)

1.一种拖挂式自行车的稳定裕度估算方法，其特征在于：

包括动态稳定裕度的估算，所述动态稳定裕度的估算步骤为：

(1)利用所述拖挂式自行车的姿态检测数据计算出ZMP在大地(7)坐标系下的坐标以及所述拖挂式自行车与地面接触的三个接地点在大地(7)坐标系下的坐标；

(2)将所述三个接地点连线，得到动态接地三角形；

(3)过ZMP做所述动态接地三角形第一条边的平行线，得到第一平行线，在所述动态接地三角形范围内，所述第一平行线与所述第一条边形成的最大梯形为第一梯形；过ZMP做所述动态接地三角形第二条边的平行线，得到第二平行线，在所述动态接地三角形范围内，所述第二平行线与所述第二条边形成的最大梯形为第二梯形；过ZMP做所述动态接地三角形第三条边的平行线，得到第三平行线，在所述动态接地三角形范围内，所述第三平行线与所述第三条边形成的最大梯形为第三梯形；

(4)所述动态接地三角形第一条边的稳定裕度为所述第一梯形的面积与当前动态接地三角形的面积之比，得到第一条边的动态稳定裕度；所述动态接地三角形第二条边的稳定裕度为所述第二梯形的面积与当前动态接地三角形的面积之比，得到第二条边的动态稳定裕度；所述动态接地三角形第三条边的稳定裕度为所述第三梯形的面积与当前动态接地三角形的面积之比，得到第三条边的动态稳定裕度；

在所述步骤(1)中，所述姿态检测数据包括所述拖挂式自行车的牵引车车架(1)的三个欧拉角度和对应的三个欧拉角速度、所述拖挂式自行车的车把(2)绕牵引车车架(1)的转角和角速度、所述拖挂式自行车的牵引车前轮(3)绕车把(2)的角速度、所述拖挂式自行车的牵引车后轮(4)绕牵引车车架(1)的角速度、所述拖挂式自行车的拖挂车车架(5)绕牵引车车架(1)左右转动的转角和角速度、所述拖挂式自行车的拖挂车车架(5)绕牵引车车架(1)上下转动的转角和角速度、所述拖挂式自行车的拖挂车车轮(6)绕拖挂车车架(5)转动的角速度；步骤(1)中所述ZMP在大地(7)坐标系下的坐标满足公式：

式中:M为所述拖挂式自行车的总质量；g为重力加速度；x_c、y_c为所述拖挂式自行车的质心在大地(7)坐标系下的坐标；P_x、P_y、P_z为所述拖挂式自行车的线动量分别在大地(7)坐标系的x、y、z轴上的分量，

为P_x的一阶导，

为P_y的一阶导，

为P_z的一阶导；L_x、L_y为绕大地(7)坐标系圆心转动的角动量在x、y轴的分量，

为L_x的一阶导，

为L_y的一阶导；x_zmp、y_zmp、z_zmp为ZMP在大地(7)坐标系下的坐标，所述拖挂式自行车在平地上运动时z_zmp＝0；

步骤(1)中，所述拖挂式自行车与地面接触的三个接地点分别为第一接地点、第二接地点和第三接地点，所述第一接地点、第二接地点和第三接地点在大地(7)坐标系下的坐标计算步骤为：

1)通过所述牵引车车架(1)的三个欧拉角速度计算出所述牵引车车架(1)在大地(7)坐标系下的角速度；

2)通过所述牵引车车架(1)的角速度计算出牵引车后轮(4)在大地(7)坐标系下的角速度；

3)通过所述牵引车后轮(4)的角速度计算出牵引车后轮(4)所在坐标系的原点在大地(7)坐标系下的线速度；

4)对所述线速度进行积分运算，得到所述牵引车后轮(4)所在坐标系的原点在大地(7)坐标系下的坐标；

5)通过所述牵引车后轮(4)所在坐标系的原点在大地(7)坐标系下的坐标以及坐标变换，计算出所述第一接地点、第二接地点以及拖挂车车架(5)所在坐标系的原点在大地(7)坐标系下的坐标；

6)通过拖挂车车架(5)所在坐标系的原点在大地(7)坐标系下的坐标计算出第三接地点在大地(7)坐标系下的坐标。

2.根据权利要求1所述的一种拖挂式自行车的稳定裕度估算方法，其特征在于：

还包括静态稳定裕度的估算；

所述静态稳定裕度的估算步骤为：

静态下的拖挂式自行车与地面接触的三个接地点形成静态接地三角形，所述静态接地三角形的面积与这三个接地点能形成的最大三角形的面积之比，得到静态稳定裕度。

3.根据权利要求1或2所述的一种拖挂式自行车的稳定裕度估算方法，其特征在于：

包括PID控制器；

所述PID控制器以

作为误差值进行PID运算，输出电流，所述电流输入至所述拖挂式自行车车把(2)的电机，以控制所述拖挂式自行车的平衡，其中，i＝1、2、3，K₁为第一条边的动态稳定裕度，K₂为第二条边的动态稳定裕度，K₃为第三条边的动态稳定裕度。

4.根据权利要求3所述的一种拖挂式自行车的稳定裕度估算方法，其特征在于：

所述PID运算表达式为：

式中，I为所述拖挂式自行车车把(2)的电机电流，K_Pi、K_Di、K_Ii为PID调节参数，

为δ_i的一阶导。

Images