CN110348140B - Method and device for modeling and static balancing of two-wheeled robot based on towing distance - Google Patents

Method and device for modeling and static balancing of two-wheeled robot based on towing distance Download PDF

Info

Publication number
CN110348140B
CN110348140B CN201910637561.5A CN201910637561A CN110348140B CN 110348140 B CN110348140 B CN 110348140B CN 201910637561 A CN201910637561 A CN 201910637561A CN 110348140 B CN110348140 B CN 110348140B
Authority
CN
China
Prior art keywords
handlebar
wheeled robot
axis
coordinate system
wheel
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
CN201910637561.5A
Other languages
Chinese (zh)
Other versions
CN110348140A (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.)
Tsinghua University
Original Assignee
Tsinghua University
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 Tsinghua University filed Critical Tsinghua University
Priority to CN201910637561.5A priority Critical patent/CN110348140B/en
Publication of CN110348140A publication Critical patent/CN110348140A/en
Application granted granted Critical
Publication of CN110348140B publication Critical patent/CN110348140B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/10—Geometric CAD
    • G06F30/17—Mechanical parametric or variational design
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F30/00—Computer-aided design [CAD]
    • G06F30/20—Design optimisation, verification or simulation
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2111/00—Details relating to CAD techniques
    • G06F2111/04—Constraint-based CAD
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F2119/00—Details relating to the type or aim of the analysis or the optimisation
    • G06F2119/06—Power analysis or power optimisation

Abstract

The invention discloses a method and a device for modeling and static balancing of a two-wheeled robot based on a towing distance, wherein the method comprises the following steps: setting the two-wheeled robot system as a multi-link multi-joint system, defining a plurality of coordinate systems in the multi-link multi-joint system and calculating the dragging distance range of the two-wheeled robot system; establishing two constraint equations and a kinematic model according to the closed-loop kinematic chain borne by the multi-connecting-rod multi-joint system and the wheel geometric characteristics of the two-wheeled robot system; solving the kinematics model by using a first class of Lagrange equations to obtain a two-wheeled robot dynamics model, analyzing singular values of a controllability matrix, an attraction domain and control cost of a closed-loop controller, and determining a towing distance meeting control requirements in a towing distance range according to an analysis result. The method can reflect the nonlinear relation between the handle turning angle and the mass center height change under different towing distances, can provide a set of analysis flow for the selection of the towing distance, and improves the control effect of static balance.

Description

Method and device for modeling and static balancing of two-wheeled robot based on towing distance
Technical Field
The invention relates to the technical field of mechanical system modeling and dynamics analysis, in particular to a method and a device for modeling and static balancing of a two-wheeled robot based on a towing distance.
Background
The towing distance is a distance between an intersection point of a handle rotation shaft and the ground and a front wheel ground contact point, and has an important influence on balance control of the two-wheeled robot by using handle steering.
To the two-wheeled robot that the distance of dragging is nonzero, the centre of mass height of robot can finely be adjusted to the handlebar that rotates, prevents to topple over. When the speed of the two-wheeled robot reaches a certain degree, the return torque provided by the ground enables the front wheel rotating shaft to automatically right the robot without extra control force; however, when the two-wheeled robot is at ultra-low speed and even is still, the self-stability disappears, and the balance is realized mainly by means of steering of the handlebar. It follows that for more challenging static balancing, the drag distance becomes critical. The existing research on the steering static balance of the handlebar indicates that a controller designed based on the positive towing distance has a small attraction area and poor robustness, so that the influence of the towing distance on the control performance needs to be researched. However, current research models only robots with a fixed tow distance and simplifies the centroid height variation to a linear model. Such a model is not sufficient to analyze the influence of the drag distance, and for this reason, it is urgently required to establish a dynamic model suitable for any drag distance.
Disclosure of Invention
The present invention is directed to solving, at least to some extent, one of the technical problems in the related art.
Therefore, an object of the present invention is to provide a method for modeling and static balancing a two-wheeled robot based on a tow distance, which can reflect a non-linear relationship between a turning angle and a height change of a center of mass of the vehicle handle at different tow distances, and can provide a set of analysis processes for selecting the tow distance to improve a control effect of static balancing.
Another objective of the present invention is to provide a device for modeling and static balancing of a two-wheeled robot based on a towing distance.
In order to achieve the above object, an embodiment of an aspect of the present invention provides a method for modeling and static balancing a two-wheeled robot based on a tow-track, including:
s1, when detecting that the two-wheeled robot system meets the preset equivalent setting condition, setting the two-wheeled robot system as a multi-link multi-joint system, defining a plurality of coordinate systems in the multi-link multi-joint system, and calculating the towing distance range of the two-wheeled robot system according to the geometrical relationship of the coordinate systems;
s2, establishing two constraint equations according to the closed-loop kinematic chain borne by the multi-link multi-joint system and the wheel geometric characteristics of the two-wheel robot system, and establishing a kinematic model according to the two constraint equations;
s3, solving the kinematics model by using a first Lagrange equation to obtain a two-wheeled robot dynamics model, analyzing a singular value of a controllability matrix, an attraction domain of a closed-loop controller and a control cost according to the two-wheeled robot dynamics model, and determining a towing distance meeting a control requirement in the towing distance range according to an analysis result.
According to the modeling and static balancing method of the two-wheeled robot based on the towing distance, firstly, according to the motion constraint, the two-wheeled robot is equivalent to a multi-connecting-rod multi-joint system with front and rear vehicle bodies hinged from the multi-rigid system. And secondly, establishing two constraint equations based on the closed-loop kinematic chain of the system and the contact characteristics of the wheels and the ground to obtain a kinematic model of the system. The first type of lagrangian equation is then used to derive a kinetic model of the system. And finally, analyzing the influence of the towing distance on the static balance from three aspects of the singular value of the system controllability matrix, the attraction domain of the closed-loop controller and the control energy consumption. The nonlinear relation between the handle turning angle and the height change of the mass center of the vehicle at different towing distances can be reflected, and a set of analysis process can be provided for the selection of the towing distances so as to improve the control effect of static balance.
In addition, the two-wheeled robot modeling and static balancing method based on the towing distance according to the above embodiment of the present invention may further have the following additional technical features:
further, in an embodiment of the present invention, the preset equivalent setting condition includes:
setting the rear body center of mass to include only roll and pitch caused by handlebar steering;
setting that the front and the rear wheels are braked and do not move relative to the frame, and the rear wheels roll with the ground;
the thickness and deformation of the tire are neglected, and the front wheel and the rear wheel are regarded as rigid thin discs with the same size.
Further, in one embodiment of the present invention, the plurality of coordinate systems are:
(1) inertial reference system { I }, A0xyz: the origin is fixed at A0Point, x axis from A0Direction E0The z-axis is vertical downwards, and the y-axis, the x-axis and the z-axis form a right-hand system; wherein A is0Contact point of rear wheel with ground when handlebar is turned, E0The contact point between the front wheel and the ground when the handlebar rotates;
(2) rear wheel coordinate system { B }, Bxbybzb: the origin is fixed at point B, xbThe axis being parallel to the x-axis of the inertial reference system, the z-and y-axes being rotatable about the x-axis by the inertial reference system
Figure BDA0002130796920000021
The angle is found, then the rotation matrix from { I } to { B } is:
Figure BDA0002130796920000022
wherein B is the circle center of the rear wheel,
Figure BDA0002130796920000023
the roll angle of the rear body;
(3) coordinate system of handlebar { C }, Cxcyczc: the origin is fixed at point C, ycAxle and rear wheel coordinate system ybThe axes being parallel, the x and z axes being surrounded by { B } around ybThe axis is rotated by an angle θ + η, and the rotation matrix from { B } to { C } is:
Figure BDA0002130796920000031
c is a connecting point of the handlebar rotating pair and the rear frame, theta is a pitch angle of the rear vehicle body, and eta is a handlebar inclination angle;
the handlebar inclination angle η satisfies the following geometrical constraints:
Figure BDA0002130796920000032
wherein, theta0Is the pitch angle of the rear frame connecting rod vector when the handlebar turning angle is zero, and epsilon is the rear frame connecting rod mounting angle;
(4) front wheel coordinate system { D }, Dxdydzd: the origin is fixed at point D, zdAxis and handlebar coordinate system zcThe axes being parallel, the x and y axes being defined by the handlebar coordinate systemcThe axis is rotated by an angle δ, and the rotation matrix from { C } to { D } is:
Figure BDA0002130796920000033
wherein D is the center of the front wheel circle, and delta is the handlebar turning angle;
(5) rear vehicle body coordinate system { G1}，G1x1y1z1: origin fixed to G1The rear vehicle body coordinate system is wound by the rear wheel coordinate system ybThe angle of rotation of the shaft is obtained by
Figure BDA0002130796920000034
Where η is 0, from { B } system to { G-1Rotation matrix of the system
Figure BDA0002130796920000035
Figure BDA0002130796920000036
Wherein G is1Is the center of mass of the rear vehicle body;
(6) front vehicle body coordinate system { G2}，G2x2y2z2: origin fixed to G2The front body coordinate system is parallel to the front wheel coordinate system, G2Is the front bodywork centroid.
Further, in one embodiment of the present invention, the tow-distance range of the two-wheeled robot system is:
Figure BDA0002130796920000037
wherein R is the wheel radius, lrIs the length of segment BC, d is the length of segment CC', lfThe length of the front frame line segment C' D, λ is the handlebar fork angle, and η is the handlebar inclination angle.
Further, in an embodiment of the present invention, the establishing two constraint equations according to the closed-loop kinematic chain suffered by the multi-link multi-joint system and the wheel geometry of the two-wheel robot system includes:
constraint 1, closed-loop kinematic chain constraint
Figure BDA0002130796920000041
Figure BDA0002130796920000042
Figure BDA0002130796920000043
Figure BDA0002130796920000044
Figure BDA0002130796920000045
Figure BDA0002130796920000046
Wherein e isz＝[0,0,1]TIs a z-axis unit direction vector, r, of the inertial reference system { I }1Is the rear wheel link vector pointing from a to B; r is2Is the rear frame link vector pointing from B to C; r is3Is the front frame link vector pointing from C to D; r is4Is a front wheel connecting rod vector pointed to E by D, the coordinate system corresponding to the vector is indicated by a superscript,
Figure BDA0002130796920000047
for the rotation matrix from { B } to { I },
Figure BDA0002130796920000048
is a rotation matrix from C to B,
Figure BDA0002130796920000049
is a rotation matrix from { D } to { C };
restraint 2, wheel geometry restraint
Figure BDA00021307969200000410
Figure BDA00021307969200000411
Wherein n isyY being the front wheel coordinate system { D }dAxial unit direction vector.
Further, in an embodiment of the present invention, the building a kinematic model according to the two constraint equations includes:
and (3) carrying out derivation on constraint conditions of the constraint 1 and the constraint 2 to obtain:
Figure BDA00021307969200000412
wherein J is a Jacobian matrix; the Jacobian matrix can be based on generalized coordinates
Figure BDA00021307969200000413
And non-independent coordinates
Figure BDA00021307969200000414
The decomposition is carried out in two parts:
Figure BDA00021307969200000415
the velocity of the non-independent coordinates is represented by the generalized velocity:
Figure BDA00021307969200000416
the kinematic model can be found as follows:
Figure BDA0002130796920000051
wherein the content of the first and second substances,
Figure BDA0002130796920000052
is an identity matrix of order 2.
Further, in one embodiment of the present invention, solving the two-wheeled robot dynamics model using the first type of lagrangian equation comprises:
Figure BDA0002130796920000053
wherein, L is a Lagrange function, T is the total kinetic energy of the two-wheeled robot system, V is the total potential energy of the two-wheeled robot system, gamma is a Lagrange multiplier,
Figure BDA0002130796920000054
the generalized non-conservative external force on the two-wheeled robot system, D and D are respectively the disturbance torque on the rolling and steering channels of the handlebar, taucIs the driving torque of the rotating shaft of the handlebar.
Further, in an embodiment of the present invention, the S3 specifically includes:
and in the dragging distance range, analyzing singular values of the controllability matrix, the attraction domain of the closed-loop controller and the control cost, making a corresponding image, and determining the dragging distance meeting the control requirement according to the corresponding image and the control requirement.
In order to achieve the above object, another embodiment of the present invention provides a device for modeling and static balancing of a two-wheeled robot based on a towing distance, including:
the system comprises an equivalence module, a control module and a control module, wherein the equivalence module is used for setting a two-wheeled robot system into a multi-connecting-rod multi-joint system when detecting that the two-wheeled robot system meets preset equivalence setting conditions, defining a plurality of coordinate systems in the multi-connecting-rod multi-joint system, and calculating the dragging distance range of the two-wheeled robot system according to the geometric relations of the coordinate systems;
the constraint module is used for establishing two constraint equations according to a closed-loop kinematic chain borne by the multi-connecting-rod multi-joint system and the wheel geometric characteristics of the two-wheeled robot system, and establishing a kinematic model according to the two constraint equations;
and the modeling analysis module is used for solving the kinematic model by utilizing a first class of Lagrange equations to obtain a two-wheeled robot dynamic model, analyzing the singular value of a controllability matrix, the attraction domain of the closed-loop controller and the control cost according to the two-wheeled robot dynamic model, and determining the towing distance meeting the control requirement in the towing distance range according to the analysis result.
According to the modeling and static balancing device of the two-wheeled robot based on the towing distance, firstly, the two-wheeled robot is equivalent to a multi-link multi-joint system with front and rear vehicle bodies hinged from the multi-rigid system according to the motion constraint. And secondly, establishing two constraint equations based on the closed-loop kinematic chain of the system and the contact characteristics of the wheels and the ground to obtain a kinematic model of the system. The first type of lagrangian equation is then used to derive a kinetic model of the system. And finally, analyzing the influence of the towing distance on the static balance from three aspects of the singular value of the system controllability matrix, the attraction domain of the closed-loop controller and the control energy consumption. The nonlinear relation between the handle turning angle and the height change of the mass center of the vehicle at different towing distances can be reflected, and a set of analysis process can be provided for the selection of the towing distances so as to improve the control effect of static balance.
In addition, the towing-distance-based two-wheeled robot modeling and static balancing apparatus according to the above-described embodiment of the present invention may further have the following additional technical features:
further, in an embodiment of the present invention, the preset equivalent setting condition includes:
setting the rear body center of mass to include only roll and pitch caused by handlebar steering;
setting that the front and the rear wheels are braked and do not move relative to the frame, and the rear wheels roll with the ground;
the thickness and deformation of the tire are neglected, and the front wheel and the rear wheel are regarded as rigid thin discs with the same size.
Additional aspects and advantages of the invention will be set forth in part in the description which follows and, in part, will be obvious from the description, or may be learned by practice of the invention.
Drawings
The foregoing and/or additional aspects and advantages of the present invention will become apparent and readily appreciated from the following description of the embodiments, taken in conjunction with the accompanying drawings of which:
FIG. 1 is a flow chart of a method for modeling and static balancing a two-wheeled robot based on a tow-track according to one embodiment of the present invention;
FIG. 2 is a schematic view of a multi-link multi-joint equivalent vehicle body according to one embodiment of the present invention;
FIG. 3 is a chart illustrating a drag distance controllability analysis according to one embodiment of the present invention;
fig. 4 is a schematic structural diagram of a tow-track-based two-wheeled robot modeling and static balancing apparatus according to an embodiment of the present invention.
Detailed Description
Reference will now be made in detail to embodiments of the present invention, examples of which are illustrated in the accompanying drawings, wherein like or similar reference numerals refer to the same or similar elements or elements having the same or similar function throughout. The embodiments described below with reference to the drawings are illustrative and intended to be illustrative of the invention and are not to be construed as limiting the invention.
The following describes a method and an apparatus for modeling and static balancing a two-wheeled robot based on a tow-track according to an embodiment of the present invention with reference to the accompanying drawings.
First, a proposed tow-distance-based two-wheeled robot modeling and static balancing method according to an embodiment of the present invention will be described with reference to the accompanying drawings.
Fig. 1 is a flowchart of a tow-range-based two-wheeled robot modeling and static balancing method according to an embodiment of the present invention.
As shown in fig. 1, the method for modeling and static balancing of a two-wheeled robot based on a tow distance comprises the following steps:
and step S1, when the two-wheeled robot system is detected to meet the preset equivalent setting condition, setting the two-wheeled robot system as a multi-link multi-joint system, defining a plurality of coordinate systems in the multi-link multi-joint system, and calculating the dragging distance range of the two-wheeled robot system according to the geometric relationship of the coordinate systems.
Further, presetting equivalent setting conditions, including:
setting the rear body center of mass to include only roll and pitch caused by handlebar steering;
setting that the front and the rear wheels are braked and do not move relative to the frame, and the rear wheels roll with the ground;
the thickness and deformation of the tire are neglected, and the front wheel and the rear wheel are regarded as rigid thin discs with the same size.
Specifically, in the process of deriving the model, some assumptions need to be made to facilitate establishing and deriving the model, such as:
(1) assuming that the rear body center of mass has no motion other than roll and pitch caused by handlebar steering;
(2) the front and the rear wheels are braked, do not move relative to the frame, and roll purely with the ground;
(3) neglecting tire thickness and deformation, both wheels are considered as equally sized thin rigid disks.
After the above-mentioned conditions that can be assumed are satisfied, the two-wheeled robot system can be equivalent to a multi-link multi-joint system, as shown in fig. 2, which is a multi-link multi-joint equivalent vehicle body schematic diagram. Wherein A is0And A is the contact point of the rear wheel and the ground when the handlebar rotates and does not rotate respectively. B is the center of the circle of the rear wheel, C is the connecting point of the handlebar rotating pair and the rear frame, D is the center of the circle of the front wheel, and E is the contact point of the front wheel and the ground.
Figure BDA0002130796920000073
Delta and theta respectively represent the roll angle, the handlebar rotation angle and the pitch angle of the rear vehicle body, theta1The angular displacement of point E on the front wheel.
Figure BDA0002130796920000071
Is the front wheel roll angular velocity. Δ xrIs a pi along the rear car body plane caused by the pitching motion of the rear wheels1Translation distance of the intersection with the ground. Δ xfAnd Δ yfIs point E relative to initial point E due to handlebar rotation0Offset coordinates on the ground. Table 1 is a summary of equivalent joints for an embodiment of the present invention, such that the above translational and rotational movements are achieved by five joints. r isi(i is 1-4) is an equivalent connecting rod vector linking the five joints, wherein r is1Is the rear wheel link vector pointing from a to B; r is2Is the rear frame link vector pointing from B to C; r is3Is the front frame link vector pointing from C to D; r is4Is the front wheel link vector pointing from D to E. Further, θ0The pitch angle of the rear frame connecting rod vector when the handlebar rotation angle is zero; epsilon is the mounting angle of the connecting rod of the rear frame and is r2The included angle between the handle bar and the rotating shaft; eta is the inclination angle of the handlebar, which is the included angle between the rotating shaft of the handlebar and the plane of the rear vehicle body in the vertical direction; lambda is the handlebar front fork angle, which is the angle between the front frame C' D and the handlebar turning axis. G2And G1Respectively front and rear vehicle body mass centers.
TABLE 1
Figure BDA0002130796920000072
After setting the two-wheeled robot system as a multi-link multi-joint system, the following six coordinate systems are defined in the multi-link multi-joint system:
(1) inertial reference system { I }, A0xyz: the origin is fixed at A0Point, x axis from A0Direction E0The z-axis is vertical downwards, and the y-axis, the x-axis and the z-axis form a right-hand system; wherein A is0Contact point of rear wheel with ground when handlebar is turned, E0The contact point between the front wheel and the ground when the handlebar rotates;
(2) rear wheel coordinate system { B }, Bxbybzb: the origin is fixed at point B, xbThe axis being parallel to the x-axis of the inertial reference system, the z-and y-axesRotatable about the x-axis by an inertial reference system
Figure BDA0002130796920000081
The angle is found, then the rotation matrix from { I } to { B } is:
Figure BDA0002130796920000082
wherein B is the circle center of the rear wheel,
Figure BDA0002130796920000083
the roll angle of the rear body;
(3) coordinate system of handlebar { C }, Cxcyczc: the origin is fixed at point C, ycAxle and rear wheel coordinate system ybThe axes being parallel, the x and z axes being surrounded by { B } around ybThe axis is rotated by an angle θ + η, and the rotation matrix from { B } to { C } is:
Figure BDA0002130796920000084
c is a connecting point of the handlebar rotating pair and the rear frame, theta is a pitch angle of the rear vehicle body, and eta is a handlebar inclination angle;
the handlebar inclination angle η satisfies the following geometrical constraints:
Figure BDA0002130796920000085
wherein, theta0Is the pitch angle of the rear frame connecting rod vector when the handlebar turning angle is zero, and epsilon is the rear frame connecting rod mounting angle;
(4) front wheel coordinate system { D }, Dxdydzd: the origin is fixed at point D, zdAxis and handlebar coordinate system zcThe axes being parallel, the x and y axes being defined by the handlebar coordinate systemcThe axis is rotated by an angle δ, and the rotation matrix from { C } to { D } is:
Figure BDA0002130796920000086
wherein D is the center of the front wheel circle, and delta is the handlebar turning angle;
(5) rear vehicle body coordinate system { G1}，G1x1y1z1: origin fixed to G1The rear vehicle body coordinate system is wound by the rear wheel coordinate system ybThe angle of rotation of the shaft is obtained by
Figure BDA0002130796920000087
Where η is 0, from { B } system to { G-1Rotation matrix of the system
Figure BDA0002130796920000088
Figure BDA0002130796920000091
Wherein G is1Is the center of mass of the rear vehicle body;
(6) front vehicle body coordinate system { G2}，G2x2y2z2: origin fixed to G2The front body coordinate system is parallel to the front wheel coordinate system, G2Is the front bodywork centroid.
According to the above coordinate system, each vector in fig. 2 can be represented as:
Figure BDA0002130796920000092
in the formula: the superscript indicates the coordinate system to which the vector corresponds, R is the wheel radius, lrIs the length of segment BC, d is the length of segment CC', lfIs the length of segment C' D.
The geometrical relationship can obtain the following towing distance of the two-wheeled robot:
Figure BDA0002130796920000093
and step S2, establishing two constraint equations according to the closed-loop kinematic chain borne by the multi-link multi-joint system and the wheel geometric characteristics of the two-wheel robot system, and establishing a kinematic model according to the two constraint equations.
Further, in one embodiment of the present invention, two constraint equations are established according to the closed-loop kinematic chain suffered by the multi-link multi-joint system and the wheel geometry of the two-wheel robot system, including:
constraint 1, closed-loop kinematic chain constraint
Figure BDA0002130796920000094
Figure BDA0002130796920000095
Figure BDA0002130796920000096
Figure BDA0002130796920000097
Figure BDA0002130796920000098
Figure BDA0002130796920000099
Wherein e isz＝[0,0,1]TIs a z-axis unit direction vector, r, of the inertial reference system { I }1Is the rear wheel link vector pointing from a to B; r is2Is the rear frame link vector pointing from B to C; r is3Is the front frame link vector pointing from C to D; r is4Is a front wheel connecting rod vector pointed to E by D, the coordinate system corresponding to the vector is indicated by a superscript,
Figure BDA0002130796920000101
for the rotation matrix from { B } to { I },
Figure BDA0002130796920000102
is a rotation matrix from C to B,
Figure BDA0002130796920000103
is a rotation matrix from { D } to { C };
restraint 2, wheel geometry restraint
Figure BDA0002130796920000104
Figure BDA0002130796920000105
Wherein n isyY being the front wheel coordinate system { D }dAxial unit direction vector.
Further, in one embodiment of the present invention, the kinematic model is built according to two constraint equations, including:
and (3) carrying out derivation on constraint conditions of the constraint 1 and the constraint 2 to obtain:
Figure BDA0002130796920000106
wherein J is a Jacobian matrix; the Jacobian matrix can be based on generalized coordinates
Figure BDA0002130796920000107
And non-independent coordinates
Figure BDA0002130796920000108
The decomposition is carried out in two parts:
Figure BDA0002130796920000109
the velocity of the non-independent coordinates is represented by the generalized velocity:
Figure BDA00021307969200001010
the kinematic model can be found as:
Figure BDA00021307969200001011
wherein the content of the first and second substances,
Figure BDA00021307969200001012
is an identity matrix of order 2.
Specifically, first, system generalized coordinates and complete constraints are defined,
in the above-described motions, only the roll angle of the rear vehicle body
Figure BDA00021307969200001013
Steering angle δ is independent of the handlebar, so they are defined as the generalized coordinates of the system:
Figure BDA00021307969200001014
in the static balance process, the front and the rear wheels of the vehicle need to be grounded to form a closed-loop kinematic chain. The existence of a closed-loop kinematic chain allows two paths from the ground to reach any point on the system. Considering that the front wheel has more non-independent coordinates, the invention has the contact point A between the rear wheel and the ground0As a starting point for the path. Thus only half the coordinates are needed to represent the position of the front wheel contact point in the I system. Thus, let the new system coordinates be:
Figure BDA0002130796920000111
the following two complete constraints are constructed according to the closed-loop kinematic chain and wheel geometry that the system is subjected to:
constraint 1, closed-loop kinematic chain constraint
Figure BDA0002130796920000112
Figure BDA0002130796920000113
Wherein e isz＝[0,0,1]TIs the z-axis unit direction vector of the inertial reference system { I }.
Restraint 2, wheel geometry restraint
Figure BDA0002130796920000114
Figure BDA0002130796920000115
Wherein n isyY being the front wheel coordinate system { D }dUnit direction vector of axis, also front body plane Π2The normal vector of (2).
When δ is 0, θ1It should satisfy:
θ1＝-η|δ＝θ＝0 (11)
further, θ0The following constraints should also be satisfied:
Figure BDA0002130796920000116
establishing a robot kinematic model according to a constraint equation to obtain a nonlinear centroid height variation model,
the derivation of the integrity constraints (9) and (10) can be:
Figure BDA0002130796920000117
in the formula: j is the Jacobian matrix. The matrix may be further based on qiAnd non-independent coordinates
Figure BDA0002130796920000118
The decomposition is carried out in two parts:
Figure BDA0002130796920000119
based on the above equation, the velocity of the dependent coordinates can be represented by a generalized velocity:
Figure BDA00021307969200001110
thus, a kinematic model of the system can be obtained as:
Figure BDA0002130796920000121
in the formula:
Figure BDA0002130796920000122
is an identity matrix of order 2.
And S3, solving the kinematic model by using a first class of Lagrange equations to obtain a two-wheeled robot dynamic model, analyzing the singular value of the controllability matrix, the attraction domain of the closed-loop controller and the control cost according to the two-wheeled robot dynamic model, and determining the towing distance meeting the control requirement in the towing distance range according to the analysis result.
Further, in one embodiment of the present invention, solving the two-wheeled robot dynamics model using the first type of lagrangian equation comprises:
Figure BDA0002130796920000123
wherein, L is a Lagrange function, T is the total kinetic energy of the two-wheeled robot system, V is the total potential energy of the two-wheeled robot system, gamma is a Lagrange multiplier,
Figure BDA0002130796920000124
the generalized non-conservative external force on the two-wheeled robot system, D and D are respectively the disturbance force on the rolling and steering channels of the handlebarMoment, τcIs the driving torque of the rotating shaft of the handlebar.
Further, in an embodiment of the present invention, S3 specifically includes:
and in the range of the dragging distance, analyzing the singular value of the controllability matrix, the attraction domain of the closed-loop controller and the control cost, making a corresponding image, and determining the dragging distance meeting the control requirement according to the corresponding image and the control requirement.
Specifically, a two-wheeled robot dynamic model is established according to a kinematic model, and first, the total kinetic energy of the two-wheeled robot system is deduced and translated from the mass center translation kinetic energy T of the front and rear vehicle bodiestiAnd rotational kinetic energy T around the center of massriConsists of the following components:
Figure BDA0002130796920000125
kinetic energy 1, center of mass G of the rear vehicle body1The coordinates are expressed under the inertial system { I }:
Figure BDA0002130796920000126
in the formula:
Figure BDA0002130796920000127
xG1＝xg1cosθ0+zg1sinθ0、zG1＝zg1cosθ0-xg1sinθ0，xg1and zg1Is the coordinate of the mass center of the rear vehicle relative to the rear vehicle frame.
Figure BDA0002130796920000128
Is the translation of the rear wheel in the x-axis due to pitch, which can be found from hypothesis 2
Figure BDA0002130796920000129
Then the angular velocity of the rear frame is projected to a rear frame coordinate system { G }1In the method, the following steps:
Figure BDA00021307969200001210
thus, the available rear body kinetic energy is as follows:
Figure BDA0002130796920000131
in the formula: m is1For rear body mass, let { G1Is the inertia main axis of the rear vehicle body, then
Figure BDA0002130796920000132
In-body coordinate system { G for rear vehicle body1The moment of inertia matrix in (1).
Kinetic energy 2, center of mass G of front vehicle body2The coordinates are expressed under the inertial system { I }:
Figure BDA0002130796920000133
in the formula:
Figure BDA0002130796920000134
xg2and zg2Is the coordinate of the center of mass of the front vehicle relative to the front frame, xG2＝d+xg2cosλ+zg2sinλ、zG2＝zg2cosλ-xg2sin lambda. Similarly, the angular velocity of the front frame is projected to a front frame coordinate system { G }2In the method, the following steps:
Figure BDA0002130796920000135
thus, the available front body kinetic energy is as follows:
Figure BDA0002130796920000136
in the formula: m is2Order { G } for front body mass2Inertia of front bodyMain axis system, then
Figure BDA0002130796920000137
In-vivo coordinate system { G for front vehicle body2The moment of inertia matrix in (1).
And taking the ground as a zero potential energy surface, and taking the z-axis component of the mass centers of the two vehicle bodies under the inertial system { I }, wherein the obtained total potential energy of the vehicle is as follows:
Figure BDA0002130796920000138
in the formula: g is the surface gravity acceleration (.)zThe orientation quantity z-axis component operator.
Solving the dynamic model of the system by applying a first type of Lagrange equation:
Figure BDA0002130796920000139
in the formula: L-T-V is the lagrange function, T is the total kinetic energy, V is the total potential energy, γ is the lagrange multiplier,
Figure BDA00021307969200001310
d and D are respectively the disturbance torque borne by the rolling and the handlebar steering channels, taucIs the driving torque of the rotating shaft of the handlebar.
Equation (23) is written as the following robot-generic euler-lagrange form:
Figure BDA0002130796920000141
in the formula: m is a generalized mass matrix, V is a centrifugal force and Coriolis force matrix, and E is a gravity matrix, specific expressions of which are presented in the appendix. The above model can be converted into ordinary differential equations containing only 2 equations by using a kinematic model (15). The system coordinate acceleration can be derived by first deriving equation (15):
Figure BDA0002130796920000142
substituting the above equation and kinematic equation (15) into equation (25), and multiplying both ends by GTThe following can be obtained:
Figure BDA0002130796920000143
according to GTJTWhen 0, γ can be eliminated, yielding a dimensionality reduction kinetic model:
Figure BDA0002130796920000144
in the formula: mi＝GTThe MG is a dimension-reduced system quality matrix,
Figure BDA00021307969200001413
is a dimension-reducing system damping matrix, Qiext＝GTQextIs a generalized external force matrix of a dimension reduction system.
The dimension reduction dynamic model (28) is arranged at an equilibrium point
Figure BDA0002130796920000145
And (3) linearization:
Figure BDA0002130796920000146
in the formula:
Figure BDA0002130796920000147
is the slight offset of the generalized coordinate relative to the equilibrium position; delta taucIs a control deviation amount;
Figure BDA0002130796920000148
easy certificate
Figure BDA0002130796920000149
And is
Figure BDA00021307969200001410
Ignore
Figure BDA00021307969200001411
A linear time-invariant kinetic equation in the form of a state space can be obtained as follows.
Figure BDA00021307969200001412
The influence of the drag distance on the handlebar steering static balance control is analyzed according to the process, and as shown in fig. 3, a drag distance controllability analysis flow chart is shown, and the influence of the drag distance is analyzed from three aspects of the singular value of the controllability matrix, the attraction domain of the closed-loop controller and the control cost within the allowable range of the drag distance.
First, the controllability is analyzed to construct a controllability matrix Q of the linear model (30)c：
Figure BDA0002130796920000151
Substituting the system matrices a and B into the above equation yields:
Figure BDA0002130796920000152
in the formula:
Figure BDA0002130796920000153
to represent
Figure BDA0002130796920000154
Column 2. To QcPerforming singular value decomposition QcAnd (5) making an image of the minimum singular value with respect to the dragging distance.
The attraction domain of the closed-loop controller is then analyzed. The following linear quadratic performance index is designed:
Figure BDA0002130796920000155
in the formula:
Figure BDA0002130796920000156
and
Figure BDA0002130796920000157
the weight coefficient matrix to be determined can be debugged according to specific control effects. Solving the Riccati algebraic equation by:
Figure BDA0002130796920000158
a matrix P can be obtained, and then the handlebar torque linear feedback control law can be obtained as follows:
Figure BDA0002130796920000159
defining the maximum value of handlebar steering moment as tauc maxThen the reachable state space of the system satisfies the inequality:
|K|∞|x|∞≤||τc||＝||Kx||≤τc max (36)
in the formula: l. capillary∞Representing taking the infinite norm of the vector. Thus, the estimated attraction domain is:
Figure BDA00021307969200001510
an image of the attraction field (37) is made with respect to the drag distance.
And finally, analyzing the influence of the dragging distance on the control cost. The control cost is defined as:
Figure BDA00021307969200001511
in the formula: the integral superscript T indicates the control termination time. An image of the control cost with respect to the drag distance is made.
According to the three images, the appropriate dragging distance can be selected according to specific control requirements, such as high controllability, large attraction area and low control cost.
The above-mentioned formulas are supplemented below.
The jacobian matrix in equation (14) is defined as follows:
Figure BDA0002130796920000161
in the formula:
Figure BDA0002130796920000162
Figure BDA0002130796920000163
Figure BDA0002130796920000164
Figure BDA0002130796920000165
Figure BDA0002130796920000166
in the formula:
Figure BDA0002130796920000167
Figure BDA0002130796920000168
Figure BDA0002130796920000169
Figure BDA00021307969200001610
the generalized mass matrix in equation of dynamics (25) is defined as follows:
Figure BDA00021307969200001611
in the formula:
Figure BDA00021307969200001612
Figure BDA0002130796920000171
Figure BDA0002130796920000172
Figure BDA0002130796920000173
Figure BDA0002130796920000174
M23＝m2RxG2sinδcos(θ+η)+m2lrxG2sinδsin(θ0-η)-m2xG2zG2sinδ
Figure BDA0002130796920000175
M32＝m2RxG2sinδcos(θ+η)+m2lrxG2sinδsin(θ0-η)-m2xG2zG2sinδ
Figure BDA0002130796920000176
the damping matrix in equation (25) of dynamics is defined as follows:
Figure BDA0002130796920000181
in the formula:
Figure BDA0002130796920000182
Figure BDA0002130796920000183
Figure BDA0002130796920000184
Figure BDA0002130796920000185
V22＝0
Figure BDA0002130796920000186
Figure BDA0002130796920000191
Figure BDA0002130796920000192
Figure BDA0002130796920000193
the gravity matrix in equation (25) of dynamics is defined as follows:
Figure BDA0002130796920000194
in the formula:
Figure BDA0002130796920000195
Figure BDA0002130796920000196
Figure BDA0002130796920000197
according to the modeling and static balancing method of the two-wheeled robot based on the towing distance, which is provided by the embodiment of the invention, firstly, according to the motion constraint, the two-wheeled robot is equivalent to a multi-link multi-joint system with front and rear vehicle bodies hinged from the multi-rigid system. And secondly, establishing two constraint equations based on the closed-loop kinematic chain of the system and the contact characteristics of the wheels and the ground to obtain a kinematic model of the system. The first type of lagrangian equation is then used to derive a kinetic model of the system. And finally, analyzing the influence of the towing distance on the static balance from three aspects of the singular value of the system controllability matrix, the attraction domain of the closed-loop controller and the control energy consumption. The nonlinear relation between the handle turning angle and the height change of the mass center of the vehicle at different towing distances can be reflected, and a set of analysis process can be provided for the selection of the towing distances so as to improve the control effect of static balance.
Next, a proposed tow-distance-based two-wheeled robot modeling and static balancing apparatus according to an embodiment of the present invention will be described with reference to the drawings.
Fig. 4 is a schematic structural diagram of a tow-track-based two-wheeled robot modeling and static balancing apparatus according to an embodiment of the present invention.
As shown in fig. 4, the apparatus for modeling and static balancing a two-wheeled robot based on a tow distance includes: an equivalence module 100, a constraint module 200, and a modeling analysis module 300.
And the equivalent module 100 is used for setting the two-wheeled robot system into a multi-link multi-joint system when detecting that the two-wheeled robot system meets preset equivalent setting conditions, defining a plurality of coordinate systems in the multi-link multi-joint system, and calculating the towing distance range of the two-wheeled robot system according to the geometric relationship of the plurality of coordinate systems.
And the constraint module 200 is used for establishing two constraint equations according to the closed-loop kinematic chain borne by the multi-link multi-joint system and the wheel geometric characteristics of the two-wheel robot system, and establishing a kinematic model according to the two constraint equations.
And the modeling analysis module 300 is configured to solve the kinematic model by using a first class of lagrangian equations to obtain a two-wheeled robot dynamic model, analyze the singular value of the controllability matrix, the attraction domain of the closed-loop controller, and the control cost according to the two-wheeled robot dynamic model, and determine a towing distance meeting the control requirement in a towing distance range according to an analysis result.
Further, in an embodiment of the present invention, the presetting of the equivalent setting condition includes:
setting the rear body center of mass to include only roll and pitch caused by handlebar steering;
setting that the front and the rear wheels are braked and do not move relative to the frame, and the rear wheels roll with the ground;
the thickness and deformation of the tire are neglected, and the front wheel and the rear wheel are regarded as rigid thin discs with the same size.
Further, in one embodiment of the present invention, the plurality of coordinate systems are:
(1) inertial reference system { I }, A0xyz: fixed in situIs fixed to A0Point, x axis from A0Direction E0The z-axis is vertical downwards, and the y-axis, the x-axis and the z-axis form a right-hand system; wherein A is0Contact point of rear wheel with ground when handlebar is turned, E0The contact point between the front wheel and the ground when the handlebar rotates;
(2) rear wheel coordinate system { B }, Bxbybzb: the origin is fixed at point B, xbThe axis being parallel to the x-axis of the inertial reference system, the z-and y-axes being rotatable about the x-axis by the inertial reference system
Figure BDA0002130796920000201
The angle is found, then the rotation matrix from { I } to { B } is:
Figure BDA0002130796920000202
wherein B is the circle center of the rear wheel,
Figure BDA0002130796920000203
the roll angle of the rear body;
(3) coordinate system of handlebar { C }, Cxcyczc: the origin is fixed at point C, ycAxle and rear wheel coordinate system ybThe axes being parallel, the x and z axes being surrounded by { B } around ybThe axis is rotated by an angle θ + η, and the rotation matrix from { B } to { C } is:
Figure BDA0002130796920000211
c is a connecting point of the handlebar rotating pair and the rear frame, theta is a pitch angle of the rear vehicle body, and eta is a handlebar inclination angle;
the handlebar inclination angle η satisfies the following geometrical constraints:
Figure BDA0002130796920000212
wherein, theta0Rear frame connecting rod when handlebar turning angle is zeroThe pitch angle of the vector, epsilon is the mounting angle of the connecting rod of the rear frame;
(4) front wheel coordinate system { D }, Dxdydzd: the origin is fixed at point D, zdAxis and handlebar coordinate system zcThe axes being parallel, the x and y axes being defined by the handlebar coordinate systemcThe axis is rotated by an angle δ, and the rotation matrix from { C } to { D } is:
Figure BDA0002130796920000213
wherein D is the center of the front wheel circle, and delta is the handlebar turning angle;
(5) rear vehicle body coordinate system { G1}，G1x1y1z1: origin fixed to G1The rear vehicle body coordinate system is wound by the rear wheel coordinate system ybThe angle of rotation of the shaft is obtained by
Figure BDA0002130796920000214
Where η is 0, from { B } system to { G-1Rotation matrix of the system
Figure BDA0002130796920000215
Figure BDA0002130796920000216
Wherein G is1Is the center of mass of the rear vehicle body;
(6) front vehicle body coordinate system { G2}，G2x2y2z2: origin fixed to G2The front body coordinate system is parallel to the front wheel coordinate system, G2Is the front bodywork centroid.
Further, in one embodiment of the present invention, the tow-throw range of the two-wheeled robotic system is:
Figure BDA0002130796920000217
wherein R is the wheel radius, lrIs the length of segment BC, d is the length of segment CC', lfThe length of the front frame line segment C' D, λ is the handlebar fork angle, and η is the handlebar inclination angle.
Further, in one embodiment of the present invention, two constraint equations are established according to the closed-loop kinematic chain suffered by the multi-link multi-joint system and the wheel geometry of the two-wheel robot system, including:
constraint 1, closed-loop kinematic chain constraint
Figure BDA0002130796920000221
Figure BDA0002130796920000222
Figure BDA0002130796920000223
Figure BDA0002130796920000224
Figure BDA0002130796920000225
Figure BDA0002130796920000226
Wherein e isz＝[0,0,1]TIs a z-axis unit direction vector, r, of the inertial reference system { I }1Is the rear wheel link vector pointing from a to B; r is2Is the rear frame link vector pointing from B to C; r is3Is the front frame link vector pointing from C to D; r is4Is a front wheel connecting rod vector pointed to E by D, the coordinate system corresponding to the vector is indicated by a superscript,
Figure BDA0002130796920000227
for the rotation matrix from { B } to { I },
Figure BDA0002130796920000228
is a rotation matrix from C to B,
Figure BDA0002130796920000229
is a rotation matrix from { D } to { C };
restraint 2, wheel geometry restraint
Figure BDA00021307969200002210
Figure BDA00021307969200002211
Wherein n isyY being the front wheel coordinate system { D }dAxial unit direction vector.
Further, in one embodiment of the present invention, the kinematic model is built according to two constraint equations, including:
and (3) carrying out derivation on constraint conditions of the constraint 1 and the constraint 2 to obtain:
Figure BDA00021307969200002212
wherein J is a Jacobian matrix; the Jacobian matrix can be based on generalized coordinates
Figure BDA00021307969200002213
And non-independent coordinates
Figure BDA00021307969200002214
The decomposition is carried out in two parts:
Figure BDA00021307969200002215
the velocity of the non-independent coordinates is represented by the generalized velocity:
Figure BDA00021307969200002216
the kinematic model can be found as:
Figure BDA0002130796920000231
wherein the content of the first and second substances,
Figure BDA0002130796920000232
is an identity matrix of order 2.
Further, in one embodiment of the present invention, solving the two-wheeled robot dynamics model using the first type of lagrangian equation comprises:
Figure BDA0002130796920000233
wherein, L is a Lagrange function, T is the total kinetic energy of the two-wheeled robot system, V is the total potential energy of the two-wheeled robot system, gamma is a Lagrange multiplier,
Figure BDA0002130796920000234
the generalized non-conservative external force on the two-wheeled robot system, D and D are respectively the disturbance torque on the rolling and steering channels of the handlebar, taucIs the driving torque of the rotating shaft of the handlebar.
Further, in an embodiment of the present invention, the modeling analysis module is specifically configured to analyze the singular value of the controllability matrix, the attraction domain of the closed-loop controller, and the control cost in the range of the towing distance, make a corresponding image, and determine the towing distance meeting the control requirement according to the corresponding image and the control requirement.
It should be noted that the foregoing explanation of the embodiment of the method for modeling and static balancing a two-wheeled robot based on a tow-track is also applicable to the apparatus of this embodiment, and will not be described herein again.
According to the modeling and static balancing device of the two-wheeled robot based on the towing distance, firstly, the two-wheeled robot is equivalent to a multi-link multi-joint system with front and rear vehicle bodies hinged from the multi-rigid system according to the motion constraint. And secondly, establishing two constraint equations based on the closed-loop kinematic chain of the system and the contact characteristics of the wheels and the ground to obtain a kinematic model of the system. The first type of lagrangian equation is then used to derive a kinetic model of the system. And finally, analyzing the influence of the towing distance on the static balance from three aspects of the singular value of the system controllability matrix, the attraction domain of the closed-loop controller and the control energy consumption. The nonlinear relation between the handle turning angle and the height change of the mass center of the vehicle at different towing distances can be reflected, and a set of analysis process can be provided for the selection of the towing distances so as to improve the control effect of static balance.
Furthermore, the terms "first", "second" and "first" are used for descriptive purposes only and are not to be construed as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one such feature. In the description of the present invention, "a plurality" means at least two, e.g., two, three, etc., unless specifically limited otherwise.
In the description herein, references to the description of the term "one embodiment," "some embodiments," "an example," "a specific example," or "some examples," etc., mean that a particular feature, structure, material, or characteristic described in connection with the embodiment or example is included in at least one embodiment or example of the invention. In this specification, the schematic representations of the terms used above are not necessarily intended to refer to the same embodiment or example. Furthermore, the particular features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples. Furthermore, various embodiments or examples and features of different embodiments or examples described in this specification can be combined and combined by one skilled in the art without contradiction.
Although embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention, and that variations, modifications, substitutions and alterations can be made to the above embodiments by those of ordinary skill in the art within the scope of the present invention.

Claims (3)

1. A method for modeling and static balancing of a two-wheeled robot based on a towing distance is characterized by comprising the following steps:
s1, when detecting that the two-wheeled robot system meets the preset equivalent setting condition, setting the two-wheeled robot system as a multi-link multi-joint system, defining a plurality of coordinate systems in the multi-link multi-joint system, and calculating the dragging distance range of the two-wheeled robot system according to the geometrical relationship of the coordinate systems, wherein the preset equivalent setting condition comprises:
setting the rear body center of mass to include only roll and pitch caused by handlebar steering;
setting that the front and the rear wheels are braked and do not move relative to the frame, and the rear wheels roll with the ground;
setting to neglect the thickness and deformation of the tire, regarding the front and the rear wheels as rigid thin disks with equal size, and setting the multiple coordinate systems as:
(1) inertial reference system { I }, A0xyz: the origin is fixed at A0Point, x axis from A0Direction E0The z-axis is vertical downwards, and the y-axis, the x-axis and the z-axis form a right-hand system; wherein A is0Contact point of rear wheel with ground when handlebar is turned, E0The contact point between the front wheel and the ground when the handlebar rotates;
(2) rear wheel coordinate system { B }, Bxbybzb: the origin is fixed at point B, xbThe axis being parallel to the x-axis of the inertial reference system, the z-and y-axes being rotatable about the x-axis by the inertial reference system
Figure FDA0002980727400000011
The angle is found, then the rotation matrix from { I } to { B } is:
Figure FDA0002980727400000012
wherein B is the circle center of the rear wheel,
Figure FDA0002980727400000013
the roll angle of the rear body;
(3) coordinate system of handlebar { C }, Cxcyczc: the origin is fixed at point C, ycAxle and rear wheel coordinate system ybThe axes being parallel, the x and z axes being surrounded by { B } around ybThe axis is rotated by an angle θ + η, and the rotation matrix from { B } to { C } is:
Figure FDA0002980727400000014
c is a connecting point of the handlebar rotating pair and the rear frame, theta is a pitch angle of the rear vehicle body, and eta is a handlebar inclination angle;
the handlebar inclination angle η satisfies the following geometrical constraints:
Figure FDA0002980727400000021
wherein, theta0Is the pitch angle of the rear frame connecting rod vector when the handlebar turning angle is zero, and epsilon is the rear frame connecting rod mounting angle;
(4) front wheel coordinate system { D }, Dxdydzd: the origin is fixed at point D, zdAxis and handlebar coordinate system zcThe axes being parallel, the x and y axes being defined by the handlebar coordinate systemcThe axis is rotated by an angle δ, and the rotation matrix from { C } to { D } is:
Figure FDA0002980727400000022
wherein D is the center of the front wheel circle, and delta is the handlebar turning angle;
(5) rear vehicle body coordinate system { G1}，G1x1y1z1: origin fixed to G1The rear vehicle body coordinate system is wound by the rear wheel coordinate system ybThe angle of rotation of the shaft is obtained by
Figure FDA0002980727400000023
Where η is 0, from { B } system to { G-1Rotation matrix of the system
Figure FDA0002980727400000024
Figure FDA0002980727400000025
Wherein G is1Is the center of mass of the rear vehicle body;
(6) front vehicle body coordinate system { G2}，G2x2y2z2: origin fixed to G2The front body coordinate system is parallel to the front wheel coordinate system, G2Is the front bodywork centroid;
the drag distance range of the two-wheeled robot system is as follows:
Figure FDA0002980727400000026
wherein R is the wheel radius, lrIs the length of segment BC, d is the length of segment CC', lfIs the length of the front frame line segment C' D, λ is the handlebar fork angle, η is the handlebar inclination;
s2, establishing two constraint equations according to the closed-loop kinematic chain borne by the multi-link multi-joint system and the wheel geometric characteristics of the two-wheel robot system, and establishing a kinematic model according to the two constraint equations, wherein,
the method comprises the following steps of establishing two constraint equations according to a closed-loop kinematic chain borne by the multi-connecting-rod multi-joint system and the wheel geometric characteristics of the two-wheeled robot system, wherein the two constraint equations comprise:
constraint 1, closed-loop kinematic chain constraint
Figure FDA0002980727400000031
Figure FDA0002980727400000032
Figure FDA0002980727400000033
Figure FDA0002980727400000034
Figure FDA0002980727400000035
Figure FDA0002980727400000036
Wherein e isz＝[0,0,1]TIs a z-axis unit direction vector, r, of the inertial reference system { I }1Is the rear wheel link vector pointing from a to B; r is2Is the rear frame link vector pointing from B to C; r is3Is the front frame link vector pointing from C to D; r is4Is a front wheel connecting rod vector pointed to E by D, the coordinate system corresponding to the vector is indicated by a superscript,
Figure FDA0002980727400000037
for the rotation matrix from { B } to { I },
Figure FDA0002980727400000038
is a rotation matrix from C to B,
Figure FDA0002980727400000039
is a rotation matrix from { D } to { C };
constraining2, wheel geometry restraint
Figure FDA00029807274000000310
Figure FDA00029807274000000311
Wherein n isyY being the front wheel coordinate system { D }dThe vector in the unit direction of the axis,
the building of the kinematic model according to the two constraint equations comprises:
and (3) carrying out derivation on constraint conditions of the constraint 1 and the constraint 2 to obtain:
Figure FDA00029807274000000312
wherein J is a Jacobian matrix; the Jacobian matrix can be based on generalized coordinates
Figure FDA0002980727400000041
And non-independent coordinates
Figure FDA0002980727400000042
The decomposition is carried out in two parts:
Figure FDA0002980727400000043
the velocity of the non-independent coordinates is represented by the generalized velocity:
Figure FDA0002980727400000044
the kinematic model can be found as follows:
Figure FDA0002980727400000045
wherein the content of the first and second substances,
Figure FDA0002980727400000046
is an identity matrix of 2 orders;
s3, solving the kinematics model by using a first type of Lagrange equation to obtain a two-wheeled robot dynamics model, analyzing a singular value of a controllability matrix, an attraction domain of a closed-loop controller and a control cost according to the two-wheeled robot dynamics model, and determining a towing distance meeting a control requirement in the towing distance range according to an analysis result, wherein the solving of the two-wheeled robot dynamics model by using the first type of Lagrange equation comprises the following steps:
Figure FDA0002980727400000047
wherein, L is a Lagrange function, T is the total kinetic energy of the two-wheeled robot system, V is the total potential energy of the two-wheeled robot system, gamma is a Lagrange multiplier,
Figure FDA0002980727400000048
the generalized non-conservative external force on the two-wheeled robot system, D and D are respectively the disturbance torque on the rolling and steering channels of the handlebar, taucIs the driving torque of the rotating shaft of the handlebar.
2. The method according to claim 1, wherein the S3 specifically includes:
and in the dragging distance range, analyzing singular values of the controllability matrix, the attraction domain of the closed-loop controller and the control cost, making a corresponding image, and determining the dragging distance meeting the control requirement according to the corresponding image and the control requirement.
3. A tow-distance-based two-wheeled robot modeling and static balancing device is characterized by comprising:
an equivalence module for setting the two-wheeled robot system as a multi-link multi-joint system when detecting that the two-wheeled robot system satisfies a preset equivalence setting condition, defining a plurality of coordinate systems in the multi-link multi-joint system, and calculating a range of a tow range of the two-wheeled robot system according to a geometric relationship of the plurality of coordinate systems,
the preset equivalent setting conditions comprise:
setting the rear body center of mass to include only roll and pitch caused by handlebar steering;
setting that the front and the rear wheels are braked and do not move relative to the frame, and the rear wheels roll with the ground;
setting to neglect the thickness and deformation of the tire, regarding the front and the rear wheels as rigid thin disks with equal size, and setting the multiple coordinate systems as:
(1) inertial reference system { I }, A0xyz: the origin is fixed at A0Point, x axis from A0Direction E0The z-axis is vertical downwards, and the y-axis, the x-axis and the z-axis form a right-hand system; wherein A is0Contact point of rear wheel with ground when handlebar is turned, E0The contact point between the front wheel and the ground when the handlebar rotates;
(2) rear wheel coordinate system { B }, Bxbybzb: the origin is fixed at point B, xbThe axis being parallel to the x-axis of the inertial reference system, the z-and y-axes being rotatable about the x-axis by the inertial reference system
Figure FDA0002980727400000051
The angle is found, then the rotation matrix from { I } to { B } is:
Figure FDA0002980727400000052
wherein B is the circle center of the rear wheel,
Figure FDA0002980727400000053
the roll angle of the rear body;
(3) coordinate system of handlebar { C }, Cxcyczc: the origin is fixed at point C, ycAxle and rear wheel coordinate system ybThe axes being parallel, the x and z axes being surrounded by { B } around ybThe axis is rotated by an angle θ + η, and the rotation matrix from { B } to { C } is:
Figure FDA0002980727400000054
c is a connecting point of the handlebar rotating pair and the rear frame, theta is a pitch angle of the rear vehicle body, and eta is a handlebar inclination angle;
the handlebar inclination angle η satisfies the following geometrical constraints:
Figure FDA0002980727400000055
wherein, theta0Is the pitch angle of the rear frame connecting rod vector when the handlebar turning angle is zero, and epsilon is the rear frame connecting rod mounting angle;
(4) front wheel coordinate system { D }, Dxdydzd: the origin is fixed at point D, zdAxis and handlebar coordinate system zcThe axes being parallel, the x and y axes being defined by the handlebar coordinate systemcThe axis is rotated by an angle δ, and the rotation matrix from { C } to { D } is:
Figure FDA0002980727400000061
wherein D is the center of the front wheel circle, and delta is the handlebar turning angle;
(5) rear vehicle body coordinate system { G1}，G1x1y1z1: origin fixed to G1The rear vehicle body coordinate system is wound by the rear wheel coordinate system ybThe angle of rotation of the shaft is obtained by
Figure FDA0002980727400000062
Where η is 0, from { B } system to { G-1Rotation matrix of the system
Figure FDA0002980727400000063
Figure FDA0002980727400000064
Wherein G is1Is the center of mass of the rear vehicle body;
(7) front vehicle body coordinate system { G2}，G2x2y2z2: origin fixed to G2The front body coordinate system is parallel to the front wheel coordinate system, G2Is the front bodywork centroid;
the drag distance range of the two-wheeled robot system is as follows:
Figure FDA0002980727400000065
wherein R is the wheel radius, lrIs the length of segment BC, d is the length of segment CC', lfIs the length of the front frame line segment C' D, λ is the handlebar fork angle, η is the handlebar inclination;
a constraint module for establishing two constraint equations according to the closed-loop kinematic chain borne by the multi-link multi-joint system and the wheel geometric characteristics of the two-wheeled robot system, and establishing a kinematic model according to the two constraint equations, wherein,
the method comprises the following steps of establishing two constraint equations according to a closed-loop kinematic chain borne by the multi-connecting-rod multi-joint system and the wheel geometric characteristics of the two-wheeled robot system, wherein the two constraint equations comprise:
constraint 1, closed-loop kinematic chain constraint
Figure FDA0002980727400000071
Figure FDA0002980727400000072
Figure FDA0002980727400000073
Figure FDA0002980727400000074
Figure FDA0002980727400000075
Figure FDA0002980727400000076
Wherein e isz＝[0,0,1]TIs a z-axis unit direction vector, r, of the inertial reference system { I }1Is the rear wheel link vector pointing from a to B; r is2Is the rear frame link vector pointing from B to C; r is3Is the front frame link vector pointing from C to D; r is4Is a front wheel connecting rod vector pointed to E by D, the coordinate system corresponding to the vector is indicated by a superscript,
Figure FDA0002980727400000077
for the rotation matrix from { B } to { I },
Figure FDA0002980727400000078
is a rotation matrix from C to B,
Figure FDA0002980727400000079
is a rotation matrix from { D } to { C };
restraint 2, wheel geometry restraint
Figure FDA00029807274000000710
Figure FDA00029807274000000711
Wherein n isyY being the front wheel coordinate system { D }dThe vector in the unit direction of the axis,
the building of the kinematic model according to the two constraint equations comprises:
and (3) carrying out derivation on constraint conditions of the constraint 1 and the constraint 2 to obtain:
Figure FDA00029807274000000712
wherein J is a Jacobian matrix; the Jacobian matrix can be based on generalized coordinates
Figure FDA00029807274000000713
And non-independent coordinates
Figure FDA00029807274000000714
The decomposition is carried out in two parts:
Figure FDA00029807274000000715
the velocity of the non-independent coordinates is represented by the generalized velocity:
Figure FDA00029807274000000716
the kinematic model can be found as follows:
Figure FDA0002980727400000081
wherein the content of the first and second substances,
Figure FDA0002980727400000082
is an identity matrix of 2 orders;
the modeling analysis module is used for solving the kinematics model by using a first type of Lagrange equation to obtain a two-wheeled robot dynamics model, analyzing a singular value of a controllability matrix, an attraction domain of a closed-loop controller and a control cost according to the two-wheeled robot dynamics model, and determining a towing distance meeting a control requirement in the towing distance range according to an analysis result, wherein the step of solving the two-wheeled robot dynamics model by using the first type of Lagrange equation comprises the following steps:
Figure FDA0002980727400000083
wherein, L is a Lagrange function, T is the total kinetic energy of the two-wheeled robot system, V is the total potential energy of the two-wheeled robot system, gamma is a Lagrange multiplier,
Figure FDA0002980727400000084
the generalized non-conservative external force on the two-wheeled robot system, D and D are respectively the disturbance torque on the rolling and steering channels of the handlebar, taucIs the driving torque of the rotating shaft of the handlebar.
CN201910637561.5A 2019-07-15 2019-07-15 Method and device for modeling and static balancing of two-wheeled robot based on towing distance Active CN110348140B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910637561.5A CN110348140B (en) 2019-07-15 2019-07-15 Method and device for modeling and static balancing of two-wheeled robot based on towing distance

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910637561.5A CN110348140B (en) 2019-07-15 2019-07-15 Method and device for modeling and static balancing of two-wheeled robot based on towing distance

Publications (2)

Publication Number Publication Date
CN110348140A CN110348140A (en) 2019-10-18
CN110348140B true CN110348140B (en) 2021-06-25

Family

ID=68176323

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910637561.5A Active CN110348140B (en) 2019-07-15 2019-07-15 Method and device for modeling and static balancing of two-wheeled robot based on towing distance

Country Status (1)

Country Link
CN (1) CN110348140B (en)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114146363B (en) * 2021-12-14 2022-08-30 国家康复辅具研究中心 Walking aid training system and integrated control method thereof
CN116184860B (en) * 2023-04-27 2023-08-01 北京国领智能科技有限公司 Magnetic attraction crawler type wall climbing robot dynamics modeling and control method

Family Cites Families (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2003078109A1 (en) * 2002-03-15 2003-09-25 Sony Corporation Operation control device for leg-type mobile robot and operation control method, and robot device
CN101791800B (en) * 2010-01-21 2011-05-25 西北工业大学 Motion control method of double-wheel differential type robot
CN102780542A (en) * 2012-07-19 2012-11-14 南京邮电大学 Gain factor adjustment method for Hopfield neural network signal blind detection
US20140130700A1 (en) * 2012-11-14 2014-05-15 Swift Tram, Inc. Suspended coach drive bogie
CN105080013B (en) * 2015-07-13 2019-03-08 李占平 Specialized robot multi link stable support structure and its control method
US9908549B2 (en) * 2015-08-18 2018-03-06 Powerhandling Inc. Dual motor drive system for a material handling device
CN105320129B (en) * 2015-10-22 2018-05-29 清华大学 A kind of method of unmanned bicycle Trajectory Tracking Control
EP3437945B1 (en) * 2016-03-31 2021-05-12 Honda Motor Co., Ltd. Brake control device for motorcycle
CN106774360B (en) * 2016-11-24 2019-06-04 西北工业大学 It is a kind of to pull the target satellite attitude stabilization method for becoming in rail and utilizing tether and connecting rod
EP3332987B1 (en) * 2016-12-09 2020-07-01 Permobil AB A caster wheel assembly and a wheelchair comprising the same
CN109202889A (en) * 2017-06-30 2019-01-15 沈阳新松机器人自动化股份有限公司 A kind of Flexible Multi-joint robot electric current Force control system and method
CN109634111B (en) * 2017-10-09 2020-08-04 中国科学院沈阳自动化研究所 Dynamic deformation calculation method for high-speed heavy-load robot
CN208181323U (en) * 2018-04-19 2018-12-04 北京特睿夫科技有限公司 A kind of motor cycle multi link steering structure
CN108839015A (en) * 2018-06-01 2018-11-20 重庆锐纳达自动化技术有限公司 A kind of robot two-wheel differential obstacle detouring bobbin movement track acquisition methods
CN109885052B (en) * 2019-02-26 2022-03-25 华南理工大学 Error model prediction control method based on omnidirectional mobile robot kinematics modeling

Also Published As

Publication number Publication date
CN110348140A (en) 2019-10-18

Similar Documents

Publication Publication Date Title
JP6845601B2 (en) In-vehicle motion simulation stage based on active suspension and its control method
CN110348140B (en) Method and device for modeling and static balancing of two-wheeled robot based on towing distance
Yang et al. RMPC-based directional stability control for electric vehicles subject to tire blowout on curved expressway
Ishigami et al. Design, development, and mobility evaluation of an omnidirectional mobile robot for rough terrain
Zhao et al. Modeling and motion control of industrial tractor–trailers vehicles using force compensation
Moshayedi et al. Review on: The service robot mathematical model
Jiang et al. LQR optimal control research for four-wheel steering forklift based-on state feedback
CN111752150A (en) Four-wheel cooperative control method for wheeled-foot robot
CN115675524A (en) Self-adaptive control method for transverse track tracking of automatic driving semi-trailer truck
Gans et al. Visual servo velocity and pose control of a wheeled inverted pendulum through partial-feedback linearization
Liu et al. Active suspension control with consensus strategy for dynamic posture tracking of wheel-legged robotic systems on uneven surfaces
CN110109353A (en) A kind of reaction wheel balance-bicycle Robot Fuzzy adaptive sliding-mode observer system
Fang et al. Research on a medium-tracked omni-vehicle
Limebeer et al. Optimal control of a NASCAR–specification race car
CN114291117A (en) Four-wheel steering unmanned vehicle trajectory tracking control method
Yakub et al. Autonomous ground vehicle of path following control through model predictive control with feed forward controller
Ajmi et al. Dynamic modeling and handling study of a two-wheeled vehicle on a curved track
Malvezzi et al. Error analysis for an active geometry control suspension system
CN114859733B (en) Differential steering unmanned vehicle trajectory tracking and attitude control method
CN108839015A (en) A kind of robot two-wheel differential obstacle detouring bobbin movement track acquisition methods
Toumi et al. Development of analytical model for modular tank vehicle carrying liquid cargo
Du et al. Control strategy for four-wheel steering vehicle based on collaborative simulation
Tan et al. A novel autonomous scaled electric combat vehicle
Chawla Estimation of side slip in vehicles
Zhang Parameter Selection for Improved Handling Performance of Automated Vehicles

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