Movatterモバイル変換

[0]ホーム
URL:

CN110083075B - Stability margin estimation method and control method for a trailer-mounted bicycle - Google Patents

Stability margin estimation method and control method for a trailer-mounted bicycleDownload PDF

Info

Publication number
CN110083075B
CN110083075BCN201910407550.8ACN201910407550ACN110083075BCN 110083075 BCN110083075 BCN 110083075BCN 201910407550 ACN201910407550 ACN 201910407550ACN 110083075 BCN110083075 BCN 110083075B
Authority
CN
China
Prior art keywords
grounding
trailer
coordinate system
stability margin
bicycle
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
Application number
CN201910407550.8A
Other languages
Chinese (zh)
Other versions
CN110083075A (en
Inventor
黄用华
李高明
庄未
胡峻榕
唐荣江
黄美发
钟艳如
孙永厚
钟永全
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Guilin University of Electronic Technology
Original Assignee
Guilin University of Electronic Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Guilin University of Electronic TechnologyfiledCriticalGuilin University of Electronic Technology
Priority to CN201910407550.8ApriorityCriticalpatent/CN110083075B/en
Publication of CN110083075ApublicationCriticalpatent/CN110083075A/en
Application grantedgrantedCritical
Publication of CN110083075BpublicationCriticalpatent/CN110083075B/en
Activelegal-statusCriticalCurrent
Anticipated expirationlegal-statusCritical

Links

Images

Classifications

Landscapes

Abstract

Translated fromChinese

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

Figure 201910407550

The invention discloses a stability margin estimation method and a control method of a trailer-type bicycle. A method for dynamically estimating the stability margin is designed, and the ZMP and the coordinates of three grounding points in the geodetic coordinate system are calculated by using attitude detection data. Make parallel lines on each side, and the parallel lines and the corresponding sides in the grounding triangle form three maximum trapezoids. By calculating the ratio of the areas of the three maximum trapezoids to the current grounding triangle area, the stability margin of each side under dynamic conditions can be obtained. , and the stability margin of each side under dynamic operation is used as the data basis for balance control. This method of dynamically estimating the stability margin can estimate the stability margin of each side of the grounding triangle under dynamic operation in real time, and use the dynamic grounding triangle to estimate the stability margin of each side. The stability margin of each side is based on data, and the balance of the trailer bicycle is controlled in a real-time, dynamic and quantitative manner, which is more targeted and accurate in estimation and control.

Figure 201910407550

Description

Translated fromChinese
一种拖挂式自行车的稳定裕度估算方法以及控制方法Stability margin estimation method and control method of a towed bicycle

技术领域technical field

本发明涉及自平衡自行车技术领域，具体涉及一种拖挂式自行车的稳定裕度估算方法以及控制方法。The invention relates to the technical field of self-balancing bicycles, in particular to a stability margin estimation method and a control method of a trailer-type bicycle.

背景技术Background technique

拖挂式自行车是由双轮牵引车和单轮拖挂车构成的新型自平衡自行车道路行走机构，其刚体结构主要包括牵引车车架、车把、牵引车前轮、牵引车后轮、拖挂车车架和拖挂车车轮。拖挂式自行车配置的是三个接地车轮，是一种三个接地点的移动机械系统。对于这种动态系统，稳定裕度是其首先需要考虑的问题。Trailer-type bicycle is a new type of self-balancing bicycle road running mechanism composed of two-wheel tractor and single-wheel trailer. Its rigid structure mainly includes tractor frame, handlebar, tractor front wheel, tractor rear wheel, and trailer Frame and trailer wheels. The trailer-mounted bicycle is equipped with three grounded wheels, which is a mobile mechanical system with three grounding points. For such a dynamic system, the stability margin is the first issue to be considered.

现有技术中，拖挂式自行车的稳定裕度的估算方法，通常是将三个接地点(如图7中的P1、P2、P3)连接形成的三角形里内接一个圆，再用该圆的圆心到零力矩点(ZMP)的距离来评判拖挂式自行车的稳定裕度，其稳定裕度计算的几何图如图7所示。该方法可以综合地、整体性地评判拖挂式自行车的稳定程度，但是估算出的稳定裕度误差较大，因而也不能针对性地进行稳定性评判，准确度相对较低，也会影响后续控制的准确性。In the prior art, the method for estimating the stability margin of a towed bicycle is usually to connect a circle in a triangle formed by connecting three grounding points (P1 , P2 and P3 in FIG. 7 ), and then The distance from the center of the circle to the zero moment point (ZMP) is used to judge the stability margin of the trailed bicycle. The geometric diagram of the stability margin calculation is shown in Figure 7. This method can comprehensively and holistically judge the stability of the trailer bicycle, but the estimated stability margin has a large error, so it cannot be targeted for stability judgment, and the accuracy is relatively low, which will also affect the subsequent Control accuracy.

发明内容SUMMARY OF THE INVENTION

本发明提供一种拖挂式自行车的稳定裕度估算方法以及控制方法，解决现有技术存在的稳定裕度估算不够准确以及控制不够准确的问题。The present invention provides a stability margin estimation method and a control method for a trailer-type bicycle, and solves the problems of inaccurate stability margin estimation and inaccurate control existing in the prior art.

本发明通过以下技术方案解决技术问题：The present invention solves the technical problem through the following technical solutions:

一种拖挂式自行车的稳定裕度估算方法，包括动态稳定裕度的估算，所述动态稳定裕度的估算步骤为：A method for estimating a stability margin of a trailer-mounted bicycle, comprising the estimation of a dynamic stability margin, and the steps of estimating the dynamic stability margin are:

(1)利用所述拖挂式自行车的姿态检测数据计算出ZMP在大地坐标系下的坐标以及所述拖挂式自行车与地面接触的三个接地点在大地坐标系下的坐标；(1) using the attitude detection data of the towed bicycle to calculate the coordinates of the ZMP under the geodetic coordinate system and the coordinates of the three grounding points of the towed bicycle in contact with the ground under the geodetic coordinate system;

(2)将所述三个接地点连线，得到动态接地三角形；(2) connecting the three grounding points to obtain a dynamic grounding triangle;

(3)过ZMP做所述动态接地三角形第一条边的平行线，得到第一平行线，在所述动态接地三角形范围内，所述第一平行线与所述第一条边形成的最大梯形为第一梯形；过ZMP做所述动态接地三角形第二条边的平行线，得到第二平行线，在所述动态接地三角形范围内，所述第二平行线与所述第二条边形成的最大梯形为第二梯形；过ZMP做所述动态接地三角形第三条边的平行线，得到第三平行线，在所述动态接地三角形范围内，所述第三平行线与所述第三条边形成的最大梯形为第三梯形；(3) Make a parallel line of the first side of the dynamic grounding triangle through ZMP to obtain the first parallel line. Within the dynamic grounding triangle range, the maximum value formed by the first parallel line and the first side The trapezoid is the first trapezoid; use ZMP to make the parallel line of the second side of the dynamic grounding triangle to obtain the second parallel line, within the range of the dynamic grounding triangle, the second parallel line and the second side The largest trapezoid formed is the second trapezoid; make a parallel line of the third side of the dynamic grounding triangle through ZMP to obtain a third parallel line, within the range of the dynamic grounding triangle, the third parallel line and the third parallel line are The largest trapezoid formed by three sides is the third trapezoid;

(4)所述动态接地三角形第一条边的稳定裕度为所述第一梯形的面积与当前动态接地三角形的面积之比，得到第一条边的动态稳定裕度；所述动态接地三角形第二条边的稳定裕度为所述第二梯形的面积与当前动态接地三角形的面积之比，得到第二条边的动态稳定裕度；所述动态接地三角形第三条边的稳定裕度为所述第三梯形的面积与当前动态接地三角形的面积之比，得到第三条边的动态稳定裕度。(4) The stability margin of the first side of the dynamic grounding triangle is the ratio of the area of the first trapezoid to the area of the current dynamic grounding triangle to obtain the dynamic stability margin of the first side; the dynamic grounding triangle The stability margin of the second side is the ratio of the area of the second trapezoid to the area of the current dynamic grounding triangle, to obtain the dynamic stability margin of the second side; the stability margin of the third side of the dynamic grounding triangle As the ratio of the area of the third trapezoid to the area of the current dynamic grounding triangle, the dynamic stability margin of the third side is obtained.

进一步地，在所述步骤(1)中，所述姿态检测数据包括所述拖挂式自行车的牵引车车架的三个欧拉角度和对应的三个欧拉角速度、所述拖挂式自行车的车把绕牵引车车架的转角和角速度、所述拖挂式自行车的牵引车前轮绕车把的角速度、所述拖挂式自行车的牵引车后轮绕牵引车车架的角速度、所述拖挂式自行车的拖挂车车架绕牵引车车架左右转动的转角和角速度、所述拖挂式自行车的拖挂车车架绕牵引车车架上下转动的转角和角速度、所述拖挂式自行车的拖挂车车轮绕拖挂车车架转动的角速度。Further, in the step (1), the attitude detection data includes three Euler angles and corresponding three Euler angular velocities of the tractor frame of the trailer bicycle, the trailer bicycle The angle and angular velocity of the handlebar around the tractor frame, the angular velocity of the tractor front wheel of the trailer bicycle around the handlebar, the angular velocity of the tractor rear wheel of the trailer bicycle around the tractor frame, the The angle and angular velocity of the left and right rotation of the trailer frame of the trailer-type bicycle around the tractor frame, the angle and angular velocity of the trailer frame of the trailer-type bicycle rotating up and down around the tractor frame, the The angular velocity at which the bicycle's trailer wheel rotates about the trailer frame.

进一步地，步骤(1)中所述ZMP在大地坐标系下的坐标满足公式：Further, the coordinates of the ZMP described in step (1) under the geodetic coordinate system satisfy the formula:

Figure BDA0002061737340000031
Figure BDA0002061737340000031

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

Figure BDA0002061737340000032
为Px的一阶导，
Figure BDA0002061737340000033
为Py的一阶导，
Figure BDA0002061737340000034
为Pz的一阶导；Lx、Ly为绕大地坐标系圆心转动的角动量在x、y轴的分量，
Figure BDA0002061737340000035
为Lx的一阶导，
Figure BDA0002061737340000036
为Ly的一阶导；xzmp、yzmp、zzmp为ZMP在大地坐标系下的坐标，所述拖挂式自行车在平地上运动时zzmp＝0。In the formula: M is the total mass of the trailer-type bicycle; g is the acceleration of gravity; xc , yc are the coordinates of the center of mass of the trailer-type bicycle under the geodetic coordinate system; Px , Py , Pz are the components of the linear momentum of the towed bicycle on the x, y, and z axes of the geodetic coordinate system, respectively,
Figure BDA0002061737340000032
is the first derivative of Px ,
Figure BDA0002061737340000033
is the first derivative of Py ,
Figure BDA0002061737340000034
is the first-order derivative of Pz ; Lx and Ly are the components of the angular momentum rotating around the center of the geodetic coordinate system on the x and y axes,
Figure BDA0002061737340000035
is the first derivative of Lx ,
Figure BDA0002061737340000036
is the first-order derivative of Ly; xzmp ,yzmp , and zzmp are the coordinates of ZMP in the geodetic coordinate system, and zzmp =0 when the towed bicycle moves on flat ground.

进一步地，步骤(1)中，所述拖挂式自行车与地面接触的三个接地点分别为第一接地点、第二接地点和第三接地点，所述第一接地点、第二接地点和第三接地点在大地坐标系下的坐标计算步骤为：Further, in step (1), the three grounding points where the trailer-type bicycle contacts the ground are respectively a first grounding point, a second grounding point and a third grounding point, and the first grounding point and the second grounding point are The coordinate calculation steps of the location and the third grounding point in the geodetic coordinate system are:

1)通过所述牵引车车架的三个欧拉角速度计算出所述牵引车车架在大地坐标系下的角速度；1) Calculate the angular velocity of the tractor frame under the geodetic coordinate system through the three Euler angular velocities of the tractor frame;

