CN113910236A - Motion planning method, system, equipment and medium for mobile dual-arm robot - Google Patents

Motion planning method, system, equipment and medium for mobile dual-arm robot Download PDF

Info

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
dual
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
Application number
CN202111277239.XA
Other languages
Chinese (zh)
Other versions
CN113910236B (en
Inventor
魏延
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Changan University
Original Assignee
Changan University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Changan University filed Critical Changan University
Priority to CN202111277239.XA priority Critical patent/CN113910236B/en
Publication of CN113910236A publication Critical patent/CN113910236A/en
Application granted granted Critical
Publication of CN113910236B publication Critical patent/CN113910236B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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

本发明公开了一种移动双臂机器人运动规划方法、系统、设备及介质，用于移动高冗余双臂机器人的智能动态运动规划，通过引入移动底座与左右手的相对逆可达性图，实现移动底座与左右手的协同运动规划；同时，根据移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图，能够确定既满足右手又满足左手操作性能需求的移动底座位姿与上臂操作位姿；本发明实现了移动双臂机器人在动态、非结构化环境中的智能仿人运动规划，过程简单，运动规划效果较好，规划效率较高。

Figure 202111277239

The invention discloses a motion planning method, system, equipment and medium for a mobile dual-arm robot, which is used for intelligent dynamic motion planning of a mobile high-redundancy dual-arm robot. The coordinated motion planning of the mobile base and the left and right hands; at the same time, according to the RiDRM diagram of the mobile base relative to the right hand and the LiDRM diagram of the mobile base relative to the left hand of the mobile dual-arm robot, the pose and posture of the mobile base that meet the performance requirements of both the right hand and the left hand can be determined. The operation posture of the upper arm; the invention realizes the intelligent humanoid motion planning of the mobile double-arm robot in a dynamic and unstructured environment, the process is simple, the motion planning effect is good, and the planning efficiency is high.

Figure 202111277239

Description

Method, system, equipment and medium for planning movement of mobile double-arm robot
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:
step 1, constructing a RiDRM graph of a mobile base of the mobile two-arm robot relative to a right hand and a LiDRM graph of the mobile base relative to a left hand.
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 1, giving a preset high-redundancy mobile double-arm robot, and 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.
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:
Figure BDA0003329889180000141
or
Figure BDA0003329889180000142
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:
Figure BDA0003329889180000151
Figure BDA0003329889180000152
Figure BDA0003329889180000153
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.一种移动双臂机器人运动规划方法，其特征在于，包括以下步骤：1. a mobile dual-arm robot motion planning method, is characterized in that, comprises the following steps: 构建移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图；Construct the RiDRM diagram of the mobile base relative to the right hand and the LiDRM diagram of the mobile base relative to the left hand of the mobile dual-arm robot; 根据待抓取目标物体，确定移动双臂机器人的右手期望抓取位姿及左手期望抓取位姿；According to the target object to be grasped, determine the desired grasping pose of the right hand and the desired grasping pose of the left hand of the mobile dual-arm robot; 对移动双臂机器人的待执行任务进行规划分解，得到若干子任务；并根据确定移动双臂机器人的右手期望抓取位姿及左手期望抓取位姿，分别确定每个子任务的右手期望位姿及左手期望位姿；Plan and decompose the tasks to be performed by the mobile dual-arm robot, and obtain several sub-tasks; and determine the expected right-hand pose of each sub-task according to the determination of the right-hand desired grasping pose and the left-hand desired grasping pose of the mobile dual-arm robot. and the desired pose of the left hand; 根据每个子任务的右手期望位姿及左手期望位姿，执行子任务，将移动双臂机器人的移动底座相对右手的RiDRM图变换至对应子任务的右手期望位置，以及将移动双臂机器人的移动底座相对左手的LiDRM图变换至对应子任务的左手期望位置；According to the right-hand desired pose and left-hand desired pose of each subtask, execute the subtask, transform the RiDRM diagram of the mobile base of the mobile dual-arm robot relative to the right hand to the right-hand desired position of the corresponding subtask, and convert the movement of the mobile dual-arm robot. The LiDRM map of the base relative to the left hand is transformed to the desired position of the left hand corresponding to the subtask; 动态检测环境中是否有新的障碍物；Dynamically detect whether there are new obstacles in the environment; 若有，则更新移动移动双臂机器人的移动底座相对右手的RiDRM图中位姿集的位姿可选使能及移动底座相对左手的LiDRM图中位姿集中的位姿可选使能；分别得到更新后的移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图；If so, update the optional enablement of the poses in the pose set in the RiDRM diagram of the mobile base relative to the right hand of the mobile mobile dual-arm robot and the optional enablement of the poses in the pose set in the LiDRM diagram of the mobile base relative to the left hand; respectively; Obtain the updated RiDRM diagram of the mobile base relative to the right hand and the LiDRM diagram of the mobile base relative to the left hand of the updated mobile dual-arm robot; 根据更新后的移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图，确定移动双臂机器人的移动底座的可选位姿集；According to the updated RiDRM diagram of the mobile base of the mobile dual-arm robot relative to the right hand and the LiDRM diagram of the mobile base relative to the left hand, determine the optional pose set of the mobile base of the mobile dual-arm robot; 设计移动双臂机器人的最优位姿集，并将移动双臂机器人的最优位姿集加入至移动双臂机器人的可选位姿集，得到更新后的移动双臂机器人的可选位姿集；Design the optimal pose set of the mobile two-arm robot, add the optimal pose set of the mobile two-arm robot to the optional pose set of the mobile two-arm robot, and obtain the updated optional pose of the mobile two-arm robot set; 利用评价函数综合评价更新后的移动双臂机器人的可选位姿集的位姿，确定移动机器人的最优位姿；Use the evaluation function to comprehensively evaluate the pose of the optional pose set of the updated mobile dual-arm robot, and determine the optimal pose of the mobile robot; 根据移动机器人的最优位姿，进行轨迹规划及轨迹跟踪控制；Carry out trajectory planning and trajectory tracking control according to the optimal pose of the mobile robot; 判断移动双臂机器人的待执行任务是否完成，如完成，则循环结束；如未完成，则返回执行下一个子任务。Determine whether the task to be executed of the mobile dual-arm robot is completed. If it is completed, the cycle ends; if it is not completed, it returns to execute the next subtask. 2.根据权利要求1所述的一种移动双臂机器人运动规划方法，其特征在于，动态检测环境中是否有新的障碍物过程，若未检测到新的障碍物，则根据移动双臂机器人的移动底座相对右手的RiDRM图及移动双臂机器人的移动底座相对左手的LiDRM图，确定移动双臂机器人的移动底座的可选位姿集。2. a kind of mobile dual-arm robot motion planning method according to claim 1 is characterized in that, whether there is a new obstacle process in the dynamic detection environment, if no new obstacle is detected, then according to the mobile dual-arm robot The RiDRM diagram of the mobile base relative to the right hand and the LiDRM diagram of the mobile base of the mobile dual-arm robot relative to the left hand, determine the optional pose set of the mobile base of the mobile dual-arm robot. 3.根据权利要求1所述的一种移动双臂机器人运动规划方法，其特征在于，设计移动双臂机器人的最优位姿，并将移动双臂机器人的最优位姿加入至移动双臂机器人的可选位姿集的过程，采用改进的MaxiMin NSGA-II算法设计得到移动双臂机器人的最优位姿。3. The motion planning method of a mobile dual-arm robot according to claim 1, wherein the optimal posture of the mobile dual-arm robot is designed, and the optimal posture of the mobile dual-arm robot is added to the mobile dual-arm In the process of the robot's optional pose set, the improved MaxiMin NSGA-II algorithm is used to design the optimal pose of the mobile dual-arm robot. 4.根据权利要求1所述的一种移动双臂机器人运动规划方法，其特征在于，根据移动机器人的最优位姿信息，进行轨迹规划及轨迹跟踪控制的过程，具体如下：4. a kind of mobile dual-arm robot motion planning method according to claim 1 is characterized in that, according to the optimal position and attitude information of the mobile robot, the process of carrying out trajectory planning and trajectory tracking control is as follows: 根据移动机器人的最优位姿，采用直接双向RRT和梯度下降算法进行轨迹规划，并使用自适应RBF神经网络进行轨迹跟踪控制。According to the optimal pose of the mobile robot, direct bidirectional RRT and gradient descent algorithms are used for trajectory planning, and adaptive RBF neural network is used for trajectory tracking control. 5.根据权利要求1所述的一种移动双臂机器人运动规划方法，其特征在于，构建移动双臂机器人的移动底座相对右手的RiDRM图的过程，具体如下：5. a kind of mobile dual-arm robot motion planning method according to claim 1, is characterized in that, the process of constructing the RiDRM diagram of the relative right hand of the mobile base of the mobile dual-arm robot is as follows: 构建移动双臂机器人的移动底座相对右手的RiDRM图的过程包括离线RiRM构建过程和在线RiDRM构建过程；The process of constructing the RiDRM diagram of the mobile base of the mobile dual-arm robot relative to the right hand includes the offline RiRM construction process and the online RiDRM construction process; 其中，离线RiRM构建过程，具体包括以下步骤：Among them, the offline RiRM construction process specifically includes the following steps: 在3D空间内，随机给定移动双臂机器人的右手目标位置，并均匀生成移动双臂机器人的右手目标位姿；In 3D space, the right-hand target position of the mobile dual-arm robot is randomly given, and the right-hand target pose of the mobile dual-arm robot is uniformly generated; 设定移动双臂机器人的右手目标位姿为参考坐标系；Set the right-hand target pose of the mobile dual-arm robot as the reference coordinate system; 根据移动双臂机器人的右手目标位姿，进行IK求解，得到右手候选位姿qkr；According to the right-hand target pose of the mobile dual-arm robot, IK is solved to obtain the right-hand candidate pose q kr ; 判断右手候选位姿qkr是否为可行解；Determine whether the right-hand candidate pose q kr is a feasible solution; 若是，则求解右手候选位姿qkr的评价函数fk，确定移动双臂机器人的移动底座的候选位姿qb；If yes, then solve the evaluation function f k of the right-hand candidate pose q kr to determine the candidate pose q b of the mobile base of the mobile dual-arm robot; 若否，则返回重新获取右手候选位姿；If not, return to re-acquire the candidate pose of the right hand; 构建与右手候选位姿qkr对应的RiRM空间像素vi的移动底座到达列表Rei，以及与右手候选位姿qkr对应的RiRM空间像素vi的移动底座占用列表Oi；Construct the moving base arrival list Re i of the RiRM space pixel v i corresponding to the right-hand candidate pose q kr , and the moving base occupancy list O i of the RiRM space pixel v i corresponding to the right-hand candidate pose q kr ; 判断右手候选位姿qkr的生成数量是否达到预设值N，若未达到，则返回继续获取右手候选位姿，否则，完成离线RiRM的构建；Determine whether the generation number of the right-hand candidate pose q kr reaches the preset value N, if not, return to continue to obtain the right-hand candidate pose, otherwise, complete the construction of the offline RiRM; 在线RiDRM的构建过程，具体包括以下步骤：The construction process of online RiDRM includes the following steps: 将RiDRM空间初始化为RiRM；Initialize the RiDRM space to RiRM; 动态检测环境中是否有新的障碍物，若无，则完成在线RiDRM的构建；Dynamically detect whether there are new obstacles in the environment, if not, complete the construction of online RiDRM; 若有，则设置与新的障碍物存在碰撞的右手候选位姿qkr的可选使能为否；同步更新RiDRM空间对应的像素vi的到达列表Rei，以及RiDRM空间对应的像素vi的占用列表Oi。If there is, set the optional enable of the right-hand candidate pose q kr that collides with the new obstacle to No; synchronously update the arrival list Re i of the pixel v i corresponding to the RiDRM space, and the pixel v i corresponding to the RiDRM space The occupancy list O i . 6.根据权利要求1所述的一种移动双臂机器人运动规划方法，其特征在于，构建移动双臂机器人的移动底座相对左手LiDRM图的过程，具体如下：6. a kind of mobile dual-arm robot motion planning method according to claim 1 is characterized in that, the process of constructing the mobile base of the mobile dual-arm robot relative to the left-hand LiDRM diagram is as follows: 构建移动双臂机器人的移动底座相对左手LiDRM图的过程包括离线LiRM构建过程和在线LiDRM构建过程；The process of constructing the LiDRM map of the mobile base relative to the left hand of the mobile dual-arm robot includes an offline LiRM construction process and an online LiDRM construction process; 其中，离线LiRM构建过程，具体包括以下步骤：Among them, the offline LiRM construction process specifically includes the following steps: 在3D空间内，随机给定移动双臂机器人的左手目标位置，并均匀生成移动双臂机器人的左手目标位姿；In the 3D space, the left-hand target position of the mobile dual-arm robot is randomly given, and the left-hand target pose of the mobile dual-arm robot is uniformly generated; 设定移动双臂机器人的左手目标位姿为参考坐标系；Set the left-hand target pose of the mobile dual-arm robot as the reference coordinate system; 根据移动双臂机器人的左手目标位姿，进行IK求解，得到左手候选位姿qkl；According to the left-hand target pose of the mobile dual-arm robot, IK solution is performed to obtain the left-hand candidate pose q kl ; 判断左手候选位姿qkl是否为可行解；Determine whether the left-hand candidate pose q kl is a feasible solution; 若是，则求解左手候选位姿qkl的评价函数fk，确定移动双臂机器人的移动底座的候选位姿qb；If yes, then solve the evaluation function f k of the left-hand candidate pose q kl to determine the candidate pose q b of the mobile base of the mobile dual-arm robot; 若否，则返回重新获取左手候选位姿；If not, return to re-obtain the candidate pose of the left hand; 构建与左手候选位姿qkl对应的LiRM空间像素vi的移动底座到达列表Rei，以及与右手候选位姿qkl对应的LiRM空间像素vi的移动底座占用列表Oi；Construct the mobile base arrival list Re i of the LiRM space pixel v i corresponding to the left-hand candidate pose q kl , and the mobile base occupancy list O i of the LiRM space pixel v i corresponding to the right-hand candidate pose q kl ; 判断左手候选位姿qkl的生成数量是否达到预设值N，若未达到，则返回继续获取左手候选位姿，否则，完成离线LiRM的构建；Determine whether the generated number of left-hand candidate poses q kl reaches the preset value N, if not, return to continue to obtain left-hand candidate poses, otherwise, complete the construction of offline LiRM; 在线LiDRM的构建过程，具体包括以下步骤：The construction process of online LiDRM includes the following steps: 将LiDRM空间初始化为LiRM；Initialize the LiDRM space to LiRM; 动态检测环境中是否有新的障碍物，若无，则完成在线LiDRM的构建；Dynamically detect whether there are new obstacles in the environment, if not, complete the construction of online LiDRM; 若有，则设置与新的障碍物存在碰撞的左手候选位姿qkl的可选使能为否；同步更新LiDRM空间对应的像素vi的到达列表Rei，以及LiDRM空间对应的像素vi的占用列表Oi。If so, set the optional enable of the left-hand candidate pose q kl that collides with the new obstacle to No; update the arrival list Re i of the pixel v i corresponding to the LiDRM space synchronously, and the pixel v i corresponding to the LiDRM space The occupancy list O i . 7.一种移动双臂机器人运动规划系统，其特征在于，包括：7. A mobile dual-arm robot motion planning system, characterized in that, comprising: 构建模块，构建移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图；Building modules, constructing the RiDRM diagram of the mobile base of the mobile dual-arm robot relative to the right hand and the LiDRM diagram of the mobile base relative to the left hand; 任务规划分解模块，用于根据待抓取目标物体，确定移动双臂机器人的右手期望抓取位姿及左手期望抓取位姿；对移动双臂机器人的待执行任务进行规划分解，得到若干子任务；并根据确定移动双臂机器人的右手期望抓取位姿及左手期望抓取位姿，分别确定每个子任务的右手期望位姿及左手期望位姿；The task planning and decomposition module is used to determine the desired grasping posture of the right hand and the desired grasping posture of the left hand of the mobile dual-arm robot according to the target object to be grasped; plan and decompose the tasks to be executed of the mobile dual-arm robot, and obtain a number of subsections. and determine the desired right-hand pose and left-hand desired pose of each subtask respectively according to the determination of the right-hand desired grasping pose and the left-hand desired grasping pose of the mobile dual-arm robot; 执行模块，用于根据每个子任务的右手期望位姿及左手期望位姿，执行子任务，将移动双臂机器人的移动底座相对右手的RiDRM图变换至对应子任务的右手期望位置，以及将移动双臂机器人的移动底座相对左手的LiDRM图变换至对应子任务的左手期望位置；动态检测环境中是否有新的障碍物；若有，则更新移动移动双臂机器人的移动底座相对右手的RiDRM图中位姿集的位姿可选使能及移动底座相对左手的LiDRM图中位姿集中的位姿可选使能；分别得到更新后的移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图；The execution module is used to execute the subtasks according to the desired right-hand pose and the desired left-hand pose of each subtask, transform the RiDRM diagram of the mobile base of the mobile dual-arm robot relative to the right hand to the right-hand desired position of the corresponding subtask, and convert the movement The mobile base of the dual-arm robot transforms relative to the left-hand LiDRM map to the desired position of the left hand corresponding to the subtask; dynamically detects whether there are new obstacles in the environment; if so, updates the mobile dual-arm robot's mobile base relative to the right-hand RiDRM map The poses of the middle pose set can be optionally enabled and the poses of the mobile base relative to the left-hand LiDRM diagram can be optionally enabled; respectively obtain the updated RiDRM diagram and movement of the mobile base relative to the right hand of the mobile dual-arm robot. LiDRM diagram of the base relative to the left hand; 优化模块，用于根据更新后的移动双臂机器人的移动底座相对右手的RiDRM图及移动底座相对左手的LiDRM图，确定移动双臂机器人的移动底座的可选位姿集；设计移动双臂机器人的最优位姿集，并将移动双臂机器人的最优位姿集加入至移动双臂机器人的可选位姿集，得到更新后的移动双臂机器人的可选位姿集；利用评价函数综合评价更新后的移动双臂机器人的可选位姿集的位姿，确定移动机器人的最优位姿；根据移动机器人的最优位姿，进行轨迹规划及轨迹跟踪控制；The optimization module is used to determine the optional pose set of the mobile base of the mobile two-arm robot according to the updated RiDRM diagram of the mobile base relative to the right hand and the LiDRM diagram of the mobile base relative to the left hand; design the mobile two-arm robot The optimal pose set of the mobile two-arm robot is added to the optional pose set of the mobile two-arm robot to obtain the updated optional pose set of the mobile two-arm robot; using the evaluation function Comprehensively evaluate the pose of the optional pose set of the updated mobile dual-arm robot, and determine the optimal pose of the mobile robot; carry out trajectory planning and trajectory tracking control according to the optimal pose of the mobile robot; 结束模块，用于判断移动双臂机器人的待执行任务是否完成，如完成，则循环结束；如未完成，则返回执行下一个子任务。The end module is used to judge whether the to-be-executed task of the mobile dual-arm robot is completed. If it is completed, the cycle ends; if it is not completed, it returns to execute the next subtask. 8.一种移动双臂机器人运动规划设备，其特征在于，包括：8. A mobile dual-arm robot motion planning device, characterized in that, comprising: 存储器，用于存储计算机程序；memory for storing computer programs; 处理器，用于执行所述计算机程序时实现如权利要求1-6任一项所述一种移动双臂机器人运动规划设备方法的步骤。The processor is configured to implement the steps of a method for moving a dual-arm robot motion planning device according to any one of claims 1 to 6 when executing the computer program. 9.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1-6任一项所述一种移动双臂机器人运动规划设备方法的步骤。9. A computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, wherein when the computer program is executed by a processor, a movement according to any one of claims 1-6 is realized. Steps of a dual-arm robot motion planning device method.
CN202111277239.XA 2021-10-29 2021-10-29 Motion planning method, system, device and medium for mobile dual-arm robot Active CN113910236B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111277239.XA CN113910236B (en) 2021-10-29 2021-10-29 Motion planning method, system, device and medium for mobile dual-arm robot

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111277239.XA CN113910236B (en) 2021-10-29 2021-10-29 Motion planning method, system, device and medium for mobile dual-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 Motion planning method, system, device and medium for mobile dual-arm robot

Country Status (1)

Country Link
CN (1) CN113910236B (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115958608A (en) * 2023-01-15 2023-04-14 长安大学 Motion planning method, system, device and medium for mobile dual-arm robot

Citations (6)

* Cited by examiner, † Cited by third party
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

Patent Citations (6)

* Cited by examiner, † Cited by third party
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

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115958608A (en) * 2023-01-15 2023-04-14 长安大学 Motion planning method, system, device and medium for mobile dual-arm robot

Also Published As

Publication number Publication date
CN113910236B (en) 2022-11-29

Similar Documents

Publication Publication Date Title
Jin et al. Distributed task allocation of multiple robots: A control perspective
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
CN108287469B (en) Mechanical arm optimal obstacle avoidance control method and device based on artificial potential field gravitation factor
CN109976347B (en) Visual servo path planning method based on rapid expansion random tree and potential field method
Bohez et al. Sensor fusion for robot control through deep reinforcement learning
Xu et al. Mechanical arm obstacle avoidance path planning based on improved artificial potential field method
CN109591014B (en) Double-arm coordinated carrying method of double-arm coordinated robot
Gómez-Bravo et al. Collision free trajectory planning for hybrid manipulators
Hua et al. Reinforcement learning-based collision-free path planner for redundant robot in narrow duct
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
Muñoz et al. Geometrically constrained path planning for robotic grasping with Differential Evolution and Fast Marching Square
CN113910236A (en) Motion planning method, system, equipment and medium for mobile dual-arm robot
Akli Trajectory planning for mobile manipulators including Manipulability Percentage Index
Tang et al. Coordinated motion planning of dual-arm space robot with deep reinforcement learning
Ananthanarayanan et al. A fast converging optimal technique applied to path planning of hyper-redundant manipulators
Zhao et al. Efficient inverse kinematics for redundant manipulators with collision avoidance in dynamic scenes
Rani et al. An optimal control approach for hybrid motion/force control of coordinated multiple nonholonomic mobile manipulators using neural network
Zhu et al. Single-step collision-free trajectory planning of biped climbing robots in spatial trusses
Al-Shanoon et al. Learn to grasp unknown-adjacent objects for sequential robotic manipulation
Zhao et al. A cooperative obstacle-avoidance approach for two-manipulator based on A* algorithm
Abainia et al. Bio-inspired approach for inverse kinematics of 6-dof robot manipulator with obstacle avoidance
Yuan et al. 6-DOF industrial manipulator motion planning based on RRT-connect algorithm

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