High-precision differential vehicle recharge function control method

Abstract

This invention provides a high-precision differential vehicle recharging function control method. First, the coordinates of the destination in the world coordinate system are identified. Then, the identified coordinates are filtered to eliminate errors. Finally, based on the real-time target position, the linear velocity v and angular velocity ω that the robot should send to the underlying control are calculated until the vehicle successfully reaches the destination. This invention proposes a novel motion control method that can compensate for the inaccurate navigation alignment caused by insufficient global positioning, thereby reducing the accuracy requirements of the sensors used for global positioning. It combines a simple motion method, high motion precision, and low spatial requirements.

Description

Technical Field

[0001] This invention relates to the field of motion control for mobile robots, specifically a high-precision differential vehicle recharge function control method. Background Technology

[0002] In recent years, mobile robots and autonomous driving technologies have emerged and made significant progress. Among the wheeled mobile robots or hybrid robots sold on the market, they are mainly classified into three types based on their wheel drive power source: differential, Ackermann, and omnidirectional. The differential model, due to its simple drive mechanism and flexible movement, is widely used in various commercial scenarios, such as unmanned warehousing, medical wheelchairs, 3C products, and indoor and outdoor cleaning.

[0003] The fundamental technologies involved in mobile robots include sensor technology, information processing technology, automated control technology, and navigation technology. The navigation control module, in particular, needs to adjust its control methods and accuracy requirements according to different usage methods, different ground surfaces, and different obstacle conditions. The automatic recharging function, as a crucial part of the product's functional modules, places extremely high demands on its control methods and accuracy; the stationary motion control algorithm within it must minimize the movement path while maintaining sufficient accuracy.

[0004] The common two types of recharge and docking systems for dual-wheel differential mobile robots are as follows:

[0005] 1. When the global positioning accuracy is high enough, a general navigation local planning algorithm can be used directly to achieve staking, which requires very high global accuracy. However, when the global positioning accuracy is not high, general navigation control algorithms will result in insufficient staking accuracy due to positioning fluctuations, or repeated attempts at the endpoint will lead to an impractical process.

[0006] 2. Real-time positioning is achieved using multiple sets of infrared sensors, thereby controlling the robot to accurately locate the charging station. However, this method places extremely high demands on the stability of the sensors. Summary of the Invention

[0007] In order to solve the problems of the prior art, the present invention provides a method that enables precise control of piles even when the accuracy of sensors is not very high.

[0008] The present invention employs the following control device: a QR code or a fixed geometric shape such as a V-shaped groove is deployed on the charging station location. At the same time, the robot is equipped with corresponding sensing sensors such as cameras and QR codes, and the location information of the QR code or geometric shape is obtained in real time through the camera or lidar sensor.

[0009] This invention includes the following steps:

[0010] 1) Identification: Identify the coordinates of the endpoint in the world coordinate system;

[0011] 2) Filtering: The coordinates identified in step 1) are filtered to eliminate errors;

[0012] The filtering process, referring to the extended Kalman filter, specifically includes the following steps:

[0013] 2.1) Establish the prior state vector for the k-th iteration. The input vector u in the k-th iteration k State transition matrix F k Input transition matrix B k And the observation transition matrix H:

[0014] u k =[v ω] T

[0015]

[0016]

[0017] H = I3

[0018] 2.2) Give the covariance matrix Q of the process noise. k The covariance matrix R of observation noise k :

[0019]

[0020] 2.3) Update the next predicted value based on the state transition relationship:

[0021]

[0022] P k =F k P k-1 F kT +Q k

[0023] in, Let P be the posterior state vector of the (k+1)th iteration. k Let be the posterior estimated covariance matrix at time k;

[0024] 2.4) Calculate the Kalman gain K based on observations. k :

[0025]

[0026] 2.5) Apply the observed values ​​to correct the predicted values, and use the state correction values ​​as the output of the filter and as the input for the next iteration:

[0027]

[0028] P k' =(IK k H)P k

[0029] in, Let z be the posterior state vector of the (k+1)th iteration. k Let P be the observation vector for the k-th iteration, given by the sensor. k' Let be the posterior estimated covariance matrix at time k.

