CN113910236B - Method, system, equipment and medium for planning movement of mobile double-arm robot - Google Patents
Method, system, equipment and medium for planning movement of mobile double-arm robot Download PDFInfo
- Publication number
- CN113910236B CN113910236B CN202111277239.XA CN202111277239A CN113910236B CN 113910236 B CN113910236 B CN 113910236B CN 202111277239 A CN202111277239 A CN 202111277239A CN 113910236 B CN113910236 B CN 113910236B
- Authority
- CN
- China
- Prior art keywords
- mobile
- hand
- pose
- arm robot
- double
- 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
Images
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/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
-
- 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/1679—Programme controls characterised by the tasks executed
- B25J9/1682—Dual arm manipulator; Coordination of several manipulators
Abstract
The invention discloses a method, a system, equipment and a medium for planning the motion of a mobile double-arm robot, which are used for intelligent dynamic motion planning of the mobile high-redundancy double-arm robot, and realize the collaborative motion planning of a mobile base and a left hand and a right hand by introducing a relative inverse reachability graph of the mobile base and the left hand and the right hand; meanwhile, according to the RiDRM graph of the mobile base of the mobile double-arm robot relative to the right hand and the LiDRM graph of the mobile base relative to the left hand, the mobile base pose and the upper arm operation pose which meet the operation performance requirements of the right hand and the left hand can be determined; the invention realizes the intelligent humanoid motion planning of the mobile double-arm robot in a dynamic and unstructured environment, and has the advantages of simple process, good motion planning effect and high planning efficiency.
Description
Technical Field
The invention belongs to the technical field of mobile robots, and particularly relates to a method, a system, equipment and a medium for planning the motion of a mobile double-arm robot.
Background
The robot is defined as a target-oriented machine with sensing, planning and action capabilities, namely with environment and self-sensing, motion planning and tracking control capabilities; in general, a humanoid dual-arm robot mounted on a mobile platform or having two legs is designed as an intelligent service robot because its appearance and performance are similar to those of a human being; the intelligent motion planning capability enables the robot to have the autonomous capability, and is very significant for intelligent service robots and intelligent manufacturing robots which need high-quality human-machine interaction.
The intelligent service robot can provide home service for daily living of human beings, and the intelligent manufacturing robot can realize personalized intelligent production and carry out cooperative operation with the human beings; however, due to many problems in the fields of perception, mechanical articulation, motion planning and tracking control, and integration and coordination of multiple skills, there are many challenges to the practical application of smart generic intelligent services and intelligent manufacturing robots; another significant advantage is that it can work naturally with humans without changing the existing infrastructure. It is clear that the intelligent service robot described above requires human-like kinematic structures, such as: the device is provided with a movable base and two flexible arms; compared with a single-arm robot or a moving trolley, the robot has the advantages of wide application range, strong cooperation capability and high reliability in complex tasks and dynamic environments, and can be widely applied to the fields of industrial manufacturing, social services, aerospace and the like. However, in order to achieve humanoid performance, the robot needs to be able to plan complex motion strategies, including the cooperative cooperation of the arms and the base, which is very challenging.
In order to make planning and operation feasible, the motion of the mobile base and upper limbs of the mobile two-arm robot needs to be pre-calculated; if a human-like mobile robot is required to have human-like behavior capability, an infinite number of expected tracks need to be designed; in response to the above problems, researchers at home and abroad have done a lot of work. The existing method respectively reflects the general rule of human arm movement and selects a proper movement mode based on a Hierarchical Planning Strategy (HPS) and a Bayesian decision, and is used for a robot arm planning algorithm to generate human-like movement. Based on the concept of the CoM motion primitives, the whole-body motion planning of the humanoid robot is realized, and a motion tree is constructed in a configuration space by realizing a series of CoM movements. However, the above methods all require the position and the grip posture (final posture) of the pre-grip moving base to be given manually, or simply assume that they are known.
In fact, it is crucial that the robot can autonomously design a suitable final pose to achieve better autonomy; for this reason, many motion planning methods introduce the concept of robot working space. For example, a method of determining an optimal pose position of a humanoid robot foot with respect to a desired gripping pose in SE (2). The reachable workspace representation may be pre-computed and an inverse reachability graph (iRM) obtained by inversion. The proposed method was evaluated by controlling the single arm of the robot Nao; the attitude with better performance of the robot can be determined by adopting the inverse surface reachability graph, and the problem based on points is popularized to the surface of a plane polygon, so that the good grabbing attitude is designed. In the existing reachability-map-based motion planning method, the reachability map is constructed by randomly sampling the configuration space, and the evaluation function value of each 3D pixel (x, y, z) is obtained by accumulating the evaluation function values of all 6D pixels (x, y, z, α, β, γ) having an arbitrary pose. In another approach to generate a base position suitable for grasping using inverse accessibility of a single arm of a humanoid robot, a directional accessibility map is established for an efficient IK solver to generate an efficient base pose and correct joint configuration, but it collapses in a dynamic environment. To address this problem, a new approach introduces dynamic inverse reachable mapping (iDRM) for floating-base systems. And constructing iDRM of the leg type humanoid robot by off-line and updating the iDRM on line when a new obstacle is detected. Two lists are constructed for each pixel of each sampling pose, where each sampling pose is obtained by a whole-body IK solver. However, this method only considers the single-arm constraint. Another existing method uses a pair of positive-negative dynamic reachability graphs to determine the final positions of one hand and two hands of the leg-type free-standing robot, and the task is completed on uneven terrain; however, the manipulation capability of the final pose of the robot or the humanoid nature of the pose are not considered. The existing improved MaxiMin NSGA-II redundant motion planning strategy is used for incompletely restricting the position of a humanoid mobile robot, namely a given end effector, and determining the optimal mobile base position and the mechanical arm configuration; however, since there is no restriction on the search area of the base, the dynamic intelligent motion planning is less efficient in real-time applications.
In conclusion, the motion planning of the mobile double-arm robot is one of the key problems in the technical field of robots, and the pose planning of the autonomous mobile base and the two arms is the key for realizing the intellectualization of the mobile double-arm robot; however, the current research on robot motion planning mainly focuses on mobile vehicles (UGV), unmanned Aerial Vehicles (UAV), and simple mechanical arms; much research has also been conducted on motion planning for mobile manipulators and humanoid robots. Particularly, the main methods for motion planning of the double-arm robot include direct search methods such as RRTs and the like, an IK-based solution, a geometric method, an inference method, a heuristic exploration method and the like; although the prior art methods involve redundant or dual arm motion planning, the cooperation between the two arms and the determination of the robot base, as well as the coordination between the robot base and the upper body arm, are not well studied, let alone the planning of human-like behavior.
Disclosure of Invention
Aiming at the technical problems in the prior art, the invention provides a method, a system, equipment and a medium for planning the motion of a mobile double-arm robot, which aim to solve the technical problem that the existing robot motion planning method can not realize the intelligent human-simulated motion planning of the mobile double-arm robot in a dynamic and unstructured environment.
In order to achieve the purpose, the invention adopts the technical scheme that:
the invention provides a motion planning method for a mobile double-arm robot, which comprises the following steps:
constructing a RiDRM graph of a moving base of the moving double-arm robot relative to a right hand and a LiDRM graph of the moving base relative to a left hand;
determining a right-hand expected grabbing pose and a left-hand expected grabbing pose of the mobile double-arm robot according to a target object to be grabbed;
planning and decomposing tasks to be executed of the mobile double-arm robot to obtain a plurality of subtasks; respectively determining a right-hand expected pose and a left-hand expected pose of each subtask according to the determined right-hand expected grabbing pose and the determined left-hand expected grabbing pose of the mobile double-arm robot;
executing the subtasks according to the expected right-hand pose and the expected left-hand pose of each subtask, transforming the RiDRM image of the mobile base of the mobile double-arm robot relative to the right hand to the expected right-hand position of the corresponding subtask, and transforming the LiDRM image of the mobile base of the mobile double-arm robot relative to the left hand to the expected left-hand position of the corresponding subtask;
dynamically detecting whether a new obstacle exists in the environment;
if so, updating the selectable enabling of the pose set in the LiDRM image of the mobile base relative to the right hand of the mobile two-arm robot and the selectable enabling of the pose set in the LiDRM image of the mobile base relative to the left hand of the mobile two-arm robot; respectively obtaining an updated RiDRM graph of the mobile base of the mobile double-arm robot relative to the right hand and an updated LiDRM graph of the mobile base relative to the left hand;
determining an optional position and posture set of the mobile base of the mobile double-arm robot according to the updated RiDRM diagram of the mobile base of the mobile double-arm robot relative to the right hand and the updated LiDRM diagram of the mobile base relative to the left hand;
designing an optimal pose set of the mobile double-arm robot, and adding the optimal pose set of the mobile double-arm robot into an optional pose set of the mobile double-arm robot to obtain an updated optional pose set of the mobile double-arm robot;
comprehensively evaluating the poses of the updated selectable pose sets of the mobile double-arm robot by utilizing an evaluation function to determine the optimal pose of the mobile robot;
according to the optimal pose of the mobile robot, carrying out trajectory planning and trajectory tracking control;
judging whether the task to be executed of the mobile double-arm robot is finished or not, if so, finishing the cycle; if not, returning to execute the next subtask.
The invention also provides a system for planning the movement of the mobile double-arm robot, which comprises:
the building module is used for building a RiDRM graph of a mobile base of the mobile double-arm robot relative to the right hand and a LiDRM graph of the mobile base relative to the left hand;
the task planning decomposition module is used for determining a right-hand expected grabbing pose and a left-hand expected grabbing pose of the mobile double-arm robot according to the target object to be grabbed; planning and decomposing tasks to be executed of the mobile double-arm robot to obtain a plurality of subtasks; respectively determining a right-hand expected pose and a left-hand expected pose of each subtask according to the determined right-hand expected grabbing pose and the determined left-hand expected grabbing pose of the mobile double-arm robot;
the execution module is used for executing the subtasks according to the expected right-hand pose and the expected left-hand pose of each subtask, transforming the RiDRM image of the mobile base of the mobile double-arm robot relative to the right hand to the expected right-hand position of the corresponding subtask, and transforming the LiDRM image of the mobile base of the mobile double-arm robot relative to the left hand to the expected left-hand position of the corresponding subtask; dynamically detecting whether a new obstacle exists in the environment; if so, updating the selectable enabling of the pose set in the LiDRM image of the mobile base relative to the right hand of the mobile two-arm robot and the selectable enabling of the pose set in the LiDRM image of the mobile base relative to the left hand of the mobile two-arm robot; respectively obtaining an updated RiDRM diagram of the mobile base of the mobile double-arm robot relative to the right hand and an updated LiDRM diagram of the mobile base relative to the left hand;
the optimization module is used for determining an optional position and pose set of the mobile base of the mobile double-arm robot according to the updated RiDRM diagram of the mobile base of the mobile double-arm robot relative to the right hand and the updated LiDRM diagram of the mobile base relative to the left hand; designing an optimal pose set of the mobile double-arm robot, adding the optimal pose set of the mobile double-arm robot into an optional pose set of the mobile double-arm robot, and obtaining an updated optional pose set of the mobile double-arm robot; comprehensively evaluating the positions of the updated selectable position sets of the mobile double-arm robot by utilizing an evaluation function to determine the optimal position of the mobile robot; according to the optimal pose of the mobile robot, carrying out trajectory planning and trajectory tracking control;
the ending module is used for judging whether the task to be executed of the mobile double-arm robot is finished or not, and if the task to be executed of the mobile double-arm robot is finished, the cycle is ended; if not, returning to execute the next subtask.
The invention also provides a motion planning device for the mobile double-arm robot, which comprises:
a memory for storing a computer program;
a processor for implementing the steps of the method of moving the dual-arm robotic motion planning apparatus when executing the computer program.
The invention also provides a computer readable storage medium having stored thereon a computer program which, when executed by a processor, carries out the steps of the method of moving a dual-arm robotic motion planning apparatus.
Compared with the prior art, the invention has the following beneficial effects:
the invention provides a method for planning the motion of a mobile double-arm robot, which is used for intelligent dynamic motion planning of a mobile high-redundancy double-arm robot and realizes the collaborative motion planning of a mobile base and a left hand and a right hand by introducing a relative inverse reachability graph of the mobile base and the left hand and the right hand; meanwhile, according to the RiDRM image of the mobile base of the mobile double-arm robot relative to the right hand and the LiDRM image of the mobile base relative to the left hand, the mobile base pose and the upper arm operation pose which meet the operation performance requirements of the right hand and the left hand can be determined, the intelligent humanoid motion planning of the mobile double-arm robot in a dynamic and unstructured environment is realized, the process is simple, the motion planning effect is good, and the planning efficiency is high.
Furthermore, the process of constructing the RiDRM graph of the mobile base of the mobile double-arm robot relative to the right hand and the LiDRM graph of the mobile base relative to the left hand adopts the combination of an offline process and an online process, the online use efficiency is not influenced, and the actual online search rate can be very high; if the barrier is detected on line, the RiDRM image and the LiDRM image are updated on line only by discarding the collided sampling poses, so that the updating efficiency is improved; and the coordinated motion planning of the mobile chassis and the two arms is realized by introducing a relative inverse reachability graph of the mobile chassis and the two arms, and human-like motion evaluation indexes of the mobile two-arm robot are defined, so that intelligent human-like motion planning is realized.
The method for planning the motion of the mobile double-arm robot can be used for various task scenes of the mobile double-arm robot, such as static/dynamic, structured/unstructured, single arm/double arms, base-upper arm cooperation and the like: in a static and structured environment, aiming at a single-arm task/double-arm cooperative task, the intelligent autonomous planning of the pose of a mobile base of the mobile double-arm robot can be realized; in a static and structured environment, aiming at a single-arm task/double-arm cooperative task, the cooperative intelligent autonomous planning of a mobile base and an end effector of a mobile double-arm robot can be realized; in a static and structured environment, aiming at a single-arm task/double-arm cooperative task, the humanoid pose planning of the robot can be realized; in a dynamic and unstructured environment, aiming at a single-arm task/double-arm cooperative task, the quick dynamic intelligent autonomous planning of the pose of a mobile base of a mobile double-arm robot can be realized; in a dynamic and unstructured environment, aiming at a single-arm task/double-arm cooperative task, the rapid dynamic intelligent autonomous planning of the cooperative motion of a mobile base and an end effector of a mobile double-arm robot can be realized; in a dynamic and unstructured environment, aiming at a single-arm task/double-arm cooperative task, the rapid intelligent planning of the humanoid pose of the robot can be realized; in a dynamic and unstructured scene needing to cooperate with or interact with human beings, the dynamic intelligent human-simulated motion planning of the mobile double-arm robot which can be foreseen by the human beings is realized.
Drawings
FIG. 1 is a flow chart of a method for planning the movement of a mobile two-arm robot according to an embodiment;
FIG. 2 is a flow chart of an offline RiRM construction in an embodiment;
FIG. 3 is a flow chart of the construction of the online RiDRM in the embodiment;
FIG. 4 is a flow chart of an offline LiRM build in an embodiment;
FIG. 5 is a flow chart of an online LiDRM build in an embodiment;
FIG. 6 is a diagram of the pose area of the motion base determined from the grab pose of the right hand and the RiDRM graph in the embodiment;
FIG. 7 is a diagram of an alternative area of the pose of the mobile base determined according to the RiDRM graph in the embodiment;
FIG. 8 is a diagram of the grabbing pose of the robot according to the right-hand grabbing pose and RiDRM diagram design in the embodiment;
FIG. 9 is a diagram of the pose area of the motion base determined from the grab pose of the left hand and the LiDRM map in the embodiment;
FIG. 10 is an alternative area diagram of the pose of the mobile base determined according to the left-hand LiDRM diagram in the embodiment;
fig. 11 is a diagram of the grabbing pose of the robot designed according to the grabbing pose of the left hand and the LiDRM diagram in the embodiment;
FIG. 12 is a diagram showing the RiDRM image with an obstacle and the pose of the mobile base in the embodiment;
FIG. 13 is a LiDRM diagram and a mobile base pose diagram with an obstacle in an embodiment;
FIG. 14 is a diagram illustrating the result of pose planning when an obstacle is present according to the grabbing pose of the right hand in the embodiment;
fig. 15 is a diagram illustrating a result of pose planning performed with an obstacle according to the grabbing pose of the left hand in the embodiment;
FIG. 16 is an alternative pose diagram for determining the mobile base according to the grabbing poses of the left hand and the right hand in the embodiment.
Detailed Description
In order to make the technical problems, technical solutions and advantageous effects of the present invention more apparent, the following embodiments further describe the present invention in detail. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention.
The invention provides a motion planning method for a mobile double-arm robot, which comprises the following steps:
The process of constructing the RiDRM graph of the moving base of the moving double-arm robot relative to the right hand is as follows:
the process of constructing the RiDRM graph of the moving base of the moving double-arm robot relative to the right hand comprises an off-line RiRM construction process and an on-line RiDRM construction process;
the off-line RiRM construction process specifically comprises the following steps:
randomly setting the position of a right-hand target of the mobile double-arm robot in a 3D space, and uniformly generating the pose of the right-hand target of the mobile double-arm robot;
setting the pose of a right-hand target of the mobile double-arm robot as a reference coordinate system;
according to the right-hand target pose of the mobile double-arm robot, IK solution is carried out to obtain a right-hand candidate pose q kr ；
Judging the candidate pose q of the right hand kr Whether it is a feasible solution;
if yes, solving the right-hand candidate pose q kr Evaluation function f of k Determining candidate poses q of a mobile base of a mobile two-arm robot b ；
If not, returning to obtain the right-hand candidate pose again;
constructing and right-hand candidate pose q kr Corresponding RiRM space pixel v i Is moving base arrival list Re i And with the right hand candidate pose q kr Corresponding RiRM space pixel v i Mobile base occupancy list O i ；
Judging the candidate pose q of the right hand kr If the generated quantity of the pose positions reaches the preset value N, returning to continuously acquire the right-hand candidate pose positions, otherwise, completing the construction of the offline RiRM;
the construction process of the online RiDRM specifically comprises the following steps:
initializing a RiDRM space to RiRM;
dynamically detecting whether a new barrier exists in the environment, and if not, completing the construction of the online RiDRM;
if yes, setting a right-hand candidate pose q colliding with a new obstacle kr Is no; synchronously updating pixel v corresponding to RiDRM space i To the arrival list Re i And a pixel v corresponding to the RiDRM space i Occupancy list O i 。
The process of constructing the relative left-hand LiDRM graph of the mobile base of the mobile double-arm robot is as follows:
the process for constructing the relative left-hand LiDRM graph of the mobile base of the mobile double-arm robot comprises an offline LiRM construction process and an online LiDRM construction process;
the offline LiRM construction process specifically comprises the following steps:
randomly giving the left-hand target position of the mobile double-arm robot in a 3D space, and uniformly generating the left-hand target pose of the mobile double-arm robot;
setting the left-hand target pose of the mobile double-arm robot as a reference coordinate system;
IK solving is carried out according to the left-hand target pose of the mobile double-arm robot to obtain a left-hand candidate pose q kl ；
Judging left hand candidate pose q kl Whether it is a feasible solution;
if yes, solving the left-hand candidate pose q kl Evaluation function f of k Determining candidate poses q of a mobile base of a mobile two-arm robot b ；
If not, returning to obtain the left-hand candidate pose again;
constructing and left-hand candidate pose q kl Corresponding LiRM space pixel v i Is moving base arrival list Re i And with the right-hand candidate pose q kl Corresponding LiRM space pixel v i Mobile base occupancy list O i ；
Judging left hand candidate pose q kl Generation amount ofWhether a preset value N is reached or not is judged, if not, the left-hand candidate pose is returned to be continuously obtained, otherwise, the construction of the offline LiRM is completed;
the construction process of the online LiDRM specifically comprises the following steps:
initializing LiDRM space to LiRM;
dynamically detecting whether a new obstacle exists in the environment, and if not, completing the construction of the online LiDRM;
if yes, setting a left-hand candidate pose q colliding with a new obstacle kl Is no; synchronous updating of corresponding pixel v of LiDRM space i To the arrival list Re i And a pixel v corresponding to the LiDRM space i Occupancy list of i 。
And 2, determining a right-hand expected grabbing pose and a left-hand expected grabbing pose of the mobile double-arm robot according to the target object to be grabbed.
Step 3, planning and decomposing the tasks to be executed of the mobile double-arm robot to obtain a plurality of subtasks; and respectively determining the right hand expected pose and the left hand expected pose of each subtask according to the determined right hand expected grabbing pose and the left hand expected grabbing pose of the mobile double-arm robot.
And 4, executing the subtasks according to the expected right-hand pose and the expected left-hand pose of each subtask, transforming the RiDRM image of the mobile base of the mobile double-arm robot relative to the right hand to the expected right-hand position of the corresponding subtask, and transforming the LiDRM image of the mobile base of the mobile double-arm robot relative to the left hand to the expected left-hand position of the corresponding subtask.
And 5, dynamically detecting whether a new obstacle exists in the environment.
Step 6, if a new obstacle is detected, updating the selectable enabling of the pose set in the RiDRM image of the moving base of the moving double-arm robot relative to the right hand and the selectable enabling of the pose set in the LiDRM image of the moving base relative to the left hand; and respectively obtaining the updated RiDRM graph of the mobile base of the mobile two-arm robot relative to the right hand and the updated LiDRM graph of the mobile base relative to the left hand.
And 7, determining an optional position set of the mobile base of the mobile double-arm robot according to the updated RiDRM image of the mobile base of the mobile double-arm robot relative to the right hand and the updated LiDRM image of the mobile base relative to the left hand.
And 8, if no new obstacle is detected, determining an optional position set of the mobile base of the mobile double-arm robot according to the RiDRM image of the mobile base of the mobile double-arm robot relative to the right hand and the LiDRM image of the mobile base of the mobile double-arm robot relative to the left hand.
Step 9, designing an optimal pose set of the mobile double-arm robot, adding the optimal pose set of the mobile double-arm robot into an optional pose set of the mobile double-arm robot, and obtaining an updated optional pose set of the mobile double-arm robot; wherein, an improved MaxiMin NSGA-II algorithm is adopted to design and obtain the optimal pose set of the mobile double-arm robot.
And step 10, determining the optimal pose of the mobile robot according to the updated selectable pose set of the mobile double-arm robot.
And 11, planning a track by adopting a direct bidirectional RRT and gradient descent algorithm according to the optimal pose of the mobile robot, and performing track tracking control by using a self-adaptive RBF neural network.
Step 12, judging whether the task to be executed of the mobile double-arm robot is finished or not, and if so, finishing the cycle; if not, returning to execute the next subtask.
The invention also provides a motion planning system of the mobile double-arm robot, which comprises a construction module, a task planning decomposition module, an execution module, an optimization module and an end module. And the building module is used for building a RiDRM graph of the moving base of the moving double-arm robot relative to the right hand and a LiDRM graph of the moving base relative to the left hand. The task planning decomposition module is used for determining a right-hand expected grabbing pose and a left-hand expected grabbing pose of the mobile double-arm robot according to the target object to be grabbed; planning and decomposing tasks to be executed of the mobile double-arm robot to obtain a plurality of subtasks; and respectively determining the right-hand expected pose and the left-hand expected pose of each subtask according to the determined right-hand expected grabbing pose and the left-hand expected grabbing pose of the mobile double-arm robot. The execution module is used for executing the subtasks according to the expected right-hand pose and the expected left-hand pose of each subtask, transforming the RiDRM image of the mobile base of the mobile double-arm robot relative to the right hand to the expected right-hand position of the corresponding subtask, and transforming the LiDRM image of the mobile base of the mobile double-arm robot relative to the left hand to the expected left-hand position of the corresponding subtask; dynamically detecting whether a new obstacle exists in the environment; if yes, updating the selectable enabling of the pose set in the RiDRM image of the moving base of the moving double-arm robot relative to the right hand and the pose set in the LiDRM image of the moving base relative to the left hand; and respectively obtaining the updated RiDRM graph of the mobile base of the mobile two-arm robot relative to the right hand and the updated LiDRM graph of the mobile base relative to the left hand. The optimization module is used for determining an optional position set of the mobile base of the mobile double-arm robot according to the updated RiDRM graph of the mobile base of the mobile double-arm robot relative to the right hand and the updated LiDRM graph of the mobile base relative to the left hand; designing an optimal pose set of the mobile double-arm robot, adding the optimal pose set of the mobile double-arm robot into an optional pose set of the mobile double-arm robot, and obtaining an updated optional pose set of the mobile double-arm robot; comprehensively evaluating the positions of the updated selectable position sets of the mobile double-arm robot by utilizing an evaluation function to determine the optimal position of the mobile robot; and carrying out trajectory planning and trajectory tracking control according to the optimal pose of the mobile robot. The ending module is used for judging whether the task to be executed of the mobile double-arm robot is finished or not, and if the task to be executed of the mobile double-arm robot is finished, the cycle is ended; if not, returning to execute the next subtask.
The invention also provides a motion planning device for the mobile double-arm robot, which comprises: a memory for storing a computer program; a processor for implementing the steps of the method for motion planning of a mobile two-arm robot when executing the computer program.
The processor implements the steps of the above-mentioned method for planning the movement of the mobile two-arm robot when executing the computer program, or implements the functions of the modules in the above-mentioned system when executing the computer program.
Illustratively, the computer program may be partitioned into one or more modules/units that are stored in the memory and executed by the processor to implement the invention. The one or more modules/units may be a series of computer program instruction segments capable of performing specific functions for describing the execution of the computer program in the mobile dual-arm robotic motion planning apparatus. For example, the computer program may be divided into a construction module, a mission planning decomposition module, an execution module, an optimization module, and an end module, and the specific functions of the modules are as follows: and the construction module is used for constructing a RiDRM graph of the mobile base of the mobile two-arm robot relative to the right hand and a LiDRM graph of the mobile base relative to the left hand. The task planning decomposition module is used for determining a right-hand expected grabbing pose and a left-hand expected grabbing pose of the mobile double-arm robot according to the target object to be grabbed; planning and decomposing tasks to be executed of the mobile double-arm robot to obtain a plurality of subtasks; and respectively determining the right hand expected pose and the left hand expected pose of each subtask according to the determined right hand expected grabbing pose and the left hand expected grabbing pose of the mobile double-arm robot. The execution module is used for executing the subtasks according to the expected right-hand pose and the expected left-hand pose of each subtask, transforming the RiDRM image of the mobile base of the mobile double-arm robot relative to the right hand to the expected right-hand position of the corresponding subtask, and transforming the LiDRM image of the mobile base of the mobile double-arm robot relative to the left hand to the expected left-hand position of the corresponding subtask; dynamically detecting whether a new obstacle exists in the environment; if yes, updating the selectable enabling of the pose set in the RiDRM image of the moving base of the moving double-arm robot relative to the right hand and the pose set in the LiDRM image of the moving base relative to the left hand; and respectively obtaining the updated RiDRM graph of the mobile base of the mobile two-arm robot relative to the right hand and the updated LiDRM graph of the mobile base relative to the left hand. The optimization module is used for determining an optional position set of the mobile base of the mobile double-arm robot according to the updated RiDRM graph of the mobile base of the mobile double-arm robot relative to the right hand and the updated LiDRM graph of the mobile base relative to the left hand; designing an optimal pose set of the mobile double-arm robot, and adding the optimal pose of the mobile double-arm robot into an optional pose set of the mobile double-arm robot; comprehensively evaluating the positions of the updated selectable position sets of the mobile double-arm robot by utilizing an evaluation function to determine the optimal position of the mobile robot; and carrying out trajectory planning and trajectory tracking control according to the optimal pose of the mobile robot. The ending module is used for judging whether the task to be executed of the mobile double-arm robot is finished or not, and if the task to be executed of the mobile double-arm robot is finished, the cycle is ended; if not, returning to execute the next subtask.
The motion planning equipment of the mobile double-arm robot can be computing equipment such as a desktop computer, a notebook computer, a palm computer and a cloud server. The mobile dual-arm robot motion planning apparatus may include, but is not limited to, a processor, a memory. It will be appreciated by those skilled in the art that the above is merely an example of a mobile two-arm robotic motion planning apparatus and does not constitute a limitation of a mobile two-arm robotic motion planning apparatus, and that more components may be included, or certain components may be combined, or different components may be included, e.g., the mobile two-arm robotic motion planning apparatus may also include input-output devices, network access devices, buses, etc.
The processor may be a Central Processing Unit (CPU), other general purpose processor, a Digital Signal Processor (DSP), an Application Specific Integrated Circuit (ASIC), an off-the-shelf programmable gate array (FPGA) or other programmable logic device, discrete gate or transistor logic, discrete hardware components, etc. The general purpose processor can be a microprocessor or the processor can be any conventional processor or the like that is the control center for the mobile dual-arm robotic motion planning apparatus, connecting the various parts of the overall mobile dual-arm robotic motion planning apparatus using various interfaces and lines.
The memory may be used to store the computer programs and/or modules, and the processor may implement the various functions of the mobile dual-arm robotic motion planning apparatus by executing or executing the computer programs and/or modules stored in the memory, as well as by invoking data stored in the memory.
The memory may mainly include a storage program area and a storage data area, wherein the storage program area may store an operating system, an application program required by at least one function (such as a sound playing function, an image playing function, etc.), and the like; the storage data area may store data (such as audio data, a phonebook, etc.) created according to the use of the cellular phone, and the like. In addition, the memory may include high speed random access memory, and may also include non-volatile memory, such as a hard disk, a memory, a plug-in hard disk, a Smart Media Card (SMC), a Secure Digital (SD) card, a flash memory card (FlashCard), at least one disk storage device, a flash memory device, or other volatile solid state storage device.
The invention also provides a computer readable storage medium storing a computer program which, when executed by a processor, implements the steps of the method for motion planning of a mobile dual-arm robot.
The modules/units integrated with the mobile two-arm robot motion planning apparatus may be stored in a computer-readable storage medium if implemented in the form of software functional units and sold or used as separate products.
Based on such understanding, the present invention realizes all or part of the processes of the above method, and can also be implemented by a computer program for instructing relevant hardware, wherein the computer program can be stored in a computer readable storage medium, and when being executed by a processor, the computer program can realize the steps of the above method for planning the movement of the mobile dual-arm robot. Wherein the computer program comprises computer program code, which may be in source code form, object code form, executable file or preset intermediate form, etc.
The computer-readable storage medium may include: any entity or device capable of carrying the computer program code, recording medium, usb disk, removable hard disk, magnetic disk, optical disk, computer memory, read-only memory (ROM), random Access Memory (RAM), electrical carrier wave signals, telecommunications signals, software distribution medium, etc.
It should be noted that the computer readable storage medium may contain content that is subject to appropriate increase or decrease as required by legislation and patent practice in jurisdictions, for example, in some jurisdictions, computer readable storage media that does not include electrical carrier signals and telecommunications signals in accordance with legislation and patent practice.
Examples
As shown in fig. 1, the embodiment provides a method for planning the movement of a mobile two-arm robot, which includes the following steps:
Step 2, presetting a certain target object to be grabbed, and determining the target object to be grabbed by using a detection system of the mobile double-arm robot; and determining a right-hand expected grabbing pose and a left-hand expected grabbing pose of the mobile double-arm robot according to the target object to be grabbed.
Step 3, planning and decomposing the tasks to be executed of the mobile double-arm robot by using the task planning system of the mobile double-arm robot to obtain a plurality of subtasks; and respectively determining the right hand expected pose and the left hand expected pose of each subtask according to the determined right hand expected grabbing pose and the left hand expected grabbing pose of the mobile double-arm robot.
And 4, executing the subtasks according to the expected right-hand pose and the expected left-hand pose of each subtask, translating and transforming the RiDRM image of the mobile base of the mobile double-arm robot relative to the right hand to the expected right-hand position of the corresponding subtask, and translating and transforming the LiDRM image of the mobile base of the mobile double-arm robot relative to the left hand to the expected left-hand position of the corresponding subtask.
And 5, dynamically detecting whether a new obstacle exists in the environment. If a new obstacle is detected, go to step 6; if no new obstacle is detected, go to step 7.
Step 6, updating the selectable enabling of the pose set in the RiDRM image of the moving base relative to the right hand and the selectable enabling of the pose set in the LiDRM image of the moving base relative to the left hand of the moving double-arm robot according to the detected new barrier; respectively obtaining an updated RiDRM graph of the mobile base of the mobile double-arm robot relative to the right hand and an updated LiDRM graph of the mobile base relative to the left hand; and determining an optional position set of the moving base of the moving double-arm robot according to the updated RiDRM diagram of the moving base of the moving double-arm robot relative to the right hand and the updated LiDRM diagram of the moving base relative to the left hand.
And 7, determining an optional pose set of the moving base of the moving double-arm robot according to the RiDRM diagram of the moving base of the moving double-arm robot relative to the right hand and the LiDRM diagram of the moving base of the moving double-arm robot relative to the left hand.
And 8, designing the optimal pose of the robot to be added into the optional pose set by adopting an improved MaxiMin NSGA-II algorithm, selecting the optional poses of the left arm and the right arm according to the right-hand BiDRM algorithm and the left-hand LiDRM algorithm to be added into the optional pose set, and obtaining the updated optional pose set of the mobile double-arm robot.
And 9, comprehensively evaluating the pose of the updated selectable pose set of the mobile double-arm robot by utilizing an evaluation function, and determining the optimal pose of the mobile robot.
And step 10, according to the optimal pose of the mobile robot, performing trajectory planning by adopting a direct bidirectional RRT and gradient descent algorithm, and performing trajectory tracking control by using a self-adaptive RBF neural network.
Step 11, judging whether the task to be executed of the mobile double-arm robot is finished or not, if so, ending the cycle; if not, returning to step 4 to execute the next subtask.
In this embodiment, the process of constructing the riddrm map of the mobile base of the mobile two-arm robot relative to the right hand includes an offline ridm construction process and an online riddrm construction process, which are specifically as follows:
as shown in fig. 2, the offline RiRM construction process specifically includes the following steps:
s11, randomly setting the position of a right-hand target of the mobile double-arm robot in a 3D space, and uniformly generating the position and posture of the right-hand target of the mobile double-arm robot;
s12, setting the pose of a right-hand target of the mobile double-arm robot as a reference coordinate system;
s13, carrying out IK solving according to the pose of the right-hand target of the mobile double-arm robot to obtain a right-hand candidate pose q kr ；
S14, judging a right-hand candidate pose q kr Whether it is a feasible solution;
s15, if yes, solving the right-hand candidate pose q kr Evaluation function f of k Determining candidate poses q of a mobile base of a mobile two-arm robot b ；
S16, if not, returning to the step S13, and re-acquiring the candidate pose of the right hand;
s17, constructing and right-hand candidate pose q kr Corresponding RiRM space pixel v i Is moving base arrival list Re i And with the right hand candidate pose q kr Corresponding RiRM space pixel v i List O of mobile base occupancies i ；
S18, judging the candidate pose q of the right hand kr If not, returning to the step S13 to continuously acquire the right-hand candidate poses, otherwise, completing the construction of the off-line RiRM.
As shown in fig. 3, the process of constructing the online riddrm specifically includes the following steps:
s19, initializing a RiDRM space into RiRM;
s110, dynamically detecting whether a new obstacle exists in the environment, and if not, completing construction of online RiDRM;
s111, if yes, setting a right-hand candidate pose q colliding with a new obstacle kr Is no; synchronously updating pixel v corresponding to RiDRM space i To the arrival list Re i And a pixel v corresponding to the RiDRM space i Occupancy list O i 。
In this embodiment, the process of constructing the mobile base of the mobile two-arm robot relative to the left-hand LiDRM diagram includes an offline LiRM construction process and an online LiDRM construction process, and specifically includes the following steps:
as shown in fig. 4, the offline LiRM building process specifically includes the following steps:
s21, randomly setting the left-hand target position of the mobile double-arm robot in a 3D space, and uniformly generating the left-hand target pose of the mobile double-arm robot;
s22, setting the left-hand target pose of the mobile double-arm robot as a reference coordinate system;
s23, carrying out IK solving according to the left-hand target pose of the mobile double-arm robot to obtain a left-hand candidate pose q kl ；
S24, judging left-hand candidate pose q kl Whether it is a feasible solution;
s25, if yes, solving the left-hand candidate pose q kl Evaluation function f of k Determining candidate poses q of a mobile base of a mobile two-arm robot b ；
S26, if not, returning to the step S23 to acquire the left-hand candidate pose again;
s27, constructing and left-hand candidate pose q kl Corresponding LiRM space pixel v i To the mobile base arrival list Re i And with the right-hand candidate pose q kl Corresponding LiRM space pixel v i Mobile base occupancy list O i ；
S28, judging left-hand candidate pose q kl If not, returning to the step S23 to continuously acquire the left-hand candidate poses, otherwise, completing the construction of the offline LiRM.
As shown in fig. 5, the construction process of the online LiDRM specifically includes the following steps:
s29, initializing the LiDRM space into LiRM;
s210, dynamically detecting whether a new obstacle exists in the environment, and if not, completing the construction of the online LiDRM;
s211, if yes, setting a left-hand candidate pose q colliding with a new obstacle kl Is no; synchronously updating pixel v corresponding to LiDRM space i ToDalisti Re i And a pixel v corresponding to the LiDRM space i Occupancy list O i 。
In this embodiment, in order to realize the intelligent humanoid motion of the mobile two-arm robot, in the process of constructing the riddrm diagram of the mobile base of the mobile two-arm robot relative to the right hand and the LiDRM diagram of the mobile base relative to the left hand, an evaluation function based on the following five evaluation indexes is introduced, wherein the following middle end effector is the left hand or the right hand of the mobile two-arm robot; the method comprises the following specific steps:
(1) End effector positioning accuracy evaluation function
f 1 (θ)＝||x R (θ)-x Rd ||+||x L (θ)-x Ld ||
Wherein, f 1 (theta) is a positioning accuracy evaluation function of the end effector; x is a radical of a fluorine atom R (θ) is the actual position of the right hand; x is a radical of a fluorine atom Rd The desired position for the right hand; x is the number of L (θ) is the actual position of the left hand; x is a radical of a fluorine atom Ld The desired position of the left hand. (Chinese physical meanings of the above letters are described separately)
(2) Evaluation function of direction tracking accuracy of end effector:
f 2 (θ)＝||e qR (θ)+||e qL (θ)||
wherein, f 2 (θ) is a direction tracking accuracy evaluation function of the end effector; e.g. of a cylinder qR (θ) is the attitude error of the right hand; e.g. of the type qL (θ) is the left hand attitude error; wherein the attitude error e of the right hand qR (theta) and left-hand attitude error e qL And (theta) is expressed by quaternion.
In this embodiment, in addition to reaching the desired position and pose of the left and right hands, the following evaluation indexes are introduced to enable the mobile two-arm robot to achieve the human-simulated motion behavior.
(3) Operational performance evaluation function of end effector:
in the embodiment, the operation performance can be used for describing the distance from the pose of the mobile double-arm robot to the singular pose; the expression of the operational performance is as follows:
Wherein Ω is the operating performance; j (theta) is a Jacobian matrix of the end effector; j is a unit of T (theta) is the transpose of the jacobian matrix J (theta) of the end effector; sigma max Is the maximum singular value of the jacobian matrix J (θ) of the end effector; sigma min Is the smallest singular value of the jacobian matrix J (θ) for the end effector.
Thus, the end effector's operability evaluation function:
f 3 (θ)＝-(Ω R (R)+Ω L (R))
wherein, f 3 (θ) is an end effector performance evaluation function; omega R (R) right hand performance; omega L (R) is the left hand performance.
(4) Joint displacement evaluation function:
in order to save energy, the mobile double-arm robot needs to have minimum joint displacement, so the joint displacement evaluation index based on quality is introduced:
wherein f is 4 (theta) is a joint displacement evaluation function, and is used for reflecting that the heaviest part of the mobile double-arm robot has the largest moving difficulty; n is the dimension of the generalized variable theta; theta i Is a generalized variable; theta imin As a generalized variable theta i The left boundary of (1); theta imax As a generalized variable theta i The right boundary of (c); w i Is a weight based on the mass distribution; m i Is the total mass of link i to link k; m j Is the total mass of link j to link k; n is e Subscripts for the end effector; m is k Is the mass of the connecting rod k.
(5) Evaluation function of displacement of the end effector relative to the base:
in order to realize the cooperation between the mobile base and the operating arm; the evaluation function based on the displacement of the end effector in the moving base coordinate system is defined as:
f 5 (θ)＝||x Rb (θ)-x Rbi ||+||x Lb (θ)-x Lbi ||
wherein, f 5 (θ) is an evaluation function of the displacement of the end effector relative to the base; x is a radical of a fluorine atom Rb (θ) is the desired position of the right hand in the mobile base coordinate system; x is a radical of a fluorine atom Rbi The initial position of the right hand in the coordinate system of the mobile base; x is the number of Lb (θ) is the desired position of the left hand in the mobile base coordinate system; x is a radical of a fluorine atom Lbi The initial position of the left hand in the mobile base coordinate system.
In the method for planning the movement of the mobile two-arm robot in this embodiment, assuming that the grasping positions of the left and right hands of the mobile two-arm robot are known, the positions of the mobile base, the upper body and the two arms of the mobile two-arm robot need to be determined.
Assuming that no barrier exists in the environment, grabbing the pose according to the expectation of the right hand or the left hand, and determining the pose area of the mobile base according to the RiDRM graph of the mobile base relative to the right hand and the LiDRM graph of the mobile base relative to the left hand; the design results are shown in fig. 6-7 and fig. 9-10, and it can be seen from fig. 6-7 and fig. 9-10 that the selectable area of the mobile base is determined as the area where the mobile base falls on the ground, i.e. the area with z coordinate [ - δ, + δ ], δ >0, and further the selectable area of the mobile base is determined as the annular area surrounding the right and left hands.
The optimal pose of the objective function is selected from the selectable pose set as the expected grabbing pose of the mobile two-arm robot, and as a result, as shown in fig. 8 and 11, it can be seen from fig. 8 and 11 that the grabbing pose of the mobile two-arm robot is predictable and humanoid, and no singular pose occurs.
Secondly, assuming that a rectangular obstacle exists in the environment, the updating time of the mobile base relative to the right-hand LiDRM image is 0.49366s, and the updating time of the mobile base relative to the left-hand LiDRM image is 0.4169617 s; the result of designing the right-handed riddrm map and the optional area of the pose of the mobile base when there is an obstacle is shown in fig. 12, and the result of designing the left-handed LiDRM map and the optional area of the pose of the mobile base when there is an obstacle is shown in fig. 13; as can be seen from fig. 12 and 13, the number of the selectable poses is changed from 17 to 6 or 8; the result of pose planning when an obstacle is present according to the grabbing pose of the right hand is shown in fig. 14, and as can be seen from fig. 14, the pose planning time when an obstacle is present according to the grabbing pose of the right hand is 17.691014s; the pose planning with the obstacle according to the left-hand grab pose is shown in fig. 15, and as can be seen from fig. 15, the pose planning time with the obstacle according to the left-hand grab pose is 17.409708s; as shown in fig. 16, it can be seen from fig. 16 that the selectable pose area of the moving base is an annular area and is more intensively distributed in the symmetry axis area of the desired positions of the left hand and the right hand when the left hand and the right hand operate simultaneously.
For a description of relevant parts in the system, the device and the computer-readable storage medium for planning the motion of the mobile dual-arm robot provided in this embodiment, reference may be made to the detailed description of the corresponding parts in the method for planning the motion of the mobile dual-arm robot described in this embodiment, and details are not repeated here.
The method for planning the motion of the mobile double-arm robot can be used for planning the motion of tasks such as single-hand operation, single-hand and base system operation, double-arm cooperative operation, double-arm-base cooperative operation and the like, can be applied to dynamic and static environments, and greatly improves the intelligent degree of the robot; at the same time, it can be extended quickly to more complex motion planning situations, such as: if the situation that the left hand needs to be controlled relative to the right hand is involved, the optimal pose of the base and the right arm (left arm) can be determined directly on the basis of the existing algorithm, and then the pose planning of the left arm (right arm) is carried out.
The motion planning method of the mobile double-arm robot can be used for the motion planning of a home assistant robot, such as: giving position information of a target object to be taken; among them, the target object is, for example: the robot can intelligently plan a selectable area of a robot base when the robot takes the object, and plan the optimal position and posture when the robot grabs the object; the method for planning the motion of the mobile double-arm robot can also be used for planning the motion of operation tasks in industrial personalized production, such as: given the position information of a target object (such as a metal part) needing to be operated (welded, assembled, sprayed and the like), the robot can intelligently plan a selectable area of a robot base when the object is operated, and plan the optimal position and posture of the robot when the object is operated.
The above-described embodiment is only one of the embodiments that can implement the technical solution of the present invention, and the scope of the present invention to be claimed is not limited to the embodiment, but includes any changes, substitutions and other embodiments that can be easily conceived by those skilled in the art within the technical scope of the present invention disclosed.
Claims (9)
1. A motion planning method for a mobile double-arm robot is characterized by comprising the following steps:
constructing a RiDRM graph of a mobile base of the mobile double-arm robot relative to a right hand and a LiDRM graph of the mobile base relative to a left hand;
determining a right-hand expected grabbing pose and a left-hand expected grabbing pose of the mobile double-arm robot according to a target object to be grabbed;
planning and decomposing tasks to be executed of the mobile double-arm robot to obtain a plurality of subtasks; respectively determining a right-hand expected pose and a left-hand expected pose of each subtask according to the determined right-hand expected grabbing pose and the determined left-hand expected grabbing pose of the mobile double-arm robot;
executing the subtasks according to the expected right-hand pose and the expected left-hand pose of each subtask, transforming the RiDRM image of the mobile base of the mobile double-arm robot relative to the right hand to the expected right-hand position of the corresponding subtask, and transforming the LiDRM image of the mobile base of the mobile double-arm robot relative to the left hand to the expected left-hand position of the corresponding subtask;
dynamically detecting whether a new obstacle exists in the environment;
if yes, updating the selectable enabling of the pose set in the RiDRM image of the moving base of the moving double-arm robot relative to the right hand and the pose set in the LiDRM image of the moving base relative to the left hand; respectively obtaining an updated RiDRM graph of the mobile base of the mobile double-arm robot relative to the right hand and an updated LiDRM graph of the mobile base relative to the left hand;
determining an optional position set of the mobile base of the mobile double-arm robot according to the updated RiDRM graph of the mobile base of the mobile double-arm robot relative to the right hand and the updated LiDRM graph of the mobile base relative to the left hand;
designing an optimal pose set of the mobile double-arm robot, adding the optimal pose set of the mobile double-arm robot into an optional pose set of the mobile double-arm robot, and obtaining an updated optional pose set of the mobile double-arm robot;
comprehensively evaluating the poses of the updated selectable pose sets of the mobile double-arm robot by utilizing an evaluation function to determine the optimal pose of the mobile robot;
according to the optimal pose of the mobile robot, carrying out trajectory planning and trajectory tracking control;
judging whether the task to be executed of the mobile double-arm robot is finished or not, if so, ending the cycle; if not, returning to execute the next subtask.
2. The method of claim 1, wherein the dynamic detection of new obstacles in the environment determines the set of alternative poses of the mobile base of the mobile dual-arm robot based on the RIDRM map of the mobile base of the mobile dual-arm robot with respect to the right hand and the LiDRM map of the mobile base of the mobile dual-arm robot with respect to the left hand if no new obstacles are detected.
3. The method of claim 1, wherein the optimal pose of the mobile dual-arm robot is designed by using a modified MaxiMin NSGA-II algorithm in the process of designing the optimal pose of the mobile dual-arm robot and adding the optimal pose of the mobile dual-arm robot to the set of selectable poses of the mobile dual-arm robot.
4. The method for planning the movement of the mobile dual-arm robot according to claim 1, wherein the trajectory planning and trajectory tracking control process is performed according to the optimal pose information of the mobile robot, and the method comprises the following steps:
and according to the optimal pose of the mobile robot, performing trajectory planning by adopting a direct bidirectional RRT and gradient descent algorithm, and performing trajectory tracking control by using a self-adaptive RBF neural network.
5. The method for planning the movement of a mobile two-arm robot according to claim 1, wherein the process of constructing the riddrm map of the mobile base of the mobile two-arm robot relative to the right hand comprises the following steps:
the process of constructing the RiDRM graph of the moving base of the moving double-arm robot relative to the right hand comprises an off-line RiRM construction process and an on-line RiDRM construction process;
the off-line RiRM construction process specifically comprises the following steps:
randomly setting the position of a right-hand target of the mobile double-arm robot in a 3D space, and uniformly generating the position and posture of the right-hand target of the mobile double-arm robot;
setting the pose of a right-hand target of the mobile double-arm robot as a reference coordinate system;
according to the right-hand target pose of the mobile double-arm robot, IK solution is carried out to obtain a right-hand candidate pose q kr ；
Judging the candidate pose q of the right hand kr Whether it is a feasible solution;
if yes, solving the right-hand candidate pose q kr Evaluation function f of k Determining candidate poses q of a mobile base of a mobile two-arm robot b ；
If not, returning to obtain the right-hand candidate pose again;
constructing and right-hand candidate pose q kr Corresponding RiRM space pixel v i Is moving base arrival list Re i And with the right-hand candidate pose q kr Corresponding RiRM space pixel v i List O of mobile base occupancies i ；
Judging the candidate pose q of the right hand kr If the generated quantity of the pose positions reaches the preset value N, returning to continuously acquire the right-hand candidate pose positions, otherwise, completing the construction of the offline RiRM;
the construction process of the online RiDRM specifically comprises the following steps:
initializing a RiDRM space to RiRM;
dynamically detecting whether a new obstacle exists in the environment, and if not, completing the construction of the online RiDRM;
if so, setting a right-hand candidate pose q colliding with a new obstacle kr Is no; synchronously updating corresponding pixel v of RiDRM space i To the arrival list Re i And a pixel v corresponding to the RiDRM space i Occupancy list O i 。
6. The method for planning the movement of the mobile two-arm robot according to claim 1, wherein the process of constructing the LiDRM map of the mobile base of the mobile two-arm robot relative to the left hand is as follows:
the process of constructing the relative left-hand LiDRM diagram of the mobile base of the mobile double-arm robot comprises an offline LiRM construction process and an online LiDRM construction process;
the offline LiRM construction process specifically comprises the following steps:
randomly giving the left-hand target position of the mobile double-arm robot in a 3D space, and uniformly generating the left-hand target pose of the mobile double-arm robot;
setting the left-hand target pose of the mobile double-arm robot as a reference coordinate system;
according to the left-hand target pose of the mobile double-arm robot, IK solution is carried out to obtain a left-hand candidate pose q kl ；
Judging left hand candidate pose q kl Whether it is a feasible solution;
if yes, solving the left-hand candidate pose q kl Evaluation function f of k Determining candidate poses q of a mobile base of a mobile two-arm robot b ；
If not, returning to obtain the left-hand candidate pose again;
constructing and left-hand candidate pose q kl Corresponding LiRM space pixel v i Is moving base arrival list Re i And with the right-hand candidate pose q kl Corresponding LiRM space pixel v i Mobile base occupancy list O i ；
Judging left hand candidate pose q kl If the number of the generated positions reaches the preset value N, returning to continuously acquire the left-hand candidate poses, otherwise, completing the construction of the offline LiRM;
the construction process of the online LiDRM specifically comprises the following steps:
initializing LiDRM space to LiRM;
dynamically detecting whether a new obstacle exists in the environment, and if not, completing the construction of the online LiDRM;
if yes, setting a left-hand candidate pose q colliding with a new obstacle kl Is no; synchronous updating of corresponding pixel v of LiDRM space i To the arrival list Re i And a pixel v corresponding to the LiDRM space i Occupancy list of i 。
7. A mobile dual-arm robotic motion planning system, comprising:
the building module is used for building a RiDRM graph of a mobile base of the mobile double-arm robot relative to the right hand and a LiDRM graph of the mobile base relative to the left hand;
the task planning decomposition module is used for determining a right-hand expected grabbing pose and a left-hand expected grabbing pose of the mobile double-arm robot according to the target object to be grabbed; planning and decomposing tasks to be executed of the mobile double-arm robot to obtain a plurality of subtasks; respectively determining a right-hand expected pose and a left-hand expected pose of each subtask according to the determined right-hand expected grabbing pose and the determined left-hand expected grabbing pose of the mobile double-arm robot;
the execution module is used for executing the subtasks according to the expected right-hand pose and the expected left-hand pose of each subtask, transforming the RiDRM diagram of the mobile base of the mobile double-arm robot relative to the right hand to the expected right-hand position of the corresponding subtask, and transforming the LiDRM diagram of the mobile base of the mobile double-arm robot relative to the left hand to the expected left-hand position of the corresponding subtask; dynamically detecting whether a new obstacle exists in the environment; if yes, updating the selectable enabling of the pose set in the RiDRM image of the moving base of the moving double-arm robot relative to the right hand and the pose set in the LiDRM image of the moving base relative to the left hand; respectively obtaining an updated RiDRM graph of the mobile base of the mobile double-arm robot relative to the right hand and an updated LiDRM graph of the mobile base relative to the left hand;
the optimization module is used for determining an optional position set of the mobile base of the mobile double-arm robot according to the updated RiDRM graph of the mobile base of the mobile double-arm robot relative to the right hand and the updated LiDRM graph of the mobile base relative to the left hand; designing an optimal pose set of the mobile double-arm robot, adding the optimal pose set of the mobile double-arm robot into an optional pose set of the mobile double-arm robot, and obtaining an updated optional pose set of the mobile double-arm robot; comprehensively evaluating the poses of the updated selectable pose sets of the mobile double-arm robot by utilizing an evaluation function to determine the optimal pose of the mobile robot; according to the optimal pose of the mobile robot, carrying out trajectory planning and trajectory tracking control;
the ending module is used for judging whether the task to be executed of the mobile double-arm robot is finished or not, and if the task to be executed of the mobile double-arm robot is finished, the cycle is ended; if not, returning to execute the next subtask.
8. A mobile dual-arm robotic motion planning apparatus, comprising:
a memory for storing a computer program;
a processor for implementing the steps of a method of motion planning for a mobile two-arm robot according to any of claims 1-6 when executing said computer program.
9. A computer-readable storage medium storing a computer program, wherein the computer program when executed by a processor implements the steps of a method for motion planning of a mobile dual-arm robot according to any of claims 1-6.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111277239.XA CN113910236B (en) | 2021-10-29 | 2021-10-29 | Method, system, equipment and medium for planning movement of mobile double-arm robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111277239.XA CN113910236B (en) | 2021-10-29 | 2021-10-29 | Method, system, equipment and medium for planning movement of mobile double-arm robot |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113910236A CN113910236A (en) | 2022-01-11 |
| CN113910236B true CN113910236B (en) | 2022-11-29 |
Family
ID=79243757
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111277239.XA Active CN113910236B (en) | 2021-10-29 | 2021-10-29 | Method, system, equipment and medium for planning movement of mobile double-arm robot |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113910236B (en) |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106945020A (en) * | 2017-05-18 | 2017-07-14 | 哈尔滨工业大学 | A kind of space double mechanical arms system motion control method for coordinating |
| CN108638058A (en) * | 2018-04-23 | 2018-10-12 | 华南理工大学 | A kind of posture decision dynamic programming method |
| CN109048890A (en) * | 2018-07-13 | 2018-12-21 | 哈尔滨工业大学（深圳） | Coordination method for controlling trajectory, system, equipment and storage medium based on robot |
| CN109591014A (en) * | 2018-12-18 | 2019-04-09 | 武汉科技大学 | A kind of Dual-Arm Coordination method for carrying of both arms cooperation robot |
| CN111390872A (en) * | 2020-03-19 | 2020-07-10 | 上海航天控制技术研究所 | Double-arm cooperative flexible dragging and butt joint inverse operation method for extravehicular robot |
| CN112549028A (en) * | 2020-12-02 | 2021-03-26 | 中国科学院自动化研究所 | Double-arm robot track planning method based on dynamic motion primitives and artificial potential field |
-
2021
- 2021-10-29 CN CN202111277239.XA patent/CN113910236B/en active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106945020A (en) * | 2017-05-18 | 2017-07-14 | 哈尔滨工业大学 | A kind of space double mechanical arms system motion control method for coordinating |
| CN108638058A (en) * | 2018-04-23 | 2018-10-12 | 华南理工大学 | A kind of posture decision dynamic programming method |
| CN109048890A (en) * | 2018-07-13 | 2018-12-21 | 哈尔滨工业大学（深圳） | Coordination method for controlling trajectory, system, equipment and storage medium based on robot |
| CN109591014A (en) * | 2018-12-18 | 2019-04-09 | 武汉科技大学 | A kind of Dual-Arm Coordination method for carrying of both arms cooperation robot |
| CN111390872A (en) * | 2020-03-19 | 2020-07-10 | 上海航天控制技术研究所 | Double-arm cooperative flexible dragging and butt joint inverse operation method for extravehicular robot |
| CN112549028A (en) * | 2020-12-02 | 2021-03-26 | 中国科学院自动化研究所 | Double-arm robot track planning method based on dynamic motion primitives and artificial potential field |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113910236A (en) | 2022-01-11 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111538949B (en) | Redundant robot inverse kinematics solving method and device and redundant robot | |
| Huang et al. | Particle swarm optimization for solving the inverse kinematics of 7-DOF robotic manipulators | |
| WO2018107851A1 (en) | Method and device for controlling redundant robot arm | |
| US9411335B2 (en) | Method and apparatus to plan motion path of robot | |
| Petrič et al. | Smooth continuous transition between tasks on a kinematic control level: Obstacle avoidance as a control problem | |
| JP4730440B2 (en) | Trajectory planning apparatus, trajectory planning method, and computer program | |
| Bosscher et al. | Real‐time collision avoidance algorithm for robotic manipulators | |
| Xu et al. | Motion planning of manipulators for simultaneous obstacle avoidance and target tracking: An RNN approach with guaranteed performance | |
| Tanaka et al. | Modeling and control of head raising snake robots by using kinematic redundancy | |
| Xu et al. | Mechanical arm obstacle avoidance path planning based on improved artificial potential field method | |
| Bohez et al. | Sensor fusion for robot control through deep reinforcement learning | |
| CN110682286A (en) | Real-time obstacle avoidance method for cooperative robot | |
| Mbakop et al. | Path planning and control of mobile soft manipulators with obstacle avoidance | |
| Hua et al. | Reinforcement learning-based collision-free path planner for redundant robot in narrow duct | |
| Waltersson et al. | Planning and control for cable-routing with dual-arm robot | |
| Li et al. | Hybrid trajectory replanning-based dynamic obstacle avoidance for physical human-robot interaction | |
| Palm et al. | Particle swarm optimization of potential fields for obstacle avoidance | |
| Wei et al. | Motion planning for a humanoid mobile manipulator system | |
| Özaln et al. | An implementation of vision based deep reinforcement learning for humanoid robot locomotion | |
| Li et al. | Human-robot collaborative manipulation with the suppression of human-caused disturbance | |
| CN113910236B (en) | Method, system, equipment and medium for planning movement of mobile double-arm robot | |
| Ananthanarayanan et al. | A fast converging optimal technique applied to path planning of hyper-redundant manipulators | |
| Wang et al. | Whole body control of a dual-arm mobile robot using a virtual kinematic chain | |
| Zhao et al. | Efficient inverse kinematics for redundant manipulators with collision avoidance in dynamic scenes | |
| Akli | Trajectory planning for mobile manipulators including Manipulability Percentage Index |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |