CN108613656B - Method for detecting stuck robot based on six-axis gyroscope - Google Patents

Method for detecting stuck robot based on six-axis gyroscope Download PDF

Info

Publication number
CN108613656B
CN108613656B CN201810458442.9A CN201810458442A CN108613656B CN 108613656 B CN108613656 B CN 108613656B CN 201810458442 A CN201810458442 A CN 201810458442A CN 108613656 B CN108613656 B CN 108613656B
Authority
CN
China
Prior art keywords
robot
determining
value
elevation angle
acceleration
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201810458442.9A
Other languages
Chinese (zh)
Other versions
CN108613656A (en
Inventor
李永勇
肖刚军
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Zhuhai Amicro Semiconductor Co Ltd
Original Assignee
Zhuhai Amicro Semiconductor Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Zhuhai Amicro Semiconductor Co Ltd filed Critical Zhuhai Amicro Semiconductor Co Ltd
Priority to CN201810458442.9A priority Critical patent/CN108613656B/en
Publication of CN108613656A publication Critical patent/CN108613656A/en
Application granted granted Critical
Publication of CN108613656B publication Critical patent/CN108613656B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C1/00—Measuring angles
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01M—TESTING STATIC OR DYNAMIC BALANCE OF MACHINES OR STRUCTURES; TESTING OF STRUCTURES OR APPARATUS, NOT OTHERWISE PROVIDED FOR
    • G01M99/00—Subject matter not provided for in other groups of this subclass
    • G01M99/005—Testing of complete machines, e.g. washing-machines or mobile phones

Landscapes

  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Manipulator (AREA)

Abstract

The invention relates to a method for detecting an elevation angle and a stuck angle of a robot based on a six-axis gyroscope. The method for detecting the elevation angle comprises the steps of solving a variance between acceleration in the vertical direction detected by a gyroscope and gravity acceleration, if the obtained variance value is within a preset variance range, indicating that the robot is on a horizontal plane, calibrating the elevation angle of the robot to be zero, otherwise, indicating that the robot is on an inclined plane, and determining the elevation angle of the robot to be an angle value rotating around an X axis detected by a six-axis gyroscope, so that detection errors caused by zero drift or other interference of the gyroscope can be avoided, and the accuracy of the robot in detecting the elevation angle is improved. In addition, the method for detecting the stuck robot accurately obtains the detection result of whether the robot is stuck or not by comparing the elevation angle data recorded in the first preset time period with the reference elevation angle data prestored in the robot and combining the comprehensive judgment of time and the slipping condition.

Description

Method for detecting stuck robot based on six-axis gyroscope
Technical Field
The invention relates to the field of robots, in particular to a method for detecting an elevation angle and a method for detecting a stuck object based on a six-axis gyroscope of a robot.
Background
The six-axis gyroscope can detect angular velocity and acceleration at the same time, is assembled on the sweeping robot and is commonly used for judging postures. The angular velocity detected by the six-axis gyroscope is generally time-integrated to obtain the corresponding rotation angle. However, for various reasons, various disturbance moments are often applied to the gyroscope, and the gyroscope of the gyroscope is precessed under the action of the small disturbance moments, so that the angular momentum is slowly deviated from the original direction, and the phenomenon is called zero drift. The zero drift phenomenon of the gyroscope can affect the detection precision of the sensor. In the sweeper, if the detected pitch angle is deviated, the robot can generate misjudgment, for example, the robot is in a horizontal plane, but can be judged to be on an inclined plane. In addition, in the actual home environment, most homes have a floor electric fan, a doorsill, a bar chair and the like, and in some offices, some chairs have U-shaped stool legs, the stool legs of the chairs contacting the ground are U-shaped stainless steel pipes, and the cross sections of the pipes are oblate. When the sweeping robot is used for sweeping, the sweeping robot is easily clamped by a floor electric fan base, a threshold, a base of a bar counter seat or U-shaped stool legs, so that the robot slips or is clamped and cannot move, the accuracy of the robot for constructing a map is influenced, and the navigation accuracy is greatly reduced. At present, many robots can not identify whether the robot is clamped by a base of a floor electric fan, a threshold, a base of a bar chair or a U-shaped stool foot, some robots can improve the problem in this respect, but detection of sensors such as vision or laser is required to be additionally added, so that the cost of the robot is increased, and the robot is not beneficial to popularization and application.
Disclosure of Invention
In order to solve the problems, the invention provides a method for detecting an elevation angle of a robot based on a six-axis gyroscope and a method for detecting a stuck robot, which can accurately detect the elevation angle of the robot and accurately detect whether the robot is stuck or not on the basis of not increasing hardware cost. The specific technical scheme of the invention is as follows:
a method for detecting a stuck robot based on a six-axis gyroscope comprises the following steps: step 1: according to the method for detecting the elevation angle of the robot based on the six-axis gyroscope, the elevation angle data detected by the robot is recorded, and the step 2 is carried out; step 2: comparing the elevation angle data recorded in a first preset time period with reference elevation angle data stored in the robot in advance, judging whether the recorded elevation angle data is in an error range of the reference elevation angle data, if so, entering a step 3, otherwise, deleting the first recorded data in the elevation angle data recorded in the first preset time period, and returning to the step 1; and step 3: continuously recording the elevation angle data detected by the robot, judging whether the duration time of the recorded elevation angle data in the error range of the reference elevation angle data reaches a second preset time period, if so, entering the step 4, and if not, determining that the robot is not blocked; and 4, step 4: judging whether the robot is detected to slip within the second preset time period or not, if the robot is detected to slip, determining that the robot is clamped, and if the robot is not detected to slip, determining that the robot is not clamped; the method for detecting the elevation angle of the robot based on the six-axis gyroscope comprises the following steps: step S1: determining an acceleration detected by an accelerometer in a vertical direction in a six-axis gyroscope of the robot for a first predetermined period of time, and proceeding to step S2; step S2: judging whether the variance value of the acceleration and the gravity acceleration is within a preset variance range, if so, entering step S3, and if not, entering step S4; step S3: determining that the elevation angle of the robot is zero; step S4: determining the angle value rotating around the X axis detected by the six-axis gyroscope as the elevation angle of the robot; the robot is characterized in that the left-right direction is an X axis, the front-back direction is a Y axis, the up-down direction is a Z axis, and the X axis, the Y axis and the Z axis form a three-dimensional coordinate system of the six-axis gyroscope.
Further, the recording of the elevation angle data detected by the robot in step 1 includes the following steps: recording an elevation angle every a third preset time period; and determining a connecting line formed by the elevation angles recorded by the robot in a coordinate system with the elevation angle as a Y axis and the recording time as an X axis.
Further, the step 2 of comparing the elevation angle data recorded in the first preset time period with the reference elevation angle data pre-stored in the robot and determining whether the recorded elevation angle data is within the error range of the reference elevation angle data includes the following steps: step 21: acquiring a connecting line formed by elevation angles in a first preset time period recorded by the robot at present, determining the change condition of the connecting line, and entering step 22; step 22: acquiring a reference line formed by reference elevation angle data detected in the first preset time period when the robot prestored in the robot walks to different inclined planes, and entering step 23; step 23: judging whether the change condition of the connecting line recorded currently is within the error range of the change condition of the reference line, if so, determining that the recorded elevation angle data is within the error range of the reference elevation angle data, and if not, determining that the recorded elevation angle data is not within the error range of the reference elevation angle data.
Further, the step 4 of determining whether the robot is detected to slip within the second preset time period includes the following steps: step 41: determining a first current travel distance traveled by a first driving wheel of the two driving wheels detected at the current recording time point and a first previous travel distance traveled by the first driving wheel detected at the last recording time point based on data detected by code wheels on the two driving wheels of the robot, and calculating the first distance traveled by the first driving wheel to be a difference value of the first current travel distance and the first previous travel distance; determining a second current travel distance traveled by a second driving wheel of the two driving wheels detected at the current recording time point and a second previous travel distance traveled by the second driving wheel detected at the last recording time point, and calculating to obtain the second distance traveled by the second driving wheel as a difference value of the second current travel distance and the second previous travel distance; determining a travel distance difference as a difference of the first distance and the second distance; determining the travel angle value of the two driving wheels in a first preset time as the ratio of the travel distance difference value to the width between the two driving wheels; determining a first angle change rate generated by the two driving wheels within a first preset time as a ratio of the travel angle value to the first preset time, and proceeding to step 42; step 42: determining a maximum error rate for both of said driven wheels, determining a maximum error value for said first rate of angular change as a product of said first rate of angular change and said maximum error rate, and proceeding to step 43; step 43: determining a current angle detected by the six-axis gyroscope at the current recording time point and an upper angle detected by the six-axis gyroscope at the previous recording time point based on data detected by the six-axis gyroscope of the robot, calculating to obtain a difference value of a change angle between the current angle and the upper angle, determining a second angle change rate generated by the six-axis gyroscope within the first preset time as a ratio of the change angle to the first preset time, and proceeding to step S44; step 44: determining the difference value between the first angle change rate and the second angle change rate as a first difference value, and entering step 45; step 45: determining an angular velocity variation error rate as a ratio of the first difference value to the maximum error value, and proceeding to step 46; step 46: and judging whether the angular speed change error rate is greater than or equal to a preset value within the second preset time period, if the angular speed change error rates determined for N times are greater than or equal to the preset value, determining that the robot slips, and if not, determining that the robot does not slip. The maximum error rate is obtained through experimental tests, and the first preset time is a time interval between the current recording time point and the last recording time point.
Further, the step S1 specifically includes the following steps: analyzing the acceleration detected by an accelerometer in the vertical direction in a six-axis gyroscope of the robot in a first preset time period, if the difference value between the currently analyzed acceleration and the previous acceleration is larger than a preset acceleration value, and the difference value between the currently analyzed acceleration and the next acceleration is also larger than the preset acceleration value, determining the currently analyzed acceleration as interference data, and deleting the interference data, otherwise, keeping the currently analyzed acceleration, and determining the kept acceleration as the acceleration detected by the accelerometer in the vertical direction in the six-axis gyroscope of the robot in the first preset time period.
Further, the step S2 of determining whether the variance value of the acceleration and the gravitational acceleration is within a preset variance range specifically includes the following steps: determining the difference value between the acceleration and the gravity acceleration as an acceleration difference value; determining the average value of the sum of squares of the acceleration difference values as an acceleration average value; determining the arithmetic square root of the acceleration average value as a variance value; determining a preset variance maximum value and a preset variance minimum value based on a preset variance range configured by the system; and comparing the variance value with the preset variance maximum value and the preset variance minimum value respectively, if the variance value is larger than the preset variance minimum value and smaller than the preset variance maximum value, determining that the variance value of the acceleration and the gravitational acceleration is within a preset variance range, otherwise, determining that the variance value of the acceleration and the gravitational acceleration is not within the preset variance range.
Further, the step S4 specifically includes the following steps: step S41: determining the angular speed of the robot rotating around the X axis, which is detected by a six-axis gyroscope; step S42: performing time integration on the angular speed to obtain an X angle of the robot rotating around an X axis; step S43: determining a zero drift value of the six-axis gyroscope; step S44: and determining the elevation angle of the robot as the difference value of the X angle and the zero drift value.
Further, the step S43 specifically includes the following steps: when the robot is in a static state, an angular value obtained by time integration of angular velocities, detected by a six-axis gyroscope of the robot, rotating around an X axis is used as a zero drift value of the six-axis gyroscope. The static state refers to a state before the robot is started or a state when the robot stops in the moving process, and the state needs to last for a second predetermined time period.
The method for detecting the elevation angle of the robot based on the six-axis gyroscope comprises the steps of solving a variance between the acceleration in the vertical direction detected by the gyroscope and the gravity acceleration, if the obtained variance value is within a preset variance range, indicating that the robot is on the horizontal plane, calibrating the elevation angle of the robot to be zero, and if the obtained variance value is not within the preset variance range, indicating that the robot is on an inclined plane, determining the elevation angle of the robot to be an angle value rotating around an X axis detected by the six-axis gyroscope, so that detection errors caused by zero drift or other interference of the gyroscope can be avoided, the accuracy of detecting the elevation angle by the robot is improved, and the probability of misjudgment is reduced. In addition, the robot provided by the invention is based on the method for detecting the stuck robot by the six-axis gyroscope, and the detection result of whether the robot is stuck or not is accurately obtained by comparing the elevation angle data recorded in the first preset time period with the reference elevation angle data prestored in the robot and combining the comprehensive judgment of time and the slipping condition.
Drawings
Fig. 1 is a schematic flow chart of a method for detecting an elevation angle of the robot based on a six-axis gyroscope.
Fig. 2 is a schematic orientation diagram of a six-axis gyroscope.
Fig. 3 is a schematic flow chart of the method for detecting the stuck object based on the six-axis gyroscope of the robot.
FIG. 4 is a schematic diagram of an analysis of the travel angle value according to the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be described in detail below with reference to the accompanying drawings in the embodiments of the present invention. It should be understood that the following specific examples are illustrative only and are not intended to limit the invention.
The robot of the present invention is an intelligent cleaning robot (such as a floor sweeping robot or a floor mopping robot), and the robots mentioned in the following embodiments refer to an intelligent cleaning robot. The robots can automatically walk on certain occasions by means of certain artificial intelligence. Various sensors are arranged on the robot body, and can detect the walking distance, the walking angle (namely the advancing direction), the body state, obstacles and the like, if the robot body touches a wall or other obstacles, the robot body can turn automatically, and can walk along different routes according to different settings, and can also construct a grid map according to various data detected in the walking process, for example, a grid unit corresponding to the detected obstacle is marked as an obstacle unit, a grid unit corresponding to the detected cliff is marked as a cliff unit, and a grid unit passing through the normal walking is marked as a passed unit and the like. The robot of the invention comprises the following structures: the robot body capable of walking independently is provided with a left driving wheel and a right driving wheel, a human-computer interaction interface is arranged on the robot body, and an obstacle detection unit is arranged on the robot body. The body is internally provided with an inertial sensor, the inertial sensor comprises an accelerometer, a six-axis gyroscope and the like, the two driving wheels are respectively provided with a speedometer (generally a coded disc) for detecting the walking distance of the driving wheels, and the inertial sensor is also provided with a control module which can process parameters of related sensors and can output control signals to an execution part. The six-axis gyroscope can detect the yaw angle, the pitch angle and the roll angle of the robot and can also detect the acceleration of the robot in the front-back direction, the left-right direction and the up-down direction.
As shown in fig. 1, a method for detecting an elevation angle of a robot based on a six-axis gyroscope includes the following steps: in step S1, the robot first determines the acceleration detected by the accelerometer in the vertical direction (i.e., the up-down direction of the robot) in the six-axis gyroscope for a first predetermined period of time, and proceeds to step S2. In step S2, the robot further determines whether the variance value between the acceleration and the gravitational acceleration is within a preset variance range. The gravity acceleration refers to an acceleration of an object under the action of gravity. The first predetermined time period may be set according to specific product design requirements, and preferably may be set to 100 milliseconds. The preset variance range is obtained through a large number of experimental tests, has accurate reference value, and is preferably in a range of 0 to 300. When the variance value is judged to be within the preset variance range, the robot is shown to be in the horizontal plane, and the elevation angle of the robot can be calibrated to be zero. If the variance value is not within the preset variance range, the robot is indicated to be on the inclined plane, and the angle value rotating around the X axis and detected by the six-axis gyroscope can be determined as the elevation angle of the robot. Therefore, detection errors caused by zero drift or other interferences of the gyroscope can be avoided, the accuracy of the robot in detecting the elevation angle is improved, and the probability of misjudgment is reduced. As shown in fig. 2, the six-axis gyroscope MPU-6000 or MUP-6050 is mounted in the robot, and the X-axis direction is the left-right direction of the robot, the Y-axis direction is the front-back direction of the robot, and the Z-axis direction is the up-down direction of the robot. The angle of rotation around the X axis along the + X direction is the elevation angle of the robot. The X-axis, the Y-axis and the Z-axis constitute a three-dimensional coordinate system of the six-axis gyroscope. According to the method, the existing six-axis gyroscope in the robot is used for detecting and determining the elevation angle of the robot, other auxiliary detection devices are not needed, and the hardware cost of the robot can be further reduced. Meanwhile, the accuracy of the six-axis gyroscope detection data of the robot can be improved through the self-correcting function of the six-axis gyroscope, and reliable reference data are provided for subsequent data analysis.
As one embodiment, the step S1 specifically includes the following steps: analyzing the acceleration detected by an accelerometer in the vertical direction in a six-axis gyroscope of the robot in a first preset time period, if the difference value between the currently analyzed acceleration and the previous acceleration is larger than a preset acceleration value, and the difference value between the currently analyzed acceleration and the next acceleration is also larger than the preset acceleration value, determining the currently analyzed acceleration as interference data, and deleting the interference data, otherwise, keeping the currently analyzed acceleration, and determining the kept acceleration as the acceleration detected by the accelerometer in the vertical direction in the six-axis gyroscope of the robot in the first preset time period. The preset acceleration value can be correspondingly set according to specific product design requirements, and preferably can be set to 1000 LSB/g. The difference is a numerical value after taking an absolute value. According to the method, the collected acceleration data are subjected to filtering processing, and some interference data are deleted, so that error influence caused by external interference can be effectively avoided, and the accuracy of robot analysis data is guaranteed.
As one embodiment, the step of determining whether the variance value of the acceleration and the gravitational acceleration is within the preset variance range in step S2 specifically includes the following steps: first, it is determined that the detected acceleration is G1, G2, G3, G4 … … GN, N being a natural number greater than 0. And determining that the gravity acceleration is G, and G is the gravity acceleration of the earth, and keeping the numerical value unchanged. The difference value of the two is the acceleration difference value, namely G1-G, G2-G, G3-G, G4-G … … GN-G. Then, the average of the sum of squares of the acceleration difference is determined as the acceleration average ((G1-G)2+(G2-G)2+(G3-G)2+(G4-G)2+……(GN-G)2) and/N. Then, the square root of the arithmetic mean value of the acceleration is determined as a variance value, i.e. a pair ((G1-G)2+(G2-G)2+(G3-G)2+(G4-G)2+……(GN-G)2) And performing square division on the/N to obtain a variance value. And then, determining a preset variance maximum value and a preset variance minimum value based on a preset variance range configured by the system, wherein the preset variance maximum value and the preset variance minimum value are summarized through a large amount of researches and experiments, and are configured in the robot system in advance during product design. Finally, the variance value is compared with the preset variance maximum value and the preset variance minimum value respectively, and if the variance value is larger than the preset variance minimum value and smaller than the preset variance maximum value, the acceleration and the gravity acceleration are determinedAnd if the variance value is within the preset variance range, the robot is positioned on the horizontal plane at the moment, otherwise, the variance value of the acceleration and the gravity acceleration is determined not to be within the preset variance range, and the robot is positioned on the inclined plane at the moment. According to the method, variance processing is carried out on the detected acceleration data, the discrete degree of the acceleration of the robot in the vertical direction can be accurately obtained, so that whether the robot is on the horizontal plane or the inclined plane can be accurately judged, and an accurate and effective reference basis is provided for subsequently judging whether the robot is clamped or not.
As one embodiment, the step S4 specifically includes the following steps: first, in step S41, the robot determines the angular velocity of rotation about the X axis (i.e., the angular velocity of rotation in the + X direction in fig. 2) detected by the six-axis gyroscope; then, step S42 is carried out, and the angular velocity is subjected to time integration to obtain an X angle of the robot rotating around the X axis; then, the process goes to step S43 to determine the zero drift value of the six-axis gyroscope; and finally, in step S44, determining the elevation angle of the robot as the difference between the X angle and the zero drift value. According to the method, the error caused by zero drift is subtracted from the angle of rotation around the X axis detected by the six-axis gyroscope, so that the detection accuracy of the gyroscope is improved.
As one embodiment, the step S43 specifically includes the following steps: when the robot is in a static state, an angular value obtained by time integration of angular velocities, detected by a six-axis gyroscope of the robot, rotating around an X axis is used as a zero drift value of the six-axis gyroscope. The static state refers to a state before the robot is started or a state when the robot stops in the moving process, and the state needs to last for a second predetermined time period. The second predetermined time period may be set according to specific design requirements, and preferably, may be set to any value from 0.5 seconds to 2 seconds. In the method of this embodiment, the robot determines the zero drift value through the angle change detected by the six-axis gyroscope in the stationary state, and can accurately calibrate the gyroscope.
An exemplary description of specific data is provided based on the method for detecting the elevation angle of the robot based on the six-axis gyroscope according to the above embodiment. Assuming that the robot reads the acceleration once every 10 milliseconds, 100 milliseconds (first predetermined period) can read 10 pieces of acceleration data, specifically 16400, 16388, 16353, 16215, 16425, 16363, 20000, 16500, 15000, and 16345, in LSB/g. The stack of data is analyzed and filtered, and since the seventh data 20000 is significantly larger than the previous data 16363 and much larger than the next data 16500, it is considered as a cause of interference and the interference data needs to be filtered out. The second to last data 15000 is much smaller than the preceding and following data, and is also interference data, which needs to be filtered out, and it should be noted that the difference between 15000 and 16500 and 16345 respectively is a value obtained by taking the absolute value, and is 1500 and 1345. The standard for data filtering is that if the difference between the contrast data exceeds 1000LSB/g, the contrast data is considered as interference data. Next, the filtered data is subtracted from data 16384 corresponding to the gravitational acceleration to obtain 16, 4, 31, 169, 41, 21, 116, and 39, square summation is performed to obtain 48693, and finally averaging and squaring the average are performed to obtain a variance value of 76. Since 76 is within the preset variance range (0-300), it can be determined that the robot is now in the horizontal plane and the elevation angle is 0. If the obtained variance value is not in the range of 0-300, it can be determined that the robot is on the inclined plane at the moment, and the elevation angle is the angle of rotation around the X axis in the + X direction detected by the six-axis gyroscope.
As shown in fig. 3, a method for detecting a stuck object based on a six-axis gyroscope by a robot specifically includes the following steps: in step S1, according to the method for detecting an elevation angle by a robot according to the above-described embodiment, the elevation angle data detected by the robot is recorded first. The timing of recording the elevation angle data may be real-time recording (i.e., every millisecond), or may be once every certain time, such as 10 milliseconds, 20 milliseconds, or 30 milliseconds. The robot performs a recording operation while walking, and the process proceeds to step S2.
In step S2, the robot compares the elevation angle data recorded in the first preset time period with the reference elevation angle data pre-stored in the robot, and the comparison may be performed directly in a point-to-point data comparison manner or in a data line manner to perform an overall trend comparison. The first preset time period may be set according to specific design requirements, for example, may be set to 0.5 second or 1 second. The pre-stored reference elevation angle data is some pre-stored reference data about elevation angles in the robot system, and the reference data can be obtained through experimental tests and stored in the robot system before the robot leaves a factory, or can be pre-run by the robot on a corresponding object (such as a base of a bar chair, a base of a floor electric fan, a threshold or a U-shaped stool foot and the like) after being purchased by a user and combined with the use environment of the user's home, and is recorded and stored in the system by the robot. According to the comparison result, when the recorded elevation angle data is judged not to be in the error range of the reference elevation angle data, the robot is shown to be not located on the object corresponding to the stored reference elevation angle data, namely the robot is not located on any inclined plane or transverse sill which can cause the robot to be clamped currently, and therefore the robot continues to walk normally and records the detected elevation angle data. If the robot travels for more than a first preset time period, deleting the first recorded data from the elevation data recorded in the first preset time period, recording the newly detected data as the last elevation data, and returning to step S1. For example, the robot records data once every 10 milliseconds, 100 data can be recorded in a time period of 1 second, the robot starts to walk until the time period of 1 second elapses, 100 data are already recorded, and after that, the data recorded in the previous 10 th millisecond is deleted from 1 second later, and other data correspondingly moves forward, that is, the data recorded in the 20 th millisecond becomes the 1 st recorded data, the data recorded in the 30 th millisecond becomes the 2 nd recorded data, and so on, the data recorded in the 1 st second becomes the 99 th data, and the data recorded in the 1.01 th second becomes the 100 th data. In this way, the robot performs rolling update of recorded data while performing walking detection, and performs comparison between the recorded data and stored data in real time. When the recorded elevation data is determined to be within the error range of the reference elevation data, which indicates that the robot may be currently located on a slope or a threshold that may cause the robot to be stuck, the process proceeds to step S3 for further determination. Wherein the error range is any one of ± 3% to ± 5%, for example, when ± 4% is selected, if one value of the reference elevation angle data is 0.06 °, the robot detected angle is any one of 0.0576 ° to 0.0624 °, and the detected angle value is considered to be within the error range.
In step S3, the robot continuously records the detected elevation angle data, and determines whether the duration of the recorded elevation angle data within the error range of the reference elevation angle data reaches a second preset time period, where the second preset time period may be set according to specific design requirements, and may be set to any time value greater than or equal to 1 second and less than or equal to 2 seconds. If the recorded elevation data does not last within the error range of the reference elevation data for the second predetermined period of time, indicating that the robot may be on another object with a similar slope and that the object does not cause the robot to slip or get stuck, the robot may pass smoothly over the indication of the object and, therefore, it may be determined that the robot is not stuck. If the duration of the recorded elevation data within the error range of the reference elevation data reaches the second preset time period, which indicates that the robot is likely to be stuck on the corresponding object, the process proceeds to step S4 for further determination.
In step S4, the robot determines whether a robot slip is detected within the second preset time period, wherein the robot slip may be detected in different manners, such as a manner of comparing the detection data of the six-axis gyroscope with the detection data of the code wheel, or a manner of comparing the detection data of the light flow sensor with the detection data of the acceleration sensor, and so on. If the robot is detected to slip, the robot can be accurately determined to be stuck by the object based on the double judgment standard that the robot slips on the object corresponding to the stored reference elevation angle data. If no robot slip is detected, indicating that the robot is likely to simply walk on an object with an approximate surface slope, it is determined that the robot is not stuck.
According to the detection method for the robot being stuck, the elevation angle data recorded in the first preset time period are compared with the reference elevation angle data stored in the robot in advance, and the comprehensive judgment of time and the slipping condition is combined, so that the detection result of whether the robot is stuck or not is accurately obtained, and the positioning and navigation accuracy of the robot is improved.
As one embodiment, the recording of the elevation angle data detected by the robot in step 1 includes the following steps: firstly, the robot records an elevation angle every a third preset time period, wherein the third preset time period may be set according to specific design requirements, preferably, the third preset time period may be set to any value between 10 milliseconds and 30 milliseconds, in this embodiment, the third preset time period is set to 20 milliseconds, that is, after the robot starts to start, the currently detected elevation angle is recorded every 20 milliseconds, the elevation angle may directly adopt a detection value of the sensor, or may convert the value detected by the sensor and an actual size of the robot to obtain a robot front end lifting height. Then, the robot determines a connection line formed by the elevation angles recorded by the robot in a coordinate system with the elevation angle as a Y axis and the recording time as an X axis, and the angle change rule of the elevation angle of the robot along with the increase of the walking time can be seen through the connection line in the coordinate system. The method of the embodiment is beneficial to determining the change condition of the elevation angle of the robot along with time in an analysis mode of a coordinate system.
As one embodiment, the step 2 of comparing the elevation angle data recorded in the first preset time period with the reference elevation angle data pre-stored in the robot and determining whether the recorded elevation angle data is within the error range of the reference elevation angle data includes the following steps: in step 21, a connection line formed by elevation angles within a first preset time period currently recorded by the robot is acquired, the change condition of the connection line is determined, and then the process goes to step 22. Preferably, the value of the first preset time period is set to 1 second or 1.5 seconds, if the set value is too small, the accuracy of the sensor cannot meet the requirement, and if the set value is too large, the detection timeliness is easily reduced. In step 22, a reference line formed by the reference elevation angle data detected in the first preset time period when the robot pre-stored in the robot walks to different slopes is obtained, and then the process proceeds to step 23. The reference line pre-stored is recorded and stored by a user when the robot runs on an object such as a floor fan base, a threshold, a base of a bar chair or a U-shaped stool foot in the home in advance, preferably, the number of times of running on the same object in advance is 10, and the average value is selected as finally stored data, so that the accuracy and the reference value of the reference data can be improved. In step 23, it is determined whether the change condition of the connecting line recorded at present is within an error range of the change condition of the reference line, where the error range may also be set according to a specific design requirement, and the error range is set to ± 5% in this embodiment. If the variation of the connecting line recorded currently is within ± 5% of the variation of the reference line, it is determined that the recorded elevation data is within the error range of the reference elevation data, indicating that the robot may be on the object corresponding to the reference line, and then the process proceeds to step S3 for subsequent operations. And if the change condition of the connecting line recorded at present is not within the range of +/-5% of the change condition of the reference line, determining that the recorded elevation angle data is not within the error range of the reference elevation angle data, indicating that the robot is not on the object corresponding to the reference line, detecting the elevation angle data while the robot continues to walk, and continuously performing rolling updating on the recorded data. According to the method, the data are compared and analyzed according to the linear change rule, and the robot can obtain the analysis result more quickly and accurately.
To illustrate with some specific data, assuming a first preset time period of 1 second, the robot records elevation angles every 20 milliseconds, and then 50 elevation angles have been recorded by the robot from the start of startup to 1 second later. Since the first 500 milliseconds the robot has been walking on a horizontal plane, the detected elevation angle is always 0 ° (not considering the six-axis gyroscope drift error, etc.). When 520 milliseconds, the elevation angle detected by the robot becomes 0.01 °; at 540 milliseconds, the elevation angle detected by the robot becomes 0.02 °; 560 milliseconds, the elevation angle detected by the robot becomes 0.03 °; at 580 milliseconds, the elevation angle detected by the robot becomes 0.04 °; at 600 milliseconds, the elevation angle detected by the robot becomes 0.05 degrees; at 620 milliseconds, the elevation angle detected by the robot becomes 0.06 degrees; the elevation angle detected by the robot is maintained at 0.06 degree in the time of 640 milliseconds to 1 second; indicating that the robot climbed a surface with a bevel angle of 0.06 deg. from the horizontal. By sequentially connecting the elevation angles with time, the change of the connecting line can be seen as follows: a horizontal straight line is formed in the range of 0 to 500 milliseconds, a horizontal straight line is formed in the range of 640 to 1 second, and a slope line changing from 0 ° to 0.06 ° is formed between 500 to 640 milliseconds. Similarly, if the variation of the reference line prestored in the robot is the same or has only a slight deviation within some error range, such as a variation from-0.003 ° to 0.057 ° or a variation from 0.003 ° to 0.063 ° in 6 consecutive data, and the connection line of the data before and after the 6 consecutive data maintains a straight line state, the surface on which the robot is located can be regarded as the surface of the object corresponding to the reference line.
As one embodiment, the determining in step 4 whether the robot is detected to slip within the second preset time period includes the following steps: in step 41, first, a first current travel distance traveled by a first driving wheel of the two driving wheels detected at a current recording time point and a first previous travel distance traveled by the first driving wheel detected at a previous recording time point are determined based on data detected by code wheels on the two driving wheels of the robot, and the first distance traveled by the first driving wheel is calculated as a difference value between the first current travel distance and the first previous travel distance. Then, a second current travel distance traveled by a second drive wheel of the two drive wheels detected at the current recording time point and a second previous travel distance traveled by the second drive wheel detected at the previous recording time point are determined, and the second distance traveled by the second drive wheel is calculated as a difference between the second current travel distance and the second previous travel distance. Then, a travel distance difference is determined as a difference between the first distance and the second distance. And determining the travel angle value of the two driving wheels in the first preset time as the ratio of the travel distance difference value to the width between the two driving wheels. Finally, the first angle change rate generated by the two driving wheels within the first preset time is determined as the ratio of the travel angle value to the first preset time, and the process goes to step 42. The first preset time is a time interval between the current recording time point and the last recording time point. The first preset time may be set according to specific design requirements, and is preferably set to 10 milliseconds. The step can obtain the difference value of the travelling distances of the two driving wheels in different time periods by analyzing and comparing the travelling distance detected at each recording time point, provide calculation data for the angle change rate in different time periods, and ensure the accuracy of the subsequent calculation of the angle change rate. As shown in fig. 2, if the robot slips, the traveling distances of the two driving wheels may be different (for example, one driving wheel idles, the other actually rotates, or the friction force between the two driving wheels and the ground is different, which may cause the wheels to slip and rotate for different numbers of turns, i.e., the traveling distances of the two driving wheels are different), so that the robot may generate a slight deflection, and if the body of the robot is stuck, although the robot does not move, the data generated by the encoder may be mistaken by the robot as generating a slight arc-shaped traveling track. For convenience of explanation, as shown in fig. 4, the travel locus of the first and second drive wheels will be expressed in the form of a straight line, and the error generated thereby is within a predictable range. In fig. 4, a first distance that the code wheel detects that the first driving wheel travels within a first preset time T is L, and a second distance that the code wheel detects that the second driving wheel travels within the first preset time is R, so that after receiving detection data of the code wheel, the processor calculates that a difference value between the travel distances of the two driving wheels within the first preset time is Δ L. Since the width between the two driving wheels is W, the value of the travel angle of the two driving wheels in the first preset time is calculated to be a, and a is Δ L/W. Finally, the first angular rate of change is calculated as P, a/T Δ L/(W × T). By the small-angle detection method, the angle change rate within each first preset time is obtained, the subsequent calculation of the angle error change rate is facilitated, and the accuracy of finally judging whether the robot slips can be improved.
In step 42, a maximum error rate for both of the drive wheels is determined, a maximum error value for the first rate of angular change is determined as a product of the first rate of angular change and the maximum error rate, and the process proceeds to step 43. Since the two driving wheels have errors in their physical structures, and the error rates generated for the same physical structure are very close to each other, the maximum error rate can be obtained through experimental tests, and the maximum error rate can be obtained by averaging the tested data. The maximum error rate is introduced to determine the maximum error value of the first angle change rate, so that accurate basis can be provided for subsequent data processing, the condition of misjudgment caused by directly referring to data with errors is avoided, and the accuracy of judging whether the robot slips is improved.
In step 43, first, based on the data detected by the six-axis gyroscope of the robot, the current angle detected by the six-axis gyroscope at the current recording time point and the previous angle detected by the six-axis gyroscope at the previous recording time point are determined. Then, the change angle is calculated to be the difference value between the current angle and the previous angle. Finally, the second angle change rate generated by the six-axis gyroscope within the first preset time is determined as the ratio of the change angle and the first preset time, and the process goes to step 44. Because the six-axis gyroscope has higher accuracy in the aspect of angle detection, the angle change rate calculated by utilizing the detection data of the six-axis gyroscope arranged in the robot is higher in accuracy. Meanwhile, data detection is carried out through the corresponding recording time points, accurate data comparison can be carried out, calculation errors of subsequent angular speed change error rates caused by errors of the compared data are avoided, and accuracy of robot slip judgment is guaranteed.
In step 44, the robot calculates a difference between the first angle change rate and the second angle change rate to obtain a first difference, and proceeds to step 45.
In step 45, the robot calculates the ratio of the first difference to the maximum error value to obtain an error rate of angular velocity change, and proceeds to step 46. Because the first angle change rates at each time are different, the obtained maximum error value is different, namely the maximum error value at each time is dynamically changed, and if the fixed absolute value is adopted as the reference for judgment, the obtained result has larger error. The step adopts a comparison mode, and judgment is carried out according to the comparison ratio, so that a more accurate result can be obtained.
In step 46, the robot determines whether the robot slips by determining whether the angular velocity change error rate is greater than or equal to a preset value during the second preset time period. And if the error rate of the angular speed change determined for N times is larger than or equal to a preset value, determining that the robot slips, otherwise, determining that the robot does not slip. The preset value is 1, so that the relation between the first difference value and the maximum error value can be accurately defined, and whether the robot is in a slipping state or not can be effectively judged according to the comparison result. Because the condition of the road surface on which the robot travels is complex, and different road surface conditions have different influences on the detection result of the robot, whether the robot slips or not is determined only by once detection and judgment of data, and the condition of misjudgment can exist. The detection should be performed continuously for a plurality of times, and the detection results of the plurality of times are analyzed, and only if the detection results are satisfied, whether the skid occurs can be determined, so that the obtained result has higher accuracy. The N may be set according to specific conditions, preferably, the N is set to be a natural number greater than or equal to 2, and optimally, the N is set to be 5 times, so that the effect of inaccuracy cannot be achieved if the N is too small, and the N is too much and the calculation resources are wasted. Wherein the maximum error rate is determined by experimental testing.
In the slip detection method of this embodiment, a first angle change rate generated by two driving wheels within a first preset time is detected and calculated by using a code wheel on the existing driving wheels of the robot, a six-axis gyroscope and a processor in the body, and a second angle change rate generated by the six-axis gyroscope within the same first preset time is determined, so as to determine an error rate of angular velocity change of the robot, and finally, whether the robot slips is determined by determining whether the error rate of angular velocity change is greater than or equal to a preset value. The method for detecting the robot slip is relatively low in cost. Meanwhile, the mode of detecting and judging by combining the code disc and the six-axis gyroscope is adopted, so that the accuracy is higher. In addition, when the robot is detected to be slipping, slipping data can be recorded, and the traveling data of the robot can be corrected, so that the influence of slipping on the traveling accuracy of the robot is avoided.
A control method for robot card release is based on the detection method for robot card clamping described in the above embodiment, and when the robot is determined to be clamped, the control method for robot card release is started, which specifically includes the following steps:
in step T0, when it is determined that the robot is stuck based on the above-described method for detecting a robot stuck, an object corresponding to the corresponding reference elevation angle data is determined. If the object is an object that the robot can cross, such as a threshold or a U-shaped stool leg, the process proceeds to step T10. If the object is an object that the robot cannot cross, such as a base of a floor standing fan or a base of a bar chair, etc., step T20 is entered.
In step T10, the robot controls the two driving wheels to simultaneously operate backward, performs the backward operation for disengaging the card, and determines whether the elevation angle detected by the robot is smaller than the set angle within the set distance of the driving wheels. The set distance can be set according to specific design requirements, the robot can stop the card backing and releasing operation at a proper time by setting the set distance, the problem of low card releasing efficiency caused by the fact that the robot carries out the card backing and releasing operation endlessly is avoided, and the robot may be clamped not firmly at the moment and can release the card slightly after backing a little, so that the set distance is preferably set to be 5 cm. The set angle can also be correspondingly set according to specific design requirements, and the robot can timely detect whether the robot returns to the horizontal plane or not by setting the set angle, so that error accumulation caused by continuous retreating after the robot is out of the card is avoided, and the set angle is preferably set to be 0.01 degrees. If the driving wheel does not run for more than 5 cm and the robot detects that the elevation angle is less than 0.01 degrees, the robot is indicated to retreat from the inclined plane of the object to the horizontal plane, and the robot can be determined to be free from the object and the card removal is successful. If the driving wheel runs for more than 5 cm, but the elevation angle detected by the robot is not less than 0.01 degrees all the time, the robot still clamps on the inclined surface of the object, and the step T11 is required to be carried out for further releasing the card.
In step T11, the robot controls the first driving wheel to move backward and the second driving wheel to move forward, performs a rotation and jam-releasing operation, and determines whether the elevation angle detected by the robot is smaller than a set angle within a first set time. The first set time can be correspondingly set according to specific design requirements, the time for the robot to rotate and release the card can be controlled by setting the first set time, the problem of low card release efficiency caused by the fact that the robot rotates and releases the card uninterruptedly is avoided, and the first set time is preferably set to be 10 seconds. And if the time for the robot to carry out the rotation card-releasing operation does not exceed 10 seconds and the robot detects that the elevation angle is less than 0.01 degrees, determining that the robot is successful in card-releasing. If the time for the robot to perform the rotation card-disengaging operation exceeds 10 seconds and the robot does not detect that the elevation angle is less than 0.01 °, indicating that the robot still gets stuck on the inclined plane of the object, the step T12 is required to perform further card-disengaging operation.
In step T12, the robot controls the second driving wheel to rotate backward and the first driving wheel to rotate forward, performs reverse rotation and card releasing operation, and determines whether the elevation angle detected by the robot is smaller than a set angle within a first set time. And if the time of the reverse rotation card-releasing operation of the robot does not exceed 10 seconds and the robot detects that the elevation angle is less than 0.01 degrees, determining that the card-releasing operation of the robot is successful. If the time for the robot to perform the reverse rotation card-disengaging operation exceeds 10 seconds and the robot does not detect that the elevation angle is less than 0.01 °, indicating that the robot still gets stuck on the inclined surface of the object, the step T13 is required to perform further card-disengaging operation.
In step T13, the robot controls the two driving wheels to simultaneously move forward to perform forward card-releasing operation, and determines whether the elevation angle detected by the robot is smaller than a set angle within a second set time, wherein the second set time can be set according to specific design requirements, and by setting the second set time, the time for the robot to perform forward card-releasing operation can be controlled, so as to avoid the problem of low card-releasing efficiency caused by the robot performing forward card-releasing operation endlessly, and preferably set to 5 seconds. And if the time of the robot for the forward card-releasing operation does not exceed 5 seconds and the robot detects that the elevation angle is less than 0.01 degrees, determining that the robot is successful in card-releasing. If the time for the robot to perform the forward card-disengaging operation exceeds 5 seconds and the robot does not detect that the elevation angle is less than 0.01 degrees, the robot still gets stuck on the inclined plane of the object, and the step T14 needs to be performed for further card-disengaging operation.
In step T14, the robot controls the two driving wheels to simultaneously operate backward, performs the backward jam clearing operation again, and determines whether the elevation angle detected by the robot is smaller than the set angle within the second set time. The time for the back card-releasing operation of this step is longer than that for the 5 cm limit back card-releasing operation in step T10. Mainly because the robot has performed long rotation and forward operations in the previous steps, and still has no card-off, it is necessary to try a relatively long backward card-off operation, so that the robot performs full attempts in all directions. And if the time of the robot for the backward card-releasing operation does not exceed 5 seconds and the robot detects that the elevation angle is less than 0.01 degrees, determining that the robot is successful in card-releasing. If the time for the robot to perform the backward card-disengaging operation exceeds 5 seconds and the robot does not detect that the elevation angle is less than 0.01 degrees, the robot still gets stuck on the inclined plane of the object, and the step T15 needs to be performed for further card-disengaging operation.
In step T15, it is determined whether the time for starting the control method for releasing the robot card exceeds a third set time, where the third set time may be set according to specific design requirements, and after the robot performs a certain time of card releasing operation, the robot still cannot release the card, which indicates that the robot is completely stuck, and the card releasing effect caused by the endless card releasing operation is not obvious, and even the robot runs out of electric power, so as to reduce the product use experience of the user, and therefore, preferably, the third set time is set to 3 minutes. If the robot detects that the card-releasing operation is not performed for 3 minutes, the robot returns to the step T10 to perform a round of card-releasing operation again. If the robot detects that the card releasing operation is performed for 3 minutes, the method goes to step T16 to determine that the robot fails to release the card, and reports an error to remind the user to help the robot to release the card.
In step T20, the robot controls the two driving wheels to simultaneously operate backward, performs the backward operation for disengaging the card, and determines whether the elevation angle detected by the robot is smaller than the set angle within the set distance of the driving wheels. The set distance can be set according to specific design requirements, the robot can stop the card backing and releasing operation at a proper time by setting the set distance, the problem of low card releasing efficiency caused by the fact that the robot carries out the card backing and releasing operation endlessly is avoided, and the robot may be clamped not firmly at the moment and can release the card slightly after backing a little, so that the set distance is preferably set to be 5 cm. The set angle can also be correspondingly set according to specific design requirements, and the robot can timely detect whether the robot returns to the horizontal plane or not by setting the set angle, so that error accumulation caused by continuous retreating after the robot is out of the card is avoided, and the set angle is preferably set to be 0.01 degrees. If the driving wheel does not run for more than 5 cm and the robot detects that the elevation angle is less than 0.01 degrees, the robot is indicated to retreat from the inclined plane of the object to the horizontal plane, and the robot can be determined to be free from the object and the card removal is successful. If the driving wheel runs for more than 5 cm, but the elevation angle detected by the robot is not less than 0.01 degrees all the time, the robot still clamps on the inclined surface of the object, and the step T21 is required to be carried out for further releasing the card.
In step T21, the robot controls the first driving wheel to move backward, the second driving wheel does not move, the rear steering and releasing operation is performed, and it is determined whether the elevation angle detected by the robot is smaller than the set angle within the first set time. The first set time can be correspondingly set according to specific design requirements, the time for the robot to perform rear steering and card releasing operation can be controlled by setting the first set time, the problem of low card releasing efficiency caused by the fact that the robot performs the rear steering and card releasing operation endlessly is avoided, and the first set time is preferably set to be 10 seconds. And if the time of the robot for the rear steering card removal operation does not exceed 10 seconds and the robot detects that the elevation angle is less than 0.01 degrees, determining that the robot card removal is successful. If the time of the robot for the rear steering card-disengaging operation exceeds 10 seconds and the robot does not detect that the elevation angle is less than 0.01 degrees, the robot still gets stuck on the inclined plane of the object, and the step T22 needs to be carried out for further card-disengaging operation.
In step T22, the robot controls the second driving wheel to rotate backward and the first driving wheel to rotate forward, performs reverse rotation and card releasing operation, and determines whether the elevation angle detected by the robot is smaller than a set angle within a first set time. And if the time of the reverse rotation card-releasing operation of the robot does not exceed 10 seconds and the robot detects that the elevation angle is less than 0.01 degrees, determining that the card-releasing operation of the robot is successful. If the time for the robot to perform the reverse rotation card-disengaging operation exceeds 10 seconds and the robot does not detect that the elevation angle is less than 0.01 °, indicating that the robot still gets stuck on the inclined surface of the object, the step T23 is required to perform further card-disengaging operation.
In step T23, the robot controls the two driving wheels to simultaneously move forward to perform forward card-releasing operation, and determines whether the elevation angle detected by the robot is smaller than a set angle within a second set time, wherein the second set time can be set according to specific design requirements, and by setting the second set time, the time for the robot to perform forward card-releasing operation can be controlled, so as to avoid the problem of low card-releasing efficiency caused by the robot performing forward card-releasing operation endlessly, and preferably set to 5 seconds. And if the time of the robot for the forward card-releasing operation does not exceed 5 seconds and the robot detects that the elevation angle is less than 0.01 degrees, determining that the robot is successful in card-releasing. If the time for the robot to perform the forward card-disengaging operation exceeds 5 seconds and the robot does not detect that the elevation angle is less than 0.01 degrees, the robot still gets stuck on the inclined plane of the object, and the step T24 needs to be performed for further card-disengaging operation.
In step T24, the robot controls the two driving wheels to simultaneously operate backward, performs the backward jam clearing operation again, and determines whether the elevation angle detected by the robot is smaller than the set angle within the second set time. The time for the back card-releasing operation of this step is longer than that for the 5 cm limit back card-releasing operation in step T20. Mainly because the robot has performed long rotation and forward operations in the previous steps, and still has no card-off, it is necessary to try a relatively long backward card-off operation, so that the robot performs full attempts in all directions. And if the time of the robot for the backward card-releasing operation does not exceed 5 seconds and the robot detects that the elevation angle is less than 0.01 degrees, determining that the robot is successful in card-releasing. If the time for the robot to perform the backward card-disengaging operation exceeds 5 seconds and the robot does not detect that the elevation angle is less than 0.01 degrees, the robot still gets stuck on the inclined plane of the object, and the step T25 needs to be performed for further card-disengaging operation.
In step T25, it is determined whether the time for starting the control method for releasing the robot card exceeds a third set time, where the third set time may be set according to specific design requirements, and after the robot performs a certain time of card releasing operation, the robot still cannot release the card, which indicates that the robot is completely stuck, and the card releasing effect caused by the endless card releasing operation is not obvious, and even the robot runs out of electric power, so as to reduce the product use experience of the user, and therefore, preferably, the third set time is set to 3 minutes. If the robot detects that the card-releasing operation is not performed for 3 minutes, the robot returns to the step T20 to perform a round of card-releasing operation again. If the robot detects that the card releasing operation is performed for 3 minutes, the method goes to step T26 to determine that the robot fails to release the card, and reports an error to remind the user to help the robot to release the card.
According to the control method for releasing the card of the robot, whether the object blocking the robot can cross or not is analyzed, and then the driving wheels of the robot are controlled according to different objects to adopt different running modes to release the card, so that the card releasing efficiency of the robot is improved, and the product use experience of a user is improved.
As one embodiment, after the control method of the robot for releasing the card is started, the robot does not update the current position information until it is determined that the robot is successfully released from the card. Therefore, the problem of accumulated errors caused by frequent skidding when the robot carries out the card-off operation can be avoided, and the accuracy of robot positioning and navigation is ensured.
As one embodiment, after the step of determining that the robot is successful in releasing the card, the method further comprises the following steps: in step T30, when it is determined that the robot is stuck based on the above-described method for detecting a robot stuck, an object corresponding to the corresponding reference elevation angle data is determined. If the object is an object that the robot can cross, such as a threshold or a U-shaped stool leg, the process proceeds to step T31. If the object is an object that the robot cannot cross, such as a base of a floor standing fan or a base of a bar chair, etc., step T32 is entered.
In step T31, the robot determines that the previous action of releasing the card is successful, and if the card is successfully released when two driving wheels run backwards simultaneously or the card is successfully released when one driving wheel runs backwards and the other driving wheel is not moving, which indicates that the robot has released from the object, the robot directly turns around to walk, and avoids the error caused by slipping when crossing the object. If one driving wheel runs forwards and the other driving wheel runs backwards, the releasing of the card is successful, which indicates that the two driving wheels of the robot are respectively positioned at the two sides of the object, the robot accelerates, turns around and walks, so that the robot completely releases from the object after one driving wheel crosses the object.
In step T32, the robot determines the working state before being stuck, and if the robot is in the edgewise walking state, walks around the edge of the object and continues to complete the remaining edgewise walking. And if the robot is in the state of regional planning walking, turning around to walk or walking after turning 90 degrees, and continuously finishing the rest planning walking after avoiding the object.
According to the method, whether the object can be crossed after the robot is detached is analyzed to determine the subsequent operation, so that the robot can be detached from the object purposefully, the action efficiency of the robot is improved, and the intelligence of the robot is improved.
The chip described in this embodiment is used to store a program. The program is used for controlling the robot to execute the detection method of the robot stuck in the above embodiment and/or used for controlling the robot to execute the control method of the robot stuck-out in the above embodiment. The chip can be a robot main control chip, a robot auxiliary control chip or a robot processor chip and the like. The robot for assembling the chip has the functions of accurately detecting whether the robot is clamped by an object or not and quickly controlling the robot to perform the releasing operation on the robot on the basis of not increasing other hardware sensors, so that the practicability of the chip and the robot for assembling the chip are improved, the low cost of the robot is ensured, and the robot is suitable for popularization and application.
Those of ordinary skill in the art will understand that: all or a portion of the steps of implementing the above-described method embodiments may be performed by hardware associated with program instructions. These programs may be stored in a computer-readable storage medium (various media that can store program codes such as ROM, RAM, CPU, MCU, magnetic or optical disk). Which when executed performs steps comprising the method embodiments described above.
Finally, it should be noted that: the embodiments in the present description are described in a progressive manner, each embodiment focuses on differences from other embodiments, the same or similar parts in the embodiments are referred to each other, and technical solutions in the embodiments may be combined with each other. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art may still modify the technical solutions described in the foregoing embodiments, or may equally replace some or all of the technical features of the foregoing embodiments; and the modifications or the substitutions do not make the essence of the corresponding technical solutions depart from the scope of the technical solutions of the embodiments of the present invention.

Claims (8)

1. The method for detecting the stuck robot based on the six-axis gyroscope is characterized by comprising the following steps:
step 1: according to the method for detecting the elevation angle of the robot based on the six-axis gyroscope, the elevation angle data detected by the robot is recorded, and the step 2 is carried out;
step 2: comparing the elevation angle data recorded in a first preset time period with reference elevation angle data stored in the robot in advance, judging whether the recorded elevation angle data is in an error range of the reference elevation angle data, if so, entering a step 3, otherwise, deleting the first recorded data in the elevation angle data recorded in the first preset time period, and returning to the step 1;
and step 3: continuously recording the elevation angle data detected by the robot, judging whether the duration time of the recorded elevation angle data in the error range of the reference elevation angle data reaches a second preset time period, if so, entering the step 4, and if not, determining that the robot is not blocked;
and 4, step 4: judging whether the robot is detected to slip within the second preset time period or not, if the robot is detected to slip, determining that the robot is clamped, and if the robot is not detected to slip, determining that the robot is not clamped;
the method for detecting the elevation angle of the robot based on the six-axis gyroscope comprises the following steps:
step S1: determining an acceleration detected by an accelerometer in a vertical direction in a six-axis gyroscope of the robot for a first predetermined period of time, and proceeding to step S2;
step S2: judging whether the variance value of the acceleration and the gravity acceleration is within a preset variance range, if so, entering step S3, and if not, entering step S4;
step S3: determining that the elevation angle of the robot is zero;
step S4: determining the angle value rotating around the X axis detected by the six-axis gyroscope as the elevation angle of the robot;
the robot is characterized in that the left-right direction is an X axis, the front-back direction is a Y axis, the up-down direction is a Z axis, and the X axis, the Y axis and the Z axis form a three-dimensional coordinate system of the six-axis gyroscope.
2. The method of claim 1, wherein the recording of the elevation angle data detected by the robot in step 1 comprises the steps of:
recording an elevation angle every a third preset time period;
and determining a connecting line formed by the elevation angles recorded by the robot in a coordinate system with the elevation angle as a Y axis and the recording time as an X axis.
3. The method according to claim 2, wherein the step 2 of comparing the elevation angle data recorded in the first preset time period with the reference elevation angle data pre-stored in the robot to determine whether the recorded elevation angle data is within the error range of the reference elevation angle data comprises the following steps:
step 21: acquiring a connecting line formed by elevation angles in a first preset time period recorded by the robot at present, determining the change condition of the connecting line, and entering step 22;
step 22: acquiring a reference line formed by reference elevation angle data detected in the first preset time period when the robot prestored in the robot walks to different inclined planes, and entering step 23;
step 23: judging whether the change condition of the connecting line recorded currently is within the error range of the change condition of the reference line, if so, determining that the recorded elevation angle data is within the error range of the reference elevation angle data, and if not, determining that the recorded elevation angle data is not within the error range of the reference elevation angle data.
4. A method according to any one of claims 1 to 3, wherein said determining in step 4 whether robot slippage is detected within said second predetermined time period comprises the steps of:
step 41: determining a first current travel distance traveled by a first driving wheel of the two driving wheels detected at the current recording time point and a first previous travel distance traveled by the first driving wheel detected at the last recording time point based on data detected by code wheels on the two driving wheels of the robot, and calculating the first distance traveled by the first driving wheel to be a difference value of the first current travel distance and the first previous travel distance; determining a second current travel distance traveled by a second driving wheel of the two driving wheels detected at the current recording time point and a second previous travel distance traveled by the second driving wheel detected at the last recording time point, and calculating to obtain the second distance traveled by the second driving wheel as a difference value of the second current travel distance and the second previous travel distance; determining a travel distance difference as a difference of the first distance and the second distance; determining the travel angle value of the two driving wheels in a first preset time as the ratio of the travel distance difference value to the width between the two driving wheels; determining a first angle change rate generated by the two driving wheels within a first preset time as a ratio of the travel angle value to the first preset time, and proceeding to step 42;
step 42: determining a maximum error rate for both of said driven wheels, determining a maximum error value for said first rate of angular change as a product of said first rate of angular change and said maximum error rate, and proceeding to step 43;
step 43: determining a current angle detected by the six-axis gyroscope at the current recording time point and an upper angle detected by the six-axis gyroscope at the previous recording time point based on data detected by the six-axis gyroscope of the robot, calculating to obtain a difference value of a change angle between the current angle and the upper angle, determining a second angle change rate generated by the six-axis gyroscope within the first preset time as a ratio of the change angle to the first preset time, and proceeding to step S44;
step 44: determining the difference value between the first angle change rate and the second angle change rate as a first difference value, and entering step 45;
step 45: determining an angular velocity variation error rate as a ratio of the first difference value to the maximum error value, and proceeding to step 46;
step 46: judging whether the angular speed change error rate is greater than or equal to a preset value within the second preset time period, if the angular speed change error rates determined for N times are greater than or equal to the preset value, determining that the robot slips, and if not, determining that the robot does not slip;
the maximum error rate is obtained through experimental tests, and the first preset time is a time interval between the current recording time point and the last recording time point.
5. The method according to claim 1, wherein the step S1 specifically comprises the steps of:
analyzing the acceleration detected by an accelerometer in the vertical direction in a six-axis gyroscope of the robot in a first preset time period, if the difference value between the currently analyzed acceleration and the previous acceleration is larger than a preset acceleration value, and the difference value between the currently analyzed acceleration and the next acceleration is also larger than the preset acceleration value, determining the currently analyzed acceleration as interference data, and deleting the interference data, otherwise, keeping the currently analyzed acceleration, and determining the kept acceleration as the acceleration detected by the accelerometer in the vertical direction in the six-axis gyroscope of the robot in the first preset time period.
6. The method according to claim 5, wherein the step of determining whether the variance value of the acceleration and the gravitational acceleration is within a preset variance range in step S2 specifically comprises the following steps:
determining the difference value between the acceleration and the gravity acceleration as an acceleration difference value;
determining the average value of the sum of squares of the acceleration difference values as an acceleration average value;
determining the arithmetic square root of the acceleration average value as a variance value;
determining a preset variance maximum value and a preset variance minimum value based on a preset variance range configured by the system;
and comparing the variance value with the preset variance maximum value and the preset variance minimum value respectively, if the variance value is larger than the preset variance minimum value and smaller than the preset variance maximum value, determining that the variance value of the acceleration and the gravitational acceleration is within a preset variance range, otherwise, determining that the variance value of the acceleration and the gravitational acceleration is not within the preset variance range.
7. The method according to claim 6, wherein the step S4 specifically comprises the steps of:
step S41: determining the angular speed of the robot rotating around the X axis, which is detected by a six-axis gyroscope;
step S42: performing time integration on the angular speed to obtain an X angle of the robot rotating around an X axis;
step S43: determining a zero drift value of the six-axis gyroscope;
step S44: and determining the elevation angle of the robot as the difference value of the X angle and the zero drift value.
8. The method according to claim 7, wherein the step S43 specifically comprises the steps of:
when the robot is in a static state, taking an angle value obtained by time integration of angular speeds rotating around an X axis and detected by a six-axis gyroscope of the robot as a zero drift value of the six-axis gyroscope;
the static state refers to a state before the robot is started or a state when the robot stops in the moving process, and the state needs to last for a second predetermined time period.
CN201810458442.9A 2018-05-14 2018-05-14 Method for detecting stuck robot based on six-axis gyroscope Active CN108613656B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810458442.9A CN108613656B (en) 2018-05-14 2018-05-14 Method for detecting stuck robot based on six-axis gyroscope

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810458442.9A CN108613656B (en) 2018-05-14 2018-05-14 Method for detecting stuck robot based on six-axis gyroscope

Publications (2)

Publication Number Publication Date
CN108613656A CN108613656A (en) 2018-10-02
CN108613656B true CN108613656B (en) 2020-12-15

Family

ID=63663207

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810458442.9A Active CN108613656B (en) 2018-05-14 2018-05-14 Method for detecting stuck robot based on six-axis gyroscope

Country Status (1)

Country Link
CN (1) CN108613656B (en)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111121705A (en) * 2019-05-07 2020-05-08 浙江利尔达客思智能科技有限公司 System and method for detecting door opening state based on six-axis detection
CN110968099B (en) * 2019-12-17 2024-05-07 小狗电器互联网科技（北京）股份有限公司 Robot trapped detection method and robot

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN204833713U (en) * 2015-07-31 2015-12-02 上海朗尚科贸有限公司 Simple and easy on -vehicle road conditions detecting system
US9285223B1 (en) * 2014-05-15 2016-03-15 State Farm Mutual Automobile Insurance Company System and method for identifying heading of a moving vehicle using accelerometer data
CN107289929A (en) * 2017-08-08 2017-10-24 珠海市微半导体有限公司 The detection method and system and chip of barrier are encountered by a kind of robot
CN107456173A (en) * 2016-06-06 2017-12-12 北京小米移动软件有限公司 Barrier crossing method and device
CN207123333U (en) * 2017-04-28 2018-03-20 深圳乐行天下科技有限公司 Collision detecting device and there is its robot

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9285223B1 (en) * 2014-05-15 2016-03-15 State Farm Mutual Automobile Insurance Company System and method for identifying heading of a moving vehicle using accelerometer data
CN204833713U (en) * 2015-07-31 2015-12-02 上海朗尚科贸有限公司 Simple and easy on -vehicle road conditions detecting system
CN107456173A (en) * 2016-06-06 2017-12-12 北京小米移动软件有限公司 Barrier crossing method and device
CN207123333U (en) * 2017-04-28 2018-03-20 深圳乐行天下科技有限公司 Collision detecting device and there is its robot
CN107289929A (en) * 2017-08-08 2017-10-24 珠海市微半导体有限公司 The detection method and system and chip of barrier are encountered by a kind of robot

Also Published As

Publication number Publication date
CN108613656A (en) 2018-10-02

Similar Documents

Publication Publication Date Title
CN108628312B (en) Method for detecting stuck robot, method for controlling stuck robot and chip
EP3682784B1 (en) Method for detecting skidding of robot, mapping method and chip
CN107456172B (en) Cleaning robot and obstacle crossing method
US11926066B2 (en) Carpet drift estimation using differential sensors or visual measurements
CN112617698B (en) Obstacle crossing method and device
KR101338143B1 (en) Apparatus and Method for Detecting Slip of a Mobile Robot
KR101008360B1 (en) Appratus and method of calibration of a gyro sensor in a mobile robot.
KR100772912B1 (en) Robot using absolute azimuth and method for mapping by the robot
JP2018101444A (en) Autonomous mobile robot
US20190243379A1 (en) Navigation of robotic pool cleaner
CN108613656B (en) Method for detecting stuck robot based on six-axis gyroscope
CN108415432B (en) Straight edge-based positioning method for robot
CN108274467A (en) The control method and chip and robot that robot takes the air line
CN108681325B (en) Elevation angle detection method based on accelerometer for robot and stuck detection and stuck-out method
JP2006313455A (en) Self-traveling cleaning robot, self-traveling robot, and program for controlling traveling of same
CN109506652B (en) Optical flow data fusion method based on carpet migration and cleaning robot
CN107678429B (en) Robot control method and chip
CN105737853B (en) A kind of drift calibration method of robot inertial navigation system
CN110320906A (en) A kind of 4 wheel driven AGV trolley differential straight-line travelling attitude adjusting method based on Mecanum wheel
KR20100134958A (en) Mobile body and method for controlling traveling of the same
CN103542864B (en) A kind of inertial navigation fall into a trap step method and device
Seyr et al. Proprioceptive navigation, slip estimation and slip control for autonomous wheeled mobile robots
JP7407421B2 (en) Mobile robot, control method, and control program
CN111474931B (en) Abnormality detection method for robot walking along straight edge and card removal processing method
CN111427362B (en) Abnormality detection method for robot walking along straight edge and card removal processing method

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant