[go: up one dir, main page]

CN116652958B - A master-slave isomorphic robot teleoperation safety control method and system - Google Patents

A master-slave isomorphic robot teleoperation safety control method and system

Info

Publication number
CN116652958B
CN116652958B CN202310761239.XA CN202310761239A CN116652958B CN 116652958 B CN116652958 B CN 116652958B CN 202310761239 A CN202310761239 A CN 202310761239A CN 116652958 B CN116652958 B CN 116652958B
Authority
CN
China
Prior art keywords
slave
robot
slave robot
obstacle
acceleration
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202310761239.XA
Other languages
Chinese (zh)
Other versions
CN116652958A (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.)
Hebei University of Technology
Original Assignee
Hebei University of Technology
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 Hebei University of Technology filed Critical Hebei University of Technology
Priority to CN202310761239.XA priority Critical patent/CN116652958B/en
Publication of CN116652958A publication Critical patent/CN116652958A/en
Application granted granted Critical
Publication of CN116652958B publication Critical patent/CN116652958B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J3/00—Manipulators of leader-follower type, i.e. both controlling unit and controlled unit perform corresponding spatial movements
    • 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/1661—Programme controls characterised by programming, planning systems for manipulators characterised by task planning, object-oriented languages
    • 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
    • B25J9/1666—Avoiding collision or forbidden zones
    • 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/1689—Teleoperation
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
    • Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
    • Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Manipulator (AREA)

Abstract

The invention relates to a teleoperation safety control method and system for a slave-end robot, wherein the method collects environment images of a working space of the slave-end robot and judges whether an obstacle exists, if the obstacle exists, expected acceleration of the tail end of the slave-end robot generated by virtual attractive force and repulsive force is calculated and mapped into a joint space to obtain expected angular acceleration of each joint, the expected angular acceleration is mapped into the joint space, meanwhile expected acceleration of a moving chassis of the slave-end robot generated by the virtual attractive force and repulsive force is calculated and acted into a motion space, if the obstacle does not exist, expected angular acceleration of each joint of the slave-end robot is calculated and acted into the joint space of the slave-end robot, and meanwhile expected acceleration of the moving chassis of the slave-end robot generated by virtual attractive force is calculated and acted into the motion space. The remote operation and obstacle avoidance of the robot are combined, so that the slave end robot can stably follow the motion track of the master end robot while the safety obstacle avoidance of the slave end robot is realized, and the aim of synchronous motion of the master end robot and the slave end robot is fulfilled.

Description

