TWI683734B - Anti-collision method for robot - Google Patents

Abstract

An anti-collision method for robot is provided. The method controls a robot to sense a Variety of electric field over continuous time via electric field sensor, determine a relative moving direction of an obstacle when detecting the obstacle by the variety of electric field, generate motor control data according to the relative moving direction of the obstacle, and control a motor to rotate according to the motor control data for making a robotic limb move along a dodge vector to dodge the obstacle.

Description

Robot anti-collision method

The invention relates to a robot, and in particular to a collision prevention method of the robot.

In the prior art, many robots with mechanical arms have been proposed. The aforementioned robotic arm can perform precise operations (such as taking objects, placing objects, or moving objects, etc.), and can replace manpower or cooperate with personnel to complete tasks. Specifically, the robot arm is provided with a plurality of motors, and each motor is set to move the robot arm in a specified axial direction, and the robot moves in a plurality of axial directions by simultaneously controlling the operation of the plurality of motors.

However, the aforementioned robots usually work in an open environment (such as an unenclosed environment) or work collaboratively with personnel, and there is a risk of collision with personnel, vehicles, or other obstacles.

In order to solve the above problems, a robot that can detect obstacles based on changes in motor current has been proposed. The aforementioned robot is provided with a current sensor on the motor, and determines whether the robotic arm collides with the obstacle by detecting the current change of the motor, such as when the current of the motor increases abnormally. The aforementioned robot must detect the obstacle after the collision occurs, and there is a risk of the robot colliding or the obstacle being damaged.

Another robot that can detect obstacles based on computer vision is proposed. The aforementioned robot is equipped with one or more cameras, and performs object recognition on the captured images to determine whether there are obstacles Obstruction. Although the aforementioned robot can detect obstacles before a collision occurs, due to the dead angle of the camera's shooting range, it cannot effectively detect obstacles from all directions.

In view of this, there is an urgent need to propose a solution that can effectively prevent the robot from colliding.

The invention provides an anti-collision method for a robot, which can detect the moving direction of an obstacle based on electric field sensing technology and automatically evade.

In an embodiment, a collision prevention method is used for a robot. The robot includes a mechanical limb, an electric field sensor disposed on the mechanical limb, and a motor for moving the mechanical limb. The anti-collision method includes the following steps: via an electric field sensor Sensing the electric field change in continuous time; determining the relative moving direction of the first obstacle when the first obstacle is detected according to the electric field change; generating the motor control data corresponding to the dodge vector according to the relative moving direction; and, controlling based on the motor control data The motor rotates to move the mechanical limb along the dodge vector to dodge the first obstacle.

The invention can effectively enable the robot to automatically evade before the collision occurs, and can avoid the collision of the robot or the destruction of obstacles.

1‧‧‧Robot

10‧‧‧Micro processing unit

11, 111-118, 511-513, 611-612, 71‧‧‧ electric field sensor

12‧‧‧Motor

13‧‧‧Motor sensor

14‧‧‧Memory unit

140‧‧‧ computer program

15‧‧‧Human-machine interface

16‧‧‧Communication unit

17‧‧‧Remote control

18‧‧‧Computer device

20-24, 30, 5‧‧‧ mechanical limbs

25, 55, 65, 750-751 ‧‧‧ movable structure

26, 56, 66, 66’, 66’’, 760-761

40, 42, 44, 46, 48

D1, D2, f1-f7, D _L , D _R ‧‧‧ Relative movement direction

X, Y, Z‧‧‧ axis

θ1, θ1’, θ2, θ3, θ4, θ5‧‧‧Angle

P1, P2‧‧‧ position

S10-S14‧‧‧The first anti-collision procedure

S200-S213‧‧‧second anti-collision procedure

S300-S309‧‧‧The third anti-collision procedure

S40-S42‧‧‧Decision steps

S50-S56‧‧‧The fourth anti-collision procedure

FIG. 1 is a structural diagram of a robot according to an embodiment of the present invention; FIG. 2 is a schematic diagram of the appearance of a robot according to another embodiment of the present invention; FIG. 3 is a schematic diagram of the appearance of a robot according to another embodiment of the present invention; A first schematic diagram of using a single electric field sensor to detect an obstacle; FIG. 4B is a second schematic diagram of using a single electric field sensor to detect an obstacle; 4C is a third schematic diagram of the present invention using a single electric field sensor to detect obstacles; FIG. 5A is a first schematic diagram of the present invention using multiple electric field sensors to detect obstacles; FIG. 5B is a use of the present invention A second schematic diagram of multiple electric field sensors detecting obstacles; FIG. 5C is a third schematic diagram of using multiple electric field sensors to detect obstacles; FIG. 5D is a use of multiple electric field sensors of the invention A fourth schematic diagram of detecting obstacles; FIG. 6A is a schematic diagram of electric field changes of the plurality of electric field sensors of FIG. 5A; FIG. 6B is a schematic diagram of electric field changes of the plurality of electric field sensors of FIG. 5B; FIG. 6C is a diagram of FIG. 5C FIG. 6D is a schematic diagram of electric field changes of multiple electric field sensors of FIG. 5D; FIG. 6E is a schematic diagram of energy center changes and maximum energy intensity changes; FIG. 7 is the invention 8 is a schematic diagram of the present invention to avoid multiple obstacles; FIG. 9A is a schematic diagram of the first avoidance strategy of the present invention; FIG. 9B is a schematic diagram of the second avoidance strategy of the present invention; FIG. 10 is A flowchart of the anti-collision method of the first embodiment of the present invention; FIG. 11A is a first flowchart of the anti-collision method of the second embodiment of the present invention; FIG. 11B is a second flow of the anti-collision method of the second embodiment of the present invention FIG. 12 is a flowchart of a collision prevention method according to a third embodiment of the invention; FIG. 13 is a partial flowchart of a collision prevention method according to a fourth embodiment of the invention; and FIG. 14 is a collision prevention method according to a fifth embodiment of the invention Flow chart of the method.

