CN110348140A - Based on towing away from two-wheel robot modeling and static balance method and device - Google Patents

Abstract

The invention discloses it is a kind of based on towing away from two-wheel robot modeling and static balance method and device, wherein, this method comprises: setting multi link multi-joint system for two-wheel robot system, defining multiple coordinate systems in multi link multi-joint system and calculating the towing of two-wheel robot system away from range；Two constraint equations are established according to the wheel geometrical property of closed loop moving chain suffered by multi link multi-joint system and two-wheel robot system and establish kinematics model；Kinematics model is solved using lagrange equations of the first kind to obtain two-wheel Dynamic Models of Robot Manipulators, and the singular value of controllability matrix, the domain of attraction of closed loop controller and control cost are analyzed, based on the analysis results towing away from determine to meet in range the towing of demand for control away from.This method can be reflected in it is different towing away from lower handlebar corner and height of center of mass variation non-linear relations, and can for towing away from selection a set of analysis process is provided, improve the control effect of static balance.

Description

Modeling and static balancing method and device for a two-wheeled robot based on trailing distance

technical field

The invention relates to the technical field of mechanical system modeling and dynamic analysis, in particular to a method and device for modeling and static balancing of a two-wheeled robot based on a drag distance.

Background technique

The drag distance is the distance between the intersection point of the handlebar shaft and the ground and the ground contact point of the front wheel, which has an important influence on the balance control of the two-wheeled robot using the handlebar steering.

For a two-wheeled robot with a non-zero trailing distance, turning the handlebar can fine-tune the height of the center of mass of the robot to prevent it from tipping over. When the speed of the two-wheeled robot reaches a certain level, the restoring torque provided by the ground enables the front wheel shaft to automatically straighten the robot without additional control force; The balance at the time mainly depends on the steering of the handlebar. It can be seen that for the more challenging static balance, the trail becomes the key. The existing research on the static balance of handlebar steering points out that the controller based on the design of positive trailing distance has a small field of attraction and poor robustness, so it is necessary to explore the influence of trailing distance on the control performance. However, current research only models the robot with a fixed drag distance, and simplifies the height change of the center of mass into a linear model. Such a model is not enough to analyze the influence of trailing distance, so it is urgent to establish a dynamic model suitable for any trailing distance.

Contents of the invention

The present invention aims to solve one of the technical problems in the related art at least to a certain extent.

For this reason, an object of the present invention is to propose a kind of two-wheel robot modeling and static balance method based on trailing distance, and this method can reflect the non-linear relation of handlebar rotation angle and barycenter height variation under different trailing distances, and can be The selection of trailing distance provides a set of analysis procedures to improve the control effect of static balance.

Another object of the present invention is to propose a two-wheeled robot modeling and static balancing device based on trailing distance.

In order to achieve the above purpose, an embodiment of the present invention proposes a trail-based two-wheeled robot modeling and static balancing method, including:

S1, when it is detected that the two-wheel robot system satisfies the preset equivalent setting conditions, setting the two-wheel robot system as a multi-link multi-joint system, and defining multiple coordinate systems in the multi-link multi-joint system, calculating the drag distance range of the two-wheel robot system according to the geometric relationship of the plurality of coordinate systems;

S2, establishing two constraint equations according to the closed-loop kinematic chain subjected to the multi-link multi-joint system and the wheel geometric characteristics of the two-wheel robot system, and establishing a kinematics model according to the two constraint equations;

S3, using the first kind of Lagrangian equation to solve the kinematics model to obtain the dynamic model of the two-wheel robot, and according to the dynamic model of the two-wheel robot, the singular value of the controllability matrix and the closed-loop controller The area of attraction and the control cost are analyzed, and the trailing distance that meets the control requirements is determined in the trailing distance range according to the analysis results.

In the trail-based two-wheeled robot modeling and static balance method of the embodiment of the present invention, firstly, according to the motion constraints, the two-wheeled robot is equivalent to a multi-link multi-joint system with two front and rear bodies articulated from the perspective of a multi-rigid body system. Secondly, based on the closed-loop kinematic chain of the system and the contact characteristics between the wheel and the ground, two constraint equations are established to obtain the kinematics model of the system. Then, the dynamic model of the system is derived by using the Lagrangian equation of the first kind. Finally, the influence of trailing distance on static balance is analyzed from three aspects: singular value of system controllability matrix, closed-loop controller attraction region and control energy consumption. It can reflect the nonlinear relationship between the handlebar angle and the height of the center of mass under different trailing distances, and can provide a set of analysis procedures for the selection of trailing distances to improve the control effect of static balance.

In addition, the trail-based two-wheeled robot modeling and static balancing method according to the above-mentioned embodiments of the present invention may also have the following additional technical features:

Further, in an embodiment of the present invention, the preset equivalent setting conditions include:

Set the rear body center of mass to only include roll and pitch caused by handlebar steering;

The front and rear wheels are set to be braked, there is no relative movement with the frame, and the rear wheel is purely rolling with the ground;

It is set to ignore the tire thickness and deformation, and treat the front and rear wheels as rigid thin discs of equal size.

Further, in one embodiment of the present invention, the multiple coordinate systems are:

(1) Inertial reference frame {I}, A ₀ xyz: the origin is fixed at A ₀ point, the x-axis points from A ₀ to E ₀ , the z-axis is vertically downward, and the y-axis forms a right-hand system with the x-axis and z-axis; where , A ₀ is the contact point between the rear wheel and the ground when the handlebar is turned, and E ₀ is the contact point between the front wheel and the ground when the handlebar is turned;

