WO2022160811A1 - 足式机器人轨迹跟踪方法、设备及可读存储介质 - Google Patents
足式机器人轨迹跟踪方法、设备及可读存储介质 Download PDFInfo
- Publication number
- WO2022160811A1 WO2022160811A1 PCT/CN2021/126532 CN2021126532W WO2022160811A1 WO 2022160811 A1 WO2022160811 A1 WO 2022160811A1 CN 2021126532 W CN2021126532 W CN 2021126532W WO 2022160811 A1 WO2022160811 A1 WO 2022160811A1
- Authority
- WO
- WIPO (PCT)
- Prior art keywords
- footed robot
- robot
- stride
- trajectory
- footed
- Prior art date
Links
- 238000000034 method Methods 0.000 title claims abstract description 63
- 238000004364 calculation method Methods 0.000 claims abstract description 37
- 238000001514 detection method Methods 0.000 claims abstract description 33
- 230000005021 gait Effects 0.000 claims abstract description 25
- 230000000875 corresponding effect Effects 0.000 claims description 71
- 230000002596 correlated effect Effects 0.000 claims description 11
- 238000012986 modification Methods 0.000 claims 1
- 230000004048 modification Effects 0.000 claims 1
- 230000001133 acceleration Effects 0.000 description 11
- 238000001914 filtration Methods 0.000 description 10
- 238000004891 communication Methods 0.000 description 6
- 238000010586 diagram Methods 0.000 description 6
- 238000004422 calculation algorithm Methods 0.000 description 5
- 230000009897 systematic effect Effects 0.000 description 3
- 238000006243 chemical reaction Methods 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 230000000737 periodic effect Effects 0.000 description 2
- 230000003247 decreasing effect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 239000000700 radioactive tracer Substances 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/08—Control of attitude, i.e. control of roll, pitch, or yaw
- G05D1/0891—Control of attitude, i.e. control of roll, pitch, or yaw specially adapted for land vehicles
Definitions
- the present invention relates to the field of robot technology, and in particular, to a trajectory tracking method, device and readable storage medium of a footed robot.
- the main purpose of the present invention is to provide a trajectory tracking method, device and readable storage medium of a footed robot, so as to realize reliable tracking of the motion trajectory of the footed robot.
- an embodiment of the present invention provides a trajectory tracking method for a footed robot, the method comprising:
- the posture is calculated to obtain the posture information of the footed robot
- the gait detection is performed to obtain the stride cycle of the footed robot and the step length corresponding to the stride cycle;
- the motion trajectory of the footed robot is calculated according to the direction angle and step length.
- the embodiment of the present invention also provides a trajectory tracking device for a footed robot, which includes:
- the attitude calculation module is used to perform attitude calculation according to the sensor data of the footed robot to obtain the attitude information of the footed robot;
- the gait detection module is used for gait detection according to the sensor data to obtain the stride cycle of the footed robot and the step length corresponding to the stride cycle;
- the acquisition module is used to acquire the direction angle corresponding to the stride period from the attitude information
- the calculation module is used to calculate the motion trajectory of the footed robot according to the direction angle and the step length.
- an embodiment of the present invention also provides a footed robot trajectory tracking device, the footed robot trajectory tracking device includes: a memory, a processor, and a footed robot trajectory tracking device stored in the memory and running on the processor.
- the trajectory tracking method of the footed robot is implemented as described above.
- an embodiment of the present invention also proposes a computer-readable storage medium, where a foot-type robot trajectory tracking program is stored on the computer-readable storage medium, and the foot-type robot trajectory tracking program is executed by a processor to achieve the above Trajectory tracking method for footed robot.
- the posture information of the footed robot is obtained by performing attitude calculation on the sensor data of the footed robot, and the stride period and step length of the footed robot are obtained by performing gait detection on the sensor data at the same time, and the stride is obtained from the posture information.
- the direction angle corresponding to the step cycle is calculated according to the direction angle and the step length to obtain the motion trajectory of the footed robot, which realizes the reliable tracking of the motion trajectory of the footed robot, so that the precise control of the robot can be realized according to the calculated accurate motion trajectory.
- FIG. 1 is a schematic structural diagram of a hardware operating environment involved in an embodiment of the present invention
- FIG. 2 is a schematic flowchart of a first embodiment of a trajectory tracking method for a footed robot according to the present invention
- FIG. 3 is a schematic diagram of an overall flow chart of a trajectory tracking involved in an embodiment of the present invention.
- FIG. 4 is a schematic diagram of an attitude calculation flowchart involved in an embodiment of the present invention.
- FIG. 5 is a schematic diagram of functional modules of a preferred embodiment of the trajectory tracking device for a footed robot according to the present invention.
- FIG. 1 is a schematic diagram of a device structure of a hardware operating environment involved in an embodiment of the present invention.
- the track tracking device of the footed robot in the embodiment of the present invention may be devices such as a smart phone, a personal computer, and a server, which are not specifically limited herein.
- the foot-type robot trajectory tracking device may include: a processor 1001 , such as a CPU, a network interface 1004 , a user interface 1003 , a memory 1005 , and a communication bus 1002 .
- the communication bus 1002 is used to realize the connection and communication between these components.
- the user interface 1003 may include a display screen (Display), an input unit such as a keyboard (Keyboard), and the optional user interface 1003 may also include a standard wired interface and a wireless interface.
- the network interface 1004 may include a standard wired interface and a wireless interface (eg, a WI-FI interface).
- the memory 1005 may be high-speed RAM memory, or may be non-volatile memory, such as disk memory.
- the memory 1005 may also be a storage device independent of the aforementioned processor 1001 .
- the device structure shown in FIG. 1 does not constitute a limitation on the trajectory tracking device of the footed robot, and may include more or less components than the one shown, or combine some components, or different Component placement.
- the memory 1005 which is a computer storage medium, may include an operating system, a network communication module, a user interface module, and a trajectory tracking program of a footed robot.
- the operating system is a program that manages and controls the hardware and software resources of the device, and supports the running of the footed robot trajectory tracking program and other software or programs.
- the user interface 1003 is mainly used for data communication with the client;
- the network interface 1004 is mainly used to establish a communication connection with the server;
- the processor 1001 can be used to call the foot robot stored in the memory 1005 track tracer, and do the following:
- the posture is calculated to obtain the posture information of the footed robot
- the gait detection is performed to obtain the stride cycle of the footed robot and the step length corresponding to the stride cycle;
- the motion trajectory of the footed robot is calculated according to the direction angle and step length.
- the sensor of the footed robot includes a three-axis accelerometer, a three-axis gyroscope and a three-axis magnetometer, and the steps of performing attitude calculation according to the sensor data of the footed robot to obtain the attitude information of the footed robot include:
- the second attitude quaternion is corrected by using the three-axis magnetometer data to obtain the third attitude quaternion;
- the step of obtaining the stride cycle of the footed robot and the step length corresponding to the stride cycle by performing gait detection according to the sensor data includes:
- the step size corresponding to the stride period is calculated according to the sensor data in the stride period.
- step of calculating the step size corresponding to the stride period according to the sensor data in the stride period includes:
- the step size corresponding to the stride period is calculated according to the preset scale factor, the maximum total accelerometer value and the minimum total accelerometer value.
- calculating the step size corresponding to the stride period according to the preset scale factor, the maximum total accelerometer value and the minimum total accelerometer value includes:
- the processor 1001 can also be used to call the footed robot trajectory tracking program stored in the memory 1005, and perform the following operations:
- the preset scale factor is adjusted according to the error, so as to calculate the step size corresponding to the next stride cycle based on the adjusted preset scale factor, wherein the error is positively correlated with the preset scale factor.
- the processor 1001 can also be used to call the foot robot trajectory tracking program stored in the memory 1005, and execute the step. Do the following:
- the method further includes:
- the preset threshold is adjusted according to the error, so as to remove the peak point corresponding to the next stride cycle based on the adjusted preset threshold, wherein the error is positively correlated with the preset threshold.
- step of calculating the motion trajectory of the footed robot according to the direction angle and the step length includes:
- the motion trajectory of the footed robot is obtained according to the current ordinate position and the current ordinate position.
- the step of obtaining the direction angle corresponding to the stride period from the attitude information includes:
- FIG. 2 is a schematic flowchart of a first embodiment of a trajectory tracking method for a footed robot according to the present invention.
- the embodiment of the present invention provides an embodiment of the method for tracking the trajectory of a footed robot. It should be noted that although the logical sequence is shown in the flowchart, in some cases, the sequence shown here may be performed in a different sequence. out or described steps.
- the execution body of each embodiment of the footed robot trajectory tracking method of the present invention may be a footed robot or a device connected to the footed robot to control the footed robot. For ease of description, the execution body is omitted in the following embodiments.
- the trajectory tracking method of the footed robot includes:
- Step S10 performing attitude calculation according to the sensor data of the footed robot to obtain the posture information of the footed robot
- sensors can be installed in various parts of the footed robot, and the sensor types can include one or more of a three-axis gyroscope, a three-axis accelerometer, and a three-axis magnetometer.
- the meter is used to measure angular acceleration
- the magnetometer is used to measure the local magnetic field.
- Each sensor can be installed on the torso and feet of the robot. The sensors installed on the feet can detect more abundant gait information, so the sensors are preferably installed on the feet.
- the posture is calculated according to the sensor data collected by each sensor according to a certain frequency, and the posture information of the footed robot is obtained.
- the attitude calculation can adopt the existing attitude calculation methods, such as EKF (Extended Kalman Filter), UKF (Unscented Kalman Filter) and so on.
- the attitude information obtained by the attitude calculation may include the attitude of the footed robot at each moment, or the more detailed attitude of each part of the footed robot.
- the posture represents the movement direction of the footed robot or the specific part of the footed robot relative to the inertial navigation coordinate system, and the posture can be represented by data forms such as quaternions, Euler angles, or rotation matrices.
- Step S20 performing gait detection according to the sensor data to obtain a stride cycle of the footed robot and a step length corresponding to the stride cycle;
- gait detection is also performed according to the sensor data.
- the gait detection is to detect the stride cycle of the footed robot and the step length corresponding to the stride cycle.
- a footed robot can be bipedal or multipedal; for a bipedal robot, one step is a stride cycle; for a quadrupedal robot, generally the left front foot and right rear The corner and the right front foot step at the same time, so each step of the left front foot and the rear right foot is a stride cycle, and each step of the left rear corner and the right front foot is a step cycle.
- the step size corresponding to the stride period is the length of one step.
- the sensor data may also be filtered and calibrated.
- the sensor data contains random noise and systematic errors.
- filtering can be achieved by filtering algorithms, such as mean filtering, high and low pass filtering, and least squares fitting filtering.
- the least squares fitting filtering algorithm should be used. This filtering algorithm has no offset to the phase of the periodic signal. Both mean filtering and high-low pass filtering have more or less effects on the phase of the periodic signal. Bias.
- the method of calibration will be adopted.
- the systematic error mainly includes bias error, scale error and quadrature error.
- the calibration method can adopt the ellipsoid fitting method.
- Step S30 obtaining the direction angle corresponding to the stride period from the attitude information
- the direction angle corresponding to the stride period can be obtained from the attitude information.
- the direction angle is the movement direction of the footed robot relative to the inertial navigation coordinate system during the stride period, that is, the direction of the footed robot's stride, which can be clamped with the X axis or the Y axis of the inertial navigation coordinate system.
- Angle representation Specifically, the frequency of attitude calculation is relatively fast, so the attitude information contains the attitude of the footed robot at each time point, and the stride period corresponds to a time period, so the time period corresponding to the stride period can be obtained from the attitude information.
- Attitude specifically the attitude corresponding to the start time point of the stride cycle in the attitude information, and the direction angle is calculated according to the obtained attitude.
- the attitude is represented by a quaternion
- the quaternion can be converted into Euler
- the yaw angle in the Euler angles is used as the direction angle corresponding to the stride period.
- step S30 includes:
- Step S301 obtaining the Euler angle corresponding to the starting time point in the stride cycle from the attitude information
- Step S302 taking the yaw angle in the Euler angles as the direction angle corresponding to the stride period.
- the attitude information obtained by the attitude calculation may include Euler angles of the footed robot at various time points. After the attitude information and the stride period are obtained by calculation, the Euler angle corresponding to the starting time point in the stride period can be obtained from the attitude information, and the Euler angle is used as the direction angle corresponding to the stride period.
- the Euler angle corresponding to the end time point in the stride period may also be obtained as the direction angle corresponding to the stride period.
- the posture information obtained by the posture calculation includes the Euler angles of each foot of the footed robot at each time point, then the foot corresponding to the stride cycle can be obtained from the posture information at the beginning of the stride cycle. Or the Euler angle corresponding to the end time point as the direction angle.
- step S40 the motion trajectory of the footed robot is calculated according to the direction angle and the step length.
- the motion trajectory of the footed robot can be calculated according to the direction angle and step length.
- the motion trajectory can be represented by the position of the footed robot in the inertial navigation coordinate system after striding, then after the footed robot moves for multiple steps, the motion trajectory of different position points in series will be obtained.
- step S40 includes:
- Step S401 multiply the step length by the cosine value of the direction angle and add the initial ordinate position of the footed robot to obtain the current ordinate position of the footed robot;
- Step S402 multiply the step length by the sine value of the direction angle and add the initial abscissa position of the footed robot to obtain the current abscissa position of the footed robot;
- step S403 the motion trajectory of the footed robot is obtained according to the current ordinate position and the current ordinate position.
- the footed robot has an initial position before striding.
- the initial position includes the initial abscissa position and the initial ordinate position.
- the footed robot multiplies the step length of the striding cycle by the cosine of the direction angle, plus the initial ordinate. Position, you can get the current ordinate position after stride, multiply the step length of the stride cycle by the sine value of the direction angle, and add the initial abscissa position to get the current abscissa position after stride.
- the footed robot takes the initial position composed of the initial abscissa position and the initial ordinate position and the current position composed of the current abscissa position and the current ordinate position as the motion trajectory of the footed robot. It can be understood that the current abscissa position and the current ordinate position are the initial positions before the footed robot performs the next step.
- the position can be calculated according to the following formula:
- the inertial navigation coordinate system adopts the northeast sky coordinate system
- E represents geographic east (horizontal coordinate)
- N geographic north (vertical coordinate)
- k represents the total number of steps
- n represents the sequence number of the stride cycle
- d represents the step length
- ⁇ represents Bearing angle (the angle between the stride direction and the ordinate axis).
- FIG. 4 is a schematic diagram of a preferred trajectory tracking process in the first.
- the posture information of the footed robot is obtained by performing attitude calculation on the sensor data of the footed robot, and the stride period and step length of the footed robot are obtained by performing gait detection on the sensor data at the same time.
- the direction angle corresponding to the stride cycle is obtained, and the motion trajectory of the footed robot is calculated according to the direction angle and the step length, which realizes the reliable tracking of the motion trajectory of the footed robot, so as to realize the accuracy of the robot according to the calculated accurate motion trajectory. control.
- step S10 includes:
- Step S101 calculating the first attitude quaternion according to the three-axis gyroscope data
- Step S102 using the three-axis accelerometer data to perform the first correction on the first attitude quaternion to obtain the second attitude quaternion;
- Step S103 using the three-axis magnetometer data to perform a second correction on the second attitude quaternion to obtain a third attitude quaternion;
- the attitude calculation can be performed according to the quaternion-based EKF attitude calculation algorithm.
- FIG. 4 it is a flowchart of the attitude calculation in this embodiment. Specifically, an attitude quaternion (referred to as a first attitude quaternion for distinction) is first calculated (also referred to as updating) according to the data collected by the three-axis gyroscope. Then, the data collected by the three-axis accelerometer is used to correct the first attitude quaternion for the first time to obtain the second attitude quaternion. Specifically, the attitude quaternion can be corrected for the first time through the data collected by the three-axis accelerometer and the estimated gravity direction error.
- an attitude quaternion referred to as a first attitude quaternion for distinction
- the data collected by the three-axis accelerometer is used to correct the first attitude quaternion for the first time to obtain the second attitude quaternion.
- the attitude quaternion can be corrected for the first
- the data collected by the accelerometer is rotated through the rotation matrix calculated by the attitude angle, and converted from the object coordinate system to the navigation coordinate system (northeast sky).
- the acceleration in the z-axis direction is The estimated gravitational acceleration should be g in theory, but there is actually an error; at this time, the error is calculated. If the error is large, the error compensation can be increased, and the error compensation can be decreased when the error is small, so as to complete the first time. Correction.
- the three-axis magnetometer uses the three-axis magnetometer to correct the second attitude quaternion for the second time, and obtain the third attitude quaternion.
- the direction of the geomagnetic intensity is constant (the geomagnetic north points to the geomagnetic north), so the geomagnetic direction can be corrected for the second time, which is similar to the above-mentioned first correction process, and will not be described in detail here.
- step S104 the third attitude quaternion is converted into Euler angles, and the Euler angles are used as the attitude information of the footed robot.
- the third attitude quaternion can be converted into an Euler angle, and the Euler angle is used as the attitude information of the footed robot.
- the method of converting the quaternion to the Euler angle may refer to the existing conversion method, which will not be described in detail in this embodiment.
- the quaternion-based EKF attitude calculation algorithm is applied to the trajectory prediction of the footed robot, and the accurate estimation of the motion trajectory of the footed robot is realized, thereby providing accurate motion control for the footed robot. Data basis.
- step S20 includes:
- Step S201 performing peak detection on sensor data to obtain each minimum peak point
- Step S202 taking the period defined by two adjacent minimum peak points in each minimum peak point as the stride period
- the acceleration and angular velocity of one foot will change from the start of the step to the end of the step, that is, there will be a process of acceleration and deceleration.
- the sensor data will have a minimum peak value, and the time point corresponding to the minimum peak value is the minimum peak value point.
- the period defined by two adjacent minimum peak points is a stride period.
- the sensor data can detect the peak value through an existing data peak value detection method, such as a sliding window+minimum value detection method, which will not be described in detail in this embodiment. It should be noted that the data of multiple sensors may be used for peak detection, so as to improve the detection accuracy of the peak point.
- the data of each sensor can be fused first, and then peak detection can be performed on the fused data, or the minimum peak point of each sensor can be detected separately, and then the minimum peak point of each sensor can be detected. Fusion is performed to obtain the final minimum peak point.
- three-axis accelerometer data and three-axis gyroscope data may be used to detect peaks, at the end of one stride period and the beginning of the next stride period, the three-axis accelerometer data and the three-axis
- the amplitude of the gyroscope data will reach the minimum peak value; the minimum peak point detected according to the three-axis accelerometer data and the minimum peak value of the three-axis gyroscope data can be averaged to obtain the final minimum peak point.
- Step S203 Calculate the step size corresponding to the stride period according to the sensor data in the stride period.
- the step size corresponding to the stride period can be calculated according to the sensor data in the stride period. Specifically, the step length can be calculated by using the acceleration value in the stride period collected by the three-axis accelerometer.
- step S203 includes:
- Step S2031 obtaining the maximum total accelerometer value and the minimum total accelerometer value in the sensor data in the stride period;
- Step S2032 Calculate the step length corresponding to the stride period according to the preset scale factor, the maximum total accelerometer value and the minimum total accelerometer value.
- the acceleration values collected by the three-axis accelerometer at each time point in the stride cycle can be obtained.
- the three-axis accelerometer will collect the acceleration values in three directions, and the three acceleration values Take the average to get the total accelerometer value corresponding to that time point.
- the maximum total accelerometer value and the minimum total accelerometer value within the stride period can be obtained.
- the step length corresponding to the stride period is obtained by calculating according to the preset scale factor, the maximum total accelerometer value and the minimum total accelerometer value.
- the difference between the maximum total accelerometer value and the minimum total accelerometer value may be calculated, and the step size may be calculated by using a preset scale factor and the cube root value of the difference.
- the step size can be calculated according to the following calculation formula:
- k is the preset scale factor
- a max is the maximum total accelerometer value
- a min is the minimum total accelerometer value. Due to the differences in hardware between different footed robots, the error of calculating the step length according to the acceleration value is also different, so a preset scale factor k is set in the above formula.
- the scale factor can be pre-calibrated, that is, how far a step actually travels is measured by an external device, and then the initial k is calculated by the above formula.
- step S40 it also includes:
- Step S50 calculating the error between the motion trajectory and the planned trajectory of the footed robot
- the error between the motion trajectory and the planned trajectory of the footed robot can be calculated. That is, the motion trajectory is the actual trajectory of the robot, and the planned trajectory is the trajectory that the robot pre-plans to take. For example, in 2 seconds, the robot moves from coordinates (0, 0) to (0.5, 0.5), (0, 0)--(0.5, 0.5) is the trajectory in these two seconds, which is the measured value. Theoretically, the footed robot is controlled to walk to (0.6, 0.6), but the actual walk (0.5, 0.5) is measured, and the error between the two is calculated. Specifically, the distance between the two can be calculated as the error.
- Step S60 adjusting the preset scale factor according to the error, so as to perform the calculation of the corresponding step size of the next stride cycle based on the adjusted preset scale factor, wherein the error is positively correlated with the preset scale factor.
- the preset scale factor can be adjusted according to the error, and when the step length of the next walking cycle is calculated, it is calculated according to the adjusted preset scale factor.
- the error is positively correlated with the preset scale factor, that is, the larger the error, the larger the preset scale factor, the smaller the error, and the smaller the preset scale factor, and the formula for calculating the preset scale factor according to the error can be set according to this relationship.
- the calculation method is not limited in this embodiment. It should be noted that because the error of each step of the footed robot is different, the reflected motion state is also different, and in different motion states, the error of calculating the step length according to the acceleration value is also different.
- the footed robot can One step is to dynamically adjust a preset scale factor according to the error between the motion trajectory of the stride cycle and the planned trajectory to adapt to the different motion states of the footed robot during the motion process, thereby making the step length calculation more accurate.
- step S202 it also includes:
- Step S204 removing the peak points whose median value is greater than the preset threshold value among the minimum peak points.
- each minimum peak point is detected according to the sensor data
- these lowest peak points can be filtered, that is, due to the hardware of the sensor itself, or the footed robot slips when walking on a slippery road surface, etc. Due to sudden movement, the sensor data may have peak noise, for which threshold filtering can be added.
- the preset threshold may be a value set in advance based on experience. When the value corresponding to the minimum peak point is greater than or equal to the preset threshold, it is considered that the minimum peak point is a gait point, and when it is greater than the preset threshold, the The minimum peak point is noise interference. Therefore, the peak points whose median value of each minimum peak point is greater than the preset threshold can be removed, so as to avoid noise affecting the accuracy of trajectory tracking.
- step S50 it also includes:
- Step S60 Adjust the preset threshold according to the error, so as to remove the peak point corresponding to the next stride cycle based on the adjusted preset threshold, wherein the error is positively correlated with the preset threshold.
- the preset threshold can also be adjusted according to the error.
- the adjusted preset threshold can be used to remove peak points.
- the error is positively correlated with the preset threshold, that is, the larger the error, the larger the preset threshold, the smaller the error, and the smaller the preset threshold, and a formula for calculating the preset threshold according to the error can be set according to this relationship, such as a linear formula , the calculation method is not limited in this embodiment. It should be noted that, because the error of each step of the footed robot is different, the reflected motion state is also different, and under different motion states, the peak value of the minimum peak point of the walking cycle is also different.
- One step is to dynamically adjust a preset threshold according to the error between the motion trajectory of the stride cycle and the planned trajectory to adapt to the different motion states of the footed robot during the movement process, thereby more accurately eliminating noise and improving the detection gait cycle. accurately, thereby improving the accuracy of trajectory tracking.
- an embodiment of the present invention also proposes a trajectory tracking device for a footed robot.
- the device includes:
- the attitude calculation module 10 is configured to perform attitude calculation according to the sensor data of the footed robot to obtain the attitude information of the footed robot;
- the gait detection module 20 is used for performing gait detection according to the sensor data to obtain the stride cycle of the footed robot and the step length corresponding to the stride cycle;
- an acquisition module 30 configured to acquire the direction angle corresponding to the stride period from the attitude information
- the calculation module 40 is used for calculating the motion trajectory of the footed robot according to the direction angle and the step length.
- the sensor of the footed robot includes a three-axis accelerometer, a three-axis gyroscope and a three-axis magnetometer, and the attitude calculation module 10 includes:
- a first calculation unit used for calculating the first attitude quaternion according to the three-axis gyroscope data
- a first correction unit configured to perform the first correction on the first attitude quaternion by using the three-axis accelerometer data to obtain the second attitude quaternion;
- the second correction unit is used to perform the second correction on the second attitude quaternion by using the three-axis magnetometer data to obtain the third attitude quaternion;
- the conversion unit is used to convert the third attitude quaternion into Euler angles, and use the Euler angles as the attitude information of the footed robot.
- the gait detection module 20 includes:
- the detection unit is used for performing peak detection on the sensor data to obtain each minimum peak point
- a first determining unit configured to use a period defined by two adjacent minimum peak points in each minimum peak point as a stride period
- the second calculation unit is configured to calculate and obtain the step size corresponding to the stride period according to the sensor data in the stride period.
- the second computing unit includes:
- the acquisition subunit is used to acquire the maximum total accelerometer value and the minimum total accelerometer value in the sensor data within the stride period;
- the calculation subunit is used for calculating the step size corresponding to the stride period according to the preset scale factor, the maximum total accelerometer value and the minimum total accelerometer value.
- calculation subunit is also used for:
- calculation module 40 is also used to calculate the error between the motion trajectory and the planned trajectory of the footed robot
- the device also includes:
- the adjustment module is configured to adjust the preset scale factor according to the error, so as to calculate the step size corresponding to the next stride cycle based on the adjusted preset scale factor, wherein the error is positively correlated with the preset scale factor.
- the device also includes:
- the removal module is used to remove the peak points whose median value is greater than the preset threshold value of each minimum peak point.
- the adjustment module is further configured to adjust the preset threshold according to the error, so as to remove the peak point corresponding to the next stride cycle based on the adjusted preset threshold, wherein the error is positively correlated with the preset threshold.
- computing module 40 includes:
- the third calculation unit is used for multiplying the step length by the cosine value of the direction angle and adding the initial ordinate position of the footed robot to obtain the current ordinate position of the footed robot;
- the fourth calculation unit is used to multiply the step length by the sine value of the direction angle and add the initial abscissa position of the footed robot to obtain the current abscissa position of the footed robot;
- the second determining unit is configured to obtain the motion trajectory of the footed robot according to the current ordinate position and the current ordinate position.
- the acquisition module 30 includes:
- an acquisition unit used to acquire the Euler angle corresponding to the start time point in the stride cycle from the attitude information
- the third determining unit is used for taking the yaw angle in the Euler angles as the direction angle corresponding to the stride period.
- the expanded content of the specific implementation of the trajectory tracking device for a footed robot of the present invention is basically the same as that of the above-mentioned embodiments of the trajectory tracking method for a footed robot, which will not be repeated here.
- an embodiment of the present invention also provides a computer-readable storage medium, in which a trajectory tracking program of a footed robot is stored, and when the trajectory tracking program of the footed robot is executed by a processor, any one of the above-mentioned method embodiments can be implemented.
- Robot trajectory tracking method Robot trajectory tracking method.
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
Abstract
一种足式机器人轨迹跟踪方法、设备及可读存储介质。足式机器人轨迹跟踪方法包括：根据足式机器人的传感器数据进行姿态解算得到足式机器人的姿态信息（S10）；根据传感器数据进行步态检测得到足式机器人的跨步周期和跨步周期对应的步长(S20)；从姿态信息中获取跨步周期对应的方向角(S30)；根据方向角和步长计算得到足式机器人的运动轨迹(S40)。足式机器人轨迹跟踪方法实现了对足式机器人的运动轨迹的可靠跟踪，从而能够根据计算得到的准确运动轨迹实现机器人的精准控制。
Description
本发明涉及机器人技术领域，尤其涉及一种足式机器人轨迹跟踪方法、设备及可读存储介质。
发明背景
随着机器人技术的不断发展和成熟，各种类型的机器人已经被广泛应用于各个领域以取代或协助人类工作。机器人在移动过程中，需要不断地跟踪机器人的实际运动轨迹，根据实际运动轨迹来规划和调整机器人各部位的运动速度和角度，从而实现机器人稳定行走。但是现阶段，对足式机器人的轨迹估计尚无可靠方法，在复杂场景中，存在较大位置误差。
发明内容
本发明的主要目的在于提供一种足式机器人轨迹跟踪方法、设备及可读存储介质，实现对足式机器人运动轨迹的可靠跟踪。
为实现上述目的，本发明实施例提供一种足式机器人轨迹跟踪方法，该方法包括：
根据足式机器人的传感器数据进行姿态解算得到足式机器人的姿态信息；
根据传感器数据进行步态检测得到足式机器人的跨步周期和跨步周期对应的步长；
从姿态信息中获取跨步周期对应的方向角；
根据方向角和步长计算得到足式机器人的运动轨迹。
为实现上述目的，本发明实施例还提供一种足式机器人轨迹跟踪装置，该装置包括：
姿态解算模块，用于根据足式机器人的传感器数据进行姿态解算得到足式机器人的姿态信息；
步态检测模块，用于根据传感器数据进行步态检测得到足式机器人的跨步周期和跨步周期对应的步长；
获取模块，用于从姿态信息中获取跨步周期对应的方向角；
计算模块，用于根据方向角和步长计算得到足式机器人的运动轨迹。
为实现上述目的，本发明实施例还提供一种足式机器人轨迹跟踪设备，足式机器人轨迹跟踪设备包括：存储器、处理器及存储在存储器上并可在处理器上运行的足式机器人轨迹跟踪程序，足式机器人轨迹跟踪程序被处理器执行时实现如上的足式机器人轨迹跟踪方法。
此外，为实现上述目的，本发明实施例还提出一种计算机可读存储介质，计算机可读存储介质上存储有足式机器人轨迹跟踪程序，足式机器人轨迹跟踪程序被处理器执行时实现如上的足式机器人轨迹跟踪方法。
本发明中，通过对足式机器人的传感器数据进行姿态解算得到足式机器人的姿态信息，同时对传感器数据进行步态检测得到足式机器人的跨步周期和步长，从姿态信息中获取跨步周期对应的方向角，根据方向角和步长计算得到足式机器人的运动轨迹，实现了对足式机器人的运动轨迹的可靠跟踪，从而能够根据计算得到的准确运动轨迹实现机器人的精准控制。
附图简要说明
图1为本发明实施例方案涉及的硬件运行环境的结构示意图；
图2为本发明足式机器人轨迹跟踪方法第一实施例的流程示意图；
图3为本发明实施例涉及的一种轨迹跟踪整体流程示意图；
图4为本发明实施例涉及的一种姿态解算流程示意图；
图5为本发明足式机器人轨迹跟踪装置较佳实施例的功能模块示意图。
本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
如图1所示，图1是本发明实施例方案涉及的硬件运行环境的设备结构示意图。
需要说明的是，本发明实施例足式机器人轨迹跟踪设备可以是智能手机、 个人计算机和服务器等设备，在此不做具体限制。
如图1所示，该足式机器人轨迹跟踪设备可以包括：处理器1001，例如CPU，网络接口1004，用户接口1003，存储器1005，通信总线1002。其中，通信总线1002用于实现这些组件之间的连接通信。用户接口1003可以包括显示屏(Display)、输入单元比如键盘(Keyboard)，可选用户接口1003还可以包括标准的有线接口、无线接口。网络接口1004可选的可以包括标准的有线接口、无线接口(如WI-FI接口)。存储器1005可以是高速RAM存储器，也可以是稳定的存储器(non-volatile memory)，例如磁盘存储器。存储器1005可选的还可以是独立于前述处理器1001的存储装置。
本领域技术人员可以理解，图1中示出的设备结构并不构成对足式机器人轨迹跟踪设备的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件布置。
如图1所示，作为一种计算机存储介质的存储器1005中可以包括操作系统、网络通信模块、用户接口模块以及足式机器人轨迹跟踪程序。操作系统是管理和控制设备硬件和软件资源的程序，支持足式机器人轨迹跟踪程序以及其它软件或程序的运行。在图1所示的设备中，用户接口1003主要用于与客户端进行数据通信；网络接口1004主要用于与服务器建立通信连接；而处理器1001可以用于调用存储器1005中存储的足式机器人轨迹跟踪程序，并执行以下操作：
根据足式机器人的传感器数据进行姿态解算得到足式机器人的姿态信息；
根据传感器数据进行步态检测得到足式机器人的跨步周期和跨步周期对应的步长；
从姿态信息中获取跨步周期对应的方向角；
根据方向角和步长计算得到足式机器人的运动轨迹。
进一步地，足式机器人的传感器包括三轴加速度计、三轴陀螺仪和三轴磁力计，根据足式机器人的传感器数据进行姿态解算得到足式机器人的姿态信息的步骤包括：
根据三轴陀螺仪数据计算第一姿态四元数；
采用三轴加速度计数据对第一姿态四元数进行第一次修正，得到第二姿态四元数；
采用三轴磁力计数据对第二姿态四元数进行第二次修正，得到第三姿态四元数；
将第三姿态四元数转换为欧拉角，将欧拉角作为足式机器人的姿态信息。
进一步地，根据传感器数据进行步态检测得到足式机器人的跨步周期和跨步周期对应的步长的步骤包括：
对传感器数据进行峰值检测得到各最小峰值点；
将各最小峰值点中相邻两个最小峰值点所限定的周期作为跨步周期；
根据跨步周期内的传感器数据计算得到跨步周期对应的步长。
进一步地，根据跨步周期内的传感器数据计算得到跨步周期对应的步长的步骤包括：
获取跨步周期内传感器数据中的最大总加速度计值和最小总加速度计值；
根据预设比例因子、最大总加速度计值和最小总加速度计值计算得到跨步周期对应的步长。
可选地，根据预设比例因子、最大总加速度计值和最小总加速度计值计算得到跨步周期对应的步长包括：
计算最大总加速度计值和最小总加速度计值的差值，利用预设比例因子与差值的立方根值计算得到步长。
进一步地，根据方向角和步长计算得到足式机器人的运动轨迹的步骤之后，处理器1001还可以用于调用存储器1005中存储的足式机器人轨迹跟踪程序，并执行以下操作：
计算运动轨迹与足式机器人的规划轨迹之间的误差；
根据误差调整预设比例因子，以基于调整后的预设比例因子进行下一跨步周期对应步长的计算，其中，误差与预设比例因子正相关。
进一步地，将各最小峰值点中相邻两个最小峰值点所限定的周期作为跨步周期的步骤之前，处理器1001还可以用于调用存储器1005中存储的足式机器人轨迹跟踪程序，并执行以下操作：
去除各最小峰值点中值大于预设阈值的峰值点。
计算运动轨迹与足式机器人的规划轨迹之间的误差的步骤之后，还包括：
根据误差调整预设阈值，以基于调整后的预设阈值进行下一跨步周期对应峰值点的去除，其中，误差与预设阈值正相关。
进一步地，根据方向角和步长计算得到足式机器人的运动轨迹的步骤包括：
将步长乘以方向角的余弦值后加上足式机器人的初始纵坐标位置，得到足式机器人的当前纵坐标位置；
将步长乘以方向角的正弦值后加上足式机器人的初始横坐标位置，得到足式机器人的当前横坐标位置；
根据当前纵坐标位置和当前纵坐标位置得到足式机器人的运动轨迹。
进一步地，从姿态信息中获取跨步周期对应的方向角的步骤包括：
从姿态信息中获取与跨步周期中开始时间点对应的欧拉角；
将欧拉角中的偏航角作为跨步周期对应的方向角。
基于上述的结构，提出足式机器人轨迹跟踪方法的各个实施例。
参照图2，图2为本发明足式机器人轨迹跟踪方法第一实施例的流程示意图。
本发明实施例提供了足式机器人轨迹跟踪方法的实施例，需要说明的是，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。本发明足式机器人轨迹跟踪方法各个实施例的执行主体可以是足式机器人或与足式机器人通行连接以控制足式机器人的设备，为便于描述，以下各实施例中省略执行主体进行阐述。在本实施例中，足式机器人轨迹跟踪方法包括：
步骤S10，根据足式机器人的传感器数据进行姿态解算得到足式机器人的姿态信息；
可在足式机器人的各个部位安装多个传感器，传感器类型可包括三轴陀螺仪、三轴加速度计和三轴磁力计中的一个或多个，其中，三轴加速度计用于测量加速度，陀螺仪用于测量角加速度，磁力计用于测量当地磁场。各个传感器可安装在机器人躯干和足上，安装在足上的传感器检测的步态信息更加丰富，故优选地将传感器安装于足上。
在足式机器人行走过程中，根据各个传感器采集的传感器数据按照一定的频率进行姿态解算，得到足式机器人的姿态信息。其中，姿态解算可采用现有的姿态解算方法，例如EKF(扩展卡尔曼滤波器)、UKF(无迹卡尔曼滤波器)等等。姿态解算得到的姿态信息可包括足式机器人在各个时刻的姿态，或者包括足式机器人各个部位更细化的姿态。其中，姿态表示足式机器人或者足式机器人具体部位相对于惯性导航坐标系的运动方向，姿态可以采用四元数、欧拉角或旋转矩阵等数据形式来表示。
步骤S20，根据传感器数据进行步态检测得到足式机器人的跨步周期和跨步周期对应的步长；
在足式机器人行走过程中，还根据传感器数据进行步态检测。步态检测即检测足式机器人的跨步周期，以及跨步周期对应的步长。其中，足式机器人可以是两足或多足的；对于两足的机器人，一只脚迈出一步为一个跨步周期；对于四足的机器人，一般是左前脚和右后脚同时迈步，左后角和右前脚同时迈步，所以左前脚和后右脚各迈一步为一个跨步周期，左后角和右前脚各迈一步为一个跨步周期。跨步周期对应的步长即一步迈出的长度。当依据多个传感器的数据进行步态检测时，可先对各个传感器的数据进行融合，再依据融合后的数据进行步态检测，传感器数据融合可采用现有的方式，在本实施例中不再详细赘述。
进一步地，在采用传感器数据进行姿态解算和步态检测之前，还可对传感器数据进行滤波和校准。具体地，传感器数据包含随机噪声和系统误差，对于随机噪声可通过滤波算法实现滤波，可选均值滤波、高低通滤波和最小二乘法拟合滤波等。对于步态信息之类的传感器数据宜采用最小二乘法拟合滤波算法，此类滤波算法对周期信号的相位无偏移，均值滤波和高低通滤波都对周期信号的相位有或多或少的偏置。对于系统误差，将采用校准的方式。系统误差主要包括偏置误差、尺度误差和正交误差。校准方法可采用椭球拟合方式。
步骤S30，从姿态信息中获取跨步周期对应的方向角；
在计算得到跨步周期后，可从姿态信息中获取与跨步周期对应的方向角。其中，方向角即跨步周期内足式机器人相对于惯性导航坐标系的运动方向，也即，足式机器人跨步的方向，可采用与惯性导航坐标系的X轴或Y轴之间的夹角表示。具体地，姿态解算的频率较快，因此姿态信息包含了足式机器人在各个时间点的姿态，而跨步周期对应一个时间段，因此可从姿态信息中获取跨步周期的时间段对应的姿态，具体可以是获取姿态信息中跨步周期的开始时间点所对应的姿态，根据获取到的姿态计算出方向角，例如姿态是采用四元数表示，则可将四元数转换为欧拉角，将欧拉角中的偏航角作为跨步周期对应的方向角。
进一步地，步骤S30包括：
步骤S301，从姿态信息中获取与跨步周期中开始时间点对应的欧拉角；
步骤S302，将欧拉角中的偏航角作为跨步周期对应的方向角。
在一实施方式中，姿态解算得到的姿态信息可以包括足式机器人在各个时间点的欧拉角。在计算得到姿态信息和跨步周期后，可从姿态信息中获取跨步周期中开始时间点对应的欧拉角，将该欧拉角作为该跨步周期对应的方向角。
在其他实施方式中，也可以是获取跨步周期中结束时间点对应的欧拉角作为该跨步周期对应的方向角。或者在其他实施方式中，姿态解算得到的姿态信息包括足式机器人的各个脚在各个时间点的欧拉角，则可以从姿态信息中获取跨步周期所对应的脚在该跨步周期开始或结束时间点对应的欧拉角作为方向角。
步骤S40，根据方向角和步长计算得到足式机器人的运动轨迹。
在计算得到跨步周期对应的方向角和步长后，可根据该方向角和步长计算得到足式机器人的运动轨迹。其中，运动轨迹可以采用足式机器人跨步后在惯性导航坐标系中的位置来表示，那么足式机器人移动多步后，将得到不同位置点串联的运动轨迹。
在一实施方式中，步骤S40包括：
步骤S401，将步长乘以方向角的余弦值后加上足式机器人的初始纵坐标位置，得到足式机器人的当前纵坐标位置；
步骤S402，将步长乘以方向角的正弦值后加上足式机器人的初始横坐标位置，得到足式机器人的当前横坐标位置；
步骤S403，根据当前纵坐标位置和当前纵坐标位置得到足式机器人的运动轨迹。
足式机器人在跨步之前有一个初始位置，初始位置包括初始横坐标位置和初始纵坐标位置，足式机器人将跨步周期的步长乘以方向角的余弦值，再加上该初始纵坐标位置，即可得到跨步后的当前纵坐标位置，将跨步周期的步长乘以方向角的正弦值，在加上该初始横坐标位置，即可得到跨步后的当前横坐标位置。足式机器人将该初始横坐标位置、初始纵坐标位置构成的初始位置与当前横坐标位置、当前纵坐标位置构成的当前位置作为足式机器人的运动轨迹。可以理解的是，当前横坐标位置和当前纵坐标位置即足式机器人进行下一步跨步之前的初始位置。
具体地，可按照如下公式计算位置：
其中，惯性导航坐标系采用东北天坐标系，E表示地理东(横坐标)，N表示地理北(纵坐标)，k表示总步数，n表示跨步周期序号，d表示步长，θ表示 方向角(跨步方向与纵坐标轴之间的夹角)。
进一步地，在计算得到运动轨迹后，可根据运动轨迹来规划行进路径，根据行进路径来对机器人各部位进行控制，具体地可参照现有的机器人步行控制方法，例如PID(比例积分微分)控制方法，在本实施例中不进行详细赘述。如图4所示为一中优选的轨迹跟踪流程示意图。
在本实施例中，通过对足式机器人的传感器数据进行姿态解算得到足式机器人的姿态信息，同时对传感器数据进行步态检测得到足式机器人的跨步周期和步长，从姿态信息中获取跨步周期对应的方向角，根据方向角和步长计算得到足式机器人的运动轨迹，实现了对足式机器人的运动轨迹的可靠跟踪，从而能够根据计算得到的准确运动轨迹实现机器人的精准控制。
进一步地，基于上述第一实施例，提出本发明足式机器人轨迹跟踪方法第二实施例，在本实施例中，足式机器人的传感器包括三轴加速度计、三轴陀螺仪和三轴磁力计，步骤S10包括：
步骤S101，根据三轴陀螺仪数据计算第一姿态四元数；
步骤S102，采用三轴加速度计数据对第一姿态四元数进行第一次修正，得到第二姿态四元数；
步骤S103，采用三轴磁力计数据对第二姿态四元数进行第二次修正，得到第三姿态四元数；
在本实施例中，可按照基于四元数的EKF姿态解算算法来进行姿态解算。如图4所示，为本实施例中姿态解算的流程框图。具体地，先根据三轴陀螺仪采集的数据计算(也称为更新)姿态四元数(称为第一姿态四元数以示区别)。再采用三轴加速度计采集的数据对第一姿态四元数进行第一次修正，得到第二姿态四元数。具体地，可通过三轴加速度计采集的数据和估计的重力方向误差对姿态四元数进行第一次修正。也即，根据物体当前姿态，将加速度计采集的数据通过姿态角解算出的旋转矩阵进行旋转，从物体坐标系，转换到导航坐标系(东北天)下，此时，z轴方向加速度即为估计的重力加速度，理论上应当是g，但是实际存在误差；此时，计算出误差，误差较大即可将误差补偿调大，误差较小即可将误差补偿调小，以完成第一次修正。
再采用三轴磁力计对第二姿态四元数进行第二次修正，得到第三姿态四元数。具体地，对于同一地点，无论姿态如何，地磁强度方向是一定的(地磁南指 向地磁北)，所以可以地磁方向进行第二次矫正，和上述第一次修正过程类似，在此不详细赘述。
步骤S104，将第三姿态四元数转换为欧拉角，将欧拉角作为足式机器人的姿态信息。
在得到第三姿态四元数后，即可将第三姿态四元数转换为欧拉角，将欧拉角作为足式机器人的姿态信息。其中，四元数转换为欧拉角的方式可参考现有的转换方式，在本实施例中不作详细赘述。
在本实施例中，将基于四元数的EKF姿态解算算法应用于足式机器人的轨迹预测中，实现了足式机器人运动轨迹的精确估计，从而为足式机器人的运动控制提供了准确的数据依据。
进一步地，基于上述第一和/或第二实施例，提出本发明足式机器人轨迹跟踪方法第三实施例，在本实施例中，步骤S20包括：
步骤S201，对传感器数据进行峰值检测得到各最小峰值点；
步骤S202，将各最小峰值点中相邻两个最小峰值点所限定的周期作为跨步周期；
具体地，由于足式机器人是以跨步的形式运动的，一只脚在开始迈步到结束迈步，其加速度和角速度都会发生变化，也即会存在加速和减速的过程，在一步开始和下一步结束时，传感器数据会存在一个最小峰值，而该最小峰值对应的时间点即为最小峰值点。两个相邻的最小峰值点所限定的周期即为一个跨步周期。传感器数据检测峰值可以是通过现有的数据峰值检测方法，例如滑窗+最小值检测方法，在本实施例中不作详细赘述。需要说明的是，可以采用多个传感器的数据来进行峰值检测，以提高峰值点的检测精度。当包括多个传感器的数据时，可先对各个传感器数据进行融合，再对融合后的数据进行峰值检测，也可以对各个传感器的数据分别检测最小峰值点后，再对各个传感器的最小峰值点进行融合得到最终的最小峰值点。例如，在一实施方式中，可以采用三轴加速度计数据和三轴陀螺仪数据来检测峰值，在一个跨步周期结束时刻和下一个跨步周期的开始时刻，三轴加速度计数据和三轴陀螺仪数据的幅值将达到最小峰值；可以将根据三轴加速度计数据检测出的最小峰值点和三轴陀螺仪数据的最小峰值点进行平均，进而得到最终的最小峰值点。
步骤S203，根据跨步周期内的传感器数据计算得到跨步周期对应的步长。
在检测到跨步周期后，可根据跨步周期内的传感器数据计算得到跨步周期对应的步长。具体地，可以采用三轴加速度计采集的跨步周期内的加速度值来就算出步长。
进一步地，步骤S203包括：
步骤S2031，获取跨步周期内传感器数据中的最大总加速度计值和最小总加速度计值；
步骤S2032，根据预设比例因子、最大总加速度计值和最小总加速度计值计算得到跨步周期对应的步长。
在一实施方式中，可获取跨步周期内三轴加速度计在各个时间点采集的加速度值，对于每一个时间点，三轴加速度计会采集三个方向的加速度值，将这个三个加速度值求平均，得到该时间点对应的总加速度计值。比较各个时间点的总加速度计值，即可得到跨步周期内的最大总加速度计值和最小总加速度计值。进一步地根据预设比例因子、最大总加速度计值和最小总加速度计值计算得到跨步周期对应的步长。在一实施方式中，可以计算最大总加速度计值和最小总加速度计值的差值，利用预设比例因子与该差值的立方根值计算得到步长。具体地，可按照如下计算公式计算步长：
其中，l是步长，k是预设比例因子，a
max是最大总加速度计值，a
min是最小总加速度计值。由于不同的足式机器人存在硬件上的差异，根据加速度值来计算步长的误差也不同，所以上述公式中设置了一个预设比例因子k。该比例因子可以预先标定，也即，通过外界设备测量一步实际走了多远，然后通过上述公式计算出初始的k。
进一步地，步骤S40之后，还包括：
步骤S50，计算运动轨迹与足式机器人的规划轨迹之间的误差；
在计算得到足式机器人的运动轨迹后，可以计算运动轨迹与足式机器人的规划轨迹之间的误差。也即，运动轨迹是机器人实际的轨迹，规划轨迹是机器人预先规划要走的轨迹。例如，在2秒内，机器人从坐标(0，0)移动到了(0.5，0.5)，(0，0)--(0.5，0.5)即为这两秒内的轨迹，为测量值。理论上控制足式机器人走到(0.6，0.6)，但测量出实际走了(0.5，0.5)，计算出两者之间的误差，具体可以计算两者之间的距离，作为误差。
步骤S60，根据误差调整预设比例因子，以基于调整后的预设比例因子进行 下一跨步周期对应步长的计算，其中，误差与预设比例因子正相关。
计算得到误差后，可根据误差调整预设比例因子，在计算下一步步行周期的步长时，根据调整后的预设比例因子来计算。其中，误差与预设比例因子正相关，也即，误差越大，预设比例因子越大，误差越小，预设比例因子越小，可以根据这个关系设置根据误差计算预设比例因子的公式，例如线性公式，在本实施例中对计算方式并不做限制。需要说明的是，由于足式机器人每走一步的误差不同，所反映的运动状态也不同，而不同的运动状态下，根据加速度值来计算步长的误差也不同，因此，足式机器人可每走一步即根据该跨步周期的运动轨迹和规划轨迹之间的误差动态调整一次预设比例因子，以适应足式机器人在运动过程中的不同运动状态，从而使得步长计算更加准确。
进一步地，步骤S202之前，还包括：
步骤S204，去除各最小峰值点中值大于预设阈值的峰值点。
在一实施方式中，在根据传感器数据检测得到各个最小峰值点后，可以对这些最下峰值点进行滤波，也即，由于传感器自身硬件原因，或者足式机器人在较滑的路面行走出现打滑等突发性移动的原因，传感器数据可能会存在峰值噪声，对此，可添加添加阈值滤波。预设阈值可以是预先根据经验进行设置的值，当最小峰值点对应的值大于或等于该预设阈值时，则认为该最小峰值点是步态点，当大于该预设阈值时，则该最小峰值点是噪声干扰。故可将各最小峰值点中值大于预设阈值的峰值点去除，从而避免噪声影响轨迹跟踪的精度。
进一步地，步骤S50之后，还包括：
步骤S60，根据误差调整预设阈值，以基于调整后的预设阈值进行下一跨步周期对应峰值点的去除，其中，误差与预设阈值正相关。
在计算得到运动轨迹与足式机器人的规划轨迹之间的误差后，还可以根据误差调整预设阈值，在计算下一步步行周期时，可以调整后的预设阈值来进行峰值点的去除。其中，误差与预设阈值正相关，也即，误差越大，预设阈值越大，误差越小，预设阈值越小，可以根据这个关系设置根据误差计算预设阈值的公式，例如线性公式，在本实施例中对计算方式并不做限制。需要说明的是，由于足式机器人每走一步的误差不同，所反映的运动状态也不同，而不同的运动状态下，步行周期的最小峰值点的峰值也不同，因此，足式机器人可每走一步即根据该跨步周期的运动轨迹和规划轨迹之间的误差动态调整一次预设阈值，以适应足式机器人在运动过程中的不同运动状态，从而更加准确地排除噪 声，提高检测步态周期的准确地，从而提高轨迹跟踪的精度。
此外，此外本发明实施例还提出一种足式机器人轨迹跟踪装置，参照图5，装置包括：
姿态解算模块10，用于根据足式机器人的传感器数据进行姿态解算得到足式机器人的姿态信息；
步态检测模块20，用于根据传感器数据进行步态检测得到足式机器人的跨步周期和跨步周期对应的步长；
获取模块30，用于从姿态信息中获取跨步周期对应的方向角；
计算模块40，用于根据方向角和步长计算得到足式机器人的运动轨迹。
进一步地，足式机器人的传感器包括三轴加速度计、三轴陀螺仪和三轴磁力计，姿态解算模块10包括：
第一计算单元，用于根据三轴陀螺仪数据计算第一姿态四元数；
第一修正单元，用于采用三轴加速度计数据对第一姿态四元数进行第一次修正，得到第二姿态四元数；
第二修正单元，用于采用三轴磁力计数据对第二姿态四元数进行第二次修正，得到第三姿态四元数；
转换单元，用于将第三姿态四元数转换为欧拉角，将欧拉角作为足式机器人的姿态信息。
进一步地，步态检测模块20包括：
检测单元，用于对传感器数据进行峰值检测得到各最小峰值点；
第一确定单元，用于将各最小峰值点中相邻两个最小峰值点所限定的周期作为跨步周期；
第二计算单元，用于根据跨步周期内的传感器数据计算得到跨步周期对应的步长。
进一步地，第二计算单元包括：
获取子单元，用于获取跨步周期内传感器数据中的最大总加速度计值和最小总加速度计值；
计算子单元，用于根据预设比例因子、最大总加速度计值和最小总加速度计值计算得到跨步周期对应的步长。
进一步地，计算子单元还用于：
计算最大总加速度计值和最小总加速度计值的差值，利用预设比例因子与差值的立方根值计算得到步长。
进一步地，计算模块40还用于计算运动轨迹与足式机器人的规划轨迹之间的误差；
装置还包括：
调整模块，用于根据误差调整预设比例因子，以基于调整后的预设比例因子进行下一跨步周期对应步长的计算，其中，误差与预设比例因子正相关。
进一步地，装置还包括：
去除模块，用于去除各最小峰值点中值大于预设阈值的峰值点。
调整模块还用于根据误差调整预设阈值，以基于调整后的预设阈值进行下一跨步周期对应峰值点的去除，其中，误差与预设阈值正相关。
进一步地，计算模块40包括：
第三计算单元，用于将步长乘以方向角的余弦值后加上足式机器人的初始纵坐标位置，得到足式机器人的当前纵坐标位置；
第四计算单元，用于将步长乘以方向角的正弦值后加上足式机器人的初始横坐标位置，得到足式机器人的当前横坐标位置；
第二确定单元，用于根据当前纵坐标位置和当前纵坐标位置得到足式机器人的运动轨迹。
进一步地，获取模块30包括：
获取单元，用于从姿态信息中获取与跨步周期中开始时间点对应的欧拉角；
第三确定单元，用于将欧拉角中的偏航角作为跨步周期对应的方向角。
本发明足式机器人轨迹跟踪装置的具体实施方式的拓展内容与上述足式机器人轨迹跟踪方法各实施例基本相同，在此不做赘述。
此外，本发明实施例还提出一种计算机可读存储介质，存储介质上存储有足式机器人轨迹跟踪程序，足式机器人轨迹跟踪程序被处理器执行时实现上述方法实施例中任一的足式机器人轨迹跟踪方法。
本发明足式机器人轨迹跟踪设备和计算机可读存储介质的各实施例，均可参照本发明足式机器人轨迹跟踪方法各个实施例，此处不再赘述。
需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵 盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者装置不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者装置所固有的要素。在没有更多限制的情况下，由语句“包括一个......”限定的要素，并不排除在包括该要素的过程、方法、物品或者装置中还存在另外的相同要素。
上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质(如ROM/RAM、磁碟、光盘)中，包括若干指令用以使得一台终端设备(可以是手机，计算机，服务器，空调器，或者网络设备等)执行本发明各个实施例的方法。
以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。
Claims (14)
- 一种足式机器人轨迹跟踪方法，其中，所述方法包括：根据足式机器人的传感器数据进行姿态解算得到所述足式机器人的姿态信息；根据所述传感器数据进行步态检测得到所述足式机器人的跨步周期和所述跨步周期对应的步长；从所述姿态信息中获取所述跨步周期对应的方向角；根据所述方向角和所述步长计算得到所述足式机器人的运动轨迹。
- 如权利要求1所述的足式机器人轨迹跟踪方法，其中，所述足式机器人的传感器包括三轴加速度计、三轴陀螺仪和三轴磁力计，所述根据足式机器人的传感器数据进行姿态解算得到所述足式机器人的姿态信息包括：根据三轴陀螺仪数据计算第一姿态四元数；采用三轴加速度计数据对所述第一姿态四元数进行第一次修正，得到第二姿态四元数；采用三轴磁力计数据对所述第二姿态四元数进行第二次修正，得到第三姿态四元数；将所述第三姿态四元数转换为欧拉角，将所述欧拉角作为所述足式机器人的姿态信息。
- 如权利要求1所述的足式机器人轨迹跟踪方法，其中，所述根据所述传感器数据进行步态检测得到所述足式机器人的跨步周期和所述跨步周期对应的步长包括：对所述传感器数据进行峰值检测得到各最小峰值点；将各所述最小峰值点中相邻两个最小峰值点所限定的周期作为跨步周期；根据所述跨步周期内的传感器数据计算得到所述跨步周期对应的步长。
- 如权利要求3所述的足式机器人轨迹跟踪方法，其中，所述根据所述跨步周期内的传感器数据计算得到所述跨步周期对应的步长包括：获取所述跨步周期内传感器数据中的最大总加速度计值和最小总加速度计值；根据预设比例因子、所述最大总加速度计值和最小总加速度计值计算得到所述跨步周期对应的步长。
- 如权利要求4所述的足式机器人轨迹跟踪方法，其中，所述根据预设比例因子、所述最大总加速度计值和最小总加速度计值计算得到所述跨步周期对应的步长包括：计算最大总加速度计值和最小总加速度计值的差值，利用预设比例因子与所述差值的立方根值计算得到步长。
- 如权利要求4所述的足式机器人轨迹跟踪方法，其中，所述根据所述方向角和所述步长计算得到所述足式机器人的运动轨迹之后，还包括：计算所述运动轨迹与所述足式机器人的规划轨迹之间的误差；根据所述误差调整所述预设比例因子，以基于调整后的预设比例因子进行下一跨步周期对应步长的计算，其中，所述误差与所述预设比例因子正相关。
- 如权利要求6所述的足式机器人轨迹跟踪方法，其中，所述将各所述最小峰值点中相邻两个最小峰值点所限定的周期作为跨步周期之前，还包括：去除各所述最小峰值点中值大于预设阈值的峰值点。所述计算所述运动轨迹与所述足式机器人的规划轨迹之间的误差的步骤之后，还包括：根据所述误差调整所述预设阈值，以基于调整后的预设阈值进行下一跨步周期对应峰值点的去除，其中，所述误差与所述预设阈值正相关。
- 如权利要求1所述的足式机器人轨迹跟踪方法，其中，所述根据所述方向角和所述步长计算得到所述足式机器人的运动轨迹包括：将所述步长乘以所述方向角的余弦值后加上所述足式机器人的初始纵坐标位置，得到所述足式机器人的当前纵坐标位置；将所述步长乘以所述方向角的正弦值后加上所述足式机器人的初始横坐标位置，得到所述足式机器人的当前横坐标位置；根据所述当前纵坐标位置和所述当前纵坐标位置得到所述足式机器人的运动轨迹。
- 如权利要求1至8任一项所述的足式机器人轨迹跟踪方法，其中，所述从所述姿态信息中获取所述跨步周期对应的方向角包括：从所述姿态信息中获取与所述跨步周期中开始时间点对应的欧拉角；将所述欧拉角中的偏航角作为所述跨步周期对应的方向角。
- 一种足式机器人轨迹跟踪设备，其中，所述足式机器人轨迹跟踪设备包括：存储器、处理器及存储在所述存储器上并可在所述处理器上运行的足式机器人轨迹跟踪程序，所述足式机器人轨迹跟踪程序被所述处理器执行时实现如下操作：根据足式机器人的传感器数据进行姿态解算得到足式机器人的姿态信息；根据传感器数据进行步态检测得到足式机器人的跨步周期和跨步周期对应的步长；从姿态信息中获取跨步周期对应的方向角；根据方向角和步长计算得到足式机器人的运动轨迹。
- 如权利要求10所述的足式机器人轨迹跟踪设备，其中，处理器还可以用于调用存储器中存储的足式机器人轨迹跟踪程序，并执行以下操作：对所述传感器数据进行峰值检测得到各最小峰值点；将各所述最小峰值点中相邻两个最小峰值点所限定的周期作为跨步周期；根据所述跨步周期内的传感器数据计算得到所述跨步周期对应的步长。
- 如权利要求11所述的足式机器人轨迹跟踪设备，其中，处理器还可以用于调用存储器中存储的足式机器人轨迹跟踪程序，并执行以下操作：获取所述跨步周期内传感器数据中的最大总加速度计值和最小总加速度计值；根据预设比例因子、所述最大总加速度计值和最小总加速度计值计算得到所述跨步周期对应的步长。
- 如权利要求11所述的足式机器人轨迹跟踪设备，其中，处理器还可以用于调用存储器中存储的足式机器人轨迹跟踪程序，并执行以下操作：计算运动轨迹与足式机器人的规划轨迹之间的误差；根据误差调整预设比例因子，以基于调整后的预设比例因子进行下一跨步周期对应步长的计算，其中，误差与预设比例因子正相关。
- 一种计算机可读存储介质，其中，所述计算机可读存储介质上存储有足式机器人轨迹跟踪程序，所述足式机器人轨迹跟踪程序被处理器执行时实现下述的足式机器人轨迹跟踪方法：根据足式机器人的传感器数据进行姿态解算得到足式机器人的姿态信息；根据传感器数据进行步态检测得到足式机器人的跨步周期和跨步周期对应的步长；从姿态信息中获取跨步周期对应的方向角；根据方向角和步长计算得到足式机器人的运动轨迹。
Applications Claiming Priority (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110120031.0 | 2021-01-28 | ||
| CN202110120031.0A CN112904884B (zh) | 2021-01-28 | 2021-01-28 | 足式机器人轨迹跟踪方法、设备及可读存储介质 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| WO2022160811A1 true WO2022160811A1 (zh) | 2022-08-04 |
Family
ID=76119727
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| PCT/CN2021/126532 WO2022160811A1 (zh) | 2021-01-28 | 2021-10-27 | 足式机器人轨迹跟踪方法、设备及可读存储介质 |
Country Status (2)
| Country | Link |
|---|---|
| CN (1) | CN112904884B (zh) |
| WO (1) | WO2022160811A1 (zh) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112904884B (zh) * | 2021-01-28 | 2023-01-24 | 歌尔股份有限公司 | 足式机器人轨迹跟踪方法、设备及可读存储介质 |
| CN113741456A (zh) * | 2021-09-02 | 2021-12-03 | 乐聚(深圳)机器人技术有限公司 | 机器人路径目标点跟踪方法、装置、电子设备及存储介质 |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20110231050A1 (en) * | 2010-03-22 | 2011-09-22 | Goulding John R | In-Line Legged Robot Vehicle and Method for Operating |
| CN108444473A (zh) * | 2018-03-20 | 2018-08-24 | 南京华苏科技有限公司 | 一种行人室内轨迹定位方法 |
| CN110053039A (zh) * | 2018-01-17 | 2019-07-26 | 深圳市优必选科技有限公司 | 一种机器人行走中重力补偿的方法、装置及机器人 |
| CN110146077A (zh) * | 2019-06-21 | 2019-08-20 | 台州知通科技有限公司 | 移动机器人姿态角解算方法 |
| CN111123945A (zh) * | 2019-12-30 | 2020-05-08 | 同济大学 | 一种基于混合控制的双足机器人步态轨迹生成方法及应用 |
| CN112904884A (zh) * | 2021-01-28 | 2021-06-04 | 歌尔股份有限公司 | 足式机器人轨迹跟踪方法、设备及可读存储介质 |
Family Cites Families (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP4513320B2 (ja) * | 2003-12-17 | 2010-07-28 | ソニー株式会社 | ロボット装置、並びにロボット装置の運動制御方法 |
| JP5463991B2 (ja) * | 2010-03-18 | 2014-04-09 | トヨタ自動車株式会社 | ２脚歩行ロボット |
| US20130185018A1 (en) * | 2010-10-01 | 2013-07-18 | Hua Sheng | Apparatuses and Methods for Estimating the Yaw Angle of a Device in a Gravitational Reference System Using Measurements of Motion Sensors and a Magnetometer Attached to the Device |
| JP5306313B2 (ja) * | 2010-12-20 | 2013-10-02 | 株式会社東芝 | ロボット制御装置 |
| US10179407B2 (en) * | 2014-11-16 | 2019-01-15 | Robologics Ltd. | Dynamic multi-sensor and multi-robot interface system |
| CN104634345B (zh) * | 2015-01-28 | 2018-02-16 | 华侨大学 | 一种自适应步长的室内轨迹追踪方法 |
| JP6660110B2 (ja) * | 2015-07-23 | 2020-03-04 | 原田電子工業株式会社 | 歩行解析方法および歩行解析システム |
| CN106705968B (zh) * | 2016-12-09 | 2020-11-27 | 北京工业大学 | 基于姿态识别和步长模型的室内惯性导航算法 |
| CN106886155B (zh) * | 2017-04-28 | 2020-01-14 | 齐鲁工业大学 | 一种基于pso-pd神经网络的四足机器人运动轨迹控制方法 |
| CN109696908B (zh) * | 2019-01-18 | 2022-06-21 | 南方科技大学 | 机器人及其航迹设置方法和系统 |
| CN111829516B (zh) * | 2020-07-24 | 2024-04-05 | 大连理工大学 | 一种基于智能手机的自主式行人定位方法 |
-
2021
- 2021-01-28 CN CN202110120031.0A patent/CN112904884B/zh active Active
- 2021-10-27 WO PCT/CN2021/126532 patent/WO2022160811A1/zh active Application Filing
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20110231050A1 (en) * | 2010-03-22 | 2011-09-22 | Goulding John R | In-Line Legged Robot Vehicle and Method for Operating |
| CN110053039A (zh) * | 2018-01-17 | 2019-07-26 | 深圳市优必选科技有限公司 | 一种机器人行走中重力补偿的方法、装置及机器人 |
| CN108444473A (zh) * | 2018-03-20 | 2018-08-24 | 南京华苏科技有限公司 | 一种行人室内轨迹定位方法 |
| CN110146077A (zh) * | 2019-06-21 | 2019-08-20 | 台州知通科技有限公司 | 移动机器人姿态角解算方法 |
| CN111123945A (zh) * | 2019-12-30 | 2020-05-08 | 同济大学 | 一种基于混合控制的双足机器人步态轨迹生成方法及应用 |
| CN112904884A (zh) * | 2021-01-28 | 2021-06-04 | 歌尔股份有限公司 | 足式机器人轨迹跟踪方法、设备及可读存储介质 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112904884B (zh) | 2023-01-24 |
| CN112904884A (zh) | 2021-06-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107084717B (zh) | 用于基于磁场地图的定位系统的移动终端及其方法 | |
| CN104121905B (zh) | 一种基于惯性传感器的航向角获取方法 | |
| WO2022160811A1 (zh) | 足式机器人轨迹跟踪方法、设备及可读存储介质 | |
| KR20190041315A (ko) | 관성 기반 항법 장치 및 상대사전적분에 따른 관성 기반 항법 방법 | |
| CN106370178B (zh) | 移动终端设备的姿态测量方法及装置 | |
| WO2016198009A1 (zh) | 一种检测航向的方法和装置 | |
| CN111896007B (zh) | 一种补偿足地冲击的四足机器人姿态解算方法 | |
| JP2012173190A (ja) | 測位システム、測位方法 | |
| JP2010112854A (ja) | 歩行者用ナビゲーション装置および歩行者用ナビゲーション装置における移動方向検出方法 | |
| CN108132053B (zh) | 一种行人轨迹构建方法、系统及惯性测量装置 | |
| JPWO2018139621A1 (ja) | 慣性計測方法と慣性計測装置及び慣性計測プログラム | |
| JP2019078560A (ja) | ジャイロセンサのオフセット補正装置、オフセット補正プログラム、歩行者自律航法装置 | |
| CN111949929A (zh) | 一种多传感器融合的四足机器人运动里程计设计方法 | |
| CN103076023A (zh) | 一种步长计算方法和装置 | |
| US10466054B2 (en) | Method and system for estimating relative angle between headings | |
| JP5511088B2 (ja) | 自律測位に用いる重力ベクトルを補正する携帯装置、プログラム及び方法 | |
| CN109866217B (zh) | 机器人里程定位方法、装置、终端设备及计算机存储介质 | |
| CN113532477A (zh) | 一种骑行码表设备及骑行码表初始姿态自动校准方法 | |
| CN106931965B (zh) | 一种确定终端姿态的方法及装置 | |
| CN117213476A (zh) | 一种隧道施工人员定位方法及装置 | |
| JP6384194B2 (ja) | 情報処理装置、情報処理方法及び情報処理プログラム | |
| TW201416815A (zh) | 用於六軸運動姿態感測之方法及其感測系統 | |
| CN114789439A (zh) | 斜坡定位校正方法、装置、机器人及可读存储介质 | |
| CN114252068A (zh) | 一种行人轨迹生成方法、装置、电子设备及存储介质 | |
| JP7156445B1 (ja) | 携帯端末、歩行ロボット、プログラム、および位置演算支援方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| 121 | Ep: the epo has been informed by wipo that ep was designated in this application |
Ref document number: 21922402 Country of ref document: EP Kind code of ref document: A1 |
|
| NENP | Non-entry into the national phase |
Ref country code: DE |
|
| 122 | Ep: pct application non-entry in european phase |
Ref document number: 21922402 Country of ref document: EP Kind code of ref document: A1 |