The technical solutions of the present invention will be described in detail below in conjunction with the drawings and specific embodiments to further understand the objectives, solutions, and effects of the present invention, but they are not intended to limit the scope of the patent application attached to the present invention.

Please refer to FIG. 1, which is an architectural diagram of a robot according to an embodiment of the present invention. The present invention discloses a robot 1. The robot 1 is configured with one or more mechanical limbs (such as the mechanical limbs 20-24 shown in FIG. 2 or the mechanical limbs 30 shown in FIG. 3), and can change the foregoing automatically or according to user operations. The movement of mechanical limbs to perform precise operations (such as picking objects, placing objects or moving objects, etc.).

The robot 1 may include one or more electric field sensors 11, one or more motors 12, a memory unit 14 for storing data, and a micro-processing unit 10 for controlling the aforementioned devices.

The electric field sensor 11 can form an electric field in the space, and sense the energy intensity of the electric field via the sensing electrode. Specifically, when an obstacle enters the aforementioned electric field, it will cause electric field disturbance and change the energy intensity of the electric field. The present invention senses whether there is an obstacle in the electric field by analyzing the change of the energy intensity of the electric field, and further analyzes the moving direction or moving speed of the obstacle.

The plurality of motors 12 are respectively connected to a plurality of movable structures (such as gear sets, transmission shafts, or other mechanical components that can transmit power, such as the movable structure 25 shown in FIG. 2 or FIG. 3), and each movable structure is used to connect the arm body in series ( The arm 26 shown in FIG. 2 or FIG. 3). Each motor 12 is used to output power to the movable structure to push the movable structure to move the arm body, so that the robot 1 swings out a specified action. Taking the robot 1 shown in FIG. 2 as an example, if the motor 12 of the right hand outputs power to the movable structure 25 of the right hand, the arm 26 of the right hand of the robot 1 can be moved, and the action such as lifting, bending, or lowering can be performed. .

The micro-processing unit 10 (such as a microcontroller) is used to control the robot 1 and can control the output power of each motor 12.

In one embodiment, the memory unit 14 includes a non-transitory computer readable medium and stores a computer program 140 (such as the firmware, operating system, or application program of the robot 1). The computer program 140 records Computer readable code. The micro-processing unit 10 can execute a computer program 140 to control the robot 1 to implement the steps of the anti-collision method of the embodiments of the present invention.

In an embodiment, the robot 1 includes one or more motor sensors 13 electrically connected to the micro-processing unit 10. Each motor sensor 13 is respectively disposed on the motor 12, and can sense the rotation state of each motor 12 (such as sensing the rotation position, speed, or current of the motor 12 ), and generate corresponding motor sensing data. In this way, the micro-processing unit 10 can determine whether the robot 1 swings out a specified action according to the motor sensing data. In an embodiment, the robot 1 may include a human-machine interface 15 (such as indicator lights, speakers, buttons, or other input/output elements) electrically connected to the micro-processing unit 10. The user can input operations through the human-machine interface 15 to make the micro-processing unit 10 execute specific commands (such as swinging out a specific action), or know the current state of the robot 1 (such as the current working state or power) through the human-machine interface 15.

In an embodiment, the robot 1 may include a communication unit 16 (such as a Bluetooth transceiver, Zig-Bee transceiver, Wi-Fi transceiver Sub-1GHz transceiver, etc.) that is electrically connected to the microprocessing unit 10 or (USB module, wired network module, serial data communication module and other wired communication modules). The user can control the robot 1 via the remote controller 17 paired with the communication unit 16 (such as a dedicated remote controller for the robot 1 or a smartphone installed with a corresponding application program). Alternatively, the robot 1 may be connected to an external computer device 18 via the communication unit 16 and be controlled by the computer device 18.

Please also refer to FIG. 2 at the same time, which is a schematic view of the appearance of a robot according to another embodiment of the present invention. In this embodiment, the robot 1 is a humanoid robot, and is provided with five sets of mechanical limbs 20-24, namely a head, a right hand, a left hand, a right foot, and a left foot. Each mechanical limb 20-24 includes one or more arm bodies 26 and a movable structure 25.

Moreover, each mechanical limb 20-24 may be provided with one or more electric field sensors 11. In an embodiment, each electric field sensor 11 is disposed on the arm 26 of each mechanical limb 20-24. Thereby, when the robot 1 swings/moves the mechanical limbs 20-24, it can sense whether there is an obstacle around the arm body 26 through the electric field sensor 11.

In this embodiment, the mechanical limb 20 of the head is provided with a set of electric field sensors 11 on the upper side, the left side, and the right side respectively, which can simultaneously sense obstacles from three directions; the mechanical limbs 21-22 of the hand are movable The range is large and the frequency of use is high. The probability of collision with obstacles is high. Therefore, multiple sets of electric field sensors 11 are provided on the inner and outer sides to accurately sense the obstacles on both sides; the mechanical limbs of the legs 23- 24 Since the movable range is small and the probability of collision with obstacles is low, only a small amount of electric field sensor 11 is provided on the outside thereof to reduce costs.

Please also refer to FIG. 3 for the appearance of the robot according to another embodiment of the invention. In this embodiment, the robot 1 is a mechanical arm robot, and only a single mechanical limb 30 is provided. The mechanical limb 30 includes a plurality of arm bodies 26 and movable structures 25.

In this embodiment, the arm 26 at the end has a higher moving frequency and a higher probability of collision with obstacles, so the density of the electric field sensors 11 is higher (four sets of electric field sensors 11 are provided) to accurately sense Detect the surrounding obstacles; due to the lower movement frequency of the middle arm 26, the probability of collision with obstacles is lower, so the installation density of the electric field sensor 11 is lower (only two sets of electric field sensors 11 are provided on the outside) Cost saving; the foremost arm 26 cannot be moved due to the connection to the base, so the electric field sensor 11 is not provided to save costs.

It is worth mentioning that the configuration of the electric field sensor 11 shown in FIG. 2 and FIG. 3 is only an example, and the number and position of the electric field sensor 11 can be arbitrarily determined according to user needs or the purpose of using the robot 1 Changes are not limited.

Please refer to FIG. 4A to FIG. 4C and FIG. 10 at the same time. 4A is a first schematic diagram of using a single electric field sensor to detect obstacles of the present invention, FIG. 4B is a second schematic diagram of using a single electric field sensor to detect obstacles of the present invention, and FIG. 4C is a single use of the present invention to detect obstacles A third schematic diagram of an electric field sensor detecting an obstacle. FIG. 10 is a flowchart of a collision prevention method according to a first embodiment of the present invention. The present invention discloses a robot anti-collision method. The anti-collision method can be applied to the robot 1 of any embodiment shown in FIGS. 1 to 3. The anti-collision method of this embodiment includes the following steps.

Step S10: The microprocessing unit 10 of the robot 1 senses the electric field change in continuous time via the electric field sensor 11. In one embodiment, the micro-processing unit 10 continuously obtains the energy intensity of the electric field from each electric field sensor 11 and calculates the electric field change within a specified time interval, such as calculating all the electric field senses within 1 second every 1 second The energy intensity of the detector 11 changes).

Step S11: The micro-processing unit 10 determines whether there is an obstacle in the electric field according to the obtained electric field change. In an embodiment, the micro-processing unit 10 determines that there is an obstacle in the electric field when the change in the electric field exceeds a critical value (for example, more than 3 watts) in a specified time interval (for example, 1 second or 3 seconds).

If the micro-processing unit 10 detects an obstacle, step S12 is executed. Otherwise, the micro-processing unit 10 executes step S10 again to continuously sense the electric field change.

Step S12: The micro-processing unit 10 determines the relative moving direction of the obstacle according to the change of the electric field.

Taking a single electric field sensor 11 as an example, as shown in FIG. 4A, when there is no obstacle in the electric field of the electric field sensor 11, the energy intensity of the electric field is a stable energy intensity (such as the strongest energy intensity, 10 watts). As shown in FIG. 4B, after the obstacle 40 (such as a human hand) enters the electric field, the obstacle 40 will interfere with the electric field and cause the electric field to change (for example, the energy intensity is reduced to 8 watts). As shown in FIG. 4C, after the obstacle 40 is closer to the electric field sensor 11, the obstacle 40 will cause greater interference to the electric field and cause a greater change in the electric field (eg, the energy intensity is reduced to 4 watts).

Finally, the micro-processing unit 10 can determine whether the obstacle 40 is approaching toward the electric field sensor 11 according to the change of the electric field, and can determine the relative moving direction D1 of the obstacle.

It is worth mentioning that the relative movement direction is relative to the installation position of the electric field sensor 11 on the mechanical limb, and the micro-processing unit 10 can perform an analysis conversion process on the relative movement direction according to the installation position and the current position of the mechanical limb, to The moving direction of the obstacle relative to the entire robot 1 is obtained.

Step S13: The micro-processing unit 10 generates motor control data corresponding to the dodge vector according to the relative movement direction. In one embodiment, the micro-processing unit 10 can determine the evasion vector of the mechanical limb according to the calculated relative movement direction (that is, the mechanical limb moves along the evasion vector to avoid obstacles), and generates the mechanical limb to move along the evasion vector Motor control data. Specifically, the aforementioned dodge vector includes the direction and magnitude (ie, the movement distance) when the corresponding mechanical limb performs the dodge action.

In an embodiment, the direction of the aforementioned dodge vector may be the same as the relative movement direction. For example, if the obstacle approaches the mechanical limb toward the left, the mechanical limb may dodge toward the left to move away from the obstacle. In an embodiment, the direction of the evasion vector and the relative movement direction may be in different planes or different axes. For example, if the obstacle approaches the mechanical limb to the left, the mechanical limb may be lifted upward to evade the obstacle.

Step S14: The micro-processing unit 10 controls the corresponding motor 12 to rotate according to the motor control data to move the mechanical limb along the evasion vector to avoid the obstacle.

In this way, the invention can effectively enable the robot to automatically dodge before the collision occurs, and can avoid the collision of the robot or the destruction of obstacles.

It is worth mentioning that in one embodiment, the micro-processing unit 10 still performs electric field sensing (ie, steps S10-S11) to detect the first obstacle (ie, perform steps S12 to S14) during the evasion of the first obstacle. Whether there is a second obstacle, and after detecting the second obstacle and evading the first obstacle, then evading the second obstacle. In this way, the present invention can continuously avoid multiple obstacles and prevent collisions with obstacles.

