CN110083075B - 一种拖挂式自行车的稳定裕度估算方法以及控制方法 - Google Patents
一种拖挂式自行车的稳定裕度估算方法以及控制方法 Download PDFInfo
- Publication number
- CN110083075B CN110083075B CN201910407550.8A CN201910407550A CN110083075B CN 110083075 B CN110083075 B CN 110083075B CN 201910407550 A CN201910407550 A CN 201910407550A CN 110083075 B CN110083075 B CN 110083075B
- Authority
- CN
- China
- Prior art keywords
- grounding
- coordinate system
- trailer
- stability margin
- triangle
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 30
- 238000001514 detection method Methods 0.000 claims abstract description 9
- 230000003068 static effect Effects 0.000 claims description 22
- 238000004364 calculation method Methods 0.000 claims description 9
- 230000009466 transformation Effects 0.000 claims description 8
- 230000005484 gravity Effects 0.000 claims description 3
- 230000001133 acceleration Effects 0.000 claims description 2
- 239000013598 vector Substances 0.000 description 6
- 230000001276 controlling effect Effects 0.000 description 4
- 238000010276 construction Methods 0.000 description 3
- 230000005540 biological transmission Effects 0.000 description 2
- 239000011159 matrix material Substances 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 230000001105 regulatory effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B17/00—Systems involving the use of models or simulators of said systems
- G05B17/02—Systems involving the use of models or simulators of said systems electric
Landscapes
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开一种拖挂式自行车的稳定裕度估算方法以及控制方法,设计了动态估算稳定裕度的方法,利用姿态检测数据计算ZMP以及三个接地点在大地坐标系下的坐标,经ZMP做各边平行线,在接地三角形内平行线与对应的边形成三个最大梯形,通过计算三个最大梯形的面积与当前接地三角形面积之比,便可获得动态下每条边的稳定裕度,并以动态下每条边的稳定裕度作为平衡控制的数据基础,该动态估算稳定裕度的方法,能实时估算动态运行下接地三角形每条边的稳定裕度,并以动态的接地三角形每条边的稳定裕度为数据基础,实时地、动态地、量化地控制拖挂式自行车的平衡,在估算和控制上更具针对性,也更准确。
Description
技术领域
本发明涉及自平衡自行车技术领域,具体涉及一种拖挂式自行车的稳定裕度估算方法以及控制方法。
背景技术
拖挂式自行车是由双轮牵引车和单轮拖挂车构成的新型自平衡自行车道路行走机构,其刚体结构主要包括牵引车车架、车把、牵引车前轮、牵引车后轮、拖挂车车架和拖挂车车轮。拖挂式自行车配置的是三个接地车轮,是一种三个接地点的移动机械***。对于这种动态***,稳定裕度是其首先需要考虑的问题。
现有技术中,拖挂式自行车的稳定裕度的估算方法,通常是将三个接地点(如图7中的P1、P2、P3)连接形成的三角形里内接一个圆,再用该圆的圆心到零力矩点(ZMP)的距离来评判拖挂式自行车的稳定裕度,其稳定裕度计算的几何图如图7所示。该方法可以综合地、整体性地评判拖挂式自行车的稳定程度,但是估算出的稳定裕度误差较大,因而也不能针对性地进行稳定性评判,准确度相对较低,也会影响后续控制的准确性。
发明内容
本发明提供一种拖挂式自行车的稳定裕度估算方法以及控制方法,解决现有技术存在的稳定裕度估算不够准确以及控制不够准确的问题。
本发明通过以下技术方案解决技术问题:
一种拖挂式自行车的稳定裕度估算方法,包括动态稳定裕度的估算,所述动态稳定裕度的估算步骤为:
(1)利用所述拖挂式自行车的姿态检测数据计算出ZMP在大地坐标系下的坐标以及所述拖挂式自行车与地面接触的三个接地点在大地坐标系下的坐标;
(2)将所述三个接地点连线,得到动态接地三角形;
(3)过ZMP做所述动态接地三角形第一条边的平行线,得到第一平行线,在所述动态接地三角形范围内,所述第一平行线与所述第一条边形成的最大梯形为第一梯形;过ZMP做所述动态接地三角形第二条边的平行线,得到第二平行线,在所述动态接地三角形范围内,所述第二平行线与所述第二条边形成的最大梯形为第二梯形;过ZMP做所述动态接地三角形第三条边的平行线,得到第三平行线,在所述动态接地三角形范围内,所述第三平行线与所述第三条边形成的最大梯形为第三梯形;
(4)所述动态接地三角形第一条边的稳定裕度为所述第一梯形的面积与当前动态接地三角形的面积之比,得到第一条边的动态稳定裕度;所述动态接地三角形第二条边的稳定裕度为所述第二梯形的面积与当前动态接地三角形的面积之比,得到第二条边的动态稳定裕度;所述动态接地三角形第三条边的稳定裕度为所述第三梯形的面积与当前动态接地三角形的面积之比,得到第三条边的动态稳定裕度。
进一步地,在所述步骤(1)中,所述姿态检测数据包括所述拖挂式自行车的牵引车车架的三个欧拉角度和对应的三个欧拉角速度、所述拖挂式自行车的车把绕牵引车车架的转角和角速度、所述拖挂式自行车的牵引车前轮绕车把的角速度、所述拖挂式自行车的牵引车后轮绕牵引车车架的角速度、所述拖挂式自行车的拖挂车车架绕牵引车车架左右转动的转角和角速度、所述拖挂式自行车的拖挂车车架绕牵引车车架上下转动的转角和角速度、所述拖挂式自行车的拖挂车车轮绕拖挂车车架转动的角速度。
进一步地,步骤(1)中所述ZMP在大地坐标系下的坐标满足公式:
式中:M为所述拖挂式自行车的总质量;g为重力加速度;xc、yc为所述拖挂式自行车的质心在大地坐标系下的坐标;Px、Py、Pz为所述拖挂式自行车的线动量分别在大地坐标系的x、y、z轴上的分量,为Px的一阶导,为Py的一阶导,为Pz的一阶导;Lx、Ly为绕大地坐标系圆心转动的角动量在x、y轴的分量,为Lx的一阶导,为Ly的一阶导;xzmp、yzmp、zzmp为ZMP在大地坐标系下的坐标,所述拖挂式自行车在平地上运动时zzmp=0。
进一步地,步骤(1)中,所述拖挂式自行车与地面接触的三个接地点分别为第一接地点、第二接地点和第三接地点,所述第一接地点、第二接地点和第三接地点在大地坐标系下的坐标计算步骤为:
1)通过所述牵引车车架的三个欧拉角速度计算出所述牵引车车架在大地坐标系下的角速度;
2)通过所述牵引车车架的角速度计算出牵引车后轮在大地坐标系下的角速度;
3)通过所述牵引车后轮的角速度计算出牵引车后轮所在坐标系的原点在大地坐标系下的线速度;
4)对所述线速度进行积分运算,得到所述牵引车后轮所在坐标系的原点在大地坐标系下的坐标;
5)通过所述牵引车后轮所在坐标系的原点在大地坐标系下的坐标以及坐标变换,计算出所述第一接地点、第二接地点以及拖挂车车架所在坐标系的原点在大地坐标系下的坐标;
6)通过拖挂车车架所在坐标系的原点在大地坐标系下的坐标计算出第三接地点在大地坐标系下的坐标。
进一步地,还包括静态稳定裕度的估算;所述静态稳定裕度的估算步骤为:静态下的拖挂式自行车与地面接触的三个接地点形成静态接地三角形,所述静态接地三角形的面积与这三个接地点能形成的最大三角形的面积之比,得到静态稳定裕度。
一种基于上述拖挂式自行车的稳定裕度估算方法的控制方法,包括PID控制器;所述PID控制器以作为误差值进行PID运算,输出电流,所述电流输入至所述拖挂式自行车车把的电机,以控制所述拖挂式自行车的平衡,其中,i=1、2、3,K1为第一条边的动态稳定裕度,K2为第二条边的动态稳定裕度,K3为第三条边的动态稳定裕度。
进一步地,所述PID运算表达式为:
与现有技术相比,具有如下特点:
1、设计了动态估算稳定裕度的方法,利用姿态检测数据计算ZMP以及三个接地点在大地坐标系下的坐标,经ZMP做各边平行线,在接地三角形内平行线与对应的边形成三个最大梯形,通过计算三个最大梯形的面积与当前接地三角形面积之比,便可获得动态下每条边的稳定裕度,并以动态下每条边的稳定裕度作为平衡控制的数据基础,该动态估算稳定裕度的方法,能实时估算动态运行下接地三角形每条边的稳定裕度,并以动态的接地三角形每条边的稳定裕度为数据基础,实时地、动态地、量化地控制拖挂式自行车的平衡,在估算和控制上更具针对性,也更准确;
2、动态估算稳定裕度时,计算ZMP和三个接地点的数据基础包括各个刚体动态运行下的姿态,涉及欧拉角度、欧拉角速度、转角以及角速度等,以上述姿态数据作为估算的数据基础,能充分反映拖挂式行车的动态运行状态,能保证稳定裕度估算的准确性;
3、对拖挂式自行车进行平衡控制时,以与接地三角形每条边的稳定裕度相关的误差值作为PID运算的输入,以车把电机电流作为输出,通过车把的运动改变接地三角形每条边的稳定裕度,进而控制拖挂式自行车的平衡,相当于将估算和控制形成闭环结构,实时地、准确地、动态地调整拖挂式自行车的平衡,使得控制更为准确。
附图说明
图1为本发明的稳定裕度的动态估算以及控制的流程图。
图2为PID控制器的流程图。
图3为构建S1区域的几何图。
图4为构建S2区域的几何图。
图5为构建S3区域的几何图。
图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中的S1;过ZMP做所述动态接地三角形第二条边的平行线,得到第二平行线,在所述动态接地三角形范围内,所述第二平行线与所述第二条边形成的最大梯形为第二梯形,如图4中的S2;过ZMP做所述动态接地三角形第三条边的平行线,得到第三平行线,在所述动态接地三角形范围内,所述第三平行线与所述第三条边形成的最大梯形为第三梯形,如图5中的S3;
(4)所述动态接地三角形第一条边的稳定裕度为所述第一梯形的面积与当前动态接地三角形的面积之比,得到第一条边的动态稳定裕度;所述动态接地三角形第二条边的稳定裕度为所述第二梯形的面积与当前动态接地三角形的面积之比,得到第二条边的动态稳定裕度;所述动态接地三角形第三条边的稳定裕度为所述第三梯形的面积与当前动态接地三角形的面积之比,得到第三条边的动态稳定裕度。
在所述步骤(1)中,所述姿态检测数据包括上述陀螺仪、增量编码器以及绝对编码器的检测数据,具体包括:所述拖挂式自行车的牵引车车架1的三个欧拉角度q1、q2、q3,对应的三个欧拉角速度所述拖挂式自行车的车把2绕牵引车车架1的转角和角速度所述拖挂式自行车的牵引车前轮3绕车把2的角速度所述拖挂式自行车的牵引车后轮4绕牵引车车架1的角速度所述拖挂式自行车的拖挂车车架5绕牵引车车架1左右转动的转角和角速度所述拖挂式自行车的拖挂车车架5绕牵引车车架1上下转动的转角和角速度所述拖挂式自行车的拖挂车车轮6绕拖挂车车架5转动的角速度
步骤(1)中,计算ZMP在大地7坐标系下的坐标的方法为:
ZMP在大地7坐标系下的坐标满足公式:
式中:M为所述拖挂式自行车的总质量;g为重力加速度;xc、yc为所述拖挂式自行车的质心在大地坐标系下的坐标;Px、Py、Pz为所述拖挂式自行车的线动量分别在大地坐标系的x、y、z轴上的分量,Px、Py、Pz为关于各个刚体线速度的函数,而各个刚体线速度由相应的检测到的角速度计算而得;为Px的一阶导,为Py的一阶导,为Pz的一阶导;Lx、Ly为绕大地坐标系圆心转动的角动量在x、y轴的分量,Lx、Ly为角动量,为关于各个刚体角速度的函数,为Lx的一阶导,为Ly的一阶导;xzmp、yzmp、zzmp为ZMP在大地坐标系下的坐标,所述拖挂式自行车在平地上运动时zzmp=0。
本发明涉及的坐标系有7个:固定在大地7上的全局坐标系,即大地7坐标系,坐标原点为O0;牵引车车架1的坐标系,且坐标原点在牵引车前轮3的几何中心处,坐标原点为O1;车把2的坐标系,且坐标原点在牵引车车架1与车把2轴线的交点处,坐标原点为O2;牵引车前轮3的坐标系,且坐标原点在牵引车前轮,3的几何中心处;坐标原点为O3;牵引车后轮4的坐标系,且坐标原点在牵引车后轮4的几何中心处,坐标原点为O4;拖挂车车架5的坐标系,且坐标原点在十字轴的几何中心处,坐标原点为O5;拖挂车车轮6的坐标系,且坐标原点在,拖挂车车轮6的几何中心处,坐标原点为O6。
各个检测到的角速度均是在其安装位置所在的坐标系下进行的检测,使用角速度数据时,均将其进行坐标转换,换算为大地7坐标系下的角速度。因此,检测车把2绕牵引车车架1的转角、拖挂车车架5绕牵引车车架1左右转动的转角以及拖挂车车架5绕牵引车车架1上下转动的转角用于获取各个转角所在的坐标系与大地7坐标系之间的旋转矩阵,利用旋转矩阵将相应的角速度换算到大地7坐标系下的角速度,以便于获取大地7坐标系下的线速度,进而获取Px、Py、Pz、Lx、Ly的值,进而求取ZMP在大地7坐标系下的坐标。
步骤(1)中,所述拖挂式自行车与地面接触的三个接地点分别为第一接地点、第二接地点和第三接地点,即图8中的P1、P2、P3,所述第一接地点、第二接地点和第三接地点在大地坐标系下的坐标计算步骤为:
1)通过所述牵引车车架1的三个欧拉角速度计算出所述牵引车车架1在大地7坐标系O0下的角速度;
2)通过所述牵引车车架1的角速度计算出牵引车后轮4在大地7坐标系O0下的角速度;
3)通过所述牵引车后轮4的角速度计算出牵引车后轮4所在坐标系的原点O4在大地7坐标系下的线速度;
4)对所述线速度进行积分运算,得到所述牵引车后轮4所在坐标系的原点O4在大地7坐标系下的坐标;
5)通过所述牵引车后轮4所在坐标系的原点O4在大地7坐标系下的坐标以及坐标变换,计算出所述第一接地点、第二接地点以及拖挂车车架5所在坐标系的原点O5在大地7坐标系下的坐标;
6)通过拖挂车车架5所在坐标系的原点O5在大地7坐标系下的坐标计算出第三接地点在大地7坐标系下的坐标。
步骤1)中,牵引车车架1在大地7坐标系下的角速度的表达式为:
步骤2)中,牵引车后轮4在大地7坐标系下的角速度表达式为:
步骤3)中,牵引车后轮4所在坐标系的原点O4在大地7坐标系下的线速度的表达式为:
步骤4)中,牵引车后轮4所在坐标系的原点O4在大地7坐标系下的坐标表达式为:
式中,s1=sin(q1),s2=sin(q2),c1=cos(q1),c2=cos(q2),r为牵引车后轮4的半径。
步骤5)中,计算第一接地点P1在大地7坐标系下的坐标:在大地7坐标系下可通过牵引车后轮4所在坐标系的向量即[0 0 -l1]T旋转变换得到,其中l1为O4到O3的距离。在大地7坐标系下可通过牵引车前轮3所在坐标系的向量即[0 0 -r]T旋转变换得到,在大地7坐标系下便可求出第一接地点P1在大地7坐标系下的坐标。
步骤5)中,计算第二接地点P2在大地7坐标系下的坐标:在大地7坐标系下得到向量在大地7坐标系下可通过牵引车后轮4所在坐标系的向量即[0 0 -r]T旋转变换得到,在大地7坐标系下,便可求出第二接地点,即图8中的第二接地点P2在大地7坐标系下的坐标;
步骤5)中,计算第二接地点O5在大地7坐标系下的坐标:通过牵引车后轮4所在坐标系的向量即[-lx1 0 lz1]T旋转变换得到,其中lx1为O4到O5的沿牵引车后轮4所在坐标系x轴方向的距离,lz为O4到O5的沿牵引车后轮4所在坐标系z轴方向的距离,在大地7坐标系下, 便可求出O5点在大地7坐标系下的坐标。
步骤6)中,通过拖挂车车架5所在坐标系的向量即[-lx2 0 -lz2]T旋转变换得到,其中lx2为O5到O6的沿拖挂车车架5所在坐标系x轴方向的距离,lz为O5到O6的沿拖挂车车架5所在坐标系z轴方向的距离;在大地7坐标系下可通过拖挂车车轮6所在坐标系的向量即[0 0 -r]T旋转变换得到,在大地7坐标系下,便便可求出第三接地点在大地7坐标系下的坐标。
上述步骤5)和步骤6)涉及相关的坐标映射计算链式图如图6所示。
在步骤(3)中,三个梯形面积的计算方法相同,以S1为例,具体的计算步骤为:
1)在大地7坐标系中,P1、P2、P3、ZMP的坐标可分别表示为P1(x1,y1)、P2(x2,y2)、P3(x3,y3)、ZMP(xzmp,yzmp),上述各坐标已经在步骤(1)中计算出,于是直线P1P2的方程为直线P2P3的方程为直线P3P1的方程为过ZMP(xzmp,yzmp)点作P1P2平行线,即第一平行线,第一平行线的方程为
2)第一平行线与直线P2P3和直线P3P1分别有1个交点,这两个交点之间的可求出为d1;通过点ZMP到直线的距离公式可求出ZMP(xzmp,yzmp)到直线P1P2的距离为d2;通过已知的P1(x1,y1),P2(x2,y2)可求出P1点到P2点的距离为d3;于是,便可求出S1的面积大小。
S2、S3的计算方法与S1的计算方法相同,S则利用三角形面积求法求得。拖挂式自行车相对于接地三角形的每条边的动态稳定裕度可表示为:其中i=1,2,3。Ki值越大,说明拖挂式自行车相对于所对应的边越稳定。
本发明还进行静态稳定裕度的估算,具体方法为:静态下的拖挂式自行车与地面接触的三个接地点形成静态接地三角形,所述静态接地三角形的面积与这三个接地点能形成的最大三角形的面积之比,得到静态稳定裕度。即,K=S/Smax,其中,K为静态稳定裕度,S为静态接地三角形的面积,Smax为三个接地点所能形成的最大三角形面积。当K=0时,拖挂式自行车三个车轮在一条直线上,拖挂式自行车稳定程度最差;当K=1时,静态接地三角形面积最大,拖挂式自行车达到最稳定状态。静态稳定裕度用于综合评判整个拖挂式自行车的稳定程度,无需考虑ZMP在静态接地三角形内的位置。
针对步骤(4)的动态接地三角形的三条边的动态稳定裕度设置了PID控制器,用于控制拖挂式自行车的动态平衡,所述PID控制器以作为误差值进行PID运算,输出电流,所述电流输入至所述拖挂式自行车车把2的电机,以控制所述拖挂式自行车的平衡。其中,i=1、2、3,即,K1为第一条边的动态稳定裕度,K2为第二条边的动态稳定裕度,K3为第三条边的动态稳定裕度;等式说明动态稳定裕度Ki越大,误差δi越小。PID控制器的流程图如图2所示。
所述PID运算表达式为:
式中,I为所述拖挂式自行车车把2的电机电流,通过调节I便可达到对车把2的调节,进而实现对拖挂式自行车动态稳定进行控制,KPi、KDi、KIi为PID调节参数,为δi的一阶导,i=1、2、3。Ki的值越大时,KPi、KDi、KIi的值越小。例如,当S1>S2>S3时,KP3>KP2>KP1、KD3>KD2>KD1、KI3>KI2>KI1,即Si越大调节越慢,以至于动态接地三角形的三个梯形面积快速的调至大小接近,从而使拖挂式自行车达到最稳定状态。调节KPi、KDi、KIi到合适的数值,和通过每一时刻各个传感器测量的数据进行反馈,便可以达到控制拖挂式自行车的稳定的目的。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为重力加速度;xc、yc为所述拖挂式自行车的质心在大地(7)坐标系下的坐标;Px、Py、Pz为所述拖挂式自行车的线动量分别在大地(7)坐标系的x、y、z轴上的分量,为Px的一阶导,为Py的一阶导,为Pz的一阶导;Lx、Ly为绕大地(7)坐标系圆心转动的角动量在x、y轴的分量,为Lx的一阶导,为Ly的一阶导;xzmp、yzmp、zzmp为ZMP在大地(7)坐标系下的坐标,所述拖挂式自行车在平地上运动时zzmp=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所述的一种拖挂式自行车的稳定裕度估算方法,其特征在于:
还包括静态稳定裕度的估算;
所述静态稳定裕度的估算步骤为:
静态下的拖挂式自行车与地面接触的三个接地点形成静态接地三角形,所述静态接地三角形的面积与这三个接地点能形成的最大三角形的面积之比,得到静态稳定裕度。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910407550.8A CN110083075B (zh) | 2019-05-15 | 2019-05-15 | 一种拖挂式自行车的稳定裕度估算方法以及控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910407550.8A CN110083075B (zh) | 2019-05-15 | 2019-05-15 | 一种拖挂式自行车的稳定裕度估算方法以及控制方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110083075A CN110083075A (zh) | 2019-08-02 |
CN110083075B true CN110083075B (zh) | 2022-04-08 |
Family
ID=67420441
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910407550.8A Active CN110083075B (zh) | 2019-05-15 | 2019-05-15 | 一种拖挂式自行车的稳定裕度估算方法以及控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110083075B (zh) |
Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH10217161A (ja) * | 1997-01-31 | 1998-08-18 | Honda Motor Co Ltd | 脚式移動ロボットの歩容生成装置 |
JP2002326173A (ja) * | 2001-04-27 | 2002-11-12 | Honda Motor Co Ltd | 脚式移動ロボットの動作生成装置 |
CN101414190A (zh) * | 2008-10-28 | 2009-04-22 | 北京理工大学 | 基于有效稳定区域仿人机器人稳定行走的控制方法和*** |
CN101950176A (zh) * | 2010-09-02 | 2011-01-19 | 北京理工大学 | 一种机器人自主进行zmp标定的方法 |
CN103963782A (zh) * | 2014-05-09 | 2014-08-06 | 济南大学 | 拖挂式移动机器人平行泊车方法 |
CN106354160A (zh) * | 2016-09-20 | 2017-01-25 | 济南大学 | 一种n节拖挂式移动机器人反向运动时的方向角控制方法 |
CN106547206A (zh) * | 2016-06-29 | 2017-03-29 | 中南大学 | 一种基于脚力估计算法的多足机器人动态稳定性定量判定方法 |
CN106737669A (zh) * | 2016-12-12 | 2017-05-31 | 杭州宇芯机器人科技有限公司 | 考虑外力冲击干扰和阻尼的多足机器人能量裕度计算方法 |
CN107168061A (zh) * | 2017-05-31 | 2017-09-15 | 江西制造职业技术学院 | 一种仿人机器人相似性运动的步态规划装置及其规划方法 |
-
2019
- 2019-05-15 CN CN201910407550.8A patent/CN110083075B/zh active Active
Patent Citations (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JPH10217161A (ja) * | 1997-01-31 | 1998-08-18 | Honda Motor Co Ltd | 脚式移動ロボットの歩容生成装置 |
JP2002326173A (ja) * | 2001-04-27 | 2002-11-12 | Honda Motor Co Ltd | 脚式移動ロボットの動作生成装置 |
CN101414190A (zh) * | 2008-10-28 | 2009-04-22 | 北京理工大学 | 基于有效稳定区域仿人机器人稳定行走的控制方法和*** |
CN101950176A (zh) * | 2010-09-02 | 2011-01-19 | 北京理工大学 | 一种机器人自主进行zmp标定的方法 |
CN103963782A (zh) * | 2014-05-09 | 2014-08-06 | 济南大学 | 拖挂式移动机器人平行泊车方法 |
CN106547206A (zh) * | 2016-06-29 | 2017-03-29 | 中南大学 | 一种基于脚力估计算法的多足机器人动态稳定性定量判定方法 |
CN106354160A (zh) * | 2016-09-20 | 2017-01-25 | 济南大学 | 一种n节拖挂式移动机器人反向运动时的方向角控制方法 |
CN106737669A (zh) * | 2016-12-12 | 2017-05-31 | 杭州宇芯机器人科技有限公司 | 考虑外力冲击干扰和阻尼的多足机器人能量裕度计算方法 |
CN107168061A (zh) * | 2017-05-31 | 2017-09-15 | 江西制造职业技术学院 | 一种仿人机器人相似性运动的步态规划装置及其规划方法 |
Non-Patent Citations (1)
Title |
---|
拖挂式自行车机器人机构设计与非完整约束分析;黄用华;《机械设计与研究》;20180630;第34卷(第3期);第54-57、62页 * |
Also Published As
Publication number | Publication date |
---|---|
CN110083075A (zh) | 2019-08-02 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110001637B (zh) | 一种基于多点跟踪的无人驾驶汽车路径跟踪控制装置及控制方法 | |
CN111610780B (zh) | 一种自动驾驶车路径跟踪控制方法及其装置 | |
JP6420173B2 (ja) | 自動走行圃場作業車両 | |
US9244462B2 (en) | Vehicle trajectory planning for autonomous vehicles | |
CN105172793B (zh) | 自动驾驶汽车的位姿估算方法 | |
EP2421742B1 (en) | Steering control device of autonomous vehicle, autonomous vehicle having the same and steering control method of autonomous vehicle | |
CN104002860B (zh) | 电动转向设备 | |
CN107839749B (zh) | 电动轮汽车转向路感及整车稳定性控制方法 | |
CN107901917B (zh) | 一种基于滑转滑移耦合估计的无人驾驶车辆轨迹跟踪控制方法 | |
WO2010032493A1 (ja) | 転倒防止制御装置及びコンピュータプログラム | |
CN110001840B (zh) | 一种基于视觉传感器的多种路况下的两轮自平衡车运动控制方法 | |
JP3430832B2 (ja) | 道路曲率推定装置 | |
JP2000302055A (ja) | 車線追従制御装置 | |
CN110386189A (zh) | 在级联调节时将干扰信号接入至基准量 | |
CN108438048B (zh) | 一种新型的履带式拖拉机自动转向控制***及控制方法 | |
KR102183204B1 (ko) | 차량 및 차량의 제어 방법 | |
CN112703539A (zh) | 行驶路径生成装置及车辆控制装置 | |
US11208160B1 (en) | Adjustable airfoil system for a vehicle | |
JP2019038535A (ja) | 自動走行圃場作業車両 | |
CN112414404B (zh) | 农机基于线控转向的自动导航建模与控制方法 | |
JP7449570B2 (ja) | コントロールモーメントジャイロ及びドライブ・バイ・ワイヤ式ステアリングシステムを用いて二輪車をバランスさせる統合制御方法 | |
CN114413791B (zh) | 一种舵轮式叉车舵轮零偏角的检测方法、***及装置 | |
JP5907037B2 (ja) | 移動体 | |
JP2004530890A (ja) | 慣性オドメータ補正による統合慣性vms航法 | |
CN110083075B (zh) | 一种拖挂式自行车的稳定裕度估算方法以及控制方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant | ||
EE01 | Entry into force of recordation of patent licensing contract |
Application publication date: 20190802 Assignee: Guangxi Cuizhi Rongchuang Technology Co.,Ltd. Assignor: GUILIN University OF ELECTRONIC TECHNOLOGY Contract record no.: X2023980046599 Denomination of invention: Stability margin estimation method and control method for a trailer mounted bicycle Granted publication date: 20220408 License type: Common License Record date: 20231108 |
|
EE01 | Entry into force of recordation of patent licensing contract |