2)通过所述牵引车车架的角速度计算出牵引车后轮在大地坐标系下的角速度；2) Calculate the angular velocity of the rear wheel of the tractor under the geodetic coordinate system by the angular velocity of the frame of the tractor;

3)通过所述牵引车后轮的角速度计算出牵引车后轮所在坐标系的原点在大地坐标系下的线速度；3) Calculate the linear velocity of the origin of the coordinate system where the rear wheel of the tractor is located under the geodetic coordinate system by the angular velocity of the rear wheel of the tractor;

4)对所述线速度进行积分运算，得到所述牵引车后轮所在坐标系的原点在大地坐标系下的坐标；4) carry out integral operation to described linear velocity, obtain the coordinates of the origin of the coordinate system where the rear wheel of the tractor is located under the geodetic coordinate system;

5)通过所述牵引车后轮所在坐标系的原点在大地坐标系下的坐标以及坐标变换，计算出所述第一接地点、第二接地点以及拖挂车车架所在坐标系的原点在大地坐标系下的坐标；5) Through the coordinates and coordinate transformation of the origin of the coordinate system where the rear wheels of the tractor are located in the geodetic coordinate system, calculate that the first grounding point, the second grounding point and the origin of the coordinate system where the trailer frame is located are in the earth. the coordinates in the coordinate system;

6)通过拖挂车车架所在坐标系的原点在大地坐标系下的坐标计算出第三接地点在大地坐标系下的坐标。6) Calculate the coordinates of the third grounding point under the geodetic coordinate system through the coordinates of the origin of the coordinate system where the frame of the trailer is located under the geodetic coordinate system.

进一步地，还包括静态稳定裕度的估算；所述静态稳定裕度的估算步骤为：静态下的拖挂式自行车与地面接触的三个接地点形成静态接地三角形，所述静态接地三角形的面积与这三个接地点能形成的最大三角形的面积之比，得到静态稳定裕度。Further, it also includes the estimation of the static stability margin; the estimation step of the static stability margin is as follows: the static grounding triangle is formed by the three grounding points of the towed bicycle in static contact with the ground, and the area of the static grounding triangle is Ratio to the area of the largest triangle that can be formed by these three ground points to obtain the static stability margin.

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

Figure BDA0002061737340000041
作为误差值进行PID运算，输出电流，所述电流输入至所述拖挂式自行车车把的电机，以控制所述拖挂式自行车的平衡，其中，i＝1、2、3，K1为第一条边的动态稳定裕度，K2为第二条边的动态稳定裕度，K3为第三条边的动态稳定裕度。A control method based on the above-mentioned method for estimating stability margin of a towed bicycle, comprising a PID controller; the PID controller is
Figure BDA0002061737340000041
PID operation is performed as the error value, and the current is output, and the current is input to the motor of the handlebar of the trailer bicycle to control the balance of the trailer bicycle, wherein i=1, 2, 3, and K1 is The dynamic stability margin of the first edge, K2 is the dynamic stability margin of the second edge, and K3 is the dynamic stability margin of the third edge.

进一步地，所述PID运算表达式为：Further, the PID operation expression is:

Figure BDA0002061737340000042
Figure BDA0002061737340000042

式中，I为所述拖挂式自行车车把的电机电流，KPi、KDi、KIi为PID调节参数，

Figure BDA0002061737340000043
为δi的一阶导。In the formula, I is the motor current of the handlebar of the towed bicycle, KPi , KDi , and KIi are the PID adjustment parameters,
Figure BDA0002061737340000043
is the first derivative of δi .

与现有技术相比，具有如下特点：Compared with the existing technology, it has the following characteristics:

1、设计了动态估算稳定裕度的方法，利用姿态检测数据计算ZMP以及三个接地点在大地坐标系下的坐标，经ZMP做各边平行线，在接地三角形内平行线与对应的边形成三个最大梯形，通过计算三个最大梯形的面积与当前接地三角形面积之比，便可获得动态下每条边的稳定裕度，并以动态下每条边的稳定裕度作为平衡控制的数据基础，该动态估算稳定裕度的方法，能实时估算动态运行下接地三角形每条边的稳定裕度，并以动态的接地三角形每条边的稳定裕度为数据基础，实时地、动态地、量化地控制拖挂式自行车的平衡，在估算和控制上更具针对性，也更准确；1. A method of dynamically estimating stability margin is designed, using attitude detection data to calculate the coordinates of ZMP and three grounding points in the geodetic coordinate system, and making parallel lines on each side through ZMP, and forming parallel lines and corresponding sides in the grounding triangle Three largest trapezoids. By calculating the ratio of the area of the three largest trapezoids to the current grounded triangle area, the stability margin of each side under dynamic conditions can be obtained, and the stability margin of each side under dynamic conditions is used as the balance control data. The method of dynamically estimating the stability margin can estimate the stability margin of each side of the grounded triangle under dynamic operation in real time. Quantitatively control the balance of the trailer bicycle, which is more targeted and accurate in estimation and control;

2、动态估算稳定裕度时，计算ZMP和三个接地点的数据基础包括各个刚体动态运行下的姿态，涉及欧拉角度、欧拉角速度、转角以及角速度等，以上述姿态数据作为估算的数据基础，能充分反映拖挂式行车的动态运行状态，能保证稳定裕度估算的准确性；2. When estimating the stability margin dynamically, the data basis for calculating ZMP and three grounding points includes the attitude of each rigid body under dynamic operation, involving Euler angle, Euler angular velocity, turning angle and angular velocity, etc. The above attitude data is used as the estimated data It can fully reflect the dynamic running state of the trailer and ensure the accuracy of the stability margin estimation;

3、对拖挂式自行车进行平衡控制时，以与接地三角形每条边的稳定裕度相关的误差值作为PID运算的输入，以车把电机电流作为输出，通过车把的运动改变接地三角形每条边的稳定裕度，进而控制拖挂式自行车的平衡，相当于将估算和控制形成闭环结构，实时地、准确地、动态地调整拖挂式自行车的平衡，使得控制更为准确。3. When the balance control of the trailer-type bicycle is performed, the error value related to the stability margin of each side of the grounding triangle is used as the input of the PID operation, and the current of the handlebar motor is used as the output, and the movement of the handlebar changes each grounding triangle. The stability margin of the strip edge, and then control the balance of the trailer bicycle, is equivalent to forming a closed-loop structure for estimation and control, and adjust the balance of the trailer bicycle in real time, accurately and dynamically, making the control more accurate.

附图说明Description of drawings

图1为本发明的稳定裕度的动态估算以及控制的流程图。FIG. 1 is a flow chart of dynamic estimation and control of the stability margin of the present invention.

图2为PID控制器的流程图。Figure 2 is a flow chart of the PID controller.

图3为构建S1区域的几何图。Figure 3 isa geometric diagram of the construction of the S1 region.

图4为构建S2区域的几何图。Figure4 is a geometric diagram of the construction of the S2 region.

图5为构建S3区域的几何图。Figure5 is a geometric diagram of the construction of the S3 region.

图6为坐标映射计算链式图。FIG. 6 is a chain diagram of coordinate mapping calculation.

图7为现有技术稳定裕度估算的几何图。Figure 7 is a geometric diagram of prior art stability margin estimation.

图8为现有技术拖挂式自行车的机械结构图。FIG. 8 is a mechanical structure diagram of a prior art trailer-type bicycle.

图中标号为：1、牵引车车架；2、车把、3、牵引车前轮；4、牵引车后轮；5、拖挂车车架；6、拖挂车车轮；7、大地。The numbers in the figure are: 1. Frame of tractor; 2. Handlebar, 3. Front wheel of tractor; 4. Rear wheel of tractor; 5. Frame of trailer; 6. Wheel of trailer; 7. Ground.

具体实施方式Detailed ways

以下结合实施例对本发明作进一步说明，但本发明并不局限于这些实施例。The present invention will be further described below with reference to the examples, but the present invention is not limited to these examples.

拖挂式自行车包括如下刚体：牵引车车架1、车把2、牵引车前轮3、牵引车后轮4、拖挂车车架5和拖挂车车轮6。在车把2上安装有电机，电机通过齿轮传动机构与牵引车前轮3相配合，驱动牵引车前轮3转动；在牵引车车架1上安装有电机，该电机通过齿轮传动机构与车把2相配合，驱动车把2转动。牵引车车架1和拖挂车车架5是通过十字轴组件连接，拖挂车车架5可以绕牵引车车架1上下转动以及左右转动。拖挂式自行车的机械结构图如图8所示。The trailer bicycle includes the following rigid bodies: atractor frame 1 , ahandlebar 2 , atractor front wheel 3 , a tractor rear wheel 4 , atrailer frame 5 and atrailer wheel 6 . A motor is installed on thehandlebar 2, and the motor cooperates with thefront wheel 3 of the tractor through a gear transmission mechanism to drive thefront wheel 3 of the tractor to rotate; a motor is installed on theframe 1 of the tractor, and the motor is connected to the vehicle through a gear transmission mechanism.Handlebar 2 is matched to drivehandlebar 2 to rotate. Thetractor frame 1 and thetrailer frame 5 are connected by a cross shaft assembly, and thetrailer frame 5 can rotate around thetractor frame 1 up and down and left and right. The mechanical structure diagram of the trailer bicycle is shown in Figure 8.

在牵引车车架1上安装陀螺仪，检测牵引车车架1的三个欧拉角度以及对应的三个欧拉角速度；在牵引车车架1上安装增量编码器和绝对编码器，检测车把2绕牵引车车架1的转角和角速度；在车把2上安装增量编码器，检测牵引车前轮3绕车把2的角速度；在牵引车车架1上设置增量编码器，检测牵引车后轮4绕牵引车车架1的角速度；在牵引车车架1上安装2个增量编码器和2个绝对编码器，检测拖挂车车架5绕牵引车车架1左右转动的转角和角速度，以及拖挂车车架5绕牵引车车架1上下转动的转角和角速度；在拖挂车车架5上安装增量编码器，检测拖挂车车轮6绕拖挂车车架5转动的角速度。Install a gyroscope on thetractor frame 1 to detect the three Euler angles and the corresponding three Euler angular velocities of thetractor frame 1; install an incremental encoder and an absolute encoder on thetractor frame 1 to detect The angle and angular velocity of thehandlebar 2 around theframe 1 of the tractor; the incremental encoder is installed on thehandlebar 2 to detect the angular velocity of thefront wheel 3 of the tractor around thehandlebar 2; the incremental encoder is set on theframe 1 of the tractor , detect the angular velocity of the rear wheel 4 of the tractor around theframe 1 of the tractor; install 2 incremental encoders and 2 absolute encoders on theframe 1 of the tractor to detect theframe 5 of the trailer around theframe 1 of the tractor The turning angle and angular velocity of the rotation, as well as the turning angle and angular velocity of thetrailer frame 5 turning up and down around thetractor frame 1; install an incremental encoder on thetrailer frame 5 to detect thetrailer wheel 6 rotates around thetrailer frame 5 angular velocity.

针对上述拖挂式自行车的机械结构，提出其稳定裕度的估算方法，包括动态稳定裕度的估算，所述动态稳定裕度的估算过程如图1所示，具体步骤为：Aiming at the mechanical structure of the above-mentioned trailer-type bicycle, an estimation method of its stability margin is proposed, including the estimation of the dynamic stability margin. The estimation process of the dynamic stability margin is shown in Figure 1, and the specific steps are as follows:

(1)利用所述拖挂式自行车的姿态检测数据计算出ZMP在大地坐标系下的坐标以及所述拖挂式自行车与地面接触的三个接地点在大地坐标系下的坐标；(1) using the attitude detection data of the towed bicycle to calculate the coordinates of the ZMP under the geodetic coordinate system and the coordinates of the three grounding points of the towed bicycle in contact with the ground under the geodetic coordinate system;

(2)将所述三个接地点连线，得到动态接地三角形；(2) connecting the three grounding points to obtain a dynamic grounding triangle;