Please refer to FIG. 5A to FIG. 6D and FIG. 11A to FIG. 11B at the same time. FIG. 5A is a first schematic diagram of using multiple electric field sensors to detect obstacles of the present invention. FIG. 5B is a sensing of multiple electric fields of the present invention. 5C is a third schematic diagram of the present invention using multiple electric field sensors to detect obstacles, and FIG. 5D is a present invention using multiple electric field sensors to detect obstacles. Fourth schematic diagram. FIG. 6A is a schematic diagram of electric field changes of multiple electric field sensors of FIG. 5A, FIG. 6B is a schematic diagram of electric field changes of multiple electric field sensors of FIG. 5B, and FIG. 6C is a plurality of electric field sensors of FIG. The change of the electric field of the detector Intention, FIG. 6D is a schematic diagram of electric field changes of a plurality of electric field sensors of FIG. 5D, FIG. 6E is a schematic diagram of energy center and maximum energy intensity changes, and FIG. 11A is a first flow of a collision prevention method according to a second embodiment of the invention FIG. 11B is a second flowchart of the anti-collision method of the second embodiment of the present invention.

In the examples of FIGS. 4A to 4C, the single electric field sensor 11 can only sense the vertical movement direction and speed of the obstacle 40, but cannot sense the horizontal movement direction and speed.

To solve the above problem, this embodiment further proposes to use a plurality of electric field sensors provided on the same side of the arm body (the examples in FIGS. 5A to 5D are provided with eight sets of electric field sensors 111-118) to sense obstacles The technical solution of the horizontal movement direction and speed. The anti-collision method of this embodiment includes the following steps.

Step S200: The micro-processing unit 10 of the robot 1 senses the energy intensity changes of the multiple electric fields in a continuous time via the multiple electric field sensors 11.

Step S201: The micro-processing unit 10 senses the change of the energy center of the multiple electric fields in a continuous time via the multiple electric field sensors 11.

Specifically, in steps S200 and S201, the microprocessor unit 10 senses the electric field changes of the multiple electric fields in the specified time interval through the multiple electric field sensors 111-118, and then according to the sensed electric field changes and time The interval calculates the aforementioned energy intensity change and energy center change.

For example, as shown in FIG. 5A, the energy intensity is 10 watts when there are no obstacles in the electric field, and the energy intensity of the electric field of each electric field sensor 111-118 changes as shown in FIG. 6A (electric field sensor 111-118 The positions in FIGS. 6A to 6D are 1-8, respectively, the horizontal axis of FIGS. 6A to 6D is the position of the electric field sensor, and the vertical axis is the change of the energy intensity of the electric field).

Next, as shown in FIG. 5B, at the first second, the obstacle 42 (such as a human hand) enters the electric field and causes different degrees of interference to the electric fields of the adjacent electric field sensors 115-118, causing these electric fields to occur to different degrees (For example, the electric field energy of the electric field sensors 111-118 is 10 watts, 10 watts, 10 watts, 10 watts, 7 watts, 5 watts, 6 watts, and 8 watts), the energy intensity changes of the electric fields of the electric field sensors 111-118 are shown in FIG. 6B.

Then, at the second second, as shown in FIG. 5C, the obstacle 42 moves along the moving direction D2 (including horizontal and vertical movements), and causes different interference to these electric fields to cause these electric fields to change again (such as electric field sensing The electric field energy intensity of the 111-118 electric field is 10 watts, 10 watts, 8 watts, 5 watts, 4 watts, 5 watts, 8 watts and 10 watts), the electric field energy intensity of each electric field sensor 111-118 changes As shown in Figure 6C.

Then, at the third second, as shown in FIG. 5D, the obstacle 42 continues to move along the moving direction D2 (including horizontal and vertical movements), and causes different interference to these electric fields to cause these electric fields to change again (such as electric field sensing). The energy intensity of the electric field of the sensors 111-118 is 10 watts, 6 watts, 3 watts, 2 watts, 3 watts, 6 watts, 10 watts and 10 watts), and the energy intensity of the electric field of each electric field sensor 111-118 The change is shown in Figure 6D.

Finally, the micro-processing unit 10 can calculate the energy center and the maximum energy intensity at each time point, and further calculate the energy intensity change and the energy center change. Specifically, as shown in FIG. 6E (the horizontal axis is time, and the vertical axis is energy intensity change/field sensor position), at the first second (the scenario shown in FIG. 5B), the maximum energy intensity change is 5, Occurs at sensor 116; at the second second (situation shown in FIG. 5C), the maximum energy intensity change is 6, occurs at sensor 115; at the third second (situation shown in FIG. 5D), the largest The energy intensity change is 8, which occurs in the sensor 114, that is, the maximum energy intensity change is 5 watts → 6 watts → 8 watts.

In addition, the micro-processing unit 10 can calculate the energy center at each time point according to the following formula (1):

Where N is the number of electric field sensors, Wn is the energy intensity of the electric field of the nth electric field sensor, and Xn is the installation position of the nth electric field sensor (1-8 in the above example).

Taking the above example as an example, the position of the energy center in the first second is about 6.36, the position of the energy center in the second second is 5, and the position of the energy center in the third second is 4, that is, the change of the energy center is 6.36→5→4 .

Step S202: The micro-processing unit 10 determines whether there is an obstacle in the electric field according to the obtained energy intensity change and energy center change. In one embodiment, the micro-processing unit 10 determines that there is an obstacle in the electric field when the energy intensity change and the energy center change exceed a critical value.

If the micro-processing unit 10 detects an obstacle, step S203 is executed. Otherwise, the micro-processing unit 10 executes step S200 again to continuously sense the electric field change.

Step S203: The micro-processing unit 10 determines the vertical movement direction between the obstacle and the multiple electric field sensors according to the change in energy intensity.

In an embodiment, as shown in FIG. 6E, the micro-processing unit 10 can calculate that the change in energy intensity increases with time, that is, the vertical distance between the obstacle 42 and the mechanical limb is gradually shortened, and the vertical direction of the obstacle 42 is toward Mechanical limbs approached.

Step S204: The micro-processing unit 10 determines the parallel moving direction of the obstacle according to the change of the energy center.

In one embodiment, as shown in FIG. 6E, the micro-processing unit 10 can calculate that the change in energy center moves from position 6.36 to position 4 with time, that is, the horizontal direction between the obstacle 42 and the mechanical limb is sensed by the electric field器116向电电感器114。 116 towards the electric field sensor 114.

Step S205: The micro-processing unit 10 determines the relative movement direction between the obstacle and the multiple electric field sensors according to the vertical movement direction calculated in step S203 and the parallel movement direction calculated in step S204.

Step S206: The micro-processing unit 10 determines the relative vertical movement speed between the obstacle and the plurality of electric field sensors according to the sensing time and energy intensity changes.

In an embodiment, the micro-processing unit 10 may calculate the corresponding actual vertical distance change according to the energy intensity change, and calculate the vertical movement speed of the obstacle based on the sensing time (eg, 3 seconds) and the vertical distance change.

Step S207: The micro-processing unit 10 determines the relative parallel moving speed between the obstacle and the multiple electric field sensors according to the sensing time and the change of the energy center.

In one embodiment, the micro-processing unit 10 can calculate the horizontal distance change according to the energy center change and the setting position of each sensor 111-118, and calculate the obstacle level according to the sensing time (such as 3 seconds) and the horizontal distance change Moving speed.

Step S208: The micro-processing unit 10 determines the relative movement speed between the obstacle and the plurality of electric field sensors according to the vertical movement speed obtained in step S206 and the parallel movement speed obtained in step S206.

Step S209: The micro-processing unit 10 can determine whether the obstacle approaches the mechanical limb of the robot 1 and may collide according to the obtained relative movement direction and/or relative movement speed of the obstacle.

If the micro-processing unit 10 determines that the obstacle may collide, step S210 is executed. If the micro-processing unit 10 determines that the obstacle will not collide with the robot 1 (for example, if the obstacle continues to move away from the robot 1 or the obstacle has stopped moving), then step S200 is executed again for continuous detection.

Step S210: The micro-processing unit 10 determines the direction and magnitude of the evasion vector according to the relative movement direction and relative movement speed of the obstacle.

In one embodiment, the magnitude of the evasion vector may be a fixed value, or determined according to the relative movement direction and/or relative movement speed of the obstacle.

Please also refer to FIG. 7 for a schematic diagram of evading obstacles of the present invention. In one embodiment, as shown in FIG. 7, in this example, three sets of electric field sensors 511-513 are provided on the outer side of the mechanical limb 5, and include an arm 56 and a movable structure 55. The robot 1 can control the movable structure 55 to rotate in three different axial directions via at least three sets of motors 12, so that the arm body 56 rolls and turns in the X axis, and in the Y axis Perform the pitch rotation operation or the yaw rotation operation on the Z axis. FIG. 7 is used to illustrate the direction of the dodge direction vector that can be adopted for obstacles from different directions, but is not intended to limit the present invention.

When the relative movement direction of the obstacle is f1, f3, f4, or f7, the micro-processing unit 10 can control the arm body 56 to perform a tumbling motion on the X axis (such as the arm body 56 moving inward) to avoid the obstacle. When the relative movement direction of the obstacle is f2 or f6, the micro-processing unit 10 can control the arm body 56 to perform the tumbling movement on the X axis, and simultaneously perform the pitch rotation movement on the Y axis (for example, the arm body 56 moves inward and Lift up) to avoid obstacles. When the relative moving direction of the obstacle is f5, the micro-processing unit 10 can control the arm 56 to perform a pitching and turning movement on the Y-axis (eg, the arm 56 is lifted upward) to avoid the obstacle.

In an embodiment, the micro-processing unit 10 may further determine the evasion timing for performing the evasion action according to the relative movement direction, relative movement speed and position of the mechanical limb of the obstacle (such as evasion after 2 seconds, or between the obstacle and the machine) (Dodge when the distance between the limbs is less than the preset value).

Next, step S211 is executed: the micro-processing unit 10 generates corresponding motor control data according to the determined evasion vector and/or evasion timing.

Step S212: The micro-processing unit 10 determines whether the current timing meets the evasion timing. If the timing of evasion has been met, the micro-processing unit 10 executes step S213. Otherwise, the micro-processing unit 10 executes step S212 again to wait for the dodge timing.

Step S213: The micro-processing unit 10 controls the motor 12 to rotate according to the motor control data so that the mechanical limb moves the magnitude of the dodge vector in the direction of the dodge vector when the dodge timing is met to avoid the obstacle.

The present invention delays the robot 1 until the timing of evasion, and can effectively reduce the amount of movement required for the mechanical limbs to evade obstacles, thereby reducing power consumption.

It is worth mentioning that, in another embodiment of the present invention, it is possible to directly control the mechanical limb to evade after detecting an obstacle regardless of the timing of evasion.

Please refer to FIG. 8 and FIG. 12 at the same time. FIG. 8 is a schematic diagram of avoiding multiple obstacles according to the present invention. FIG. 12 is a flowchart of a collision prevention method according to a third embodiment of the present invention. As shown in FIG. 8, in this embodiment, the mechanical limb of the robot 1 includes a movable structure 65 and an arm body 66. An electric field sensor 611 is provided on the left side of the arm body 66, and an electric field sensor 612 is provided on the right side. The robot 1 of this embodiment can sense the relative movement direction of the same obstacle according to the electric field sensors 611-612 provided on different sides. Moreover, the robot 1 of this embodiment can avoid one obstacle while avoiding collision with another obstacle. Specifically, the anti-collision method of this embodiment includes the following steps.

Step S300: The micro-processing unit 10 of the robot 1 senses the electric field change in continuous time via the electric field sensors 611-612 on different sides.

Step S301: The micro-processing unit 10 determines whether there is an obstacle 44 (first obstacle) in the electric field according to the acquired electric field change.

If the micro-processing unit 10 detects an obstacle, steps S302-S303 are executed. Otherwise, the micro-processing unit 10 executes step S300 again to continuously sense the electric field change.

Step S302: a first micro processing unit 10 determines the moving direction D _L obstacle 44 based on change in electric field on the left side of the field sensor 611 sensed.

Step S303: micro processing unit 10 determines a second obstacle movement direction D _R 44 changes according to an electric field on the other side (right side) of the electric field sensor 612 sensed.

It is worth mentioning that step S302 and step S303 can be executed simultaneously or sequentially, without limitation.

Step S304: The micro-processing unit 10 determines the relative movement direction according to the first movement direction and the second movement direction.

In an embodiment, the micro-processing unit 10 may use the average direction of the first movement direction and the second movement direction as the relative movement direction.

Step S305: The micro-processing unit 10 determines the dodge vector according to the determined relative movement direction.

In one embodiment, the micro-processing unit 10 calculates the position of the intersection point of the relative movement direction and the arm body 66 before the movement, and calculates the evasion vector that can move the arm body 66 away from the intersection point position.

In one embodiment, the micro-processing unit 10 can calculate the relative movement vector of the obstacle 44 and set it as the dodge vector.

Step S306: The micro-processing unit 10 determines in advance whether the movement of the mechanical limb along the determined evasion vector will collide with another obstacle 48 (second obstacle).

For example, if the evasion vector before the correction is turned to the right by the angle θ1, when the arm 66 moves to the right by the turning angle θ1 and moves to the position of the arm 66', the obstacle 44 can be avoided but it will collide with another obstacle Object 48 (such as a wall). Therefore, the movement of the mechanical limb along the current evasion vector does not necessarily avoid all obstacles 44 and 48.

In one embodiment, the memory unit 14 of the robot 1 records the environment map and the positions of the obstacles 48 (such as fixed obstacles, including obstacles with a fixed moving route) on the environment map. The microprocessing unit 10 can determine whether the movement of the mechanical limb along the evasion vector will collide with any obstacle 48 according to the environment map, the current position of the robot 1 on the environment map, and the position of each obstacle 48 on the environment map.

In an embodiment, the micro-processing unit 10 can also determine (e.g., determine and determine the movement method, speed, or trajectory of each obstacle based on the change of the electric field) that each obstacle is a movable obstacle (such as a person or pet, etc.) , Including obstacles without a fixed movement route) or fixed obstacles (such as tables and chairs, walls, electric doors, etc.).

If the micro-processing unit 10 determines that it will collide with another obstacle, step S307 is executed. Otherwise, the micro-processing unit 10 executes step S308.

Step S307: The micro-processing unit 10 corrects the dodge vector. In one embodiment, the micro-processing unit 10 corrects the direction of the ducking vector or reduces the magnitude of the ducking vector according to the preset value.

In one embodiment, the micro-processing unit 10 corrects the evasion vector according to the position of the obstacle 48 that may collide, so that the movement of the mechanical limb along the corrected evasion vector will not collide with the obstacle 48.

For example, if the evasion vector before correction is turned to the right by an angle θ1 (such as 30 degrees), the micro-processing unit 10 can reduce the angle to θ1′ (such as 10 degrees) according to the distance from the obstacle 48 as a correction After the dodge vector. Thereby, when the arm body 66 is moved to the right by the rotation angle θ1' to the position of the arm body 66'', collision of the obstacle 44 and the obstacle 48 can be avoided at the same time.

In one embodiment, the micro-processing unit 10 can change the axis of movement of the arm 66, for example, to increase the component of any axis to move the mechanical limb along the corrected evasion vector away from the plane where the obstacle 44 and the obstacle 48 are located. For example, the evasion vector before correction is rotated by the angle θ1 toward the right, and the evasion vector after correction is rotated by the angle θ1' toward the right, and is rotated upward by a specified angle (such as 60 degrees).

It is worth mentioning that there are actually situations where the mechanical limb moves along the corrected evasion vector and still rubs against the obstacle 44. However, the movable obstacle 44 may actively change the direction of movement to avoid the mechanical limb (such as personnel It is found that the impending collision will stop approaching the mechanical limb), and the fixed obstacle 48 will not actively change the direction of movement.

Therefore, according to an embodiment of the present invention, when the movable obstacle 44 and the fixed obstacle 48 are detected at the same time, priority is given to avoiding the fixed obstacle 48 (such as the dodge vector before correction is to avoid the priority). Moving obstacles 44. The corrected dodge vector is the priority to avoid fixed obstacles 48, that is, the movement of the mechanical limb along the corrected dodge vector must not collide with the fixed obstacles 48) The dodge method can effectively reduce the probability of collision .

Step S308: The micro-processing unit 10 generates motor control data corresponding to the evasion vector (the modified evasion vector if step S307 is executed).

In one embodiment, the motor control data includes the rotation angle of each motor 12. The micro-processing unit 10 is a combination of converting the dodge vector into rotation angles of a plurality of motors.

Step S309: The micro-processing unit 10 controls each motor 12 to rotate by a corresponding rotation angle to move the mechanical limb to avoid the obstacle 44. Moreover, if the dodge vector is corrected, the mechanical limb can also avoid collision with the obstacle 48.

In this way, the present invention can avoid collision with other obstacles during the evasion process.

Please refer to FIG. 9A, FIG. 9B, FIG. 10 and FIG. 13 at the same time. FIG. 9A is a schematic diagram of the first dodge strategy of the present invention, FIG. 9B is a schematic diagram of the second dodge strategy of the present invention, and FIG. 13 is a fourth implementation of the present invention Flow chart of an example anti-collision method.