Master-slave isomorphic robot teleoperation safety control method and system
Technical Field
The invention belongs to the technical field of teleoperation of robots, and particularly relates to a teleoperation safety control method and system for a master-slave isomorphic robot.
Background
The teleoperation of the robot means that an operator controls the master end robot, the slave end robot follows the motion track of the master end robot to enable the two robots to synchronously move, and then various operations are completed through the slave end robot, so that the robot replaces a human to execute various tasks in environments which cannot touch or even endanger the health or life safety of the human, and the perception capability of the human is extended. The teleoperation of the robot can overcome the limitation of the geographic space and is widely applied to the fields of health care, space exploration, deep sea exploration and the like.
Different from the working environment of the traditional industrial robot, the working environment of the teleoperation robot is usually an unstructured environment with obstacles, has more uncertainty and complex and changeable tasks, so that the teleoperation robot is required to have the stability, reliability and high precision of the traditional industrial robot, and can accurately avoid the obstacles according to the external environment, so that the slave end robot can safely avoid the obstacles while completely following the motion trail of the master end robot, and the sharing control of the robot and the slave end robot is realized. In summary, the invention provides a teleoperation safety control method and a teleoperation safety control system for a master-slave isomorphic robot, which combine teleoperation and obstacle avoidance of the robot, realize safe obstacle avoidance of a slave-end robot based on a manual potential field method, and enable the slave-end robot to stably follow the motion trail of the master-end robot so as to achieve the aim of synchronous motion of the master-end robot and the slave-end robot.
Disclosure of Invention
Aiming at the defects of the prior art, the invention aims to provide a remote operation safety control method and system for a robot with isomorphic master and slave.
The invention solves the technical problems by adopting the following technical scheme:
The teleoperation safety control method of the robot with the same master-slave structure is characterized by comprising the following steps:
the method comprises the steps of firstly, collecting joint angles, tail end pose and moving speed of a movable chassis of a master end robot and a slave end robot, collecting environment images of a working space of the slave end robot, and judging whether an obstacle exists in the working space of the slave end robot;
if an obstacle exists, calculating the expected acceleration of the tail end of the slave robot generated by the virtual attractive force according to the following formula;
Δp=p-p0 (1)
Fa＝ka·Δp (2)
Wherein Deltap is the end pose error of the master end robot and the slave end robot, p 0 is the end pose of the slave end robot, p is the end pose of the master end robot, F a is the virtual attractive force received by the end of the slave end robot, k a is a constant matrix, For the expected acceleration of the slave end robot end generated by the virtual attractive force, M a is a virtual centroid inertia matrix of the slave end robot end, and 0 3×1 is a zero matrix;
mapping the expected acceleration of the tail end of the slave end robot generated by the virtual gravitation into a joint space through a jacobian matrix to obtain the expected angular acceleration of each joint of the slave end robot generated by the virtual gravitation;
calculating a desired acceleration of the slave robot moving chassis generated by the virtual attractive force;
Δv=v-v0 (5)
Fm＝ka·Δv (6)
Wherein Deltav is the motion speed error of the moving chassis of the slave end robot, v and v 0 are the motion speeds of the moving chassis of the master end robot and the slave end robot respectively, F m is the virtual attractive force received by the moving chassis of the slave end robot, M m is the virtual mass center inertia matrix of the moving chassis of the slave end robot, A desired acceleration of the slave end robot chassis for virtual attraction;
Calculating the virtual repulsive force of the obstacle to a connecting rod lm by the formula (8), wherein the connecting rod lm is the connecting rod closest to the obstacle from the end robot;
Wherein d lm is the shortest distance from the link lm to the obstacle envelope box, k r is a constant coefficient, d is a safety distance, f r is the virtual repulsive force of the obstacle to the link lm, and the direction of the virtual repulsive force is from the obstacle to the foot of the link lm;
Calculating the expected acceleration of the end of the slave end robot taking the connecting rod lm as the end by using the formula (9), and mapping the expected acceleration of the end into a joint space through a jacobian matrix to obtain the expected angular acceleration of each joint of the slave end robot generated by virtual repulsive force;
In the formula, For a virtual repulsive matrix of the obstacle to the link lm,AndAs the component of the virtual repulsive force f r of the obstacle to the link lm on three coordinate axes,For the end desired acceleration of the slave end robot ending with the link lm, M lm is the virtual centroid inertial matrix of the slave end robot ending with the link lm;
Calculating the expected angular acceleration of each joint of the slave end robot according to the formula (11), and applying the expected angular acceleration of each joint of the slave end robot into the joint space, wherein the expected angular velocity of each joint of the slave end robot is as shown in the formula (12);
In the formula, For a desired angular acceleration matrix of the slave robotic joint,For the desired angular acceleration matrix of the slave end robot joint at time t +1,Respectively obtaining expected angular velocity matrixes of the slave robot joints at the moments t and t+1, wherein alpha is a weight coefficient of obstacle avoidance of the mechanical arm;
Calculating the expected acceleration of the chassis of the secondary end robot by the formulas (13) and (14), and applying the expected acceleration of the chassis of the secondary end robot to a motion space, wherein the motion speed of the chassis of the secondary end robot is as shown in the formula (15);
In the formula, For the expected acceleration of the chassis of the slave end robot due to the virtual repulsive force, r is the distance vector from the geometric center of the obstacle to the geometric center of the slave end robot, M 0 is the inertial matrix of the whole slave end robot,For the desired acceleration of the chassis of the slave end robot, 1-alpha is the weight coefficient of the chassis of the slave end robot to avoid the obstacle,The motion speeds of the chassis are respectively moved by the slave end robot at the time t+1 and the time t,The expected acceleration of the chassis is moved by the slave end robot at the time t+1;
Thirdly, if no obstacle exists, calculating expected angular acceleration of each joint of the slave end robot by the following formula, and applying the expected angular acceleration of each joint to a joint space of the slave end robot;
Δθ=θ-θ0(16)
Wherein delta theta is a joint angle error matrix, and theta 0 are joint angle matrices of the master end robot and the slave end robot respectively;
Calculating the expected acceleration of the slave robot moving chassis due to the virtual attractive force according to the formula (7), and calculating the expected acceleration of the slave robot moving chassis due to the virtual repulsive force Set to zero and calculate the desired acceleration of the slave end robot moving chassis by equation (14), and apply the desired acceleration of the slave end robot moving chassis into the motion space.
Compared with the prior art, the invention has the beneficial effects that:
1. The existing artificial potential field obstacle avoidance method is to make the obstacle equivalent to a sphere, and substitute the shortest distance from the geometric center of the sphere to the robot into a repulsive force function to calculate virtual repulsive force, wherein the obstacle is actually a three-dimensional object, and the equivalent of the obstacle is obviously not in accordance with the reality, but the invention uses an enveloping box to make the obstacle equivalent, thereby retaining the three-dimensional characteristics of the obstacle, and simultaneously uses a capsule equivalent model to simplify the slave robot, calculate the shortest distance from the robot connecting rod to all edges of the enveloping box, and substitute the shortest distance into the repulsive force function to calculate virtual repulsive force, so that the slave robot has good obstacle avoidance performance, and the safety of the slave robot is improved.
2. According to the invention, teleoperation and obstacle avoidance of the robots are combined, the expected acceleration of the tail end of the slave end robot and the expected acceleration of the moving chassis of the slave end robot, which are generated by virtual gravitation and repulsive force, are calculated based on the artificial potential field concept, the expected acceleration of the tail end of the slave end robot is acted in the joint space, the expected acceleration of the moving chassis of the slave end robot is acted in the motion space, the safe obstacle avoidance of the slave end robot is realized, the slave end robot stably follows the motion track of the master end robot, the aim of synchronous motion of the master end robot and the slave end robot is achieved, and the slave end robot simultaneously has the capability of tracking the master end robot and avoiding the obstacle in the teleoperation process.
Drawings
FIG. 1 is an overall flow chart;
FIG. 2 is a simplified schematic diagram of a slave end robotic link;
fig. 3 is a schematic diagram of calculating the shortest distance between two line segments.
Detailed Description
The following specific embodiments are given by way of illustration only and not by way of limitation of the scope of the application.
The invention discloses a teleoperation safety control system of a master-slave isomorphic robot, which comprises a master end robot, a master end controller, a slave end robot, a slave end controller and a vision sensor, wherein the master end robot and the master end controller are positioned in a remote control room, the slave end robot and the slave end controller are positioned in an actual working environment, and the vision sensor is arranged on the slave end robot and is used for collecting an environment image. The method comprises the steps that an RS485 bus is adopted between a master end robot and a master end controller to communicate, a TCP protocol is adopted between the master end controller and a slave end controller, an EtherCAT protocol is adopted between the slave end controller and the slave end robot to communicate, a Kienct sensor is adopted by a visual sensor and data transmission is carried out by using a USB-C data interface, the master end controller collects joint angles and tail end pose of the master end robot and sends the joint angles and tail end pose to the slave end controller, the slave end controller collects joint angles, tail end pose and movement speed of a movable chassis of the slave end robot, and receives environmental images collected by the visual sensor, and the slave end controller serves as a processor of the method. The main end robot and the auxiliary end robot are both double-arm robots, each double-arm robot comprises a movable chassis and left and right mechanical arms with 8 degrees of freedom, each mechanical arm comprises 7 connecting rods, the waist shares two connecting rods, the tail ends of the left and right mechanical arms of the main end robot are respectively provided with a handle used for controlling the movement of the robot, the left handle controls the robot to move back and forth and turn left and right, the right handle controls the robot to move left and right and turn right, and the movement speed of the movable chassis of the main end robot can be obtained through the handles.
A master-slave isomorphic robot teleoperation safety control method comprises the following steps:
The method comprises the steps that a first step, a main end controller collects joint angles, tail end pose and movement speed of a movable chassis of a main end robot and sends the joint angles, tail end pose and movement speed of the movable chassis to a slave end controller, the slave end controller collects joint angles, tail end pose and movement speed of the movable chassis of the slave end robot, a vision sensor collects environment images, the slave end controller processes the environment images, and whether an obstacle exists in a space of the slave end robot is judged;
If an obstacle exists, calculating the position and posture errors of the tail ends of the master and slave robots, calculating the virtual attractive force received by the tail ends of the slave robots according to the position and posture errors of the tail ends, and calculating the expected acceleration of the tail ends of the slave robots, which is generated by the virtual attractive force;
Δp=p-p0 (1)
Fa＝ka·Δp (2)
Wherein Deltap is the end pose error of the master end robot and the slave end robot, p 0 is the end pose of the slave end robot, p is the end pose of the master end robot, namely the expected end pose of the slave end robot, F a is the virtual attractive force received by the end of the slave end robot, k a is a constant matrix, For the expected acceleration of the slave end robot end generated by the virtual attractive force, M a is a virtual centroid inertia matrix of the slave end robot end, and 0 3×1 is a zero matrix;
mapping the expected acceleration of the tail end of the slave end robot generated by the virtual gravitation into a joint space through a jacobian matrix to obtain the expected angular acceleration of each joint of the slave end robot generated by the virtual gravitation;
In the formula, The desired angular acceleration matrix of the slave end robot joint, J -1 being the inverse of the slave end robot jacobian,For the angular velocity jacobian of the slave robot,Delta is a constant for the velocity of the slave robot tip;
Calculating the motion speed errors of the moving chassis of the master end robot and the slave end robot, calculating the virtual gravitation received by the moving chassis of the slave end robot according to the motion speed errors of the moving chassis, and calculating the expected acceleration of the moving chassis of the slave end robot generated by the virtual gravitation;
Δv=v-v0 (5)
Fm＝ka·Δv (6)
Wherein Deltav is the motion speed error of the moving chassis of the slave end robot, v and v 0 are the motion speeds of the moving chassis of the master end robot and the slave end robot respectively, F m is the virtual attractive force received by the moving chassis of the slave end robot, M m is the virtual mass center inertia matrix of the moving chassis of the slave end robot, A desired acceleration of the slave end robot chassis for virtual attraction;
The aim of safety control is to enable the slave end robot to have good obstacle avoidance performance, the slave end robot has the premise of avoiding obstacles, the distance between each connecting rod and each obstacle is calculated, and the accurate distance cannot be calculated usually due to the complexity of the mechanical structure of the robot; in order to calculate the distance between the obstacle and each connecting rod of the slave robot, firstly, adopting an enveloping box method to carry out enveloping on the slave robot and the obstacle, using an OBB enveloping box to carry out enveloping on each connecting rod of the slave robot mechanical arm by using a capsule equivalent model, referring to fig. 2, then, calculating the shortest distance from the connecting rod of the slave robot to the obstacle enveloping box, firstly, calculating the shortest distance from the connecting rod to each edge of the obstacle enveloping box, namely, the shortest distance from the connecting rod to the obstacle enveloping box, assuming that a line segment l 1 is any edge of the obstacle enveloping box, two end points of a line segment l 1 are marked as points P 1 and P 2, a line segment l 2 is any connecting rod of the slave robot, and two end points of a line segment l 2 are marked as points Q 1 and Q 2;
line segment l1：P(ω1)＝P1+ω1S1,S1＝P2-P1,ω1∈[0,1]
Line segment l2：Q(ω2)＝Q1+ω2S2,S2＝Q2-Q1,ω2∈[0,1]
The shortest distance between line segments l 1 and l 2 can be translated into a constrained optimization problem:
minf(ω1,ω2)＝‖P(ω1)-Q(ω2)‖2＝‖(P1+ω1S1)-(Q1+ω2S2)‖2
s.t.ω1,ω2∈[0,1]
By minimum conditions: The method can obtain:
if 0≤ω 1,ω2≤1, the shortest distance d min＝f(ω1,ω2 between line segments l 1 and l 2), otherwise, the shortest distances d 1 and d 2 from the point P 1、P2 to the line segment l 2, respectively, and the shortest distances d 3 and d 4 from the point Q 1、Q2 to the line segment l 1, respectively, are calculated, so that the shortest distance d min＝min{d1,d2,d3,d4 between line segments l 1 and l 2 can be obtained.
Calculating the shortest distance between all the connecting rods and the obstacle enveloping box according to the mode, and selecting a connecting rod lm closest to the obstacle from the end robot according to the shortest distance between all the connecting rods of the end robot and the obstacle enveloping box;
Wherein d lm is the shortest distance from the link lm to the obstacle envelope box, k r is a constant coefficient, d is a safety distance, and repulsive force is not generated when the shortest distance d lm is greater than or equal to the safety distance d;
calculating the expected acceleration of the tail end robot taking the tail end of the connecting rod lm as a tail end by utilizing a formula (9) according to the virtual attractive force F a received by the tail end of the tail end robot and the virtual repulsive force of the obstacle to the connecting rod lm, and mapping the expected acceleration of the tail end into a joint space through a jacobian matrix to obtain the expected angular acceleration of each joint of the tail end robot, which is generated by the virtual repulsive force;
In the formula, For a virtual repulsive matrix of the obstacle to the link lm,AndAs the component of the virtual repulsive force f r of the obstacle to the link lm on three coordinate axes,For the desired acceleration from the end robot at the end of link lm, M lm is the virtual centroid inertial matrix of the end robot at the end of link lm,A desired angular acceleration matrix of the slave end robot joint generated for the virtual repulsive force,Represents the inverse of the jacobian matrix of the slave robot ending in link lm,A jacobian matrix of the angular velocity of the slave robot terminating in a link lm is shown,Representing the speed of the slave robot tip ending in link lm;
Calculating the expected angular acceleration of each joint of the slave end robot according to the formula (11), and applying the expected angular velocity of each joint of the slave end robot to the joint space to enable each joint of the slave end robot to rotate according to the expected angular velocity;
In the formula, For a desired angular acceleration matrix of the slave robotic joint,For the desired angular acceleration matrix of the slave end robot joint at time t +1,Respectively obtaining expected angular velocity matrixes of the slave robot joints at the moments t and t+1, wherein alpha is a weight coefficient of obstacle avoidance of the mechanical arm;
Calculating the expected acceleration of the moving chassis of the slave end robot, which is generated by the virtual repulsive force, according to the virtual repulsive force of the obstacle to the connecting rod lm through the formula (13), and calculating the expected acceleration of the moving chassis of the slave end robot;
In the formula, For the expected acceleration of the chassis of the slave end robot due to the virtual repulsive force, r is the distance vector from the geometric center of the obstacle to the geometric center of the slave end robot, M 0 is the inertial matrix of the whole slave end robot,For the expected acceleration of the chassis of the slave end robot, 1-alpha is the weight coefficient of the obstacle avoidance of the chassis of the slave end robot;
the slave end controller applies the expected acceleration of the slave end robot moving chassis to the motion space to enable the slave end robot to synchronously move with the master end robot, and the motion speed of the slave end robot moving chassis is expressed as:
In the formula, The motion speeds of the chassis are respectively moved by the slave end robot at the time t+1 and the time t,The desired acceleration of the chassis is moved for the slave end robot at time t+1.
Thirdly, if no obstacle exists, calculating joint angle errors of the master and slave robots through a formula (16), calculating expected angular acceleration of each joint of the slave robots according to the joint angle errors, and applying the expected angular acceleration of each joint to a joint space of the slave robots through a formula (17);
Δθ=θ-θ0 (16)
Wherein delta theta is a joint angle error matrix, and theta 0 are joint angle matrices of the master end robot and the slave end robot respectively;
Meanwhile, according to the formula (7), the expected acceleration of the moving chassis of the slave robot generated by the virtual attractive force is calculated, and the virtual attractive force is only present because no obstacle is present Set to zero, calculate the expected acceleration of the chassis of the slave end robot by the equation (14), and apply the expected acceleration of the chassis of the slave end robot to the motion space, make the slave end robot move synchronously with the master end robot.
The invention is applicable to the prior art where it is not described.

Claims (3)

1.一种主从同构的机器人遥操作安全控制方法，其特征在于，该方法包括以下步骤：1. A method for safe remote control of a master-slave isomorphic robot, characterized in that the method comprises the following steps: 第一步、采集主、从端机器人的关节角度、末端位姿和移动底盘的运动速度，同时采集从端机器人工作空间的环境图像，并判断从端机器人工作空间中是否存在障碍物；The first step is to collect the joint angles, end-end poses, and movement speed of the mobile chassis of the master and slave robots, and at the same time collect the environmental image of the slave robot's workspace and determine whether there are any obstacles in the slave robot's workspace; 第二步、若存在障碍物，则通过下式计算虚拟引力产生的从端机器人末端的期望加速度；Step 2: If there is an obstacle, calculate the expected acceleration of the slave robot end caused by the virtual gravity using the following formula; Δp＝p-p0 (1)Δp＝pp 0 (1) Fa＝ka·Δp (2)F a = k a ·Δp (2) 式中，Δp为主、从端机器人的末端位姿误差，p0为从端机器人的末端位姿，p为主端机器人的末端位姿，Fa为从端机器人末端受到的虚拟引力，ka为常数矩阵，为虚拟引力产生的从端机器人末端的期望加速度，Ma为从端机器人末端的虚拟质心惯性矩阵，03×1为零矩阵；Where Δp is the end-position error between the master and slave robots, p0 is the end-position of the slave robot, p is the end-position of the master robot, Fa is the virtual gravity received by the end of the slave robot, and ka is a constant matrix. is the expected acceleration of the slave robot end caused by the virtual gravity, Ma is the virtual center of mass inertia matrix of the slave robot end, and 0 3×1 is the zero matrix; 通过雅可比矩阵将虚拟引力产生的从端机器人末端的期望加速度映射到关节空间中，得到虚拟引力产生的从端机器人各个关节的期望角加速度；The desired acceleration of the slave robot end generated by the virtual gravity is mapped to the joint space through the Jacobian matrix to obtain the desired angular acceleration of each joint of the slave robot generated by the virtual gravity; 通过下式计算虚拟引力产生的从端机器人移动底盘的期望加速度；The expected acceleration of the slave robot's mobile chassis generated by the virtual gravity is calculated by the following formula: Δv＝v-v0 (5)Δv＝vv 0 (5) Fm＝ka·Δv (6)F m = k a ·Δv (6) 式中，Δv为从端机器人移动底盘的运动速度误差，v、v0分别为主、从端机器人移动底盘的运动速度，Fm为从端机器人移动底盘受到的虚拟引力，Mm为从端机器人移动底盘的虚拟质心惯性矩阵，为虚拟引力产生的从端机器人移动底盘的期望加速度；Where Δv is the motion velocity error of the slave robot mobile chassis, v and v0 are the motion velocities of the master and slave robot mobile chassis respectively, Fm is the virtual gravitational force on the slave robot mobile chassis, and Mm is the virtual center of mass inertia matrix of the slave robot mobile chassis. The expected acceleration of the slave robot's mobile chassis generated by the virtual gravity; 通过式(8)计算障碍物对连杆lm的虚拟斥力，连杆lm为从端机器人距离障碍物最近的连杆；The virtual repulsive force of the obstacle on the link lm is calculated by formula (8), where the link lm is the link closest to the obstacle on the slave robot. 式中，dlm为连杆lm到障碍物包络盒的最短距离，kr为常系数，d为安全距离，fr为障碍物对连杆lm的虚拟斥力，方向由障碍物垂足指向连杆lm的垂足；Where dlm is the shortest distance from the link lm to the obstacle envelope, kr is a constant coefficient, d is the safety distance, and fr is the virtual repulsive force of the obstacle on the link lm, with the direction pointing from the foot of the obstacle perpendicular to the foot of the link lm. 利用式(9)计算从端机器人以连杆lm为末端的末端期望加速度，并通过雅可比矩阵将该末端期望加速度映射到关节空间中，得到虚拟斥力产生的从端机器人各个关节的期望角加速度；Formula (9) is used to calculate the desired acceleration of the slave robot with the link lm as the end, and the desired acceleration of the end is mapped to the joint space through the Jacobian matrix to obtain the desired angular acceleration of each joint of the slave robot generated by the virtual repulsive force; 式中，为障碍物对连杆lm的虚拟斥力矩阵，和为障碍物对连杆lm的虚拟斥力fr在三个坐标轴上的分量，为从端机器人以连杆lm为末端的末端期望加速度，Mlm为从端机器人以连杆lm为末端的虚拟质心惯性矩阵；Where, is the virtual repulsive force matrix of the obstacle on the link lm, and is the component of the virtual repulsive force fr of the obstacle on the connecting rod lm on the three coordinate axes, is the desired acceleration of the slave robot with the link lm as the end, M lm is the inertia matrix of the virtual center of mass of the slave robot with the link lm as the end; 根据式(11)计算从端机器人各个关节的期望角加速度，并将从端机器人各个关节的期望角加速度作用到关节空间中，从端机器人关节的期望角速度如式(12)；Calculate the expected angular acceleration of each joint of the slave robot according to formula (11), and apply the expected angular acceleration of each joint of the slave robot to the joint space. The expected angular velocity of the joint of the slave robot is as shown in formula (12); 式中，为从端机器人关节的期望角加速度矩阵，为t+1时刻从端机器人关节的期望角加速度矩阵，分别为t、t+1时刻从端机器人关节的期望角速度矩阵，α为机械臂避障的权重系数；Where, is the expected angular acceleration matrix of the slave robot joint, is the expected angular acceleration matrix of the slave robot joint at time t+1, are the expected angular velocity matrices of the slave robot joints at time t and t+1, respectively, and α is the weight coefficient of the robot arm's obstacle avoidance; 通过式(13)、(14)计算从端机器人移动底盘的期望加速度，并将从端机器人移动底盘的期望加速度作用到运动空间中，从端机器人移动底盘的运动速度如式(15)；The expected acceleration of the slave robot's mobile chassis is calculated by equations (13) and (14), and the expected acceleration of the slave robot's mobile chassis is applied to the motion space. The motion speed of the slave robot's mobile chassis is as shown in equation (15); 式中，为虚拟斥力产生的从端机器人移动底盘的期望加速度，r为障碍物几何中心到从端机器人几何中心的距离矢量，M0为从端机器人整体的惯性矩阵，为从端机器人移动底盘的期望加速度，1-α为从端机器人移动底盘避障的权重系数，分别为t+1、t时刻从端机器人移动底盘的运动速度，为t+1时刻从端机器人移动底盘的期望加速度；Where, is the expected acceleration of the slave robot's mobile chassis generated by the virtual repulsion, r is the distance vector from the geometric center of the obstacle to the geometric center of the slave robot, M0 is the inertia matrix of the slave robot as a whole, is the expected acceleration of the slave robot’s mobile chassis, 1-α is the weight coefficient of the slave robot’s mobile chassis’ obstacle avoidance, are the movement speeds of the robot's chassis at time t+1 and t, respectively. is the expected acceleration of the slave robot moving chassis at time t+1; 第三步，若不存在障碍物，则通过下式计算从端机器人各个关节的期望角加速度，并将关节的期望角加速度作用到从端机器人的关节空间中；In the third step, if there are no obstacles, the expected angular acceleration of each joint of the slave robot is calculated using the following formula, and the expected angular acceleration of the joint is applied to the joint space of the slave robot; Δθ＝θ-θ0 (16)Δθ＝θ-θ 0 (16) 式中，Δθ为关节角度误差矩阵，θ、θ0分别为主、从端机器人的关节角度矩阵；Where Δθ is the joint angle error matrix, θ and θ0 are the joint angle matrices of the master and slave robots respectively; 根据式(7)计算虚拟引力产生的从端机器人移动底盘的期望加速度，将虚拟斥力产生的从端机器人移动底盘的期望加速度设置为零，并通过式(14)计算从端机器人移动底盘的期望加速度，将从端机器人移动底盘的期望加速度作用到运动空间中。According to formula (7), the expected acceleration of the slave robot's mobile chassis generated by the virtual gravity is calculated, and the expected acceleration of the slave robot's mobile chassis generated by the virtual repulsion is calculated as Set it to zero, and calculate the expected acceleration of the slave robot's mobile chassis through formula (14), and apply the expected acceleration of the slave robot's mobile chassis to the motion space. 2.根据权利要求1所述的主从同构的机器人遥操作安全控制方法，其特征在于，在第二步中，采用OBB包围盒对障碍物进行等效，得到障碍物包络盒；采用胶囊等效模型对从端机器人进行等效，得到从端机器人的各个连杆；2. The master-slave isomorphic robot teleoperation safety control method according to claim 1 is characterized in that, in the second step, an OBB bounding box is used to equate the obstacle to obtain an obstacle envelope; and a capsule equivalent model is used to equate the slave robot to obtain each link of the slave robot; 计算从端机器人连杆到障碍物包络盒各条棱的最短距离，这些最短距离的最小值即为从端机器人连杆到障碍物包络盒的最短距离；假定线段l1为障碍物包络盒的任意一条棱，线段l1的两端点记为点P1和P2；线段l2为从端机器人的任意一个连杆，线段l2的两端点记为点Q1和Q2；Calculate the shortest distance from the slave robot's connecting rod to each edge of the obstacle envelope. The minimum of these shortest distances is the shortest distance from the slave robot's connecting rod to the obstacle envelope. Assume that line segment l1 is any edge of the obstacle envelope, and the two endpoints of line segment l1 are denoted as points P1 and P2 . Line segment l2 is any connecting rod of the slave robot, and the two endpoints of line segment l2 are denoted as points Q1 and Q2 . 线段l1：P(ω1)＝P1+ω1S1，S1＝P2-P1，ω1∈[0，1]Line segment l 1 : P(ω 1 )=P 1 +ω 1 S 1 , S 1 =P 2 -P 1 , ω 1 ∈[0, 1] 线段l2：Q(ω2)＝Q1+ω2S2，S2＝Q2-Q1，ω2∈[0，1]Line segment l 2 : Q(ω 2 )=Q 1 +ω 2 S 2 , S 2 =Q 2 -Q 1 , ω 2 ∈[0, 1] 线段l1与l2之间的最短距离可转化为有约束的最优化问题：The shortest distance between line segments l1 and l2 can be transformed into a constrained optimization problem: minf(ω1，ω2)＝||P(ω1)-Q(ω2)||2＝||(P1+ω1S1)-(Q1+ω2S2)||2 minf(ω 1 , ω 2 )=||P(ω 1 )-Q(ω 2 )|| 2 =||(P 1 +ω 1 S 1 )-(Q 1 +ω 2 S 2 )|| 2 s.t.ω1，ω2∈[0，1]stω 1 ，ω 2 ∈[0，1] 由极小值条件：可得：According to the minimum condition: We can get: 若0≤ω1，ω2≤1，则线段l1与l2之间的最短距离dmin＝f(ω1，ω2)，否则分别计算点P1、P2分别到线段l2的最短距离d1和d2，点Q1、Q2分别到线段l1的最短距离d3和d4，可得线段l1与l2之间的最短距离dmin＝min{d1，d2，d3，d4}；If 0≤ω1 , ω2≤1 , then the shortest distance between line segments l1 and l2 is dmin =f( ω1 , ω2 ). Otherwise, calculate the shortest distances d1 and d2 from points P1 and P2 to line segment l2 , and the shortest distances d3 and d4 from points Q1 and Q2 to line segment l1 , respectively. The shortest distance between line segments l1 and l2 is dmin =min{ d1 , d2 , d3 , d4 }. 按照上述方式计算从端机器人所有连杆到障碍物包络盒的最短距离，这些最短距离的最小值即为连杆lm到障碍物包络盒的最短距离。The shortest distances from all the links of the end robot to the obstacle envelope are calculated in the above manner. The minimum value of these shortest distances is the shortest distance from the link lm to the obstacle envelope. 3.一种主从同构的机器人遥操作安全控制系统，该系统通过权利要求1或2所述的方法实现机器人遥操作；其特征在于，包括主端机器人、主端控制器、从端机器人、从端控制器和视觉传感器；主端机器人位于远程控制室内，从端机器人位于实际环境中，主端控制器用于采集主端机器人的关节角度、末端位姿和移动底盘的运动速度，从端控制器采集从端机器人的关节角度、末端位姿和移动底盘的运动速度，视觉传感器用于采集从端机器人工作空间的环境图像。3. A master-slave isomorphic robot teleoperation safety control system, which realizes robot teleoperation through the method described in claim 1 or 2; it is characterized in that it includes a master robot, a master controller, a slave robot, a slave controller and a visual sensor; the master robot is located in a remote control room, and the slave robot is located in an actual environment, the master controller is used to collect the joint angles, end postures and movement speed of the mobile chassis of the master robot, the slave controller collects the joint angles, end postures and movement speed of the mobile chassis of the slave robot, and the visual sensor is used to collect the environmental image of the slave robot's workspace.
CN202310761239.XA 2023-06-27 2023-06-27 A master-slave isomorphic robot teleoperation safety control method and system Active CN116652958B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310761239.XA CN116652958B (en) 2023-06-27 2023-06-27 A master-slave isomorphic robot teleoperation safety control method and system

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310761239.XA CN116652958B (en) 2023-06-27 2023-06-27 A master-slave isomorphic robot teleoperation safety control method and system

Publications (2)

Publication Number Publication Date
CN116652958A CN116652958A (en) 2023-08-29
CN116652958B true CN116652958B (en) 2025-08-22

Family

ID=87724156

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310761239.XA Active CN116652958B (en) 2023-06-27 2023-06-27 A master-slave isomorphic robot teleoperation safety control method and system

Country Status (1)

Country Link
CN (1) CN116652958B (en)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117921618A (en) * 2024-01-16 2024-04-26 浙江大学 A master-slave teleoperation robot system based on force mixed reality
CN117984322A (en) * 2024-02-26 2024-05-07 东南大学 Force sense guiding teleoperation system based on double-arm cooperation potential field and control method

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
AU2004200083A1 (en) * 1998-09-28 2004-02-05 The Chamberlain Group, Inc. Movable Barrier Operator
CN108555911A (en) * 2018-04-22 2018-09-21 北京工业大学 Remote operating machinery arm, three-D barrier-avoiding method based on virtual thrust

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108908331B (en) * 2018-07-13 2022-04-22 哈尔滨工业大学（深圳） Obstacle avoidance method and system for hyper-redundant flexible robot, and computer storage medium
CN110561419B (en) * 2019-08-09 2021-01-12 哈尔滨工业大学（深圳） Method and device for trajectory planning of flexible robot with arm shape line constraint
US11766775B2 (en) * 2021-05-20 2023-09-26 Ubkang (Qingdao) Technology Co., Ltd. Method, device and computer-readable storage medium for designing serial manipulator
CN115026816B (en) * 2022-06-09 2024-09-20 安徽工业大学 A method for avoiding obstacles at the end of a robotic arm based on virtual force
CN115357020B (en) * 2022-08-23 2025-06-13 深圳市越疆科技股份有限公司 Obstacle avoidance method, device and intelligent equipment

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
AU2004200083A1 (en) * 1998-09-28 2004-02-05 The Chamberlain Group, Inc. Movable Barrier Operator
CN108555911A (en) * 2018-04-22 2018-09-21 北京工业大学 Remote operating machinery arm, three-D barrier-avoiding method based on virtual thrust

Also Published As

Publication number Publication date
CN116652958A (en) 2023-08-29

Similar Documents

Publication Publication Date Title
CN116652958B (en) A master-slave isomorphic robot teleoperation safety control method and system
CN109079799B (en) Robot perception control system and control method based on bionics
CN104950885B (en) A kind of view-based access control model and power feel the UAV group's bilateral teleoperation control system and its method of feedback
CN106842954B (en) Control method of semi-flexible mechanical arm system
CN110405762B (en) An attitude control method of biped robot based on space second-order inverted pendulum model
CN115469576B (en) A teleoperation system based on hybrid mapping of human-robot heterogeneous motion space
CN111290272B (en) Attitude stationarity adjusting method based on multi-legged robot
CN113843799B (en) A quadruped robot posture reset control method, device and storage medium
CN109129523A (en) Mobile robot real-time remote control system based on human-computer interaction
CN107968915A (en) Underwater robot camera pan-tilt real-time control system and its method
CN108638068B (en) A design method of a flying robot control system carrying a redundant manipulator
CN114571469A (en) Zero-space real-time obstacle avoidance control method and system for mechanical arm
CN112558621A (en) Decoupling control-based flying mechanical arm system
CN113352300B (en) Spraying robot demonstrator and method
CN111230888A (en) An obstacle avoidance method for upper limb exoskeleton robot based on RGBD camera
CN113848908B (en) Modeling and control method of visual servo system for omnidirectional mobile robot
CN111301552A (en) A robot leg dynamic system and its control method
CN108508906A (en) A kind of bilateral tactile remote control system of novel multi-foot robot and control method under outdoor environment
CN115741732A (en) Interactive path planning and motion control method of massage robot
CN110775288A (en) A bionic-based flying mechanical neck-eye system and control method
CN111590567A (en) Space manipulator teleoperation planning method based on Omega handle
CN110039511B (en) An 8-axis linkage robot and its control system and control method
CN117288195A (en) Robot navigation obstacle avoidance method based on depth camera
CN118832603B (en) Mobile robot and mobile robot control method
CN115648200A (en) Composite robot collaborative control method and system

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