[0030] 3) Motion control: Based on the real-time target position, calculate the linear velocity v and angular velocity ω that the robot should send to the lower-level control until the robot successfully reaches the destination. The specific method is as follows:

[0031] 3.1) Calculate the following distance based on the lateral error of the robot's distance from the target along the line;

[0032] l d =|y rg |+l control

[0033] Among them, I d Forward sight distance, y rg Let l be the y-coordinate of the target point in the world coordinate system. control To adjust the parameters, adjust l control To control the effect of following;

[0034] 3.2) Calculate the orientation δ of the line connecting the robot's center point and the tracking point in the world coordinate system based on the following distance;

[0035]

[0036] 3.3) Based on δ and the robot's orientation θ in the world coordinate system wr Calculate the angle β between the line connecting the robot's center point and the tracking point and the robot's orientation;

[0037] β=δ+θ wr

[0038] 3.4) Calculate the velocity orientation of the vehicle's control point based on β, so that after the control point moves in this direction for a period of time, the robot's center zone just passes through the pre-aiming point, that is, the exact center point at the front or rear of the robot.

[0039]

[0040] Where θ represents the orientation of the robot control point's velocity in the robot coordinate system, l is the distance of the control point from the center, v is the velocity of the vehicle's center point in the world coordinate system, and k control To control the intensity;

[0041] 3.5) After obtaining θ, calculate the angular velocity ω and radius of rotation R of the vehicle based on geometric relationships, and then calculate the velocity v at the center point:

[0042]

[0043]

[0044] v=ωR

[0045] Among them, v c This is the velocity of the control point in the world coordinate system.

[0046] The beneficial effects of this invention are as follows:

[0047] 1. This invention proposes a novel motion control method that can compensate for the inaccuracy of navigation alignment caused by insufficient global positioning, thereby reducing the accuracy requirements of the sensors used for global positioning.

[0048] 2. Based on this method, the correct direction of travel can be maintained even when features are not visible.

[0049] 3. The motion control of this method is compatible with both forward and backward motion modes.

[0050] 4. This method combines the simplicity of motion mode, high precision of motion, and low space requirements of motion. Attached Figure Description

[0051] To more clearly illustrate the technical solutions in the embodiments of the present invention, the accompanying drawings used in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0052] Figure 1 A simplified diagram of the recharge function control system;

[0053] Figure 2 Control flowchart for the recharge function control system;

[0054] Figure 3 Here is the filtering flowchart;

[0055] Figure 4(a) is a schematic diagram of the orientation δ of the line connecting the robot's center point and the tracking point in the world coordinate system, calculated based on the following distance;

[0056] Figure 4(b) shows the relationship between δ and the robot's orientation θ in the world coordinate system. wr A schematic diagram showing the angle β between the line connecting the robot's center point and the tracking point and the robot's orientation;

[0057] Figure 4(c) is a schematic diagram of the velocity orientation of the vehicle's control point calculated based on β. Detailed Implementation

[0058] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0059] The present invention employs the following control device, such as... Figure 1 As shown, a QR code or a fixed geometric shape such as a V-shaped groove is deployed at the charging station location. At the same time, the robot is equipped with corresponding sensing sensors such as cameras and QR codes, and the location information of the QR code or geometric shape is obtained in real time through the camera or LiDAR sensor.

[0060] The control flowchart of the recharge function control system provided by this invention is as follows: Figure 2 As shown, it is mainly divided into three parts:

[0061] 1) Identification: Identify the coordinates of the endpoint in the world coordinate system;

[0062] 2) Filtering: The coordinates identified in step 1) are filtered to eliminate errors;

[0063] The filtering process, referring to the extended Kalman filter, specifically includes the following steps:

[0064] 2.1) Establish the prior state vector for the k-th iteration. The input vector u in the k-th iteration k State transition matrix F k Input transition matrix B k And the observation transition matrix H:

[0065] u k =[v ω] T

[0066]

[0067]

[0068] H = I3

[0069] 2.2) Give the covariance matrix Q of the process noise. k The covariance matrix R of observation noise k :

[0070]

[0071] 2.3) Update the next predicted value based on the state transition relationship:

[0072]

