CN109557929B - The motion control method and device of mobile robot - Google Patents
The motion control method and device of mobile robot Download PDFInfo
- Publication number
- CN109557929B CN109557929B CN201910119532.XA CN201910119532A CN109557929B CN 109557929 B CN109557929 B CN 109557929B CN 201910119532 A CN201910119532 A CN 201910119532A CN 109557929 B CN109557929 B CN 109557929B
- Authority
- CN
- China
- Prior art keywords
- motor point
- mobile robot
- curvature
- point
- motor
- 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.)
- Active
Links
- 230000033001 locomotion Effects 0.000 title claims abstract description 116
- 238000000034 method Methods 0.000 title claims abstract description 48
- 238000006073 displacement reaction Methods 0.000 claims description 6
- 238000004891 communication Methods 0.000 claims description 3
- 230000008569 process Effects 0.000 description 12
- 239000002245 particle Substances 0.000 description 9
- 238000010586 diagram Methods 0.000 description 5
- 238000013461 design Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 230000004807 localization Effects 0.000 description 3
- 238000007726 management method Methods 0.000 description 3
- 230000009471 action Effects 0.000 description 2
- 230000004888 barrier function Effects 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012952 Resampling Methods 0.000 description 1
- 230000005540 biological transmission Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000004140 cleaning Methods 0.000 description 1
- 238000011217 control strategy Methods 0.000 description 1
- 238000009826 distribution Methods 0.000 description 1
- 239000000428 dust Substances 0.000 description 1
- 238000000265 homogenisation Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- XOFYZVNMUHMLCC-ZPOLXVRWSA-N prednisone Chemical compound O=C1C=C[C@]2(C)[C@H]3C(=O)C[C@](C)([C@@](CC4)(O)C(=O)CO)[C@@H]4[C@@H]3CCC2=C1 XOFYZVNMUHMLCC-ZPOLXVRWSA-N 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000003860 storage Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
Abstract
The present embodiments relate to motion control fields, disclose the motion control method and device of a kind of mobile robot, wherein the motion control method of mobile robot, comprising: obtain the motion path of the mobile robot；The motion path is split as multiple motor points；Calculate curvature of the motion path at each motor point；Mobile robot described in the curvature control at motor point being presently according to the mobile robot is mobile to next motor point, until the mobile robot reaches the terminal of the motion path.The motion control method and device of mobile robot provided by embodiment of the present invention have the advantages that promote control precision when mobile robot is moved along the movement routine of on-fixed curvature.
Description
Technical field
The present embodiments relate to motion control field, in particular to the motion control method and dress of a kind of mobile robot
It sets.
Background technique
Mobile robot is that a kind of autonomous control is mobile, the automatic intelligent apparatus for executing work, it can on the ground or
It is moved in other surfaces, it can not only receive the commander of user, but also can run the program of preparatory layout, can also be according to people
The principle program action that work intellectual technology is formulated.With the progress of science and technology, mobile robot is in such as production, military, clothes
There is application in the fields such as business, especially more and more wider in household service field application, for example, dust suction, sweep the floor, mop floor, cleaning the windows etc. it is clear
Clean robot and the grass-removing robot for mowing etc..
In order to guarantee safety and the working efficiency of moveable robot movement, mobile robot is usually to track to preset
Good path is moved, and the method for realizing route tracking mainly includes control algolithm combination manual control strategy in the prior art
With two kinds of feature of control algolithm combination fixed route.The principle of two kinds of path following methods is all to deposit in mobile robot in advance
The motion state for storing up mobile robot under multiple discrete curvature and the curvature of various discrete, after the completion of trajectory path planning,
The curvature of motion path is supplied to mobile robot, mobile robot automatically by the curvature of motion path and the curvature that prestores into
Row matching, obtains motion state, so that motion path be followed to be moved.
However, the inventors found that: the either method or control algolithm of control algolithm combination manual control
In conjunction with the method for fixed route feature, being all confined to the path with fixed curvature, (such as broken line or Straight Line and Arc combination are bent
Line), it is poor for the tracking control precision in the path of on-fixed curvature, it is easy to cause mobile robot to be detached from path.
Summary of the invention
The motion control method and device for being designed to provide a kind of mobile robot of embodiment of the present invention are promoted and are moved
Control precision when mobile robot is along the movement path of on-fixed curvature.
In order to solve the above technical problems, embodiments of the present invention provide a kind of motion control side of mobile robot
Method, the motion path including obtaining the mobile robot；The motion path is split as multiple motor points；Calculate the fortune
Dynamic curvature of the path at each motor point；The curvature control at motor point being presently according to the mobile robot
The mobile robot is mobile to next motor point, until the mobile robot reaches the end of the motion path
Point.
Embodiments of the present invention additionally provide a kind of motion control device of mobile robot, comprising: at least one
Manage device；And the memory being connect at least one described processor communication；Wherein, be stored with can be described for the memory
The instruction that at least one processor executes, described instruction executed by least one described processor, so that described at least one
Reason device is able to carry out the motion control method of mobile robot as the aforementioned.
Embodiment of the present invention is split as multiple motor points in terms of existing technologies, by motion path, by seeking
Curvature of the motion path at each motor point, and the curvature control at the motor point being presently according to mobile robot moves
Robot is mobile to next motor point, i.e., the process that mobile robot follows motion path mobile is split as mobile robot
The process mobile towards next motor point from a motor point.Since mobile robot is from a motor point towards next fortune
The curvature control in the motor point that the moving process of dynamic point is presently according to mobile robot, control precision is higher, to make
It is higher to obtain the walking precision that mobile robot follows motion path mobile.Since the application embodiment obtains each motor point
Curvature, therefore, even mobile robot can still keep higher control essence along the movement path of on-fixed curvature
Degree.
In addition, the curvature for calculating the motion path at each motor point, specifically includes: establishing space seat
Mark system, the motion path and the multiple motor point are mapped in the space coordinates；Obtain each motor point
Coordinate value in the space coordinates；According to the coordinate value in each motor point, the motion path is calculated each
Curvature at the motor point.Space coordinates are established, motion path and motor point are mapped in space coordinates, pass through sky
Between coordinate system coordinate representation motor point position, according to the coordinate value in motor point calculate motion path at each motor point
Curvature, so that the calculating of curvature is more convenient.
In addition, the coordinate value according to each motor point, calculates the motion path in each motor point
The curvature at place, specifically includes: according to the coordinate value in each motor point, calculating the k_cos curvature at each motor point；
Curvature using the k_cos curvature at each motor point as the motion path at each motor point.
In addition, the coordinate value according to corresponding to each motor point, calculates the k_cos at each motor point
Curvature specifically includes: calculating the k_cos curvature at each motor pointWherein,
pi、pi-k、pi+kI-th of motor point on the respectively described motion path, the i-th-k motor points and i-th+
K coordinates of the motor point in the space coordinates.
In addition, the coordinate value according to corresponding to each motor point, calculates the motion path each described
Curvature at motor point, specifically includes: according to coordinate value corresponding to each motor point, calculating at each motor point
U chord length curvature；Using the U chord length curvature at each motor point as the motion path at each motor point
Curvature.
In addition, the coordinate value according to corresponding to each motor point, calculates the U chord length at each motor point
Curvature specifically includes: calculating the U chord length curvature at each motor pointWherein,
ULTo determine constant parameter,Respectively jth+UfA motor point and jth-
UbCoordinate of a motor point in the space coordinates, For the U string at j-th of motor point
The symbol of long curvature,For jth+UfA motor point and jth-UbEuclidean between a motor point away from
From.
In addition, mobile robot described in curvature control at the motor point being presently according to the mobile robot
It is mobile to next motor point, it specifically includes: obtaining the curvature at the motor point that the mobile robot is presently in and make
For current curvature；Judge whether the current curvature is greater than preset value；If it is not, then according to path model- following control algorithm, control institute
It is mobile to next motor point to state mobile robot；If so, according to control algolithm, the control moving machine is rotated in place
After device people rotates in place first angle, according to the path model- following control algorithm, the mobile robot is controlled to next institute
It states mobile at motor point.When the curvature in the motor point that mobile robot is presently in is greater than preset value, controlled according to rotating in place
Algorithm processed control mobile robot rotates in place first angle, prevents curvature excessive using rotating in place and leads to not in movement
Wide-angle turning is realized in the process, ensures tracking, promotes the kinematic accuracy in deep camber path.
Judge whether the current curvature is greater than preset value in addition, described, specifically includes: obtaining the mobile robot and work as
Curvature in one N neighborhood in the motor point locating for preceding, at each motor point in addition to the motor point being presently in,
In, N is the integer greater than 0；Judge the song whether current curvature is simultaneously greater than at preset threshold and each motor point
The presupposition multiple of maximum value in rate；If so, the current curvature is greater than preset value, if it is not, then the current curvature is not more than
Preset value.
In addition, described move according to path model- following control algorithm, the control mobile robot to next motor point
It is dynamic, it specifically includes: obtaining the current pose p of the mobile robot1(x1,y1,θ1) and next motor point at target
Pose p2(x2,y2,θ2), wherein x1,y1For the coordinate value in the motor point being presently in, x2,y2It is described next
The coordinate value in the motor point, θ1Direction for the mobile robot in the current location, θ2Exist for the mobile robot
The target direction in next motor point；Displacement error e is calculated according to the current pose and the object pose1
(ex,ey,eθ), wherein ex=x1-x2, ey=y1-y2, eθ=θ1-θ2；The mobile robot is sought according to the displacement error
Linear velocity v and angular speed w, wherein
vrLinear velocity is referred to be preset,For the curvature of current location, l1、l2、l3, β be respectively the first parameter preset,
Second parameter preset, third parameter preset and the 4th parameter preset；The mobile robot is controlled according to the linear velocity and angle
Speed is mobile to next motor point.
Detailed description of the invention
One or more embodiments are illustrated by the picture in corresponding attached drawing, these exemplary theorys
The bright restriction not constituted to embodiment, the element in attached drawing with same reference numbers label are expressed as similar element, remove
Non- to have special statement, composition does not limit the figure in attached drawing.
Fig. 1 is the kernel program flow chart of embodiment of the present invention；
Fig. 2 is the program flow diagram of the motion control method of mobile robot provided by first embodiment of the invention；
Fig. 3 is the k_cos curvature for obtaining mobile robot provided by first embodiment of the invention and being presently in position
Program flow diagram；
Fig. 4 is the program flow diagram of the motion control method of mobile robot provided by second embodiment of the invention；
Fig. 5 is the U chord length curvature for obtaining mobile robot provided by second embodiment of the invention and being presently in position
Program flow diagram；
Fig. 6 is the structural schematic diagram of the motion control device of mobile robot provided by third embodiment of the invention.
Specific embodiment
In order to make the object, technical scheme and advantages of the embodiment of the invention clearer, below in conjunction with attached drawing to the present invention
Each embodiment be explained in detail.However, it will be understood by those skilled in the art that in each embodiment party of the present invention
In formula, many technical details are proposed in order to make reader more fully understand the present invention.But even if without these technical details
And various changes and modifications based on the following respective embodiments, claimed technical solution of the invention also may be implemented.
Referring to Fig. 1, the core of embodiment of the present invention is: step S101: obtaining the movement road of the mobile robot
Diameter.Step S102: the motion path is split as multiple motor points.Step S103: the motion path is calculated in each institute
State the curvature at motor point.Step S104: described in the curvature control at motor point being presently according to the mobile robot
Mobile robot is mobile to next motor point, until the mobile robot reaches the terminal of the motion path.This
Embodiment is by seeking curvature of the motion path at each motor point, and the motor point being presently according to mobile robot
The curvature control mobile robot at place is mobile to next motor point, so that the walking that mobile robot follows motion path mobile
Precision is higher.Since the application embodiment obtains the curvature in each motor point, even the movement road of on-fixed curvature
Diameter, mobile robot can still keep higher walking precision.Below to mobile robot provided by present embodiment
The realization details of motion control method is specifically described, and the following contents is only for convenience of the realization details provided is understood, not
Implement the necessary of this programme.
The first embodiment of the present invention is related to a kind of motion control methods of mobile robot, as shown in Fig. 2, this method
Include the following steps.
Step S201: the motion path of mobile robot is obtained.
Specifically, in this step, working condition and barrier situation setting movement road previously according to mobile robot
Diameter.For example, mobile robot needs to be finally moved to location of C from location A by B location, then location A is obtained to location of C and arrives B again
The moving line of position is bent setting moving line avoiding obstacles if there are barriers on moving line, final to obtain
The motion path of mobile robot.
Step S202: motion path is split as multiple motor points.
Specifically, in this step, can will be moved according to parameters such as the size of mobile robot and mobile control precision
Path is split as multiple motor points, each motor point is the shop on motion path.Each motor point is mobile machine
Mobile robot is split as along the long-range motion process that motion path moves multiple by people along the relay point of movement path
Short range motion process, the mobile robot process mobile from the motor point being presently in next motor point are mobile machine
Process of the people along motion path.
Step S203: the k_cos curvature that mobile robot is presently in motor point is obtained, using k_cos curvature as movement
Robot is presently in the curvature in motor point.
In this step, k_cos curvature is k cosine curvature, is presently in motor point by calculating mobile robot
K_cos curvature is presently in the curvature in motor point using k_cos curvature as mobile robot.Obtain the current institute of mobile robot
The step of locating the k_cos curvature of position is as shown in Figure 3, comprising:
Step S301: establishing space coordinates, and motion path and each motor point are respectively mapped in space coordinates,
Obtain coordinate value of each motor point in space coordinates.
In this step, space coordinates are world coordinate system.A frame of reference is selected to describe to take the photograph in the environment
The position of camera, and the position of any object in environment is described with it, which is known as world coordinate system.It is understood that
It is that space coordinates are that world coordinate system is only one of present embodiment specific embodiment, space coordinates can also be with
It is the coordinate system established according to other standards, herein without enumerating.
Step S302: determine that mobile robot is presently in the pose of position according to Monte Carlo localization algorithm.
Specifically, the specific calculating step of Monte Carlo localization algorithm are as follows: the location information of recorder people meets more
First Gaussian Profile N (μ, Σ), μ are robot initial pose (x0,y0,θ0), Σ is covariance matrix.From Gaussian Profile N (μ, Σ)
In sample out M particle, obtain the set of M particle x of initial t momentTo indicate t moment
Confidence level, each particle x represent a possible pose p (x, y, θ), and (x, y are robot coordinate under world coordinate system
Value, θ are robot direction, i.e., world coordinate system x-axis to robot coordinate system's x-axis to angle).In t moment, first according to odometer
Move mould p (xt|ut,xt-1) (u indicates that mileage counts), it samples out each particle and obtains the prior distribution p (x of particle collectiont),
Then further according to laser measurement model p (zt|m,xt) (z, m respectively indicate laser measurement data and grating map), it updates every
The weight ω of one particle, homogenization weight obtain the Posterior distrbutionp p (x of particle collectiont|zt).Particle after successive ignition in order to prevent
It degenerates, needs from Posterior distrbutionp p (xt|zt) in resampling.Finally according to apart from nearest principle, by particle collection sub-clustering, weight is maximum
The mean value of cluster is optimal estimation pose popt(x,y,θ).By poptThe pose of position is presently in as mobile robot.
Step S303: it is current in mobile robot that motion path is calculated according to the coordinate that mobile robot is presently in position
The k_cos curvature in locating motor point.
Specifically, assuming piFor i-th point on control route, then the k_cos curvature at i point are as follows:
pi、pi-k、pi+kI-th of motor point on the respectively described motion path, the i-th-k motor points and i-th+
K coordinates of the motor point in the space coordinates, pipi-kFor pi、pi-kVector between two o'clock, pipi+kFor pi、
pi+kVector between two o'clock, ‖ pipi-k‖ is vector pipi-kNorm, ‖ pipi+k‖ is vector pipi+kNorm.
It is understood that the above-mentioned specific formula to calculate the k_cos curvature on moving line at i-th of motor point,
K_cos curvature on motion path at each motor point can be calculated by the above method.
Step S204: judge that mobile robot is presently in the k_cos curvature of position and whether is greater than preset value, if it is not, then
Step S205 is executed, if so, thening follow the steps S206.
In this step, if the motor point that mobile robot is presently in is pi, motor point piThe k_cos curvature at place isFirstly, obtaining motor point piA N neighborhood [i-N, i+N] (N be positive integer) greater than 0 in remove motor point piOutside, institute
There is the k_cos curvature at motor point, obtains and remove motor point piOutside, the maximum value in the k_cos curvature at all motor points
JudgementWhether preset threshold c is simultaneously greater thanthresholdWith presupposition multiple α'sPublicity is expressed as follows:IfSimultaneously greater than preset threshold
cthresholdWith presupposition multiple α'sThen determine mobile robot be presently in motor point k_cos curvature be greater than preset value,
Execute step S206；IfAt least below preset threshold cthresholdWith presupposition multiple α'sOne of, then determine
Mobile robot is presently in the k_cos curvature of position no more than preset value, executes step S205.
In the present embodiment, preset threshold cthresholdIt is -0.4, presupposition multiple α is 1.2.It is understood that above-mentioned
Some design parameters only in present embodiment for example, in actual application, preset threshold cthresholdWith it is pre-
If multiple α can flexible setting according to the actual situation.
Step S205: mobile to next motor point according to path model- following control algorithm, control mobile robot.
Specifically, in this step, obtaining the current pose p of mobile robot first1(x1,y1,θ1) and next movement
Object pose p at point2(x2,y2,θ2), then according to current pose p1With object pose p2Displacement error e is calculated1(ex,
ey,eθ)。
Wherein, x1,y1For the coordinate value in the motor point that mobile robot is presently in, x2,y2For the seat in next motor point
Scale value, θ1For the current direction of mobile robot, θ2Target direction for mobile robot in next motor point；ex=x1-x2,
ey=y1-y2, eθ=θ1-θ2。
According to displacement error e1With path model- following control algorithmThe linear velocity v and angular speed of mobile robot is calculated
W, whereinvrLinear velocity is referred to be preset,
For the curvature of current location, l1、l2、l3, β be respectively the first parameter preset, the second parameter preset, third parameter preset and the 4th
Parameter preset.It should be noted that reference angular velocities vr, the first parameter preset l1, the second parameter preset l2, third parameter preset l3
It is pre-set fixed value parameter with the 4th parameter preset β, can be chosen according to the actual situation, for example, in this reality
It applies in mode, l1、l2、l3, β take 2,2,2,1.5 respectively.
It finally controls under mobile robot on-line velocity v and angular speed w and is moved to next motor point.
Step S206: control mobile robot stops movement, and according to rotating in place, control algolithm, control mobile robot are former
Ground rotates first angle.
Specifically, in this step, when the k_cos curvature that judgement mobile robot is presently in motor point is greater than preset value
When, control mobile robot stops movement, obtains the current pose p of mobile robot1(x1,y1,θ1) and rotation after pose p3
(x1,y1,θ3), according to current pose p1With the pose p after rotation3First angle θ is calculatedt, wherein θt=θ1-θ2。
According to rotating in place control algolithmIt is calculated and rotates in place angular speed w0, whereinl
For adjustment factor, it is to be understood that l is the adjustment factor chosen according to actual conditions, herein without limiting.
Further, mobile robot rotates in place control algolithm in basisRotate in place first angle θtAfterwards, according to
Path model- following control algorithmIt is mobile to next motor point to control mobile robot, specific steps and step S205 substantially phase
Together, herein without repeating.
Step S207: judging whether to reach home, if it is not, thening follow the steps S203.
Specifically, in this step, mobile robot is according to path model- following control algorithmIt is moved to next motor point
Afterwards, judge whether mobile robot reaches home, if it is not, then returning to step S203.
Compared with prior art, the motion control method of mobile robot provided by first embodiment of the invention, will
Motion path is split as multiple motor points, by seeking curvature of the motion path at each motor point, and according to mobile machine
Curvature control mobile robot at the motor point that people is presently in is mobile to next motor point, since mobile robot is from one
The curvature control in the motor point that the moving process in a motor point towards next motor point is presently according to mobile robot,
It is higher to control precision, to promote the precision that mobile robot is moved along motion path.Since each motor point is calculated
Therefore the curvature at place is the motion path of on-fixed curvature, mobile robot can still keep higher walking precision.
Second embodiment of the present invention is related to a kind of motion control method of mobile robot.Second embodiment and the
One embodiment is roughly the same, is in place of the main distinction: in the first embodiment, the k_cos curvature at motor point being made
For the curvature at motor point.And in second embodiment of the invention, using the U chord length curvature at motor point as motor point at
Curvature.In addition, it will be understood by those skilled in the art that the k_cos curvature at motor point as the curvature at motor point and is incited somebody to action
U chord length curvature at motor point is only two kinds of concrete applications citing of the invention as the curvature at motor point, of the invention
In other embodiment, it can also be other calculation methods, herein without exhaustion.Specific steps are as shown in Figure 4, comprising:
Step S401: the motion path of mobile robot is obtained.
Step S402: motion path is split as multiple motor points.
Specifically, the step S401 and step S402 in present embodiment and the step S201 and step in first embodiment
Rapid S202 is roughly the same, is no longer repeated herein.
Step S403: the U chord length curvature that mobile robot is presently in motor point is obtained, using U chord length curvature as movement
Robot is presently in the curvature in motor point.
In this step, the U chord length curvature for being presently in motor point by calculating mobile robotBy U chord length curvature
The curvature in motor point is presently in as mobile robot.Obtain the step that mobile robot is presently in the U chord length curvature of position
It is rapid as shown in Figure 5, comprising:
Step S501: establishing space coordinates, and motion path and each motor point are respectively mapped in space coordinates,
Obtain coordinate value of each motor point in space coordinates.
Step S502: determine that mobile robot is presently in the pose of position according to Monte Carlo localization algorithm.
Specifically, the step S501 and step S502 in present embodiment and the step S301 and step in first embodiment
Rapid S302 is roughly the same, is no longer repeated herein
Step S503: it is current in mobile robot that motion path is calculated according to the coordinate that mobile robot is presently in position
The U chord length curvature in locating motor point.
Specifically, setting pjTo control j-th of motor point on route,WithRespectively jth+UfA and jth-
UbA motor point, then the U chord length curvature at j pointAre as follows:
Wherein, ULTo determine constant parameter,RespectivelyWithIn space
Coordinate in coordinate system.PointAnd pointFor the point on motor point or two motor point lines, It is bent for the U chord length at j-th of motor point
The symbol of rate,For jth+UfA motor point and jth-UbEuclidean distance between a motor point.
It should be noted that ULIt can flexibly be chosen according to actual needs, herein without limiting.
Further,WithRespectively pjPoint traverses forward point and traverses backward a little, and pointAnd pointMeet constraint:PointDetermination method are as follows: with point pjForward time for starting point
It goes through, if meeting | | pjpj+k||<ULAnd | | pjpj+k+1||>ULThen stop traversing.If It solvesT can be found out, to obtainCoordinate value.If | | pjpj+k| |=ULOr | | pjpj+k+1| |=UL, thenTake pj+kOr pj+k+1.According to identical
Method can acquire a littleDetails are not described herein.
It is understood that the above-mentioned specific formula to calculate the U chord length curvature on moving line at j-th of motor point,
U chord length curvature on motion path at each motor point can be calculated by the above method.
Step S404: judge that mobile robot is presently in the U chord length curvature of position and whether is greater than preset value, if it is not, then
Step S405 is executed, if so, thening follow the steps S406.
Step S405: mobile to next motor point according to path model- following control algorithm, control mobile robot.
Step S406: control mobile robot stops movement, and according to rotating in place, control algolithm, control mobile robot are former
Ground rotates first angle.
Step S407: judging whether to reach home, if it is not, thening follow the steps S403.
Specifically, step S404 to the step S407 and step S204 in first embodiment in present embodiment is extremely walked
Rapid S207 is roughly the same, is no longer repeated herein.
Compared with prior art, the motion control method of mobile robot provided by present embodiment, by motion path
Multiple motor points are split as, by seeking U chord length curvature of the motion path at each motor point as the reality at the motor point
Border curvature, and the U chord length curvature control mobile robot at the motor point being presently according to mobile robot is to next fortune
Dynamic point movement, since the moving process in mobile robot from motor point towards a next motor point is worked as according to mobile robot
The U chord length curvature control in preceding locating motor point, control precision is higher, moves to promote mobile robot along motion path
Precision.Since the U chord length curvature at each motor point is calculated, it is the motion path of on-fixed curvature, it is mobile
Robot can still keep higher walking precision.
The step of various methods divide above, be intended merely to describe it is clear, when realization can be merged into a step or
Certain steps are split, multiple steps are decomposed into, as long as including identical logical relation, all in the protection scope of this patent
It is interior；To adding inessential modification in algorithm or in process or introducing inessential design, but its algorithm is not changed
Core design with process is all in the protection scope of the patent.
Third embodiment of the invention is related to a kind of motion control device of mobile robot, as shown in Figure 6, comprising: packet
It includes: at least one processor 601；And the memory 602 with the communication connection of at least one processor 601；Wherein, memory
602 are stored with the instruction that can be executed by least one processor 601, and instruction is executed by least one processor 601, so that at least
One processor 601 is able to carry out the motion control method such as above-mentioned mobile robot.
Wherein, memory 602 is connected with processor 601 using bus mode, and bus may include any number of interconnection
Bus and bridge, bus is by one or more processors 601 together with the various circuit connections of memory 602.Bus may be used also
With by such as peripheral equipment, voltage-stablizer, together with various other circuit connections of management circuit or the like, these are all
It is known in the art, therefore, it will not be further described herein.Bus interface provides between bus and transceiver
Interface.Transceiver can be an element, be also possible to multiple element, such as multiple receivers and transmitter, provide for
The unit communicated on transmission medium with various other devices.The data handled through processor 601 pass through antenna on the radio medium
It is transmitted, further, antenna also receives data and transfers data to processor 601.
Processor 601 is responsible for management bus and common processing, can also provide various functions, including timing, periphery connects
Mouthful, voltage adjusting, power management and other control functions.And memory 602 can be used for storage processor 601 and execute
Used data when operation.
It will be understood by those skilled in the art that the respective embodiments described above are to realize specific embodiments of the present invention,
And in practical applications, can to it, various changes can be made in the form and details, without departing from the spirit and scope of the present invention.
Claims (8)
1. a kind of motion control method of mobile robot characterized by comprising
Obtain the motion path of the mobile robot；
The motion path is split as multiple motor points；
Calculate curvature of the motion path at each motor point；
Mobile robot described in the curvature control at motor point being presently according to the mobile robot is to next described
Motor point is mobile, until the mobile robot reaches the terminal of the motion path；
Wherein, the curvature for calculating the motion path at each motor point, specifically includes:
Space coordinates are established, the motion path and the multiple motor point are mapped in the space coordinates；
Obtain coordinate value of each motor point in the space coordinates；
According to the coordinate value in each motor point, curvature of the motion path at each motor point is calculated；
Wherein, mobile robot described in the curvature control at the motor point being presently according to the mobile robot is downward
One motor point is mobile, specifically includes:
The curvature at the motor point that the mobile robot is presently in is obtained as current curvature；
Judge whether the current curvature is greater than preset value；
If it is not, then mobile to next motor point according to path model- following control algorithm, the control mobile robot；
If so, according to control algolithm is rotated in place, after the control mobile robot rotates in place first angle, according to described
Path model- following control algorithm controls the mobile robot to movement at next motor point.
2. the motion control method of mobile robot according to claim 1, which is characterized in that described according to each described
The coordinate value in motor point calculates curvature of the motion path at each motor point, specifically includes:
According to the coordinate value in each motor point, the k_cos curvature at each motor point is calculated；
Curvature using the k_cos curvature at each motor point as the motion path at each motor point.
3. the motion control method of mobile robot according to claim 2, which is characterized in that described according to each described
Coordinate value corresponding to motor point calculates the k_cos curvature at each motor point, specifically includes:
Calculate the k_cos curvature at each motor pointWherein,
pi、pi-k、pi+kI-th of motor point, the i-th-k motor points and the i-th+k institutes on the respectively described motion path
State coordinate of the motor point in the space coordinates.
4. the motion control method of mobile robot according to claim 1, which is characterized in that described according to each described
Coordinate value corresponding to motor point calculates curvature of the motion path at each motor point, specifically includes:
According to coordinate value corresponding to each motor point, the U chord length curvature at each motor point is calculated；
Curvature using the U chord length curvature at each motor point as the motion path at each motor point.
5. the motion control method of mobile robot according to claim 4, which is characterized in that described according to each described
Coordinate value corresponding to motor point calculates the U chord length curvature at each motor point, specifically includes:
Calculate the U chord length curvature at each motor pointWherein,
ULTo determine constant parameter,Respectively jth+UfA point and jth-UbA point is in institute
State the coordinate in space coordinates, jth+UfA point and jth-UbA point is the point on motor point or two motor point lines,For the U string at j-th of motor point
The symbol of long curvature,For jth+UfA motor point and jth-UbEuclidean between a motor point away from
From.
6. the motion control method of mobile robot according to claim 1, which is characterized in that the judgement is described current
Whether curvature is greater than preset value, specifically includes:
It obtains in a N neighborhood in the motor point that the mobile robot is presently in, in addition to the motor point being presently in
Each motor point at curvature, wherein N is integer greater than 0；
Judge whether the current curvature is simultaneously greater than the pre- of maximum value in the curvature at preset threshold and each motor point
If multiple；
If so, the current curvature is greater than preset value, if it is not, then the current curvature is not more than preset value.
7. the motion control method of mobile robot according to claim 1, which is characterized in that described to be followed according to path
Control algolithm, the control mobile robot are mobile to next motor point, specifically include:
Obtain the current pose p of the mobile robot1(x1,y1,θ1) and next motor point at object pose p2
(x2,y2,θ2), wherein x1,y1For the coordinate value in the motor point being presently in, x2,y2For next fortune
The coordinate value of dynamic point, θ1Direction for the mobile robot in the current pose, θ2It is the mobile robot next
The target direction in a motor point；
Displacement error e is calculated according to the current pose and the object pose1(ex,ey,eθ), wherein ex=x1-x2, ey
=y1-y2, eθ=θ1-θ2；
The linear velocity v and angular speed w of the mobile robot are sought according to the displacement error, wherein
vrLinear velocity is referred to be preset,For the curvature of current location, l1、l2、l3, β be respectively the first parameter preset, second
Parameter preset, third parameter preset and the 4th parameter preset；
It is mobile to next motor point according to the linear velocity and angular speed to control the mobile robot.
8. a kind of motion control device of mobile robot characterized by comprising
At least one processor；And
The memory being connect at least one described processor communication；Wherein,
The memory is stored with the instruction that can be executed by least one described processor, and described instruction is by described at least one
It manages device to execute, so that at least one described processor is able to carry out such as the mobile robot as described in any in claim 1 to 7
Motion control method.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910119532.XA CN109557929B (en) | 2019-02-18 | 2019-02-18 | The motion control method and device of mobile robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910119532.XA CN109557929B (en) | 2019-02-18 | 2019-02-18 | The motion control method and device of mobile robot |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109557929A CN109557929A (en) | 2019-04-02 |
| CN109557929B true CN109557929B (en) | 2019-07-05 |
Family
ID=65874164
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910119532.XA Active CN109557929B (en) | 2019-02-18 | 2019-02-18 | The motion control method and device of mobile robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109557929B (en) |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111158368B (en) * | 2019-12-31 | 2024-02-02 | 深圳市优必选科技股份有限公司 | Biped robot and track following method and device thereof |
| CN111665845B (en) * | 2020-06-24 | 2023-09-22 | 阿波罗智能技术(北京)有限公司 | Method, apparatus, device and storage medium for planning path |
| TWI780468B (en) | 2020-08-13 | 2022-10-11 | 國立陽明交通大學 | Method and system of robot for human following |
| CN112847434B (en) * | 2020-12-30 | 2022-06-17 | 深兰智能科技(上海)有限公司 | Control method and device for robot chassis, robot chassis and storage medium |
| CN113608525A (en) * | 2021-06-23 | 2021-11-05 | 北京迈格威科技有限公司 | Robot motion state indicating method and device, electronic equipment and storage medium |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105739505A (en) * | 2016-04-13 | 2016-07-06 | 上海物景智能科技有限公司 | Path control method and system for robot |
| CN105867379A (en) * | 2016-04-13 | 2016-08-17 | 上海物景智能科技有限公司 | Method and system for controlling motion of robot |
| CN107390691A (en) * | 2017-07-28 | 2017-11-24 | 广东嘉腾机器人自动化有限公司 | A kind of AGV path following methods |
| CN107672585A (en) * | 2017-08-29 | 2018-02-09 | 广州小鹏汽车科技有限公司 | A kind of automatic parking paths planning method and system |
| CN108205318A (en) * | 2017-11-30 | 2018-06-26 | 香港中文大学（深圳） | Method for planning track of robot and device |
| CN108415413A (en) * | 2018-03-28 | 2018-08-17 | 华南农业大学 | A kind of intelligent forklift part obstacle-avoiding route planning method based on round region of interest |
| CN108673496A (en) * | 2018-05-02 | 2018-10-19 | 苏州智伟达机器人科技有限公司 | A kind of industrial robot path following method based on pure tracing model |
-
2019
- 2019-02-18 CN CN201910119532.XA patent/CN109557929B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105739505A (en) * | 2016-04-13 | 2016-07-06 | 上海物景智能科技有限公司 | Path control method and system for robot |
| CN105867379A (en) * | 2016-04-13 | 2016-08-17 | 上海物景智能科技有限公司 | Method and system for controlling motion of robot |
| CN107390691A (en) * | 2017-07-28 | 2017-11-24 | 广东嘉腾机器人自动化有限公司 | A kind of AGV path following methods |
| CN107672585A (en) * | 2017-08-29 | 2018-02-09 | 广州小鹏汽车科技有限公司 | A kind of automatic parking paths planning method and system |
| CN108205318A (en) * | 2017-11-30 | 2018-06-26 | 香港中文大学（深圳） | Method for planning track of robot and device |
| CN108415413A (en) * | 2018-03-28 | 2018-08-17 | 华南农业大学 | A kind of intelligent forklift part obstacle-avoiding route planning method based on round region of interest |
| CN108673496A (en) * | 2018-05-02 | 2018-10-19 | 苏州智伟达机器人科技有限公司 | A kind of industrial robot path following method based on pure tracing model |
Non-Patent Citations (1)
| Title |
|---|
| U弦长曲率：一种离散曲率计算方法;郭娟娟;《模式识别与人工智能》;20140831;第27卷(第8期);第684-686页 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN109557929A (en) | 2019-04-02 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109557929B (en) | The motion control method and device of mobile robot | |
| CN110497901B (en) | Parking space automatic searching method and system based on robot VSLAM technology | |
| CN110000786B (en) | Historical map utilization method based on visual robot | |
| CN109579849A (en) | Robot localization method, apparatus and robot and computer storage medium | |
| CN109282822B (en) | Storage medium, method and apparatus for constructing navigation map | |
| US11877716B2 (en) | Determining region attribute | |
| EP3660618A1 (en) | Map construction and positioning of robot | |
| CA2392231C (en) | Autonomous multi-platform robot system | |
| CN111240319A (en) | Outdoor multi-robot cooperative operation system and method thereof | |
| CN108230247B (en) | Generation method, device, equipment and the computer-readable storage medium of three-dimensional map based on cloud | |
| CN111324848B (en) | Vehicle-mounted track data optimization method of mobile laser radar measurement system | |
| CN110895334A (en) | Unmanned sweeper calibration device and method based on laser radar and GPS fusion virtual wall | |
| CN111152266B (en) | Control method and system of cleaning robot | |
| CN112444246B (en) | Laser fusion positioning method in high-precision digital twin scene | |
| Fossel et al. | 2D-SDF-SLAM: A signed distance function based SLAM frontend for laser scanners | |
| Leonard et al. | Towards robust data association and feature modeling for concurrent mapping and localization | |
| CN113674351A (en) | Robot and drawing establishing method thereof | |
| Tanaka et al. | Incremental ransac for online relocation in large dynamic environments | |
| CN116105742A (en) | Composite scene inspection navigation method, system and related equipment | |
| CN112198876A (en) | Map-contained full-coverage sweeping modular control method suitable for sweeping robot | |
| CN113947623A (en) | Roughness detection method, SLAM positioning method and construction method | |
| CN112923927A (en) | Robot scanning map processing method, device, terminal and storage medium | |
| Ahmadi et al. | HDPV-SLAM: Hybrid depth-augmented panoramic visual SLAM for mobile mapping system with tilted LiDAR and panoramic visual camera | |
| Mazl et al. | Building a 2D environment map from laser range-finder data | |
| CN116429121A (en) | Positioning method and device based on multiple sensors, self-mobile device and storage medium |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| TR01 | Transfer of patent right | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20220424 Address after: 213000 No. 8, Hongyang Road, Tianning District, Changzhou City, Jiangsu Province Patentee after: DEEPBLUE ROBOT (CHANGZHOU) Co.,Ltd. Address before: 213611 room 103, building 4, chuangyangang, Changzhou science and Education City, No. 18, changwuzhong Road, Wujin District, Changzhou City, Jiangsu Province Patentee before: SHENLAN ARTIFICIAL INTELLIGENCE CHIP RESEARCH INSTITUTE (JIANGSU) Co.,Ltd. Patentee before: DEEPBLUE TECHNOLOGY (SHANGHAI) Co.,Ltd. Patentee before: DEEPBLUE ROBOT (CHANGZHOU) Co.,Ltd. |
|
| TR01 | Transfer of patent right | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20220801 Address after: 476000 shop 301, office building, northeast corner, intersection of Bayi Road and Pingyuan Road, Liangyuan District, Shangqiu City, Henan Province Patentee after: Shenlan robot industry development (Henan) Co.,Ltd. Address before: 213000 No. 8, Hongyang Road, Tianning District, Changzhou City, Jiangsu Province Patentee before: DEEPBLUE ROBOT (CHANGZHOU) Co.,Ltd. |