CN105437232B - A kind of method and device of control multi-joint Mobile Robot Obstacle Avoidance - Google Patents
A kind of method and device of control multi-joint Mobile Robot Obstacle Avoidance Download PDFInfo
- Publication number
- CN105437232B CN105437232B CN201610014945.8A CN201610014945A CN105437232B CN 105437232 B CN105437232 B CN 105437232B CN 201610014945 A CN201610014945 A CN 201610014945A CN 105437232 B CN105437232 B CN 105437232B
- Authority
- CN
- China
- Prior art keywords
- joint
- robot
- coordinate system
- theta
- under
- 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
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1674—Programme controls characterised by safety, monitoring, diagnostic
- B25J9/1676—Avoiding collision or forbidden zones
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
- B25J19/06—Safety devices
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention provides a kind of method and device of control multi-joint Mobile Robot Obstacle Avoidance, the method includes：During robot is moved according to the global optimum path of advance planning from starting point to impact point, using vision guided navigation and the mutually coordinated control robot motion of infrared obstacle avoidance, when having barrier in vision system detects predeterminable range, suspend the advance of robot base, with reference to the kinematics geometrical model in each joint for pre-building, the automatic athletic posture for adjusting each joint step by step, the swinging joint in the feasible zone in each joint, from end effector to base, avoiding obstacles step by step, pose and bobbin movement direction by each joint of servo system control, continue to control robot to be moved to impact point, until robot reaches impact point, adjust the athletic posture of each joint and base simultaneously during infrared obstacle avoidance, avoid subsequently adjusting repeatedly, improve the avoidance efficiency of multi-joint mobile robot.
Description
Technical field
The present invention relates to robotics, in particular to a kind of control multi-joint Mobile Robot Obstacle Avoidance
Method and device.
Background technology
At present, with the fast development and the continuous propulsion of social intelligence of social economy, intelligent robot meets the tendency of therewith
And give birth to, intelligent robot is can be in road and field continuously paleocinetic robot in real time, it has also become current science and technology is ground
Study carefully the focus in field, main purpose is that artificial intelligence technology is applied in research, so that intelligent robot is in environment complicated and changeable
Under realize autonomous reasoning, planning and control；In addition, as expanding day by day for robot application scope is progressively strengthened with intellectuality,
Intelligent robot gradually develops to the direction personalized, thus, multi-joint mobile robot application is more and more extensive, many in complexity
Multi-joint mobile robot is likely encountered barrier at any time in the process of moving in the environment of change, accordingly, it would be desirable to enter to robot
The planning in row avoidance path makes its avoiding obstacles, and then smoothly arrives at.
Currently, a kind of method for controlling multi-joint Mobile Robot Obstacle Avoidance is provided in correlation technique, the method includes：It is first
First, global path planning is carried out to robot, that is, controls robot to move to final position by original position, by robot
Vision system collects the surrounding environment of motion path and the image information of fixed obstacle, generates the environment of whole motion path
Image, global path is determined according to the ambient image for collecting by shortest path first；Then, robot is from source location set
Set out, by installing infrared induction sensor on robot base, when infrared induction sensor has detected barrier, rule
The feasible zone that is moved near global point of base avoiding obstacles of exploring robot is drawn, but only considered the base of robot
Motion and avoidance, the avoidance for the multidimensional multi-joint of spatially robot are not planned, because existing multi-joint is moved
The infrared sensor and vision system of mobile robot are installed on the base of robot, when robot runs into barrier, only
Can be by changing the direction of motion of base come avoidance, and the adjustment of athletic posture merely ensures that base can be with avoidance, nothing every time
Method ensures the surely avoidance of mechanical arm one, so that the athletic posture of base needs to adjust repeatedly.
Realize it is of the invention during, inventor find correlation technique at least there is problems with：In correlation technique
When robot perception is to barrier, avoidance treatment only is carried out to base, cause each joint will be with during continuing to move to
Barrier produces collision, so as to need constantly to adjust driving path, the avoidance path degree of accuracy that planning is obtained is low, both increased tune
The whole time, avoidance efficiency is reduced again, it is impossible to which realization makes multi-joint mobile robot rapidly and accurately avoiding obstacles.
The content of the invention
In view of this, the purpose of the embodiment of the present invention is to provide a kind of method for controlling multi-joint Mobile Robot Obstacle Avoidance
And device, to avoid subsequently adjusting repeatedly, improve the avoidance efficiency of multi-joint mobile robot.
In a first aspect, the embodiment of the invention provides a kind of method for controlling multi-joint Mobile Robot Obstacle Avoidance, the method
Including：
Above-mentioned institute of robot is gathered by the vision system set on the end effector of multi-joint mobile robot in place
The ambient image put, and above-mentioned robot is controlled according to the global optimum road of advance planning according to the above-mentioned ambient image for collecting
Movement of the footpath from starting point to impact point；Wherein, each joint of above-mentioned robot is to that should have kinematics geometrical model, above-mentioned fortune
Dynamic geometrical model of learning is the relative motion by analyzing above-mentioned each joint of robot, is pre-build using classical D-H methods；
During above-mentioned robot is moved, when above-mentioned vision system is detected has barrier in predeterminable range, temporarily
Stop the advance of above-mentioned robot base；
The corresponding local coordinate system of above-mentioned kinematics geometrical model is obtained, under above-mentioned local coordinate system, according to above-mentioned machine
Spacing, rod length, rod member torsional angle between the corresponding corner in each joint of device people, adjacent segment, and detect above-mentioned barrier
Vision system position adjust the corner in above-mentioned each joint of robot, until above-mentioned barrier is avoided in each joint；
Above-mentioned corner after according to regulation determines the athletic posture in each joint under global coordinate system；
The ambient image of above-mentioned robot current location is gathered by above-mentioned vision system, and according to the above-mentioned of each joint
Athletic posture adjusts the global optimum path that above-mentioned robot is moved to above-mentioned impact point, according to the above-mentioned global optimum after adjustment
Path continues to control above-mentioned robot to be moved to above-mentioned impact point, until above-mentioned robot reaches above-mentioned impact point.
With reference in a first aspect, the embodiment of the invention provides the first possible implementation method of first aspect, wherein, on
Above-mentioned corner after stating according to regulation determines that the athletic posture in each joint under global coordinate system includes：
Determine neat between adjacent coordinates system in the local coordinate system that the corresponding kinematics geometrical model in each joint is set up
Secondary transformation matrix formula
Wherein, diRepresent distance, θ between joint i and joint i-1iRepresent the corner of joint i, aiRepresent joint i and joint i-1
Between rod length, αiRepresent the rod member torsional angle of joint i, Transz(di) represent i-1 coordinate system transformations under i coordinate systems along z-axis
Translation diThe translation matrix that distance is obtained, Rotz(θi) represent i-1 coordinate system transformations under i coordinate systems along z-axis rotate counterclockwise θi
The spin matrix that angle is obtained, Transx(ai) represent i-1 coordinate system transformations under i coordinate systems along x-axis translation aiWhat distance was obtained
Translation matrix, Rotx(αi) represent i-1 coordinate system transformations under i coordinate systems along x-axis rotate counterclockwise αiThe spin moment that angle is obtained
Battle array；
During according to above-mentioned homogeneous transform matrix formula and each joint avoiding obstacles between corresponding corner, adjacent segment between
Obtain joint i-1 coordinate systems and change to the matrix of joint i coordinate systems away from, rod length and rod member Twist Angle Calculation；
Changed according to above-mentioned joint i-1 coordinate systems to the matrix and athletic posture computing formula of joint i coordinate systemsThe athletic posture in local coordinate system hypozygal i is calculated, wherein, i represents that joint is numbered, Ti
Athletic posture of i-th joint under local coordinate system is represented,Represent the conversion that i-1 coordinate system transformations are obtained to i coordinate systems
Matrix；
According to the above-mentioned athletic posture and computing formula P in each joint under local coordinate systemi=P × TiIt is calculated complete
The athletic posture in each joint under office's coordinate system, wherein, PiRepresent athletic posture of i-th joint under global coordinate system, TiTable
Show athletic posture of i-th joint under local coordinate system, P represents multi-joint mobile robot base under global coordinate system
The matrix expression of athletic posture and
With reference to the first possible implementation method of first aspect, second of first aspect is the embodiment of the invention provides
Possible implementation method, wherein, it is above-mentioned that above-mentioned robot is adjusted to above-mentioned impact point according to the above-mentioned athletic posture in each joint
Mobile global optimum path includes：
Above-mentioned athletic posture according to each joint under global coordinate system determines above-mentioned robot under global coordinate system
Avoidance movement locus；
The global optimum path that above-mentioned robot is moved to above-mentioned impact point is adjusted according to above-mentioned avoidance movement locus.
With reference to any one implementation method in second possible implementation method of first aspect to first aspect, the present invention
Embodiment provides the third possible implementation method of first aspect, wherein, it is above-mentioned under local coordinate system, according to above-mentioned machine
Spacing, rod length, rod member torsional angle between the corresponding corner in each joint of device people, adjacent segment, and detect above-mentioned barrier
Vision system position adjust the corner in above-mentioned each joint of robot, until above-mentioned barrier bag is avoided in each joint
Include：
The joint on the end effector of above-mentioned robot is selected as the first regulation joint；
The barrier model regulation above-mentioned first set up according to vision system in the feasible zone in the above-mentioned first regulation joint
The corner in joint is adjusted, storage makes above-mentioned first regulation joint corner first corresponding to avoiding obstacles；
By the above-mentioned first regulation joint drive with the above-mentioned first adjacent joint motions in regulation joint, continue to adjust with it is above-mentioned
The corner in the adjacent joint in the first regulation joint, storage makes the joint adjacent with the above-mentioned first regulation joint avoiding obstacles first
Corresponding corner, circulates successively, until the joint for being currently needed for regulation is base.
Second aspect, the embodiment of the present invention additionally provides a kind of device of control multi-joint Mobile Robot Obstacle Avoidance, the dress
Put including：
First control module, for the vision system collection set on the end effector by multi-joint mobile robot
The ambient image of above-mentioned robot position, and above-mentioned robot is controlled according to advance according to the above-mentioned ambient image for collecting
Movement of the global optimum path of planning from starting point to impact point；Wherein, each joint of above-mentioned robot is to that should have motion
Geometrical model is learned, above-mentioned kinematics geometrical model is the relative motion by analyzing above-mentioned each joint of robot, using classics
D-H methods pre-build；
Pause module, during being moved in above-mentioned robot, when above-mentioned vision system is detected in predeterminable range
When having barrier, suspend the advance of above-mentioned robot base；
Adjustment module, for obtaining the corresponding local coordinate system of above-mentioned kinematics geometrical model, in above-mentioned local coordinate system
Under, according to spacing, rod length, rod member torsional angle between the corresponding corner in each joint of above-mentioned robot, adjacent segment, and detection
The corner in above-mentioned each joint of robot is adjusted to the vision system position of above-mentioned barrier, until each joint is avoided
Above-mentioned barrier；
Determining module, the motion appearance in each joint under global coordinate system is determined for the above-mentioned corner after according to regulation
State；
Second control module, the ambient image for gathering above-mentioned robot current location by above-mentioned vision system, and
Above-mentioned athletic posture according to each joint adjusts the global optimum path that above-mentioned robot is moved to above-mentioned impact point, according to tune
Above-mentioned global optimum path after whole continues to control above-mentioned robot to be moved to above-mentioned impact point, until in the arrival of above-mentioned robot
State impact point.
With reference to second aspect, the first possible implementation method of second aspect is the embodiment of the invention provides, wherein, on
Stating determining module includes：
First determining unit, for phase in the local coordinate system for determining the corresponding kinematics geometrical model foundation in each joint
Homogeneous transform matrix formula between adjacent coordinate system
Wherein, diRepresent distance, θ between joint i and joint i-1iRepresent the corner of joint i, aiRepresent joint i and joint i-1
Between rod length, αiRepresent the rod member torsional angle of joint i, Transz(di) represent i-1 coordinate system transformations under i coordinate systems along z-axis
Translation diThe translation matrix that distance is obtained, Rotz(θi) represent i-1 coordinate system transformations under i coordinate systems along z-axis rotate counterclockwise θi
The spin matrix that angle is obtained, Transx(ai) represent i-1 coordinate system transformations under i coordinate systems along x-axis translation aiWhat distance was obtained
Translation matrix, Rotx(αi) represent i-1 coordinate system transformations under i coordinate systems along x-axis rotate counterclockwise αiThe spin moment that angle is obtained
Battle array；
First computing unit, it is corresponding during for according to above-mentioned homogeneous transform matrix formula with each joint avoiding obstacles
Spacing, rod length and rod member Twist Angle Calculation obtain joint i-1 coordinate systems and change to joint i coordinate systems between corner, adjacent segment
Matrix；
Second computing unit, for being changed according to above-mentioned joint i-1 coordinate systems to the matrix and motion appearance of joint i coordinate systems
State computing formulaThe athletic posture in local coordinate system hypozygal i is calculated, wherein, i represents pass
Section numbering, TiAthletic posture of i-th joint under local coordinate system is represented,Represent i-1 coordinate system transformations to i coordinate systems
The transformation matrix for obtaining；
3rd computing unit, for the above-mentioned athletic posture and computing formula P in basis each joint under local coordinate systemi
=P × TiThe athletic posture in each joint under global coordinate system is calculated, wherein, PiRepresent i-th joint in global coordinate system
Under athletic posture, TiAthletic posture of i-th joint under local coordinate system is represented, P represents multi-joint mobile robot bottom
The matrix expression of athletic posture of the seat under global coordinate system and
With reference to the first possible implementation method of second aspect, second of second aspect is the embodiment of the invention provides
Possible implementation method, wherein, above-mentioned second control module includes：
Second determining unit, for determining above-mentioned robot according to the above-mentioned athletic posture in each joint under global coordinate system
Avoidance movement locus under global coordinate system；
Adjustment unit, for adjusting the overall situation that above-mentioned robot is moved to above-mentioned impact point according to above-mentioned avoidance movement locus
Optimal path.
With reference to any one implementation method in second possible implementation method of second aspect to second aspect, the present invention
Embodiment provides the third possible implementation method of second aspect, wherein, above-mentioned adjustment module includes：
Unit is chosen, for the joint on the end effector for selecting above-mentioned robot as the first regulation joint；
First adjustment unit, for the barrier set up according to vision system in the feasible zone in the above-mentioned first regulation joint
The corner in the above-mentioned first regulation joint of model regulation, storage makes above-mentioned first regulation joint turning corresponding to avoiding obstacles first
Angle；
Second adjustment unit, for driving the joint adjacent with the above-mentioned first regulation joint to transport by the above-mentioned first regulation joint
It is dynamic, continue to adjust the corner for adjusting the adjacent joint in joint with above-mentioned first, storage makes adjacent with the above-mentioned first regulation joint
Joint corner first corresponding to avoiding obstacles, circulates successively, until the joint for being currently needed for regulation is base.
The method and device of control multi-joint Mobile Robot Obstacle Avoidance provided in an embodiment of the present invention, the method includes：
During robot is moved according to the global optimum path of advance planning from starting point to impact point, using vision guided navigation and red
The outer mutually coordinated control robot motion of avoidance, when having barrier in vision system detects predeterminable range, suspends robot
The advance of base, with reference to the kinematics geometrical model in each joint for pre-building, adjusts the motion in each joint step by step automatically
Attitude, the swinging joint in the feasible zone in each joint, from end effector to base, avoiding obstacles step by step, by servo
System controls pose and the bobbin movement direction in each joint, continues to control robot to be moved to impact point, until robot is arrived
Up to impact point, the athletic posture of each joint and base is adjusted simultaneously during infrared obstacle avoidance, it is to avoid subsequently adjust repeatedly, carry
The avoidance efficiency of multi-joint mobile robot high.
To enable the above objects, features and advantages of the present invention to become apparent, preferred embodiment cited below particularly, and coordinate
Appended accompanying drawing, is described in detail below.
Brief description of the drawings
Technical scheme in order to illustrate more clearly the embodiments of the present invention, below will be attached to what is used needed for embodiment
Figure is briefly described, it will be appreciated that the following drawings illustrate only certain embodiments of the present invention, thus be not construed as it is right
The restriction of scope, for those of ordinary skill in the art, on the premise of not paying creative work, can also be according to this
A little accompanying drawings obtain other related accompanying drawings.
Fig. 1 shows a kind of flow of the method for control multi-joint Mobile Robot Obstacle Avoidance that the embodiment of the present invention is provided
Schematic diagram；
Fig. 2 a show the D-H modelings in each joint of the analysis multi-joint mobile robot that the embodiment of the present invention is provided
During adjacent segment Kinematic Model schematic diagram；
Fig. 2 b show the D-H modelings in each joint of the analysis multi-joint mobile robot that the embodiment of the present invention is provided
During adjacent x-axis it is parallel, z-axis rotate θ angles schematic diagram；
Fig. 2 c show the D-H modelings in each joint of the analysis multi-joint mobile robot that the embodiment of the present invention is provided
During by coordinate system along z-axis translate d distances, make the schematic diagram that x-axis is conllinear；
Fig. 2 d show the D-H modelings in each joint of the analysis multi-joint mobile robot that the embodiment of the present invention is provided
During it is a kind of by coordinate system along x-axis translate a distances schematic diagram；
Fig. 2 e show the D-H modelings in each joint of the analysis multi-joint mobile robot that the embodiment of the present invention is provided
During another schematic diagram that coordinate system is translated a distances along x-axis；
Fig. 2 f show the D-H modelings in each joint of the analysis multi-joint mobile robot that the embodiment of the present invention is provided
During it is a kind of z-axis is rotated around x-axis, complete from coordinate system xi-ziMove to coordinate system xi+1-zi+1Schematic diagram；
Fig. 2 g show the D-H modelings in each joint of the analysis multi-joint mobile robot that the embodiment of the present invention is provided
During it is another z-axis is rotated around x-axis, complete from coordinate system xi-ziMove to coordinate system xi+1-zi+1Schematic diagram；
Fig. 3 shows the coordinate system schematic diagram of the multi-joint mobile robot that the embodiment of the present invention is provided；
Fig. 4 show that the embodiment of the present invention provided under global coordinate system, multi-joint mobile robot is from starting point A
To the schematic diagram of the track of impact point B；
Fig. 5 shows that the multi-joint mobile robot that the embodiment of the present invention is provided moves to impact point B's from starting point A
The schematic diagram of optimal avoidance track；
Fig. 6 shows a kind of structure of the device of control multi-joint Mobile Robot Obstacle Avoidance that the embodiment of the present invention is provided
Schematic diagram.
Specific embodiment
To make the purpose, technical scheme and advantage of the embodiment of the present invention clearer, below in conjunction with the embodiment of the present invention
Middle accompanying drawing, is clearly and completely described to the technical scheme in the embodiment of the present invention, it is clear that described embodiment is only
It is a part of embodiment of the invention, rather than whole embodiments.The present invention generally described and illustrated in accompanying drawing herein is real
The component for applying example can be arranged and designed with a variety of configurations.Therefore, it is of the invention to what is provided in the accompanying drawings below
The detailed description of embodiment is not intended to limit the scope of claimed invention, but is merely representative of selected reality of the invention
Apply example.Based on embodiments of the invention, the institute that those skilled in the art are obtained on the premise of creative work is not made
There is other embodiment, belong to the scope of protection of the invention.
In view of, when robot perception is to barrier, only avoidance treatment is carried out to base in correlation technique, cause each
Joint will produce collision during continuing to move to barrier, so as to need constantly to adjust driving path, what planning was obtained keeps away
The barrier path degree of accuracy is low, both increased the time of adjustment, and avoidance efficiency is reduced again, it is impossible to which realization makes multi-joint mobile robot
Rapidly and accurately avoiding obstacles.Based on this, a kind of control multi-joint Mobile Robot Obstacle Avoidance is the embodiment of the invention provides
Method and device, is described below by embodiment.
As shown in figure 1, the embodiment of the invention provides a kind of method for controlling multi-joint Mobile Robot Obstacle Avoidance, the method
It is specific as follows including step S102-S110：
Step S102：Above-mentioned machine is gathered by the vision system set on the end effector of multi-joint mobile robot
The ambient image of people position, and above-mentioned robot is controlled according to the complete of advance planning according to the above-mentioned ambient image for collecting
Movement of office's optimal path from starting point to impact point；Wherein, each joint of above-mentioned robot is to that should have kinematics geometry mould
Type, above-mentioned kinematics geometrical model is the relative motion by analyzing above-mentioned each joint of robot, advance using classical D-H methods
Set up；
Step S104：During above-mentioned robot is moved, there is barrier in predeterminable range when above-mentioned vision system is detected
When hindering thing, suspend the advance of above-mentioned robot base；
Step S106：Obtain the corresponding local coordinate system of above-mentioned kinematics geometrical model, under above-mentioned local coordinate system, root
According to spacing, rod length, rod member torsional angle between the corresponding corner in each joint of above-mentioned robot, adjacent segment, and detect
The vision system position for stating barrier adjusts the corner in above-mentioned each joint of robot, until each joint avoids above-mentioned
Barrier；
Step S108：Above-mentioned corner after according to regulation determines the athletic posture in each joint under global coordinate system；
Step S110：The ambient image of above-mentioned robot current location is gathered by above-mentioned vision system, and according to each
The above-mentioned athletic posture in joint adjusts the global optimum path that above-mentioned robot is moved to above-mentioned impact point, according to upper after adjustment
State global optimum path to continue to control above-mentioned robot to be moved to above-mentioned impact point, until above-mentioned robot reaches above-mentioned target
Point.
In embodiments of the present invention, first, to set up corresponding kinematics to each joint of multi-joint mobile robot several
What model, while preserving respective coordinates corner of each joint under local coordinate system, the starting point from regulation is true to impact point
Vertical global optimum path；Wherein, above-mentioned kinematics geometrical model is the relative motion by each joint of analysis robot, is used
Classical D-H methods are set up, and above-mentioned kinematics geometrical model is robot motion space inner sealing geometrical model, including, physical model,
Polygonal grid model, Raster Data Model；Above-mentioned coordinate corner be multi-joint mobile robot in motion process, on teaching box show
The corner in each joint for showing, the athletic posture in each joint of robot is drawn by matrixing；Above-mentioned global optimum path
Obtained by Bugs algorithmic rules, under global context, be it is a kind of can be in the Geometric expression form of rasterizing；Above-mentioned obstacle
Thing includes on multi-joint moveable robot movement track, the people that is perceived by infrared sensor on end effector and
Object；
Then, multi-joint mobile robot is from starting point, by the two field picture of robotic vision system acquisition one,
Path is extracted in the gray level image of environment, when infrared sensor detects barrier, robot base is halted, and end is held
Row device drives adjacent segment motion, and robot base is delivered to step by step, changes the attitude in each joint, and final guided robot is each
The individual equal avoiding obstacles in joint；Wherein, above-mentioned starting point is that under global reference frame, multi-joint mobile robot is visual
Change under environment, the characteristic point represented by a rasterizing；Multi-joint relative motion is completed by servo-control system；Above-mentioned vision
System include installed in end effector of robot vision sensor, the vision sensor include laser radar, Kinect,
Realsense even depth cameras, above-mentioned vision system also includes the infrared sensor installed in end effector of robot, should
Infrared sensor is mainly used in infrared distance measurement；The athletic posture in above-mentioned each joint of robot is by setting up in multi-joint moving machine
The motion of the local coordinate system of two driving wheel midpoints of device people's base and the midpoint of the driving wheel of base two under global coordinate system
Track is established, wherein, the former by robot adjacent segment relative motion and be stored in the corner of teaching box upper joint and calculate
Go out, under the latter is with starting point as the global reference frame that origin is set up, track robot absolute path；
Next, multi-joint mobile robot is moved from starting point to impact point in the global optimum path of planning, by
Vision guided navigation complete, when perceive there is barrier in predeterminable range when, multi-joint mobile robot carries out infrared obstacle avoidance, automatically
Adjust the athletic posture of whole robot system, each joint of robot avoidance step by step, then transported to impact point under vision guided navigation
It is dynamic；Wherein, above-mentioned vision guided navigation is completed by the vision system installed in end effector；Above-mentioned infrared obstacle avoidance passes through
Infrared sensor transmitting in robotic vision system it is infrared, when having obstacle in the predeterminable range of multi-joint mobile robot
Thing, is reflected back vision system receiver, measures the distance of barrier, and analysis is compared so as to many passes with default avoidance threshold value
Section mobile robot is reacted in time to this, i.e., infrared obstacle avoidance refers to：When that will run into barrier, robot base stops
Advance, made a response by the vision system on end effector of robot, adjustment multi-joint mobile robot each
The attitude avoiding obstacles of axle, in order to ensure reliability and stability and the path planning more fairing of whole mechanical system, from
The nearer joint rotation angle of end effector is changed greatly, and the joint rotation angle change near base is smaller.
The embodiment of the present invention provides a kind of multi-joint Mobile Robot Obstacle Avoidance paths planning method, while being moved to multi-joint
The base of robot and each joint carry out kinematics analysis, first, using classical D-H methods to each of multi-joint mobile robot
Individual joint carries out Kinematic Model, sets up the kinematics geometrical model in each joint, the specified machine under global reference frame
The starting point and impact point of people's motion, by depth test, based on Bugs algorithms, obtain multi-joint mobile robot and are specifying
The global optimum path moved between initial point and impact point, if multi-joint mobile robot is held by installed in robot end
Vision system on row device perceives barrier, it is considered to which the dynamic characteristic in each joint of robot, mechanical arm is in feasible zone
Swinging joint, from end effector to base, avoiding obstacles, servo system control machine human body posture, this mistake step by step
Journey is infrared obstacle avoidance, while machine human body posture, can carry out corresponding matrixing by the corner on teaching box
Obtain.Then, multi-joint mobile robot continues to be moved towards impact point according to vision guided navigation, until reaching impact point.This hair
The method computational efficiency of the control multi-joint Mobile Robot Obstacle Avoidance that bright embodiment is provided is high, is modeled by the D-H to robot,
The movement locus in each joint when can quickly calculate robot obstacle-avoiding, it is adaptable to which multi-joint mobile robot is to complex geometry
The barrier of model carries out automatic obstacle avoiding, it is possible to achieve be efficiently multi-joint mobile robot planning avoidance path, so that many
Joint mobile robot rapidly reaches impact point in complex environment, it is ensured that robot autonomous movement and avoidance are simultaneously smoothly complete
Into preplanned mission.
Understand that compared with correlation technique, the embodiment of the present invention is on multi-joint Mobile Robot Obstacle Avoidance road with reference to above-mentioned analysis
In the planning of footpath, vision system is arranged on the end effector of mobile robot, it is mutually coordinated with vision guided navigation and infrared obstacle avoidance
The navigation of robot is controlled, and wherein to the model of any barrier, robot adjusts the motion in each joint step by step automatically
Attitude, pose and bobbin movement direction by each joint of servo system control mobile robot, in joint, swing is feasible
Optimize robot motion direction in domain, it is to avoid repeatedly adjustment of the robot in obstacle-avoiding route planning, reduce robot and keep away
Hinder the time of path planning, cooked up the avoidance path of optimization, whole process can be automatically performed, and computational efficiency is high, programming
Realize simply, can apply to multi-joint Mobile Robot Obstacle Avoidance path planning under indoor and outdoors complex environment.
Further, it is above-mentioned according to regulation after above-mentioned corner determine the athletic posture in each joint under global coordinate system
Including：
Determine neat between adjacent coordinates system in the local coordinate system that the corresponding kinematics geometrical model in each joint is set up
Secondary transformation matrix formula
Wherein, diRepresent distance, θ between joint i and joint i-1iRepresent the corner of joint i, aiRepresent joint i and joint i-1
Between rod length, αiRepresent the rod member torsional angle of joint i, Transz(di) represent i-1 coordinate system transformations under i coordinate systems along z-axis
Translation diThe translation matrix that distance is obtained, Rotz(θi) represent i-1 coordinate system transformations under i coordinate systems along z-axis rotate counterclockwise θi
The spin matrix that angle is obtained, Transx(ai) represent i-1 coordinate system transformations under i coordinate systems along x-axis translation aiWhat distance was obtained
Translation matrix, Rotx(αi) represent i-1 coordinate system transformations under i coordinate systems along x-axis rotate counterclockwise αiThe spin moment that angle is obtained
Battle array；
During according to above-mentioned homogeneous transform matrix formula and each joint avoiding obstacles between corresponding corner, adjacent segment between
Obtain joint i-1 coordinate systems and change to the matrix of joint i coordinate systems away from, rod length and rod member Twist Angle Calculation；
Changed according to above-mentioned joint i-1 coordinate systems to the matrix and athletic posture computing formula of joint i coordinate systems(the athletic posture computing formula can also be expressed as) be calculated in local coordinate
It is the athletic posture of hypozygal i, wherein, i represents that joint is numbered, TiRepresent motion appearance of i-th joint under local coordinate system
State,Represent the transformation matrix that i-1 coordinate system transformations are obtained to i coordinate systems；
Above-mentioned athletic posture under the corresponding local coordinate system in each joint is changed the motion appearance to global coordinate system
State.
Specifically, showing the D-H modeling process in each joint of analysis multi-joint mobile robot such as Fig. 2 a to Fig. 2 g
Schematic diagram.As shown in Figure 2 a, to robot adjacent segment Kinematic Model, it is necessary to preferential respectively to determine z-axis, origin, x-axis, y
Axle (negligible).Following labor whole coordinate system sets up process：
(1) generally z-axis is all each corresponding joint, either revolute or prismatic pair be not always the case, it is necessary to
It is specifically intended that the z-axis at the i of joint, its numbering is i-1.Joint number is that numbering is i for the z-axis of i+1.It is suitable by these numberings
Sequence, is also all easier to mark to all of joint variable.
(2) from the z-axis set up it is seen that, adjacent z-axis may parallel or extended line it is intersecting, more likely antarafacial,
All it is the third situation to the z-axis set up using D-H methods in the embodiment of the present invention, therefore can obtains most short between them
Distance, that is, different surface beeline common vertical line, on coordinate space, common vertical line must be conllinear with x-axis, and positive direction refers to always
Latter.For example, aiIt is zi-1With ziBetween common vertical line, then the direction of axle is directed to ai, as shown in Figure 2 b.
(3) determination of origin, joint ziWith zi-1Between common vertical line, i.e., adjacent x-axis such as schemes not in a plane
Shown in 2b.
(4) when parallel for adjacent segment, can adjacent common vertical line is conllinear, thus enormously simplify whole coordinate
Spatial model, reduces and solves the larger amount of calculation brought to transformation matrix of coordinates.
(5) for another special circumstances, for example, ziWith zi-1It is intersecting, so can determine one with two intersecting straight lines
Plane, does after vertical line to confirm corresponding x-axis.
The joint of robot and connecting rod all respectively have two variables, joint variable θ respectivelyiAnd di, link parameters αiAnd ai, such as
Shown in Fig. 2 a.
Next, using kinematic viewpoint to setting up correlation between each reference axis, the translation according to rigid body and rotation
Turn theory, from coordinate system xi-ziMove to coordinate system xi+1-zi+1。
(1) in order that adjacent x-axis is parallel, it is necessary to z-axis is rotated θiAngle, as shown in Figure 2 b.
(2) after adjacent x-axis is parallel, might not be conllinear, coordinate system is at this moment translated into d along z-axisiDistance, reaches x-axis
Collinearly, as shown in Figure 2 c.
(3) origin of adjacent coordinates system overlaps：I coordinate systems are translated into a along x-axisiDistance, as shown in Fig. 2 d and Fig. 2 e.
(4) finally find to differ an angle [alpha] between the twoi, z-axis can be completed from coordinate system x around x-axis rotationi-
ziMove to coordinate system xi+1-zi+1, as shown in Fig. 2 f and Fig. 2 g.
Analyzed more than, following multi-joint mobile robot D-H Mo Xing is set up, as shown in figure 3, wherein, 4 hold for end
The origin of row device joint coordinate system, 0 is the origin of base basis coordinates system, and 1,2,3 is the origin of the coordinate system of corresponding joint, is passed through
After the link rod coordinate system of foundation, following four parameter can be completely established：
Rod length ai：Adjacent two axles different surface beeline beeline, points to the former for just.
Rod member torsional angle αi：Angle needed for adjacent diarticular torsion is conllinear, is just around the former direction of rotation.
Joint is apart from di：The most short distance of adjacent two x-axis, points to the former for just.
Joint rotation angle θi：The angle of rotation required for adjacent two x-axis is coplanar, around current joint direction of rotation for just, and rule
Determine θi∈[-π,π)。
The final coordinate system for establishing each joint of multi-joint mobile robot, as shown in figure 3, being with four articulated robots
Example.
By as above analyzing, the homogeneous transform matrix between adjacent coordinates system can be obtainedI.e.
By taking four joint mobile robots as an example, according to above-mentioned homogeneous transform matrixObtain each joint adjacent coordinates system
Between homogeneous transform matrix, such as following formula：
And then the multi-joint mobile robot is obtained under local coordinate system, the athletic posture in joint 1,2,3,4 is respectively：
Next, determining the motion under each joint global coordinate system according to the athletic posture under each local joint coordinate
Attitude, wherein, the above-mentioned athletic posture under the above-mentioned corresponding local coordinate system by each joint is changed to global coordinate system
Athletic posture includes：
According to the above-mentioned athletic posture and computing formula P in each joint under local coordinate systemi=P × TiIt is calculated the overall situation
The athletic posture in each joint under coordinate system, wherein, PiRepresent athletic posture of i-th joint under global coordinate system, TiRepresent
Athletic posture of i-th joint under local coordinate system, P represents fortune of the multi-joint mobile robot base under global coordinate system
The matrix expression of dynamic attitude and
It should be noted that motion of the multi-joint mobile robot in circumstances not known, for the ease of analysis, by robot bottom
The plane of seat motion regards horizontal plane, and by each joint centered on base, obtains each joint phase under global coordinate system
For the absolute movement attitude of base, as shown in figure 4, wherein, θ is robot motion direction, the x-y in robot locally sits
Mark system, origin in base, by the point (x, y) in plane, i.e. coordinate position where multi-joint mobile robot base, in space
Coordinates matrix expression formula beThe expression formula first three columns of the matrix are respectively along x, y, the side of z-axis
To because z-axis is perpendicular to plane, therefore the last factor of the 3rd row is 1.
Still by taking four joint mobile robots as an example, under global coordinate system, the absolute movement of joint of robot 1,2,3,4
Attitude is：
P1=P × T1, P2=P × T2, P3=P × T3,
P4=P × T4,
Wherein, matrix P1, P2, P3, P44x4 matrixes are, the first three columns of matrix represent the motion side in each joint of robot
To last row is illustrated respectively in correspondence x, y, the mapping value above z-axis, accordingly, it is determined that the position in each joint of robot
And attitude.
In embodiments of the present invention, the global optimum path based on Bugs algorithmic rule multi-joint mobile robots, generation
Robot obstacle-avoiding path, obtains movement locus feasible zone；As shown in figure 4, under global coordinate system, mesh is moved to from starting point A
The global path planning of punctuate B, the coordinates of motion of robot and direction can be by (x, y, θi) determine, each joint of robot
Pose can be established by the matrix conversion between local coordinate system and global coordinate system.
Wherein, it is above-mentioned according to the above-mentioned athletic posture in each joint adjust above-mentioned robot to above-mentioned impact point move it is complete
Office's optimal path includes：
Above-mentioned athletic posture according to each joint under global coordinate system determines above-mentioned robot under global coordinate system
Avoidance movement locus；
The global optimum path that above-mentioned robot is moved to above-mentioned impact point is adjusted according to above-mentioned avoidance movement locus.
As shown in figure 5, process a, c, e be vision guided navigation, process b, d, f be infrared obstacle avoidance, barrier profile be based on
The barrier depth information that the robot of Bugs algorithms is scanned by vision system.
Specifically, it is above-mentioned under local coordinate system, according between the corresponding corner in each joint of above-mentioned robot, adjacent segment
Spacing, rod length, rod member torsional angle, and detecting the vision system position of above-mentioned barrier, to adjust above-mentioned robot each
The corner in individual joint, avoiding above-mentioned barrier until each joint includes：
The joint on the end effector of above-mentioned robot is selected as the first regulation joint；
The barrier model regulation above-mentioned first set up according to vision system in the feasible zone in the above-mentioned first regulation joint
The corner in joint is adjusted, storage makes above-mentioned first regulation joint corner first corresponding to avoiding obstacles；
By the above-mentioned first regulation joint drive with the above-mentioned first adjacent joint motions in regulation joint, continue to adjust with it is above-mentioned
The corner in the adjacent joint in the first regulation joint, storage makes the joint adjacent with the above-mentioned first regulation joint avoiding obstacles first
Corresponding corner, circulates successively, until the joint for being currently needed for regulation is base.
The method of control multi-joint Mobile Robot Obstacle Avoidance provided in an embodiment of the present invention, in robot according to planning in advance
Global optimum path moved from starting point to impact point during, using vision guided navigation and the mutually coordinated control of infrared obstacle avoidance
Robot motion, when having barrier in vision system detects predeterminable range, suspends the advance of robot base, with reference to advance
Set up each joint kinematics geometrical model, adjust the athletic posture in each joint step by step automatically, in each joint can
Swinging joint in row domain, from end effector to base, avoiding obstacles step by step, by the position in each joint of servo system control
Appearance and bobbin movement direction, continue to control robot to be moved to impact point, until robot reaches impact point, in infrared obstacle avoidance mistake
The athletic posture of each joint and base is adjusted in journey simultaneously, it is to avoid subsequently adjust repeatedly, improve multi-joint mobile robot
Avoidance efficiency.
As shown in fig. 6, the embodiment of the present invention additionally provides a kind of device of control multi-joint Mobile Robot Obstacle Avoidance, the dress
Put including：
First control module 602, for the vision system set on the end effector by multi-joint mobile robot
Gather the ambient image of above-mentioned robot position, and according to the above-mentioned ambient image for collecting control above-mentioned robot according to
Movement of the global optimum path of planning from starting point to impact point in advance；Wherein, each joint of above-mentioned robot is to that should have
Kinematics geometrical model, above-mentioned kinematics geometrical model is the relative motion by analyzing above-mentioned each joint of robot, is used
What classical D-H methods pre-build；
Pause module 604, during being moved in above-mentioned robot, when above-mentioned vision system detects predeterminable range
When inside having barrier, suspend the advance of above-mentioned robot base；
Adjustment module 606, for obtaining the corresponding local coordinate system of above-mentioned kinematics geometrical model, in above-mentioned local coordinate
Under system, according to spacing, rod length, rod member torsional angle, Yi Jijian between the corresponding corner in each joint of above-mentioned robot, adjacent segment
The vision system position for measuring above-mentioned barrier adjusts the corner in above-mentioned each joint of robot, until each joint is kept away
Open above-mentioned barrier；
Determining module 608, the motion in each joint under global coordinate system is determined for the above-mentioned corner after according to regulation
Attitude；
Second control module 610, the ambient image for gathering above-mentioned robot current location by above-mentioned vision system,
And the global optimum path that above-mentioned robot is moved to above-mentioned impact point is adjusted according to the above-mentioned athletic posture in each joint, according to
Above-mentioned global optimum path after adjustment continues to control above-mentioned robot to be moved to above-mentioned impact point, until above-mentioned robot is reached
Above-mentioned impact point.
Further, above-mentioned determining module 608 includes：
First determining unit, for phase in the local coordinate system for determining the corresponding kinematics geometrical model foundation in each joint
Homogeneous transform matrix formula between adjacent coordinate system
Wherein, diRepresent distance, θ between joint i and joint i-1iRepresent the corner of joint i, aiRepresent joint i and joint i-1
Between rod length, αiRepresent the rod member torsional angle of joint i, Transz(di) represent i-1 coordinate system transformations under i coordinate systems along z-axis
Translation diThe translation matrix that distance is obtained, Rotz(θi) represent i-1 coordinate system transformations under i coordinate systems along z-axis rotate counterclockwise θi
The spin matrix that angle is obtained, Transx(ai) represent i-1 coordinate system transformations under i coordinate systems along x-axis translation aiWhat distance was obtained
Translation matrix, Rotx(αi) represent i-1 coordinate system transformations under i coordinate systems along x-axis rotate counterclockwise αiThe spin moment that angle is obtained
Battle array；
First computing unit, it is corresponding during for according to above-mentioned homogeneous transform matrix formula with each joint avoiding obstacles
Spacing, rod length and rod member Twist Angle Calculation obtain joint i-1 coordinate systems and change to joint i coordinate systems between corner, adjacent segment
Matrix；
Second computing unit, for being changed according to above-mentioned joint i-1 coordinate systems to the matrix and motion appearance of joint i coordinate systems
State computing formulaThe athletic posture in local coordinate system hypozygal i is calculated, wherein, i represents pass
Section numbering, TiAthletic posture of i-th joint under local coordinate system is represented,Represent i-1 coordinate system transformations to i coordinate systems
The transformation matrix for obtaining；
3rd computing unit, for the above-mentioned athletic posture and computing formula P in basis each joint under local coordinate systemi
=P × TiThe athletic posture in each joint under global coordinate system is calculated, wherein, PiRepresent i-th joint in global coordinate system
Under athletic posture, TiAthletic posture of i-th joint under local coordinate system is represented, P represents multi-joint mobile robot bottom
The matrix expression of athletic posture of the seat under global coordinate system and
Further, above-mentioned second control module 610 includes：
Second determining unit, for determining above-mentioned robot according to the above-mentioned athletic posture in each joint under global coordinate system
Avoidance movement locus under global coordinate system；
Adjustment unit, for adjusting the overall situation that above-mentioned robot is moved to above-mentioned impact point according to above-mentioned avoidance movement locus
Optimal path.
Further, above-mentioned adjustment module 606 includes：
Unit is chosen, for the joint on the end effector for selecting above-mentioned robot as the first regulation joint；
First adjustment unit, for the barrier set up according to vision system in the feasible zone in the above-mentioned first regulation joint
The corner in the above-mentioned first regulation joint of model regulation, storage makes above-mentioned first regulation joint turning corresponding to avoiding obstacles first
Angle；
Second adjustment unit, for driving the joint adjacent with the above-mentioned first regulation joint to transport by the above-mentioned first regulation joint
It is dynamic, continue to adjust the corner for adjusting the adjacent joint in joint with above-mentioned first, storage makes adjacent with the above-mentioned first regulation joint
Joint corner first corresponding to avoiding obstacles, circulates successively, until the joint for being currently needed for regulation is base.
Understood based on above-mentioned analysis, the device of control multi-joint Mobile Robot Obstacle Avoidance provided in an embodiment of the present invention,
During robot is moved according to the global optimum path of advance planning from starting point to impact point, using vision guided navigation and red
The outer mutually coordinated control robot motion of avoidance, when having barrier in vision system detects predeterminable range, suspends robot
The advance of base, with reference to the kinematics geometrical model in each joint for pre-building, adjusts the motion in each joint step by step automatically
Attitude, the swinging joint in the feasible zone in each joint, from end effector to base, avoiding obstacles step by step, by servo
System controls pose and the bobbin movement direction in each joint, continues to control robot to be moved to impact point, until robot is arrived
Up to impact point, the athletic posture of each joint and base is adjusted simultaneously during infrared obstacle avoidance, it is to avoid subsequently adjust repeatedly, carry
The avoidance efficiency of multi-joint mobile robot high.
The device of the control multi-joint Mobile Robot Obstacle Avoidance that the embodiment of the present invention is provided can be specific in equipment
Hardware or the software being installed in equipment or firmware etc..The device that the embodiment of the present invention is provided, its realization principle and generation
Technique effect it is identical with preceding method embodiment, be briefly describe, device embodiment part do not refer to part, refer to foregoing
Corresponding contents in embodiment of the method.It is apparent to those skilled in the art that, it is for convenience and simplicity of description, preceding
The specific work process of system, device and the unit of description is stated, the corresponding process in above method embodiment is may be referred to,
This is repeated no more.
If the function is to realize in the form of SFU software functional unit and as independent production marketing or when using, can be with
Storage is in a computer read/write memory medium.Based on such understanding, technical scheme is substantially in other words
The part contributed to prior art or the part of the technical scheme can be embodied in the form of software product, the meter
Calculation machine software product is stored in a storage medium, including some instructions are used to so that a computer equipment (can be individual
People's computer, server, or network equipment etc.) perform all or part of step of each embodiment methods described of the invention.
And foregoing storage medium includes：USB flash disk, mobile hard disk, read-only storage (ROM, Read-Only Memory), arbitrary access are deposited
Reservoir (RAM, Random Access Memory), magnetic disc or CD etc. are various can be with the medium of store program codes.
It should be noted that：Similar label and letter represents similar terms in following accompanying drawing, therefore, once a certain Xiang Yi
It is defined in individual accompanying drawing, then it need not be further defined and explained in subsequent accompanying drawing, additionally, term " the
One ", " second ", " the 3rd " etc. are only used for distinguishing description, and it is not intended that indicating or implying relative importance.
Finally it should be noted that：Embodiment described above, specific embodiment only of the invention, is used to illustrate the present invention
Technical scheme, rather than its limitations, protection scope of the present invention is not limited thereto, although with reference to the foregoing embodiments to this hair
It is bright to be described in detail, it will be understood by those within the art that：Any one skilled in the art
The invention discloses technical scope in, it can still modify to the technical scheme described in previous embodiment or can be light
Change is readily conceivable that, or equivalent is carried out to which part technical characteristic；And these modifications, change or replacement, do not make
The essence of appropriate technical solution departs from the spirit and scope of embodiment of the present invention technical scheme.Should all cover in protection of the invention
Within the scope of.Therefore, protection scope of the present invention described should be defined by scope of the claims.
Claims (8)
1. it is a kind of control multi-joint Mobile Robot Obstacle Avoidance method, it is characterised in that methods described includes：
The robot position is gathered by the vision system set on the end effector of multi-joint mobile robot
Ambient image, and according to the ambient image that collects control the robot according to advance planning global optimum path from
Movement from starting point to impact point；Wherein, each joint of the robot is to that should have kinematics geometrical model, the kinematics
Geometrical model is the relative motion by analyzing each joint of robot, is pre-build using classical D-H methods；
During the robot is moved, when the vision system is detected has barrier in predeterminable range, suspend institute
State the advance of robot base；
The corresponding local coordinate system of the kinematics geometrical model is obtained, under the local coordinate system, according to the robot
Spacing, rod length, rod member torsional angle between the corresponding corner in each joint, adjacent segment, and detect regarding for the barrier
Feel system position adjusts the corner in each joint of robot, until the barrier is avoided in each joint；
The corner after according to regulation determines the athletic posture in each joint under global coordinate system；
The ambient image of the robot current location is gathered by the vision system, and according to the motion in each joint
The global optimum path that robot described in pose adjustment is moved to the impact point, according to the global optimum path after adjustment
Continue to control the robot to be moved to the impact point, until the robot reaches the impact point.
2. it is according to claim 1 control multi-joint Mobile Robot Obstacle Avoidance method, it is characterised in that it is described according to tune
The corner after section determines that the athletic posture in each joint under global coordinate system includes：
Determine the homogeneous change between adjacent coordinates system in the local coordinate system that the corresponding kinematics geometrical model in each joint is set up
Change Matrix Formula
Wherein, diRepresent distance, θ between joint i and joint i-1iRepresent the corner of joint i, aiBetween representing joint i and joint i-1
Rod length, αiRepresent the rod member torsional angle of joint i, Transz(di) represent i-1 coordinate system transformations under i coordinate systems along z-axis translation
diThe translation matrix that distance is obtained, Rotz(θi) represent i-1 coordinate system transformations under i coordinate systems along z-axis rotate counterclockwise θiAngle
The spin matrix for obtaining, Transx(ai) represent i-1 coordinate system transformations under i coordinate systems along x-axis translation aiThe translation that distance is obtained
Matrix, Rotx(αi) represent i-1 coordinate system transformations under i coordinate systems along x-axis rotate counterclockwise αiThe spin matrix that angle is obtained；
Spacing between corresponding corner, adjacent segment during according to the homogeneous transform matrix formula and each joint avoiding obstacles,
Rod length and rod member Twist Angle Calculation obtain joint i-1 coordinate systems and change to the matrix of joint i coordinate systems；
Changed according to the joint i-1 coordinate systems to the matrix and athletic posture computing formula of joint i coordinate systems
The athletic posture in local coordinate system hypozygal i is calculated, wherein, i represents that joint is numbered, TiRepresent i-th joint in office
Athletic posture under portion's coordinate system,Represent the transformation matrix that i-1 coordinate system transformations are obtained to i coordinate systems；
According to the athletic posture and computing formula P in each joint under local coordinate systemi=P × TiIt is calculated global seat
The athletic posture in lower each joint of mark system, wherein, PiRepresent athletic posture of i-th joint under global coordinate system, TiRepresent the
Athletic posture of the i joint under local coordinate system, P represents motion of the multi-joint mobile robot base under global coordinate system
The matrix expression of attitude and
3. it is according to claim 2 control multi-joint Mobile Robot Obstacle Avoidance method, it is characterised in that the basis is each
The athletic posture in individual joint adjusts the robot to be included to the global optimum path that the impact point is moved：
The athletic posture according to each joint under global coordinate system determines avoidance of the robot under global coordinate system
Movement locus；
The global optimum path that the robot is moved to the impact point is adjusted according to the avoidance movement locus.
4. according to claim any one of 1-3 control multi-joint Mobile Robot Obstacle Avoidance method, it is characterised in that institute
State under local coordinate system, according to spacing, rod length, bar between the corresponding corner in each joint of the robot, adjacent segment
Part torsional angle, and the corner in vision system position regulation each joint of robot of the barrier is detected, directly
Avoiding the barrier to each joint includes：
The joint on the end effector of the robot is selected as the first regulation joint；
Barrier model regulation first regulation set up according to vision system in the feasible zone in the described first regulation joint
The corner in joint, storage makes the first regulation joint corner first corresponding to avoiding obstacles；
The joint motions adjacent with the described first regulation joint are driven by the described first regulation joint, continues to adjust and described first
The corner in the adjacent joint in regulation joint, storage makes the joint adjacent with the described first regulation joint, and avoiding obstacles institute is right first
The corner answered, circulates successively, until the joint for being currently needed for regulation is base.
5. it is a kind of control multi-joint Mobile Robot Obstacle Avoidance device, it is characterised in that described device includes：
First control module, the vision system collection for being set on the end effector by multi-joint mobile robot is described
The ambient image of robot position, and the robot is controlled according to planning in advance according to the ambient image for collecting
Movement of the global optimum path from starting point to impact point；Wherein, each joint of the robot is to that should have kinematics several
What model, the kinematics geometrical model is the relative motion by analyzing each joint of robot, using classical D-H methods
Pre-build；
Pause module, during being moved in the robot, has barrier when the vision system is detected in predeterminable range
When hindering thing, suspend the advance of the robot base；
Adjustment module, for obtaining the corresponding local coordinate system of the kinematics geometrical model, under the local coordinate system, root
According to spacing, rod length, rod member torsional angle between the corresponding corner in each joint of the robot, adjacent segment, and detect institute
The vision system position for stating barrier adjusts the corner in each joint of robot, until each joint avoids described
Barrier；
Determining module, the athletic posture in each joint under global coordinate system is determined for the corner after according to regulation；
Second control module, the ambient image for gathering the robot current location by the vision system, and according to
The athletic posture in each joint adjusts the global optimum path that the robot is moved to the impact point, after adjustment
The global optimum path continue to control the robot move to the impact point, until the robot arrival mesh
Punctuate.
6. it is according to claim 5 control multi-joint Mobile Robot Obstacle Avoidance device, it is characterised in that the determination mould
Block includes：
First determining unit, for adjacent seat in the local coordinate system for determining the corresponding kinematics geometrical model foundation in each joint
Homogeneous transform matrix formula between mark system
Wherein, diRepresent distance, θ between joint i and joint i-1iRepresent the corner of joint i, aiBetween representing joint i and joint i-1
Rod length, αiRepresent the rod member torsional angle of joint i, Transz(di) represent i-1 coordinate system transformations under i coordinate systems along z-axis translation
diThe translation matrix that distance is obtained, Rotz(θi) represent i-1 coordinate system transformations under i coordinate systems along z-axis rotate counterclockwise θiAngle
The spin matrix for obtaining, Transx(ai) represent i-1 coordinate system transformations under i coordinate systems along x-axis translation aiThe translation that distance is obtained
Matrix, Rotx(αi) represent i-1 coordinate system transformations under i coordinate systems along x-axis rotate counterclockwise αiThe spin matrix that angle is obtained；
First computing unit, corresponding turn during for according to the homogeneous transform matrix formula with each joint avoiding obstacles
Spacing, rod length and rod member Twist Angle Calculation obtain joint i-1 coordinate systems and change to joint i coordinate systems between angle, adjacent segment
Matrix；
Second computing unit, by being changed to the matrix and athletic posture of joint i coordinate systems according to the joint i-1 coordinate systems
Calculate formulaThe athletic posture in local coordinate system hypozygal i is calculated, wherein, i represents that joint is compiled
Number, TiAthletic posture of i-th joint under local coordinate system is represented,Represent that i-1 coordinate system transformations to i coordinate systems are obtained
Transformation matrix；
3rd computing unit, for the athletic posture and computing formula P in basis each joint under local coordinate systemi=P ×
TiThe athletic posture in each joint under global coordinate system is calculated, wherein, PiRepresent i-th joint under global coordinate system
Athletic posture, TiAthletic posture of i-th joint under local coordinate system is represented, P represents that multi-joint mobile robot base exists
The matrix expression of the athletic posture under global coordinate system and
7. it is according to claim 6 control multi-joint Mobile Robot Obstacle Avoidance device, it is characterised in that it is described second control
Molding block includes：
Second determining unit, for determining the robot complete according to the athletic posture in each joint under global coordinate system
Avoidance movement locus under office's coordinate system；
Adjustment unit, for adjusting the global optimum that the robot is moved to the impact point according to the avoidance movement locus
Path.
8. according to claim any one of 5-7 control multi-joint Mobile Robot Obstacle Avoidance device, it is characterised in that institute
Stating adjustment module includes：
Unit is chosen, for the joint on the end effector for selecting the robot as the first regulation joint；
First adjustment unit, for the barrier model set up according to vision system in the feasible zone in the described first regulation joint
The corner in the first regulation joint is adjusted, storage makes the first regulation joint corner first corresponding to avoiding obstacles；
Second adjustment unit, for driving the joint motions adjacent with the described first regulation joint by the described first regulation joint,
Continue to adjust the corner for adjusting the adjacent joint in joint with described first, storage makes the joint adjacent with the described first regulation joint
The corner corresponding to avoiding obstacles, circulates successively first, until the joint for being currently needed for regulation is base.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610014945.8A CN105437232B (en) | 2016-01-11 | 2016-01-11 | A kind of method and device of control multi-joint Mobile Robot Obstacle Avoidance |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610014945.8A CN105437232B (en) | 2016-01-11 | 2016-01-11 | A kind of method and device of control multi-joint Mobile Robot Obstacle Avoidance |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN105437232A CN105437232A (en) | 2016-03-30 |
| CN105437232B true CN105437232B (en) | 2017-07-04 |
Family
ID=55548101
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201610014945.8A Active CN105437232B (en) | 2016-01-11 | 2016-01-11 | A kind of method and device of control multi-joint Mobile Robot Obstacle Avoidance |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN105437232B (en) |
Families Citing this family (29)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106354161A (en) * | 2016-09-26 | 2017-01-25 | 湖南晖龙股份有限公司 | Robot motion path planning method |
| JP6457469B2 (en) * | 2016-12-08 | 2019-01-23 | ファナック株式会社 | Mobile robot interference area setting device |
| JP6998660B2 (en) | 2017-02-21 | 2022-01-18 | 株式会社安川電機 | Robot simulator, robot system and simulation method |
| CN107186711B (en) * | 2017-05-12 | 2021-06-15 | 广州视源电子科技股份有限公司 | Limiting protection method and device for mechanical arm and robot |
| CN108942914B (en) * | 2017-06-09 | 2021-04-06 | 北京猎户星空科技有限公司 | Robot obstacle avoidance processing method and device and robot |
| CN107309873B (en) * | 2017-06-20 | 2020-01-07 | 广州视源电子科技股份有限公司 | Mechanical arm motion control method and system |
| WO2019018961A1 (en) * | 2017-07-22 | 2019-01-31 | 深圳市萨斯智能科技有限公司 | Method for detecting object by robot, and robot |
| CN108733065B (en) * | 2017-09-29 | 2021-06-04 | 北京猎户星空科技有限公司 | Obstacle avoidance method and device for robot and robot |
| CN107942748B (en) * | 2017-11-28 | 2020-08-07 | 上海师范大学 | Mechanical arm space dynamic obstacle avoidance induction bracelet and control system |
| CN109978309B (en) * | 2017-12-28 | 2022-04-26 | 北京京东乾石科技有限公司 | Method and device for determining working efficiency of equipment |
| CN108241373B (en) * | 2017-12-29 | 2022-12-30 | 日照市越疆智能科技有限公司 | Obstacle avoidance method and intelligent robot |
| CN108527368B (en) * | 2018-03-30 | 2020-08-25 | 清华大学 | Method for determining optimal initial pose of flexible support series industrial robot operation |
| CN108544491B (en) * | 2018-03-30 | 2020-04-28 | 杭州电子科技大学 | Mobile robot obstacle avoidance method comprehensively considering two factors of distance and direction |
| US10899006B2 (en) * | 2018-05-01 | 2021-01-26 | X Development Llc | Robot navigation using 2D and 3D path planning |
| US11007031B2 (en) * | 2018-08-14 | 2021-05-18 | Verb Surgical Inc. | Setup of surgical robots using an augmented mirror display |
| CN109108974B (en) * | 2018-08-29 | 2020-07-07 | 广州市君望机器人自动化有限公司 | Robot avoidance method and device, background server and storage medium |
| CN109048917B (en) * | 2018-09-12 | 2021-06-29 | 南方电网电力科技股份有限公司 | Robot automatic control method, device, equipment and computer readable storage medium |
| CN109079799B (en) * | 2018-10-23 | 2021-11-12 | 哈尔滨工业大学（深圳） | Robot perception control system and control method based on bionics |
| CN109333506B (en) * | 2018-10-23 | 2021-12-17 | 广东工业大学 | Humanoid intelligent robot system |
| DE102018133472B3 (en) * | 2018-12-21 | 2020-03-12 | Franka Emika Gmbh | Motion monitoring of a robot manipulator |
| CN110487263A (en) * | 2019-08-15 | 2019-11-22 | 北京致行慕远科技有限公司 | The coordinate acquiring method and device of movable equipment |
| EP4019204A4 (en) * | 2019-09-30 | 2023-05-03 | Siemens Ltd., China | Method, device and system for controlling robot, storage medium and terminal |
| CN110613511B (en) * | 2019-10-16 | 2021-03-16 | 武汉联影智融医疗科技有限公司 | Obstacle avoidance method for surgical robot |
| CN110909859B (en) * | 2019-11-29 | 2023-03-24 | 中国科学院自动化研究所 | Bionic robot fish motion control method and system based on antagonistic structured control |
| CN111735601B (en) * | 2020-08-04 | 2021-03-30 | 中国空气动力研究与发展中心低速空气动力研究所 | Wall collision prevention method for double-engine refueling wind tunnel test supporting device |
| CN112847340B (en) * | 2020-12-25 | 2022-09-16 | 深圳市优必选科技股份有限公司 | Control method, control device and robot |
| CN113954073B (en) * | 2021-11-08 | 2024-03-22 | 北京华航唯实机器人科技股份有限公司 | Track analysis method and device for movable part of robot and robot equipment |
| CN114543723B (en) * | 2022-02-28 | 2023-02-03 | 苏州康多机器人有限公司 | DH model parameter measuring and calculating method, device and system |
| CN115657645B (en) * | 2022-11-16 | 2023-04-11 | 北京理工大学 | Intelligent vehicle chassis and task load integrated control method and system |
Family Cites Families (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR101553654B1 (en) * | 2009-02-13 | 2015-10-01 | 삼성전자 주식회사 | Mobile robot and method for moving of mobile robot |
| CN102124866B (en) * | 2011-01-19 | 2013-05-29 | 南京农业大学 | Wheel type mobile fruit picking robot and fruit picking method |
| US9352469B2 (en) * | 2013-05-03 | 2016-05-31 | Michael Stewart | Robotic disinfection system |
| CN203726478U (en) * | 2014-03-14 | 2014-07-23 | 浙江师范大学 | Robot capable of realizing obstacle avoidance control during moving |
| CN105022394B (en) * | 2014-04-29 | 2019-05-21 | 东北大学 | Mobile robot reliable location method under dynamic environment |
| CN204585197U (en) * | 2015-03-18 | 2015-08-26 | 重庆交通大学 | Can automatic obstacle-avoiding Work robot |
| CN104647344B (en) * | 2015-03-18 | 2016-11-30 | 重庆交通大学 | Three-dimensional map structure Work robot |
-
2016
- 2016-01-11 CN CN201610014945.8A patent/CN105437232B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN105437232A (en) | 2016-03-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105437232B (en) | A kind of method and device of control multi-joint Mobile Robot Obstacle Avoidance | |
| US11833694B2 (en) | Robot motion planning | |
| CN109108942B (en) | Mechanical arm motion control method and system based on visual real-time teaching and adaptive DMPS | |
| CN105710888B (en) | Dynamic obstacle avoidance in robot system | |
| WO2020207017A1 (en) | Method and device for collaborative servo control of uncalibrated movement vision of robot in agricultural scene | |
| US11694432B2 (en) | System and method for augmenting a visual output from a robotic device | |
| US8606402B2 (en) | Manipulator and control method thereof | |
| US20150094855A1 (en) | Imitation learning method for a multi-axis manipulator | |
| Kaldestad et al. | Collision avoidance with potential fields based on parallel processing of 3D-point cloud data on the GPU | |
| CN109108978B (en) | Three-degree-of-freedom space manipulator motion planning method based on learning generalization mechanism | |
| CN106527438A (en) | Robot navigation control method and device | |
| Rioux et al. | Autonomous SLAM based humanoid navigation in a cluttered environment while transporting a heavy load | |
| Monica et al. | Humanoid robot next best view planning under occlusions using body movement primitives | |
| Materna et al. | Teleoperating assistive robots: A novel user interface relying on semi-autonomy and 3D environment mapping | |
| Gäbert et al. | Generation of human-like arm motions using sampling-based motion planning | |
| Srivastava et al. | Range estimation and visual servoing of a dynamic target using a monocular camera | |
| JP2021529674A (en) | Systems and methods for natural tasks in one or more robot cross-reference paragraphs | |
| CN113146637A (en) | Robot Cartesian space motion planning method | |
| Cooperstock et al. | Self-supervised learning for docking and target reaching | |
| Paz-Delgado et al. | Combined path and motion planning for workspace restricted mobile manipulators in planetary exploration | |
| Cong et al. | Visual servoing control schemes of 4 DOF robot manipulators | |
| Borangiu et al. | Heuristic solution for constrained 7-DOF motion planning in 3D scanning application | |
| Seppänen | Aiding mobile robot teleoperator with short-term autonomous control system based on robot vision | |
| Priyadharshini et al. | SLAM (Simultaneous localisation and mapping) Using E-PUCK Robot In Webots Software | |
| Abdi et al. | A Hybrid AI-based Adaptive Path Planning for Intelligent Robot Arms |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant | ||
| CB03 | Change of inventor or designer information | ||
| CB03 | Change of inventor or designer information |
Inventor after: Lu Min Inventor after: Zhang Peng Inventor after: Teng Shuhua Inventor before: Dai Xiongming Inventor before: Duan Lian Inventor before: Fu Shuai Inventor before: Li Hong |