(3)过ZMP做所述动态接地三角形第一条边的平行线，得到第一平行线，在所述动态接地三角形范围内，所述第一平行线与所述第一条边形成的最大梯形为第一梯形，如图3中的S1；过ZMP做所述动态接地三角形第二条边的平行线，得到第二平行线，在所述动态接地三角形范围内，所述第二平行线与所述第二条边形成的最大梯形为第二梯形，如图4中的S2；过ZMP做所述动态接地三角形第三条边的平行线，得到第三平行线，在所述动态接地三角形范围内，所述第三平行线与所述第三条边形成的最大梯形为第三梯形，如图5中的S3；(3) Make a parallel line of the first side of the dynamic grounding triangle through ZMP to obtain the first parallel line. Within the dynamic grounding triangle range, the maximum value formed by the first parallel line and the first side The trapezoid is the first trapezoid, such as S1 in FIG. 3 ; make a parallel line of the second side of the dynamic grounding triangle through ZMP to obtain a second parallel line, within the range of the dynamic grounding triangle, the second parallel line The largest trapezoid formed by the line and the second side is the second trapezoid, such as S2 in Fig. 4 ; make the parallel line of the third side of the dynamic grounding triangle through ZMP to obtain the third parallel line, in the Within the dynamic grounding triangle range, the largest trapezoid formed by the third parallel line and the third side is the third trapezoid, such as S3 in FIG. 5 ;

(4)所述动态接地三角形第一条边的稳定裕度为所述第一梯形的面积与当前动态接地三角形的面积之比，得到第一条边的动态稳定裕度；所述动态接地三角形第二条边的稳定裕度为所述第二梯形的面积与当前动态接地三角形的面积之比，得到第二条边的动态稳定裕度；所述动态接地三角形第三条边的稳定裕度为所述第三梯形的面积与当前动态接地三角形的面积之比，得到第三条边的动态稳定裕度。(4) The stability margin of the first side of the dynamic grounding triangle is the ratio of the area of the first trapezoid to the area of the current dynamic grounding triangle to obtain the dynamic stability margin of the first side; the dynamic grounding triangle The stability margin of the second side is the ratio of the area of the second trapezoid to the area of the current dynamic grounding triangle, to obtain the dynamic stability margin of the second side; the stability margin of the third side of the dynamic grounding triangle As the ratio of the area of the third trapezoid to the area of the current dynamic grounding triangle, the dynamic stability margin of the third side is obtained.

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

Figure BDA0002061737340000071
所述拖挂式自行车的车把2绕牵引车车架1的转角和角速度
Figure BDA0002061737340000072
所述拖挂式自行车的牵引车前轮3绕车把2的角速度
Figure BDA0002061737340000073
所述拖挂式自行车的牵引车后轮4绕牵引车车架1的角速度
Figure BDA0002061737340000074
所述拖挂式自行车的拖挂车车架5绕牵引车车架1左右转动的转角和角速度
Figure BDA0002061737340000075
所述拖挂式自行车的拖挂车车架5绕牵引车车架1上下转动的转角和角速度
Figure BDA0002061737340000076
所述拖挂式自行车的拖挂车车轮6绕拖挂车车架5转动的角速度
Figure BDA0002061737340000087
In the step (1), the attitude detection data includes the detection data of the above-mentioned gyroscope, incremental encoder and absolute encoder, and specifically includes: three ohms of thetractor frame 1 of the trailer-type bicycle Pull angles q1 , q2 , q3 , corresponding to the three Euler angular velocities
Figure BDA0002061737340000071
The angle and angular velocity of thehandlebar 2 of the trailer bicycle around theframe 1 of the tractor
Figure BDA0002061737340000072
The angular velocity of thetractor front wheel 3 of the trailer bicycle around thehandlebar 2
Figure BDA0002061737340000073
The angular velocity of the tractor rear wheel 4 of the trailer bicycle around thetractor frame 1
Figure BDA0002061737340000074
The angle and angular velocity of the left and right rotation of thetrailer frame 5 of the trailer bicycle around thetractor frame 1
Figure BDA0002061737340000075
The angle of rotation and angular velocity of thetrailer frame 5 of the trailer bicycle around thetractor frame 1
Figure BDA0002061737340000076
The angular velocity at which thetrailer wheel 6 of the trailer bicycle rotates around thetrailer frame 5
Figure BDA0002061737340000087

步骤(1)中，计算ZMP在大地7坐标系下的坐标的方法为：In step (1), the method for calculating the coordinates of ZMP in the geodetic 7 coordinate system is:

ZMP在大地7坐标系下的坐标满足公式：The coordinates of ZMP in the geodetic 7 coordinate system satisfy the formula:

Figure BDA0002061737340000081
Figure BDA0002061737340000081

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

Figure BDA0002061737340000082
为Px的一阶导，
Figure BDA0002061737340000083
为Py的一阶导，
Figure BDA0002061737340000084
为Pz的一阶导；Lx、Ly为绕大地坐标系圆心转动的角动量在x、y轴的分量，Lx、Ly为角动量，为关于各个刚体角速度的函数，
Figure BDA0002061737340000085
为Lx的一阶导，
Figure BDA0002061737340000086
为Ly的一阶导；xzmp、yzmp、zzmp为ZMP在大地坐标系下的坐标，所述拖挂式自行车在平地上运动时zzmp＝0。In the formula: M is the total mass of the trailer-type bicycle; g is the acceleration of gravity; xc , yc are the coordinates of the center of mass of the trailer-type bicycle under the geodetic coordinate system; Px , Py , Pz are the components of the linear momentum of the towed bicycle on the x, y, and z axes of the geodetic coordinate system, respectively, Px , Py , and Pz are functions related to the linear velocity of each rigid body, and the linear velocity of each rigid body is determined by the corresponding is calculated from the detected angular velocity of ;
Figure BDA0002061737340000082
is the first derivative of Px ,
Figure BDA0002061737340000083
is the first derivative of Py ,
Figure BDA0002061737340000084
is the first-order derivative of Pz ; Lx andLy are the components of the angular momentum rotating around the center of the geodetic coordinate system on the x and y axes, and Lx andLy are the angular momentum, which are functions of the angular velocity of each rigid body,
Figure BDA0002061737340000085
is the first derivative of Lx ,
Figure BDA0002061737340000086
is the first-order derivative of Ly; xzmp ,yzmp , and zzmp are the coordinates of ZMP in the geodetic coordinate system, and zzmp =0 when the towed bicycle moves on flat ground.

本发明涉及的坐标系有7个：固定在大地7上的全局坐标系，即大地7坐标系，坐标原点为O0；牵引车车架1的坐标系，且坐标原点在牵引车前轮3的几何中心处，坐标原点为O1；车把2的坐标系，且坐标原点在牵引车车架1与车把2轴线的交点处，坐标原点为O2；牵引车前轮3的坐标系，且坐标原点在牵引车前轮,3的几何中心处；坐标原点为O3；牵引车后轮4的坐标系，且坐标原点在牵引车后轮4的几何中心处，坐标原点为O4；拖挂车车架5的坐标系，且坐标原点在十字轴的几何中心处，坐标原点为O5；拖挂车车轮6的坐标系，且坐标原点在,拖挂车车轮6的几何中心处，坐标原点为O6。There are 7 coordinate systems involved in the present invention: a global coordinate system fixed on theearth 7, that is, theearth 7 coordinate system, the coordinate origin is O0 ; the coordinate system of thetractor frame 1, and the coordinate origin is on thefront wheel 3 of the tractor At the geometric center of , the coordinate origin is O1 ; the coordinate system of thehandlebar 2, and the coordinate origin is at the intersection of the axis of thetractor frame 1 and thehandlebar 2, the coordinate origin is O2 ; the coordinate system of thefront wheel 3 of the tractor , and the origin of the coordinates is at the geometric center of thefront wheel 3 of the tractor; the origin of the coordinates is O3 ; the coordinate system of the rear wheel 4 of the tractor, and the origin of the coordinates is at the geometric center of the rear wheel 4 of the tractor, and the origin of the coordinates is O4 ; The coordinate system of thetrailer frame 5, and the coordinate origin is at the geometric center of the cross axis, and the coordinate origin is O5 ; The coordinate system of thetrailer wheel 6, and the coordinate origin is at the geometric center of thetrailer wheel 6, the coordinates The origin is O6 .

各个检测到的角速度均是在其安装位置所在的坐标系下进行的检测，使用角速度数据时，均将其进行坐标转换，换算为大地7坐标系下的角速度。因此，检测车把2绕牵引车车架1的转角、拖挂车车架5绕牵引车车架1左右转动的转角以及拖挂车车架5绕牵引车车架1上下转动的转角用于获取各个转角所在的坐标系与大地7坐标系之间的旋转矩阵，利用旋转矩阵将相应的角速度换算到大地7坐标系下的角速度，以便于获取大地7坐标系下的线速度，进而获取Px、Py、Pz、Lx、Ly的值，进而求取ZMP在大地7坐标系下的坐标。Each detected angular velocity is detected in the coordinate system where its installation location is located. When using the angular velocity data, it is transformed into the angular velocity in the geodetic 7 coordinate system. Therefore, the angle of rotation of thehandlebar 2 around thetractor frame 1, the angle of the left and right rotation of thetrailer frame 5 around thetractor frame 1, and the angle of the up and down rotation of thetrailer frame 5 around thetractor frame 1 are detected to obtain each The rotation matrix between the coordinate system where the corner is located and theGeodetic 7 coordinate system, use the rotation matrix to convert the corresponding angular velocity to the angular velocity under theGeodetic 7 coordinate system, so as to obtain the linear velocity under theGeodetic 7 coordinate system, and then obtain Px , The values of Py , Pz , Lx , andLy , and then the coordinates of the ZMP in the geodetic 7 coordinate system are obtained.

步骤(1)中，所述拖挂式自行车与地面接触的三个接地点分别为第一接地点、第二接地点和第三接地点，即图8中的P1、P2、P3，所述第一接地点、第二接地点和第三接地点在大地坐标系下的坐标计算步骤为：In step (1), the three grounding points where the trailer-type bicycle contacts the ground are the first grounding point, the second grounding point and the third grounding point, namely P1, P2, and P3 in FIG. The coordinate calculation steps of the first ground point, the second ground point and the third ground point in the geodetic coordinate system are as follows:

1)通过所述牵引车车架1的三个欧拉角速度计算出所述牵引车车架1在大地7坐标系O0下的角速度；1) Calculate the angular velocity of thetractor frame 1 under the geodetic 7 coordinate system O0 through the three Euler angular velocities of thetractor frame 1;

2)通过所述牵引车车架1的角速度计算出牵引车后轮4在大地7坐标系O0下的角速度；2) Calculate the angular velocity of the rear wheel 4 of the tractor under the coordinate system O0 of theearth 7 by the angular velocity of theframe 1 of the tractor;

3)通过所述牵引车后轮4的角速度计算出牵引车后轮4所在坐标系的原点O4在大地7坐标系下的线速度；3) Calculate the linear velocity of the origin O4 of the coordinate system where the rear wheel 4 of the tractor is located under theearth 7 coordinate system by the angular velocity of the rear wheel 4 of the tractor;

4)对所述线速度进行积分运算，得到所述牵引车后轮4所在坐标系的原点O4在大地7坐标系下的坐标；4) carry out integral operation to described linear velocity, obtain the coordinates of the origin O4 of the coordinate system where the rear wheel 4 of the tractor is located under theearth 7 coordinate system;

5)通过所述牵引车后轮4所在坐标系的原点O4在大地7坐标系下的坐标以及坐标变换，计算出所述第一接地点、第二接地点以及拖挂车车架5所在坐标系的原点O5在大地7坐标系下的坐标；5) Through the coordinates and coordinate transformation of the origin O4 of the coordinate system where the rear wheel 4 of the tractor is located in theearth 7 coordinate system, calculate the coordinates where the first grounding point, the second grounding point and thetrailer frame 5 are located The coordinates of the origin O5 of the system in the geodetic 7 coordinate system;

6)通过拖挂车车架5所在坐标系的原点O5在大地7坐标系下的坐标计算出第三接地点在大地7坐标系下的坐标。6) Calculate the coordinates of the third grounding point under theEarth 7 coordinate system through the coordinates of the origin O5 of the coordinate system where thetrailer frame 5 is located under theEarth 7 coordinate system.

步骤1)中，牵引车车架1在大地7坐标系下的角速度的表达式为：In step 1), the expression of the angular velocity of thetractor frame 1 in theearth 7 coordinate system is:

Figure BDA0002061737340000101
式中，ωB1为牵引车车架1在大地7坐标系下的角速度，s1＝sin(q1)，s2＝sin(q2)，c1＝cos(q1)，c2＝cos(q2)，c3＝cos(q3)。
Figure BDA0002061737340000101
In the formula, ωB1 is the angular velocity of thetractor frame 1 in theearth 7 coordinate system, s1 =sin(q1 ), s2 =sin(q2 ), c1 =cos(q1 ), c2 = cos(q2 ), c3 =cos(q3 ).

步骤2)中，牵引车后轮4在大地7坐标系下的角速度表达式为：In step 2), the angular velocity expression of the rear wheel 4 of the tractor under the coordinate system of theearth 7 is:

Figure BDA0002061737340000102
式中，ωB4牵引车后轮4在大地7坐标系下的角速度，s1＝sin(q1)，s2＝sin(q2)，c1＝cos(q1)，c2＝cos(q2)。
Figure BDA0002061737340000102
In the formula, the angular velocity of the rear wheel 4 of the ωB4 tractor in the geodetic 7 coordinate system, s1 =sin(q1 ), s2 =sin(q2 ), c1 =cos(q1 ), c2 =cos (q2 ).

步骤3)中，牵引车后轮4所在坐标系的原点O4在大地7坐标系下的线速度的表达式为：In step 3), the expression of the linear velocity of the origin O4 of the coordinate system where the rear wheel 4 of the tractor is located in theearth 7 coordinate system is:

Figure BDA0002061737340000103
式中，vB4表示牵引车后轮4所在坐标系的原点O4在大地7坐标系下的线速度，s1＝sin(q1)，s2＝sin(q2)，c1＝cos(q1)，c2＝cos(q2)，r为牵引车后轮4的半径。
Figure BDA0002061737340000103
In the formula, vB4 represents the linear velocity of the origin O4 of the coordinate system where the rear wheel 4 of the tractor is located in theearth 7 coordinate system, s1 =sin(q1 ), s2 =sin(q2 ), c1 =cos (q1 ), c2 =cos(q2 ), and r is the radius of the rear wheel 4 of the tractor.

步骤4)中，牵引车后轮4所在坐标系的原点O4在大地7坐标系下的坐标表达式为：In step 4), the coordinate expression of the origin O4 of the coordinate system where the rear wheel 4 of the tractor is located under theearth 7 coordinate system is:

Figure BDA0002061737340000104
Figure BDA0002061737340000104

式中，s1＝sin(q1)，s2＝sin(q2)，c1＝cos(q1)，c2＝cos(q2)，r为牵引车后轮4的半径。In the formula, s1 =sin(q1 ), s2 =sin(q2 ), c1 =cos(q1 ), c2 =cos(q2 ), and r is the radius of the rear wheel 4 of the tractor.

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

Figure BDA0002061737340000111
可通过牵引车后轮4所在坐标系的向量
Figure BDA0002061737340000112
即[0 0 -l1]T旋转变换得到，其中l1为O4到O3的距离。在大地7坐标系下
Figure BDA0002061737340000113
可通过牵引车前轮3所在坐标系的向量
Figure BDA0002061737340000114
即[0 0 -r]T旋转变换得到，在大地7坐标系下
Figure BDA0002061737340000115
便可求出第一接地点P1在大地7坐标系下的坐标。In step 5), calculate the coordinates of the first ground point P1 in the geodetic 7 coordinate system: in the geodetic 7 coordinate system
Figure BDA0002061737340000111
The vector of the coordinate system where the rear wheel 4 of the tractor is located
Figure BDA0002061737340000112
That is, [0 0 -l1 ]T is obtained by T rotation transformation, where l1 is the distance from O4 to O3 . In the geodetic 7 coordinate system
Figure BDA0002061737340000113
The vector of the coordinate system where thefront wheel 3 of the tractor is located
Figure BDA0002061737340000114
That is, [0 0 -r]T rotation transformation is obtained, in the geodetic 7 coordinate system
Figure BDA0002061737340000115
The coordinates of the first ground point P1 in the geodetic 7 coordinate system can be obtained.

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

Figure BDA0002061737340000116
在大地7坐标系下
Figure BDA0002061737340000117
可通过牵引车后轮4所在坐标系的向量
Figure BDA0002061737340000118
即[0 0 -r]T旋转变换得到，在大地7坐标系下，
Figure BDA0002061737340000119
便可求出第二接地点，即图8中的第二接地点P2在大地7坐标系下的坐标；In step 5), calculate the coordinates of the second ground point P2 in the geodetic 7 coordinate system: obtain a vector in the geodetic 7 coordinate system
Figure BDA0002061737340000116
In the geodetic 7 coordinate system
Figure BDA0002061737340000117
The vector of the coordinate system where the rear wheel 4 of the tractor is located
Figure BDA0002061737340000118
That is, [0 0 -r]T rotation transformation is obtained, in the geodetic 7 coordinate system,
Figure BDA0002061737340000119
The second ground point can be obtained, that is, the coordinates of the second ground point P2 in FIG. 8 in theearth 7 coordinate system;

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

Figure BDA00020617373400001110
即[-lx1 0 lz1]T旋转变换得到，其中lx1为O4到O5的沿牵引车后轮4所在坐标系x轴方向的距离，lz为O4到O5的沿牵引车后轮4所在坐标系z轴方向的距离，在大地7坐标系下，
Figure BDA00020617373400001111
Figure BDA00020617373400001112
便可求出O5点在大地7坐标系下的坐标。In step 5), calculate the coordinates of the second grounding point O5 in theearth 7 coordinate system: the vector of the coordinate system where the rear wheel 4 of the tractor is located
Figure BDA00020617373400001110
That is, [-lx1 0 lz1 ]T is obtained by the rotation transformation, where lx1 is the distance from O4 to O5 along the x-axis direction of the coordinate system where the rear wheel 4 of the tractor is located, and lz is the distance along the tractor from O4 to O5 The distance in the z-axis direction of the coordinate system where the rear wheel 4 of the vehicle is located, in theearth 7 coordinate system,
Figure BDA00020617373400001111
Figure BDA00020617373400001112
The coordinates of the O5 point in the geodetic 7 coordinate system can be obtained.

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

Figure BDA00020617373400001113
即[-lx2 0 -lz2]T旋转变换得到，其中lx2为O5到O6的沿拖挂车车架5所在坐标系x轴方向的距离，lz为O5到O6的沿拖挂车车架5所在坐标系z轴方向的距离；在大地7坐标系下
Figure BDA00020617373400001114
可通过拖挂车车轮6所在坐标系的向量
Figure BDA00020617373400001115
即[0 0 -r]T旋转变换得到，在大地7坐标系下，
Figure BDA00020617373400001116
便便可求出第三接地点在大地7坐标系下的坐标。In step 6), through the vector of the coordinate system where thetrailer frame 5 is located
Figure BDA00020617373400001113
That is, [-lx2 0 -lz2 ]T is obtained by the rotation transformation, wherelx2 is the distance from O5 to O6 along the x-axis direction of the coordinate system where thetrailer frame 5 is located, and lz is the distance from O5 to O6 along the x-axis The distance in the z-axis direction of the coordinate system where thetrailer frame 5 is located; in the geodetic 7 coordinate system
Figure BDA00020617373400001114
Passable vector of the coordinate system where thetrailer wheel 6 is located
Figure BDA00020617373400001115
That is, [0 0 -r]T rotation transformation is obtained, in the geodetic 7 coordinate system,
Figure BDA00020617373400001116
Then, the coordinates of the third grounding point in the geodetic 7 coordinate system can be obtained.

上述步骤5)和步骤6)涉及相关的坐标映射计算链式图如图6所示。The above-mentioned steps 5) and 6) involve the related coordinate mapping calculation chain diagram as shown in FIG. 6 .

在步骤(3)中，三个梯形面积的计算方法相同，以S1为例，具体的计算步骤为：In step (3), the calculation methods of the three trapezoid areas are the same. Taking S1 asan example, the specific calculation steps are:

1)在大地7坐标系中，P1、P2、P3、ZMP的坐标可分别表示为P1(x1,y1)、P2(x2,y2)、P3(x3,y3)、ZMP(xzmp,yzmp)，上述各坐标已经在步骤(1)中计算出，于是直线P1P2的方程为

Figure BDA0002061737340000121
直线P2P3的方程为
Figure BDA0002061737340000122
直线P3P1的方程为
Figure BDA0002061737340000123
过ZMP(xzmp,yzmp)点作P1P2平行线，即第一平行线，第一平行线的方程为
Figure BDA0002061737340000124
1) In the geodetic 7 coordinate system, the coordinates of P1, P2, P3, and ZMP can be expressed as P1 (x1 , y1 ), P2 (x2 , y2 ), P3 (x3 , y3 ), respectively ), ZMP(xzmp , yzmp ), the above coordinates have been calculated in step (1), so the equation of the straight line P1 P2 is
Figure BDA0002061737340000121
The equation of the straight line P2 P3 is
Figure BDA0002061737340000122
The equation of the straight line P3 P1 is
Figure BDA0002061737340000123
Draw a P1 P2 parallel line through the point ZMP(xzmp , yzmp ), that is, the first parallel line. The equation of the first parallel line is
Figure BDA0002061737340000124

2)第一平行线与直线P2P3和直线P3P1分别有1个交点，这两个交点之间的可求出为d1；通过点ZMP到直线的距离公式可求出ZMP(xzmp,yzmp)到直线P1P2的距离为d2；通过已知的P1(x1,y1)，P2(x2,y2)可求出P1点到P2点的距离为d3；于是，

Figure BDA0002061737340000125
便可求出S1的面积大小。2) The first parallel line has one intersection point with the straight line P2 P3 and the straight line P3 P1 respectively, and the distance between these two intersection points can be obtained as d1 ; ZMP can be obtained through the distance formula from the point ZMP to the straight line The distance from (xzmp , yzmp ) to the straight line P1 P2 is d2 ; through the known P1 (x1 , y1 ), P2 (x2 , y2 ) can be obtained from the point P1 to P The distance between2 points is d3 ; thus,
Figure BDA0002061737340000125
The sizeof the area of S1 can be obtained.

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

Figure BDA0002061737340000126
其中i＝1,2,3。Ki值越大，说明拖挂式自行车相对于所对应的边越稳定。The calculation method of S2 and S3 is the same as that of S1 , and S is obtained by using the triangular area calculation method. The dynamic stability margin of a trailer bike with respect to each side of the grounded triangle can be expressed as:
Figure BDA0002061737340000126
where i=1,2,3. The larger the value of Ki , the more stable the trailer bicycle is relative to the corresponding side.

本发明还进行静态稳定裕度的估算，具体方法为：静态下的拖挂式自行车与地面接触的三个接地点形成静态接地三角形，所述静态接地三角形的面积与这三个接地点能形成的最大三角形的面积之比，得到静态稳定裕度。即，K＝S/Smax，其中，K为静态稳定裕度，S为静态接地三角形的面积，Smax为三个接地点所能形成的最大三角形面积。当K＝0时，拖挂式自行车三个车轮在一条直线上，拖挂式自行车稳定程度最差；当K＝1时，静态接地三角形面积最大，拖挂式自行车达到最稳定状态。静态稳定裕度用于综合评判整个拖挂式自行车的稳定程度，无需考虑ZMP在静态接地三角形内的位置。The present invention also estimates the static stability margin, and the specific method is as follows: the three grounding points of the trailer-type bicycle in static contact with the ground form a static grounding triangle, and the area of the static grounding triangle and the three grounding points can form a static grounding triangle. The ratio of the areas of the largest triangles to obtain the static stability margin. That is, K=S/Smax , where K is the static stability margin, S is the area of the static grounding triangle, and Smax is the maximum triangle area that can be formed by three grounding points. When K=0, the three wheels of the trailer bicycle are in a straight line, and the stability of the trailer bicycle is the worst; when K=1, the static grounding triangle area is the largest, and the trailer bicycle reaches the most stable state. The static stability margin is used to comprehensively judge the stability of the entire trailer bike, regardless of the position of the ZMP within the static grounding triangle.

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

Figure BDA0002061737340000133
作为误差值进行PID运算，输出电流，所述电流输入至所述拖挂式自行车车把2的电机，以控制所述拖挂式自行车的平衡。其中，i＝1、2、3，即，K1为第一条边的动态稳定裕度，K2为第二条边的动态稳定裕度，K3为第三条边的动态稳定裕度；等式
Figure BDA0002061737340000134
说明动态稳定裕度Ki越大，误差δi越小。PID控制器的流程图如图2所示。A PID controller is set for the dynamic stability margin of the three sides of the dynamic grounding triangle in step (4) to control the dynamic balance of the trailer bicycle, and the PID controller is based on
Figure BDA0002061737340000133
PID operation is performed as an error value, and a current is output, and the current is input to the motor of thehandlebar 2 of the trailer-type bicycle, so as to control the balance of the trailer-type bicycle. Among them, i=1, 2, 3, that is, K1 is the dynamic stability margin of the first edge, K2 is the dynamic stability margin of the second edge, and K3 is the dynamic stability margin of the third edge. ; equation
Figure BDA0002061737340000134
It shows that the larger the dynamic stability margin Ki is, the smaller the error δi is. The flow chart of the PID controller is shown in Figure 2.

所述PID运算表达式为：The PID operation expression is:

Figure BDA0002061737340000131
式中，I为所述拖挂式自行车车把2的电机电流，通过调节I便可达到对车把2的调节，进而实现对拖挂式自行车动态稳定进行控制，KPi、KDi、KIi为PID调节参数，
Figure BDA0002061737340000132
为δ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所示。
Figure BDA0002061737340000131
In the formula, I is the motor current of thehandlebar 2 of the trailer bicycle, and the adjustment of thehandlebar 2 can be achieved by adjusting I, and then the dynamic stability of the trailer bicycle can be controlled, KPi , KDi , KIi is the PID adjustment parameter,
Figure BDA0002061737340000132
is the first derivative of δi , i=1, 2, 3. When the value of Ki is larger, the values of KPi , KDi , and KIi are smaller. For example, when S1 > S2 > S3 , KP3 > KP2 > KP1 , KD3 > KD2 > KD1 , KI3 > KI2 > KI1 , that is, the larger the Si is, the slower the adjustment will be. As for the three trapezoidal areas of the dynamic grounding triangle, the size is quickly adjusted to a similar size, so that the trailing bicycle can reach the most stable state. Adjusting KPi , KDi , and KIi to appropriate values, and feeding back the data measured by each sensor at each moment, can achieve the purpose of controlling the stability of the trailer bicycle. The flow chart of the PID controller is shown in Figure 2.

Claims (4)

1. A method for estimating stability margin of a pull-type bicycle is characterized in that:
the method comprises the following steps of estimating a dynamic stability margin:
(1) calculating the coordinates of the ZMP in the coordinate system of the earth (7) and the coordinates of three grounding points of the trailer bicycle, which are contacted with the ground, in the coordinate system of the earth (7) by utilizing the attitude detection data of the trailer bicycle;
(2) connecting the three grounding points to obtain a dynamic grounding triangle;
(3) performing ZMP to form parallel lines of a first side of the dynamic grounding triangle to obtain first parallel lines, wherein the maximum trapezoid formed by the first parallel lines and the first side is a first trapezoid in the range of the dynamic grounding triangle; a ZMP is used for making a parallel line of a second side of the dynamic grounding triangle to obtain a second parallel line, and the maximum trapezoid formed by the second parallel line and the second side is a second trapezoid in the range of the dynamic grounding triangle; a ZMP is used as a parallel line of a third edge of the dynamic grounding triangle to obtain a third parallel line, and the maximum trapezoid formed by the third parallel line and the third edge is a third trapezoid in the range of the dynamic grounding triangle;
(4) the stability margin of the first side of the dynamic grounding triangle is the ratio of the area of the first trapezoid to the area of the current dynamic grounding triangle, and the dynamic stability margin of the first side is obtained; the stability margin of the second side of the dynamic grounding triangle is the ratio of the area of the second trapezoid to the area of the current dynamic grounding triangle, and the dynamic stability margin of the second side is obtained; the stability margin of the third side of the dynamic grounding triangle is the ratio of the area of the third trapezoid to the area of the current dynamic grounding triangle, and the dynamic stability margin of the third side is obtained;
in the step (1), the posture detection data includes three euler angles and three corresponding euler angular velocities of the trailer frame (1) of the trailer bicycle, a rotation angle and an angular velocity of the handlebar (2) of the trailer bicycle around the trailer frame (1), an angular velocity of the front wheel (3) of the trailer bicycle around the handlebar (2), an angular velocity of the rear wheel (4) of the tractor of the trailer bicycle around the trailer frame (1), a rotation angle and an angular velocity of the trailer frame (5) of the trailer bicycle left and right around the trailer frame (1), a rotation angle and an angular velocity of the trailer frame (5) of the trailer bicycle up and down around the trailer frame (1), and an angular velocity of the trailer wheel (6) of the trailer bicycle around the trailer frame (5); the coordinates of the ZMP in the coordinate system of the earth (7) in the step (1) satisfy the formula:
Figure FDA0003480947570000021
wherein M is the total mass of the trailer bicycle; g is the acceleration of gravity; x is the number ofc、ycCoordinates of the center of mass of the trailer bicycle under a geodetic (7) coordinate system; px、Py、PzIs the component of the linear momentum of the trailer bicycle on the x, y and z axes of a coordinate system of the ground (7),
Figure FDA0003480947570000022
is PxA first derivative of (a) is obtained,
Figure FDA0003480947570000023
is PyA first derivative of (a) is obtained,
Figure FDA0003480947570000024
is PzA first derivative of (1); l isx、LyIs the component of the angular momentum rotating around the center of the earth (7) coordinate system on the x and y axes,
Figure FDA0003480947570000025
is LxA first derivative of (a) is obtained,
Figure FDA0003480947570000026
is LyA first derivative of (1); x is the number ofzmp、yzmp、zzmpIs the coordinate of ZMP under the coordinate system of the ground (7), and z is the coordinate of the trailer bicycle when the trailer bicycle moves on the flat groundzmp＝0；
In the step (1), the three grounding points of the pull-type bicycle, which are in contact with the ground, are respectively a first grounding point, a second grounding point and a third grounding point, and the coordinate calculation steps of the first grounding point, the second grounding point and the third grounding point under a geodetic (7) coordinate system are as follows:
1) calculating the angular speed of the tractor frame (1) under a geodetic (7) coordinate system through the three Euler angular speeds of the tractor frame (1);
2) calculating the angular speed of the rear wheel (4) of the tractor under a geodetic (7) coordinate system according to the angular speed of the tractor frame (1);
3) calculating the linear speed of the origin of the coordinate system of the rear wheel (4) of the tractor under the coordinate system of the ground (7) through the angular speed of the rear wheel (4) of the tractor;
4) performing integral operation on the linear speed to obtain the coordinate of the origin of the coordinate system of the rear wheel (4) of the tractor under the coordinate system of the ground (7);
5) calculating coordinates of the first grounding point, the second grounding point and the coordinate system of the trailer frame (5) under the coordinate system of the earth (7) through the coordinates and coordinate transformation of the origin of the coordinate system of the rear wheel (4) of the tractor under the coordinate system of the earth (7);
6) and calculating the coordinate of the third grounding point in the coordinate system of the earth (7) according to the coordinate of the origin of the coordinate system of the trailer frame (5) in the coordinate system of the earth (7).
2. The method for estimating the stability margin of a pull-type bicycle according to claim 1, wherein:
further comprising an estimation of a static stability margin;
the static stability margin estimation step comprises the following steps:
the three grounding points of the pulling type bicycle in the static state, which are contacted with the ground, form a static grounding triangle, and the static stability margin is obtained by the ratio of the area of the static grounding triangle to the area of the maximum triangle formed by the three grounding points.
3. A stability margin estimation method of a pull-type bicycle according to claim 1 or 2, wherein:
comprises a PID controller;
the PID controller to
Figure FDA0003480947570000031
Performing a PID operation as an error value, outputting a current, which is input to a motor of the handlebar (2) of the pull-type bicycle to control the balance of the pull-type bicycle, wherein i is 1,2,3, K1Is the dynamic stability margin of the first edge, K2Is the dynamic stability margin of the second side, K3Is the dynamic stability margin of the third edge.
4. The method of claim 3, wherein the step of estimating the stability margin of the trailer-type bicycle comprises:
the PID operation expression is as follows:
Figure FDA0003480947570000041
wherein I is the pull-type bicycleMotor current of handlebar (2), KPi、KDi、KIiIn order to adjust the parameters for the PID,
Figure FDA0003480947570000042
is deltaiA first derivative of (1).
CN201910407550.8A2019-05-152019-05-15 Stability margin estimation method and control method for a trailer-mounted bicycleActiveCN110083075B (en)

