A mobile robot servo control method, device and medium

Abstract

The present application relates to the technical field of robot movement control, and particularly relates to a mobile robot servo control method and device and medium. The present application designs a real-time control mode for a mobile robot when passing through a curved channel. According to the real-time calculation of the servo control force based on the boundary and target position of the mobile robot, and in combination with the servo control force and the dynamics equation of the mobile robot, the mobile robot can be safely controlled to pass through the curved channel, and the universality is strong. Compared with the traditional mode, the calculation step is reduced, and the processing capacity of the emergency situation is significantly improved. The present application innovatively maps the boundary constraint information to the unbounded space through differential homeomorphism, and combines the equality constraint force and the inequality constraint force to calculate the servo control force. In the calculation formula of the servo control force, the closer the distance between the mobile robot and the boundary, the greater the strength of the servo control force to control the mobile robot to move away from the boundary. The calculation is simple and safe.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robot movement control, and particularly relates to a mobile robot servo control method and device and medium. BACKGROUND

[0002] Mobile robots play an important role in modern society and are widely used in logistics, warehousing, medical treatment and other fields. In many practical applications, robots need to move on a bounded curved path, such as narrow corridors, curved passages or directional movement in complex environments. The current method for mobile robots on a bounded curved path is usually to plan the path for the mobile robot, such as A* algorithm, Dijkstra algorithm and RRT algorithm. The path planning method usually determines the path in advance before the movement of the mobile robot, and divides the path into a series of discrete points. This way may cause the mobile robot to fail to accurately follow the path or even collide when facing a curved path, and has the disadvantages of high computational complexity, insufficient accuracy and poor adaptability to the environment. Therefore, the method mentioned in the present application starts from the control level, aiming to improve the control accuracy, adaptability and real-time performance to cope with the movement task of the mobile robot in the complex curved path environment. SUMMARY

[0003] The present application discloses a mobile robot servo control method, device and medium, which can calculate the servo control force in real time when the mobile robot passes through the curved passage, and control the mobile robot to pass through the curved passage safely according to the servo control force.

[0004] To achieve the above purpose, on the one hand, the present application provides a mobile robot servo control method, and the specific method is as follows:

[0005] Obtaining the initial position, initial speed, target position of the mobile robot, and boundary information of the bounded curved path in the movement process of the mobile robot;

[0006] Moving arbitrarily in the curved path in any initial state;

[0007] Real-time calculation of the servo control force for controlling the mobile robot according to the distance between the mobile robot and the boundary and the target position, respectively. The closer the mobile robot is to the boundary, the greater the inequality servo constraint force is and the farther the mobile robot is from the boundary.

[0008] The embodiment has the advantages that a real-time control mode of a mobile robot passing through a curved channel is designed, the path of the mobile robot moving in the boundary is controlled from the control level, the inequality servo control force can be calculated in real time according to the distance of any mobile robot from the boundary, the servo control force and the kinematics equation of the mobile robot are combined, the mobile robot can be controlled to pass through the curved channel safely, and the method has the characteristics of high precision, high adaptability and high real-time performance. Compared with the traditional mode, the passing path does not need to be planned in advance, the calculation steps in advance are reduced, the connection is smoother through the speed, and the processing capacity of the emergency situation is significantly improved.

[0009] Further, the specific calculation mode of the servo control force is:

[0010] Based on the working environment of the mobile robot, a motion equation of the mobile robot under no constraint is established;

[0011] The initial state and the target position of the mobile robot are obtained, and the boundary constraint information obtained from the boundary information is obtained;

[0012] According to the target position of the mobile robot, it is written as a second-order equality constraint form;

[0013] Based on the Udwadia-Kalaba equation, the equality constraint force is determined;

[0014] The boundary constraint information is mapped to the unbounded space by using the differential homeomorphism;

[0015] The control vector is determined according to the second-order equality constraint and the state variable of the unbounded space;

[0016] According to the control vector, the inequality constraint force is determined in the orthogonal space of the constraint coefficient matrix;

[0017] According to the equality constraint force and the inequality constraint force, the servo control force of the mobile robot is calculated.