[0073] P k =F k P k-1 F kT +Q k

[0074] in, Let P be the posterior state vector of the (k+1)th iteration. k Let be the posterior estimated covariance matrix at time k;

[0075] 2.4) Calculate the Kalman gain K based on observations. k :

[0076]

[0077] 2.5) Apply the observed values ​​to correct the predicted values, and use the state correction values ​​as the output of the filter and as the input for the next iteration:

[0078]

[0079] P k' =(IK k H)P k

[0080] in, Let z be the posterior state vector of the (k+1)th iteration. k Let P be the observation vector for the k-th iteration, given by the sensor. k' Let be the posterior estimated covariance matrix at time k.

[0081] 3) Motion control: Based on the real-time target position, calculate the linear velocity v and angular velocity ω that the robot should send to the lower-level control until the robot successfully reaches the destination. The specific method is as follows:

[0082] 3.1) Calculate the following distance based on the lateral error of the robot's distance from the target along the line;

[0083] l d =|y rg |+l control

[0084] Among them, l d Forward sight distance, y rg Let l be the y-coordinate of the target point in the world coordinate system. control To adjust the parameters, adjust l control To control the effect of following;

[0085] 3.2) Calculate the orientation δ of the line connecting the robot's center point and the tracking point in the world coordinate system based on the following distance, as shown in Figure 4(a);

[0086]

[0087] 3.3) Based on δ and the robot's orientation θ in the world coordinate system wr Calculate the angle β between the line connecting the robot's center point and the tracking point and the robot's orientation, as shown in Figure 4(b);

[0088] β=δ+θ wr

[0089] 3.4) Calculate the speed and direction of the vehicle's control point based on β, so that after the control point moves in this direction for a period of time, the robot's center zone just passes through the pre-aiming point, as shown in Figure 4(c), which is the exact center point of the robot's foremost or rearmost position (in the example, both are forward movement modes).

[0090]

[0091] Where θ represents the orientation of the robot control point's velocity in the robot coordinate system, l is the distance of the control point from the center, v is the velocity of the vehicle's center point in the world coordinate system, and k control To control the intensity;

[0092] 3.5) After obtaining θ, calculate the angular velocity ω and radius of rotation R of the vehicle based on geometric relationships, and then calculate the velocity v at the center point:

[0093]

[0094]

[0095] v=ωR

[0096] Among them, v c This is the velocity of the control point in the world coordinate system.

[0097] This invention follows the following notation conventions:

[0098] x wg The x-coordinate of the target point in the world coordinate system

[0099] y wg : The y-coordinate of the target point in the world coordinate system

[0100] θ wg The orientation of the target point in the world coordinate system

[0101] x wr The robot's x-coordinate in the world coordinate system

[0102] y wr The robot's y-coordinate in the world coordinate system

[0103] θ wr The robot's orientation in the world coordinate system

[0104] x rg The x-coordinate of the target point in the world coordinate system

[0105] y rg : The y-coordinate of the target point in the world coordinate system

[0106] θ rg Orientation of the target point in the world coordinate system

[0107] β: The angle between the line connecting the robot's center point and the tracking point and the robot's orientation.

[0108] δ: Orientation of the line connecting the robot's center point and the tracking point in the world coordinate system.

[0109] l d Forward sight distance

[0110] θ: The orientation of the robot control point's velocity in the robot coordinate system

[0111] R: Robot's rotation radius

[0112] v c : The velocity of the control point in the world coordinate system

[0113] v: The velocity of the vehicle's center point in the world coordinate system

[0114] ω: angular velocity of the vehicle

[0115] l: The distance from the control point to the center, generally equal to the vehicle length.

[0116] d: width of the vehicle

[0117] k control Control intensity

[0118] The posterior state vector of the (k+1)th iteration, and the filtering result of the (k+1)th iteration.

[0119] The prior state vector of the kth iteration

[0120] The posterior state vector of the (k+1)th iteration, and the filtering result.

[0121] P k The prior estimate of the covariance matrix at time k.

[0122] P k′ The posterior estimated covariance matrix at time k

[0123] u k The input vector for the k-th iteration

[0124] z k The observation vector for the k-th iteration is given by the sensor.

