CN113910236A - 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
- CN113910236A CN113910236A CN202111277239.XA CN202111277239A CN113910236A CN 113910236 A CN113910236 A CN 113910236A CN 202111277239 A CN202111277239 A CN 202111277239A CN 113910236 A CN113910236 A CN 113910236A
- 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.)
- Granted
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
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
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 two-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 perform 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 also 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 movement of the mobile base and the upper limbs of the mobile double-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 grasping attitude (final attitude) of the pre-grasping 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 posture 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 obtained by Inversion (iRM). 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, a reachability map is constructed by randomly sampling a 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 dynamic inverse reachable mapping (iDRM) is introduced in the new approach 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 double arms is the key for realizing the intellectualization of the mobile double-arm robot; while the current research on robot motion planning is mainly focused on mobile carts (UGVs), Unmanned Aerial Vehicles (UAVs) and simple mechanical arms; much research has also been conducted on motion planning for mobile manipulators and humanoid robots. Particularly, the main methods for planning the movement of the double-arm robot include direct search methods such as RRTs, etc., an IK-based solution, a geometric method, a reasoning method, a heuristic exploration method, etc.; 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 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 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 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, 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 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.
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 beneficial effects that:
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 right-hand RiDRM graph of the mobile base of the mobile double-arm robot and the left-hand LiDRM graph of the mobile base is combined with an offline process and an online process, so that 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 motion planning method of the mobile double-arm robot can be used for various mobile double-arm robot task scenes such as static/dynamic, structured/unstructured, single arm/double arms, base-upper arm coordination 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; the dynamic intelligent human-simulated motion planning of the human-foreseeable mobile double-arm robot is realized in a dynamic and unstructured scene which needs to be cooperated with or interacted with the human.
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 designed according to the grabbing pose of the right hand and the RiDRM graph 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 pose planning result 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 moving base according to the grabbing pose of the left hand and the grabbing pose of 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 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 qkr；
Judging the candidate pose q of the right handkrWhether it is a feasible solution;
if yes, solving the right-hand candidate pose qkrEvaluation function f ofkDetermining candidate poses q of a mobile base of a mobile two-arm robotb；
If not, returning to obtain the right-hand candidate pose again;
constructing and right-hand candidate pose qkrCorresponding RiRM space pixel viIs moving base arrival list ReiAnd with the right-hand candidate pose qkrCorresponding RiRM space pixel viList O of mobile base occupanciesi；
Judging the candidate pose q of the right handkrIf the generated number of the right-hand candidate poses does not reach the preset value N, returning to continuously obtain the right-hand candidate poses, 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 obstaclekrIs no; synchronously updating pixel v corresponding to RiDRM spaceiTo the arrival list ReiAnd a pixel v corresponding to the RiDRM spaceiOccupancy list Oi。
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;
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 qkl；
Judging left hand candidate pose qklWhether it is a feasible solution;
if yes, solving the left-hand candidate pose qklEvaluation function f ofkDetermining candidate poses q of a mobile base of a mobile two-arm robotb；
If not, returning to obtain the left-hand candidate pose again;
constructing and left-hand candidate pose qklCorresponding LiRM space pixel viIs moving base arrival list ReiAnd with the right-hand candidate pose qklCorresponding LiRM space pixel viList O of mobile base occupanciesi；
Judging left hand candidate pose qklIf 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 obstacleklIs no; synchronous updating of corresponding pixel v of LiDRM spaceiTo the arrival list ReiAnd a pixel v corresponding to the LiDRM spaceiOccupancy list Oi。
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; and the optimal position set of the mobile double-arm robot is designed by adopting an improved MaxiMin NSGA-II algorithm.
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 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 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; 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 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; 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, the instruction segments being used to describe 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 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; 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 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; 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 two-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 the 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 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.
And 7, 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.
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, and if so, finishing 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 right-hand target position of the mobile double-arm robot in the 3D space, and uniformly generating the right-hand target pose of the mobile double-arm robot;
s12, setting the pose of the right-hand target of the mobile double-arm robot as a reference coordinate system;
s13, carrying out IK solving according to the right hand target pose of the mobile double-arm robot to obtain a right hand candidate pose qkr；
S14, judging the candidate pose q of the right handkrWhether it is a feasible solution;
s15, if yes, solving the right-hand candidate pose qkrEvaluation function f ofkDetermining candidate poses q of a mobile base of a mobile two-arm robotb；
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 qkrCorresponding RiRM space pixel viIs moving base arrival list ReiAnd with the right-hand candidate pose qkrCorresponding RiRM space pixel viList O of mobile base occupanciesi；
S18, judgmentCandidate pose q for right-handed offkrIf not, returning to the step S13 to continuously acquire the right-hand candidate pose, otherwise, completing the construction of the offline RiRM.
As shown in fig. 3, the construction process of the online riddrm specifically includes the following steps:
s19, initializing the 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 obstaclekrIs no; synchronously updating pixel v corresponding to RiDRM spaceiTo the arrival list ReiAnd a pixel v corresponding to the RiDRM spaceiOccupancy list Oi。
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 the 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 qkl；
S24, judging left hand candidate pose qklWhether it is a feasible solution;
s25, if yes, solving the left hand candidate pose qklEvaluation function f ofkDetermining candidate poses q of a mobile base of a mobile two-arm robotb；
S26, if not, returning to the step S23 to obtain the left-hand candidate pose again;
s27, constructing and left-hand candidate pose qklCorresponding LiRM spacePixel viIs moving base arrival list ReiAnd with the right-hand candidate pose qklCorresponding LiRM space pixel viList O of mobile base occupanciesi；
S28, judging left hand candidate pose qklIf not, returning to the step S23 to continue to acquire the left-hand candidate pose, 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 obstacleklIs no; synchronous updating of corresponding pixel v of LiDRM spaceiTo the arrival list ReiAnd a pixel v corresponding to the LiDRM spaceiOccupancy list Oi。
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
f1(θ)＝||xR(θ)-xRd||+||xL(θ)-xLd||
Wherein f is1(θ) is a positioning accuracy evaluation function of the end effector; x is the number ofR(θ) is the actual position of the right hand; x is the number ofRdThe desired position for the right hand; x is the number ofL(θ) is the actual position of the left hand; x is the number ofLdThe 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:
f2(θ)＝||eqR(θ)+||eqL(θ)||
wherein, f2(θ) is a direction tracking accuracy evaluation function of the end effector; e.g. of the typeqR(θ) is the attitude error of the right hand; e.g. of the typeqL(θ) is the left hand attitude error; wherein the attitude error e of the right handqR(theta) and left hand attitude error eqLAnd (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. the design is a squareT(theta) is the transpose of the jacobian matrix J (theta) of the end effector; sigmamaxThe maximum singular value of a jacobian matrix J (theta) of the end effector; sigmaminIs the smallest singular value of the jacobian matrix J (θ) for the end effector.
Thus, the operational performance evaluation function of the end effector:
f3(θ)＝-(ΩR(R)+ΩL(R))
wherein f is3(θ) is an end effector performance evaluation function; omegaR(R) right hand performance; omegaL(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 is4(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; thetaiIs a generalized variable; thetaiminAs a generalized variable thetaiThe left boundary of (1); thetaimaxAs a generalized variable thetaiThe right boundary of (c); wiIs a weight based on the mass distribution; miIs the total mass of link i to link k; mjIs the total mass of link j to link k; n iseSubscripts for the end effector; m iskIs the mass of the connecting rod k.
(5) Evaluation function of displacement of end effector relative to 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:
f5(θ)＝||xRb(θ)-xRbi||+||xLb(θ)-xLbi||
wherein f is5(θ) is an evaluation function of the displacement of the end effector relative to the base; x is the number ofRb(θ) is the desired position of the right hand in the mobile base coordinate system; x is the number ofRbiThe initial position of the right hand in the coordinate system of the mobile base; x is the number ofLb(θ) is the desired position of the left hand in the mobile base coordinate system; x is the number ofLbiThe 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.41696517 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 grasp 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 grasp pose of the right hand is 17.691014 s; the pose planning when an obstacle is present according to the grasp pose of the left hand is shown in fig. 15, and as can be seen from fig. 15, the pose planning time when an obstacle is present according to the grasp pose of the left hand is 17.409708 s; 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 movement 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 movement of the mobile dual-arm robot described in this embodiment, which is not described herein again.
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 is not limited by the embodiment, but includes any variations, 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 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 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, finishing the cycle; if not, returning to execute the next subtask.
2. The method according to claim 1, wherein the process of dynamically detecting new obstacles in the environment is performed, and if no new obstacle is detected, the selectable pose sets of the mobile base of the mobile two-arm robot are determined according to the RiDRM map of the mobile base of the mobile two-arm robot relative to the right hand and the LiDRM map of the mobile base of the mobile two-arm robot relative to the left hand.
3. The method of claim 1, wherein the optimal pose of the mobile two-armed robot is designed and added to the set of selectable poses of the mobile two-armed robot, and the optimal pose of the mobile two-armed robot is designed using an improved MaxiMin NSGA-II algorithm.
4. The method for planning the movement of a 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 specifically 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 qkr；
Judging the candidate pose q of the right handkrWhether it is a feasible solution;
if yes, solving the right-hand candidate pose qkrEvaluation function f ofkDetermining candidate poses q of a mobile base of a mobile two-arm robotb；
If not, returning to obtain the right-hand candidate pose again;
constructing and right-hand candidate pose qkrCorresponding RiRM space pixel viIs moving base arrival list ReiAnd with the right-hand candidate pose qkrCorresponding RiRM space pixel viList O of mobile base occupanciesi；
Judging the candidate pose q of the right handkrIf the generated number of the right-hand candidate poses does not reach the preset value N, returning to continuously obtain the right-hand candidate poses, 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 obstaclekrIs no; synchronous update RiDRM nullPixel v corresponding to each otheriTo the arrival list ReiAnd a pixel v corresponding to the RiDRM spaceiOccupancy list Oi。
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 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;
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 qkl；
Judging left hand candidate pose qklWhether it is a feasible solution;
if yes, solving the left-hand candidate pose qklEvaluation function f ofkDetermining candidate poses q of a mobile base of a mobile two-arm robotb；
If not, returning to obtain the left-hand candidate pose again;
constructing and left-hand candidate pose qklCorresponding LiRM space pixel viIs moving base arrival list ReiAnd with the right-hand candidate pose qklCorresponding LiRM space pixel viList O of mobile base occupanciesi；
Judging left hand candidate pose qklIf 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 obstacleklIs no; synchronous updating of corresponding pixel v of LiDRM spaceiTo the arrival list ReiAnd a pixel v corresponding to the LiDRM spaceiOccupancy list Oi。
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 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;
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 moving a dual-arm robotic motion planning apparatus as claimed in any one of claims 1 to 6 when executing the computer program.
9. A computer-readable storage medium, having a computer program stored thereon, wherein the computer program, when executed by a processor, performs the steps of a method of moving a dual-arm robotic motion planning apparatus 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 true CN113910236A (en) | 2022-01-11 |
| CN113910236B 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 |
|---|---|
| CN113910236B (en) | 2022-11-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Huang et al. | Particle swarm optimization for solving the inverse kinematics of 7-DOF robotic manipulators | |
| Xu et al. | Motion planning of manipulators for simultaneous obstacle avoidance and target tracking: An RNN approach with guaranteed performance | |
| 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 | |
| Bosscher et al. | Real‐time collision avoidance algorithm for robotic manipulators | |
| Xu et al. | Mechanical arm obstacle avoidance path planning based on improved artificial potential field method | |
| Tanaka et al. | Modeling and control of head raising snake robots by using kinematic redundancy | |
| Bohez et al. | Sensor fusion for robot control through deep reinforcement learning | |
| CN110682286A (en) | Real-time obstacle avoidance method for cooperative robot | |
| Hua et al. | Reinforcement learning-based collision-free path planner for redundant robot in narrow duct | |
| Parsa et al. | Reconfigurable mass parameters to cross direct kinematic singularities in parallel manipulators | |
| Shi et al. | Adaptive image-based visual servoing for hovering control of quad-rotor | |
| Mbakop et al. | Path planning and control of mobile soft manipulators with obstacle avoidance | |
| Reimann et al. | Generating collision free reaching movements for redundant manipulators using dynamical systems | |
| Li et al. | Hybrid trajectory replanning-based dynamic obstacle avoidance for physical human-robot interaction | |
| Wei et al. | Motion planning for a humanoid mobile manipulator system | |
| Li et al. | Human-robot collaborative manipulation with the suppression of human-caused disturbance | |
| CN117182932B (en) | Method and device for planning obstacle avoidance action of mechanical arm and computer equipment | |
| Ananthanarayanan et al. | A fast converging optimal technique applied to path planning of hyper-redundant manipulators | |
| Akli | Trajectory planning for mobile manipulators including Manipulability Percentage Index | |
| Andaluz et al. | Numerical methods for cooperative control of double mobile manipulators | |
| CN113910236B (en) | Method, system, equipment and medium for planning movement of mobile double-arm robot | |
| Zhao et al. | Efficient inverse kinematics for redundant manipulators with collision avoidance in dynamic scenes | |
| Wang et al. | Whole body control of a dual-arm mobile robot using a virtual kinematic chain | |
| Rani et al. | An optimal control approach for hybrid motion/force control of coordinated multiple nonholonomic mobile manipulators using neural network |
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 |