Disclosure of Invention
The embodiment of the invention aims to provide an AGV trackless guiding method and system based on optical positioning, and aims to solve the technical scheme in the prior artProvided withThe visual guidance technology can not realize trackless guidance.
In a first aspect, an AGV trackless guidance method based on optical positioning is provided, where the method includes the following steps:
two pieces of displacement information of AGV automobile body are obtained through two at least optical positioning sensors, and two pieces of displacement information are respectively: s1 and S2;
calculating the coordinate value and the azimuth angle of the current moment through a discrete motion equation according to S1 and S2;
calculating a deviation control parameter according to the coordinate value and the azimuth of the current time and an equation of a preset route;
and inputting the deviation control parameters into a driving control system to control the AGV to run according to a set route.
Optionally, the discrete equation of motion specifically includes:
wherein T represents the time of two adjacent samples, Tn represents the time of the nth sample, S1(Tn) the first displacement of the Tn th sample, S2(Tn) the second displacement of the Tn th sample, T represents the current time, and X (T) represents the X-axis coordinate of the current time; y (t) represents a Y-axis coordinate of the current time t, θ (t) represents an azimuth angle of the current time, and X (0) represents an X-axis coordinate of the starting point (0); y (0) represents the Y-axis coordinate of the start point (0), and θ (0) represents the azimuth angle of the start point (0).
Optionally, the calculating of the deviation control parameter according to the coordinate value of the current time, the azimuth angle, and the equation of the preset route specifically includes:
the equation of the preset route is specifically as follows: ax + by + c is 0;
θ (t) - θ (line);
wherein theta (t) represents the azimuth angle at the current moment, and theta (line) represents the included angle of the preset route; MN denotes a deviation control parameter.
Optionally, after the deviation control parameter is input into the driving control system to control the AGV to run along the set route, the method further includes:
when the AGV body needs to switch the line, after the AGV body is determined to be in the switching range according to the coordinate value and the azimuth angle of the current time, the switched deviation control parameter is calculated according to the coordinate value and the azimuth angle of the current time and the equation of the switched line, and the switched deviation control parameter is input into the driving control system to control the AGV line to run according to the switched line.
Optionally, after determining that the AGV body is within the switching range according to the coordinate value and the azimuth of the current time, calculating a switched deviation control parameter according to the coordinate value and the azimuth of the current time and an equation of the switched route specifically includes:
the equation of the switched line may specifically be: a 'x + b' y + c ═ 0;
θ (t) - θ (line)';
where θ (line) 'represents the angle of the line after switching, and MN' is the deviation control parameter after switching.
In another aspect, an AGV trackless guidance system based on optical positioning is provided, the system comprising: positioning system, navigation system and drive control system, positioning system includes: at least two optical position sensors and a light source cooperating with the at least two optical equipotential sensors; wherein,
positioning system is used for acquireing two displacement information of AGV automobile body, and two displacement information do respectively: s1 and S2, and transmitting the two pieces of displacement information to a navigation system;
the navigation system is used for calculating the coordinate value and the azimuth angle of the current moment through a discrete motion equation according to S1 and S2; calculating a deviation control parameter according to the coordinate value and the azimuth of the current time and an equation of a preset route; the navigation system transmits the deviation control parameters to a driving control system;
and the driving control system is used for controlling the AGV to run according to the set route according to the deviation control parameter.
Optionally, the discrete equation of motion specifically includes:
wherein T represents the time of two adjacent samples, Tn represents the time of the nth sample, S1(Tn) the first displacement of the Tn th sample, S2(Tn) the second displacement of the Tn th sample, T represents the current time, and X (T) represents the X-axis coordinate of the current time; y (t) represents a Y-axis coordinate of the current time t, θ (t) represents an azimuth angle of the current time, and X (0) represents an X-axis coordinate of the starting point (0); y (0) represents the Y-axis coordinate of the start point (0), and θ (0) represents the azimuth angle of the start point (0).
Optionally, the navigation system is specifically configured to:
the equation of the preset route is specifically as follows: ax + by + c is 0;
θ (t) - θ (line);
wherein theta (t) represents the azimuth angle at the current moment, and theta (line) represents the included angle of the preset route; MN denotes a deviation control parameter.
Alternatively to this, the first and second parts may,
when the AGV body needs to switch the line, the navigation system is further used for determining that the AGV body is in the switching range according to the coordinate value and the azimuth angle of the current time, calculating a switched deviation control parameter according to the coordinate value and the azimuth angle of the current time and an equation of the switched route, and inputting the switched deviation control parameter into the driving control system;
and the driving control system is also used for controlling the AGV to run according to the switched route according to the switched deviation control parameters.
Optionally, the navigation system is specifically configured to:
the equation of the switched line may specifically be: a 'x + b' y + c ═ 0;
θ (t) - θ (line)';
where θ (line) 'represents the angle of the line after switching, and MN' is the deviation control parameter after switching.
In the embodiment of the invention, the technical scheme provided by the invention adopts the optical positioning sensor to automatically position the AGV and determine the position and attitude information of the AGV, thereby directly completing trackless guidance without laying a track.
Detailed Description
In order to make the objects, technical solutions and advantages of the present invention more apparent, the present invention is described in further detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.
The specific embodiment of the invention provides an AGV trackless guiding method based on optical positioning, which is shown in fig. 7 and comprises the following steps:
101. two pieces of displacement information of AGV automobile body are obtained through two at least optical positioning sensors, and two pieces of displacement information are respectively: s1 and S2;
102. calculating the coordinate value and the azimuth angle of the current moment through a discrete motion equation according to S1 and S2;
103. calculating a deviation control parameter according to the coordinate value and the azimuth of the current time and an equation of a preset route;
104. and inputting the deviation control parameters into a driving control system to control the AGV to run according to a set route.
Optionally, the discrete equation of motion may specifically be:
wherein T represents the time of two adjacent samples, Tn represents the time of the nth sample, S1(Tn) the first displacement of the Tn th sample, S2(Tn) the second displacement of the Tn th sample, T represents the current time, and X (T) represents the X-axis coordinate of the current time; y (t) represents a Y-axis coordinate of the current time t, θ (t) represents an azimuth angle of the current time, and X (0) represents an X-axis coordinate of the starting point (0); y (0) represents the Y-axis coordinate of the start point (0), and θ (0) represents the azimuth angle of the start point (0).
Optionally, the implementation method of 103 may specifically be: the equation for the preset route may specifically be: ax + by + c is 0;
θ (t) - θ (line);
wherein theta (t) represents the azimuth angle at the current moment, and theta (line) represents the included angle of the preset route; MN denotes a deviation control parameter.
Optionally, after 104, the method may further include:
when the AGV body needs to switch the line, after the AGV body is determined to be in the switching range according to the coordinate value and the azimuth angle of the current time, the switched deviation control parameter is calculated according to the coordinate value and the azimuth angle of the current time and the equation of the switched line, and the switched deviation control parameter is input into the driving control system to control the AGV line to run according to the switched line.
The specific implementation mode is as follows:
the equation of the switched line may specifically be: a 'x + b' y + c ═ 0;
θ (t) - θ (line)';
where θ (line) 'represents the angle of the line after switching, and MN' is the deviation control parameter after switching.
The specific implementation mode of the invention also provides an AGV trackless guiding system based on optical positioning, which comprises: positioning system, navigation system and drive control system, positioning system includes: at least two optical position sensors and a light source cooperating with the at least two optical equipotential sensors; wherein,
positioning system is used for acquireing two displacement information of AGV automobile body, and two displacement information do respectively: s1 and S2, and transmitting the two pieces of displacement information to a navigation system;
the navigation system is used for calculating the coordinate value and the azimuth angle of the current moment through a discrete motion equation according to S1 and S2; calculating a deviation control parameter according to the coordinate value and the azimuth of the current time and an equation of a preset route; the navigation system transmits the deviation control parameters to a driving control system;
and the driving control system is used for controlling the AGV to run according to the set route according to the deviation control parameter.
Optionally, the discrete equation of motion specifically includes:
wherein T represents the time of two adjacent samples, Tn represents the time of the nth sample, S1(Tn) the first displacement of the Tn th sample, S2(Tn) the second displacement of the Tn th sample, T represents the current time, and X (T) represents the X-axis coordinate of the current time; y (t) represents a Y-axis coordinate of the current time t, θ (t) represents an azimuth angle of the current time, and X (0) represents an X-axis coordinate of the starting point (0); y (0) represents the Y-axis coordinate of the start point (0), and θ (0) represents the azimuth angle of the start point (0).
Optionally, the navigation system is specifically configured to:
the equation of the preset route is specifically as follows: ax + by + c is 0;
θ (t) - θ (line);
wherein theta (t) represents the azimuth angle at the current moment, and theta (line) represents the included angle of the preset route; MN denotes a deviation control parameter.
Optionally, when the AGV needs to switch the line, the navigation system is further configured to determine that the AGV is within the switching range according to the coordinate value and the azimuth of the current time, calculate a switched deviation control parameter according to the coordinate value and the azimuth of the current time and an equation of the switched route, and input the switched deviation control parameter into the driving control system;
and the driving control system is also used for controlling the AGV to run according to the switched route according to the switched deviation control parameters.
Optionally, the navigation system is specifically configured to:
the equation of the switched line may specifically be: a 'x + b' y + c ═ 0;
θ (t) - θ (line)';
where θ (line) 'represents the angle of the line after switching, and MN' is the deviation control parameter after switching.
The principles of the present invention are briefly described below with reference to the accompanying drawings:
FIG. 1 is a block diagram of the AGV positioning system according to the present invention, wherein the AGV positioning system includes an optical positioning sensor and a position and attitude calculation, the optical positioning sensor is mounted on a driving wheel platform of the AGV body according to a certain mounting manner, and a motion equation of the AGV is calculated through a corresponding algorithm, so that the position and attitude information of the AGV at any time is calculated. And uploading the obtained position and posture information to an AGV route navigation system in a data format of a coordinate starting point (x0, y0, theta 0), wherein the coordinate of the starting point is a point determined by a user. The AGV route navigation system establishes a reference coordinate system according to the actual situation of the field. Based on the reference coordinate system, different route equations to be traveled by the AGV are set, and deviation control parameters Err are calculated by comparing the AGV coordinates and azimuth angle information (x0, y0, theta 0) with the route equations. The deviation control parameter Err is uploaded to the drive control system. The drive control system can use a PLC or a single chip microcomputer as a main control unit and drive the motor through a PID closed-loop control algorithm, so that the aim of driving the AGV according to a specified route is fulfilled.
Fig. 2 shows an irradiation mode in which the optical positioning sensor is provided with a laser light source. In order to enable the optical positioning sensor to work normally at a certain height from the ground, a light source in a certain wavelength range needs to be additionally arranged, and the embodiment adopts 650nm red line laser. The normal working height of the optical positioning sensor from the ground can be adjusted by adjusting the irradiation angle of the laser head. Therefore, the AGV can normally run under different ground environments. As shown in fig. 2, among others, a laser head 202, an optical positioning sensor 201, a photoreceptor lens 2011, and an AGV body 203.
Fig. 3 shows the mounting position of the optical position sensor. In the embodiment, two optical positioning sensors are adopted, and the installation of the two positioning sensors and the calculation of the corresponding position and attitude are provided. The optical positioning sensor is arranged at the bottom of a driving wheel platform of the AGV body, the installation position is equal to the distance between the two driving wheels and the central point O on the same axis, and the distance between the two optical positioning sensors AB is L. The mounting mode can ensure that the optical positioning sensor only returns to the displacement in the y-axis direction when moving, and does not return to the value in the x-axis direction.
FIG. 4 is an algorithmic resolution of the position attitude (x0, y0, θ 0) of the optical position sensor locating the AGV. The installation position of the optical positioning sensor can ensure that the optical positioning sensor does not return the displacement value of the AGV in the x-axis direction but only returns the displacement value in the y-axis direction when the AGV runs. The motion of any object can be regarded as inertial motion and the reference coordinate system can be regarded as a two-dimensional planar coordinate system. The position and attitude information of the object can be known by knowing the real-time speed of any two points of the object according to the law of the two-dimensional inertial platform. Then knowing the speed information of two points a and B of the AGV drive wheel platform, the equation of motion of the AGV drive wheel can be derived as follows:
let the coordinates of the point O at any time of AGV be (x0, y0), the azimuth angle be θ 0, the real-time speed of the point O be v0, and the angular speed be ω 0, then there are
If the real-time speed of the point A provided with the optical positioning sensor is vA, the real-time speed of the point B is vB and the distance between the two points is L, the real-time speed of the point A and the real-time speed of the point B are respectively vA and vB, and the distance between the two points is L
The equation of motion of the O point of the AGV driving wheel platform can be deduced as follows:
in the formula, [ x (0), y (0), θ (0) ] represents the coordinates and azimuth angle at the initial time.
Since the data read from the optical positioning sensor is periodic discrete data when the software program is written, the obtained data is A, B displacement information of two points, the sampling time of each period is short, and the arc length of motion of A, B two points in each sampling period is short, so that the displacement can be approximated to a straight line displacement, which is S2 and S1 respectively. Assuming that the sampling period is Tn, where (n is 1,2,3 … …), and T1 is T2 is T3 is T … … is T, the arc length displacement of the two-point sampling in the nth period AB can be represented as S2(Tn) and S1(Tn), respectively. Discretizing the motion equation of the O point to obtain a motion equation beneficial to programming, wherein the motion equation is as follows:
when the angle is used for calculating the coordinate, the value of the angle is taken as the weighted average value of the angle value at the previous moment and the angle value at the current moment, so that the sampling and calculation errors are reduced. From this equation of motion, the coordinates and azimuth [ x (t), y (t), θ (t) ] for the AGV at any one time are obtained. And uploading the position and the posture to a route navigation system, and calculating a deviation control parameter for controlling a motor to drive the AGV to run.
FIG. 5 is an analytic graph of deviation control parameter algorithm of navigation coordinates during AGV traveling. Given the algorithmic analysis of a straight Line, the straight Line equation of a Line in the established reference coordinate system is ax + by + c, where ax + by + c is 0, the azimuth angle of the Line is θ 1, and the coordinates and azimuth angle of the AGV are (x0, y0, θ 0). And in order to better control the AGV to run according to the appointed route, selecting N point coordinates (x1, y1) which lead the ON distance along the azimuth angle of the AGV as control coordinates so as to achieve the purpose of advanced correction. Passing through the N points, making a perpendicular Line perpendicular to the ON direction and crossing the Line at the M point. The length of the line segment MN is used as the deviation control parameter Err to be controlled. The calculation is as follows:
distance NP from N point to Line of
ByThe length Err of the MN can be calculated.
The deviation control parameter Err calculated in this way can reflect the deviation between the AGV coordinates and the route, and also reflects the relative relationship between the azimuth angle of the AGV and the azimuth angle of the route. And uploading the deviation control parameter Err to a drive control system, and controlling the AGV to run according to the designated route through PID control.
Fig. 6 shows a route switching processing method during the travel of the AGV. When the AGV switches the travel from the Line1 to the Line2, the control coordinates of the AGV are (x1, y1), the switching point Q is the intersection of the Line1 and the Line2, and the coordinates are (x2, y 2). Since the control coordinates of the AGV are not always on the route trajectory Line1, a fault-tolerant process is required when the route is switched. The point Q is used as the center of a circle, the switching radius is r to make a circle, when the point N moves into the switching circle, the switching of the reference route is carried out, namely, when | x2-x1| < ═ r and | y2-y1| < | >, the reference route is switched to Line2 from Line1, and therefore the AGV can be guaranteed to normally carry out route switching.
The above description is only for the purpose of illustrating the preferred embodiments of the present invention and is not to be construed as limiting the invention, and any modifications, equivalents and improvements made within the spirit and principle of the present invention are intended to be included within the scope of the present invention.