CN115256468A - State detection and standing planning method for humanoid robot after falling - Google Patents

Abstract

The invention provides a method for detecting the state and planning the standing of a humanoid robot after falling, after the robot falls, the position and the direction of each connecting rod are calculated according to the direction of the robot and the motor angle value corresponding to the joint, meanwhile, the position and the direction of each connecting rod are obtained by a motion capture suit, the position and the direction of each connecting rod are respectively compared, when the absolute value of the difference value between the position and the direction is less than or equal to the set threshold value, the body structure of the robot is all normal, then, the external environment of the robot is detected, so that whether the robot can stand is judged, if the robot has the capability of standing, the distance between the real-time state value and the reference value corresponding to the state which the robot must experience in the standing process is calculated by using a Mahalanobis distance calculation method, and the state corresponding to the minimum value of the distance of the corresponding joint of the robot is controlled. The invention solves the problems of state detection and standing planning of the robot in a complex environment and improves the adaptability of the robot.

Description

A state detection and stand planning method for a humanoid robot after a fall

technical field

The invention relates to the technical field of humanoid robots, in particular to a state detection and standing planning method of a humanoid robot after a fall.

Background technique

Humans design humanoid robots in the hope that they can help or replace humans to complete certain tasks, but due to the complexity of the environment, humanoid robots are prone to fall when performing certain tasks. Since a humanoid robot cannot perceive the position of the body after a fall and judge the joint damage after the fall like a human being, a method is needed to detect the state of the robot after the fall and decide whether to use it according to the detected state. Continue to complete the task.

Existing technologies focus on how humanoid robots avoid falling, or how to complete protective actions when falling is inevitable, but there are few researches on robot state detection after falling.

Contents of the invention

In view of this, the present invention provides a method for state detection and standing planning of a humanoid robot after a fall.

The present invention achieves the above-mentioned technical purpose through the following technical means.

A state detection and stand planning method for a humanoid robot after a fall:

When the robot falls, the industrial computer calculates the position and direction of each connecting rod from the direction of the robot and the corresponding motor angle value of the joint, which is recorded as S1; at the same time, the motion capture suit obtains the position and direction of each connecting rod, which is recorded as S2. And transmit to the industrial computer;

If |S1-S2|≤E, the structure of the robot body is normal, check the external environment where the robot is located, and plan for the robot to stand; if |S1-S2|>E, the robot body is abnormal and needs to be repaired immediately; Where E is the set threshold.

In the above technical solution, the external environment of the robot is detected, specifically: install the piezoelectric film sensor at the key collision point of the robot, and if the initial value of the piezoelectric film sensor changes, determine the body part corresponding to the key collision point contact with the ground.

In the above technical solution, the key collision points include the robot's forehead, back of the head, front chest, back, front waist, back waist, left and right palms, left and right backs of hands, left and right knees, front heels and rear heels.

In the above technical solution, the robot stands are planned, specifically: calculating the distance between the real-time state value of the robot and the reference value corresponding to the state that must be experienced during the standing process, and controlling the corresponding joints of the robot to reach the state corresponding to the minimum distance.

In the above technical solution, the real-time state value of the robot and the reference value corresponding to the state that must be experienced during the standing process all include: the motor angle value of the joint, the position and direction of the connecting rod, the data collected by the inertial measurement unit, and the data collected by the force sensor and the value of the key collision point.

In the above technical solution, when the corresponding joints of the robot are controlled to reach the state corresponding to the minimum distance, the angle value of the joint motor, the position and direction of the connecting rod, the data collected by the inertial measurement unit, the data collected by the force sensor, and the key collision are detected. Whether the value of the point is the set reference value, if so, continue to the next state transition, otherwise repeat the state detection, find the optimal state, and control the corresponding joints to reach the optimal state.

In the above technical solution, the distance is calculated using a Mahalanobis distance calculation method.

In the above technical solution, the inertial measurement unit is installed on the head of the robot, and the force sensors are installed on both ankles of the robot.

The beneficial effects of the present invention are as follows: the present invention firstly detects the state of the robot after it has fallen, including posture detection based on the body and detection based on the external environment, and judges whether the robot has the ability to stand up according to the detected state. ability, then use the Mahalanobis distance calculation method to calculate the distance between the real-time state value and the reference value corresponding to the state that must be experienced during the standing process, and control the corresponding joints of the robot to reach the state corresponding to the minimum distance, so as to complete the standing of the robot; The problem of state detection and standing planning of the robot in complex environments is solved, and the adaptability of the robot is improved.

Description of drawings

Fig. 1 is the flow chart of the state detection and standing planning method of the humanoid robot after falling down according to the present invention;

Fig. 2 is the structural representation of humanoid robot described in the present invention;

Fig. 3 is a schematic diagram of key collision points of the robot of the present invention;

Fig. 4 is a transition diagram of the standing state of the robot according to the present invention.

Detailed ways

The present invention will be further described below in conjunction with the accompanying drawings and specific embodiments, but the protection scope of the present invention is not limited thereto.

When a humanoid robot performs a certain task, it is prone to fall behavior due to the complexity of the environment. When a fall occurs, humans expect the robot to stand up immediately and continue to complete the task. However, since the humanoid robot does not have the keen perception function of the human body, after a fall, the robot cannot judge the state of its own body, nor can it know which part of the body is in contact with the ground, so it is very difficult to stand up.

Considering the above-mentioned problems, the present invention provides a state detection and stand planning method for a humanoid robot after a fall. As shown in FIG. 1 , the method includes ontology-based posture detection and external environment-based detection.

Figure 2 is a humanoid robot structure designed based on the principle of bionics. The robot has 20 degrees of freedom (that is, 20 joints), corresponding to 20 joint code discs and 20 joint motors. The robot head is equipped with an inertial Measuring unit (IMU), a force sensor is installed on the two ankles of the robot. In addition, the robot is also wearing a motion capture suit. The model of the motion capture suit in this embodiment is Noitom Perception Neuron3. When the motion capture suit is working, it can record the position and direction of the connecting rods between the joints of the robot in real time.

The state detection and standing planning method of a humanoid robot after a fall of the present invention will be introduced in detail below.

Ontology-based attitude detection: When the robot is detected to fall (the detection method is an existing technology), the inertial measurement unit (IMU) obtains the direction of the robot and transmits it to the industrial computer, and the industrial computer obtains the corresponding positions of the 20 joints of the robot. The motor angle value, thereby using the robot’s forward kinematics principle to calculate the position and direction of each connecting rod, which is recorded as S1; the process of calculating the position and direction of each connecting rod from the direction of the robot and the motor angle value is a prior art. Since the joint code disc is abnormal and the connecting rod breaks or falls off after the robot falls, the position and direction of each connecting rod are obtained according to the motion capture suit installed on the robot, recorded as S2, and transmitted to the industrial computer at the same time. After obtaining these two arrays, compare the position and direction of each connecting rod respectively. When the absolute value of the difference between the two is less than or equal to the set threshold E, it is considered that the structure of the robot body is normal and can continue to work; when If the absolute value of the difference between the two is greater than the set threshold E, it is considered that the robot body is abnormal and needs to be repaired immediately.

Detection based on the external environment: When the posture detection based on the body is completed and the structure of the robot body is normal, it is necessary to detect the external environment of the robot to complete the standing task of the robot. Piezoelectric film sensors are installed at the key collision points of the robot, including the robot's forehead, back of the head, front chest, back, front waist, back waist, left and right palms, left and right backs of hands, left and right knees, front heels and rear heels, As shown in Figure 3. The data collected by the piezoelectric film sensor is transmitted to the industrial computer, and the initial value of the piezoelectric film sensor is set to 0. When the pressure is applied, the initial value becomes 1. Based on this, it can be judged which part of the body is in contact with the ground. At the same time, the data collected by the force sensor is transmitted to the industrial computer in real time.

After the attitude detection based on the body and the detection based on the external environment are completed, the motor angle values of the 20 joints of the robot, the position and direction of the connecting rod, the data collected by the inertial measurement unit (IMU), the data collected by the force sensor, and the key points can be obtained. The value of the collision point (that is, the data collected by the piezoelectric film sensor), and set these values as an array Mx.

Robot standing planning: Regarding the robot’s standing planning, in the form of state transfer, the robot’s standing process is divided into several states that must be experienced. State A, state B, state C..., and these states include the motors of the robot’s 20 joints The angle value, the position and direction of the connecting rod, the data collected by the inertial measurement unit (IMU), the data collected by the force sensor, and the value of the key collision point are respectively set as an array M _A , M _B , M _C ...; when the robot wants to When you want to stand, according to the state value at this time (that is, the array Mx obtained after the state detection is completed), the reference values M _A , M _B , M _C ... The distance between the state value and the reference value is the smallest. Once the minimum value is found, the corresponding joints of the robot are controlled to reach the state corresponding to the minimum value.

After the robot reaches the target point (the state corresponding to the minimum value), there may be some interference sliding phenomenon, so it needs to be detected twice to prevent false arrival phenomenon. The specific method is: after the robot reaches the target point, it detects whether the angle values of the 20 joint motors, the position and direction of the connecting rod, the data collected by the inertial measurement unit (IMU), the data collected by the force sensor, and the value of the key collision point are For the set reference value (M _A , M _B , M _C ...), if it is, continue to the next state transition; if not, repeat the above state detection, and use the Mahalanobis distance calculation method to calculate the state of the target point The distance between the value and the reference value is the smallest, find the optimal state, and control the corresponding joints to reach this state.

Embodiment: As shown in the lower left corner of Fig. 4, after the robot falls, the state is unknown at this time, and the robot starts to start the attitude detection based on the body, first obtains the direction of the robot according to the inertial measurement unit (IMU), and then according to 20 For the motor angle value of the joint, the position and direction of each connecting rod are calculated by using the robot’s positive kinematics principle, and a set of data is recorded as S1; then the motion capture suit on the robot is started, because the motion capture suit can provide real-time information to the industrial control after it is started. The machine transmits the position and direction of each connecting rod, record the transmitted data as S2, then subtract the two sets of data, and take the absolute value, if the absolute value is less than the set threshold E, it means that each connecting rod is normal, not broken or falling off And so on, then the robot will start detection based on the external environment; otherwise, the robot will stop working. If the robot can work normally, start to detect the key collision points. Still take the illustration in the lower left corner as an example. The readings of the electric film sensor all change, which means that these key collision points are in contact with the ground, and the value corresponding to this state is set to the array Mx=[the motor angle value of 20 joints; the position and direction of the connecting rod; the inertial measurement unit (IMU) collected data; force sensor collected data; key collision point values]. Since the robot stores several states that the robot must go through during the standing process, such as state A, state B, state C... in the figure, set the corresponding values of M _A , M _B , M _C ...; then use The Mahalanobis distance calculates which reference state is closer to the state value Mx at this time. After calculation and comparison, it is found that state A is the closest, so the corresponding joints of the robot are controlled to reach state A. Due to the phenomenon of interference sliding on the soles of the feet when the robot reaches state A, although the joint motor has reached the corresponding angle, it is possible that other parameter values have not reached, so it needs to be checked again. If the detected state value If it is correct, it means that the robot has reached the target state, and can continue to the next target state transition, as shown in state B in the figure, followed by state C, and finally stands; if the detected state value is incorrect, for example, in the process of standing When the robot is disturbed, the robot changes from the unknown state to the state H. When it reaches the state H, it starts to detect the angle values of the 20 joint motors, the position and direction of the connecting rod, the data collected by the inertial measurement unit (IMU), and the data collected by the force sensor. Whether the value of the data and the key collision point is the set reference value M _A , and it is found that it is not the expected state A, then find the optimal state according to the state at the moment, and the result of the search is state D, and control the robot joints to reach the target state D, Then state C, finally standing.

The described embodiment is a preferred implementation of the present invention, but the present invention is not limited to the above-mentioned implementation, without departing from the essence of the present invention, any obvious improvement, replacement or modification that those skilled in the art can make Modifications all belong to the protection scope of the present invention.

Claims (8)

1. A method for detecting the state and planning standing of a humanoid robot after falling is characterized in that:

when the robot falls down, the industrial personal computer calculates the position and the direction of each connecting rod according to the direction of the robot and the motor angle value corresponding to the joint, and records the position and the direction as S1; meanwhile, the motion capture suit acquires the position and the direction of each connecting rod, records the position and the direction as S2 and transmits the position and the direction to the industrial personal computer;

if the absolute value of S1-S2 is less than or equal to E, the structure of the robot body is normal, the external environment of the robot is detected, and the robot is planned to stand; if the absolute value of S1-S2 is greater than E, the robot body is abnormal and needs to be overhauled immediately; where E is the set threshold.

2. The method for detecting the fallen state and planning the standing state of the humanoid robot as claimed in claim 1, wherein the detection of the external environment of the robot is specifically as follows: and (3) mounting a piezoelectric film sensor at the key collision point of the robot, and judging that the body part corresponding to the key collision point is in contact with the ground if the initial value of the piezoelectric film sensor changes.

3. The method of claim 2, wherein the key collision points include the forehead, the hindbrain, the chest, the back, the front waist, the back waist, the palms, the knees, the front heel, and the heel of the robot.

4. The method for detecting the fallen state and planning the standing of the humanoid robot as claimed in claim 3, wherein the robot is planned to stand, and specifically comprises: and calculating the distance between the real-time state value of the robot and a reference value corresponding to the state which the robot must experience in the standing process, and controlling the corresponding joint of the robot to reach the state corresponding to the minimum distance.

5. The method for detecting the fallen state and planning the standing state of the humanoid robot as claimed in claim 4, wherein the real-time state value of the robot and the reference value corresponding to the state that the robot must experience during the standing process both comprise: the motor angle value of the joint, the position and the direction of the connecting rod, data collected by the inertia measurement unit, data collected by the force sensor and the value of the key collision point.

6. The method for detecting the fallen state and planning the standing state of the humanoid robot as claimed in claim 5, wherein after the robot is controlled to reach the state corresponding to the minimum distance of the corresponding joints, the angle value of the joint motor, the position and direction of the connecting rod, the data collected by the inertial measurement unit, the data collected by the force sensor, and the value of the key collision point are detected to determine whether the values are the set reference values, if so, the next state transition is continued, otherwise, the state detection is repeated to find the optimal state, and the corresponding joints are controlled to reach the optimal state.

7. The method for detecting the fallen state and planning the standing state of the humanoid robot as claimed in claim 6, wherein the distance is calculated by using a mahalanobis distance calculation method.

8. The post-fall state detection and standing planning method for a humanoid robot as claimed in claim 7, wherein said inertial measurement unit is mounted on the robot head and said force sensors are mounted on the ankles of the robot.

Images