In this embodiment, the mechanical limb includes multiple arm bodies (FIG. 9A and FIG. 9B take two arm bodies 760-761 as an example), and the multiple arm bodies 760-761 pass through multiple movable structures (with two movable structures 750-751 as an example) in series, a plurality of motors 12 are respectively configured to control the movement of a plurality of movable structures 750-751 to move a plurality of arm bodies 760-761.

This embodiment mainly provides a function of planning an evasion strategy, which can plan a group of evasion strategies. When the robot 1 evades an obstacle according to the evasion strategy, it can have a higher probability of success in evasion or less energy consumption (such as multiple arms) 760-761 has the least total movement).

Compared with the anti-collision method shown in FIG. 10, step S13 of the anti-collision method of this embodiment further includes the following steps.

Step S40: The micro-processing unit 10 of the robot 1 obtains the current position (eg position P1) of the last arm (eg arm 761), and determines the dodge position (eg position P2) according to the relative movement direction and the current position.

In one embodiment, the micro-processing unit 10 can estimate the movement trajectory of the obstacle 46 according to the relative moving party, and set the position outside the movement trajectory as the aforementioned dodge position

Step S41: The micro-processing unit 10 plans a dodge strategy according to the current position and the dodge position.

In one embodiment, the memory unit 14 records the movable range of each movable structure 750-751. The microprocessing unit 10 plans a set of evasion strategies according to the movable range of each movable structure 750-751. The evasion strategy includes the rotation vectors of each movable structure 750-751, and corresponds to multiple evasion vectors of multiple arm bodies 760-761. When each movable structure 750-751 rotates along the rotation vector, the connected arm body 760-761 can be moved along the corresponding dodge vector, and the endmost arm body 761 is moved from the current position P1 to the dodge position P2.

In one embodiment, if less energy is to be consumed, the micro-processing unit 10 may be based on the shortest path algorithm, the minimum movement energy algorithm or the shortest movement time algorithm based on the current position P1 (starting point) and the dodge position P2 (end point) To plan a set of evasion strategies.

For example, as shown in FIG. 9A, an evasion strategy that consumes less energy is adopted. For example, only the rotation angle θ3 of the movable structure 751 is controlled to move the endmost arm 761 to the evasion position P2.

In one embodiment, if the probability of evasion success is increased, the micro-processing unit 10 can plan according to the current position P1 (start point) and the evasion position P2 (end point) so that all the moved arms 760-761 are farther away from the obstacle 46 A set of evasion strategies.

For example, as shown in FIG. 9B, an evasion strategy with a higher probability of evasion success is adopted, such as controlling the rotation angle θ4 of the movable structure 750 to move the arm body 760 away from the obstacle 46, and controlling the rotation angle θ5 of the movable structure 751 to cause the arm The body 761 moves away from the obstacle 46 and moves the arm 761 at the end to the dodge position P2. Furthermore, the aforementioned angle sum of the angle θ4 and the angle θ5 may be greater than the angle sum of the angle θ2 and the angle θ3.

Step S42: The micro-processing unit 10 generates motor control data corresponding to the evasion strategy. In one embodiment, the aforementioned motor control data includes the rotation angle of each motor 12.

Specifically, in step S14, if the micro-processing unit 10 controls the rotation of each motor 12 according to a plurality of rotation angles of a plurality of motor control data, each movable structure 750-751 can be rotated by a corresponding angle, so that each arm body 760-761 moves along the corresponding dodge vector, and moves the last arm 761 to the dodge position P2.

In this way, the present invention can provide more diversified evasion methods by determining the evasion strategy according to the demand, and has higher practicability.

Please also refer to FIG. 14 for the flow chart of the anti-collision method according to the fifth embodiment of the present invention.

The anti-collision method of this embodiment further provides an anti-collision method that can perform dodge during the movement of the robot 1. Specifically, the anti-collision method of this embodiment includes the following steps.

Step S50: The microprocessing unit 10 of the robot 1 controls the mechanical limb to move along the movement vector. In one embodiment, the micro-processing unit 10 controls the mechanical limb to move along the movement vector after receiving the movement instruction indicating the movement vector from the remote controller 17 or the computer device 18.

Step S51: During the movement of the mechanical limb, the micro-processing unit 10 senses the electric field change in continuous time via the electric field sensor 11.

Step S52: The micro-processing unit 10 determines whether there is an obstacle in the electric field according to the obtained electric field change.

In one embodiment, the micro-processing unit 10 continues to sense the obstacle through the electric field sensor 11 without interruption after starting, but it is not limited thereto.

In an embodiment, the micro-processing unit 10 only enables the electric field sensor 11 to detect obstacles during the movement of the robot 1, and disables the electric field sensor 11 to stop sensing obstacles when the robot stops moving, In order to reduce power consumption.

If the micro-processing unit 10 detects an obstacle, step S53 is executed. Otherwise, the micro-processing unit 10 executes step S51 again to continuously sense the electric field change.

Step S53: The micro-processing unit 10 determines the relative moving direction of the obstacle according to the change of the electric field.

Step S54: The micro-processing unit 10 corrects the movement vector according to the relative movement direction of the obstacle, and uses the corrected movement vector as the dodge vector. In one embodiment, the end points of the motion vector and the dodge vector are the same, but the paths are different.

In one embodiment, the micro-processing unit 10 determines the dodge vector that can avoid the obstacle according to the relative movement direction of the obstacle, and then combines the dodge vector and the movement vector as the final dodge vector.

Step S55: The micro-processing unit 10 generates motor control data corresponding to the dodge vector.

Step S56: The micro-processing unit 10 controls each motor 12 to rotate according to the motor control data, so that the mechanical limbs evade along the evasion vector. In this way, the present invention can effectively prevent the robot 1 from colliding or damaging the obstacle by detecting and avoiding the obstacle during the movement of the robot 1.

Of course, the present invention can have other various embodiments. Without departing from the spirit and essence of the present invention, persons with ordinary knowledge in the technical field to which the present invention belongs can make various corresponding changes and modifications according to the present invention, but these Corresponding changes and deformations should belong to the scope of the patent application attached to the present invention.

S10-S14‧‧‧‧Protection steps

Claims (13)

An anti-collision method for a robot including a mechanical limb, a plurality of electric field sensors disposed on the mechanical limb, and a motor for moving the mechanical limb, at least two of the electric field sensors are disposed on On the same side of the mechanical limb, the anti-collision method includes the following steps: a) sensing an electric field change of multiple electric fields in continuous time through the electric field sensors, wherein the electric field change includes an energy intensity change and an energy Center change; b) when a first obstacle is detected according to the electric field change, a relative movement direction of the first obstacle is determined; c) a motor control data corresponding to an evasion vector is generated according to the relative movement direction; and d) The motor is controlled to rotate according to the motor control data so that the mechanical limb moves along the evasion vector to evade the first obstacle. The anti-collision method according to claim 1, wherein the step b) includes the following steps: b11) determining a vertical movement direction of the first obstacle according to the energy intensity change; b12) determining the first according to the energy center change A parallel movement direction of the obstacle; and b13) determining the relative movement direction according to the vertical movement direction and the parallel movement direction. The anti-collision method according to claim 2, wherein the step b) further includes the following steps: b14) determining a vertical movement speed of the first obstacle according to a sensing time and the energy intensity change; b15) according to the sense The measured time and the change of the energy center determine a parallel moving speed of the first obstacle; and b16) determine a relative moving speed according to the vertical moving speed and the parallel moving speed. The anti-collision method according to claim 3, wherein the step c) further includes the following steps: c11) determine the direction and magnitude of the dodge vector and a dodge timing according to the relative movement direction and the relative movement speed; and c12) generate the motor control data according to the dodge vector and the dodge timing; wherein, the step d) is According to the motor control data, the motor is controlled to rotate when the evasion timing is met so that the mechanical limb moves the magnitude of the evasion vector in the direction of the evasion vector to evade the first obstacle. The anti-collision method according to claim 1, wherein at least one of the electric field sensors is disposed on one side of the mechanical limb, and at least one of the electric field sensors is disposed on the other side of the mechanical limb; the step b) includes The following steps: b21) when the first obstacle is detected, a first movement direction is determined according to the electric field change sensed by the electric field sensor on one side; b22) based on the electric field sensor on the other side The sensed change in the electric field determines a second moving direction; and b23) the relative moving direction is determined according to the first moving direction and the second moving direction. The anti-collision method according to claim 1, wherein the step c) includes the following steps: c21) determining the dodge vector according to the relative movement direction; c22) predicting that the mechanical limb moving along the dodge vector will collide When there are two obstacles, the dodge vector is corrected according to the position of the second obstacle; and c23) generating the motor control data corresponding to the corrected dodge vector, wherein the motor control data includes a rotation angle; wherein, the step d ) Is to control the motor to rotate the rotation angle. The anti-collision method according to claim 6, wherein the robot includes a memory unit that records an environment map and the position of each second obstacle on the environment map; the step c22) is based on the environment map , The current position of the robot on the environment map and each of the second obstacles When the position of the environment map determines that the mechanical limb moves along the evasion vector and collides with any of the second obstacles, the direction of the evasion vector is corrected or the magnitude of the evasion vector is reduced. The anti-collision method according to claim 6, wherein the step c22) is to predict that the mechanical limb moving along the dodge vector will collide with the second obstacle, the first obstacle is a movable obstacle and the first When the second obstacle is a fixed obstacle, the dodge vector is corrected according to the position of the second obstacle so that the corrected dodge vector is used to preferentially dodge the second obstacle; the step d) is based on the motor control The data controls the motor to rotate so that the mechanical limb does not collide with the second obstacle when moving along the corrected dodge vector. The anti-collision method according to claim 1, wherein the mechanical limb includes a plurality of arm bodies, the plurality of arm bodies are connected in series via a plurality of movable structures, and the plurality of motors are respectively configured to control the movement of the plurality of movable structures to Moving the plurality of arms; the step c) includes the following steps: c31) determining an evasion position based on the relative movement direction and a current position of the arm of the last stage; and c32) based on the current position and the evasion position Planning a dodge strategy corresponding to multiple dodge vectors, and generating the motor control data according to the dodge strategy, wherein the motor control data includes a plurality of rotation angles of the plurality of motors; wherein, step d) is based on the plurality of The motor control data controls each of the motors to rotate corresponding to the rotation angle to move the plurality of arm bodies along the plurality of dodge vectors, respectively, to move the arm body at the last stage to the dodge position. The anti-collision method according to claim 9, wherein the step c32) is to plan the dodge strategy based on the shortest path algorithm, the minimum movement energy algorithm or the shortest movement time algorithm. The anti-collision method of claim 1, wherein the step b) is to detect the first obstacle based on the electric field change during the movement of the mechanical limb; the step c) is based on the relative movement direction Correct a current movement vector of the mechanical limb as the dodge vector, and generate the motor control data corresponding to the dodge vector. The anti-collision method according to claim 1, wherein the step d) is to move the mechanical limb after movement away from an intersection point of the relative movement direction and the mechanical limb before movement. The anti-collision method according to claim 1, wherein the step c) is to generate the motor control data when determining that the first obstacle is approaching the mechanical limb according to the relative movement direction.

Images