Priority Applications (1)

Application NumberPriority DateFiling DateTitle
CN201910407550.8ACN110083075B (en)2019-05-152019-05-15 Stability margin estimation method and control method for a trailer-mounted bicycle

Applications Claiming Priority (1)

Application NumberPriority DateFiling DateTitle
CN201910407550.8ACN110083075B (en)2019-05-152019-05-15 Stability margin estimation method and control method for a trailer-mounted bicycle

Publications (2)

Publication NumberPublication Date
CN110083075A CN110083075A (en)2019-08-02
CN110083075Btrue CN110083075B (en)2022-04-08

Family

ID=67420441

Family Applications (1)

Application NumberTitlePriority DateFiling Date
CN201910407550.8AActiveCN110083075B (en)2019-05-152019-05-15 Stability margin estimation method and control method for a trailer-mounted bicycle

Country Status (1)

CountryLink
CN (1)CN110083075B (en)

Citations (9)

* Cited by examiner, † Cited by third party
Publication numberPriority datePublication dateAssigneeTitle
JPH10217161A (en)*1997-01-311998-08-18Honda Motor Co Ltd Gait generator for legged mobile robot
JP2002326173A (en)*2001-04-272002-11-12Honda Motor Co Ltd Motion generation device for legged mobile robot
CN101414190A (en)*2008-10-282009-04-22北京理工大学Method and system for controlling apery robot stabilized walking based on effective stable domain
CN101950176A (en)*2010-09-022011-01-19北京理工大学Method for performing zero moment point (ZMP) calibration autonomously by robot
CN103963782A (en)*2014-05-092014-08-06济南大学Pull type mobile robot parallel parking method
CN106354160A (en)*2016-09-202017-01-25济南大学Direction angle control method of n-section pull-type moving robot during reverse motion
CN106547206A (en)*2016-06-292017-03-29中南大学A kind of multi-foot robot dynamic stability based on strength of one's legs algorithm for estimating quantitatively judges method
CN106737669A (en)*2016-12-122017-05-31杭州宇芯机器人科技有限公司Consider the multi-foot robot energy margin computational methods of external impacts interference and damping
CN107168061A (en)*2017-05-312017-09-15江西制造职业技术学院The gait planning device and its planing method of a kind of anthropomorphic robot similitude motion

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication numberPriority datePublication dateAssigneeTitle
JPH10217161A (en)*1997-01-311998-08-18Honda Motor Co Ltd Gait generator for legged mobile robot
JP2002326173A (en)*2001-04-272002-11-12Honda Motor Co Ltd Motion generation device for legged mobile robot
CN101414190A (en)*2008-10-282009-04-22北京理工大学Method and system for controlling apery robot stabilized walking based on effective stable domain
CN101950176A (en)*2010-09-022011-01-19北京理工大学Method for performing zero moment point (ZMP) calibration autonomously by robot
CN103963782A (en)*2014-05-092014-08-06济南大学Pull type mobile robot parallel parking method
CN106547206A (en)*2016-06-292017-03-29中南大学A kind of multi-foot robot dynamic stability based on strength of one's legs algorithm for estimating quantitatively judges method
CN106354160A (en)*2016-09-202017-01-25济南大学Direction angle control method of n-section pull-type moving robot during reverse motion
CN106737669A (en)*2016-12-122017-05-31杭州宇芯机器人科技有限公司Consider the multi-foot robot energy margin computational methods of external impacts interference and damping
CN107168061A (en)*2017-05-312017-09-15江西制造职业技术学院The gait planning device and its planing method of a kind of anthropomorphic robot similitude motion

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
拖挂式自行车机器人机构设计与非完整约束分析;黄用华;《机械设计与研究》;20180630;第34卷(第3期);第54-57、62页*

Also Published As

Publication numberPublication date
CN110083075A (en)2019-08-02

Similar Documents

PublicationPublication DateTitle
CN108482379B (en)Wheel-hub motor driven vehicle coefficient of road adhesion and the synchronous real-time estimation system and method for road gradient
He et al.Path tracking control method and performance test based on agricultural machinery pose correction
US20210240192A1 (en)Estimation method and estimator for sideslip angle of straight-line navigation of agricultural machinery
CN112050809B (en)Wheel type odometer and gyroscope information fusion unmanned vehicle directional positioning method
CN109606378B (en) A Vehicle Driving State Estimation Method for Non-Gaussian Noise Environment
CN110001637A (en)A kind of pilotless automobile path following control device and control method based on multiple spot tracking
CN112014849B (en)Unmanned vehicle positioning correction method based on sensor information fusion
CN109324616B (en) Alignment method of unmanned parking and handling robot based on on-board sensors
CN113060143B (en) A system and method for determining pavement adhesion coefficient
CN108646747A (en)Agri-vehicle path tracking control method
CN114966629A (en)Vehicle body laser radar external reference calibration method based on EKF algorithm framework
WO2020191977A1 (en)Dead reckoning method of automatic parking positioning system
CN111750897B (en) A yaw rate gyroscope bias estimation method based on Romberg observer
CN107016157A (en)Distributed-driving electric automobile pavement self-adaptive longitudinal direction speed estimating system and method
CN110007667A (en)A kind of crawler tractor and its path tracking control method and system
WO2019047509A1 (en)Two-wheel monorail vehicle and balance control method therefor
CN102717726A (en)Electronic differential control method and electrically driven mine car using same
CN114217620A (en)Intelligent obstacle avoidance control system and method for wheeled robot
CN108860137B (en) Control method, device and intelligent vehicle for unstable vehicle
CN110083075B (en) Stability margin estimation method and control method for a trailer-mounted bicycle
WO2025060782A1 (en)Yaw rate-based trajectory tracking method for tracked vehicle
CN110104102B (en) Estimation Method of Longitudinal Slip State of Self-balancing Bicycle Driving Wheel
CN110155236B (en)Boolean judgment method for toppling direction of trailing bicycle based on ZMP
CN110949499B (en) A commercial vehicle unmanned steering angle compensation system and its control method
KR102497027B1 (en)Control method and system for estimating position of vehicle

Legal Events

DateCodeTitleDescription
PB01Publication
PB01Publication
SE01Entry into force of request for substantive examination
SE01Entry into force of request for substantive examination
GR01Patent grant
GR01Patent grant
EE01Entry into force of recordation of patent licensing contract
EE01Entry 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
[8]ページ先頭
©2009-2025 Movatter.jp