[0018] The embodiment has the advantages that the path of the mobile robot moving in the boundary is controlled from the control level, the boundary constraint information is mapped to the unbounded space, the servo control force can be calculated by combining the equality constraint force and the inequality constraint force, in the calculation formula of the servo control force, the closer the distance between the mobile robot and the boundary, the greater the strength of the servo control force for controlling the mobile robot to move away from the boundary, and the calculation is simple and safe.

[0019] Specifically, the motion equation of the mobile robot under no constraint is

[0020]

[0021] Wherein, the independent variable t is time, q is a generalized position, is the generalized velocity, is the generalized acceleration, M(q, t) is the inertia matrix of the mobile robot, may be expressed as

[0022]

[0023] wherein, is the Coriolis matrix, is other disturbance forces;

[0024] The initial state includes an initial position q0= [x0 y0] T , an initial velocity The target position is x goal , and the boundary constraint information is: wherein g(q, t) is a constraint function, b and are constants, representing the lower bound and upper bound of the constraint equation, respectively;

[0025] According to the target position constraint x = x goal , the second-order equality constraint is expressed as:

[0026]

[0027] wherein a > 0 and β > 0 are constants;

[0028] The second-order equality constraint is expressed as an equality constraint matrix:

[0029]

[0030] Based on the UK equation, the motion equation of the constraint system is:

[0031]

[0032] The equality constraint force is determined as

[0033]

[0034] wherein "+" represents the generalized inverse of the matrix;

[0035] The boundary constraint is mapped to an unbounded space by using a differential homeomorphism, and can be specifically expressed as

[0036]

[0037] wherein is an unbounded space state variable, φ(q) is a continuously differentiable mapping function, and satisfies q = φ -1 (η), and can be specifically expressed as

[0038]

[0039] At this time, when g(q, t)→b, η→-∞; when , η→+∞;

[0040] The control vector is determined, which can be expressed as:

[0041]

[0042] Wherein, λ1>0 and λ2>0 are constants; in addition

[0043]

[0044]

[0045] Wherein,

[0046] The inequality constraint force is determined in the orthogonal space of the constraint coefficient matrix, which can be expressed as

[0047]

[0048] According to the equality constraint force and the inequality constraint force, the servo control force is calculated as:

[0049]

[0050] Optionally, the mobile robot is a two-wheel differential mobile robot, and the specific method for controlling the mobile robot according to the servo control force is:

[0051] According to the servo control force, the dynamics equation of the two-wheel differential mobile robot can be obtained

[0052]

[0053] Wherein, γ=[xθ] T is a state variable, x is the forward displacement of the two-wheel differential mobile robot, and θ is the rotation angle of the two-wheel differential mobile robot; is the servo control force, M(γ, t) is the inertia matrix, and the specific expression is

[0054]

[0055] Wherein, m is the total mass of the whole vehicle body, r is the radius of the driving wheel, 2b is the axial distance of the two driving wheels, I w is the inertia moment of the driving wheel around the wheel axis, I is the inertia moment set of the vehicle body and the driving wheel around the vertical axis through the center of mass, which can be expressed as:

[0056] I=I c +2I m +mc a 2 +2m w b 2

[0057] Where, m c For the mass of the vehicle body, m w For the mass of the drive wheel, I c Let I be the moment of inertia of the car body about the vertical axis passing through its center of mass. m Let be the moment of inertia of the driving wheel about the vertical axis around the center of mass, and let a be the distance from the center of mass to the center of the axes of the two driving wheels;

[0058] It is a combination of gravity and Coriolis force. Furthermore, for a robot moving in a planar motion, the gravity term is 0, i.e.

[0059]

[0060] in, The Coriolis force matrix can be specifically written as:

[0061]

[0062] Optionally, the mobile robot is a four-wheel steering mobile robot, and the specific method for controlling the mobile robot based on servo control force is as follows:

[0063] The dynamic equations of a four-wheel steering mobile robot can be obtained based on the servo control force.

[0064]

[0065] in, Let y be the forward displacement of the two-wheeled differential speed robot, and ψ be the turning angle of the two-wheeled differential speed robot. For servo control force, The inertia matrix is ​​specifically...

[0066]

[0067] Where m is the vehicle mass and I is the vehicle's moment of inertia.

[0068] It is a combination of gravity and Coriolis force. Furthermore, for a robot moving in a planar motion, the gravity term is 0, i.e.

[0069]

[0070] in, The Coriolis force matrix can be specifically written as:

[0071]

[0072] Among them, Cf The sum of the lateral stiffness of the two front tires, and C r The sum of the lateral stiffness of the two rear tires, v is the vehicle's forward speed, and l is the sum of the lateral stiffness of the two rear tires. f and l r These are the distances between the vehicle's center of gravity and the front and rear axles, respectively.

[0073] To achieve the above objectives, the present invention also provides a mobile robot servo control device, characterized in that it includes: a mobile robot information acquisition module, an environmental information acquisition module, and a servo control force calculation module;

[0074] The mobile robot information acquisition module is used to acquire the initial position, initial speed, position during movement, and speed during movement of the mobile robot;

[0075] The environmental information acquisition module is used to acquire the target position of the mobile robot and the boundary information of the bounded curved path during the movement of the mobile robot.

[0076] The servo control force calculation module calculates the servo control force for controlling the mobile robot in real time based on the distances of the mobile robot to the boundary and the target position, respectively, when the robot moves towards the finish line. The closer the mobile robot is to the boundary, the greater the inequality servo constraint force and the further the mobile robot moves away from the boundary.

[0077] Furthermore, it also includes a mobile robot control module, which controls the movement of the mobile robot based on servo control force.

[0078] To achieve the above objectives, the present invention also provides a storage medium storing a plurality of instructions suitable for loading by a processor to execute the above-described mobile robot servo control method.

[0079] Other advantages, objectives, and features of the invention will be set forth in part in the description which follows, and in part will be apparent to those skilled in the art from the following examination, or may be learned from practice of the invention. The objectives and other advantages of the invention can be realized and obtained from the following description and claims. Attached Figure Description

[0080] The accompanying drawings of this invention are described below.

[0081] Figure 1 This is a schematic diagram of the design process of the servo control method of the present invention.

[0082] Figure 2 This is a schematic diagram of a mobile robot moving within a bounded curved path.

[0083] Figure 3 Trajectory plot for the motion of the mobile robot in the narrow curved passage in Example 1.

[0084] Figure 4 Forward direction displacement and error plot for the motion of the mobile robot in the narrow curved passage in Example 1.

[0085] Figure 5 Longitudinal direction displacement change plot for the motion of the mobile robot in the narrow curved passage in Example 1.

[0086] Figure 6 Controlled force and component force change plot for the motion of the mobile robot in the narrow curved passage in Example 1. DETAILED DESCRIPTION

[0087] The present application will be further described below in conjunction with the accompanying drawings and examples.

[0088] Example 1:

[0089] A mobile robot servo control method, as shown in Figure 1 , taking the example of movement constrained by given narrow curved passage boundaries, the movement schematic diagram thereof is as shown in Figure 2 , and the specific steps are as follows:

[0090] S1, based on the working environment in which the mobile robot is located, a motion equation under no constraint is established

[0091]

[0092] wherein the independent variable t is time, q∈R 2 is the generalized position, is the generalized velocity, is the generalized acceleration, and M is the mass of the mobile robot, which can be expressed as

[0093]

[0094] In the formula, represents the friction force, and further has

[0095]

[0096] wherein f v is the friction coefficient

[0097] wherein the specific values are shown in the following table

[0098] Parameter Value Unit M 3.0 kg f v ]]> 0.15 -

[0099] S2, obtain the initial state and target position of the mobile robot, and the narrow curved channel constraint equation, wherein the initial state includes the initial position q0=[x0 y0] T , the initial velocity , the target position x goal , and the narrow curved channel constraint equation in this case can be described as

[0100] wherein g(q, t) = y-xcos(x) is a constraint function, b and are constants, representing the lower bound and upper bound of the constraint function, respectively.

[0101] The above parameters take the following values

[0102]

[0103] S3, optimize the second-order constraint form of the target position and write it as an equality constraint matrix form. According to the target position constraint x=x goal , the optimization can be expressed as

[0104]

[0105] wherein α>0 and β>0 are constants. Specifically, take α=12 and β=15.

[0106] Further, write it as an equality constraint matrix form, that is,

[0107]

[0108] wherein

[0109] S4, according to the second-order standard constraint form, determine the equality constraint force through the Udwadia-Kalaba constraint system motion equation, that is,

[0110]

[0111] The equality constraint force is

[0112]

[0113] wherein "+" represents the generalized inverse of the matrix.

[0114] S5, map the boundary constraint space to an unbounded space using a differential homeomorphism, which can be specifically expressed as

[0115]

[0116] wherein is the state variable of the unbounded space, φ(q) is a continuously differentiable mapping function, and satisfies q=φ-1 (η), which can be expressed as

[0117]

[0118] Specifically, and when y-xcos(x)→-1, η→-∞; when y-xcos(x)→1, η→+∞.

[0119] S6, the control vector is determined according to the second-order equation constraint of step two and the new state variable in the unbounded space of step five, which can be expressed as

[0120]

[0121] Wherein, λ1>0 and λ2>0 are constants, wherein λ1=20, λ2=50. In addition

[0122]

[0123]

[0124] Wherein,

[0125]

[0126] σ2=[0J2]

[0127] Wherein,

[0128]

[0129]

[0130]

[0131]

[0132]

[0133] Further, the inequality constraint force is determined in the orthogonal space of the constraint coefficient matrix, which can be expressed as

[0134]

[0135] S7, combined with step four and step six, the final servo control force of the mobile robot in the narrow curved channel is obtained, that is

[0136]

[0137] The simulation analysis of the mobile robot in the narrow curved channel is carried out, such as Figure 2The mobile robot moves in the narrow curved channel from the initial position q0to the target position x goal In order to better reflect the effective processing of the control method (CBC) to the narrow curved channel constraint, while not affecting the equality constraint of the target position, the traditional Udwadia-Kalaba method (NCBC) is compared and the results are analyzed. The given mobile robot initial position is

[0138]

[0139] The target position is

[0140] x goal = 5

[0141] In order to distinguish the news of the control method, the initial speed is

[0142]

[0143] The narrow curved channel constraint function is

[0144] xcos(x)-1<y<xcos(x)+1

[0145] The simulation results are shown in Figures 3 to 6 . Among them, Figure 3 is the moving trajectory of the mobile robot in the narrow curved channel, the horizontal coordinate is the forward X direction, and the vertical coordinate is the numerical Y direction. From the figure, it can be seen that the NCBC method can reach the final target position x goal = 5, but the motion process cannot meet the narrow curved channel constraint; on the contrary, the CBC method not only meets the requirement of reaching the final target position x goal = 5, but also can stably run in the narrow curved channel even under the premise of offset speed.

[0146] Figure 4 is the forward X direction displacement and error diagram of the mobile robot in the narrow curved channel. It can be seen that in the forward X direction, the NCBC control method and the CBC control method are basically consistent, and the final error can guarantee the final error e x < 4 × 10 -5 , that is, it reflects that the CBC control method meets the narrow curved channel constraint while not affecting the equality constraint of the target position.

[0147] Figure 5is the longitudinal displacement variation diagram of the mobile robot in the narrow curved channel. It can be seen that in the longitudinal Y direction, the NCBC control method has no any constraint limitation, and cannot meet the narrow curved channel constraint; while the CBC method can effectively control the longitudinal Y direction displacement within the narrow curved channel constraint, thereby avoiding collision with the channel, and effectively controlling the mobile robot in the longitudinal Y direction.

[0148] Figure 6 is the control force and component force variation diagram of the mobile robot in the narrow curved channel. It can be seen from the diagram that in the forward X direction, the control force τ x of the NCBC control method and the CBC control method is basically consistent, which shows that both methods can guarantee the final arrival at the target position x goal = 5; in the longitudinal Y direction, the control force τ z of the NCBC control method is 0, that is, the NCBC control method cannot meet the narrow curved channel constraint of the mobile robot, and will produce collision with the channel, while the control force τ z of the CBC control method changes with the movement of the mobile robot in the narrow curved channel, that is, the control force meeting the narrow curved channel constraint can be calculated.

[0149] Embodiment 2:

