WO2021114666A1 - 人机协作中人体安全评估方法及系统 - Google Patents

人机协作中人体安全评估方法及系统 Download PDF

Info

Publication number
WO2021114666A1
WO2021114666A1 PCT/CN2020/102934 CN2020102934W WO2021114666A1 WO 2021114666 A1 WO2021114666 A1 WO 2021114666A1 CN 2020102934 W CN2020102934 W CN 2020102934W WO 2021114666 A1 WO2021114666 A1 WO 2021114666A1
Authority
WO
WIPO (PCT)
Prior art keywords
coordinate system
human body
robot
posture
inertial measurement
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.)
Ceased
Application number
PCT/CN2020/102934
Other languages
English (en)
French (fr)
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.)
Shandong University
Original Assignee
Shandong 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 Shandong University filed Critical Shandong University
Publication of WO2021114666A1 publication Critical patent/WO2021114666A1/zh
Anticipated expiration legal-status Critical
Ceased legal-status Critical Current

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • B25J19/02—Sensing devices
    • B25J19/04—Viewing devices
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • B25J19/06—Safety devices
    • G—PHYSICS
    • G06—COMPUTING OR CALCULATING; COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T17/00—Three-dimensional [3D] modelling for computer graphics

Definitions

  • the present invention relates to the field of human-machine cooperation, in particular to a method and system for human body safety assessment in the process of collaborative robot and human cooperation.
  • the present invention provides a method and system for evaluating human body safety in man-machine collaboration, which makes man-machine collaboration safer and more efficient in man-machine collaboration.
  • the present invention provides a human body safety assessment method in human-machine collaboration, including:
  • the safety level of the human body in the human-robot collaboration is determined.
  • the acquiring the pose of the human body in the global coordinate system specifically includes:
  • the ankle on the side where the human foot touches the ground is the origin of the human body base coordinate system; the origin of the human body base coordinate system is switched between the human body's left ankle and the human body's right ankle;
  • first inertial measurement unit there are multiple first inertial measurement units, which are respectively arranged on the upper torso, left forearm, right forearm, left forearm, right forearm, and left of the human body. Thigh, right thigh, left calf, right calf, left instep and right instep;
  • the posture of the human body in the global coordinate system is determined.
  • the determining the pose of the human body in the global coordinate system according to the origin of the basic coordinate system of the human body, the first rotation matrix, and the third posture relationship specifically includes:
  • the calibration process is carried out on all the first inertial measurement units, each of which is associated with a body part with a predefined coordinate system.
  • the associated first inertial measurement unit is used in real-time motion capture;
  • the length of the human body limb is determined and known, and the first inertial measurement unit after the association is placed on the upper torso of the human body, and the initial state is when the human body is upright and stationary; when the human is walking , Always keep one foot touching the ground, and take that side ankle as the origin of the human body base coordinate system.
  • the first rotation matrix between each first inertial measurement unit coordinate system and the reference coordinate system be the identity matrix
  • the first rotation matrix between each first inertial measurement unit coordinate system and the reference coordinate system be the identity matrix
  • the first rotation matrix R ⁇ 1 measured by the first inertial measurement unit at the left calf is:
  • the coordinate system is established with the left knee as the origin of the coordinate system, and the posture of the coordinate system is consistent with the basic coordinate system of the human body, and the coordinate system established with the left knee as the origin of the coordinate system is used as the reference coordinate system for the movement of the first inertial measurement unit at the left thigh;
  • the first rotation matrix between the first inertial measurement unit coordinate system at the left thigh and the left knee coordinate system be the identity matrix.
  • the coordinates of the left hip joint are Obtain the posture and position information of the left thigh in real time through the first rotation matrix R ⁇ 2 expressed by the quaternion returned by the first inertial measurement unit at the left thigh;
  • the coordinate system is established with the left hip joint as the origin of the coordinate system.
  • the posture of the coordinate system is consistent with the basic coordinate system of the human body. Assuming that the posture of the pelvis does not change during the movement of the person, the position of the pelvis in the coordinate system of the left hip joint is fixed , And get the position of the pelvis in the basic coordinate system of the human body at the same time;
  • the coordinate system is established with the center of the pelvis as the origin of the coordinate system.
  • the posture of the coordinate system is consistent with the basic coordinate system of the human body.
  • the position of the right hip joint in the pelvic coordinate system is fixed, and the position of the right hip joint in the basic coordinate system of the human body is obtained. ;
  • the coordinate system is established with the right hip joint as the origin of the coordinate system.
  • the posture of the coordinate system is consistent with the basic coordinate system of the human body.
  • the coordinate system of the right hip joint is used as the reference coordinate system of the first inertial measurement unit at the right thigh.
  • the first rotation matrix between the coordinate system of the first inertial measurement unit and the coordinate system of the right hip joint is the unit matrix. In the coordinate system of the right hip joint, it is represented by the quaternion returned by the first inertial measurement unit at the right thigh.
  • the first rotation matrix obtains the posture and position information of the right thigh in real time, and obtains the position coordinates of the right knee in the basic coordinate system of the human body according to the coordinate transformation;
  • the coordinate system is established with the right knee as the origin of the coordinate system.
  • the posture of the coordinate system is consistent with the basic coordinate system of the human body.
  • the coordinate system of the right knee is used as the reference coordinate system of the first inertial measurement unit at the right calf.
  • the right calf is the first
  • the first rotation matrix between the inertial measurement unit coordinate system and the right knee coordinate system is the unit matrix.
  • the first rotation matrix expressed by the quaternion returned by the first inertial measurement unit at the right calf Obtain the posture and position information of the right calf, and obtain the position coordinates of the right ankle in the basic coordinate system of the human body according to the coordinate transformation;
  • the pelvic coordinate system is used as the reference coordinate system of the upper body torso.
  • the first rotation matrix between the upper body torso inertial measurement unit coordinate system and the pelvic coordinate system is the identity matrix.
  • the pelvic coordinate system Through the first rotation matrix represented by the quaternion returned by the first inertial measurement unit of the upper body torso, the posture and position information of the upper body torso are obtained in real time, and the shoulder position coordinates in the basic coordinate system of the human body are obtained according to the coordinate transformation;
  • the coordinate system is established with the shoulder as the origin of the coordinate system.
  • the posture of the coordinate system is consistent with the basic coordinate system of the human body, and the shoulder coordinate system is used as the reference coordinate system for the movement of the boom, which is expressed by the quaternion returned by the first inertial measurement unit at the boom
  • the first rotation matrix of obtains the posture and position information of the boom in real time, and obtains the position coordinates of the elbow in the basic coordinate system of the human body according to the coordinate transformation;
  • the coordinate system is established with the elbow as the origin of the coordinate system.
  • the posture of the coordinate system is consistent with the basic coordinate system of the human body.
  • the elbow coordinate system is used as the reference coordinate system for the forearm movement, and the quaternion is returned by the first inertial measurement unit at the forearm.
  • the first rotation matrix represented by the number obtains the posture and position information of the forearm in real time, and obtains the position coordinates of the wrist in the basic coordinate system of the human body according to the coordinate transformation;
  • the acquiring the pose of the robot in the global coordinate system specifically includes:
  • the pose of the robot in the global coordinate system is determined.
  • the determining the distance between the human body and the robot and the movement speed of the robot according to the pose of the human body in the global coordinate system and the pose of the robot in the global coordinate system specifically includes:
  • the pose of the human body in the global coordinate system and the pose of the robot in the global coordinate system determine the distance set between the human body and the robot; the distance set includes each human body in the human body and the robot respectively The distance between the connecting rods; the human body torso includes upper body torso, left big arm, right big arm, left forearm, right forearm, left thigh, right thigh, left calf and right calf;
  • the movement speed of the robot is determined according to the distance between the human body and the robot.
  • the determining the safety level of the human body in human-robot collaboration according to the distance between the human body and the robot and the movement speed of the robot specifically includes:
  • the distance between the human body and the robot is greater than the second preset distance threshold, the distance between the human body and the robot is less than or equal to the first preset distance threshold, and the movement speed of the robot is less than the preset speed threshold, Determining that the human body is in a second safe state; the first preset distance threshold is greater than the second preset distance threshold;
  • the distance between the human body and the robot is greater than a second preset distance threshold
  • the distance between the human body and the robot is less than or equal to the first preset distance threshold
  • the movement speed of the robot is greater than or equal to the preset speed threshold
  • the first preset distance threshold is L max ; the second preset distance threshold is L max /3; the preset speed threshold L max /1.25; where L max is the abduction length of the robot The maximum value.
  • a human body safety assessment system in human-machine collaboration including:
  • the first acquisition module is used to acquire the pose of the human body in the global coordinate system
  • the second acquisition module is used to acquire the pose of the robot in the global coordinate system
  • the distance and speed determination module is used to determine the distance between the human body and the robot and the movement speed of the robot according to the pose of the human body in the global coordinate system and the pose of the robot in the global coordinate system;
  • the human body safety level determination module is used to determine the safety level of the human body in human-machine collaboration according to the distance between the human body and the robot and the movement speed of the robot.
  • the first obtaining module specifically includes:
  • the first posture relationship and the second posture relationship acquisition unit are used to acquire the first posture relationship between the human body base coordinate system and the global coordinate system and the second posture relationship between the inertial measurement unit coordinate system and the global coordinate system;
  • the third posture relationship determining unit is configured to determine a third posture relationship between the basic coordinate system of the human body and the coordinate system of the inertial measurement unit according to the first posture relationship and the second posture relationship; wherein, Is the first posture relationship, Is the second posture relationship, Is the third posture relationship, G is the global coordinate system, B is the human body base coordinate system, and S is the inertial measurement unit coordinate system;
  • the origin determining unit of the human body base coordinate system is used to select the ankle on the side where the human foot touches the ground when the human body is walking as the origin of the human body base coordinate system;
  • the origin of the human body base coordinate system is the left ankle of the human body and the right of the human body Switch between ankles;
  • the first rotation matrix acquisition unit is used to acquire the first rotation matrix measured by the first inertial measurement unit; there are multiple first inertial measurement units, which are respectively arranged on the upper torso, left forearm, right forearm, and Left forearm, right forearm, left thigh, right thigh, left calf, right calf, left instep and right instep;
  • the posture determination unit of the human body in the global coordinate system is used to determine the position of the human body according to the origin of the basic coordinate system of the human body, the first rotation matrix, and the third posture relationship.
  • the second acquiring module specifically includes:
  • the base coordinate acquisition unit is used to acquire the coordinates of the robot base in the global coordinate system
  • the second rotation matrix obtaining unit is used to obtain the second rotation matrix measured by the second inertial measurement unit; there are multiple second inertial measurement units, which are respectively arranged on the base and the connecting rod of the robot;
  • the pose determination unit of the robot in the global coordinate system is configured to determine the pose of the robot in the global coordinate system according to the coordinates of the robot base in the global coordinate system and the second rotation matrix.
  • the present invention discloses the following technical effects:
  • the robot and the operator wear the inertial measurement unit, the collaborative robot works according to a predetermined trajectory, and the operator cooperates with the collaborative robot.
  • the inertial measurement unit collects the movement of the robot and the human body. According to the data, the distance between the human body and the robot and the speed of the robot are calculated based on the collected data. This data is used to evaluate the degree of cooperation between the collaborative robot and the human body and the safety of the personnel.
  • the present invention determines the current safety state of the human body by monitoring the distance between the robot and the human body and the running speed of the robot, and makes the next work plan according to the different safety states, effectively avoiding dangerous situations and enabling the human-machine Collaboration is safer and more efficient.
  • Figure 1 is a schematic diagram of a test system scenario provided by an embodiment of the present invention.
  • FIG. 2 is a schematic diagram of the placement of a human inertial measurement unit provided by an embodiment of the present invention
  • FIG. 3 is a sequence diagram for estimating the whole body posture when the left foot of a human body touches the ground according to an embodiment of the present invention
  • FIG. 4 is a sequence diagram of estimating the whole body posture when the right foot of the human body touches the ground according to an embodiment of the present invention
  • Fig. 5 is a schematic diagram of the placement of the robot inertial measurement unit provided by the embodiment of the present invention.
  • the purpose of the present invention is to provide a method and system for evaluating human body safety in human-machine collaboration, so as to make human-machine collaboration safer and more efficient.
  • both the robot and the operator wear an inertial measurement unit (IMU) suit.
  • IMU inertial measurement unit
  • the collaborative robot works according to a predetermined trajectory, and the operator cooperates with the collaborative robot.
  • multiple inertial measurement units first collect the rotation matrix of the robot and the human body, then filter the rotation matrix, and then use the positive kinematics function to obtain the position information and movement posture of the robot and the human body, and finally calculate the human body and the robot Evaluation parameters such as the distance between the robot and the robot's movement speed, to evaluate the degree of cooperation between the collaborative robot and the personnel and the safety of the personnel.
  • the rotation matrix is filtered to make the obtained rotation matrix more accurate, and then the positive kinematics function is used to determine the position of each joint of the human body and the posture of each torso, as well as the position of each joint of the robot and the posture of each link, thereby obtaining The position information and movement posture of the robot and the human body, and finally the evaluation parameters such as the distance between the human body and the robot and the movement speed of the robot are calculated, and the degree of cooperation between the collaborative robot and the personnel and the safety of the personnel are evaluated.
  • the present invention is a method for evaluating human body safety in human-machine collaboration, including:
  • S3 Determine the distance between the human body and the robot and the movement speed of the robot according to the pose of the human body in the global coordinate system and the pose of the robot in the global coordinate system.
  • S4 Determine the safety level of the human body in the human-robot collaboration according to the distance between the human body and the robot and the movement speed of the robot.
  • S1 specifically includes:
  • the established human base coordinate system is consistent with the posture of the global coordinate system.
  • the posture relationship between the basic coordinate system of the human body and the global coordinate system is expressed as
  • the posture relationship between the inertial measurement unit coordinate system and the global coordinate system is expressed as
  • the posture relationship between the basic coordinate system of the human body and the coordinate system of the inertial measurement unit is expressed as Through the transformation between coordinate systems:
  • S15 Determine the pose of the human body in the global coordinate system according to the origin of the basic coordinate system of the human body, the first rotation matrix, and the third posture relationship.
  • the placement position of the human inertial measurement unit is shown in Figure 2.
  • the first inertial measurement unit and the second inertial measurement unit used are both XSENS MTwAwinda, and the ankle at rest when walking is selected as the origin of the basic coordinate system of the human body.
  • the origin of the basic coordinate system of the human body is switched between the left and right feet.
  • the position of the pelvis is calculated by the ankle position, and then the posture of each torso of the whole body is calculated according to the position of the pelvis; 5 inertial measurement units are placed on the upper body, respectively.
  • the position of the inertial measurement unit placed on the human body is not fixed. Changes.
  • each inertial measurement unit is associated with a body part with a predefined coordinate system. Use it for real-time motion capture.
  • the posture of the human body The length of the limbs of the human body is determined and known. After the inertial measurement unit is calibrated, it is placed on the torso of the human body. The initial state is when the human body is upright and stationary. When a person walks, he always keeps one foot touching the ground, and the ankle on that side is taken as the origin of the basic coordinate system of the human body. When the human body is stationary, the rotation matrix between the coordinate system of each inertial measurement unit and its reference coordinate system is the unit matrix; When the human body moves, through the quaternion ( ⁇ 1 , ⁇ 2 , ⁇ 3 , ⁇ 4 ) returned by the inertial measurement unit, the posture and position information of each torso of the body can be obtained in real time.
  • the sequence of calculating the whole body posture when the left foot touches the ground is shown in Fig. 3, and the sequence of calculating the whole body posture when the right foot touches the ground is shown in Fig. 4.
  • the left ankle human body base coordinate system (left ankle coordinate system) is used as the reference coordinate system of the left calf. It is known that when the human body is stationary, the coordinates of the left knee in the human body base coordinate system are
  • the rotation matrix R ⁇ 1 represented by the quaternion measured by the first inertial measurement unit at the left calf is:
  • the coordinate system is established with the left knee as the origin of the coordinate system, and the posture of the coordinate system is consistent with the basic coordinate system of the human body, and this coordinate system is used as the reference coordinate system for the movement of the left thigh inertial measurement unit.
  • the rotation matrix between the coordinate system of the left thigh inertial measurement unit and the coordinate system of the left knee be the unit matrix.
  • the coordinates of the left hip joint are The posture and position information of the left thigh can be obtained in real time through the rotation matrix R ⁇ 2 represented by the quaternion returned by the left thigh inertial measurement unit.
  • the coordinate system is established with the left hip joint as the origin of the coordinate system.
  • the posture of the coordinate system is consistent with the basic coordinate system of the human body. Assuming that the posture of the pelvis does not change during the movement of the person, the position of the pelvis in the coordinate system of the left hip joint is fixed At the same time, the position of the pelvis in the basic coordinate system of the human body can be obtained.
  • the coordinate system is established with the center of the pelvis as the origin of the coordinate system.
  • the posture of the coordinate system is consistent with the basic coordinate system of the human body.
  • the position of the right hip joint in the pelvic coordinate system is fixed, and the right hip joint can be obtained in the basic coordinate system of the human body. s position.
  • the distance from the left hip joint to the pelvis and the distance from the right hip joint to the pelvis of the same person are already available. According to the distance and the coordinates of the left hip joint position, the pelvic coordinates and the coordinates of the right hip joint can be obtained.
  • the coordinate system is established with the right hip joint as the origin of the coordinate system.
  • the posture of the coordinate system is consistent with the basic coordinate system of the human body.
  • the right hip joint coordinate system is used as the reference coordinate system of the right thigh inertial measurement unit.
  • the right thigh inertial measurement unit coordinates are set
  • the rotation matrix between the right hip joint coordinate system and the right hip joint coordinate system is the unit matrix.
  • the rotation matrix represented by the quaternion returned by the right thigh inertial measurement unit can be used to obtain the right thigh posture and position in real time According to the coordinate transformation, the coordinates of the right knee position in the basic coordinate system of the human body are obtained.
  • the coordinate system is established with the right knee as the origin of the coordinate system.
  • the posture of the coordinate system is consistent with the basic coordinate system of the human body.
  • the coordinate system of the right knee is used as the reference coordinate system of the right calf inertial measurement unit.
  • the coordinate system of the right calf inertial measurement unit and The rotation matrix between the right knee coordinate system is the unit matrix.
  • the rotation matrix represented by the quaternion returned by the right calf inertial measurement unit can be used to obtain the right calf posture and position information in real time, according to the coordinates Transform to obtain the position coordinates of the right ankle in the basic coordinate system of the human body.
  • the pelvic coordinate system is used as the upper body torso reference coordinate system.
  • the rotation matrix between the upper body torso inertial measurement unit coordinate system and the pelvic coordinate system is the unit matrix.
  • the upper body The rotation matrix represented by the quaternion returned by the torso inertial measurement unit can obtain the posture and position information of the upper body torso in real time.
  • the shoulder position coordinates in the basic coordinate system of the human body can be obtained.
  • the coordinate system is established with the shoulder as the origin of the coordinate system.
  • the posture of the coordinate system is consistent with the basic coordinate system of the human body.
  • the shoulder coordinate system is used as the reference coordinate system of the boom movement, and the rotation matrix represented by the quaternion returned by the boom inertial measurement unit ,
  • the posture and position information of the forearm can be obtained in real time, and the position coordinates of the elbow in the basic coordinate system of the human body can be obtained according to the coordinate transformation.
  • the coordinate system is established with the elbow as the origin of the coordinate system.
  • the posture of the coordinate system is consistent with the basic coordinate system of the human body.
  • the elbow coordinate system is used as the reference coordinate system for the forearm movement, which is expressed by the quaternion returned by the forearm inertial measurement unit.
  • the rotation matrix can obtain the posture and position information of the forearm in real time, and obtain the position coordinates of the wrist in the basic coordinate system of the human body according to the coordinate transformation.
  • the complete posture of the human body in the basic coordinate system of the human body and the position coordinates of each joint are obtained.
  • the complete posture of the human body in the global coordinate system and the position coordinates of each joint can be obtained.
  • S2 specifically includes:
  • S21 Obtain the coordinates of the robot base in the global coordinate system; the base is the base on which the robot touches the ground. Specifically, the position of the robot base in the global coordinate system is fixed, and the center of the base is the origin of the global coordinate system, and the posture of the coordinate system is consistent with the global coordinate system. The base coordinate system is established, and the center of the base is in the global coordinate system. The coordinates below are obtained from actual measurements.
  • S22 Obtain a second rotation matrix measured by a second inertial measurement unit; there are multiple second inertial measurement units, which are respectively arranged on the base and the connecting rod of the robot.
  • IMU1 is placed on the base of the robot, and the remaining 5 IMUs are placed on each link of the robot.
  • the remaining 5 IMUs are IMU2, IMU3, IMU4, IMU5, and IMU6.
  • S23 Determine the pose of the robot in the global coordinate system according to the coordinates of the robot base in the global coordinate system and the second rotation matrix.
  • the placement position of the inertial measurement unit on the robot is shown in Figure 5.
  • the example of the present invention selects a 6-degree-of-freedom manipulator, and places an inertial measurement unit on the base and each link to obtain the posture of the manipulator. information.
  • the above-mentioned method of obtaining the posture of the upper body of the human body is also applicable to obtaining the posture of the mechanical arm, and will not be repeated. In this way, the posture of the robot in the global coordinate system is obtained.
  • S3 specifically includes:
  • S31 Determine a set of distances between the human body and the robot according to the pose of the human body in the global coordinate system and the pose of the robot in the global coordinate system; the distance set includes each torso in the human body.
  • the distance between the connecting rod and the robot; the human body torso includes the upper body torso, the left big arm, the right big arm, the left forearm, the right forearm, the left thigh, the right thigh, the left calf and the right calf.
  • S32 Select the minimum value in the distance set as the distance between the human body and the robot.
  • S33 Determine the movement speed of the robot according to the distance between the human body and the robot.
  • the torso of the human body and the links of the robotic arm are regarded as cylinders in space, and the shortest distance between the cylinders is solved by mathematical formulas.
  • each part of the torso of the human body is regarded as a cylinder, and each connecting rod of the robot is regarded as a cylinder, and the shortest distance between the cylinders is solved by a mathematical formula.
  • H x b- x a
  • I y b -y a
  • J z b- z a
  • K X d- x c
  • L y d -y c
  • M z d- z c
  • intersection point F(x F ,y F ,z F ) of the common perpendicular and the straight line AB can be obtained.
  • the distance between the two cylinders is the difference between the distance between the central axes of the two cylinders and the radii of the two cylinders, that is, the distance between the central axes of the two cylinders minus the radii of the two cylinders.
  • S4 specifically includes:
  • the distance threshold and the speed threshold are determined according to the abduction distance of the robot, the length of the installed tool, the working performance of the robot, and a priori experience.
  • the first preset distance threshold is L max
  • the second preset distance threshold is L max /3
  • L max is the maximum abduction length of the robot
  • the second preset distance threshold is one third of the maximum abduction length of the robot, that is, 0.33L max
  • the reaction ability parameter of a normal person is 1.25 seconds. That is, it takes 0.5 seconds to find that there is a target in the front to reflect the brain, and it takes 0.75 seconds to take measures from the brain to the hands and feet.
  • the current safety state of the human body can be determined by monitoring the distance between the robot and the human body and the running speed of the robot, and make the next work plan according to different safety states, effectively avoiding dangerous situations. Make human-machine collaboration safer and more efficient.
  • the present invention also provides a human body safety assessment system in human-machine collaboration, including:
  • the first acquisition module is used to acquire the pose of the human body in the global coordinate system.
  • the second acquisition module is used to acquire the pose of the robot in the global coordinate system.
  • the distance and speed determination module is used to determine the distance between the human body and the robot and the movement speed of the robot according to the pose of the human body in the global coordinate system and the pose of the robot in the global coordinate system.
  • the human body safety level determination module is used to determine the safety level of the human body in human-machine collaboration according to the distance between the human body and the robot and the movement speed of the robot.
  • the first acquisition module specifically includes:
  • the first posture relationship and the second posture relationship acquisition unit are used to acquire the first posture relationship between the human body base coordinate system and the global coordinate system and the second posture relationship between the inertial measurement unit coordinate system and the global coordinate system.
  • the third posture relationship determining unit is configured to determine a third posture relationship between the basic coordinate system of the human body and the coordinate system of the inertial measurement unit according to the first posture relationship and the second posture relationship; wherein, Is the first posture relationship, Is the second posture relationship, Is the third posture relationship, G is the global coordinate system, B is the human body base coordinate system, and S is the inertial measurement unit coordinate system.
  • the origin determination unit of the human body base coordinate system is used to select the ankle on the side where the human foot touches the ground as the origin of the human body base coordinate system when the human body is walking; the origin of the human body base coordinate system is the left ankle of the human body and the right of the human body Switch between ankles.
  • the first rotation matrix acquisition unit is used to acquire the first rotation matrix measured by the first inertial measurement unit; there are multiple first inertial measurement units, which are respectively arranged on the upper torso, left forearm, and right forearm of the human body. Left forearm, right forearm, left thigh, right thigh, left calf, right calf, left instep and right instep.
  • the posture determination unit of the human body in the global coordinate system is used to determine the position of the human body according to the origin of the basic coordinate system of the human body, the first rotation matrix, and the third posture relationship.
  • the first acquisition module specifically includes:
  • the calibration unit is used to calibrate the inertial measurement unit to obtain the coordinate transformation matrix between the inertial measurement unit coordinate system and its corresponding human body coordinate system.
  • the data receiving unit is used to receive the quaternion and angular velocity returned by the inertial measurement unit.
  • the pose acquisition unit is used to obtain the posture and position coordinates of the human body in the basic coordinate system of the human body according to the obtained angular velocity and quaternion of each inertial measurement unit.
  • the coordinate conversion unit is used to convert the position coordinates of the human body in the basic human body coordinate system into the position coordinates of the human body in the global coordinate system.
  • the second acquisition module specifically includes:
  • the base coordinate acquisition unit is used to acquire the coordinates of the robot base in the global coordinate system.
  • the second rotation matrix obtaining unit is used to obtain the second rotation matrix measured by the second inertial measurement unit; there are multiple second inertial measurement units, which are respectively arranged on the base and the connecting rod of the robot.
  • the pose determination unit of the robot in the global coordinate system is configured to determine the pose of the robot in the global coordinate system according to the coordinates of the robot base in the global coordinate system and the second rotation matrix.

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Physics & Mathematics (AREA)
  • Computer Graphics (AREA)
  • Geometry (AREA)
  • Software Systems (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Measurement Of The Respiration, Hearing Ability, Form, And Blood Characteristics Of Living Organisms (AREA)
  • Manipulator (AREA)

Abstract

人机协作中人体安全评估方法及系统，包括：获取人体在全局坐标系下的位姿（S1）；获取机器人在全局坐标系下的位姿（S2）；根据人体在全局坐标系下的位姿和机器人在全局坐标系下的位姿，确定人体与机器人之间的距离以及机器人的运动速度（S3）；根据人体与机器人之间的距离和机器人的运动速度，确定人机协作中人体的安全等级（S4）。在人机协作过程中，通过监控机器人与人体之间的距离和机器人的运行速度确定当前人体的安全状态，根据不同安全状态做出下一步工作计划，有效避免危险情况发生，使人机协作更安全更高效。

Description

人机协作中人体安全评估方法及系统
本申请要求于2019年12月11日提交中国专利局、申请号为201911262776.X、发明名称为“人机协作中人体安全评估方法及系统”的中国专利申请的优先权，其全部内容通过引用结合在本申请中。
技术领域
本发明涉及人机协作领域，特别是涉及在协作型机器人与人协作过程中的人体安全评估方法及系统。
背景技术
机器人行业的快速发展对人与机器人密切协作提出了新要求，逐渐催生出协作型机器人。传统工业机器人设计有围栏隔离，在机器人工作时不允许工作人员进入，或者在工作人员进入时，机器人必须停止工作，这种方法不仅浪费工作空间，而且降低了整体的工作效率。而协作型机器人，可以实现与人协同工作，不再需要防护栏进行隔离，将人的“知识、分析、决策”能力与机器人的“力量、精度、可重复性”结合在一起。协作型机器人与人共同处于同一工作空间，协作等级不断加强，二者紧密协调，不仅节省了工作空间，而且大大提高了工作效率，并且在确保人员安全的前提下，机器人能够自主提高技能，实现与人自然交互协作。
根据阿西莫夫机器人三定律，不管何种机器人，在任何情况和环境下都不应伤害人类及机器人自身，所以安全性是机器人在人类生产生活领域应用的首要问题和强制性约束，但机器人本身并没有安全意识，人与机器人协同工作时如果发生碰撞，处于弱势地位的操作人员安全隐患相对较大，所以必须针对协作机器人可能出现的各个风险点，进行充分的安全评估，制定相应的保护措施来确保操作人员的安全。
目前存在的人机协作中人体安全评估方法，只通过计算机器人与人体之间的距离来判断人体的安全状况，而忽略机器人的运动速度这一重要参数对人体安全的影响，造成了人机协作中人体安全评估不准确、特定风险无法排查等现象。
发明内容
本发明提供一种人机协作中人体安全评估方法及系统，在人机协作中 使人机协作更安全更高效。
为实现上述目的，本发明提供一种人机协作中人体安全评估方法，包括：
获取人体在全局坐标系下的位姿；
获取机器人在全局坐标系下的位姿；
根据所述人体在全局坐标系下的位姿和所述机器人在全局坐标系下的位姿，确定人体与机器人之间的距离以及机器人的运动速度；
根据所述人体与机器人之间的距离和所述机器人的运动速度，确定人机协作中人体的安全等级。
可选的，所述获取人体在全局坐标系下的位姿，具体包括：
获取人体基坐标系与全局坐标系之间的第一姿态关系
Figure PCTCN2020102934-appb-000001
以及惯性测量单元坐标系与全局坐标系之间的第二姿态关系
Figure PCTCN2020102934-appb-000002
根据所述第一姿态关系和所述第二姿态关系确定人体基坐标系与惯性测量单元坐标系之间的第三姿态关系
Figure PCTCN2020102934-appb-000003
其中，
Figure PCTCN2020102934-appb-000004
为第一姿态关系，
Figure PCTCN2020102934-appb-000005
为第二姿态关系，
Figure PCTCN2020102934-appb-000006
为第三姿态关系，G为全局坐标系，B为人体基坐标系，S为惯性测量单元坐标系；
选取人体行走时，人体脚触地所在侧的脚踝为所述人体基坐标系的原点；所述人体基坐标系的原点在人体的左脚踝和人体的右脚踝之间切换；
获取利用第一惯性测量单元测量的第一旋转矩阵；所述第一惯性测量单元为多个，分别设置于人体的上身躯干、左大臂、右大臂、左小臂、右小臂、左大腿、右大腿、左小腿、右小腿、左脚背和右脚背；
根据所述人体基坐标系的原点、所述第一旋转矩阵和所述第三姿态关系，确定人体在全局坐标系下的位姿。
可选的，所述根据所述人体基坐标系的原点、所述第一旋转矩阵和所述第三姿态关系，确定人体在全局坐标系下的位姿，具体包括：
在开始运动之前，人体保持校准姿势，校准过程为：在所有的第一惯性测量单元上进行，每个所述第一惯性测量单元都与一个具有预定义的坐标系的身体部分相关联，将关联后的第一惯性测量单元用于实时运动捕捉中；
获取人体在人体基坐标系下的位姿：人体肢体长度是确定已知的，将关联后的第一惯性测量单元放置在人体的上身躯干，以人体直立静止时为初始状态；人在行走时，总会保持一只脚触地，取该侧脚踝为人体基坐标系原点，人体静止时，令每个第一惯性测量单元坐标系与参照坐标系之间的第一旋转矩阵为单位矩阵，人体运动时，通过第一惯性测量单元返回的四元数(ε 1,ε 2,ε 3,ε 4)，实时得到人体每一躯干的姿态以及位置信息；
左脚触地时，左脚踝人体基坐标系作为左小腿的参照坐标系，已知人体静止时，在人体基坐标系中左膝盖的坐标为
Figure PCTCN2020102934-appb-000007
通过左小腿处的第一惯性测量单元测量的第一旋转矩阵R ε1为：
Figure PCTCN2020102934-appb-000008
则人体姿态发生变换后的左膝盖坐标为：P′ L1＝R ε1P L1；
以左膝盖为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，并以左膝盖为坐标系原点建立的坐标系作为左大腿处的第一惯性测量单元运动的参照坐标系；人体静止时，令左大腿处的第一惯性测量单元坐标系与左膝盖坐标系之间的第一旋转矩阵为单位矩阵，在左膝盖坐标系中，左髋关节的坐标为
Figure PCTCN2020102934-appb-000009
通过左大腿处的第一惯性测量单元传回的四元数表示的第一旋转矩阵R ε2，实时得到左大腿的姿态以及位置信息；
则人体姿态发生变换后的左髋关节坐标为：P′ L2＝R ε2P L2；
根据所述人体姿态发生变换后的左髋关节坐标和所述人体姿态发生变换后的左膝盖坐标，得到左髋关节在人体基坐标系下的坐标：P″ L2＝P′ L1+P′ L2；
以左髋关节为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，假设人在移动过程中，盆骨的姿态不发生变化，则盆骨在左髋关节坐 标系中的位置固定，同时得盆骨在人体基坐标系中的位置；
以盆骨中心作为坐标系的原点建立坐标系，坐标系姿态与人体基坐标系一致，右髋关节在盆骨坐标系中的位置固定不变，得到右髋关节在人体基坐标系中的位置；
以右髋关节为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，以右髋关节坐标系作为右大腿处第一惯性测量单元的参照坐标系，人体静止时，令右大腿处第一惯性测量单元坐标系与右髋关节坐标系之间的第一旋转矩阵为单位矩阵，在右髋关节坐标系中，通过右大腿处的第一惯性测量单元传回的四元数表示的第一旋转矩阵，实时得到右大腿的姿态以及位置信息，根据坐标变换，获得人体基坐标系下右膝盖位置坐标；
以右膝盖为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，以右膝盖坐标系作为右小腿处第一惯性测量单元的参照坐标系，人体静止时，令右小腿处第一惯性测量单元坐标系与右膝盖坐标系之间的第一旋转矩阵为单位矩阵，在右膝盖坐标系中，通过右小腿处的第一惯性测量单元传回的四元数表示的第一旋转矩阵，得到右小腿的姿态以及位置信息，根据坐标变换，获得人体基坐标系下右脚踝位置坐标；
右脚触地时，以右脚踝为原点，建立人体基坐标系，腿关节坐标系建立方法同上；
同时，以盆骨坐标系作为上身躯干的参照坐标系，人体静止时，令上身躯干惯性测量单元坐标系与盆骨坐标系之间的第一旋转矩阵为单位矩阵，在盆骨坐标系中，通过上身躯干处第一惯性测量单元传回的四元数表示的第一旋转矩阵，实时得到上身躯干的姿态以及位置信息，根据坐标变换，获得人体基坐标系下的肩膀位置坐标；
以肩膀为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，并将肩膀坐标系作为大臂运动的参照坐标系，通过大臂处第一惯性测量单元传回的四元数表示的第一旋转矩阵，实时得到大臂的姿态以及位置信息，根据坐标变换，获得人体基坐标系下手肘的位置坐标；
以手肘为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，并将手肘坐标系作为小臂运动的参照坐标系，通过小臂处第一惯性测量单元传回的四元数表示的第一旋转矩阵，实时得到小臂的姿态以及位置信 息，根据坐标变换，获得人体基坐标系下手腕的位置坐标；
从而获得人体在人体基坐标系下的完整姿态以及各个关节的位置坐标；通过人体基坐标系和全局坐标系之间的第三姿态关系，得到全局坐标系下的人体完整姿态以及各个关节的位置坐标。
可选的，所述获取机器人在全局坐标系下的位姿，具体包括：
获取全局坐标系下机器人基座的坐标；
获取利用第二惯性测量单元测量的第二旋转矩阵；所述第二惯性测量单元为多个，分别设置于机器人的基座和连杆上；
根据所述全局坐标系下机器人基座的坐标和所述第二旋转矩阵，确定机器人在全局坐标系下的位姿。
可选的，所述根据所述人体在全局坐标系下的位姿和所述机器人在全局坐标系下的位姿，确定人体与机器人之间的距离以及机器人的运动速度，具体包括：
根据所述人体在全局坐标系下的位姿和所述机器人在全局坐标系下的位姿，确定人体与机器人之间的距离集合；所述距离集合中包括人体中每个人体躯干分别与机器人连杆之间的距离；所述人体躯干包括上身躯干、左大臂、右大臂、左小臂、右小臂、左大腿、右大腿、左小腿和右小腿；
选取所述距离集合中最小值作为人体与机器人之间的距离；
根据所述人体与机器人之间的距离确定机器人的运动速度。
可选的，所述根据所述人体与机器人之间的距离和所述机器人的运动速度，确定人机协作中人体的安全等级，具体包括：
当所述人体与机器人之间的距离大于第一预设距离阈值且所述机器人的运动速度小于预设速度阈值时，确定人体处于第一安全状态；
当所述人体与机器人之间的距离大于第一预设距离阈值且机器人速度大于或等于预设速度阈值时，确定人体处于第二安全状态；
当所述人体与机器人之间的距离大于第二预设距离阈值且所述人体与机器人之间的距离小于或等于第一预设距离阈值且所述机器人的运动速度小于预设速度阈值时，确定人体处于第二安全状态；所述第一预设距离阈值大于所述第二预设距离阈值；
当所述人体与机器人之间的距离大于第二预设距离阈值且所述人体与机器人之间的距离小于或等于第一预设距离阈值且所述机器人的运动速度大于或等于预设速度阈值时，确定人体处于第二危险状态；
当所述人体与机器人之间的距离小于或等于第二预设距离阈值且所述机器人的运动速度小于预设速度阈值时，确定人体处于第二危险状态；
当所述人体与机器人之间的距离小于或等于第二预设距离阈值且所述机器人的运动速度大于或等于预设速度阈值时，确定人体处于第一危险状态。
可选的，所述第一预设距离阈值为L max；所述第二预设距离阈值为L max/3；所述预设速度阈值L max/1.25；其中，L max为机器人外展长度的最大值。
一种人机协作中人体安全评估系统，包括：
第一获取模块，用于获取人体在全局坐标系下的位姿；
第二获取模块，用于获取机器人在全局坐标系下的位姿；
距离和速度确定模块，用于根据所述人体在全局坐标系下的位姿和所述机器人在全局坐标系下的位姿，确定人体与机器人之间的距离以及机器人的运动速度；
人体安全等级确定模块，用于根据所述人体与机器人之间的距离和所述机器人的运动速度，确定人机协作中人体的安全等级。
可选的，所述第一获取模块具体包括：
第一姿态关系和第二姿态关系获取单元，用于获取人体基坐标系与全局坐标系之间的第一姿态关系以及惯性测量单元坐标系与全局坐标系之间的第二姿态关系；
第三姿态关系确定单元，用于根据所述第一姿态关系和所述第二姿态关系确定人体基坐标系与惯性测量单元坐标系之间的第三姿态关系；其中，
Figure PCTCN2020102934-appb-000010
为第一姿态关系，
Figure PCTCN2020102934-appb-000011
为第二姿态关系，
Figure PCTCN2020102934-appb-000012
为第三姿态关系，G为全局坐标系，B为人体基坐标系，S为惯性测量单元坐标系；
人体基坐标系原点确定单元，用于选取人体行走时，人体脚触地所在 侧的脚踝为所述人体基坐标系的原点；所述人体基坐标系的原点在人体的左脚踝和人体的右脚踝之间切换；
第一旋转矩阵获取单元，用于获取利用第一惯性测量单元测量的第一旋转矩阵；所述第一惯性测量单元为多个，分别设置于人体的上身躯干、左大臂、右大臂、左小臂、右小臂、左大腿、右大腿、左小腿、右小腿、左脚背和右脚背；
人体在全局坐标系下位姿确定单元，用于根据所述人体基坐标系的原点、所述第一旋转矩阵和所述第三姿态关系。
可选的，所述第二获取模块具体包括：
基座坐标获取单元，用于获取全局坐标系下机器人基座的坐标；
第二旋转矩阵获取单元，用于获取利用第二惯性测量单元测量的第二旋转矩阵；所述第二惯性测量单元为多个，分别设置于机器人的基座和连杆上；
机器人在全局坐标系下位姿确定单元，用于根据所述全局坐标系下机器人基座的坐标和所述第二旋转矩阵，确定机器人在全局坐标系下的位姿。
根据本发明提供的具体实施例，本发明公开了以下技术效果：
本发明通过设置的测试环境，机器人和操作人员均穿戴惯性测量单元，协作型机器人按照预定轨迹进行工作，操作人员与协作型机器人进行协作，在整个过程中，惯性测量单元采集机器人与人体的运动数据，根据采集到的数据计算人体和机器人之间的距离及机器人的运动速度，以此数据来评估协作型机器人与人体的协作程度及对人员的安全性。
本发明在人机协作过程中，通过监控机器人与人体之间的距离和机器人的运行速度确定当前人体的安全状态，根据不同安全状态做出下一步工作计划，有效避免危险情况发生，使人机协作更安全更高效。
说明书附图
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例所提供的测试系统场景示意图；
图2为本发明实施例所提供的人体惯性测量单元放置示意图；
图3为本发明实施例所提供的人体左脚触地时全身姿态推算顺序图；
图4为本发明实施例所提供的人体右脚触地时全身姿态推算顺序图；
图5为本发明实施例所提供的机器人惯性测量单元放置示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的目的是提供一种人机协作中人体安全评估方法及系统，在人机协作中使人机协作更安全更高效。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
本发明通过设置的测试环境，机器人和操作人员均穿戴惯性测量单元(Inertialmeasurement unit，简称IMU)套装，如图1所示，协作型机器人按照预定轨迹进行工作，操作人员与协作型机器人进行协作，在整个过程中，多个惯性测量单元首先采集机器人和人体的旋转矩阵，之后对旋转矩阵进行滤波处理，然后利用正运动学函数，获取机器人和人体的位置信息及运动姿态，最后计算人体和机器人之间的距离及机器人的运动速度等评价参数，评估协作型机器人与人员的协作程度及对人员的安全性。具体的，对旋转矩阵进行滤波处理，使获取的旋转矩阵更加准确，然后利用正运动学函数确定人体各个关节的位置和各个躯干的姿态以及机器人各个关节的位置和各个连杆的姿态，从而获取机器人和人体的位置信息及运动姿态，最后计算人体和机器人之间的距离及机器人的运动速度等评价参数，评估协作型机器人与人员的协作程度及对人员的安全性。
本发明一种人机协作中人体安全评估方法，包括：
S1，获取人体在全局坐标系下的位姿。
S2，获取机器人在全局坐标系下的位姿。
S3，根据所述人体在全局坐标系下的位姿和所述机器人在全局坐标系下的位姿，确定人体与机器人之间的距离以及机器人的运动速度。
S4，根据所述人体与机器人之间的距离和所述机器人的运动速度，确定人机协作中人体的安全等级。
S1具体包括：
S11，获取人体基坐标系与全局坐标系之间的第一姿态关系
Figure PCTCN2020102934-appb-000013
以及惯性测量单元坐标系与全局坐标系之间的第二姿态关系
Figure PCTCN2020102934-appb-000014
S12，根据所述第一姿态关系和所述第二姿态关系确定人体基坐标系与惯性测量单元坐标系之间的第三姿态关系
Figure PCTCN2020102934-appb-000015
其中，
Figure PCTCN2020102934-appb-000016
为第一姿态关系，
Figure PCTCN2020102934-appb-000017
为第二姿态关系，
Figure PCTCN2020102934-appb-000018
为第三姿态关系，G为全局坐标系，B为人体基坐标系，S为惯性测量单元坐标系。
具体的，获取人体基坐标系与全局坐标系之间的姿态关系
Figure PCTCN2020102934-appb-000019
时，简单起见，所建立的人体基坐标系与全局坐标系姿态一致。人体基坐标系与全局坐标系中的姿态关系表示为
Figure PCTCN2020102934-appb-000020
惯性测量单元坐标系与全局坐标系之间的姿态关系表示为
Figure PCTCN2020102934-appb-000021
人体基坐标系与惯性测量单元坐标系之间的姿态关系表示为
Figure PCTCN2020102934-appb-000022
通过坐标系之间的变换：
Figure PCTCN2020102934-appb-000023
可得
Figure PCTCN2020102934-appb-000024
S13，选取人体行走时，人体脚触地所在侧的脚踝为所述人体基坐标系的原点；所述人体基坐标系的原点在人体的左脚踝和人体的右脚踝之间切换。
S14，获取利用第一惯性测量单元测量的第一旋转矩阵；所述第一惯性测量单元为多个，分别设置于人体的上身躯干、左大臂、右大臂、左小臂、右小臂、左大腿、右大腿、左小腿、右小腿、左脚背和右脚背。
S15，根据所述人体基坐标系的原点、所述第一旋转矩阵和所述第三姿态关系，确定人体在全局坐标系下的位姿。
具体的，人体惯性测量单元的放置位置如图2所示，使用的第一惯性测量单元和第二惯性测量单元均为XSENS MTwAwinda，选取行走时静止一侧脚踝为人体的基坐标系原点，人行走时，人体基坐标系原点在左右脚之间切换，首先通过脚踝位置推算出盆骨的位置，再根据盆骨位置推算全身各躯干的位姿；上半身放置5块惯性测量单元，分别置于上身躯干、左 右大臂(左大臂和右大臂)和左右小臂(左小臂和右小臂)，用于获取上半身的位姿信息；下半身放置6块惯性测量单元，分别置于左右大腿(左大腿和右大臂)、左右小腿(左小腿和右小腿)和左右脚背(左脚背和右脚背)，用于获取下半身的位姿信息，放置在人体上的惯性测量单元位置固定不发生变化。
在开始运动之前，受试者(人体)保持校准姿势，这个校准过程是在所有的惯性测量单元上进行的，其中每个惯性测量单元都与一个具有预定义的坐标系的身体部分相关联，将其用于实时运动捕捉中。
获取人体的姿态，人体肢体长度是确定已知的，对惯性测量单元进行标定之后放置在人体身躯干上，以人体直立静止时为初始状态。人在行走时，总会保持一只脚触地，取该侧脚踝为人体基坐标系原点，人体静止时，令每个惯性测量单元坐标系与其参照坐标系之间的旋转矩阵为单位矩阵；人体运动时，通过惯性测量单元返回的四元数(ε 1,ε 2,ε 3,ε 4)，可以实时得到身体每一躯干的姿态以及位置信息。左脚触地时全身姿态的推算顺序如图3所示，右脚触地时全身姿态的推算顺序如图4所示。
现以左脚触地为例，来说明完整人体姿态获取的过程：
左脚踝人体基坐标系(左脚踝坐标系)作为左小腿的参照坐标系，已知人体静止时，在人体基坐标系中左膝盖的坐标为
Figure PCTCN2020102934-appb-000025
通过左小腿处的第一惯性测量单元测量的四元数表示的旋转矩阵R ε1为：
Figure PCTCN2020102934-appb-000026
则人体姿态发生变换后的左膝盖坐标为：P′ L1＝R ε1P L1。
以左膝盖为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，并以此坐标系作为左大腿惯性测量单元运动的参照坐标系。人体静止时，令左大腿惯性测量单元坐标系与左膝盖坐标系之间的旋转矩阵为单位 矩阵，在左膝盖坐标系中，左髋关节的坐标为
Figure PCTCN2020102934-appb-000027
通过左大腿惯性测量单元传回的四元数表示的旋转矩阵R ε2，可以实时得到左大腿的姿态以及位置信息。
则人体姿态发生变换后的左髋关节坐标为：P′ L2＝R ε2P L2。
同时可得左髋关节相对于人体基坐标系的坐标：P″ L2＝P′ L1+P′ L2。
以左髋关节为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，假设人在移动过程中，盆骨的姿态不发生变化，则盆骨在左髋关节坐标系中的位置固定，同时可得盆骨在人体基坐标系中的位置。
以盆骨中心为坐标系的原点建立坐标系，坐标系姿态与人体基坐标系一致，右髋关节在盆骨坐标系中的位置固定不变，同时可得右髋关节在人体基坐标系中的位置。同一个人体左髋关节到盆骨的距离，右髋关节到盆骨的距离是已经的，根据距离和左髋关节位置坐标，则能求出盆骨坐标和右髋关节坐标。
以右髋关节为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，以右髋关节坐标系作为右大腿惯性测量单元的参照坐标系，人体静止时，令右大腿惯性测量单元坐标系与右髋关节坐标系之间的旋转矩阵为单位矩阵，在右髋关节坐标系中，通过右大腿惯性测量单元传回的四元数表示的旋转矩阵，可以实时得到右大腿的姿态以及位置信息，根据坐标变换，获得人体基坐标系下的右膝盖位置坐标。
以右膝盖为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，以右膝盖坐标系作为右小腿惯性测量单元的参照坐标系，人体静止时，令右小腿惯性测量单元坐标系与右膝盖坐标系之间的旋转矩阵为单位矩阵，在右膝盖坐标系中，通过右小腿惯性测量单元传回的四元数表示的旋转矩阵，可以实时得到右小腿的姿态以及位置信息，根据坐标变换，获得人体基坐标系下的右脚踝位置坐标。
右脚触地时，以右脚脚踝为原点，建立人体基坐标系，腿关节坐标系建立方法同上。
同时，以盆骨坐标系作为上身躯干的参照坐标系，人体静止时，令上 身躯干惯性测量单元坐标系与盆骨坐标系之间的旋转矩阵为单位矩阵，在盆骨坐标系中，通过上身躯干惯性测量单元传回的四元数表示的旋转矩阵，可以实时得到上身躯干的姿态以及位置信息，根据坐标变换，获得人体基坐标系下的肩膀位置坐标。
以肩膀为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，并将肩膀坐标系作为大臂运动的参照坐标系，通过大臂惯性测量单元传回的四元数表示的旋转矩阵，可以实时得到大臂的姿态以及位置信息，根据坐标变换，获得人体基坐标系下手肘的位置坐标。
以手肘为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，并将手肘坐标系作为小臂运动的参照坐标系，通过小臂惯性测量单元传回的四元数表示的旋转矩阵，可以实时得到小臂的姿态以及位置信息，根据坐标变换，获得人体基坐标系下手腕的位置坐标。
从而获得人体在人体基坐标系下的完整姿态以及各个关节的位置坐标。通过人体基坐标系和全局坐标系之间的变换矩阵，可以得到全局坐标系下的人体完整姿态以及各个关节的位置坐标。
S2具体包括：
S21，获取全局坐标系下机器人基座的坐标；所述基座为机器人触地的基座。具体的，全局坐标系下机器人基座的位置固定不变，以基座中心点为全局坐标系原点，坐标系姿态与全局坐标系一致，建立基座坐标系，基座中心点在全局坐标系下的坐标由实际测量获得。
S22，获取利用第二惯性测量单元测量的第二旋转矩阵；所述第二惯性测量单元为多个，分别设置于机器人的基座和连杆上。如图5所示，IMU1放置在机器人基座上，其余5个IMU放置在机器人各个连杆上，其余5个IMU分别为IMU2、IMU3、IMU4、IMU5和IMU6。
S23，根据所述全局坐标系下机器人基座的坐标和所述第二旋转矩阵，确定机器人在全局坐标系下的位姿。
具体的，机器人上的惯性测量单元放置位置如图5所示，本发明实例选取6自由度机械臂，在基座和每个连杆上分别放置一个惯性测量单元，用于获取机械臂的姿态信息。上述提到的获得人体上半身姿态的方法同样适用于机械臂姿态的获取，不再赘述。以此，获得全局坐标系下机器人的 姿态。
S3具体包括：
S31，根据所述人体在全局坐标系下的位姿和所述机器人在全局坐标系下的位姿，确定人体与机器人之间的距离集合；所述距离集合中包括人体中每个人体躯干分别与机器人连杆之间的距离；所述人体躯干包括上身躯干、左大臂、右大臂、左小臂、右小臂、左大腿、右大腿、左小腿和右小腿。
S32，选取所述距离集合中最小值作为人体与机器人之间的距离。
S33，根据所述人体与机器人之间的距离确定机器人的运动速度。
将人体各部分躯干以及机械臂的各个连杆视为空间中的圆柱体，利用数学公式求解圆柱体间的最短距离。具体的，将人体每部分身体躯干各视为一个圆柱体，机器人的每个连杆各视为一个圆柱体，利用数学公式求解圆柱间的最短距离。
首先求出两个圆柱体中轴线之间的距离，问题转换为求解空间中两条线段之间的距离。以任意两条线段为例，人体上的一条线段AB，其中A和B的坐标分别为A(x a,y a,z a)和B(x b,y b,z b)，机械臂上的一条线段CD，其中C和D的坐标分别为C(x c,y c,z c)和D(x d,y d,z d)。
定义中间变量：H、I、J、K、L、M、N、O、P和Q，令H＝x b-x a，I＝y b-y a，J＝z b-z a，K＝x d-x c,L＝y d-y c，M＝z d-z c，
Figure PCTCN2020102934-appb-000028
可以得到直线AB与直线CD之间的公垂线方程：N·x-0·y+P·z+Q＝0,令
Figure PCTCN2020102934-appb-000029
可得公垂线与直线CD的交点E(x E,y E,z E)，其中x E＝K·k′+x c，y E＝L·k′+y c，z E＝M·k′+z c。
同理可得公垂线与直线AB的交点F(x F,y F,z F)。
若点E存在于线段CD上，点F存在于线段AB上，则线段AB与线段CD间的最短距离为：
Figure PCTCN2020102934-appb-000030
若点E不存在于线段CD上或点F不存在于线段AB上时，则使min{L AC，L AD，L BC，L BD}为直线AB与线段CD间的最短距离。
Figure PCTCN2020102934-appb-000031
则两个圆柱体之间的距离为两个圆柱中轴线之间的距离与两个圆柱体的半径之差，即两个圆柱中轴线之间的距离减去两个圆柱体的半径。
分别计算人体各部分躯干与机器人各个连杆之间的距离，搜索距离的最小值作为人体和机器人之间的距离。根据已知的机器人末端连杆的位置，对时间进行微分获得机器人的运动速度。
S4具体包括：
S41，当所述人体与机器人之间的距离大于第一预设距离阈值且所述机器人的运动速度小于预设速度阈值时，确定人体处于第一安全状态，即视人体处于安全状态。
S42，当所述人体与机器人之间的距离大于第一预设距离阈值且机器人速度大于或等于预设速度阈值时，确定人体处于第二安全状态，即视人体处于相对安全状态。
S43，当所述人体与机器人之间的距离大于第二预设距离阈值且所述人体与机器人之间的距离小于或等于第一预设距离阈值且所述机器人的运动速度小于预设速度阈值时，确定人体处于第二安全状态，即视人体处于相对安全状态，其中，所述第一预设距离阈值大于所述第二预设距离阈值。
S44，当所述人体与机器人之间的距离大于第二预设距离阈值且所述人体与机器人之间的距离小于或等于第一预设距离阈值且所述机器人的运动速度大于或等于预设速度阈值时，确定人体处于第二危险状态，即视人体处于相对危险状态。
S45，当所述人体与机器人之间的距离小于或等于第二预设距离阈值且所述机器人的运动速度小于预设速度阈值时，确定人体处于第二危险状态，即视人体处于相对危险状态。
S46，当所述人体与机器人之间的距离小于或等于第二预设距离阈值且所述机器人的运动速度大于或等于预设速度阈值时，确定人体处于第一危险，即视人体处于危险状态。
具体的，根据机器人的外展距离、所安装的工具的长度、机器人的工作性能以及先验经验来确定距离阈值和速度阈值，本发明实施例中所述第一预设距离阈值为L max，所述第二预设距离阈值为L max/3，所述预设速度阈值L max/1.25。其中，L max为机器人外展长度的最大值，第二预设距离阈值为机器人可外展长度的最大值的三分之一，即0.33L max，而正常人的反应能力参数为1.25秒，即发现前方有目标反映到大脑需0.5秒，从大脑反应到手、脚后并采取措施需0.75秒。
通过上述方法，可以在人机协作过程中，通过监控机器人与人体之间的距离和机器人的运行速度确定当前人体的安全状态，根据不同安全状态做出下一步工作计划，有效避免危险情况发生，使人机协作更安全更高效。
本发明还提供了一种人机协作中人体安全评估系统，包括：
第一获取模块，用于获取人体在全局坐标系下的位姿。
第二获取模块，用于获取机器人在全局坐标系下的位姿。
距离和速度确定模块，用于根据所述人体在全局坐标系下的位姿和所述机器人在全局坐标系下的位姿，确定人体与机器人之间的距离以及机器人的运动速度。
人体安全等级确定模块，用于根据所述人体与机器人之间的距离和所述机器人的运动速度，确定人机协作中人体的安全等级。
优选的，所述第一获取模块具体包括：
第一姿态关系和第二姿态关系获取单元，用于获取人体基坐标系与全局坐标系之间的第一姿态关系以及惯性测量单元坐标系与全局坐标系之间的第二姿态关系。
第三姿态关系确定单元，用于根据所述第一姿态关系和所述第二姿态关系确定人体基坐标系与惯性测量单元坐标系之间的第三姿态关系；其 中，
Figure PCTCN2020102934-appb-000032
为第一姿态关系，
Figure PCTCN2020102934-appb-000033
为第二姿态关系，
Figure PCTCN2020102934-appb-000034
为第三姿态关系，G为全局坐标系，B为人体基坐标系，S为惯性测量单元坐标系。
人体基坐标系原点确定单元，用于选取人体行走时，人体脚触地所在侧的脚踝为所述人体基坐标系的原点；所述人体基坐标系的原点在人体的左脚踝和人体的右脚踝之间切换。
第一旋转矩阵获取单元，用于获取利用第一惯性测量单元测量的第一旋转矩阵；所述第一惯性测量单元为多个，分别设置于人体的上身躯干、左大臂、右大臂、左小臂、右小臂、左大腿、右大腿、左小腿、右小腿、左脚背和右脚背。
人体在全局坐标系下位姿确定单元，用于根据所述人体基坐标系的原点、所述第一旋转矩阵和所述第三姿态关系。
具体的，在本发明实施例中，所述第一获取模块具体包括：
标定单元，用于对惯性测量单元进行标定，得到惯性测量单元坐标系与其对应人体坐标系之间的坐标变换矩阵。
数据接收单元，用于接收惯性测量单元返回的四元数和角速度。
位姿获取单元，用于根据所得各个惯性测量单元的角速度以及四元数，得到人体在人体基坐标系下的姿态及位置坐标。
坐标转换单元，用于将所述人体在人体基坐标系下的位置坐标转换为人体在全局坐标系下的位置坐标。
优选的，所述第二获取模块具体包括：
基座坐标获取单元，用于获取全局坐标系下机器人基座的坐标。
第二旋转矩阵获取单元，用于获取利用第二惯性测量单元测量的第二旋转矩阵；所述第二惯性测量单元为多个，分别设置于机器人的基座和连杆上。
机器人在全局坐标系下位姿确定单元，用于根据所述全局坐标系下机器人基座的坐标和所述第二旋转矩阵，确定机器人在全局坐标系下的位姿。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的 都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

  1. 一种人机协作中人体安全评估方法，其特征在于，包括：
    获取人体在全局坐标系下的位姿；
    获取机器人在全局坐标系下的位姿；
    根据所述人体在全局坐标系下的位姿和所述机器人在全局坐标系下的位姿，确定人体与机器人之间的距离以及机器人的运动速度；
    根据所述人体与机器人之间的距离和所述机器人的运动速度，确定人机协作中人体的安全等级。
  2. 根据权利要求1所述的人机协作中人体安全评估方法，其特征在于，所述获取人体在全局坐标系下的位姿，具体包括：
    获取人体基坐标系与全局坐标系之间的第一姿态关系
    Figure PCTCN2020102934-appb-100001
    以及惯性测量单元坐标系与全局坐标系之间的第二姿态关系
    Figure PCTCN2020102934-appb-100002
    根据所述第一姿态关系和所述第二姿态关系确定人体基坐标系与惯性测量单元坐标系之间的第三姿态关系
    Figure PCTCN2020102934-appb-100003
    其中，
    Figure PCTCN2020102934-appb-100004
    为第一姿态关系，
    Figure PCTCN2020102934-appb-100005
    为第二姿态关系，
    Figure PCTCN2020102934-appb-100006
    为第三姿态关系，G为全局坐标系，B为人体基坐标系，S为惯性测量单元坐标系；
    选取人体行走时，人体脚触地所在侧的脚踝为所述人体基坐标系的原点；所述人体基坐标系的原点在人体的左脚踝和人体的右脚踝之间切换；
    获取利用第一惯性测量单元测量的第一旋转矩阵；所述第一惯性测量单元为多个，分别设置于人体的上身躯干、左大臂、右大臂、左小臂、右小臂、左大腿、右大腿、左小腿、右小腿、左脚背和右脚背；
    根据所述人体基坐标系的原点、所述第一旋转矩阵和所述第三姿态关系，确定人体在全局坐标系下的位姿。
  3. 根据权利要求2所述的人机协作中人体安全评估方法，其特征在于，所述根据所述人体基坐标系的原点、所述第一旋转矩阵和所述第三姿态关系，确定人体在全局坐标系下的位姿，具体包括：
    在开始运动之前，人体保持校准姿势，校准过程为：在所有的第一惯性测量单元上进行，每个所述第一惯性测量单元都与一个具有预定义的坐标系的身体部分相关联，将关联后的第一惯性测量单元用于实时运动捕捉中；
    获取人体在人体基坐标系下的位姿：人体肢体长度是确定已知的，将关联后的第一惯性测量单元放置在人体的上身躯干，以人体直立静止时为初始状态；人在行走时，总会保持一只脚触地，取该侧脚踝为人体基坐标系原点，人体静止时，令每个第一惯性测量单元坐标系与参照坐标系之间的第一旋转矩阵为单位矩阵，人体运动时，通过第一惯性测量单元返回的四元数(ε 1,ε 2,ε 3,ε 4)，实时得到人体每一躯干的姿态以及位置信息；
    左脚触地时，左脚踝人体基坐标系作为左小腿的参照坐标系，已知人体静止时，在人体基坐标系中左膝盖的坐标为
    Figure PCTCN2020102934-appb-100007
    通过左小腿处的第一惯性测量单元测量的第一旋转矩阵R ε1为：
    Figure PCTCN2020102934-appb-100008
    则人体姿态发生变换后的左膝盖坐标为：P′ L1＝R ε1P L1；
    以左膝盖为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，并以左膝盖为坐标系原点建立的坐标系作为左大腿处的第一惯性测量单元运动的参照坐标系；人体静止时，令左大腿处的第一惯性测量单元坐标系与左膝盖坐标系之间的第一旋转矩阵为单位矩阵，在左膝盖坐标系中，左髋关节的坐标为
    Figure PCTCN2020102934-appb-100009
    通过左大腿处的第一惯性测量单元传回的四元数表示的第一旋转矩阵R ε2，实时得到左大腿的姿态以及位置信息；
    则人体姿态发生变换后的左髋关节坐标为：P′ L2＝R ε2P L2；
    根据所述人体姿态发生变换后的左髋关节坐标和所述人体姿态发生变换后的左膝盖坐标，得到左髋关节在人体基坐标系下的坐标：P″ L2＝P′ L1+P′ L2；
    以左髋关节为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，假设人在移动过程中，盆骨的姿态不发生变化，则盆骨在左髋关节坐 标系中的位置固定，同时得盆骨在人体基坐标系中的位置；
    以盆骨中心作为坐标系的原点建立坐标系，坐标系姿态与人体基坐标系一致，右髋关节在盆骨坐标系中的位置固定不变，得到右髋关节在人体基坐标系中的位置；
    以右髋关节为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，以右髋关节坐标系作为右大腿处第一惯性测量单元的参照坐标系，人体静止时，令右大腿处第一惯性测量单元坐标系与右髋关节坐标系之间的第一旋转矩阵为单位矩阵，在右髋关节坐标系中，通过右大腿处的第一惯性测量单元传回的四元数表示的第一旋转矩阵，实时得到右大腿的姿态以及位置信息，根据坐标变换，获得人体基坐标系下右膝盖位置坐标；
    以右膝盖为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，以右膝盖坐标系作为右小腿处第一惯性测量单元的参照坐标系，人体静止时，令右小腿处第一惯性测量单元坐标系与右膝盖坐标系之间的第一旋转矩阵为单位矩阵，在右膝盖坐标系中，通过右小腿处的第一惯性测量单元传回的四元数表示的第一旋转矩阵，得到右小腿的姿态以及位置信息，根据坐标变换，获得人体基坐标系下右脚踝位置坐标；
    右脚触地时，以右脚踝为原点，建立人体基坐标系，腿关节坐标系建立方法同上；
    同时，以盆骨坐标系作为上身躯干的参照坐标系，人体静止时，令上身躯干惯性测量单元坐标系与盆骨坐标系之间的第一旋转矩阵为单位矩阵，在盆骨坐标系中，通过上身躯干处第一惯性测量单元传回的四元数表示的第一旋转矩阵，实时得到上身躯干的姿态以及位置信息，根据坐标变换，获得人体基坐标系下的肩膀位置坐标；
    以肩膀为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，并将肩膀坐标系作为大臂运动的参照坐标系，通过大臂处第一惯性测量单元传回的四元数表示的第一旋转矩阵，实时得到大臂的姿态以及位置信息，根据坐标变换，获得人体基坐标系下手肘的位置坐标；
    以手肘为坐标系原点建立坐标系，坐标系姿态与人体基坐标系一致，并将手肘坐标系作为小臂运动的参照坐标系，通过小臂处第一惯性测量单元传回的四元数表示的第一旋转矩阵，实时得到小臂的姿态以及位置信 息，根据坐标变换，获得人体基坐标系下手腕的位置坐标；
    从而获得人体在人体基坐标系下的完整姿态以及各个关节的位置坐标；通过人体基坐标系和全局坐标系之间的第三姿态关系，得到全局坐标系下的人体完整姿态以及各个关节的位置坐标。
  4. 根据权利要求1所述的人机协作中人体安全评估方法，其特征在于，所述获取机器人在全局坐标系下的位姿，具体包括：
    获取全局坐标系下机器人基座的坐标；
    获取利用第二惯性测量单元测量的第二旋转矩阵；所述第二惯性测量单元为多个，分别设置于机器人的基座和连杆上；
    根据所述全局坐标系下机器人基座的坐标和所述第二旋转矩阵，确定机器人在全局坐标系下的位姿。
  5. 根据权利要求1所述的人机协作中人体安全评估方法，其特征在于，所述根据所述人体在全局坐标系下的位姿和所述机器人在全局坐标系下的位姿，确定人体与机器人之间的距离以及机器人的运动速度，具体包括：
    根据所述人体在全局坐标系下的位姿和所述机器人在全局坐标系下的位姿，确定人体与机器人之间的距离集合；所述距离集合中包括人体中每个人体躯干分别与机器人连杆之间的距离；所述人体躯干包括上身躯干、左大臂、右大臂、左小臂、右小臂、左大腿、右大腿、左小腿和右小腿；
    选取所述距离集合中最小值作为人体与机器人之间的距离；
    根据所述人体与机器人之间的距离确定机器人的运动速度。
  6. 根据权利要求1所述的人机协作中人体安全评估方法，其特征在于，所述根据所述人体与机器人之间的距离和所述机器人的运动速度，确定人机协作中人体的安全等级，具体包括：
    当所述人体与机器人之间的距离大于第一预设距离阈值且所述机器人的运动速度小于预设速度阈值时，确定人体处于第一安全状态；
    当所述人体与机器人之间的距离大于第一预设距离阈值且机器人速度大于或等于预设速度阈值时，确定人体处于第二安全状态；
    当所述人体与机器人之间的距离大于第二预设距离阈值且所述人体 与机器人之间的距离小于或等于第一预设距离阈值且所述机器人的运动速度小于预设速度阈值时，确定人体处于第二安全状态；所述第一预设距离阈值大于所述第二预设距离阈值；
    当所述人体与机器人之间的距离大于第二预设距离阈值且所述人体与机器人之间的距离小于或等于第一预设距离阈值且所述机器人的运动速度大于或等于预设速度阈值时，确定人体处于第二危险状态；
    当所述人体与机器人之间的距离小于或等于第二预设距离阈值且所述机器人的运动速度小于预设速度阈值时，确定人体处于第二危险状态；
    当所述人体与机器人之间的距离小于或等于第二预设距离阈值且所述机器人的运动速度大于或等于预设速度阈值时，确定人体处于第一危险状态。
  7. 根据权利要求6所述的人机协作中人体安全评估方法，其特征在于，所述第一预设距离阈值为L max；所述第二预设距离阈值为L max/3；所述预设速度阈值L max/1.25；其中，L max为机器人外展长度的最大值。
  8. 一种人机协作中人体安全评估系统，其特征在于，包括：
    第一获取模块，用于获取人体在全局坐标系下的位姿；
    第二获取模块，用于获取机器人在全局坐标系下的位姿；
    距离和速度确定模块，用于根据所述人体在全局坐标系下的位姿和所述机器人在全局坐标系下的位姿，确定人体与机器人之间的距离以及机器人的运动速度；
    人体安全等级确定模块，用于根据所述人体与机器人之间的距离和所述机器人的运动速度，确定人机协作中人体的安全等级。
  9. 根据权利要求8所述的人机协作中人体安全评估系统，其特征在于，所述第一获取模块具体包括：
    第一姿态关系和第二姿态关系获取单元，用于获取人体基坐标系与全局坐标系之间的第一姿态关系以及惯性测量单元坐标系与全局坐标系之间的第二姿态关系；
    第三姿态关系确定单元，用于根据所述第一姿态关系和所述第二姿态关系确定人体基坐标系与惯性测量单元坐标系之间的第三姿态关系；其 中，
    Figure PCTCN2020102934-appb-100010
    为第一姿态关系，
    Figure PCTCN2020102934-appb-100011
    为第二姿态关系，
    Figure PCTCN2020102934-appb-100012
    为第三姿态关系，G为全局坐标系，B为人体基坐标系，S为惯性测量单元坐标系；
    人体基坐标系原点确定单元，用于选取人体行走时，人体脚触地所在侧的脚踝为所述人体基坐标系的原点；所述人体基坐标系的原点在人体的左脚踝和人体的右脚踝之间切换；
    第一旋转矩阵获取单元，用于获取利用第一惯性测量单元测量的第一旋转矩阵；所述第一惯性测量单元为多个，分别设置于人体的上身躯干、左大臂、右大臂、左小臂、右小臂、左大腿、右大腿、左小腿、右小腿、左脚背和右脚背；
    人体在全局坐标系下位姿确定单元，用于根据所述人体基坐标系的原点、所述第一旋转矩阵和所述第三姿态关系。
  10. 根据权利要求8所述的人机协作中人体安全评估系统，其特征在于，所述第二获取模块具体包括：
    基座坐标获取单元，用于获取全局坐标系下机器人基座的坐标；
    第二旋转矩阵获取单元，用于获取利用第二惯性测量单元测量的第二旋转矩阵；所述第二惯性测量单元为多个，分别设置于机器人的基座和连杆上；
    机器人在全局坐标系下位姿确定单元，用于根据所述全局坐标系下机器人基座的坐标和所述第二旋转矩阵，确定机器人在全局坐标系下的位姿。
PCT/CN2020/102934 2019-12-11 2020-07-20 人机协作中人体安全评估方法及系统 Ceased WO2021114666A1 (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN201911262776.X 2019-12-11
CN201911262776.XA CN110978064B (zh) 2019-12-11 2019-12-11 人机协作中人体安全评估方法及系统

Publications (1)

Publication Number Publication Date
WO2021114666A1 true WO2021114666A1 (zh) 2021-06-17

Family

ID=70092202

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2020/102934 Ceased WO2021114666A1 (zh) 2019-12-11 2020-07-20 人机协作中人体安全评估方法及系统

Country Status (2)

Country Link
CN (1) CN110978064B (zh)
WO (1) WO2021114666A1 (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114706403A (zh) * 2022-04-19 2022-07-05 广西大学 一种基于红外摄像头与定位识别垫的人机协作与预防人机碰撞的系统及方法
CN114886567A (zh) * 2022-05-12 2022-08-12 苏州大学 一种面向具有远心不动点约束的手术机器人手眼标定方法
CN115457440A (zh) * 2022-09-09 2022-12-09 北京理工大学 一种基于单目视频评估滑雪回转水平的方法
CN118918614A (zh) * 2024-10-10 2024-11-08 浙江大学 一种融合人本智造的人体姿态估计方法和装置

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110978064B (zh) * 2019-12-11 2022-06-24 山东大学 人机协作中人体安全评估方法及系统
CN112936268A (zh) * 2021-01-30 2021-06-11 埃夫特智能装备股份有限公司 一种协作机器人安全控制系统
CN112957033B (zh) * 2021-02-01 2022-10-18 山东大学 人机协作中人体实时室内定位与运动姿态捕捉方法与系统
CN113066337B (zh) * 2021-04-20 2025-05-06 上海松鼠课堂人工智能科技有限公司 基于虚拟现实场景的化学实验实操教学方法
CN114714358A (zh) * 2022-04-18 2022-07-08 山东大学 基于手势协议遥操作机械臂方法及系统
CN114757293B (zh) * 2022-04-27 2024-09-27 山东大学 基于动作识别和人机距离的人机共融风险预警方法及系统
CN114952854B (zh) * 2022-06-16 2024-07-09 山东大学 基于人机协作的人体碰撞物对接方法及系统
CN115407750B (zh) * 2022-08-12 2023-11-21 北京津发科技股份有限公司 人机协同智能系统决策能力的测评方法及系统

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4798461A (en) * 1985-07-08 1989-01-17 Bertin & Cie. System for monitoring the geometry of a mechanical structure
CN103386683A (zh) * 2013-07-31 2013-11-13 哈尔滨工程大学 一种基于Kinect的体感控制机械臂方法
CN106153077A (zh) * 2016-09-22 2016-11-23 苏州坦特拉自动化科技有限公司 一种用于m‑imu人体运动捕获系统的初始化校准方法
CN108527370A (zh) * 2018-04-16 2018-09-14 北京卫星环境工程研究所 基于视觉的人机共融安全防护控制系统
CN108527305A (zh) * 2018-04-11 2018-09-14 南京理工大学 一种基于虚拟现实技术的带电作业机器人力反馈主从控制方法及系统
CN110978064A (zh) * 2019-12-11 2020-04-10 山东大学 人机协作中人体安全评估方法及系统

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105252532B (zh) * 2015-11-24 2017-07-04 山东大学 动作捕捉机器人协同柔性姿态控制的方法
GB2559020B (en) * 2016-11-28 2021-11-03 Faro Tech Inc Noncontact safety sensor and method of operation
CN108247654A (zh) * 2018-02-05 2018-07-06 遨博（北京）智能科技有限公司 一种机器人的控制方法、装置和系统
CN109822579A (zh) * 2019-04-10 2019-05-31 江苏艾萨克机器人股份有限公司 基于视觉的协作机器人安全控制方法
CN110238831B (zh) * 2019-07-23 2020-09-18 青岛理工大学 基于rgb-d图像及示教器的机器人示教系统及方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4798461A (en) * 1985-07-08 1989-01-17 Bertin & Cie. System for monitoring the geometry of a mechanical structure
CN103386683A (zh) * 2013-07-31 2013-11-13 哈尔滨工程大学 一种基于Kinect的体感控制机械臂方法
CN106153077A (zh) * 2016-09-22 2016-11-23 苏州坦特拉自动化科技有限公司 一种用于m‑imu人体运动捕获系统的初始化校准方法
CN108527305A (zh) * 2018-04-11 2018-09-14 南京理工大学 一种基于虚拟现实技术的带电作业机器人力反馈主从控制方法及系统
CN108527370A (zh) * 2018-04-16 2018-09-14 北京卫星环境工程研究所 基于视觉的人机共融安全防护控制系统
CN110978064A (zh) * 2019-12-11 2020-04-10 山东大学 人机协作中人体安全评估方法及系统

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114706403A (zh) * 2022-04-19 2022-07-05 广西大学 一种基于红外摄像头与定位识别垫的人机协作与预防人机碰撞的系统及方法
CN114886567A (zh) * 2022-05-12 2022-08-12 苏州大学 一种面向具有远心不动点约束的手术机器人手眼标定方法
CN114886567B (zh) * 2022-05-12 2023-06-23 苏州大学 一种面向具有远心不动点约束的手术机器人手眼标定方法
CN115457440A (zh) * 2022-09-09 2022-12-09 北京理工大学 一种基于单目视频评估滑雪回转水平的方法
CN115457440B (zh) * 2022-09-09 2025-10-17 北京理工大学 一种基于单目视频评估滑雪回转水平的方法
CN118918614A (zh) * 2024-10-10 2024-11-08 浙江大学 一种融合人本智造的人体姿态估计方法和装置

Also Published As

Publication number Publication date
CN110978064A (zh) 2020-04-10
CN110978064B (zh) 2022-06-24

Similar Documents

Publication Publication Date Title
WO2021114666A1 (zh) 人机协作中人体安全评估方法及系统
Verdel et al. An identification-based method improving the transparency of a robotic upper limb exoskeleton
EP3173191A2 (en) Method for estimating posture of robotic walking aid
WO2018196227A1 (zh) 人体运动能力评价方法、装置及系统
US20200178851A1 (en) Systems and methods for tracking body movement
Palani et al. Real-time joint angle estimation using mediapipe framework and inertial sensors
Jung et al. Kinematic analysis of a 5-DOF upper-limb exoskeleton with a tilted and vertically translating shoulder joint
CN110442947B (zh) 融合平衡策略的下肢机器人动力学仿真平台及仿真方法
Bonnet et al. Fast determination of the planar body segment inertial parameters using affordable sensors
Maurice Virtual ergonomics for the design of collaborative robots
CN108170281A (zh) 一种工作姿势分析系统测算方法
Rakhodaei et al. Path planning of the hybrid parallel robot for ankle rehabilitation
Fard A manipulability analysis of human walking
Hsu et al. Development of an ergonomic evaluation system based on inertial measurement unit and its application for exoskeleton load reduction
CN107320106B (zh) 一种测量手扶式起立时人体重心位置的装置
Padua et al. A framework for casualty manipulation with biomechanical joint-level reaction analysis
KR20160118678A (ko) 근골격계 질환 예방을 위한 작업부하 측정 및 작업순환 순서 결정 방법
Schemschat et al. Joint torque analysis of push recovery motions during human walking
Lu et al. Gait-Event-Based human intention recognition approach for lower limb
Yoon et al. Feasibility of using depth cameras for evaluating human-exoskeleton interaction
KR102053383B1 (ko) 사용자의 움직임을 관측하여 요추 부하량을 실시간으로 계측하기 위한 장치
Yang et al. Research on human motion data filtering based on unscented Kalman filter algorithm
Ceccarelli The historical development of catrasys, a cable system
Chang et al. A research on the postural stability of a person wearing the lower limb exoskeletal robot by the HAT model
Liu et al. Configuration comparison and design of an upper limb exoskeleton for robot teleoperation

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 20900179

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 20900179

Country of ref document: EP

Kind code of ref document: A1

122 Ep: pct application non-entry in european phase

Ref document number: 20900179

Country of ref document: EP

Kind code of ref document: A1

32PN Ep: public notification in the ep bulletin as address of the adressee cannot be established

Free format text: NOTING OF LOSS OF RIGHTS PURSUANT TO RULE 112(1) EPC (EPO FORM 1205A DATED 07/03/2023)

122 Ep: pct application non-entry in european phase

Ref document number: 20900179

Country of ref document: EP

Kind code of ref document: A1