WO2021114666A1 - Human body safety evaluation method and system in human-machine collaboration - Google Patents

Abstract

A human body safety evaluation method and system in human-machine collaboration. Said method comprises: acquiring a pose of a human body in a global coordinate system (S1); acquiring a pose of a robot in the global coordinate system (S2); according to the pose of the human body in the global coordinate system and the pose of the robot in the global coordinate system, determining the distance between the human body and the robot and the movement speed of the robot (S3); and according to the distance between the human body and the robot and the movement speed of the robot, determining a safety level of the human body in human-machine collaboration (S4). During a human-machine collaboration process, a current safety state of a human body is determined by monitoring the distance between a robot and the human body and the movement speed of the robot, and a next work plan is made according to different safety states, thereby effectively avoiding the occurrence of a dangerous situation, allowing for a safe and efficient human-machine collaboration.

Description

Human body safety assessment method and system in human-machine collaboration

This application claims the priority of a Chinese patent application filed with the Chinese Patent Office on December 11, 2019, the application number is 201911262776.X, and the invention title is "Human Body Safety Evaluation Method and System in Human-Machine Cooperation", the entire content of which is incorporated by reference Incorporated in this application.

Technical field

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.

Background technique

The rapid development of the robotics industry has put forward new requirements for the close collaboration between humans and robots, and gradually gave birth to collaborative robots. Traditional industrial robots are designed with fence isolation. Workers are not allowed to enter when the robot is working, or the robot must stop working when the worker enters. This method not only wastes working space, but also reduces the overall work efficiency. Collaborative robots can work collaboratively with humans, without the need for barriers to isolate them, and combine human "knowledge, analysis, and decision-making" capabilities with robots' "power, precision, and repeatability". Collaborative robots and humans are in the same working space. The level of collaboration is continuously strengthened. The two are closely coordinated, which not only saves working space, but also greatly improves work efficiency. On the premise of ensuring the safety of personnel, robots can independently improve their skills and achieve Naturally interact and collaborate with people.

According to Asimov’s Three Laws of Robotics, no matter what kind of robot, it should not harm humans and the robot itself under any situation and environment. Therefore, safety is the primary problem and mandatory constraint for the application of robots in the field of human production and life. There is no sense of safety in itself. If a collision occurs when humans and robots work together, operators in a disadvantaged position will have relatively greater safety risks. Therefore, it is necessary to conduct adequate safety assessments and formulate corresponding protections for each possible risk point of collaborative robots. Measures to ensure the safety of operators.

The existing human-machine safety assessment methods in human-machine collaboration only judge the safety of the human body by calculating the distance between the robot and the human body, while ignoring the impact of the robot's movement speed, an important parameter, on human safety, resulting in human-machine collaboration. China's human safety assessment is inaccurate, and specific risks cannot be investigated.

Summary of the invention

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.

In order to achieve the above objective, the present invention provides a human body safety assessment method in human-machine collaboration, including:

Get the pose of the human body in the global coordinate system;

Obtain the pose of the robot in the global coordinate system;

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;

According to the distance between the human body and the robot and the movement speed of the robot, the safety level of the human body in the human-robot collaboration is determined.

Optionally, the acquiring the pose of the human body in the global coordinate system specifically includes:

以及惯性测量单元坐标系与全局坐标系之间的第二姿态关系

Obtain the first posture relationship between the human base coordinate system and the global coordinate system

And the second attitude relationship between the inertial measurement unit coordinate system and the global coordinate system

其中，

为第一姿态关系，

为第二姿态关系，

为第三姿态关系，G为全局坐标系，B为人体基坐标系，S为惯性测量单元坐标系； Determine the third posture relationship between the human body base coordinate system and the inertial measurement unit coordinate system according to the first posture relationship and the second posture relationship

among them,

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;

When the human body is selected to walk, 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;

Obtain 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, left forearm, right forearm, and left of the human body. Thigh, right thigh, left calf, right calf, left instep and right instep;

According to the origin of the basic coordinate system of the human body, the first rotation matrix and the third posture relationship, the posture of the human body in the global coordinate system is determined.

Optionally, 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:

Before starting to move, the human body maintains a calibration posture. 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;

Obtain the position and posture of the human body in the basic coordinate system of the human body: 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. When the human body is stationary, let the first rotation matrix between each first inertial measurement unit coordinate system and the reference coordinate system be the identity matrix, When the human body moves, through the quaternion (ε ₁ , ε ₂ , ε ₃ , ε ₄ ) returned by the first inertial measurement unit, the posture and position information of each torso of the human body can be obtained in real time;

通过左小腿处的第一惯性测量单元测量的第一旋转矩阵R _ε1为： When the left foot touches the ground, the base coordinate system of the left ankle 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 base coordinate system of the human body are

_{The first rotation matrix R ε1} measured by the first inertial measurement unit at the left calf is:

Then the coordinates of the left knee after the transformation of the human body posture is: P′ _L1 = R _ε1 P _L1 ;

通过左大腿处的第一惯性测量单元传回的四元数表示的第一旋转矩阵R _ε2，实时得到左大腿的姿态以及位置信息； 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; When at rest, let 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. In the left knee coordinate system, 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;

Then the coordinates of the left hip joint after the posture of the human body is transformed are: P′ _L2 = R _ε2 P _L2 ;

According to the coordinates of the left hip joint after the transformation of the human body posture and the coordinates of the left knee after the transformation of the human body posture, the coordinates of the left hip joint in the basic human body coordinate system are obtained: P″ _L2 = P′ _L1 + P′ _L2 ；

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. When the human body is stationary, 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. In the right knee coordinate system, 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;

When the right foot touches the ground, use the right ankle as the origin to establish the basic coordinate system of the human body. The method of establishing the coordinate system of the leg joints is the same as above;

At the same time, the pelvic coordinate system is used as the reference coordinate system of the upper body torso. When the human body is stationary, the first rotation matrix between the upper body torso inertial measurement unit coordinate system and the pelvic coordinate system is the identity matrix. In 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;

In order to obtain the complete posture of the human body in the human body base coordinate system and the position coordinates of each joint; through the third posture relationship between the human base coordinate system and the global coordinate system, the complete posture of the human body and the position of each joint in the global coordinate system are obtained coordinate.

Optionally, the acquiring the pose of the robot in the global coordinate system specifically includes:

Obtain the coordinates of the robot base in the global coordinate system;

Acquiring 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;

According to the coordinates of the robot base in the global coordinate system and the second rotation matrix, the pose of the robot in the global coordinate system is determined.

Optionally, 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:

According to 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;

Selecting the minimum value in the distance set as the distance between the human body and the robot;

The movement speed of the robot is determined according to the distance between the human body and the robot.

Optionally, 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:

When the distance between the human body and the robot is greater than 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 the first safe state;

When the distance between the human body and the robot is greater than the first preset distance threshold and the robot speed is greater than or equal to the preset speed threshold, determining that the human body is in the second safe state;

When 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;

When 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, and the movement speed of the robot is greater than or equal to the preset speed threshold At the time, it is determined that the human body is in the second dangerous state;

When the distance between the human body and the robot is less than or equal to the second preset distance threshold and the movement speed of the robot is less than the preset speed threshold, it is determined that the human body is in the second dangerous state;

When the distance between the human body and the robot is less than or equal to the second preset distance threshold and the movement speed of the robot is greater than or equal to the preset speed threshold, it is determined that the human body is in the first dangerous state.

Optionally, 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.

Optionally, 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;

为第一姿态关系，

为第二姿态关系，

为第三姿态关系，G为全局坐标系，B为人体基坐标系，S为惯性测量单元坐标系； 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.

Optionally, 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.

According to the specific embodiments provided by the present invention, the present invention discloses the following technical effects:

In the present invention, through the set test environment, 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. In the whole process, 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.

In the process of human-machine cooperation, 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.

Attached drawings

In order to explain the embodiments of the present invention or the technical solutions in the prior art more clearly, the following will briefly introduce the drawings that need to be used in the embodiments. Obviously, the drawings in the following description are only some of the present invention. Embodiments, for those of ordinary skill in the art, without creative labor, other drawings can be obtained based on these drawings.

Figure 1 is a schematic diagram of a test system scenario provided by an embodiment of the present invention;

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;

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.

Detailed ways

The technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative work shall fall within the protection scope 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.

In order to make the above-mentioned objects, features and advantages of the present invention more obvious and understandable, the present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments.

In the present invention, through the set test environment, both the robot and the operator wear an inertial measurement unit (IMU) suit. As shown in Fig. 1, the collaborative robot works according to a predetermined trajectory, and the operator cooperates with the collaborative robot. In the whole process, 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. Specifically, 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:

S1, obtain the pose of the human body in the global coordinate system.

S2: Obtain the pose of the robot in the global coordinate system.

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:

以及惯性测量单元坐标系与全局坐标系之间的第二姿态关系

S11. Obtain the first posture relationship between the human base coordinate system and the global coordinate system

And the second attitude relationship between the inertial measurement unit coordinate system and the global coordinate system

其中，

为第一姿态关系，

为第二姿态关系，

为第三姿态关系，G为全局坐标系，B为人体基坐标系，S为惯性测量单元坐标系。 S12. Determine a third posture relationship between the human body base coordinate system and the inertial measurement unit coordinate system according to the first posture relationship and the second posture relationship

among them,

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.

时，简单起见，所建立的人体基坐标系与全局坐标系姿态一致。人体基坐标系与全局坐标系中的姿态关系表示为

惯性测量单元坐标系与全局坐标系之间的姿态关系表示为

人体基坐标系与惯性测量单元坐标系之间的姿态关系表示为

通过坐标系之间的变换： Specifically, obtain the posture relationship between the basic coordinate system of the human body and the global coordinate system

For simplicity, 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:

可得

Available

S13: When the human body is walking, the ankle on the side where the human foot touches the ground is selected as 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.

S14. Obtain a first rotation matrix measured by a first inertial measurement unit; there are multiple first inertial measurement units, which are respectively arranged on the upper torso, left big arm, right big arm, left forearm, and right forearm of the human body , Left thigh, right thigh, left calf, right calf, left instep and right instep.

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.

Specifically, 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. When walking, the origin of the basic coordinate system of the human body is switched between the left and right feet. First, 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. Upper torso, left and right arms (left and right arms) and left and right forearms (left forearms and right forearms), used to obtain the posture information of the upper body; 6 inertial measurement units are placed on the lower body, placed on the left and right respectively Thigh (left thigh and right upper arm), left and right calf (left calf and right calf) and left and right instep (left instep and right instep) are used to obtain posture information of the lower body. The position of the inertial measurement unit placed on the human body is not fixed. Changes.

Before starting the exercise, the subject (human body) maintains a calibration posture. This calibration process is carried out on all inertial measurement units, where each inertial measurement unit is associated with a body part with a predefined coordinate system. Use it for real-time motion capture.

Obtain 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 (ε ₁ , ε ₂ , ε ₃ , ε ₄ ) 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.

Now take the left foot touching the ground as an example to illustrate the process of obtaining a complete body posture:

通过左小腿处的第一惯性测量单元测量的四元数表示的旋转矩阵R _ε1为： 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:

Then the coordinate of the left knee after the posture of the human body is transformed is: P′ _L1 =R _ε1 P _L1 .

通过左大腿惯性测量单元传回的四元数表示的旋转矩阵R _ε2，可以实时得到左大腿的姿态以及位置信息。 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. When the human body is stationary, let 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. In the coordinate system of the left knee, 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.

Then the coordinates of the left hip joint after the posture of the human body is transformed are: P′ _L2 =R _ε2 P _L2 .

At the same time, the coordinates of the left hip joint relative to the basic coordinate system of the human body can be obtained: P″ _L2 = P′ _L1 + P′ _L2 .

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. When the human body is stationary, 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. In the right hip joint coordinate system, 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. When the human body is stationary, the coordinate system of the right calf inertial measurement unit and The rotation matrix between the right knee coordinate system is the unit matrix. In the right knee coordinate system, 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.

When the right foot touches the ground, use the ankle of the right foot as the origin to establish the basic coordinate system of the human body. The method for establishing the coordinate system of the leg joints is the same as above.

At the same time, the pelvic coordinate system is used as the upper body torso reference coordinate system. When the human body is stationary, the rotation matrix between the upper body torso inertial measurement unit coordinate system and the pelvic coordinate system is the unit matrix. In the pelvic coordinate system, 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. According to the coordinate transformation, 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.

In this way, 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. Through the transformation matrix between the human base coordinate system and the global coordinate system, 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. As shown in Figure 5, 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.

Specifically, 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. Specifically, 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.

First find the distance between the central axes of the two cylinders, and the problem is converted to the distance between the two line segments in the solution space. Take any two line segments as an example, a line segment AB on the human body, where the coordinates of A and B are A (x _a , y _a , z _a ) and B (x _b , y _b , z _b ), respectively, on the robot arm A line segment CD of, where the coordinates of C and D are C (x _c , y _c , z _c ) and D (x _d , y _d , z _d ), respectively.

Define intermediate variables: H, I, J, K, L, M, N, O, P, and Q, let 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 ,

The common perpendicular equation between the line AB and the line CD can be obtained: N·x-0·y+P·z+Q=0, so

_{Obtain the intersection point E(x E} ,y _E ,z _E ) of the common vertical line and the straight line CD, where x _E =K·k′+x _c , y _E =L·k′+y _c , z _E =M· k′+z _c .

In the same way, the intersection point F(x _F ,y _F ,z _F ) of the common perpendicular and the straight line AB can be obtained.

If point E exists on line segment CD and point F exists on line segment AB, the shortest distance between line segment AB and line segment CD is:

If the point E does not exist on the line segment CD or the point F does not exist on the line segment AB, let min{L _AC , L _AD , L _BC , L _BD } be the shortest distance between the line AB and the line segment CD.

Then 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.

Calculate the distance between the torso of each part of the human body and each link of the robot, and the minimum value of the search distance is used as the distance between the human body and the robot. According to the known position of the end link of the robot, the time is differentiated to obtain the movement speed of the robot.

S4 specifically includes:

S41: When the distance between the human body and the robot is greater than a first preset distance threshold and the movement speed of the robot is less than a preset speed threshold, it is determined that the human body is in a first safe state, that is, the human body is considered to be in a safe state.

S42: When the distance between the human body and the robot is greater than the first preset distance threshold and the robot speed is greater than or equal to the preset speed threshold, it is determined that the human body is in a second safe state, that is, the human body is considered to be in a relatively safe state.

S43: When 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, and the movement speed of the robot is less than the preset speed threshold When it is determined that the human body is in a second safe state, that is, the human body is considered to be in a relatively safe state, wherein the first preset distance threshold is greater than the second preset distance threshold.

S44, when 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, and the movement speed of the robot is greater than or equal to a preset distance At the speed threshold, it is determined that the human body is in the second dangerous state, that is, the human body is considered to be in a relatively dangerous state.

S45: When the distance between the human body and the robot is less than or equal to a second preset distance threshold and the movement speed of the robot is less than the preset speed threshold, it is determined that the human body is in a second dangerous state, that is, the human body is considered to be in a relatively dangerous state .

S46: When the distance between the human body and the robot is less than or equal to the second preset distance threshold and the movement speed of the robot is greater than or equal to the preset speed threshold, it is determined that the human body is in the first danger, that is, the human body is in a dangerous state .

Specifically, 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. In the embodiment of the present invention, the first preset distance threshold is L _max , The second preset distance threshold is L _max /3, and the preset speed threshold L _max /1.25. Among them, 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 , and 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.

Through the above method, in the process of human-machine collaboration, 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.

Preferably, 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.

为第一姿态关系，

为第二姿态关系，

为第三姿态关系，G为全局坐标系，B为人体基坐标系，S为惯性测量单元坐标系。 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.

Specifically, in the embodiment of the present invention, 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.

Preferably, 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.

The various embodiments in this specification are described in a progressive manner. Each embodiment focuses on the differences from other embodiments, and the same or similar parts between the various embodiments can be referred to each other. As for the system disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant information can be referred to the description of the method part.

Specific examples are used in this article to illustrate the principles and implementation of the present invention. The descriptions of the above examples are only used to help understand the methods and core ideas of the present invention; at the same time, for those of ordinary skill in the art, according to the present invention There will be changes in the specific implementation and scope of application. In summary, the content of this specification should not be construed as a limitation to the present invention.

Claims (10)

A method for human body safety assessment in human-machine collaboration, which is characterized in that it includes: Get the pose of the human body in the global coordinate system; Obtain the pose of the robot in the global coordinate system; 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; According to the distance between the human body and the robot and the movement speed of the robot, the safety level of the human body in the human-robot collaboration is determined. The human body safety assessment method in human-machine collaboration according to claim 1, wherein said acquiring the human body's pose in a global coordinate system specifically comprises: Obtain the first posture relationship between the human base coordinate system and the global coordinate system

And the second attitude relationship between the inertial measurement unit coordinate system and the global coordinate system

Determine the third posture relationship between the human body base coordinate system and the inertial measurement unit coordinate system according to the first posture relationship and the second posture relationship