(2) Rear wheel coordinate system {B}, Bx _b y _b z _b : the origin is fixed at point B, the x and _b axes are parallel to the x axis of the inertial reference system, and the z and y axes can be rotated around the x axis by the inertial reference system Angle is obtained, then the rotation matrix from {I} to {B} is:

Among them, B is the center of the rear wheel, is the roll angle of the rear body;

(3) Handlebar coordinate system {C}, Cx _c y _c z _c : the origin is fixed at point C, the y _c axis is parallel to the y _b axis of the rear wheel coordinate system, and the x and z axes can be circled by {B} around the y _b axis It is obtained by rotating θ+η angle, then the rotation matrix from {B} to {C} is:

Wherein, C is the connection point between the handlebar swivel and the rear frame, θ is the pitch angle of the rear body, and η is the inclination angle of the handlebar;

The handlebar inclination η satisfies the following geometric constraints:

Among them, _θ0 is the pitch angle of the rear frame link vector when the handlebar rotation angle is zero, and ε is the installation angle of the rear frame link;

(4) Front wheel coordinate system {D}, Dx _d y _d z _d : the origin is fixed at point D, the z _d axis is parallel to the z _c axis of the handlebar coordinate system, and the x and y axes can be circled by the handlebar coordinate system z _c The axis is rotated by δ angle, then the rotation matrix from {C} to {D} is:

Among them, D is the center of the front wheel, δ is the handlebar angle;

(5) Rear car body coordinate system {G ₁ }, G ₁ x ₁ y ₁ z ₁ : the origin is fixed at G ₁ , and the rear car body coordinate system is obtained by rotating the rear wheel coordinate system around the y _b axis by an angle θ. Let In η=0, the rotation matrix from {B} system to {G ₁ } system can be obtained

Among them, G ₁ is the center of mass of the rear car body;

(6) Front car body coordinate system {G ₂ }, G ₂ x ₂ y ₂ z ₂ : the origin is fixed at G ₂ , the front car body coordinate system is parallel to the front wheel coordinate system, and G ₂ is the center of mass of the front car body.

Further, in one embodiment of the present invention, the range of the trailing distance of the two-wheeled robot system is:

Among them, R is the radius of the wheel, l _r is the length of line segment BC, d is the length of line segment CC′, l _f is the length of line segment C′D of the front frame, λ is the front fork angle of the handlebar, and η is the inclination angle of the handlebar.

Further, in one embodiment of the present invention, the two constraint equations are established according to the closed-loop kinematic chain subjected to the multi-link multi-joint system and the wheel geometric characteristics of the two-wheel robot system, including:

Constraint 1, closed-loop kinematic chain constraint

Among them, e _z =[0,0,1] ^T is the z-axis unit direction vector of the inertial reference frame {I}, r ₁ is the rear wheel linkage vector from A to B; r ₂ is the rear wheel linkage vector from B to C The frame link vector; r ₃ is the front frame link vector from C to D; r ₄ is the front wheel link vector from D to E, and the superscript indicates the coordinate system corresponding to this vector, is the rotation matrix from {B} to {I}, is the rotation matrix from {C} to {B}, is the rotation matrix from {D} to {C};

Constraint 2, Wheel Geometry Constraints

Among them, n _y is the unit direction vector of the y _d axis of the front wheel coordinate system {D}.

Further, in an embodiment of the present invention, the establishment of a kinematics model according to the two constraint equations includes:

Deriving the constraint conditions of constraint 1 and constraint 2, we get:

Among them, J is the Jacobian matrix; the Jacobian matrix can be based on the generalized coordinate with dependent coordinates Break it down into two parts:

Velocities of dependent coordinates are represented by generalized velocities:

It can be obtained that the kinematic model is:

in, is a second-order identity matrix.

Further, in one embodiment of the present invention, using the first type of Lagrangian equation to solve the dynamic model of the two-wheeled robot includes:

Among them, L=TV is the Lagrange function, T is the total kinetic energy of the two-wheel robot system, V is the total potential energy of the two-wheel robot system, γ is the Lagrangian multiplier, is the generalized non-conservative external force on the two-wheel robot system, D and _d are the disturbance moments on the roll and handlebar steering channel respectively, and τc is the driving torque on the handlebar shaft.

Further, in an embodiment of the present invention, the S3 specifically includes:

Within the range of the trailing distance, analyze the singular value of the controllability matrix, the attractive region of the closed-loop controller and the control cost, and make a corresponding image, and determine according to the corresponding image and the control requirement Trailing distance that meets the stated control needs.

In order to achieve the above purpose, another embodiment of the present invention proposes a two-wheeled robot modeling and static balancing device based on drag distance, including:

An equivalent module, configured to set the two-wheel robot system as a multi-link multi-joint system when detecting that the two-wheel robot system satisfies preset equivalent setting conditions, and define multiple a coordinate system, and calculate the drag distance range of the two-wheel robot system according to the geometric relationship of the plurality of coordinate systems;

A constraint module, configured to establish two constraint equations according to the closed-loop kinematic chain subjected to the multi-link multi-joint system and the wheel geometric characteristics of the two-wheel robot system, and establish a kinematics model according to the two constraint equations;

The modeling analysis module is used to solve the kinematics model by using the first kind of Lagrangian equation to obtain the dynamic model of the two-wheel robot, and analyze the singular value of the controllability matrix according to the dynamic model of the two-wheel robot 1. The attraction domain and control cost of the closed-loop controller are analyzed, and the trailing distance that meets the control requirements is determined in the trailing distance range according to the analysis results.

The trail-based two-wheel robot modeling and static balancing device of the embodiment of the present invention, firstly, according to the motion constraints, from the perspective of a multi-rigid body system, the two-wheel robot is equivalent to a multi-link multi-joint system with two front and rear bodies articulated. Secondly, based on the closed-loop kinematic chain of the system and the contact characteristics between the wheel and the ground, two constraint equations are established to obtain the kinematics model of the system. Then, the dynamic model of the system is derived by using the Lagrangian equation of the first kind. Finally, the influence of trailing distance on static balance is analyzed from three aspects: singular value of system controllability matrix, closed-loop controller attraction region and control energy consumption. It can reflect the nonlinear relationship between the handlebar angle and the height of the center of mass under different trailing distances, and can provide a set of analysis procedures for the selection of trailing distances to improve the control effect of static balance.

In addition, the trail-based two-wheeled robot modeling and static balancing device according to the above-mentioned embodiments of the present invention may also have the following additional technical features:

Further, in an embodiment of the present invention, the preset equivalent setting conditions include:

Set the rear body center of mass to only include roll and pitch caused by handlebar steering;

The front and rear wheels are set to be braked, there is no relative movement with the frame, and the rear wheel is purely rolling with the ground;

It is set to ignore the tire thickness and deformation, and treat the front and rear wheels as rigid thin discs of equal 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.

Description of drawings

The above and/or additional aspects and advantages of the present invention will become apparent and easy to understand from the following description of the embodiments in conjunction with the accompanying drawings, wherein:

Fig. 1 is a flow chart of a two-wheeled robot modeling and static balancing method based on a trailing distance according to an embodiment of the present invention;

Fig. 2 is a schematic diagram of a multi-link multi-joint equivalent vehicle body according to an embodiment of the present invention;

Fig. 3 is a flow chart of drag distance controllability analysis according to an embodiment of the present invention;

Fig. 4 is a schematic structural diagram of a two-wheeled robot modeling and static balancing device based on trailing distance according to an embodiment of the present invention.

Detailed ways

Embodiments of the present invention are described in detail below, examples of which are shown in the drawings, wherein the same or similar reference numerals designate the same or similar elements or elements having the same or similar functions throughout. The embodiments described below by referring to the figures are exemplary and are intended to explain the present invention and should not be construed as limiting the present invention.

The method and device for modeling and static balancing of a two-wheeled robot based on trailing distance proposed according to an embodiment of the present invention will be described below with reference to the accompanying drawings.

Firstly, a method for modeling and static balancing of a two-wheeled robot based on a trailing distance proposed according to an embodiment of the present invention will be described with reference to the accompanying drawings.

Fig. 1 is a flowchart of a method for modeling and static balancing of a two-wheeled robot based on trailing distance according to an embodiment of the present invention.

As shown in Figure 1, the trail-based two-wheel robot modeling and static balancing method includes the following steps:

Step S1, when it is detected that the two-wheel robot system satisfies the preset equivalent setting conditions, set the two-wheel robot system as a multi-link multi-joint system, and define multiple coordinate systems in the multi-link multi-joint system, according to multiple The geometric relationship of the coordinate system is used to calculate the drag distance range of the two-wheel robot system.

Further, preset equivalent setting conditions include:

Set the rear body center of mass to only include roll and pitch caused by handlebar steering;

The front and rear wheels are set to be braked, there is no relative movement with the frame, and the rear wheel is purely rolling with the ground;

It is set to ignore the tire thickness and deformation, and treat the front and rear wheels as rigid thin discs of equal size.

Specifically, in the process of deriving the model, some assumptions need to be made to facilitate the establishment and derivation of the model, such as:

(1) It is assumed that the center of mass of the rear body has no other motion except roll and pitch caused by handlebar steering;

(2) Both the front and rear wheels have been braked, there is no relative movement with the frame, and the rear wheels are purely rolling with the ground;

(3) Neglecting the tire thickness and deformation, the two wheels are regarded as rigid thin discs of equal size.

After the above assumptions are satisfied, the two-wheel robot system can be equivalent to a multi-link multi-joint system, as shown in Figure 2, which is a schematic diagram of a multi-link multi-joint equivalent car body. Among them, A ₀ and A are the contact points between the rear wheel and the ground when the handlebar is turned and not turned, respectively. B is the center of the rear wheel, C is the connection point between the handlebar swivel and the rear frame, D is the center of the front wheel, and E is the contact point between the front wheel and the ground. δ and θ respectively represent the roll angle of the rear body, the handlebar angle and the pitch angle of the rear body, and _θ1 is the angular displacement of point E on the front wheel. is the front wheel roll angular velocity. _Δxr is the translational distance along the intersection line between the rear body plane _Π1 and the ground caused by the pitching motion of the rear wheel. Δx _f and Δy _f are the offset coordinates of point E on the ground relative to the initial point E ₀ due to handlebar rotation. Table 1 is a summary table of equivalent joints of the embodiment of the present invention, so that the above translation and rotation motions are realized by five joints. r _i (i=1~4) is the equivalent link vector linking these five joints, where r ₁ is the rear wheel link vector from A to B; r ₂ is the rear frame link from B to C Rod vector; r ₃ is the front frame linkage vector from C to D; r ₄ is the front wheel linkage vector from D to E. In addition, θ ₀ is the pitch angle of the rear frame link vector when the handlebar rotation angle is zero; ε is the installation angle of the rear frame link, which is the angle between r ₂ and the handlebar rotation axis; η is the handlebar inclination angle, λ is the angle between the handlebar shaft and the vertical direction of the rear body plane; λ is the front fork angle of the handlebar, which is the angle between the front frame C′D and the handlebar shaft. G ₂ and G ₁ are the center of mass of the front and rear car bodies respectively.

Table 1

After setting the two-wheel robot system as a multi-link multi-joint system, define the following six coordinate systems in the multi-link multi-joint system:

(1) Inertial reference frame {I}, A ₀ xyz: the origin is fixed at A ₀ point, the x-axis points from A ₀ to E ₀ , the z-axis is vertically downward, and the y-axis forms a right-hand system with the x-axis and z-axis; where , A ₀ is the contact point between the rear wheel and the ground when the handlebar is turned, and E ₀ is the contact point between the front wheel and the ground when the handlebar is turned;

(2) Rear wheel coordinate system {B}, Bx _b y _b z _b : the origin is fixed at point B, the x and _b axes are parallel to the x axis of the inertial reference system, and the z and y axes can be rotated around the x axis by the inertial reference system Angle is obtained, then the rotation matrix from {I} to {B} is:

Among them, B is the center of the rear wheel, is the roll angle of the rear body;

(3) Handlebar coordinate system {C}, Cx _c y _c z _c : the origin is fixed at point C, the y _c axis is parallel to the y _b axis of the rear wheel coordinate system, and the x and z axes can be circled by {B} around the y _b axis Obtained by rotating θ+η angle, then the rotation matrix from {B} to {C} is:

Wherein, C is the connection point between the handlebar swivel and the rear frame, θ is the pitch angle of the rear body, and η is the inclination angle of the handlebar;

The handlebar inclination η satisfies the following geometric constraints:

Among them, _θ0 is the pitch angle of the rear frame link vector when the handlebar rotation angle is zero, and ε is the installation angle of the rear frame link;

(4) Front wheel coordinate system {D}, Dx _d y _d z _d : the origin is fixed at point D, the z _d axis is parallel to the z _c axis of the handlebar coordinate system, and the x and y axes can be circled by the handlebar coordinate system z _c The axis is rotated by δ angle, then the rotation matrix from {C} to {D} is:

Among them, D is the center of the front wheel, δ is the handlebar angle;

(5) Rear car body coordinate system {G ₁ }, G ₁ x ₁ y ₁ z ₁ : the origin is fixed at G ₁ , and the rear car body coordinate system is obtained by rotating the rear wheel coordinate system around the y _b axis by an angle θ. Let In η=0, the rotation matrix from {B} system to {G ₁ } system can be obtained

Among them, G ₁ is the center of mass of the rear car body;

(6) Front car body coordinate system {G ₂ }, G ₂ x ₂ y ₂ z ₂ : the origin is fixed at G ₂ , the front car body coordinate system is parallel to the front wheel coordinate system, and G ₂ is the center of mass of the front car body.

According to the above coordinate system, each vector in Fig. 2 can be expressed as:

In the formula: the superscript indicates the coordinate system corresponding to the vector, R is the radius of the wheel, l _r is the length of the line segment BC, d is the length of the line segment CC′, and l _f is the length of the line segment C′D.

From the geometric relationship, the drag distance of the two-wheeled robot can be obtained as:

In step S2, two constraint equations are established according to the closed-loop kinematic chain of the multi-link multi-joint system and the wheel geometric characteristics of the two-wheel robot system, and a kinematic model is established 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 subjected to the multi-link multi-joint system and the wheel geometric characteristics of the two-wheel robot system, including:

Constraint 1, closed-loop kinematic chain constraint

Among them, e _z =[0,0,1] ^T is the z-axis unit direction vector of the inertial reference frame {I}, r ₁ is the rear wheel linkage vector from A to B; r ₂ is the rear wheel linkage vector from B to C The frame link vector; r ₃ is the front frame link vector from C to D; r ₄ is the front wheel link vector from D to E, and the superscript indicates the coordinate system corresponding to this vector, is the rotation matrix from {B} to {I}, is the rotation matrix from {C} to {B}, is the rotation matrix from {D} to {C};

Constraint 2, Wheel Geometry Constraints

Among them, n _y is the unit direction vector of the y _d axis of the front wheel coordinate system {D}.

Further, in one embodiment of the present invention, a kinematics model is established according to two constraint equations, including:

Deriving the constraint conditions of constraint 1 and constraint 2, we get:

Among them, J is the Jacobian matrix; the Jacobian matrix can be based on the generalized coordinate with dependent coordinates Break it down into two parts:

Velocities of dependent coordinates are represented by generalized velocities:

It can be obtained that the kinematic model is:

in, is a second-order identity matrix.

Specifically, first define the generalized coordinates and complete constraints of the system,

In the above motion, only the roll angle of the rear body are independent of the steering angle δ of the handlebar, so define them as generalized coordinates of the system:

In the process of static balance, the front and rear wheels of the vehicle need to touch the ground, forming a closed-loop kinematic chain. The existence of the closed-loop kinematic chain makes there are two paths starting from the ground to reach any point on the system. Considering that the front wheels involve more dependent coordinates, the present invention uses the contact point _A0 between the rear wheels and the ground as the starting point of the path. In this way, only half of the coordinates are needed to represent the position of the front wheel contact point under the {I} system. Therefore, let the new system coordinates be:

Construct the following two complete constraints based on the closed-loop kinematic chain and wheel geometry to which the system is subjected:

Constraint 1, closed-loop kinematic chain constraint

Wherein, e _z =[0,0,1] ^T is the z-axis unit direction vector of the inertial reference frame {I}.

Constraint 2, Wheel Geometry Constraints

Among them, n _y is the unit direction vector of the y _d axis of the front wheel coordinate system {D}, which is also the normal vector of the front body plane Π ₂ .

When δ=θ=0, _θ1 should satisfy:

θ ₁ =-η | _{δ = θ = 0} (11)

In addition, θ ₀ should also satisfy the following constraints:

Establish the robot kinematics model according to the constraint equation to obtain the nonlinear height change model of the center of mass,

Deriving the complete constraints (9) and (10) gives:

In the formula: J is the Jacobian matrix. The matrix can be based on q _i and dependent coordinates Break it down into two parts:

Based on the above formula, the velocity of the dependent coordinates can be expressed by the generalized velocity: Therefore, the system kinematics model can be obtained as:

In the formula: is a second-order identity matrix.

Step S3, using the first kind of Lagrangian equation to solve the kinematics model to obtain the dynamic model of the two-wheel robot, and according to the dynamic model of the two-wheel robot, the singular value of the controllability matrix, the attraction domain of the closed-loop controller and The control cost is analyzed, and the trailing distance that meets the control requirements is determined in the trailing distance range according to the analysis results.

Further, in one embodiment of the present invention, using the first type of Lagrangian equation to solve the dynamic model of the two-wheeled robot includes:

Among them, L=TV is the Lagrange function, T is the total kinetic energy of the two-wheel robot system, V is the total potential energy of the two-wheel robot system, γ is the Lagrangian multiplier, is the generalized non-conservative external force on the two-wheel robot system, D and _d are the disturbance moments on the roll and handlebar steering channel respectively, and τc is the driving torque on the handlebar shaft.

Further, in an embodiment of the present invention, S3 specifically includes:

Within the range of the trailing distance, the singular values of the controllability matrix, the domain of attraction and the control cost of the closed-loop controller are analyzed, and the corresponding images are made, and the trailing distance that meets the control requirements is determined according to the corresponding images and control requirements.

Specifically, the dynamic model of the two-wheeled robot is established according to the kinematics model. First, the total kinetic energy of the two-wheeled robot system is derived, which is composed of the translational kinetic energy T _ti of the center of mass of the front and rear bodies and the rotational kinetic energy T _ri around the center of mass:

Kinetic energy 1. Express the G ₁ coordinates of the center of mass of the rear car body under the inertial system {I}:

In the formula: x _G1 =x _g1 cosθ ₀ +z _g1 sinθ ₀ , z _G1 =z _g1 cosθ ₀ -x _g1 sinθ ₀ , x _g1 and z _g1 are the coordinates of the center of mass of the rear vehicle relative to the rear frame. is the translation of the rear wheel on the x-axis caused by the pitch, and can be obtained according to assumption 2 Then project the angular velocity of the rear frame into the coordinate system of the rear body {G ₁ }:

Therefore, the kinetic energy of the rear car body can be obtained as follows:

In the formula: m ₁ is the mass of the rear car body, let {G ₁ } be the main axis of inertia of the rear car body, then is the moment of inertia matrix of the rear body in the body coordinate system {G ₁ }.

Kinetic energy 2. Express the G ₂ coordinates of the center of mass of the front car body under the inertial system {I}:

In the formula: x _g2 and z _g2 are the coordinates of the center of mass of the front vehicle relative to the front frame, x _G2 =d+x _g2 cosλ+z _g2 sinλ, z _G2 =z _g2 cosλ-x _g2 sinλ. Similarly, project the angular velocity of the front frame into the front body coordinate system {G ₂ }:

Therefore, the kinetic energy of the front body can be obtained as follows:

In the formula: m ₂ is the mass of the front car body, let {G ₂ } the main axis of inertia of the front car body, then is the moment of inertia matrix of the front body in the body coordinate system {G ₂ }.

Taking the ground as the zero potential energy surface, then taking the z-axis component of the center of mass of the two vehicles under the inertial frame {I}, the total potential energy of the vehicle can be obtained as:

In the formula: g is the surface gravitational acceleration, (·) _z is the operator for taking the z-axis component of the vector.

Solve the dynamics model of the system using Lagrangian equations of the first kind:

In the formula: L=TV is the Lagrangian function, T is the total kinetic energy, V is the total potential energy, γ is the Lagrangian multiplier, is the generalized non-conservative external force on the system, D and _d are the disturbance moments on the roll and handlebar steering channel respectively, and τc is the driving torque of the handlebar shaft.

Write formula (23) as the following general Euler-Lagrangian form for robots:

In the formula: M is the generalized mass matrix, V is the centrifugal force and Coriolis force matrix, E is the gravity matrix, and its specific expressions are presented in the appendix. Using the kinematic model (15), the above model can be transformed into an ordinary differential equation containing only two equations. Firstly, the system coordinate acceleration can be obtained by deriving formula (15):

Substitute the above formula and kinematic equation (15) into formula (25), and then multiply both ends by G ^T to get:

According to G ^T J ^T = 0, γ can be eliminated to obtain the dimensionality reduction dynamic model:

In the formula: M _i ＝ G ^T MG is the mass matrix of dimensionality reduction system, is the damping matrix of the dimensionality reduction system, and Q _iext = G ^T Q _ext is the generalized external force matrix of the dimensionality reduction system.

Put the dimensionality reduction dynamic model (28) at the equilibrium point Linearize at:

In the formula: is the small offset of the generalized coordinates relative to the equilibrium position; Δτ _c is the control deviation; Easy proof and neglect The linear time-invariant kinetic equation can be obtained in the following state-space form.

According to the above process, the influence of the trailing distance on the static balance control of the handlebar steering is analyzed, as shown in Figure 3, which shows the flow chart of the trailing distance controllability analysis. Within the allowable range of the trailing distance, from the singular value of the controllability matrix, The influence of the trailing distance is analyzed from three aspects of the closed-loop controller's domain of attraction and the control cost.

First, analyze the controllability and construct the controllability matrix Q _c of the linear model (30):

Substituting the system matrices A and B into the above formula can be obtained:

In the formula: express column 2 of the . Singular value decomposition Q _c = U∑V is performed on Q _c to make an image of the minimum singular value with respect to the trailing distance.

Then, the domain of attraction of the closed-loop controller is analyzed. Design the following linear quadratic performance index:

In the formula: and is an undetermined weight coefficient matrix, which can be debugged according to the specific control effect. Solve the Riccati algebraic equation by:

The matrix P can be obtained, and then the handlebar torque linear feedback control law can be obtained as:

Define the maximum value of the handlebar steering torque as τ _{c max} , then the reachable state space of the system satisfies the inequality:

|K| _∞ |x| _∞ ≤||τ _c ||＝||Kx||≤τ _{c max} (36)

In the formula: |·| _∞ means taking the infinite norm of the vector. Therefore, the estimated domain of attraction is:

Make a map of the field of attraction (37) with respect to the drag distance.

Finally, the influence of trailing distance on control cost is analyzed. Define the control cost as:

In the formula: the integral superscript T represents the control termination time. Make a graph of the control cost with respect to the drag distance.

According to the above three images, the appropriate drag distance can be selected according to the specific control requirements, such as high controllability, large attraction field and low control cost.

The formulas used above are supplemented below.

The Jacobian matrix in formula (14) is defined as follows:

In the formula:

In the formula:

The generalized mass matrix in the kinetic equation (25) is defined as follows:

In the formula:

M ₂₃ ＝m ₂ Rx _G2 sinδcos(θ+η)+m ₂ l _r x _G2 sinδsin(θ ₀ -η)-m ₂ x _G2 z _G2 sinδ

M ₃₂ ＝m ₂ Rx _G2 sinδcos(θ+η)+m ₂ l _r x _G2 sinδsin(θ ₀ -η)-m ₂ x _G2 z _G2 sinδ

The damping matrix in the dynamic equation (25) is defined as follows:

In the formula:

V ₂₂ =0

The gravity matrix in the kinetic equation (25) is defined as follows:

In the formula:

According to the two-wheeled robot modeling and static balance method based on the trailing distance proposed by the embodiment of the present invention, firstly, according to the motion constraints, from the perspective of a multi-rigid body system, the two-wheeled robot is equivalent to a multi-link multi-joint with two front and rear car bodies articulated system. Secondly, based on the closed-loop kinematic chain of the system and the contact characteristics between the wheel and the ground, two constraint equations are established to obtain the kinematics model of the system. Then, the dynamic model of the system is derived by using the Lagrangian equation of the first kind. Finally, the influence of trailing distance on static balance is analyzed from three aspects: singular value of system controllability matrix, closed-loop controller attraction region and control energy consumption. It can reflect the nonlinear relationship between the handlebar angle and the height of the center of mass under different trailing distances, and can provide a set of analysis procedures for the selection of trailing distances to improve the control effect of static balance.

Next, the two-wheeled robot modeling and static balancing device based on the trailing distance proposed according to the embodiments of the present invention will be described with reference to the accompanying drawings.

Fig. 4 is a schematic structural diagram of a two-wheeled robot modeling and static balancing device based on trailing distance according to an embodiment of the present invention.

As shown in FIG. 4 , the trail-based two-wheel robot modeling and static balancing device includes: an equivalent module 100 , a constraint module 200 and a modeling analysis module 300 .

Equivalent module 100, configured to set the two-wheel robot system as a multi-link multi-joint system and define multiple coordinate systems in the multi-link multi-joint system when detecting that the two-wheel robot system satisfies preset equivalent setting conditions , calculate the drag range of the two-wheel robot system according to the geometric relationship of multiple coordinate systems.

The constraint module 200 is used to establish two constraint equations according to the closed-loop kinematic chain of the multi-link multi-joint system and the geometric characteristics of the wheels of the two-wheel robot system, and establish a kinematics model according to the two constraint equations.

The modeling analysis module 300 is used to solve the kinematics model by using the first kind of Lagrangian equation to obtain the dynamic model of the two-wheel robot, and to analyze the singular value of the controllability matrix and the closed-loop control according to the dynamic model of the two-wheel robot. The attraction domain and control cost of the controller are analyzed, and the trailing distance that meets the control requirements is determined in the trailing distance range according to the analysis results.

Further, in one embodiment of the present invention, the preset equivalent setting conditions include:

Set the rear body center of mass to only include roll and pitch caused by handlebar steering;

The front and rear wheels are set to be braked, there is no relative movement with the frame, and the rear wheel is purely rolling with the ground;

It is set to ignore the tire thickness and deformation, and treat the front and rear wheels as rigid thin discs of equal size.

Further, in one embodiment of the present invention, the multiple coordinate systems are:

(1) Inertial reference frame {I}, A ₀ xyz: the origin is fixed at A ₀ point, the x-axis points from A ₀ to E ₀ , the z-axis is vertically downward, and the y-axis forms a right-hand system with the x-axis and z-axis; where , A ₀ is the contact point between the rear wheel and the ground when the handlebar is turned, and E ₀ is the contact point between the front wheel and the ground when the handlebar is turned;

(2) Rear wheel coordinate system {B}, Bx _b y _b z _b : the origin is fixed at point B, the x and _b axes are parallel to the x axis of the inertial reference system, and the z and y axes can be rotated around the x axis by the inertial reference system Angle is obtained, then the rotation matrix from {I} to {B} is:

Among them, B is the center of the rear wheel, is the roll angle of the rear body;

(3) Handlebar coordinate system {C}, Cx _c y _c z _c : the origin is fixed at point C, the y _c axis is parallel to the y _b axis of the rear wheel coordinate system, and the x and z axes can be circled by {B} around the y _b axis Obtained by rotating θ+η angle, then the rotation matrix from {B} to {C} is:

Wherein, C is the connection point between the handlebar swivel and the rear frame, θ is the pitch angle of the rear body, and η is the inclination angle of the handlebar;

The handlebar inclination η satisfies the following geometric constraints:

Among them, _θ0 is the pitch angle of the rear frame link vector when the handlebar rotation angle is zero, and ε is the installation angle of the rear frame link;

(4) Front wheel coordinate system {D}, Dx _d y _d z _d : the origin is fixed at point D, the z _d axis is parallel to the z _c axis of the handlebar coordinate system, and the x and y axes can be circled by the handlebar coordinate system z _c The axis is rotated by δ angle, then the rotation matrix from {C} to {D} is:

Among them, D is the center of the front wheel, δ is the handlebar angle;

(5) Rear car body coordinate system {G ₁ }, G ₁ x ₁ y ₁ z ₁ : the origin is fixed at G ₁ , and the rear car body coordinate system is obtained by rotating the rear wheel coordinate system around the y _b axis by an angle θ. Let In η=0, the rotation matrix from {B} system to {G ₁ } system can be obtained

Among them, G ₁ is the center of mass of the rear car body;

(6) Front car body coordinate system {G ₂ }, G ₂ x ₂ y ₂ z ₂ : the origin is fixed at G ₂ , the front car body coordinate system is parallel to the front wheel coordinate system, and G ₂ is the center of mass of the front car body.

Further, in one embodiment of the present invention, the range of the trailing distance of the two-wheel robot system is:

Among them, R is the radius of the wheel, l _r is the length of line segment BC, d is the length of line segment CC′, l _f is the length of line segment C′D of the front frame, λ is the front fork angle of the handlebar, and η is the inclination angle of the handlebar.

Further, in one embodiment of the present invention, two constraint equations are established according to the closed-loop kinematic chain subjected to the multi-link multi-joint system and the wheel geometric characteristics of the two-wheel robot system, including:

Constraint 1, closed-loop kinematic chain constraint

Among them, e _z =[0,0,1] ^T is the z-axis unit direction vector of the inertial reference frame {I}, r ₁ is the rear wheel linkage vector from A to B; r ₂ is the rear wheel linkage vector from B to C The frame link vector; r ₃ is the front frame link vector from C to D; r ₄ is the front wheel link vector from D to E, and the superscript indicates the coordinate system corresponding to this vector, is the rotation matrix from {B} to {I}, is the rotation matrix from {C} to {B}, is the rotation matrix from {D} to {C};

Constraint 2, Wheel Geometry Constraints

Among them, n _y is the unit direction vector of the y _d axis of the front wheel coordinate system {D}.

Further, in one embodiment of the present invention, a kinematics model is established according to two constraint equations, including:

Deriving the constraint conditions of constraint 1 and constraint 2, we get:

Among them, J is the Jacobian matrix; the Jacobian matrix can be based on the generalized coordinate with dependent coordinates Break it down into two parts:

Velocities of dependent coordinates are represented by generalized velocities:

It can be obtained that the kinematic model is:

in, is a second-order identity matrix.

Further, in one embodiment of the present invention, using the first type of Lagrangian equation to solve the dynamic model of the two-wheeled robot includes:

Among them, L=TV is the Lagrange function, T is the total kinetic energy of the two-wheel robot system, V is the total potential energy of the two-wheel robot system, γ is the Lagrangian multiplier, is the generalized non-conservative external force on the two-wheel robot system, D and _d are the disturbance moments on the roll and handlebar steering channel respectively, and τc is the driving torque on the handlebar shaft.

Furthermore, in one embodiment of the present invention, the modeling and analysis module is specifically used to analyze the singular values of the controllability matrix, the attractive domain of the closed-loop controller and the control cost within the range of the trailing distance, and make Corresponding to the image, according to the corresponding image and the control requirement, determine the drag distance that meets the control requirement.

It should be noted that the foregoing explanations for the embodiment of the trail-based two-wheeled robot modeling and static balancing method are also applicable to the device of this embodiment, and will not be repeated here.

According to the two-wheel robot modeling and static balance device based on the trailing distance proposed by the embodiment of the present invention, firstly, according to the motion constraints, from the perspective of a multi-rigid body system, the two-wheel robot is equivalent to a multi-link multi-joint hinged front and rear two car bodies system. Secondly, based on the closed-loop kinematic chain of the system and the contact characteristics between the wheel and the ground, two constraint equations are established to obtain the kinematics model of the system. Then, the dynamic model of the system is derived by using the Lagrangian equation of the first kind. Finally, the influence of trailing distance on static balance is analyzed from three aspects: singular value of system controllability matrix, closed-loop controller attraction region and control energy consumption. It can reflect the nonlinear relationship between the handlebar angle and the height of the center of mass under different trailing distances, and can provide a set of analysis procedures for the selection of trailing distances to improve the control effect of static balance.

