Robot local paths planning methodTechnical field
The present invention relates to robot system, especially a kind of robot local paths planning method.
Background technology
The development of Robot industry, the production and living to the mankind bring very big help, while human demand is increasinglyRaising is also the power of Robot industry development, and the two complements each other.Operability and mobility are the most basic functions of robotConstitute.The tradition machinery arm commonly used in industrial production, position is fixed, and working space is limited, and the expansion to mechanical arm function has veryBig restriction.And common wheeled robot at present, operational capacity is also required to further reinforcement.Just because of this, mechanically movingArm arises at the historic moment, not only the locomotivity with wheeled robot, also the operating characteristics with mechanical arm, has become robotThe trend of development.Resulting mechanical arm system and the motion planning of mobile chassis system, to the work energy that robot is overallConsumption has very big influence.
Common planning mode, such as mechanical arm and mobile chassis integrated planning mode, although taken into full account mechanically movingThe Holonomic Dynamics model of arm, but controller design is complex.Meanwhile, the energy consumption of mobile chassis will be far longer than mechanical armThe energy consumption of system, by the way of integrated planning, the energy consumption for causing robot total system is increased.Moreover, applied to officeThe service robot of living scene, its task is relatively simple, it is only necessary to which mechanical arm is moved into suitable working space, completion pairThe crawl or placement of target object, the increase by workload is caused is planned using complex monoblock type.And use machineThe mode that tool arm is planned respectively with two independent subsystems of mobile chassis, it focuses on how choosing suitable pedestalPosition, so that mechanical arm realizes good crawl function.
To realize the selection of suitable base position, generally there are the local paths such as Grid Method, Artificial Potential Field Method, genetic algorithm ruleThe technology of drawing.And these conventional methods there are problems that the response time it is long, it cannot be guaranteed that path.
The content of the invention
It is an object of the invention to overcome the deficiencies in the prior art, there is provided a kind of local paths planning side of robotMethod, the method for closed loop detection is carried out using vision, and mechanical arm is separated with the planning of mobile chassis, by mobile bottomThe different planning modes of disk and mechanical arm are combined, and realize the overall coordinate operation of robot.The technical solution adopted by the present inventionIt is：
A kind of robot local paths planning method, including following two parts：
(1) closed loop detection is carried out using vision subsystem：Robot arm subsystem is separated with mobile chassis subsystem,Closed loop detection control is carried out by vision subsystem, realizes and crawl of the mechanical arm to target object is completed in optimal base positionOr place；
(2) local paths planning being combined by Artificial Potential Field Method with RRT algorithms, realizes mobile chassis and mechanical arm pointFrom planning；When carrying out local paths planning, mobile chassis is planned using Artificial Potential Field Method, and on local road each timeAfter the planning of footpath, mechanical arm is planned by RRT algorithms, judges that the action of avoiding obstacles can be completed and smoothly captures meshMark object.
Further, described (1) is partly specifically included：
First, robot is moved to target object near zone；Now, target object is known by vision subsystemNot with the calibration of positioning, draw more accurate position, be converted to the work coordinate system of mechanical arm, and judge target object whetherIn the Work Space Range that mechanical arm can be captured；If target object is in the Work Space Range that mechanical arm can be captured, directlyGrasping movement is completed by mechanical arm, then performs subsequent action；If beyond working space, whether judging target object frontThere is barrier to block, during clear, pass through the work sky moved horizontally to make target object be in mechanical arm of mobile chassisBetween in the range of；If barrier needs the local paths planning for carrying out robot beyond the scope of mechanical arm itself avoidanceAvoidance is completed, by the closed loop of vision system detects whether can complete grasping movement after judging local paths planning each time；Each time carry out local paths planning when, can all add 1 to counter, when counter value beyond setting threshold value when, thenFed back to user, explanation can not complete currently to capture task.
Further, mobile chassis is planned using Artificial Potential Field Method, is specifically included：
Using target object as the gravitational field in Artificial Potential Field, barrier is as repulsion, and Artificial Potential Field Method is by robotMotion be assumed to be gravitation and repulsion and interact the result made a concerted effort produced, work as in this way to search out one from robotOptimal path of the front position to target location；
The mathematical description of Artificial Potential Field Method be formulated it is as follows, wherein, UrepRepresent repulsion field function, UattRepresent gravitationField function；DR-ORepresent the distance of robot and barrier, DSafeRepresent the safe distance not collided set, ΔrepForRepulsion gain, ΔattFor gravitational field gain, UapFor Artificial potential functions, DR-TFor the distance of robot and target object, FJoinVirtual for hypothesis is made a concerted effort, and FJoinIt is identical with the negative value of Artificial potential functions gradient；Obtain FJoinMinimum value, be localThe optimal path that path planning is drawn；
Uatt=DR-T2Δatt/ 2 formula (2)
Uap=Uatt+UrepFormula (3)
FJoin=-UapFormula (4).
Further, planning is carried out to mechanical arm by RRT algorithms to specifically include：
Mechanical arm current location is initial point, initial point x0As root node, search tree T is generated, with Probability p0Do not arriveStochastical sampling selection pose point x in the working space reachedrandFor destination node, tree T growth is realized；Afterwards, select tree T inxrandClosest node xnear, and make tree T according to xnearPoint to xrandDirection is grown, and produces new node xnew；IfBarrier is run into growth course, then is stopped growing, stochastical sampling point is reselected；Move in circles, when growing into crawl targetThe pose point x of objecttargetWhen, terminate the search procedure of random tree；
If in search procedure, it is impossible to find the pose path of the crawl point from current pose to target object, then instruction sheetSolely by the motion of mechanical arm itself, it is impossible to complete avoidance, it is necessary to carry out local paths planning to mobile chassis, be adjusted to moreSuitable pose, re-starts the structure of search tree.
The advantage of the invention is that：
(1) method of closed loop detection is carried out using vision, and mechanical arm is separated with the planning of mobile chassis, both may be usedMake, to the more accurate of the position acquisition of target object, to improve the success rate of crawl by constantly calibrating, again can be to greatest extentReduce the overall energy consumption of robot.
(2) it is combined using Artificial Potential Field Method and RRT methods, overcomes deficiency when every kind of method is individually used, be used inAfter each vision subsystem is calibrated, can mechanical arm smoothly complete the judgement of crawl target object, and offer needs to carry outSolution when local paths planning is finely tuned.It is combined, is realized by the different planning modes to mobile chassis and mechanical armThe overall coordinate operation of robot, completes crawl task.
Brief description of the drawings
Fig. 1 is mobile mechanical arm workflow diagram of the invention.
Fig. 2 is RRT algorithm flow charts of the invention.
Embodiment
With reference to specific drawings and examples, the invention will be further described.
With reference to specific drawings and examples, the invention will be further described.
Robot local paths planning method proposed by the present invention, mainly including following two large divisions：
(1) closed loop detection technique is carried out using vision subsystem；
Robot arm subsystem is separated with mobile chassis subsystem, carrying out closed loop detection by vision subsystem controlsSystem, realizes and crawl or placement of the mechanical arm to target object is completed in optimal base position, while reducing robot systemEnergy consumption.
For robot distance objective object farther out, low to its positioning precision the problem of, proposition is entered using vision subsystemRow closed loop detects that robot is leaned on into after target object, and calibration is identified to its position again；And in local path each timeAfter planning, repositioning calibration is carried out, the position detection accuracy to target object is improved, and improve the success of mechanical arm crawlRate.
As shown in figure 1, first, robot is moved to target object near zone；Now, by vision subsystem to targetThe calibration with positioning is identified in object, draws more accurate position, is converted to the work coordinate system of mechanical arm, and judges targetWhether object is in the Work Space Range that mechanical arm can be captured；If the Work Space Range that target object can be captured in mechanical armIt is interior, then grasping movement directly can be completed by mechanical arm, then perform subsequent action；If beyond working space, judging targetWhether there is barrier to block in front of object, during clear, can be in target object by moving horizontally for mobile chassisIn the Work Space Range of mechanical arm；If barrier beyond mechanical arm can itself avoidance scope, need carry out robotLocal paths planning complete avoidance, detected by the closed loop of vision system, judge after local paths planning each time whetherGrasping movement can be completed；When carrying out local paths planning each time, can all it add 1 to counter, when the value of counter exceedsDuring the threshold value of setting, then fed back to user, explanation can not complete currently to capture task, the follow-up life of wait userOrder.
(2) local paths planning being combined by Artificial Potential Field Method with RRT algorithms, realizes mobile chassis and mechanical arm pointFrom planning；
Energy consumption for mobile chassis subsystem and the planning mode of robot arm subsystem, and mobile chassis is far longer thanThe problem of energy consumption of robot arm subsystem, in terms of the cooperation of chassis and arm, propose based on manipulator motion, mobile chassisSupplemented by motion, that is, select the mode of subsystem separating planning.Mobile chassis is mainly responsible for the pose of adjustment mechanical arm grasping movement,Closed loop detection is constantly carried out by vision subsystem, judge mobile chassis move to crawl object best base seat postpone, machineTool arm starts to perform the action of crawl again.
When carrying out local paths planning, mobile chassis is planned using Artificial Potential Field Method, and in part each timeAfter path planning, plan that mechanical arm can judgement complete avoiding obstacles by RRT (Quick Extended random tree) algorithmAction and smoothly capture target object；
Using target object as the gravitational field in Artificial Potential Field, barrier is as repulsion, in the distance apart from barrierDuring diminution, robot is by by bigger repulsive force, and when distant, target object is by with bigger gravitation；Artificial gestureThe motion of robot is assumed to be gravitation and interacted with repulsion the result with joint efforts that produces by method, searches out one in this wayOptimal path of the bar from robot current location to target location；After new target location is reached, by RRT algorithms to machineryArm be operated space planning analysis, determine whether can avoiding obstacles realize crawl object optimal base position, thusDetermine the action of next step.
The mathematical description of Artificial Potential Field Method be formulated it is as follows, wherein, UrepRepresent repulsion field function, UattRepresent gravitationField function；DR-ORepresent the distance of robot and barrier, DSafeRepresent the safe distance not collided set, ΔrepForRepulsion gain, ΔattFor gravitational field gain, UapFor Artificial potential functions, DR-TFor the distance of robot and target object, FJoinVirtual for hypothesis is made a concerted effort, and FJiinIt is identical with the negative value of Artificial potential functions gradient；Obtain FJoinMinimum value, be localThe optimal path that path planning is drawn；
Uatt=DR-T2Δatt/ 2 formula (2)
Uap=Uatt+UrepFormula (3)
FJoin=-UapFormula (4)
Artificial Potential Field Method realizes the local paths planning function in Fig. 1 flow charts.Meanwhile, Artificial Potential Field is being carried out each timeAfter the analysis of method, add 1 to counting variable cnt, as threshold value δs of the cnt beyond setting, that is, illustrate that robot can not be completed to specifyingThe crawl of Place object object, is fed back to host computer and user, is prepared to receive follow-up instruction, is acted accordingly.
Mechanical arm carries out separating planning with mobile chassis, using RRT algorithms, realizes that the mechanical arm in Fig. 1 itself judges energyThe no function of completing avoidance：The flow of RRT algorithms is as shown in Figure 2；
Mechanical arm current location is initial point, initial point x0As root node, search tree T is generated, with Probability p0Do not arriveStochastical sampling selection pose point x in the working space reachedrandFor destination node, tree T growth is realized；Afterwards, select tree T inxrandClosest node xnear, and make tree T according to xnearPoint to xrandDirection is grown, and produces new node xnew；IfBarrier is run into growth course, then is stopped growing, stochastical sampling point is reselected；Move in circles, when growing into crawl targetThe pose point x of objecttargetWhen, terminate the search procedure of random tree.
If in search procedure, it is impossible to find the pose path of the crawl point from current pose to target object, then instruction sheetSolely by the motion of mechanical arm itself, it is impossible to complete avoidance, it is necessary to carry out local paths planning to mobile chassis, be adjusted to moreSuitable pose, re-starts the structure of search tree, that is, captures the selection of track.