[0150] The mobile robot is a two-wheel differential mobile robot, and the specific method for controlling the mobile robot according to the servo control force is as follows:

[0151] According to the servo control force, the dynamics equation of the two-wheel differential mobile robot can be obtained

[0152]

[0153] wherein, γ = [x θ] T is a state variable, x is the forward displacement of the two-wheel differential mobile robot, and θ is the turning angle of the two-wheel differential mobile robot; is the servo control force, M(γ, t) ∈ R 2×2 is the inertia matrix, and the specific expression is

[0154]

[0155] wherein, m is the total mass of the entire vehicle body, r is the radius of the driving wheel, 2b is the axial distance between the two driving wheels, I w is the inertia moment of the driving wheel around the wheel axis, and I is the inertia moment set of the vehicle body and the driving wheel around the vertical axis through the center of mass, which can be expressed as:

[0156] I = I c + 2I m + m c a2 + 2m w b 2

[0157] wherein, m c is the mass of the vehicle body, m w is the mass of the driving wheels, I c is the moment of inertia of the vehicle body about the vertical axis through the center of mass, I m is the moment of inertia of the driving wheels about the vertical axis through the center of mass, a is the distance from the center of mass to the center of the two driving wheel axes;

[0158] is the sum of the gravitational and Coriolis forces, and for a planar moving robot the gravitational term is zero, i.e.

[0159]

[0160] wherein, is the Coriolis force matrix, which can be written in detail as

[0161]

[0162] Embodiment 3:

[0163] The mobile robot is a four-wheel steering mobile robot, and the specific method for controlling the mobile robot according to the servo control force is:

[0164] According to the servo control force, the dynamic equation of the four-wheel steering mobile robot can be obtained

[0165]

[0166] wherein, is the state variable, y is the forward displacement of the two-wheel differential mobile robot, ψ is the turning angle of the two-wheel differential mobile robot, is the servo control force, is the inertia matrix, which is specifically

[0167]

[0168] wherein, m is the mass of the vehicle, I is the rotational inertia of the vehicle

[0169] is the sum of the gravitational and Coriolis forces, and for a planar moving robot the gravitational term is zero, i.e.

[0170]

[0171] wherein, is the Coriolis force matrix, which can be written in detail as

[0172]

[0173] wherein C f is the sum of the cornering stiffness of the two front tires, and C r is the sum of the cornering stiffness of the two rear tires, v is the forward speed of the vehicle, l f and l r are the distances from the center of mass of the vehicle to the front and rear axles, respectively.

[0174] According to embodiments 2 and 3, the mobile robot can be any existing mobile robot of the same type or with a simplified dynamics equation as the above examples.

[0175] Those skilled in the art will understand that embodiments of the present application can be provided as methods, systems, or computer program products. Thus, the present application can take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present application can take the form of a computer program product on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROMs, optical storage devices, etc.) embodying computer-readable program code.

[0176] The present application is described in reference to the flowchart and / or block diagrams of the methods, apparatus (systems) and computer program products according to embodiments of the present application. It is understood that each flow and / or block in the flowchart and / or block diagrams, and combinations of flows and / or blocks in the flowchart and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart and / or block diagrams block or blocks. Figure 1 The flowchart and / or block diagrams can include one or more flows and / or blocks that can represent a computer program instructions, or a portion of a computer program instructions. Figure 1 The flowchart and / or block diagrams can include one or more flows and / or blocks that can represent a computer program instructions, or a portion of a computer program instructions.

[0177] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instructions means that implement the function specified in the flowchart and / or block diagrams block or blocks. Figure 1 The flowchart and / or block diagrams can include one or more flows and / or blocks that can represent a computer program instructions, or a portion of a computer program instructions. Figure 1 The flowchart and / or block diagrams can include one or more flows and / or blocks that can represent a computer program instructions, or a portion of a computer program instructions.

[0178] These computer program instructions can also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process such that the instructions executed on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart and / or block diagrams block or blocks. Figure 1 The flowchart and / or block diagrams can include one or more flows and / or blocks that can represent a computer program instructions, or a portion of a computer program instructions.Figure 1 the steps of the functions specified in the one or more blocks.

