CN116703970A - High-precision differential vehicle recharging function control method - Google Patents
High-precision differential vehicle recharging function control method Download PDFInfo
- Publication number
- CN116703970A CN116703970A CN202310606880.6A CN202310606880A CN116703970A CN 116703970 A CN116703970 A CN 116703970A CN 202310606880 A CN202310606880 A CN 202310606880A CN 116703970 A CN116703970 A CN 116703970A
- Authority
- CN
- China
- Prior art keywords
- point
- robot
- control
- coordinate system
- world coordinate
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 32
- 238000001914 filtration Methods 0.000 claims abstract description 20
- 239000011159 matrix material Substances 0.000 claims description 30
- 230000007704 transition Effects 0.000 claims description 10
- 230000008569 process Effects 0.000 claims description 8
- 238000012937 correction Methods 0.000 claims description 6
- 238000012546 transfer Methods 0.000 claims description 6
- 230000000694 effects Effects 0.000 claims description 4
- 238000005516 engineering process Methods 0.000 description 6
- 230000006870 function Effects 0.000 description 6
- 238000010586 diagram Methods 0.000 description 5
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004140 cleaning Methods 0.000 description 1
- 239000002131 composite material Substances 0.000 description 1
- 230000010365 information processing Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 230000000630 rising effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
- G06T7/246—Analysis of motion using feature-based methods, e.g. the tracking of corners or segments
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/60—Other road transportation technologies with climate change mitigation effect
- Y02T10/70—Energy storage systems for electromobility, e.g. batteries
Abstract
The invention provides a high-precision differential vehicle recharging function control method, which comprises the steps of firstly identifying the coordinates of an end point coordinate under a world coordinate system, then carrying out filtering treatment on the identified coordinates to eliminate errors, and finally calculating the linear velocity v and the angular velocity omega of a robot which should be sent to a bottom layer for control according to the real-time target point position until the vehicle is successful to the end point. The invention provides a novel motion control method which can make up for inaccurate navigation and piling caused by insufficient global positioning, thereby reducing the precision requirement of a sensor for global positioning, and having the advantages of simple motion mode, high motion precision and low space requirement of motion.
Description
Technical Field
The invention relates to the field of motion control of mobile robots, in particular to a high-precision differential car recharging function control method.
Background
In recent years, technologies in the fields of mobile robots and autopilot are rising and developing. In a wheel type mobile robot or a composite robot sold in the market, the power sources are mainly divided into three types of differential models, ackerman models and omnidirectional models according to a wheel driving mode. The differential model is widely applied to various commercial scenes, such as unmanned warehouse, medical wheelchair, 3C, indoor and outdoor cleaning and the like, due to the simple driving mode and flexible movement mode.
The basic technologies related to the mobile robot include sensor technology, information processing technology, automation control technology and navigation technology. The navigation control module adjusts the control mode and the precision requirement according to different use modes, different ground pavements and different obstacle conditions. The automatic recharging function is an important part of the functional module of the product, and has very high requirements on the control mode and precision, wherein the pile motion control algorithm needs to simplify the motion path as much as possible under the condition of meeting the precision.
The common double-wheel differential mobile robot recharging pile alignment system mainly comprises the following two types:
1. under the condition that the global positioning precision is high enough, the pile is directly realized by using a general navigation local planning algorithm, and the requirement on the global precision is very high. Under the condition of low global positioning accuracy, a general navigation control algorithm can cause insufficient pile accuracy due to positioning jump, or repeated attempts at the end point can cause the process to be not concise.
2. And real-time positioning is performed by utilizing a plurality of groups of infrared sensors, so that the robot is controlled to be accurately positioned to the position of the charging pile. However, this method requires too high a sensor stability.
Disclosure of Invention
The invention provides a device for accurately controlling piles under the condition that the accuracy of a sensor is not very high.
The invention adopts a control device, a two-dimensional code is deployed on the position of a charging pile point or information of a fixed geometric shape such as a V-shaped groove is provided, and meanwhile, a corresponding sensing sensor such as a camera and a two-dimensional code is arranged on a robot, and the position information of the two-dimensional code or the geometric shape is acquired in real time through the camera or a laser radar sensor.
The invention comprises the following steps:
1) And (3) identification: identifying coordinates of the end point coordinates in a world coordinate system;
2) And (3) filtering: filtering the coordinates identified in the step 1) to eliminate errors;
the filtering process refers to extended Kalman filtering, and specifically comprises the following steps:
2.1 A priori state vector for the kth iteration is establishedThe input vector u of the kth iteration k State transition matrix F k Input transfer matrix B k And observing a transfer matrix H:
u k ＝[v ω] T
H＝I 3
2.2 Covariance matrix Q) giving process noise k And covariance matrix R of observed noise k ：
2.3 Updating the predicted value of the next step according to the state transition relation:
P k ＝F k P k-1 F kT +Q k
wherein ,posterior state vector, P, for the k+1th iteration k Estimating a covariance matrix for a posterior at a kth time;
2.4 Calculating Kalman gain K from observations k ：
2.5 Applying the observed value to correct the predicted value, taking the state correction value as the output of filtering and taking the state correction value as the input of the next iteration to carry out the next iteration:
P k' ＝(I-K k H)P k
wherein ,posterior state vector, z, for the k+1th iteration k For the observation vector of the kth iteration, given by the sensor, P k' A covariance matrix is estimated for the posterior at time k.
3) Motion control: according to the real-time target point position, calculating the linear velocity v and the angular velocity omega which the robot should send to the bottom layer control until the vehicle is successful to the end point, and the specific method is as follows:
3.1 Calculating a following distance according to a transverse error along the robot from the target;
l d ＝|y rg |+l control
wherein ,Id For forward looking distance, y rg Is the y coordinate, l of the target point in the world coordinate system control To adjust the parameters, by adjusting l control To control the effect of the follow-up;
3.2 Calculating the orientation delta of the connecting line between the robot center point and the tracking point under the world coordinate system according to the following distance;
3.3 According to delta and orientation theta of the robot in world coordinate system wr Calculating an included angle beta between a connecting line between the central point of the robot and the tracking point and the direction of the robot;
β＝δ+θ wr
3.4 Calculating the speed orientation of the control point of the vehicle according to beta, so that when the control point advances for a period of time in the direction, the center belt of the robot just passes through the pre-aiming point, namely the forefront or rearmost center point of the robot;
where θ is the representation of the orientation of the speed of the robot control point in the robot coordinate system, l is the length of the control point from the center, v is the speed of the vehicle center point in the world coordinate system, k control To control intensity;
3.5 After θ is obtained, the angular velocity ω of the vehicle is calculated from the geometric relationship, the radius R is rotated, and then the center point velocity v is calculated:
v＝ωR
wherein ,vc To control the speed of the point in the world coordinate system.
The invention has the beneficial effects that:
1. the invention provides a new motion control method which can make up for inaccurate navigation and piling caused by insufficient global positioning, thereby reducing the accuracy requirement of a sensor for global positioning.
2. The method can still maintain the correct traveling direction when the characteristics are not seen.
3. The motion control of the method can be compatible with two motion modes of forward motion and backward motion.
4. The method has the advantages of simple movement mode, high movement precision and low space requirement of movement.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings that are needed in the embodiments will be briefly described below, and it is obvious that the drawings in the following description are only some embodiments of the present invention, and other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
FIG. 1 is a schematic diagram of a recharging function control system;
FIG. 2 is a control flow diagram of the recharging function control system;
FIG. 3 is a filtering flow chart;
FIG. 4 (a) is a schematic view showing the direction delta of the connection line between the robot center point and the tracking point in the world coordinate system according to the following distance;
FIG. 4 (b) is a diagram showing the orientation θ in world coordinate system based on δ and robot wr Calculating an included angle beta schematic diagram between a connecting line between a robot center point and a tracking point and a robot direction;
fig. 4 (c) is a schematic diagram showing the speed orientation of the control point of the vehicle calculated from β.
Detailed Description
The following description of the embodiments of the present invention will be made clearly and completely with reference to the accompanying drawings, in which it is apparent that the embodiments described are only some embodiments of the present invention, but not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
The invention adopts a control device, as shown in fig. 1, a two-dimensional code is deployed on the position of a charging pile point or information of a fixed geometric shape such as a V-shaped groove is provided, and meanwhile, a corresponding perception sensor such as a camera, a two-dimensional code and the like is arranged on a robot, and the position information of the two-dimensional code or the geometric shape is acquired in real time through the camera or a laser radar sensor.
The control flow chart of the recharging function control system provided by the invention is shown in fig. 2, and is mainly divided into three parts:
1) And (3) identification: identifying coordinates of the end point coordinates in a world coordinate system;
2) And (3) filtering: filtering the coordinates identified in the step 1) to eliminate errors;
the filtering process refers to extended Kalman filtering, and specifically comprises the following steps:
2.1 A priori state vector for the kth iteration is establishedThe input vector u of the kth iteration k State transition matrix F k Input transfer matrix B k And observing a transfer matrix H:
u k ＝[v ω] T
H＝I 3
2.2 Covariance matrix Q) giving process noise k And covariance matrix R of observed noise k ：
2.3 Updating the predicted value of the next step according to the state transition relation:
P k ＝F k P k-1 F kT +Q k
wherein ,posterior state vector, P, for the k+1th iteration k Estimating a covariance matrix for a posterior at a kth time;
2.4 Calculating Kalman gain K from observations k ：
2.5 Applying the observed value to correct the predicted value, taking the state correction value as the output of filtering and taking the state correction value as the input of the next iteration to carry out the next iteration:
P k' ＝(I-K k H)P k
wherein ,posterior state vector, z, for the k+1th iteration k For the observation vector of the kth iteration, given by the sensor, P k' A covariance matrix is estimated for the posterior at time k.
3) Motion control: according to the real-time target point position, calculating the linear velocity v and the angular velocity omega which the robot should send to the bottom layer control until the vehicle is successful to the end point, and the specific method is as follows:
3.1 Calculating a following distance according to a transverse error along the robot from the target;
l d ＝|y rg |+l control
wherein ,ld For forward looking distance, y rg Is the y coordinate, l of the target point in the world coordinate system control To adjust the parameters, by adjusting l control To control the effect of the follow-up;
3.2 Calculating the orientation delta of the connecting line between the robot center point and the tracking point in the world coordinate system according to the following distance, referring to fig. 4 (a);
3.3 According to delta and orientation theta of the robot in world coordinate system wr Calculating an included angle beta between a connecting line between the center point of the robot and the tracking point and the direction of the robot, referring to fig. 4 (b);
β＝δ+θ wr
3.4 Calculating the speed orientation of the control point of the vehicle according to beta, so that when the control point advances in the direction for a period of time, the center zone of the robot just passes through the pre-aiming point, referring to fig. 4 (c), namely the center point of the front most or rear most (in the example, both advancing modes) of the robot;
where θ is the representation of the orientation of the speed of the robot control point in the robot coordinate system, l is the length of the control point from the center, v is the speed of the vehicle center point in the world coordinate system, k control To control intensity;
3.5 After θ is obtained, the angular velocity ω of the vehicle is calculated from the geometric relationship, the radius R is rotated, and then the center point velocity v is calculated:
v＝ωR
wherein ,vc To control the speed of the point in the world coordinate system.
The present invention complies with the following symbol convention:
x wg x-coordinate of target point in world coordinate system
y wg Y-coordinate of target point in world coordinate system
θ wg Orientation of target point in world coordinate system
x wr X-coordinate of robot in world coordinate system
y wr Y-coordinate of robot in world coordinate system
θ wr : orientation of robot in world coordinate system
x rg : x-coordinate of target point in world coordinate system
y rg : y-coordinate of target point in world coordinate system
θ rg : orientation of target point in world coordinate system
Beta: included angle between connecting line between robot center point and tracking point and robot direction
Delta: orientation of connection line between robot center point and tracking point in world coordinate system
l d : distance of front vision
θ: representation of the orientation of the speed of a robot control point in the robot coordinate system
R: radius of rotation of robot
v c : controlling the speed of a point in world coordinates
v: speed of vehicle center point in world coordinate system
Omega: angular velocity of vehicle
l: the length of the control point from the center is generally equal to the length of the vehicle
d: width of vehicle
k control : controlling intensity
Posterior state vector of the k+1th iteration, filtering result of the k+1th iteration
Prior state vector for the kth iteration
Posterior state vector of the (k+1) th iteration, filtering result
P k : priori estimated covariance matrix at k-th moment
P k′ : posterior estimated covariance matrix at kth time
u k : input vector for the kth iteration
z k : the observation vector of the kth iteration is given by the sensor
F k : state transition matrix, in effect a guess model for state transitions of objects
B k : an input transition matrix, which is a matrix that converts an input into a state
H: an observation transition matrix is a matrix for converting an observation into a state
Q k : covariance matrix of process noise
R k : covariance matrix of observation noise
K k : the kalman gain is an intermediate result of the filtering.
In this specification, each embodiment is described in a progressive manner, and identical and similar parts of each embodiment are all referred to each other, and each embodiment mainly describes differences from other embodiments. In particular, for the equipment examples, what has been described above is merely a preferred embodiment of the invention, which, since it is substantially similar to the method examples, is described relatively simply, as relevant to the description of the method examples. The foregoing is merely illustrative of specific embodiments of the present invention, and the scope of the invention is not limited thereto, since modifications and substitutions will be readily made by those skilled in the art without departing from the spirit of the invention. Therefore, the protection scope of the present invention should be subject to the protection scope of the claims.
Claims (6)
1. The high-precision differential vehicle recharging function control method is characterized by comprising the following steps of:
1) And (3) identification: identifying coordinates of the end point coordinates in a world coordinate system;
2) And (3) filtering: filtering the coordinates identified in the step 1) to eliminate errors;
3) Motion control: according to the real-time target point position, calculating the linear velocity v and the angular velocity omega which the robot should send to the bottom layer control until the vehicle is successful to the end point, and the specific method is as follows:
3.1 Calculating a following distance according to a transverse error along the robot from the target;
l d ＝|y rg |+l control
wherein ,ld For forward looking distance, y rg Is the y coordinate, l of the target point in the world coordinate system control To adjust the parameters, by adjusting l control To control the effect of the follow-up;
3.2 Calculating the orientation delta of the connecting line between the robot center point and the tracking point under the world coordinate system according to the following distance;
3.3 According to delta and orientation theta of the robot in world coordinate system wr Calculating an included angle beta between a connecting line between the central point of the robot and the tracking point and the direction of the robot;
β＝δ+θ wr
3.4 Calculating the speed orientation of the control point of the vehicle according to beta, so that when the control point advances for a period of time in the direction, the center belt of the robot just passes through the pre-aiming point, namely the forefront or rearmost center point of the robot;
where θ is the representation of the orientation of the speed of the robot control point in the robot coordinate system, l is the length of the control point from the center, v is the speed of the vehicle center point in the world coordinate system, k control To control intensity;
3.5 After θ is obtained, the angular velocity ω of the vehicle is calculated from the geometric relationship, the radius R is rotated, and then the center point velocity v is calculated:
v＝ωR
wherein ,vc To control the speed of the point in the world coordinate system.
2. The high-precision differential car recharging function control method according to claim 1, characterized by comprising the following steps: step 2) the filtering process refers to extended kalman filtering, and specifically comprises the following steps:
2.1 A priori state vector for the kth iteration is establishedThe input vector u of the kth iteration k State transition matrix F k Input transfer matrix B k And observing a transfer matrix H:
u k ＝[v ω] T
H＝I 3
2.2 Covariance matrix Q) giving process noise k And covariance matrix R of observed noise k ：
2.3 Updating the predicted value of the next step according to the state transition relation:
P k ＝F k P k-1 F k T+Q k
wherein ,posterior state vector, P, for the k+1th iteration k Estimating a covariance matrix for a posterior at a kth time;
2.4 Calculating Kalman gain K from observations k ：
2.5 Applying the observed value to correct the predicted value, taking the state correction value as the output of filtering and taking the state correction value as the input of the next iteration to carry out the next iteration:
P k+1 ＝(I-K k H)P k
wherein ,posterior state vector, z, for the k+1th iteration k For the observation vector of the kth iteration, given by the sensor, P k' A covariance matrix is estimated for the posterior at time k.
3. The high-precision differential car recharging function control method according to claim 1, characterized by comprising the following steps: in the step 1), an identification device is deployed on the charging pile point, and a sensing device corresponding to the identification device is configured on the robot.
4. The high-precision differential car recharging function control method according to claim 3, wherein the method comprises the following steps of: the identification device is a two-dimensional code or information of a fixed geometric shape.
5. The high-precision differential car recharging function control method according to claim 4, wherein the method comprises the following steps: the information of the fixed geometric shape is a V-shaped groove.
6. The high-precision differential car recharging function control method according to claim 3, wherein the method comprises the following steps of: the sensing device is a camera.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310606880.6A CN116703970A (en) | 2023-05-26 | 2023-05-26 | High-precision differential vehicle recharging function control method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310606880.6A CN116703970A (en) | 2023-05-26 | 2023-05-26 | High-precision differential vehicle recharging function control method |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116703970A true CN116703970A (en) | 2023-09-05 |
Family
ID=87840209
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310606880.6A Pending CN116703970A (en) | 2023-05-26 | 2023-05-26 | High-precision differential vehicle recharging function control method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116703970A (en) |
-
2023
- 2023-05-26 CN CN202310606880.6A patent/CN116703970A/en active Pending
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108955688B (en) | Method and system for positioning double-wheel differential mobile robot | |
| WO2019104581A1 (en) | Track generating method and apparatus, and unmanned ground vehicle | |
| CN106123890A (en) | A kind of robot localization method of Fusion | |
| Chen et al. | Qualitative vision-based path following | |
| CN108274467B (en) | Robot straight line control method, chip and robot | |
| CN108469823B (en) | Homography-based mobile robot formation following method | |
| CN114184200B (en) | Multi-source fusion navigation method combined with dynamic mapping | |
| CN107065873B (en) | Multi-curvature circumferential path tracking control method based on tape guidance AGV | |
| US11952038B2 (en) | Transverse steering method and transverse steering device for moving a vehicle into a target position, and vehicle for this purpose | |
| CN112183133A (en) | Aruco code guidance-based mobile robot autonomous charging method | |
| CN110763224A (en) | Navigation method and navigation system for automatic guided transport vehicle | |
| CN111487976B (en) | Backing track tracking method | |
| CN112762928B (en) | ODOM and DM landmark combined mobile robot containing laser SLAM and navigation method | |
| CN110673608A (en) | Robot navigation method | |
| CN109085840B (en) | Vehicle navigation control system and control method based on binocular vision | |
| CN111857114A (en) | Robot formation moving method, system, equipment and storage medium | |
| CN116337045A (en) | High-speed map building navigation method based on karto and teb | |
| US11087158B2 (en) | Error correction of airborne vehicles using natural patterns | |
| Peng et al. | Depth camera based row-end detection and headland manuvering in orchard navigation without GNSS | |
| CN115993089B (en) | PL-ICP-based online four-steering-wheel AGV internal and external parameter calibration method | |
| CN116703970A (en) | High-precision differential vehicle recharging function control method | |
| Krejsa et al. | Fusion of local and global sensory information in mobile robot outdoor localization task | |
| Jin et al. | Development of autonomous driving systems using state estimator with multi-rate sampled-data | |
| Alvarez et al. | The software stack that won the formula student driverless competition | |
| Atsuzawa et al. | Robot navigation in outdoor environments using odometry and convolutional neural network |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |