阅读说明：本技术 一种方法、装置、机器人及存储介质 (Method, device, robot and storage medium ) 是由 周步青 程平恒 王建长 于 2021-07-09 设计创作，主要内容包括：本申请实施例公开了一种方法、装置、机器人及存储介质。所述方法包括：获取测量区域各测量点在第一坐标系下的测量点位置；对所述测量点位置进行位置转换,得到各测量点在第二坐标系下的目标测量点位置,其中,所述第二坐标系为机器人空间坐标系；基于所述目标测量点位置,若确定机械臂的关节点不在安全作业范围内,发出报警信号进行安全预警。通过上述方法,实现了机器人在工作时,在检测到障碍物时,可以自动计算出机器人的机械臂的各关节点到各测量点(也就是障碍物,例如障碍物可以为天花板、墙面或地板等)的距离,进而在确定机械臂的各关节点不在安全作业范围内时,可以发出报警信号进行安全预警,进而可以提升对机器人进行有效保护的概率。(The embodiment of the application discloses a method, a device, a robot and a storage medium. The method comprises the following steps: acquiring the position of each measuring point in the measuring area in a measuring point position under a first coordinate system; performing position conversion on the measuring point positions to obtain target measuring point positions of the measuring points in a second coordinate system, wherein the second coordinate system is a robot space coordinate system; and based on the position of the target measuring point, if the joint point of the mechanical arm is determined not to be in the safe operation range, sending an alarm signal to perform safety early warning. By the method, when the robot works and an obstacle is detected, the distance from each joint point of the mechanical arm of the robot to each measuring point (namely the obstacle, such as a ceiling, a wall surface or a floor) can be automatically calculated, and then when the joint points of the mechanical arm are determined not to be in a safe operation range, an alarm signal can be sent out to perform safety early warning, so that the probability of effectively protecting the robot can be improved.)

1. A method for safety precaution applied to a robot, the method comprising:

acquiring the position of each measuring point in the measuring area in a measuring point position under a first coordinate system;

performing position conversion on the measuring point positions to obtain target measuring point positions of the measuring points in a second coordinate system, wherein the second coordinate system is a robot space coordinate system;

and based on the position of the target measuring point, if the joint point of the mechanical arm is determined not to be in the safe operation range, sending an alarm signal to perform safety early warning.

2. The method according to claim 1, wherein the measurement point position comprises a first measurement point position and a second measurement point position, and the measurement area comprises a first measurement area and a second measurement area, wherein the first measurement point position is coordinates of each measurement point of the first measurement area obtained after filtering, and the second measurement point position is coordinates of each measurement point of the second measurement area obtained after filtering; the acquiring of the measurement point position of each measurement point in the measurement area in the first coordinate system includes:

collecting measurement data of each measurement point of the first measurement area in a first coordinate system;

filtering the measurement data through a preset filtering algorithm to obtain the position of the measurement point corresponding to each measurement point in the first measurement area after filtering;

and taking the position of the measuring point corresponding to each measuring point in the first measuring area after filtering as the position of the first measuring point.

3. The method according to claim 2, wherein the filtering the measurement data by a preset filtering algorithm to obtain the filtered measurement point positions corresponding to the measurement points in the first measurement region comprises:

acquiring the running state of the robot;

if the robot is in a motion state, filtering the measurement data through a first preset filtering algorithm to obtain the position of the measurement point corresponding to each measurement point in the first measurement area after filtering;

and if the robot is in a static state, filtering the measurement data through a second preset filtering algorithm to obtain the position of the measurement point corresponding to each measurement point in the first measurement area after filtering, wherein the first preset filtering algorithm has stronger dynamic response than the second preset filtering algorithm.

4. The method of claim 2, wherein the first and second measurement regions are different planes and are connected to each other; the acquiring of the measurement point position of each measurement point in the measurement area in the first coordinate system includes:

collecting the measurement data of each measurement point of the second measurement area in a first coordinate system;

filtering the measurement data through a third preset filtering algorithm to obtain the position of the measurement point corresponding to each measurement point in the second measurement area after filtering;

and taking the position of the measuring point corresponding to each measuring point in the second measuring area after filtering as the position of the second measuring point.

5. The method of claim 1, wherein the obtaining the measurement point position of each measurement point in the measurement area in the first coordinate system further comprises:

modeling the pose transformation of each joint point of the mechanical arm, and establishing a pose transformation matrix of each connecting rod between each joint point by using a D-H representation method;

obtaining a pose transformation matrix of each joint point of the mechanical arm relative to an original point based on the pose transformation matrix of each connecting rod;

and obtaining the space coordinates of each joint point of the mechanical arm in the motion process based on the pose transformation matrix of each joint point relative to the original point.

6. The method of claim 5, wherein the sending out a warning signal for a safety precaution if it is determined that the joint point of the robot arm is not within the safe working range based on the target coordinates comprises:

determining the distance between the joint point of the mechanical arm and each measuring point based on the position of the target measuring point and the space coordinate;

determining whether a joint point of the robot arm is within a safe working range based on the distance;

and if the joint point of the mechanical arm is not in the safe operation range, sending an alarm signal to perform safety early warning.

7. The method of claim 6, wherein said issuing a warning signal for a safety precaution if the joint point of the robotic arm is not within a safe operating range comprises:

comparing the distance with a preset distance;

and if the distance is smaller than the preset distance, determining that the joint point of the mechanical arm is not in the safe operation range, and sending an alarm signal to perform safety early warning.

8. An apparatus for safety precaution applied to a robot, the apparatus comprising:

the position acquisition unit is used for acquiring the position of each measuring point in the measuring area in a first coordinate system;

the position conversion unit is used for carrying out position conversion on the measuring point positions to obtain target measuring point positions of the measuring points in a second coordinate system, wherein the second coordinate system is a robot space coordinate system;

and the early warning unit is used for sending out a warning signal to carry out safety early warning if the joint point of the mechanical arm is determined not to be in the safety operation range based on the position of the target measuring point.

9. A robot comprising one or more processors and memory; one or more programs stored in the memory and configured to be executed by the one or more processors to perform the method of any of claims 1-7.

10. A computer-readable storage medium, having program code stored therein, wherein the program code when executed by a processor performs the method of any of claims 1-7.

Technical Field

The application belongs to the technical field of robots, and particularly relates to a method, a device, a robot and a storage medium.

Background

The mobile robot technology is one of representative strategic technologies in the high-tech field, and the multi-degree-of-freedom mechanical arm has a large reachable range and strong load capacity, plays an important role in production and life, and is widely applied. The kinematics problem of the mechanical arm is the basis of the application of the industrial robot, is the premise of mechanical arm track planning and real-time control, and can be programmed and controlled only by determining the mapping relation between the space pose of the mechanical arm end effector and the mechanical arm joint variables.

In the research on the related safety early warning method, the distance between each joint point of the mechanical arm and the measuring area is not judgeable, and the safety protection of the robot is lacked.

Disclosure of Invention

In view of the above, the present application proposes a method, an apparatus, a robot, and a storage medium to achieve an improvement of the above problem.

In a first aspect, an embodiment of the present application provides a method, which is applied to safety precaution of a robot, and the method includes: acquiring the position of each measuring point in the measuring area in a measuring point position under a first coordinate system; performing position conversion on the measuring point positions to obtain target measuring point positions of the measuring points in a second coordinate system, wherein the second coordinate system is a robot space coordinate system; and based on the position of the target measuring point, if the joint point of the mechanical arm is determined not to be in the safe operation range, sending an alarm signal to perform safety early warning.

In a second aspect, an embodiment of the present application provides an apparatus, which operates in a safety precaution of a robot, and the apparatus includes: the position acquisition unit is used for acquiring the position of each measuring point in the measuring area in a first coordinate system; the position conversion unit is used for carrying out position conversion on the measuring point positions to obtain target measuring point positions of the measuring points in a second coordinate system, wherein the second coordinate system is a robot space coordinate system; and the early warning unit is used for sending out a warning signal to carry out safety early warning if the joint point of the mechanical arm is determined not to be in the safety operation range based on the position of the target measuring point.

In a third aspect, embodiments of the present application provide a robot, including one or more processors and a memory; one or more programs, wherein the one or more programs are stored in the memory and configured to be executed by the one or more processors, the one or more programs configured to perform the methods described above.

In a fourth aspect, the present application provides a computer-readable storage medium, in which a program code is stored, wherein the program code performs the above-mentioned method when running.

The embodiment of the application provides a method, a device, a robot and a storage medium. Firstly, measuring point positions of measuring points in a measuring area under a first coordinate system are obtained, then position conversion is carried out on the measuring point positions, target measuring point positions of the measuring points under a second coordinate system are obtained, wherein the second coordinate system is a robot space coordinate system, and finally, based on the target measuring point positions, if the joint points of the mechanical arm are determined not to be in a safe operation range, an alarm signal is sent out to carry out safety early warning. By the method, when the robot works and an obstacle is detected, the distance from each joint point of the mechanical arm of the robot to each measuring point (namely the obstacle, such as a ceiling, a wall surface or a floor) can be automatically calculated, and then when the joint points of the mechanical arm are determined not to be in a safe operation range, an alarm signal can be sent out to perform safety early warning, so that the probability of effectively protecting the robot can be improved.

Drawings

In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings needed to be used in the description of the embodiments are briefly introduced below, and it is obvious that the drawings in the following description are only some embodiments of the present application, and it is obvious for those skilled in the art to obtain other drawings based on these drawings without creative efforts.

FIG. 1 illustrates a flow chart of a method proposed by an embodiment of the present application;

FIG. 2 illustrates a flow chart of a method according to another embodiment of the present application;

FIG. 3 shows a schematic view of a first measurement region and a second measurement region as proposed by another embodiment of the present application;

FIG. 4 illustrates a flow chart of a method according to yet another embodiment of the present application;

fig. 5 is a schematic structural diagram illustrating a robot arm of a robot according to still another embodiment of the present disclosure;

fig. 6 is a block diagram illustrating a structure of an apparatus according to an embodiment of the present application;

fig. 7 is a block diagram showing another apparatus according to an embodiment of the present application;

FIG. 8 shows a block diagram of a robot in real-time for performing a method according to an embodiment of the present application;

fig. 9 illustrates a storage unit for storing or carrying program code implementing a method according to an embodiment of the present application in real time.

Detailed Description

The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application, and it is obvious that the described embodiments are only a part of the embodiments of the present application, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present application.

Robotic arms, also known as robotic arms, manipulators, and the like, can mimic certain motion functions of human hands and arms for use in robotic manipulators that grasp, transport objects or manipulate tools in a fixed sequence. Therefore, the safety of the robot arm of the robot is of great importance in current factory and other application scenarios. In the related safety early warning method, the safety protection of the robot is lacked.

Therefore, the inventor proposes that in the present application, first, the measurement point position of each measurement point in a measurement area in a first coordinate system is obtained, then, the measurement point position is subjected to position conversion, so as to obtain the target measurement point position of each measurement point in a second coordinate system, wherein the second coordinate system is a robot space coordinate system, and finally, based on the target measurement point position, if it is determined that the joint point of the mechanical arm is not within the safe operation range, an alarm signal is sent out to perform a safety early warning. By the method, when the robot works and an obstacle is detected, the distance from each joint point of the mechanical arm of the robot to each measuring point (namely the obstacle, such as a ceiling, a wall surface or a floor) can be automatically calculated, and when the joint points of the mechanical arm are determined to be out of a safe operation range, an alarm signal can be sent out to perform safety early warning, so that the probability of effectively protecting the robot can be improved.

Embodiments of the present application will be described in detail below with reference to the accompanying drawings.

Referring to fig. 1, a method provided in the embodiment of the present application is applied to safety pre-warning of a robot, and the method includes:

step S110: and acquiring the position of each measuring point in the measuring area in the first coordinate system.

In the embodiment of the present application, the first coordinate system is a spatial rectangular coordinate system; each measuring point is a point in the measuring area which is measured when the distance measuring device arranged on the robot measures the distance between the robot and the measuring area when the distance measuring device detects the existence of the measuring area; the measurement point position may be represented by coordinates of the measurement point.

As one mode, the coordinates of each measurement point in the measurement area in the space rectangular coordinate system may be acquired by a distance measuring device mounted on the robot. The distance measuring device can be a distance measuring sensor, a laser distance measuring device, an ultrasonic distance measuring device, a wavelet distance measuring device, an infrared distance measuring device and the like, and is not specifically limited in the embodiment of the application.

Specifically, when coordinates of each measurement point in the measurement area under the rectangular space coordinate system are acquired by a distance measuring device mounted on the robot, a distance measuring signal can be sent to the measurement area at regular time by the distance measuring device, and when a signal returned by the measurement area based on the distance measuring signal is received, the position of the measurement point can be determined by the signal.

The position of the measurement point in the first coordinate system of each measurement point may be acquired in real time, or may be acquired in advance and stored in a storage area of the robot. If the measuring points in the first coordinate system are previously acquired and stored in the storage area of the robot, the robot can directly read the measuring point positions of all the stored measuring points from the storage area.

Optionally, the measurement point position of each measurement point in the first coordinate system is a position coordinate of each measurement point in the first coordinate system obtained after the filtering operation is performed.

Step S120: and performing position conversion on the measuring point position to obtain the target measuring point position of each measuring point in a second coordinate system, wherein the second coordinate system is a robot space coordinate system.

In the embodiment of the present application, the position conversion of the measurement point position may be understood as converting the coordinates of the measurement point in the rectangular spatial coordinate system into the coordinates of the measurement point in the spatial coordinate system of the robot.

Specifically, a mapping relationship between the first coordinate system and the second coordinate system may be established in advance, and further, coordinate conversion may be performed on the measurement point position in the first coordinate system or the measurement point position in the second coordinate system through the mapping relationship between the two coordinate systems.

Step S130: and based on the position of the target measuring point, if the joint point of the mechanical arm is determined not to be in the safe operation range, sending an alarm signal to perform safety early warning.

In one aspect, the safe working range is a preset distance range in which the robot can safely perform a work. After the measuring point positions of the measuring points are obtained by the method, the distance between the measuring point positions of the measuring points and the coordinates of the joint points of the mechanical arm of the robot can be calculated according to the measuring point positions of the measuring points and the coordinates of the joint points, and therefore the situation that the joint points of the mechanical arm are out of the safe operation range can be determined. When the joint point of the mechanical arm is determined not to be in the safe operation range, the robot can send out an alarm signal to perform safe early warning. Specifically, safety early warning can be performed through a buzzer or a display screen installed on the robot.

The method comprises the steps of firstly obtaining the measuring point position of each measuring point in a measuring area under a first coordinate system, then carrying out position conversion on the measuring point position to obtain the target measuring point position of each measuring point under a second coordinate system, wherein the second coordinate system is a robot space coordinate system, and finally sending an alarm signal to carry out safety early warning if the joint point of a mechanical arm is determined not to be in a safety operation range based on the target measuring point position. By the method, when the robot works and an obstacle is detected, the distance from each joint point of the mechanical arm of the robot to each measuring point (namely the obstacle, for example, the obstacle can be a ceiling, a wall surface or a floor) can be automatically calculated, and then when the joint points of the mechanical arm are determined not to be in a safe operation range, an alarm signal can be sent out to perform safety early warning, so that the probability of effectively protecting the robot can be improved, wherein the obstacle can be in various forms, such as a ceiling, a wall surface or a ground surface.

Referring to fig. 2, a method provided in the embodiment of the present application is applied to safety pre-warning of a robot, and the method includes:

step S210: and acquiring measurement data of each measurement point of the first measurement area in a first coordinate system.

In this application embodiment, the measurement point position may include a first measurement point position and a second measurement point position, the measurement area includes a first measurement area and a second measurement area, where the first measurement point position is a coordinate of each measurement point of the first measurement area obtained after filtering, the second measurement point position is a coordinate of each measurement point of the second measurement area obtained after filtering, and the first measurement area and the second measurement area are different planes and are connected to each other. As shown in fig. 3, 301 in fig. 3 may be a first measurement region, and 302 may be a second measurement region, and of course, 301 may also be a second measurement region, and 302 may be a first measurement region, and the connection manner of 301 and 302 is shown in fig. 3, which is not limited in detail herein.

As one mode, the distance measuring device may include a first distance measuring device and a second distance measuring device, and then the measurement data of each measurement point of different measurement areas may be collected by different distance measuring devices. Specifically, the measurement data of each measurement point in the first measurement area in the first coordinate system may be acquired by the first distance measuring device. Optionally, the first distance measuring device may be a laser radar, and for example, the laser radar may be a laser radar of which the model is N301-03A, the effective measurement distance is 200mm-30000mm, the communication protocol is UDP communication protocol, the measurement accuracy is ± 30mm, and the light source is a 905nm laser. When the laser radar is used for collecting the measurement data of each measurement point in the first measurement area under the first coordinate system, signals returned by each measurement point in the first measurement area with the level of 360 degrees can be read through the laser radar, and then the measurement data of each measurement point can be obtained according to the signals returned by each measurement point. In the embodiment of the application, the measurement data is the position of the measurement point of each measurement point which is not operated by the filtered wave.

Step S220: and carrying out filtering operation on the measurement data through a preset filtering algorithm to obtain the position of the measurement point corresponding to each measurement point in the first measurement area after filtering.

In the embodiment of the present application, since the measurement data is not subjected to the filtering operation, the measurement data may have some noise, and then the filtering operation needs to be performed on the noise in the measurement data, so that the measurement error of the measurement data can be reduced through the filtering operation.

Specifically, filtering operation can be performed on the acquired measurement data of each measurement point in the first coordinate system through a preset filtering algorithm, so that a measurement point position corresponding to each measurement point in the first measurement area with a smaller measurement error can be obtained.

As a mode, the step of performing a filtering operation on the measurement data through a preset filtering algorithm to obtain the filtered measurement point positions corresponding to the measurement points in the first measurement area includes: acquiring the running state of the robot; if the robot is in a motion state, filtering the measurement data through a first preset filtering algorithm to obtain the position of the measurement point corresponding to each measurement point in the first measurement area after filtering; and if the robot is in a static state, filtering the measurement data through a second preset filtering algorithm to obtain the position of the measurement point corresponding to each measurement point in the first measurement area after filtering, wherein the first preset filtering algorithm has stronger dynamic response than the second preset filtering algorithm.

The data index for evaluating the dynamic responsiveness may include a maximum overshoot, a response time, a steady-state error, and the like. In the embodiment of the application, both the first preset filtering algorithm and the second preset filtering algorithm can be applied to the robot motion process, but when the first preset filtering algorithm and the second preset filtering algorithm are applied to the robot motion process, the dynamic response of the first preset filtering algorithm is better than that of the second preset filtering algorithm. The dynamic responsiveness of the first preset filtering algorithm is better than the dynamic responsiveness of the second preset filtering algorithm, which may mean that the first preset filtering algorithm has a faster response speed to the measurement data acquired by the robot during the movement process, or the maximum overshoot of the first preset filtering algorithm is smaller than the maximum overshoot of the second preset filtering algorithm, or the steady-state error of the first preset filtering algorithm is smaller than the steady-state error of the second preset filtering algorithm.

Because the noise data contained in the measurement data collected by the distance measuring device are different between the moving operation process and the static operation process of the robot, different filtering algorithms can be selected to filter the measurement data according to the operation state of the robot.

Specifically, the first preset filtering algorithm may be a Savitzky-Golay convolution smoothing filtering algorithm; the second predetermined filtering algorithm may be a one-dimensional kalman filtering algorithm.

The operation state of the robot can be acquired in real time by an Inertial Measurement Unit (IMU) installed in the robot. When the IMU determines that the robot is in a motion state, after the robot acquires measurement data of each measurement point of a first measurement area in a first coordinate system through a laser radar, filtering operation can be performed on the measurement data through a Savitzky-Golay convolution smoothing filtering algorithm. When the IMU determines that the robot is in a static state, after the robot acquires measurement data of each measurement point of a first measurement area in a first coordinate system through a laser radar, the measurement data can be filtered through a one-dimensional Kalman filtering algorithm.

The laser radar can directly measure the polar diameter rho from 0.0 degree to 360 degrees_k∈{ρ₀，ρ_0.1，ρ_0.2，...，ρ₃₆₀}。

When selecting to perform filtering operation on the measurement data through the Savitzky-Golay convolution smoothing filtering algorithm, the specific filtering process is as follows:

firstly, establishing a motion model when the robot moves, and filtering measurement data through the motion model, wherein the motion model is as follows: m (i) ═ a₀+a₁i+a₂i²。

After the motion model is established, 5 pieces of measurement data before and after measurement at a certain angle k returned by the laser radar are selected, wherein the measurement data are respectively rho_k(-2)、ρ_k(-1)、ρ_k(0)、ρ_k(1)、ρ_k(2) (ii) a The model after smooth filtering is as follows:

then the model is replaced by an operation matrix,

namely:to approximate it to the true value, let itInfinite equal toNamely, it isIs provided with T × a ═ P, one can deduce: therefore m_k(0) Is the filtered data.

In this embodiment of the application, for the measurement data obtained when each robot is in a motion state, the measurement data may be filtered through the above process, so as to obtain the measurement point position corresponding to each measurement point in the filtered first measurement area.

And when the robot is in a static state, the one-dimensional Kalman filtering algorithm is adopted to carry out filtering operation on the measured data. Specifically, when the robot is in a static state, the laser radar only has one set of measurement data and no motion data (such as acceleration, process data and the like), so a one-dimensional Kalman filtering algorithm is adopted to perform filtering operation on the measurement data.

When the measurement data is filtered by the one-dimensional kalman filtering algorithm, the specific filtering process is as follows:

the state quantity is rho of the laser radar measuring distance_k(ii) a The observed quantity is also ρ_k；

1. A priori estimation formula:wherein F is a state transition matrix,which is a priori estimated value at time t, is the intermediate calculation result of the filtering,the optimal estimated value at the moment of t-1 is one of the filtering results, namely the updated result; b is a control matrix, a matrix for converting an input into a state, u_t-1Control quantity, w, for the state at time t-1_kIs process noise; being a one-dimensional kalman filter, B ═ 1]，F＝[1]，w_k＝0，u_t-1After simplification, 0:

2. prior estimate covariance formula:where Q is the covariance of the process noise, P_t-1The covariance of the best result, which is at time t-1, is one of the results of the filtering,is the prior estimation covariance at time t, F is the state transition matrix, after simplification:

3. establishing a measurement equation: z_t＝H*x_t+ v; wherein Z is_tFor the measured value at time t, H is a transformation matrix of the state variables into measures (observations), representing the relation connecting the states and observations, one of the preconditions for filtering, x_tV is measurement noise and follows Gaussian distribution, and H is [1 ] due to a one-dimensional Kalman filter](ii) a Measured value of the lidar is rho_kAlready containing the measurement noise v, after simplification: z_t＝ρ_k。

4. Updating the Kalman gain:wherein R is the covariance of the observed noise, K_tThe Kalman gain is an intermediate calculation result of filtering, also called Kalman coefficient, and is simplified as follows:

5. and (3) correction estimation:wherein the content of the first and second substances,for an optimum estimate of time t, z_tIs the measurement at time t, i.e. is p_kFiltered value

6. Updating the posterior covariance:wherein, P_tAnd (3) simplifying the covariance as the optimal result covariance at the moment t and the unit matrix I:adjusting the hyper-parameters in the algorithm: and after field adjustment, Q is 0 and R is 0.01 in the filtering process.

Step S230: and taking the position of the measuring point corresponding to each measuring point in the first measuring area after filtering as the position of the first measuring point.

As one mode, the filtering result m obtained after the filtering operation is performed by the two filtering algorithms_kAs the first measurement point position.

Step S240: and collecting the measurement data of each measurement point of the second measurement area in a first coordinate system.

In this embodiment, the first measurement area and the second measurement area are different planes and are connected to each other, and measurement data of each measurement point in the second measurement area in the first coordinate system may be acquired by a second distance measuring device. Optionally, the second distance measuring device may be a laser displacement sensor, which may be, for example, a sensor with a model number LR-TB5000 and an effective measuring distance of 60mm to 5000 mm. Optionally, the response time of the laser displacement sensor may be set to 100ms, and the corresponding repetition schedule is shown in table 1:

TABLE 1

Detecting distance

60mm

200mm

1000mm

2000mm

3000mm

5000mm

White paper reflectivity (90%)

±3

±3

±3

±3

±3

±5

Grey paper reflectivity (18%)

±6

±3

±3

±5

±6

±14

Step S250: and carrying out filtering operation on the measurement data through a third preset filtering algorithm to obtain the position of the measurement point corresponding to each measurement point in the second measurement area after filtering.

In an embodiment of the present application, the third predetermined filtering algorithm may be an anti-jitter filter algorithm. Because the laser displacement sensor detects that the measurement data of each measurement point in the second measurement area has repeated errors in real time, in order to reduce the errors, a jitter elimination filter algorithm can be designed to eliminate fluctuation of the measurement data acquired by the laser displacement sensor. Specifically, after the measurement data of each measurement point in the second measurement area in the first coordinate system is acquired through the second distance measuring device, the measurement data may be filtered through a jitter elimination filter algorithm. The filtering operation may be performed on the measurement data by the following formula:

wherein x is_kData collected for the kth laser displacement sensor; p is a counter which mainly counts fluctuation data; and z is the position of the measurement point after the jitter is eliminated.

Step S260: and taking the position of the measuring point corresponding to each measuring point in the second measuring area after filtering as the position of the second measuring point.

As one mode, a filtering result z obtained after the filtering operation is performed by the jitter elimination filter algorithm is used as the second measurement point position.

Step S270: and performing position conversion on the measuring point position to obtain the target measuring point position of each measuring point in a second coordinate system, wherein the second coordinate system is a robot space coordinate system.

As one mode, firstly, the position of the first measurement point obtained by performing the filtering operation through the two filtering algorithms is converted into a rectangular coordinate:where k is ∈ {0, 0.1, 0.2.., 360 }.

The laser radar reads the position coordinates (x) of the first measuring point in the first measuring area after filtering_k，y_k) In the embodiment of the present application, since the lidar is mounted on the robot and the robot arm of the robot is also mounted on the robot, it can be known from the robot design structure diagram that the coordinate of the lidar in the robot space coordinate system where the robot arm of the robot is located is (x)_k+l_x，y_k+l_y) In the same way, the coordinate of the laser sensor in the robot space coordinate system where the mechanical arm of the robot is located is (z + l)_z) The origin of coordinates (X) of a robot space coordinate system in which the robot arm is located can also be known₀，Y₀，Z₀) Height to ground of l_ground。

Step S280: and based on the position of the target measuring point, if the joint point of the mechanical arm is determined not to be in the safe operation range, sending an alarm signal to perform safety early warning.

Step S280 may specifically refer to the detailed explanation in the above embodiments, which is not described herein in detail in this embodiment.

The method comprises collecting measurement data of a first measurement region under a first coordinate system, filtering the measurement data by a preset wave algorithm to obtain measurement point positions corresponding to the measurement points of the first measurement region, taking the measurement point positions corresponding to the measurement points of the first measurement region as the first measurement point positions, collecting measurement data of the measurement points of a second measurement region under the first coordinate system, filtering the measurement data by a third preset filter algorithm to obtain measurement point positions corresponding to the measurement points of the second measurement region, taking the measurement point positions corresponding to the measurement points of the second measurement region as the second measurement point positions, converting the measurement point positions to obtain target measurement point positions of the measurement points under the second coordinate system, and finally, if the joint point of the mechanical arm is determined not to be in the safe operation range based on the position of the target measurement point, an alarm signal is sent out to carry out safety early warning. By the method, the measurement data are filtered through different filtering algorithms, the measurement error of the measurement data can be reduced, and moreover, when the robot works and detects an obstacle, the distance from each joint point of a mechanical arm of the robot to each measurement point (namely the obstacle, such as a ceiling, a wall surface or a floor) can be automatically calculated, and further when the joint points of the mechanical arm are determined not to be in a safe operation range, an alarm signal can be sent out to perform safety early warning, so that the probability of effectively protecting the robot can be improved.

Referring to fig. 4, a method provided in the embodiment of the present application is applied to safety pre-warning of a robot, and the method includes:

step S310: and modeling the pose transformation of each joint point of the mechanical arm, and establishing a pose transformation matrix of each connecting rod among the joint points by using a D-H representation method.

In the embodiment of the application, a robot space coordinate system is firstly established according to the positions of all joint points of the robot mechanical arm, as shown in fig. 5, (X) in fig. 5₀，Y₀，Z₀) Is the origin of coordinates of the robot coordinate system. X in the figure₁-Y₁-Z₁、X₂-Y₂-Z₂、X₃-Y₃-Z₃、X₄-Y₄-Z₄、X₅-Y₅-Z₅、X₆-Y₆-Z₆And X_Tcp-Y_Tcp-Z_TcpThe coordinate systems are respectively corresponding to joint point 1, joint point 2, joint point 3, joint point 4, joint point 5, joint point 6 and joint point 7 of the mechanical arm.

As one way, before establishing the pose transformation matrix of each link between each joint point by using the D-H representation method, a D-H table may be established according to the structure of the robot arm of the robot as shown in fig. 5, as shown in table 2:

TABLE 2

AXIS numbering	αi-1Connecting rod torsion angle	αi-1Length of connecting rod	diDistance of joint	θiJoint angle
					1	0	0	d1	θ1
2	-90°	0	d2	θ2
					3	0	α2	0	θ3
4	0	α3	-d4	θ4
					5	90°	0	d5	θ5
6	-90°	0	d6	θ6

And then, a central change matrix of the AXIS of each AXIS is calculated according to the D-H table, the space coordinate system of the robot and Z-Y-Z Euler Angles (Euler rotations).

Specifically, first, from X-Y-Z Fixed Angles (change of rotation matrix of the rotation axis to the robot space coordinate system (Fixed)) and the D-H table, the following can be obtained:

is a coordinate system wound aroundRotation (alpha)_i-1) An angle change matrix; is a coordinate system extensionMovement (alpha)_i-1) A change matrix of (a); is a coordinate system wound aroundRotation (theta)_i) An angle change matrix; is a coordinate system extensionMoving (d)_i) An angle change matrix.

Step S320: and obtaining a pose transformation matrix of each joint point of the mechanical arm relative to the original point based on the pose transformation matrix of each connecting rod.

After the pose transformation matrix of each connecting rod is determined by the method, the matrix formula and the Z-Y-Z Euler Angles (Euler rotations) principle are transformed according to the Craig method:

then, according to the Rotation matrix, the decomposition and derivation are carried outA formula; the pose transformation matrix between two adjacent joint points can be obtained through the carry-in calculation:

according to the rotation of the secondary axis of the mechanical arm under the rotation basis of the front axis, the secondary axis rotates, and the coordinate origin (X) of each joint point of the mechanical arm relative to the robot coordinate system can be obtained by adopting the Z-Y-Z Euler Angles Euler angular coordinate system conversion₀，Y₀，Z₀) The specific formula algorithm of the pose transformation matrix is as follows:

step S330: and obtaining the space coordinates of each joint point of the mechanical arm in the motion process based on the pose transformation matrix of each joint point relative to the original point.

In the embodiment of the application, after the pose change matrix of each joint point relative to the dots is obtained, the change matrix of each axis coordinate origin (also called as an axis joint point) can be respectively calculated by substituting the parameters in the D-H table into the calculation through the computer, that is, the coordinate origin (X) of each axis coordinate origin in the robot coordinate system can be obtained₀，Y₀，Z₀) Lower spatial rectangular coordinate (x)_i′，y_i′，z_i') is (x)₁′，y₁′，z₁′)，(x₂′，y₂′，z₂′)，(x₃′，y₃′，z₃′)，(x₄′，y₄′，z₄′)，(x₅′，y₅′，z₅′)，(x₆′，y₆′，z₆′)，(x′_Tcp，y′_Tcp，z′_Tcp)。

Step S340: and acquiring the position of each measuring point in the measuring area in the first coordinate system.

Step S340 may specifically refer to the detailed explanation in the foregoing embodiments, which are not described herein in detail in this embodiment.

Step S350: and performing position conversion on the measuring point position to obtain the target measuring point position of each measuring point in a second coordinate system, wherein the second coordinate system is a robot space coordinate system.

Step S350 may specifically refer to the detailed explanation in the foregoing embodiments, which are not described herein in detail in this application embodiment.

Step S360: and determining the distance between the joint point of the mechanical arm and each measuring point based on the position of the target measuring point and the space coordinate.

In the embodiment of the present application, the distance between two points may be calculated according to the coordinates of the two points. When the robot enters into operation, the mechanical arm program calculates the shortest distance between each joint point and the first measurement area and the second measurement areai is each axis joint point of the mechanical arm. Wherein the first measuring area may be a horizontal wall surface and the second measuring area may be a floor surface and a ceiling surface. The specific calculation formula is as follows:

wherein x is_kAnd y_kRespectively measuring point coordinates of each measuring point in a first measuring area measured by the laser radar in {0, 0.1, 0.2.. multidot.360 } degrees; z is the distance between the robot arm and the second measuring area measured by the laser displacement sensor, l_x、l_yAnd l_zRespectively are the coordinates of the laser radar and the laser displacement sensor under the robot coordinate system.

Step S370: determining whether a joint point of the robot arm is within a safe working range based on the distance.

As one mode, the step of determining whether the joint point of the robot arm is within the safe working range based on the distance includes: comparing the distance with a preset distance; and if the distance is smaller than the preset distance, determining that the joint point of the mechanical arm is not in the safe operation range, and sending an alarm signal to perform safety early warning.

The preset distance is a preset distance for safe operation of the robot. For example, the preset distance may be set as a safe distance h, and when the robot obtains the shortest distance between each joint point of the robot and the first measurement area and the second measurement area, the robot program will determine that the robot is in the shortest distanceIf the distances between each joint point of the mechanical arm of the robot and the first measuring area and the distance between each joint point of the mechanical arm of the robot and the second measuring area are both greater than the safety distance h, the robot is in a safety distance working area, otherwise, the robot is not in the safety distance working area, and the robot can send out an alarm signal to perform safety early warning.

Step S380: and if the joint point of the mechanical arm is not in the safe operation range, sending an alarm signal to perform safety early warning.

Step S380 may specifically refer to the detailed explanation in the foregoing embodiments, and is not described herein in detail in this embodiment.

The method comprises modeling pose transformation of each joint point of a mechanical arm, establishing a pose transformation matrix of each connecting rod between each joint point by using a D-H expression method, obtaining the pose transformation matrix of each joint point of the mechanical arm relative to an original point based on the pose transformation matrix of each connecting rod, obtaining space coordinates of each joint point of the mechanical arm in the motion process based on the pose transformation matrix of each joint point relative to the original point, obtaining the measurement point position of each measurement point in a measurement area under a first coordinate system, performing position conversion on the measurement point position to obtain the target measurement point position of each measurement point under a second coordinate system, wherein the second coordinate system is a robot space coordinate system, and finally determining the distance between each joint point of the mechanical arm and each measurement point based on the target measurement point position and the space coordinates, and determining whether the joint point of the mechanical arm is in a safe operation range or not based on the distance, and if not, sending an alarm signal to perform safety early warning. By the method, when the robot works and an obstacle is detected, the distance from each joint point of the mechanical arm of the robot to each measuring point (namely the obstacle, such as a ceiling, a wall surface or a floor) can be automatically calculated, and then when the joint points of the mechanical arm are determined not to be in a safe operation range, an alarm signal can be sent out to perform safety early warning, so that the probability of effectively protecting the robot can be improved.

Referring to fig. 6, a device 400 provided in the present embodiment is applied to safety precaution of a robot, where the device 400 includes:

a position obtaining unit 410, configured to obtain positions of measurement points of the measurement area in the first coordinate system.

As a mode, the measurement point position includes a first measurement point position and a second measurement point position, and the measurement area includes a first measurement area and a second measurement area, where the first measurement point position is coordinates of each measurement point in the first measurement area obtained after filtering, and the second measurement point position is coordinates of each measurement point in the second measurement area obtained after filtering; the position obtaining unit 410 is configured to collect measurement data of each measurement point in the first measurement area in a first coordinate system; filtering the measurement data through a preset filtering algorithm to obtain the position of the measurement point corresponding to each measurement point in the first measurement area after filtering; and taking the position of the measuring point corresponding to each measuring point in the first measuring area after filtering as the position of the first measuring point.

Further, the position obtaining unit 410 is further configured to obtain an operation state of the robot; if the robot is in a motion state, filtering the measurement data through a first preset filtering algorithm to obtain the position of the measurement point corresponding to each measurement point in the first measurement area after filtering; and if the robot is in a static state, filtering the measurement data through a second preset filtering algorithm to obtain the position of the measurement point corresponding to each measurement point in the first measurement area after filtering, wherein the first preset filtering algorithm has stronger dynamic response than the second preset filtering algorithm.

As another mode, the first measurement area and the second measurement area are different planes and are connected to each other, and the position obtaining unit 410 is configured to collect measurement data of each measurement point of the second measurement area in a first coordinate system; filtering the measurement data through a third preset filtering algorithm to obtain the position of the measurement point corresponding to each measurement point in the second measurement area after filtering; and taking the position of the measuring point corresponding to each measuring point in the second measuring area after filtering as the position of the second measuring point.

And a position conversion unit 420, configured to perform position conversion on the measurement point positions to obtain target measurement point positions of the measurement points in a second coordinate system, where the second coordinate system is a robot space coordinate system.

And the early warning unit 430 is configured to send out an alarm signal to perform a safety early warning if it is determined that the joint point of the robot arm is not within the safe operation range based on the target measurement point position.

As a way, the early warning unit 430 is configured to determine distances between joint points of the robot arm and the measurement points based on the target measurement point position and the space coordinates; determining whether a joint point of the robot arm is within a safe working range based on the distance; and if the joint point of the mechanical arm is not in the safe operation range, sending an alarm signal to perform safety early warning.

Further, the early warning unit 430 is further configured to compare the distance with a preset distance; and if the distance is smaller than the preset distance, determining that the joint point of the mechanical arm is not in the safe operation range, and sending an alarm signal to perform safety early warning.

Referring to fig. 7, the apparatus 400 further includes:

the first matrix obtaining unit 440 is configured to model a pose transformation of each joint point of the mechanical arm, and establish a pose transformation matrix of each link between the joint points by using a D-H representation.

A second matrix obtaining unit 450, configured to obtain a pose transformation matrix of each joint point of the mechanical arm relative to an origin based on the pose transformation matrix of each link;

and a coordinate obtaining unit 460, configured to obtain a spatial coordinate of each joint point of the mechanical arm in the motion process based on a pose transformation matrix of each joint point relative to the origin.

It should be noted that the device embodiment and the method embodiment in the present application correspond to each other, and specific principles in the device embodiment may refer to the contents in the method embodiment, which is not described herein again.

A robot provided by the present application will be described with reference to fig. 8.

Referring to fig. 8, based on the above method and apparatus, another robot 800 capable of performing the method is provided in the embodiment of the present application. The robot 800 includes one or more processors 802 (only one shown), a memory 804, and a network module 806 coupled to each other. The memory 804 stores programs that can execute the content of the foregoing embodiments, and the processor 802 can execute the programs stored in the memory 804.

Processor 802 may include one or more processing cores, among others. The processor 802 interfaces with various components throughout the robot 800 using various interfaces and lines to perform various functions of the robot 800 and process data by executing or executing instructions, programs, code sets, or instruction sets stored in the memory 804 and invoking data stored in the memory 804. Alternatively, the processor 802 may be implemented in hardware using at least one of Digital Signal Processing (DSP), Field-Programmable Gate Array (FPGA), and Programmable Logic Array (PLA). The processor 802 may integrate one or more of a Central Processing Unit (CPU), a Graphics Processing Unit (GPU), a modem, and the like. Wherein, the CPU mainly processes an operating system, a user interface, an application program and the like; the GPU is used for rendering and drawing display content; the modem is used to handle wireless communications. It is understood that the modem may not be integrated into the processor 802, but may be implemented by a single communication chip.

The Memory 804 may include a Random Access Memory (RAM) or a Read-Only Memory (Read-Only Memory). The memory 804 may be used to store instructions, programs, code, sets of codes, or sets of instructions. The memory 804 may include a stored program area and a stored data area, wherein the stored program area may store instructions for implementing an operating system, instructions for implementing at least one function (such as a touch function, a sound playing function, an image playing function, etc.), instructions for implementing various method embodiments described below, and the like. The stored data area may also store data created by the terminal 800 during use (e.g., phone books, audio-visual data, chat log data), and the like.

The network module 806 is configured to receive and transmit electromagnetic waves, and achieve interconversion between the electromagnetic waves and the electrical signals, so as to communicate with a communication network or other devices, for example, an audio playing device. The network module 806 may include various existing circuit elements for performing these functions, such as an antenna, a radio frequency transceiver, a digital signal processor, an encryption/decryption chip, a Subscriber Identity Module (SIM) card, memory, and so forth. The network module 806 may communicate with various networks, such as the internet, an intranet, a wireless network, or with other devices via a wireless network. The wireless network may comprise a cellular telephone network, a wireless local area network, or a metropolitan area network. For example, the network module 806 can interact with the base station.

Referring to fig. 9, a block diagram of a computer-readable storage medium according to an embodiment of the present application is shown. The computer-readable medium 900 has stored therein program code that can be called by a processor to perform the methods described in the above-described method embodiments.

The computer-readable storage medium 900 may be an electronic memory such as a flash memory, an EEPROM (electrically erasable programmable read only memory), an EPROM, a hard disk, or a ROM. Alternatively, the computer-readable storage medium 900 includes a non-volatile computer-readable storage medium. The computer readable storage medium 900 has storage space for program code 910 to perform any of the method steps of the method described above. The program code can be read from or written to one or more computer program products. The program code 910 may be compressed, for example, in a suitable form.

According to the method, the device, the robot and the storage medium, firstly, measuring point positions of measuring points in a measuring area under a first coordinate system are obtained, then position conversion is carried out on the measuring point positions, target measuring point positions of the measuring points under a second coordinate system are obtained, wherein the second coordinate system is a robot space coordinate system, and finally if the joint points of the mechanical arm are determined not to be in a safe operation range based on the target measuring point positions, an alarm signal is sent out to carry out safety early warning. By the method, when the robot works and an obstacle is detected, the distance from each joint point of the mechanical arm of the robot to each measuring point (namely the obstacle, such as a ceiling, a wall surface or a floor) can be automatically calculated, and then when the joint points of the mechanical arm are determined not to be in a safe operation range, an alarm signal can be sent out to perform safety early warning, so that the probability of effectively protecting the robot can be improved.

While the present invention has been described with reference to the embodiments shown in the drawings, the present invention is not limited to the embodiments, which are illustrative and not restrictive, and it will be apparent to those skilled in the art that various changes and modifications can be made therein without departing from the spirit and scope of the invention as defined in the appended claims.