[0179] It should be noted that the above-mentioned embodiments are only used to illustrate the technical solutions of the present application, but not to limit the present application. Although the present application has been described in detail with reference to the above-mentioned embodiments, those skilled in the art should understand that the specific embodiments of the present application can be modified or equivalent replacements without departing from the spirit and scope of the present application, and any modifications or equivalent replacements should be covered in the protection scope of the claims of the present application.

Claims

1. A mobile robot servo control method characterized by comprising: The specific method is: Obtaining the initial position, initial speed, target position of the mobile robot, and boundary information of the bounded curved path in the movement process of the mobile robot; Moving arbitrarily in the curved path in any initial state; Real-time calculating a servo control force for controlling the mobile robot according to the distance between the mobile robot and the boundary and the target position respectively; the closer the mobile robot is to the boundary, the greater the inequality servo constraint force is and the farther the mobile robot is from the boundary; The specific calculation method of the servo control force is: Based on the working environment of the mobile robot, establishing a motion equation of the mobile robot under no constraint; Obtaining the initial state and target position of the mobile robot, and boundary constraint information obtained from the boundary information; According to the target position of the mobile robot, writing it into a second-order equality constraint form; Based on the Udwadia-Kalaba equation, determining an equality constraint force; Mapping the boundary constraint information to an unbounded space by using a differential homeomorphism; Determining a control vector according to the state variables of the second-order equality constraint and the unbounded space; Determining an inequality constraint force in the orthogonal space of a constraint coefficient matrix according to the control vector; Calculating the servo control force of the mobile robot according to the equality constraint force and the inequality constraint force; The motion equation of the mobile robot under no constraint is where the independent variable t is time, q is the generalized position, is the generalized velocity, is the generalized acceleration, M(q, t) is the inertia matrix of the mobile robot, may be expressed as wherein is the Coriolis matrix, is other disturbance forces; The initial state includes an initial position q0=[x0 y0] T , an initial velocity , a target position x goal , and boundary constraint information: In the formula, g(q, t) is a constraint function, b and are constants, representing the lower bound and upper bound of the constraint equation respectively; According to the position constraint of the finish line x = x goal , the second-order equality constraint is expressed as: Wherein, α>0 and β>0 are constants; The second-order equality constraint is expressed as an equality constraint matrix: Based on the UK equation constraint system motion equation: The equality constraint force is determined as Wherein, "+" represents the generalized inverse of the matrix; The boundary constraint is mapped to an unbounded space by using a differential homeomorphism, which can be specifically represented as wherein is a state variable in unbounded space, φ(q) is a continuously differentiable mapping function, and satisfies q = φ -1 (η), which can be specifically represented as At this time, when g(q, t)→ b η→ -∞; when η→ +∞; The control vector is determined, which can be represented as: Wherein, λ1>0 and λ2>0 are constants; in addition wherein, The inequality constraint force in the orthogonal space of the constraint coefficient matrix is determined, which can be represented as The servo control force is calculated as:

2. The mobile robot servo control method of claim 1, wherein, The mobile robot is a two-wheel differential mobile robot, and the specific method for controlling the mobile robot according to the servo control force is: The dynamics equation of the two-wheel differential mobile robot can be obtained according to the servo control force where γ = [x θ] T is a state variable, x is the forward displacement of the two-wheel differential mobile robot, and θ is the turning angle of the two-wheel differential mobile robot; is a servo control force, and M(γ, t) is an inertia matrix, and the specific expression is where m is the total mass of the entire vehicle body, r is the radius of the drive wheels, 2b is the axial distance between the two drive wheels, I w is the moment of inertia of the drive wheels about the wheel axis, and I is the set of moments of inertia of the vehicle body and drive wheels about the vertical axis through the center of mass, which can be expressed as: I = I c + 2I m + m c a 2 + 2m w b 2 where m c is the mass of the vehicle body, m w is the mass of the drive wheels, I c is the moment of inertia of the vehicle body about the vertical axis through the center of mass, I m is the moment of inertia of the drive wheels about the vertical axis through the center of mass, and a is the distance from the center of mass to the center of the two drive wheel axes. is the sum of the gravity and the Coriolis force, and additionally the gravity term is zero for planar motion of the robot, i.e. wherein, is the Coriolis matrix, which can be written in detail as 3. The mobile robot servo control method of claim 1, wherein, The mobile robot is a four-wheel steering mobile robot, and the specific method for controlling the mobile robot according to the servo control force is: The dynamics equation of the four-wheel steering mobile robot can be obtained according to the servo control force wherein, is a state variable, y is the forward displacement of the two-wheel differential mobile robot, ψ is the turning angle of the two-wheel differential mobile robot, is a servo control force, is an inertia matrix, specifically Wherein, m is the vehicle mass, and I is the vehicle rotational inertia is the sum of the gravity and the Coriolis force, and additionally the gravity term is zero for planar motion of the robot, i.e. wherein, is the Coriolis matrix, which can be written in detail as where C f is the sum of the cornering stiffness of the two front tires, and C r is the sum of the cornering stiffness of the two rear tires, v is the forward speed of the vehicle, l f and l r are the distances from the center of mass of the vehicle to the front and rear axles, respectively.

