CN110155236B - Boolean judgment method for toppling direction of trailing bicycle based on ZMP - Google Patents
Boolean judgment method for toppling direction of trailing bicycle based on ZMP Download PDFInfo
- Publication number
- CN110155236B CN110155236B CN201910430131.6A CN201910430131A CN110155236B CN 110155236 B CN110155236 B CN 110155236B CN 201910430131 A CN201910430131 A CN 201910430131A CN 110155236 B CN110155236 B CN 110155236B
- Authority
- CN
- China
- Prior art keywords
- vector
- coordinate system
- zmp
- trailer
- tractor
- 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 20
- 239000013598 vector Substances 0.000 claims abstract description 150
- 230000009466 transformation Effects 0.000 claims description 8
- 238000011156 evaluation Methods 0.000 claims description 5
- 231100001261 hazardous Toxicity 0.000 claims description 5
- 230000001133 acceleration Effects 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 241000764238 Isis Species 0.000 claims description 2
- 238000010586 diagram Methods 0.000 description 13
- 230000005540 biological transmission Effects 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000007405 data analysis Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
- B62K—CYCLES; CYCLE FRAMES; CYCLE STEERING DEVICES; RIDER-OPERATED TERMINAL CONTROLS SPECIALLY ADAPTED FOR CYCLES; CYCLE AXLE SUSPENSIONS; CYCLE SIDE-CARS, FORECARS, OR THE LIKE
- B62K13/00—Cycles convertible to, or transformable into, other types of cycles or land vehicle
- B62K13/02—Cycles convertible to, or transformable into, other types of cycles or land vehicle to a tandem
- B62K13/025—Cycles convertible to, or transformable into, other types of cycles or land vehicle to a tandem from two or more cycles
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
- B62K—CYCLES; CYCLE FRAMES; CYCLE STEERING DEVICES; RIDER-OPERATED TERMINAL CONTROLS SPECIALLY ADAPTED FOR CYCLES; CYCLE AXLE SUSPENSIONS; CYCLE SIDE-CARS, FORECARS, OR THE LIKE
- B62K27/00—Sidecars; Forecars; Trailers or the like specially adapted to be attached to cycles
- B62K27/10—Other component parts or accessories
- B62K27/12—Coupling parts for attaching cars or the like to cycle; Arrangements thereof
Landscapes
- Engineering & Computer Science (AREA)
- Mechanical Engineering (AREA)
- Transportation (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Length Measuring Devices With Unspecified Measuring Means (AREA)
Abstract
The invention discloses a Boolean judging method of a toppling direction of a trailed bicycle based on a ZMP, which is characterized in that the ZMP coordinate and the coordinates of three grounding points are calculated by combining each corner, angular velocity, size parameters and the like of the trailed bicycle, a vector triangle is constructed by the three grounding points, the inner product operation is carried out on a direction vector and a characteristic vector, a corresponding symbolic function is used for judging dangerous edges, and the toppling direction is confirmed by the number of the dangerous edges.
Description
Technical Field
The invention relates to the technical field of self-balancing bicycles, in particular to a Boolean judgment method for the toppling direction of a trailed bicycle based on ZMP.
Background
The trailer type bicycle is a novel self-balancing bicycle road running mechanism composed of a double-wheel tractor and a single-wheel trailer, and the rigid body structure of the trailer type bicycle mainly comprises a tractor frame, a handlebar, a tractor front wheel, a tractor rear wheel, a trailer frame and trailer wheels. The trailed bicycle is provided with three grounding wheels, and is a moving mechanical system with three grounding points.
For a two-wheeled self-balancing bicycle, the bicycle is usually balanced by reducing the inclination angle of the bicycle body, i.e., the inclination angle is used to measure the degree of deviation of the bicycle from the balance. Research finds that the error is larger when the measuring method is applied to the pulling type bicycle. The trailer type bicycle mainly comprises a double-wheel tractor and a single-wheel trailer, and when the toppling direction is judged, the lateral inclination angle of the double-wheel tractor or the single-wheel trailer can not be used as an judging object, and the simple algebraic sum of the double-wheel tractor and the single-wheel trailer can not be used as the judging object. Therefore, a method for judging the falling direction of the trailer bicycle is required to be provided so as to ensure the accuracy of the subsequent data analysis and the balance control.
Disclosure of Invention
The invention provides a Boolean judgment method for the toppling direction of a trailing bicycle based on ZMP, which solves the problem that the toppling direction is not accurately judged in the prior art.
The invention solves the technical problem by the following technical scheme:
a Boolean judgment method for the toppling direction of a trailing bicycle based on ZMP comprises the following steps:
(1) calculating the ZMP coordinates and the coordinates of three grounding points of the trailer bicycle, which are contacted with the ground 7, and constructing a vector triangle by the three grounding points; the vector triangle comprises three directed edges which are respectively a first directed edge, a second directed edge and a third directed edge;
(2) selecting any two adjacent directed edges from the vector triangle to perform cross product operation to obtain a direction vector of the vector triangle;
(3) connecting the starting end of the first directed edge with the ZMP to obtain a first vector, wherein the direction of the first vector is that the starting end points to the ZMP; connecting the ZMP with the first directed edge stopping end to obtain a second vector, wherein the direction of the second vector is that the ZMP points to the stopping end; performing cross product operation on the first vector and the second vector to obtain a first feature vector; performing inner product operation on the direction vector and the first feature vector to obtain a symbolic function, and judging whether the first directed edge is a dangerous edge or a non-dangerous edge according to the result of the symbolic function;
(4) judging the dangerous edges of the second directed edge and the third directed edge according to the method in the step (3);
(5) and (4) obtaining the total number of the dangerous edges according to the steps (3) and (4), if the total number of the dangerous edges is 1, the dumping direction is the normal direction of the dangerous edge, and if the total number of the dangerous edges is 2, the dumping direction is the synthetic direction of the normal directions of the 2 dangerous edges.
Further, in step (1), the coordinates of the ZMP in the geodetic 7 coordinate system satisfy the formula:
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、PzThe components of the linear momentum of the trailer bicycle on the x axis, the y axis and the z axis of the coordinate system of the earth 7 are respectively,
is PxA first derivative of (a) is obtained,
is PyA first derivative of (a) is obtained,
is PzA first derivative of (1); l isx、LyThe components of angular momentum rotating around the center of a coordinate system of the earth 7 on the x and y axes,
is LxA first derivative of (a) is obtained,
is LyA first derivative of (1); x is the number ofzmp、yzmp、zzmpAs the coordinates of ZMP in the geodetic 7 coordinate systemWhen the trailer bicycle moves on the flat ground zzmp＝0。
Further, in the step (1), the three grounding points are respectively a first grounding point, a second grounding point and a third grounding point, and the coordinate calculation step of the first grounding point, the second grounding point and the third grounding point in a ground 7 coordinate system is as follows:
1) calculating the angular velocity of the tractor frame 1 under a geodetic 7 coordinate system according to the three Euler angular velocities of the tractor frame 1 of the trailer bicycle;
2) calculating the angular velocity of the tractor rear wheel 4 of the trailer bicycle in a geodetic 7 coordinate system according to the angular velocity of the tractor frame 1;
3) calculating the linear velocity of the origin of the coordinate system of the rear wheel 4 of the tractor under the coordinate system of the ground 4 according to the angular velocity of the rear wheel 4 of the tractor;
4) performing integral operation on the linear speed to obtain the coordinates of the origin of the coordinate system of the rear wheel 4 of the tractor in the coordinate system of the ground 7;
5) calculating the coordinates of the first grounding point, the second grounding point and the origin of the coordinate system of the trailer frame 5 of the trailer bicycle in the coordinate system of the ground 7 through the coordinates of the origin of the coordinate system of the rear wheel 4 of the tractor in the coordinate system of the ground 7 and coordinate transformation;
6) the coordinates of the third grounding point in the coordinate system of the ground 7 are calculated through the coordinates of the origin of the coordinate system of the trailer frame 5 in the coordinate system of the ground 7.
Further, in step (3), the first ground point is P1, the second ground point is P2, and the third ground point is P3, and the direction vector expression is:
further, in the step (3), the ZMP is the point A; the first directed edge is
The first vector is
The second vector is
The first feature vector expression is
The first feature vector is 0, or the direction of the first feature vector is the same as the direction of the direction vector, or the direction of the first feature vector is opposite to the direction of the direction vector.
Further, in step (3), the direction vector and the first feature vector perform an inner product operation, and the expression is
The symbolic function expression is
The sign1(x1) When 1 or 0, the first directed edge is
In order to be a dangerous edge,
when the first directed edge is
Is a non-hazardous edge.
Compared with the prior art, the method has the following characteristics:
the method for judging the toppling direction by the Boolean comprises the steps of calculating a ZMP coordinate and coordinates of three grounding points by combining all rotation angles, angular speeds, size parameters and the like of a pull-type bicycle, constructing a vector triangle by the three grounding points, carrying out inner product operation on a direction vector and a characteristic vector, using a corresponding symbolic function as judgment of dangerous edges, and confirming the toppling direction by the number of the dangerous edges.
Drawings
FIG. 1 is a diagram of the present invention of evaluating directed edges when a ZMP is within a vector triangle
And (4) constructing a characteristic vector diagram for the dangerous edge.
FIG. 2 is a diagram of the present invention of evaluating directed edges when a ZMP is within a vector triangle
And (4) constructing a characteristic vector diagram for the dangerous edge.
FIG. 3 is a diagram of the present invention of evaluating directed edges when a ZMP is within a vector triangle
And (4) constructing a characteristic vector diagram for the dangerous edge.
FIG. 4 is a diagram of a ZMP of the present invention being evaluated as a directed edge outside a vector triangle
And (4) constructing a characteristic vector diagram for the dangerous edge.
FIG. 5 is a diagram of a ZMP of the present invention being evaluated as a directed edge outside a vector triangle
And (4) constructing a characteristic vector diagram for the dangerous edge.
FIG. 6 is a diagram of a ZMP of the present invention being evaluated as a directed edge outside a vector triangle
And (4) constructing a characteristic vector diagram for the dangerous edge.
Fig. 7 is a mechanical structural view of a prior art pull-type bicycle.
The reference numbers in the figures are: 1. a tractor frame; 2. a handlebar, 3, a front wheel of the tractor; 4. a rear wheel of the tractor; 5. a trailer frame; 6. trailer wheels; 7. and (4) the earth.
FIG. 8 is a chain of coordinate mapping calculations.
Detailed Description
The present invention will be further described with reference to the following examples, but the present invention is not limited to these examples.
The pulling type bicycle comprises the following rigid bodies: tractor frame 1, handlebar 2, tractor front wheel 3, tractor rear wheel 4, trailer frame 5 and trailer wheel 6. A motor is arranged on the handlebar 2, and the motor is matched with the front wheel 3 of the tractor through a gear transmission mechanism to drive the front wheel 3 of the tractor to rotate; the tractor frame 1 is provided with a motor which is matched with the handlebar 2 through a gear transmission mechanism to drive the handlebar 2 to rotate. The trailer frame 1 and the trailer frame 5 are connected through a cross shaft assembly, and the trailer frame 5 can rotate up and down and rotate left and right around the trailer frame 1. The mechanical structure of the trailer bicycle is shown in fig. 7.
The method comprises the following steps that a gyroscope is installed on a tractor frame 1, and three Euler angles and three corresponding Euler angular velocities of the tractor frame 1 are detected; an incremental encoder and an absolute encoder are installed on a tractor frame 1, and the rotation angle and the angular speed of a handlebar 2 around the tractor frame 1 are detected; an incremental encoder is arranged on the handlebar 2, and the angular speed of the front wheel 3 of the tractor around the handlebar 2 is detected; an incremental encoder is arranged on the tractor frame 1, and the angular speed of the tractor rear wheel 4 around the tractor frame 1 is detected; 2 incremental encoders and 2 absolute encoders are mounted on the tractor frame 1, and the rotation angle and the angular speed of the trailer frame 5 rotating around the tractor frame 1 left and right and the rotation angle and the angular speed of the trailer frame 5 rotating around the tractor frame 1 up and down are detected; an incremental encoder is mounted on the trailer frame 5 to detect the angular velocity of the trailer wheels 6 rotating about the trailer frame 5.
The detection data of the gyroscope, the incremental encoder and the absolute encoder specifically include: three euler angles q of a trailer frame 1 of the pull-type bicycle1、q2、q3Corresponding three ohmsPull angular velocity
The handlebar 2 of the trailer bicycle is around the corner and angular velocity of the frame 1 of the tractor
The angular speed of the front wheel 3 of the tractor of the trailer bicycle around the handlebar 2
The angular speed of the tractor rear wheel 4 of the trailer bicycle around the tractor frame 1
The corner and the angular speed of the trailer frame 5 of the trailer bicycle rotating around the tractor frame 1
The angle and the angular speed of the trailer frame 5 of the trailer bicycle rotating up and down around the trailer frame 1
Angular velocity of trailer wheel 6 of said trailer bicycle rotating around trailer frame 5
The invention provides a Boolean judgment method for the toppling direction of a trailed bicycle based on ZMP, which comprises the following steps:
(1) calculating the ZMP coordinates and the coordinates of three grounding points of the trailer bicycle, which are contacted with the ground 7, and constructing a vector triangle by the three grounding points; the vector triangle comprises three directed edges which are respectively a first directed edge, a second directed edge and a third directed edge;
(2) selecting any two adjacent directed edges from the vector triangle to perform cross product operation to obtain a direction vector of the vector triangle;
(3) connecting the starting end of the first directed edge with the ZMP to obtain a first vector, wherein the direction of the first vector is that the starting end points to the ZMP; connecting the ZMP with the first directed edge stopping end to obtain a second vector, wherein the direction of the second vector is that the ZMP points to the stopping end; performing cross product operation on the first vector and the second vector to obtain a first feature vector; performing inner product operation on the direction vector and the first feature vector to obtain a symbolic function, and judging whether the first directed edge is a dangerous edge or a non-dangerous edge according to the result of the symbolic function;
(4) judging the dangerous edges of the second directed edge and the third directed edge according to the method in the step (3);
(5) and (4) obtaining the total number of the dangerous edges according to the steps (3) and (4), if the total number of the dangerous edges is 1, the dumping direction is the normal direction of the dangerous edge, and if the total number of the dangerous edges is 2, the dumping direction is the synthetic direction of the normal directions of the 2 dangerous edges.
In the step (1), the coordinates of the ZMP in the geodetic 7 coordinate system satisfy the formula:
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 coordinate system; px、Py、PzIs the component of the linear momentum of the trailer bicycle on the x, y and z axes of a geodetic coordinate system, Px、Py、PzAs a function of the linear velocity of each rigid body calculated from the corresponding detected angular velocity;
is PxA first derivative of (a) is obtained,
is PyA first derivative of (a) is obtained,
is PzA first derivative of (1); l isx、LyIs the component of angular momentum rotating around the center of the earth coordinate system on the x and y axes, Lx、LyAs angular momentum, as a function of angular velocity for each rigid body,
is LxA first derivative of (a) is obtained,
is LyA first derivative of (1); x is the number ofzmp、yzmp、zzmpIs the coordinate of ZMP in the geodetic coordinate system, z when the trailer bicycle moves on the flat groundzmp＝0。
The invention relates to a coordinate system which comprises 7 coordinate systems: a global coordinate system fixed on the earth 7, i.e. the earth 7 coordinate system, with the origin of coordinates O0(ii) a A coordinate system of the frame 1 of the tractor with an origin of coordinates at the geometric centre of the front wheel 3 of the tractor, the origin of coordinates being O1(ii) a A coordinate system of the handlebar 2, and an origin of coordinates at the intersection point of the tractor frame 1 and the axis of the handlebar 2, the origin of coordinates being O2(ii) a The coordinate system of the front wheel 3 of the tractor, and the origin of coordinates is in the geometric center of the front wheel 3 of the tractor; origin of coordinates of O3(ii) a The coordinate system of the tractor rear wheel 4, and the origin of coordinates is at the geometric center of the tractor rear wheel 4, the origin of coordinates is O4(ii) a The coordinate system of the trailer frame 5 and the origin of coordinates is at the geometric center of the cross axle and the origin of coordinates is O5(ii) a The coordinate system of the trailer wheel 6 and the origin of coordinates at the geometric center of the trailer wheel 6, the origin of coordinates being O6。
Each detected angular velocity is detected in the coordinate system where the mounting position is located, and when angular velocity data is used, the angular velocity data is subjected to coordinate conversion and converted into an angular velocity in the coordinate system of the earth 7. Thus, the angle of rotation of the handlebar 2 about the tractor frame 1, the angle of rotation of the trailer frame 5 about the tractor frame 1, and the angle of rotation of the trailer frame 5 about the trailer frame 1 are detectedThe rotation angles of the frame 1 rotating up and down are used for acquiring rotation matrixes between a coordinate system where each rotation angle is located and a geodetic 7 coordinate system, and the corresponding angular speed is converted into the angular speed under the geodetic 7 coordinate system by using the rotation matrixes, so that the linear speed under the geodetic 7 coordinate system can be acquired, and further P can be acquiredx、Py、Pz、Lx、LyFurther, the coordinates of ZMP in the geodetic 7 coordinate system are obtained.
In the step (1), the three grounding points of the trailer bicycle contacting the ground are respectively a first grounding point, a second grounding point and a third grounding point, i.e. P1, P2 and P3 in fig. 7, and the coordinate calculation steps of the first grounding point, the second grounding point and the third grounding point in the coordinate system of the ground 7 are as follows:
1) calculating the coordinate system O of the tractor frame 1 in the ground 7 according to the three Euler angular velocities of the tractor frame 10Angular velocity of the rotor;
2) calculating the coordinate system O of the rear wheel 4 of the tractor in the ground 7 according to the angular velocity of the tractor frame 10Angular velocity of the rotor;
3) calculating the origin O of the coordinate system of the rear wheel 4 of the tractor according to the angular velocity of the rear wheel 4 of the tractor4Linear velocity in the geodetic 7 coordinate system;
4) performing integral operation on the linear velocity to obtain an origin O of a coordinate system where the rear wheel 4 of the tractor is located4Coordinates in the geodetic 7 coordinate system;
5) through the origin O of the coordinate system of the rear wheel 4 of the tractor4Transforming coordinates and coordinates in a coordinate system of the ground 7 to calculate the first grounding point, the second grounding point and an origin O of the coordinate system of the trailer frame 55Coordinates in the geodetic 7 coordinate system;
6) through the origin O of the coordinate system in which the trailer frame 5 is located5The coordinates of the third grounding point in the coordinate system of the earth 7 are calculated.
In the step 1), the expression of the angular speed of the tractor frame 1 in the coordinate system of the ground 7 is as follows:
in the formula, ωB1For the angular velocity, s, of the tractor frame 1 in the geodetic 7 coordinate system1＝sin(q1)，s2＝sin(q2)，c1＝cos(q1)，c2＝cos(q2)，c3＝cos(q3)。
In step 2), the expression of the angular velocity of the rear wheel 4 of the tractor in the coordinate system of the ground 7 is as follows:
in the formula, ωB4Is the angular velocity, s, of the rear wheel 4 of the tractor in the coordinate system of the ground 71＝sin(q1)，s2＝sin(q2)，c1＝cos(q1)，c2＝cos(q2)。
In step 3), the origin O of the coordinate system of the rear wheel 4 of the tractor4The expression for the linear velocity in the geodetic 7 coordinate system is:
in the formula, vB4The origin O of the coordinate system in which the rear wheel 4 of the tractor is located4Linear velocity, s, in the geodetic 7 coordinate system1＝sin(q1)，s2＝sin(q2)，c1＝cos(q1)，c2＝cos(q2) And r is the radius of the rear wheel 4 of the tractor.
In the step 4), the origin O of the coordinate system of the rear wheel 4 of the tractor4The coordinate expression under the geodetic 7 coordinate system is:
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.
In step 5), the coordinates of the first grounding point P1 in the geodetic 7 coordinate system are calculated: under the geodetic 7 coordinate system
Vector passing through coordinate system of tractor rear wheel 4
I.e., [ 00-l1]TRotational transformation to obtain1Is O4To O3The distance of (c). Under the geodetic 7 coordinate system
Vector passing through the coordinate system of the front wheel 3 of the tractor
I.e. [ 00-r ]]TThe rotation transformation is obtained under the coordinate system of the earth 7
The coordinates of the first grounding point P1 in the coordinate system of the earth 7 can be found.
In step 5), the coordinates of the second grounding point P2 in the geodetic 7 coordinate system are calculated: obtaining vectors under the geodetic 7 coordinate system
Under the geodetic 7 coordinate system
Vector passing through coordinate system of tractor rear wheel 4
I.e. [ 00-r ]]TThe rotation transformation is carried out, under the coordinate system of the earth 7,
the coordinates of a second grounding point, i.e. the second grounding point P2 in fig. 8, in the coordinate system of the earth 7 can be found;
in step 5), calculating a second grounding point O5Coordinates in the geodetic 7 coordinate system: vector through the coordinate system of the rear wheel 4 of the tractor
I.e., [ -l ]x1 0 lz1]TRotational transformation to obtainx1Is O4To O5Along the x-axis of the coordinate system in which the rear wheels 4 of the tractor are located, lzIs O4To O5The distance along the z-axis direction of the coordinate system of the rear wheel 4 of the tractor is determined by the distance, under the coordinate system of the ground 7,
can find O5The coordinates of the point in the geodetic 7 coordinate system.
In step 6), passing through the vector of the coordinate system of the trailer frame 5
I.e., [ -l ]x2 0 -lz2]TRotational transformation to obtainx2Is O5To O6In the x-axis direction of the coordinate system in which the trailer frame 5 is located, lzIs O5To O6The distance along the z-axis direction of the coordinate system in which the trailer frame 5 is located; under the geodetic 7 coordinate system
Vector that can pass through the coordinate system of the trailer wheel 6
I.e. [ 00-r ]]TThe rotation transformation is carried out, under the coordinate system of the earth 7,
the coordinates of the third grounding point in the coordinate system of the earth 7 can be found.
The steps 5) and 6) above relate to the calculation of the chain map of the associated coordinate map as shown in fig. 8. The above-described method for calculating the coordinates of ZMPs and three grounding points is prior art.
The trailer bicycle is balanced by the common point force of three grounding points P1, P2 and P3, a vector triangle delta P1P2P3 constructed by the three grounding points changes in real time, and vector arrows are connected end to form a closed triangle. The first directed edge of the vector triangle Δ P1P2P3 is
The second directed edge is
The third directed edge is
According to the direction of the vector arrow, performing cross product operation on any two adjacent directed edges to obtain a direction vector
Whether three ground points are collinear or not can be judged through the direction vector, if the direction vector is 0, the three ground points are shown to be on the same straight line, at the moment, three edges are dangerous edges, if the direction vector is not 0, the three ground points are not collinear, whether the trailer bicycle is balanced or not can be judged only, and specific which edge is the dangerous edge cannot be judged, so that unbalance is caused, the toppling direction can be judged only through the direction vector, and the accuracy is low. The invention judges the dumping direction by utilizing the relation between the ZMP and the direction vector, and can effectively improve the accuracy of judging the dangerous edge and the dumping direction. In the context of figures 1 to 6,
indicates a direction perpendicular to the paper surface inwards, indicates a direction perpendicular to the paper surfaceAnd outwards.
FIGS. 1-3 show oriented edges for evaluation when ZMP is within a vector triangle
And (4) constructing a characteristic vector diagram for the dangerous edge. Evaluating the first directed edge of the ZMP of FIG. 1 when inside a vector triangle
Whether it is a dangerous edge: ZMP is A, and a first vector is constructed
Constructing a second vector
Obtaining a first feature vector
The first characteristic vector is 0, or the direction of the first characteristic vector is the same as that of the direction vector, or the direction of the first characteristic vector is opposite to that of the direction vector; the direction vector and the first feature vector perform an inner product operation, i.e.,
obtain the following sign function
If sign1(x1) When the direction vector is the same as the first feature vector, the first directional edge is described as 1
As a dangerous side if sign1(x1) When 0, the ZMP is illustrated at the first directed edge
Upper, first directed edge
As a dangerous edge, if sign1(x1) 1, the direction of the first characteristic vector is opposite to the direction vector, and the first directed edge
Is a non-hazardous edge.
ZMP of FIG. 2 evaluates second directed edge when within vector triangle
Whether it is a dangerous edge: ZMP is A, and a third vector is constructed
Constructing a fourth vector
Obtaining a second feature vector
The second feature vector is 0, or the direction of the second feature vector is the same as that of the direction vector, or the direction of the second feature vector is opposite to that of the direction vector; the direction vector and the second feature vector perform an inner product operation, i.e.,
obtain the following sign function
If sign2(x2) When the direction vector is the same as the direction vector, the second feature vector has a second directed edge of 1
As a dangerous side if sign2(x2) When 0, the ZMP is illustrated at the second directed edge
Upper, second directed edge
As a dangerous edge, if sign2(x2) The second feature vector is opposite to the direction vector direction, and the second directional edge is 1
Is a non-hazardous edge.
Evaluating the third directed edge for the ZMP of FIG. 3 when inside a vector triangle
Whether it is a dangerous edge: ZMP is A, and a fifth vector is constructed
Constructing a sixth vector
Obtaining a third feature vector
The third feature vector is 0, or the direction of the third feature vector is the same as that of the direction vector, or the direction of the third feature vector is opposite to that of the direction vector; the direction vector and the third feature vector perform an inner product operation, that is,
obtain the following sign function
If sign3(x3) When the direction vector is the same as the direction vector, the third feature vector has a third directed edge of 1
As a dangerous side if sign3(x3) When 0, the ZMP is illustrated on the third directed edge
Upper, third directed edge
Dangerous edge, ifsign3(x3) The third characteristic vector is opposite to the direction vector direction, and the third directional edge is 1
Is a non-hazardous edge.
FIGS. 4-6 show oriented edges for ZMP outside the vector triangle for evaluation
The method for judging the toppling orientation is the same whether the ZMP is inside or outside the vector triangle or not, and the judgment process of FIGS. 4-6 is not repeated herein.
And (4) combining the dangerous edge evaluation results in the steps (3) and (4) to obtain the total number of the dangerous edges, wherein if the total number of the dangerous edges is 1, the dumping direction is the normal direction of the dangerous edge, and if the total number of the dangerous edges is 2, the dumping direction is the synthetic direction of the normal directions of the 2 dangerous edges.
Claims (4)
1. The Boolean judgment method for the toppling direction of the trailing bicycle based on the ZMP is characterized by comprising the following steps of:
(1) calculating the ZMP coordinates and the coordinates of three grounding points of the trailer bicycle contacted with the earth (7) to construct a vector triangle by the three grounding points; the vector triangle comprises three directed edges which are respectively a first directed edge, a second directed edge and a third directed edge;
(2) selecting any two adjacent directed edges from the vector triangle to perform cross product operation to obtain a direction vector of the vector triangle;
(3) connecting the starting end of the first directed edge with the ZMP to obtain a first vector, wherein the direction of the first vector is that the starting end points to the ZMP; connecting the ZMP with the first directed edge stopping end to obtain a second vector, wherein the direction of the second vector is that the ZMP points to the stopping end; performing cross product operation on the first vector and the second vector to obtain a first feature vector; performing inner product operation on the direction vector and the first feature vector to obtain a symbolic function, and judging whether the first directed edge is a dangerous edge or a non-dangerous edge according to the result of the symbolic function;
(4) judging the dangerous edges of the second directed edge and the third directed edge according to the method in the step (3);
(5) obtaining the total number of dangerous edges according to the steps (3) and (4), wherein if the total number of the dangerous edges is 1, the dumping direction is the normal direction of the dangerous edge, and if the total number of the dangerous edges is 2, the dumping direction is the synthetic direction of the normal directions of the 2 dangerous edges;
in the step (1), the coordinates of the ZMP in the geodetic (7) coordinate system satisfy the formula:
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),
is PxA first derivative of (a) is obtained,
is PyA first derivative of (a) is obtained,
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,
is LxA first derivative of (a) is obtained,
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 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 coordinate system of the earth (7) are as follows:
1) calculating the angular speed of the tractor frame (1) of the trailer bicycle under a geodetic (7) coordinate system through the three Euler angular speeds of the tractor frame (1) of the trailer bicycle;
2) calculating the angular speed of the rear wheel (4) of the tractor of the trailer bicycle 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 the coordinates of the first grounding point, the second grounding point and the origin of the coordinate system of the trailer frame (5) of the trailer bicycle in the coordinate system of the ground (7) through the coordinates of the origin of the coordinate system of the rear wheel (4) of the tractor in the coordinate system of the ground (7) and coordinate transformation;
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 ZMP-based boolean evaluation of trailer bicycle toppling orientation of claim 1 wherein:
in step (3), the first grounding point is P1, the second grounding point is P2, the third grounding point is P3, and the direction vector expression is：
。
3. The ZMP-based boolean evaluation of trailer bicycle toppling orientation of claim 2 wherein:
in the step (3), the ZMP is the point A; the first directed edge is
The first vector is
The second vector is
The first feature vector expression is
The first feature vector is 0, or the direction of the first feature vector is the same as the direction of the direction vector, or the direction of the first feature vector is opposite to the direction of the direction vector.
4. The ZMP-based Boolean judgment method for the toppling orientation of a towed bicycle of claim 3, wherein:
in the step (3), the direction vector and the first feature vector execute inner product operation, and the expression is
The symbolic function expression is
The sign1(x1) When 1 or 0, the first directed edge is
Sign as a dangerous side1(x1) When-1, the first directed edge is
Is a non-hazardous edge.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910430131.6A CN110155236B (en) | 2019-05-22 | 2019-05-22 | Boolean judgment method for toppling direction of trailing bicycle based on ZMP |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910430131.6A CN110155236B (en) | 2019-05-22 | 2019-05-22 | Boolean judgment method for toppling direction of trailing bicycle based on ZMP |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110155236A CN110155236A (en) | 2019-08-23 |
| CN110155236B true CN110155236B (en) | 2020-11-13 |
Family
ID=67632064
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910430131.6A Active CN110155236B (en) | 2019-05-22 | 2019-05-22 | Boolean judgment method for toppling direction of trailing bicycle based on ZMP |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110155236B (en) |
Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN2315027Y (en) * | 1998-01-12 | 1999-04-21 | 深圳保安自行车有限公司 | Foldable tailer for bicycle |
| EP1288116A2 (en) * | 2001-08-10 | 2003-03-05 | Riccardo Borelli | Device for pulling a stroller by means of a bicycle |
| CN201021085Y (en) * | 2007-02-09 | 2008-02-13 | 北京凯鼎国际科贸有限责任公司 | Tow truck coupler |
| CN205737928U (en) * | 2016-01-04 | 2016-11-30 | 代运波 | A kind of Foldable light two-wheeled towed vehicle |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2004085234A2 (en) * | 2003-03-24 | 2004-10-07 | Isotec, Inc. | Bicycle towing device |
-
2019
- 2019-05-22 CN CN201910430131.6A patent/CN110155236B/en active Active
Patent Citations (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN2315027Y (en) * | 1998-01-12 | 1999-04-21 | 深圳保安自行车有限公司 | Foldable tailer for bicycle |
| EP1288116A2 (en) * | 2001-08-10 | 2003-03-05 | Riccardo Borelli | Device for pulling a stroller by means of a bicycle |
| CN201021085Y (en) * | 2007-02-09 | 2008-02-13 | 北京凯鼎国际科贸有限责任公司 | Tow truck coupler |
| CN205737928U (en) * | 2016-01-04 | 2016-11-30 | 代运波 | A kind of Foldable light two-wheeled towed vehicle |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110155236A (en) | 2019-08-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN104590414B (en) | A kind of Omnibearing wheel type mobile robot with relative pose detection function | |
| JP5393432B2 (en) | Roll angle estimation device and transport equipment | |
| CN104089617B (en) | A kind of mobile robot positioner and localization method | |
| CN105946858A (en) | Method for optimizing parameters of four-driving electric car state observer based on genetic algorithm | |
| Cossalter et al. | The modal analysis of a motorcycle in straight running and on a curve | |
| JP5029442B2 (en) | Vehicle attitude angle estimation device and program | |
| CN102582626B (en) | Method for estimating heavy semitrailer status | |
| WO2017175844A1 (en) | Orientation estimation device and transport equipment | |
| CN104061899A (en) | Kalman filtering based method for estimating roll angle and pitching angle of vehicle | |
| CN110530361A (en) | A kind of steering angle estimator based on agricultural machinery double antenna GNSS automated navigation system | |
| Galati et al. | Terrain awareness using a tracked skid-steering vehicle with passive independent suspensions | |
| US20230386050A1 (en) | Optical sensor for odometry tracking to determine trajectory of a wheel | |
| CN111006884B (en) | Method for measuring wheel axle slip angle and slip stiffness based on Fourier transform | |
| Zhou et al. | PTV longitudinal-lateral state estimation considering unknown control inputs and uncertain model parameters | |
| JP6547065B2 (en) | Attitude estimation device and transportation equipment | |
| CN101294872A (en) | Vehicle four wheel locating parameter detecting mathematical model based on computer vision | |
| CN110155236B (en) | Boolean judgment method for toppling direction of trailing bicycle based on ZMP | |
| JP2014185988A (en) | Construction vehicle | |
| CN110104102A (en) | The estimation method of self-balancing bicycle driving wheel straight skidding state | |
| CN110083075B (en) | Stability margin estimation method and control method for pull-type bicycle | |
| Reina | Methods for wheel slip and sinkage estimation in mobile robots | |
| JP5591185B2 (en) | Vehicle position and orientation measurement device | |
| Zhou et al. | Vision-based control of an industrial vehicle in unstructured environments | |
| Trojnacki | Modeling and motion simulation of a three-wheeled mobile robot with front wheel driven and steered taking into account wheels’ slip | |
| CN204043675U (en) | A kind of mobile robot's locating device |
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: 20190823 Assignee: Guangxi Cuizhi Rongchuang Technology Co.,Ltd. Assignor: GUILIN University OF ELECTRONIC TECHNOLOGY Contract record no.: X2023980046599 Denomination of invention: A Boolean Evaluation Method for the Tilting Direction of Trailing Bicycles Based on ZMP Granted publication date: 20201113 License type: Common License Record date: 20231108 |
|
| EE01 | Entry into force of recordation of patent licensing contract |