[0125] F k The state transition matrix is ​​actually a conjectured model of the target state transition.

[0126] B k The input transition matrix is ​​the matrix that transforms the input into states.

[0127] H: Observation transition matrix, which is the matrix that transforms observations into states.

[0128] Q k Covariance matrix of process noise

[0129] R k : Covariance matrix of observation noise

[0130] K k Kalman gain is an intermediate result of filtering.

[0131] The various embodiments in this specification are described in a progressive manner. Similar or identical parts between embodiments can be referred to interchangeably. Each embodiment focuses on its differences from other embodiments. In particular, for the device embodiments, the above descriptions are merely preferred embodiments of the present invention. Since they are fundamentally similar to the method embodiments, the descriptions are relatively simple, and relevant parts can be referred to the descriptions of the method embodiments. The above descriptions are merely specific embodiments of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention, without departing from the principle of the present invention, should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A high-precision differential vehicle recharging function control method, characterized by The method comprises the following steps: 1) identification: identify the coordinates of the end point coordinates in the world coordinate system; 2) filtering: filtering the coordinates identified in step 1) to eliminate errors; 3) motion control: according to the real-time target point, calculate the linear velocity v and angular velocity ω of the robot that should be sent to the bottom control until the car successfully reaches the end point, the specific method is as follows: 3.1) calculate the following distance according to the lateral error of the robot distance along the line; l d ＝|y rg |+l control Wherein, l d is the front distance, y rg is the y coordinate of the target point in the world coordinate system, l control is an adjustment parameter, and l control is adjusted to control the following effect. 3.2) calculate the orientation δ of the connecting line between the center point of the robot and the tracking point in the world coordinate system; 3.3) According to delta and orientation of the robot in the world coordinate system, theta wr Calculate the angle beta between the line connecting the robot center point and the tracking point and the robot orientation; β = δ + θ wr 3.4) calculate the velocity orientation of the control point of the car according to β, so that when the control point advances in this direction for a period of time, the center of the robot just passes through the preview point, that is, the positive center point at the front or back of the robot; Wherein, Θ is the representation of the orientation of the velocity of the robot control point in the robot coordinate system, / is the length of the control point from the center, v is the velocity of the center of the vehicle in the world coordinate system, k control is the control intensity; 3.5) after obtaining θ, calculate the angular velocity ω of the car according to the geometric relationship, the radius of rotation R, and then calculate the center point velocity v: v = ωR where v c is the velocity of the control point in the world coordinate system.

2. The high-precision differential car charging function control method according to claim 1, wherein the filtering process of step 2) refers to extended Kalman filtering, which comprises the following steps: 2.1) Establish the prior state vector for the kth iteration Input vector u for the kth iteration k State transition matrix F k Input transition matrix B k and observation transition matrix H: u k = [v ω] T H = I3 2.2) Give the covariance matrix Q of the process noise k and the covariance matrix R of the observation noise k : 2.3) update the next predicted value according to the state transition relationship: P k = F k P k-1 F kT + Q k wherein, is the posterior state vector for the k+1th iteration, P k is the posterior estimation covariance matrix at the kth time instant; 2.4) Calculate Kalman gain K from observations k : 2.5) apply the observation value to correct the predicted value, and take the state correction value therein as the output of the filtering and as the input of the next iteration for the next iteration: P k+1 = (I - K k H)P k wherein, is the posterior state vector for the k+1 iteration, z k is the observation vector for the k iteration, given by the sensor, P k+1 is the prior estimation covariance matrix for the k+1 iteration.

3. The high-precision differential vehicle recharging function control method according to claim 1, characterized in that: In the identification process of step 1), an identification device is deployed above the charging pile point, and a sensing device corresponding to the identification device is configured on the robot.

4. The high-precision differential vehicle recharging function control method according to claim 3, characterized in that: The identification device is a two-dimensional code or a fixed geometric shape information.

5. The high-precision differential vehicle recharging function control method according to claim 4, characterized in that: The fixed geometric shape information is a V-shaped groove.

6. The high-precision differential vehicle recharging function control method according to claim 3, characterized in that: The sensing device is a camera.

Images