In addition, the terms "first" and "second" are used for descriptive purposes only, and cannot be interpreted as indicating or implying relative importance or implicitly specifying the quantity of indicated technical features. Thus, the features defined as "first" and "second" may explicitly or implicitly include at least one of these features. In the description of the present invention, "plurality" means at least two, such as two, three, etc., unless otherwise specifically defined.

In the description of this specification, descriptions referring to the terms "one embodiment", "some embodiments", "example", "specific examples", or "some examples" mean that specific features described in connection with the embodiment or example , structure, material or feature is included in at least one embodiment or example of the present invention. In this specification, the schematic representations of the above terms are not necessarily directed to the same embodiment or example. Furthermore, the described specific features, structures, materials or characteristics may be combined in any suitable manner in any one or more embodiments or examples. In addition, those skilled in the art can combine and combine different embodiments or examples and features of different embodiments or examples described in this specification without conflicting with each other.

Although the embodiments of the present invention have been shown and described above, it can be understood that the above embodiments are exemplary and should not be construed as limiting the present invention, and those skilled in the art can make the above-mentioned The embodiments are subject to changes, modifications, substitutions and variations.

Claims (10)

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 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.

2. The method according to claim 1, 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;

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.

3. The method of claim 1, wherein the plurality of coordinate systems are:

(1) inertial reference system { I }, A₀xyz: the origin is fixed at A₀Point, x axis from A₀Direction E₀The z-axis is vertical downwards, and the y-axis, the x-axis and the z-axis form a right-hand system; wherein A is₀Contact point of rear wheel with ground when handlebar is turned, E₀The contact point between the front wheel and the ground when the handlebar rotates;

(2) rear wheel coordinate system { B }, Bx_by_bz_b: the origin is fixed at point B, x_bThe 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 systemThe angle is found, then the rotation matrix from { I } to { B } is:

wherein B is the circle center of the rear wheel,the roll angle of the rear body;

(3) coordinate system of handlebar { C }, Cx_cy_cz_c: the origin is fixed at point C, y_cAxle and rear wheel coordinate system y_bThe axes being parallel, the x and z axes being surrounded by { B } around y_bThe axis is rotated by an angle θ + η, and the rotation matrix from { B } to { C } is:

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:

wherein, theta₀Is 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 }, Dx_dy_dz_d: the origin is fixed at point D, z_dAxis and handlebar coordinate system z_cThe axes being parallel, the x and y axes being defined by the handlebar coordinate system_cThe axis is rotated by an angle δ, and the rotation matrix from { C } to { D } is:

wherein D is the center of the front wheel circle, and delta is the handlebar turning angle;

(5) rear vehicle body coordinate system { G₁}，G₁x₁y₁z₁: origin fixed to G₁The rear vehicle body coordinate system is wound by the rear wheel coordinate system y_bThe angle of rotation of the shaft is obtained byWhere η is 0, from { B } system to { G-₁Rotation matrix of the system

Wherein G is₁Is the center of mass of the rear vehicle body;

(6) front vehicle body coordinate system { G₂}，G₂x₂y₂z₂: origin fixed to G₂The front body coordinate system is parallel to the front wheel coordinate system, G₂Is the front bodywork centroid.

4. The method of claim 3, wherein the tow-throw range of the two-wheeled robotic system is:

wherein R is the wheel radius, l_rIs the length of segment BC, d is the length of segment CC', l_fThe length of the front frame line segment C' D, λ is the handlebar fork angle, and η is the handlebar inclination angle.

5. The method of claim 4, wherein establishing two constraint equations based on the closed-loop kinematic chain experienced by the multi-link multi-joint system and the wheel geometry of the two-wheeled robotic system comprises:

constraint 1, closed-loop kinematic chain constraint

Wherein e is_z＝[0,0,1]^TIs a z-axis unit direction vector, r, of the inertial reference system { I }₁Is a rear wheel link pointing from A to BVector quantity; r is₂Is the rear frame link vector pointing from B to C; r is₃Is the front frame link vector pointing from C to D; r is₄Is a front wheel connecting rod vector pointed to E by D, the coordinate system corresponding to the vector is indicated by a superscript,for the rotation matrix from { B } to { I },is a rotation matrix from C to B,is a rotation matrix from { D } to { C };

restraint 2, wheel geometry restraint

Wherein n is_yY being the front wheel coordinate system { D }_dAxial unit direction vector.

6. The method of claim 5, wherein said building a kinematic model according to said two constraint equations comprises:

and (3) carrying out derivation on constraint conditions of the constraint 1 and the constraint 2 to obtain:

wherein J is a Jacobian matrix; the Jacobian matrix can be based on generalized coordinatesAnd non-independent coordinatesThe decomposition is carried out in two parts:

the velocity of the non-independent coordinates is represented by the generalized velocity:

the kinematic model can be found as follows:

wherein,is an identity matrix of order 2.

7. The method of claim 5, wherein solving the two-wheeled robot dynamics model using a first class of lagrangian equations comprises:

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,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, tau_cIs the driving torque of the rotating shaft of the handlebar.

8. 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.

9. A tow-distance-based two-wheeled robot modeling and static balancing device is characterized by comprising:

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.

10. The apparatus of claim 9, 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;

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.