among them,

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; When the human body is selected to walk, 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; Obtain 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, left forearm, right forearm, and left of the human body. Thigh, right thigh, left calf, right calf, left instep and right instep; According to the origin of the basic coordinate system of the human body, the first rotation matrix and the third posture relationship, the posture of the human body in the global coordinate system is determined. The human body safety assessment method in human-machine collaboration according to claim 2, wherein said determining that the human body is in a global position according to the origin of the human body base coordinate system, the first rotation matrix and the third posture relationship The pose in the coordinate system includes: Before starting to move, the human body maintains a calibration posture. 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; Obtain the position and posture of the human body in the basic coordinate system of the human body: 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. When the human body is stationary, let the first rotation matrix between each first inertial measurement unit coordinate system and the reference coordinate system be the identity matrix, When the human body moves, through the quaternion (ε ₁ , ε ₂ , ε ₃ , ε ₄ ) returned by the first inertial measurement unit, the posture and position information of each torso of the human body can be obtained in real time; When the left foot touches the ground, the base coordinate system of the left ankle 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 base coordinate system of the human body are

_{The first rotation matrix R ε1} measured by the first inertial measurement unit at the left calf is:

Then the coordinates of the left knee after the transformation of the human body posture is: P′ _L1 = R _ε1 P _L1 ; 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; When at rest, let 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. In the left knee coordinate system, 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; Then the coordinates of the left hip joint after the posture of the human body is transformed are: P′ _L2 = R _ε2 P _L2 ; According to the coordinates of the left hip joint after the transformation of the human body posture and the coordinates of the left knee after the transformation of the human body posture, the coordinates of the left hip joint in the basic human body coordinate system are obtained: P″ _L2 = P′ _L1 + P′ _L2 ； 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. When the human body is stationary, 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. In the right knee coordinate system, 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; When the right foot touches the ground, use the right ankle as the origin to establish the basic coordinate system of the human body. The method of establishing the coordinate system of the leg joints is the same as above; At the same time, the pelvic coordinate system is used as the reference coordinate system of the upper body torso. When the human body is stationary, the first rotation matrix between the upper body torso inertial measurement unit coordinate system and the pelvic coordinate system is the identity matrix. In 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; In order to obtain the complete posture of the human body in the human body base coordinate system and the position coordinates of each joint; through the third posture relationship between the human base coordinate system and the global coordinate system, the complete posture of the human body and the position of each joint in the global coordinate system are obtained coordinate. The human body safety assessment method in human-machine collaboration according to claim 1, wherein said acquiring the pose of the robot in the global coordinate system specifically comprises: Obtain the coordinates of the robot base in the global coordinate system; Acquiring 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; According to the coordinates of the robot base in the global coordinate system and the second rotation matrix, the pose of the robot in the global coordinate system is determined. The human body safety assessment method in human-machine collaboration according to claim 1, wherein the human body and the robot are determined 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 between the robots and the movement speed of the robots include: According to 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; Selecting the minimum value in the distance set as the distance between the human body and the robot; The movement speed of the robot is determined according to the distance between the human body and the robot. The human body safety assessment method in human-machine collaboration according to claim 1, wherein the safety level of the human body in human-machine collaboration is determined according to the distance between the human body and the robot and the movement speed of the robot, Specifically: When the distance between the human body and the robot is greater than 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 the first safe state; When the distance between the human body and the robot is greater than the first preset distance threshold and the robot speed is greater than or equal to the preset speed threshold, determining that the human body is in the second safe state; When 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; When 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, and the movement speed of the robot is greater than or equal to the preset speed threshold At the time, it is determined that the human body is in the second dangerous state; When the distance between the human body and the robot is less than or equal to the second preset distance threshold and the movement speed of the robot is less than the preset speed threshold, it is determined that the human body is in the second dangerous state; When the distance between the human body and the robot is less than or equal to the second preset distance threshold and the movement speed of the robot is greater than or equal to the preset speed threshold, it is determined that the human body is in the first dangerous state. The human body safety assessment method in human-machine collaboration according to claim 6, wherein the first preset distance threshold is L _max ; the second preset distance threshold is L _max /3; and the preset Speed threshold L _max /1.25; where L _max is the maximum abduction length of the robot. A human body safety assessment system in human-machine collaboration is characterized in that it includes: 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 human body safety assessment system in human-machine collaboration according to claim 8, wherein the first acquisition module specifically comprises: 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, 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 human body safety assessment system in human-machine collaboration according to claim 8, wherein the second acquisition module specifically comprises: 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.

Images