4. A mobile robot servo control device characterized by comprising: Comprise: A mobile robot information acquisition module, an environment information acquisition module, and a servo control force calculation module; The mobile robot information acquisition module is used to obtain the initial position, initial speed, position during movement, and speed during movement of the mobile robot; The environment information acquisition module is used to obtain the target position of the mobile robot and the boundary information of the bounded curved path in the movement process of the mobile robot; The servo control force calculation module calculates a servo control force for controlling the mobile robot in real time according to distances of the mobile robot from the boundary and the target position when the robot moves towards the terminal line; the closer the mobile robot is to the boundary, the greater the inequality servo constraint force is and the farther the mobile robot is from the boundary; The servo control force is specifically calculated as follows: Based on the working environment of the mobile robot, a motion equation of the mobile robot under no constraint is established; Initial state and target position of the mobile robot are acquired, as well as boundary constraint information obtained from boundary information; According to the target position of the mobile robot, the target position is written as a second-order equality constraint form; Based on the Udwadia-Kalaba equation, an equality constraint force is determined; The boundary constraint information is mapped to an unbounded space by using a differential isomorphism; A control vector is determined according to the second-order equality constraint and state variables of the unbounded space; An inequality constraint force is determined in an orthogonal space of a constraint coefficient matrix according to the control vector; The servo control force of the mobile robot is calculated according to the equality constraint force and the inequality constraint force; The motion equation of the mobile robot under no constraint is where the independent variable t is time, q is the generalized position, is the generalized velocity, is the generalized acceleration, M(q, t) is the inertia matrix of the mobile robot, may be expressed as wherein is the Coriolis matrix, is other disturbance forces; The initial state includes an initial position q0 = [x0 y0] T , an initial velocity The target position is x goal , and the boundary constraint information is: In the formula, g(q, t) is a constraint function, b and are constants, representing the lower bound and upper bound of the constraint equation, respectively; According to the position constraint of the finish line x = x goal , the second-order equality constraint is expressed as: Wherein, α>0 and β>0 are constants; The second-order equality constraint is expressed as an equality constraint matrix as follows: Based on the UK equation, a constraint system motion equation is determined as follows: The equality constraint force is determined as follows Wherein, "+" represents a generalized inverse of a matrix; The boundary constraint is mapped to an unbounded space by using a differential isomorphism, which can be specifically expressed as follows wherein is a state variable in unbounded space, φ(q) is a continuously differentiable mapping function, and satisfies q = φ -1 (η), which can be specifically represented as At this time, when g(q, t)→ b η→ -∞; when η→ +∞; The control vector is determined as follows Wherein, λ1>0 and λ2>0 are constants; in addition wherein, The inequality constraint force is determined in an orthogonal space of a constraint coefficient matrix as follows The servo control force is calculated as follows 5. The mobile robot servo control device according to claim 4, wherein The mobile robot control module controls the mobile robot to move according to the servo control force.

6. A storage medium, characterized by The storage medium stores a plurality of instructions, and the instructions are suitable for being loaded by the processor to execute the method in any one of claims 1 to 3. The storage medium stores a plurality of instructions, and the instructions are suitable for being loaded by the processor to execute the method in any one of claims 1 to 3.

Images