A humanoid robot trajectory tracking method and a trajectory tracking system thereof

Abstract

The present application relates to the technical field of humanoid robot control, and discloses a humanoid robot trajectory tracking method and a trajectory tracking system thereof, which collects an image to be processed and pre-processes the image; divides the image into regions by using a region of interest (ROI), divides the image into five rectangular regions horizontally and three rectangular regions vertically; detects color blocks of the image in the regions according to the ROI; performs weighted processing on x-axis coordinates of the color blocks in ABCDE regions to obtain basic x-axis coordinates; judges a current route type according to the presence or absence of the color blocks in FG regions and the basic x-axis coordinates; and obtains a robot motion instruction according to the trajectory type and a coordinate position of a trajectory center point, judges the correctness of the motion instruction, and determines a final motion instruction of the robot. The present application divides and stores a motion process of a humanoid robot according to a period, calculates a required instruction of a current robot by using a historical instruction pool, greatly improves resource utilization, and increases the accuracy of tracking.

Description

Technical Field

[0001] This invention relates to the field of humanoid robot control technology, specifically to a humanoid robot trajectory tracking method and trajectory tracking system. Background Technology

[0002] In the field of robotics, developing bipedal humanoid walking robots with similar walking and thinking abilities to humans, capable of adapting to environmental changes, has been a focal point of academic research and a hot topic of public interest. Among these, visual trajectory tracking methods for humanoid robots have been a key focus of academic research. Despite years of research, current tracking methods for humanoid robots still have significant room for improvement in terms of stability and adaptability. The realization and improvement of these capabilities depend on the combined development of mechanical structure and algorithm design.

[0003] Current robot trajectory tracking methods include: visual tracking methods that use visual sensors (such as cameras) to capture target images and achieve tracking through image processing algorithms; radar scanning methods that use lidar to scan the target and achieve tracking through data processing; and sensor measurement methods that use millimeter-wave or ultrasonic sensors to measure the target and achieve tracking through data processing.

[0004] The binocular vision-based mobile robot environment recognition system proposed in "CNKI:SUN:SYSY.0.2022-05-014" employs a method of analyzing images frame by frame to plan the robot's future route based on the current environment. When the algorithm calculates a new route, it discards the previously calculated route, thus adjusting the robot's motion posture in real time. However, when the robot runs in a real-world scenario, most of the results generated by the algorithm are redundant data, which are directly discarded by the program. This results in extremely low image utilization and low trajectory tracking accuracy, leading to wasted resources and ineffective computation.

[0005] Meanwhile, since robot trajectory tracking requires processing a large amount of data and efficient computing power, how to reuse limited image and computing resources has always been a hot research topic in academia. Summary of the Invention

[0006] Purpose of the invention: To address the problems existing in the prior art, this invention provides a humanoid robot trajectory tracking method and system, which divides and stores the humanoid robot's motion process according to a cycle, and uses a historical instruction pool to calculate the instructions required by the robot at present, greatly improving resource utilization and thus increasing the accuracy of tracking.

[0007] Technical Solution: This invention provides a humanoid robot trajectory tracking method, based on a bipedal humanoid robot and an OpenMV4 monocular camera vision system. The monocular camera vision system is installed in the robot's chest cavity, and includes the following steps:

[0008] Step 1: The monocular camera vision system acquires the image to be processed and performs preprocessing on the image while acquiring it;

[0009] Step 2: Use the Region of Interest (ROI) to divide the preprocessed image into regions. Divide the image horizontally into five rectangular regions and vertically into three rectangular regions. The horizontal regions are A, B, C, D and E from top to bottom. The vertical regions ignore the middle region and only include the leftmost region F and the rightmost region G.

[0010] Step 3: Detect color patches in the divided regions of the image according to ROI;

[0011] Step 4: Weight the x-axis coordinates of the color blocks in regions ABCDE to obtain the basic x-axis coordinates; determine the current route type based on the presence or absence of color blocks in region FG and the basic x-axis coordinates, and then derive the robot motion command based on the trajectory type and the coordinate position of the trajectory center point.

[0012] Step 5: Determine the correctness of the motion commands obtained in Step 4, thereby determining the robot's final motion commands.

[0013] Furthermore, the preprocessing operation in step 1 includes: performing distortion correction and image compensation on the image, using the LAB color space model, turning all pixels in the acquired image that are within the threshold range into black, and turning the remaining pixels into white.

[0014] Furthermore, the specific operation of detecting color blocks according to ROI in step 3 is as follows: find color blocks in each region, output the coordinates of the center point of the color block and draw a cross mark on it. If there is no color block, output the coordinates of the center point of the entire image to be included as negligible error in the calculation process.

[0015] Furthermore, the weighted processing of the x-axis coordinates of the color blocks in regions ABCDE in step 4 specifically involves:

[0016] With the center of the entire image as the origin of the binary coordinate system, the coordinates of the upper left corner of region A are (-80, 60), and the coordinates of the lower right corner of region E are (80, -60). Within regions ABCDE, the y-axis coordinates are ignored, so the x-axis coordinates of the color blocks within them are ax, bx, cx, dx, and ex, respectively.

[0017] Divide each of the five coordinates above by 80 for normalization, then weight them according to the ratio of 1, 2, 3, 2, 2. The final basic x-axis coordinates are shown below:

[0018] x=1×ax+2×bx+3×cx+2×dx+2×ex.

[0019] Furthermore, the specific operation of judging the correctness of the motion command in step 5 is as follows:

[0020] Step 5.1: Compare the motion command obtained in Step 4 with the historical motion commands within one engineering cycle. If the motion commands are the same, record a value of 1; otherwise, record a value of -1.

[0021] Step 5.2: Assign corresponding weights to each comparison value according to its position within the project cycle of the instruction being compared, using the following formula:

[0022]

[0023] Where x represents the current position within the project cycle. If a project cycle length t is set to 10 seconds, the current project cycle starts from 0 seconds, and the current time t1 is 2.3 seconds, then x = 0.23. That is, divide the current time by the project cycle and take the remainder, divide the resulting number by the project cycle length t, and you can calculate x. Then, substitute it into the formula to get the weight of the current instruction.

[0024] Step 5.3: Let there be α motion commands accumulated in the current project cycle, and the project cycle length be t. The values ​​obtained by comparing each historical motion command with the current command are α1, α2, α3, ..., respectively. Therefore, the invalidation command is obtained as shown in the following formula:

[0025]

[0026] If y < 0, the current instruction is invalidated and will not be entered into the instruction history pool; if y ≥ 0, the current instruction is entered into the instruction history pool.

[0027] Step 5.4: After the robot finishes one action, the motion command that appears most frequently within a project cycle is selected as the next action command.

[0028] Furthermore, the engineering cycle is 1.5 times the time of a single complete action of the humanoid robot.

[0029] This invention also discloses a trajectory tracking system based on the above-described humanoid robot trajectory tracking method, comprising:

[0030] A bipedal humanoid robot with 19 mechanical degrees of freedom, the bipedal humanoid robot including a servo control board;

[0031] An OpenMV4 monocular camera vision system used to acquire images to be processed.

[0032] A vision control unit is provided, wherein the vision control unit is equipped with the humanoid robot trajectory tracking method as described in any one of claims 1 to 5, and is connected to the servo control board of the bipedal humanoid robot via a UART serial port.

[0033] Beneficial effects:

[0034] 1. This patent proposes a method for dividing and storing the motion process of a humanoid robot according to cycles and using a historical instruction pool to calculate the instructions needed for the current robot. A complete and effective causal calculation system is designed. Since the visual algorithm works continuously during the humanoid robot's movement, forcibly stopping the robot during a single movement to perform other actions is impractical. Therefore, most of the motion instructions obtained by the visual algorithm are redundant data. This patent selects to use a weighted calculation method to judge the correctness of the current motion instruction. If it is judged to be incorrect, the current motion instruction is directly discarded. This method can greatly improve resource utilization and thus increase the accuracy of line tracking.

[0035] 2. In the trajectory tracking algorithm proposed in this patent, the image captured by the camera is divided into segments. Each segment is processed before being aggregated. Compared to processing the entire image, this method not only saves computational resources but also significantly improves the flexibility of debugging. During debugging, different types of trajectories can be classified based on the coordinates of the color blocks in each segment returned during the computation. Attached Figure Description

[0036] Figure 1 This is a flowchart of the humanoid robot trajectory tracking method of the present invention;

[0037] Figure 2 The image preprocessing procedure of this invention is shown in (a) where (a) represents the image acquired by the image sensor; (b) represents the corrected image; and (c) represents the image after binarization.

[0038] Figure 3 This is a schematic diagram of image region division according to the present invention;

[0039] Figure 4 This describes the image region division under different conditions according to the present invention;

[0040] Figure 5 This invention provides a two-dimensional image obtained by assigning corresponding weights to the positions within the engineering cycle. Detailed Implementation

[0041] The present invention will be further described below with reference to the accompanying drawings. The following embodiments are only used to more clearly illustrate the technical solution of the present invention, and should not be used to limit the scope of protection of the present invention.

[0042] This invention discloses a trajectory tracking method and system for a humanoid robot. The entire trajectory tracking method is based on a bipedal humanoid robot with 19 degrees of freedom and an OpenMV4 monocular camera vision system. The vision system is installed in the robot's chest cavity and can acquire images in real time during the robot's movement. The robot's two legs have a total of 10 degrees of freedom, enabling basic walking, turning, and climbing functions. The trajectory tracking algorithm program is located in the vision control unit, which is based on an STM32H750VBT6 chip and burned into the OpenMV system framework. This control unit serves as the control center of the bipedal humanoid robot used in this paper and is connected to the robot's servo control board via a UART serial port to achieve control of the robot's walking posture.

[0043] The system uses a control board built around an STM32H750VBT6 chip as the main controller. It processes images captured by an OV7725 camera using the programmed OpenMV framework, and visualizes the processed images and calculation results on an LCD screen, thus forming the hardware component of the image processing algorithm proposed in this invention. The main controller is connected to the servo control board via serial communication, thereby operating the humanoid robot motion system composed of the servo control board and nineteen servos. Hardware buttons are used to operate the robot, and multiple LED status indicators display the current working status, facilitating operator debugging and maintenance.

[0044] The humanoid robot trajectory tracking method disclosed in this invention includes the following steps:

[0045] Step 1: The monocular camera vision system acquires the image to be processed and performs preprocessing on the image while acquiring it.

[0046] Image acquisition is performed using an OV7725 CMOS digital image sensor, which outputs 20,000-pixel images (160x120 resolution). The output images are in RGB565 data format. Distortion correction and image compensation are performed on the images while they are being acquired.

[0047] Figure 2 (a) The image acquired by the OV7725 image sensor exhibits distortion due to errors in the geometric positions of image points on the image plane, resulting in a discrepancy between the image and the actual scene's central projection. After acquiring the distorted image, the image acquisition module employs software algorithms to correct the lens distortion. Figure 2(b) is the corrected image.

[0048] The image acquisition system uses the LAB color space model, which converts all pixels in the acquired image that fall within a threshold range to black, and the remaining pixels to white. Figure 2 (c) is the image after binarization.

[0049] Step 2: Divide the preprocessed image into regions using the Region of Interest (ROI), such as... Figure 3 The image is divided into five rectangular regions horizontally and three rectangular regions vertically. The horizontal regions, from top to bottom, are region A, region B, region C, region D, and region E. The vertical region ignores the middle region and only includes the leftmost region F and the rightmost region G.

[0050] Step 3: Detect color patches in the divided regions of the image according to ROI.

[0051] like Figure 4 As shown, the image with divided regions is used to detect color blocks according to ROI. The core idea of ​​color block detection is to find color blocks in each region, output the coordinates of the center point of the color block and draw a cross mark on it. If there is no color block, the coordinates of the center point of the entire image are output to be included as negligible error in the calculation process.

[0052] Step 4: Weight the x-axis coordinates of the color blocks in regions ABCDE to obtain the basic x-axis coordinates; determine the current route type based on the presence or absence of color blocks in region FG and the basic x-axis coordinates, and then derive the robot motion command based on the trajectory type and the coordinate position of the trajectory center point.

[0053] The weighted processing of the x-axis coordinates of the color blocks in regions ABCDE is as follows:

[0054] With the center of the entire image as the origin of the binary coordinate system, the coordinates of the upper left corner of region A are (-80, 60), and the coordinates of the lower right corner of region E are (80, -60). Within regions ABCDE, the y-axis coordinates are ignored, so the x-axis coordinates of the color blocks within them are ax, bx, cx, dx, and ex, respectively.

[0055] Divide each of the five coordinates above by 80 for normalization, then weight them according to the ratio of 1, 2, 3, 2, 2. The final basic x-axis coordinates are shown below:

[0056] x=1×ax+2×bx+3×cx+2×dx+2×ex.

[0057] Let the coordinates within regions F and G be represented as fx, fy, gx, and gy, respectively. First, determine the current route type based on the presence or absence of color blocks within regions F and G and their basic x-coordinates. Then, determine the route type based on the relative positions of fy and gy. Figure 4The robot identifies the basic trajectory types shown and then derives motion commands based on the trajectory type and the coordinates of the trajectory center point.

[0058] Step 5: Determine the correctness of the motion commands obtained in Step 4, thereby determining the robot's final motion commands.

[0059] Since the visual algorithm of a humanoid robot operates continuously during movement, forcibly stopping the robot during a single movement to perform other actions is impractical. Therefore, most of the motion commands generated by the visual algorithm are redundant data. This invention chooses to use a weighted calculation method to determine the correctness of the current motion command. If the command is determined to be incorrect, it is directly discarded. This method can greatly improve resource utilization and thus increase the accuracy of line tracking. The specific operation is as follows:

[0060] Step 5.1: Compare the motion command obtained in Step 4 with the historical motion commands within one engineering cycle. If the motion commands are the same, record a value of 1; if they are different, record a value of -1. This engineering cycle is generally 1.5 times the time of one complete action of the robot.

[0061] Step 5.2: Assign corresponding weights to each comparison value according to its position within the project cycle of the instruction being compared, using the following formula:

[0062]

[0063] Here, x represents the current position within the project cycle. Assuming a project cycle length t is 10 seconds, and the current project cycle starts at 0 seconds, with the current time t1 being 2.3 seconds, then x = 0.23. This is calculated by dividing the current time by the project cycle length t and taking the remainder, then dividing the result by the project cycle length t. This result is then substituted into the formula to determine the weight of the current instruction. The resulting two-dimensional image based on the assigned weights is shown below. Figure 5 As shown.

[0064] Step 5.3: Let there be α motion commands accumulated in the current project cycle, and the project cycle length be t. The values ​​obtained by comparing each historical motion command with the current command are α1, α2, α3, ..., respectively. Therefore, the invalidation command is obtained as shown in the following formula:

[0065]

[0066] If y < 0, the current instruction is invalidated and not entered into the instruction history pool; if y ≥ 0, the current instruction is entered into the instruction history pool.

[0067] Step 5.4: After the robot finishes one action, the motion command that appears most frequently within a project cycle is selected as the next action command.

[0068] The above embodiments are only for illustrating the technical concept and features of the present invention, and are intended to enable those skilled in the art to understand the content of the present invention and implement it accordingly. They should not be construed as limiting the scope of protection of the present invention. All equivalent transformations or modifications made in accordance with the spirit and essence of the present invention should be covered within the scope of protection of the present invention.

Claims

1. A humanoid robot trajectory tracking method, based on a biped humanoid robot and an openmv4 monocular camera vision system, the monocular camera vision system is installed on the chest of the robot, characterized in that, Includes the following steps: Step 1: The monocular camera vision system acquires the image to be processed and performs preprocessing on the image while acquiring it; Step 2: Use the Region of Interest (ROI) to divide the preprocessed image into regions. Divide the image vertically into five rectangular regions and horizontally into three rectangular regions. The vertical regions from top to bottom are region A, region B, region C, region D, and region E. The horizontal regions ignore the middle region and only include the leftmost region F and the rightmost region G. Step 3: Detect color patches in the divided regions of the image according to ROI; Step 4: Weight the x-axis coordinates of the color blocks in regions ABCDE to obtain the basic x-axis coordinates; determine the current route type based on the presence or absence of color blocks in region FG and the basic x-axis coordinates, and then derive the robot motion command according to the trajectory type and the coordinate position of the trajectory center point. With the center of the entire image as the origin of the binary coordinate system, the coordinates of the upper left corner of region A are (-80, 60), and the coordinates of the lower right corner of region E are (80, -60). Within regions ABCDE, the y-axis coordinates are ignored, so the x-axis coordinates of the color blocks within them are ax, bx, cx, dx, and ex, respectively. Divide each of the five coordinates above by 80 for normalization, then weight them according to the ratio of 1, 2, 3, 2, 2. The final basic x-axis coordinates are shown below: ； Step 5: Determine the correctness of the motion commands obtained in Step 4, thereby determining the robot's final motion commands; Step 5.1: Compare the motion command obtained in Step 4 with the historical motion commands within one engineering cycle. If the motion commands are the same, record a value of 1; otherwise, record a value of -1. Step 5.2: Assign corresponding weights to each comparison value according to its position within the project cycle of the instruction being compared, using the following formula: ； in, This represents the current time's position within the project cycle. It is calculated by dividing the current time by the project cycle and taking the remainder, then dividing the result by the project cycle length t. ; Step 5.3: Assume that the cumulative total during the current project cycle is There are 3 motion commands, with a project cycle length of t. The values ​​obtained by comparing each historical motion command with the current command are as follows: , , Therefore, the invalidation instruction shown in the following formula is obtained: ； If y < 0, the current instruction is invalidated and will not be entered into the instruction history pool; if y ≥ 0, the current instruction is entered into the instruction history pool. Step 5.4: After the robot finishes one action, the motion command that appears most frequently within a project cycle is selected as the next action command.

2. The humanoid robot trajectory tracking method according to claim 1, characterized in that, The preprocessing operation in step 1 includes: performing distortion correction and image compensation on the image, using the LAB color space model, turning all pixels in the acquired image that are within the threshold range into black, and turning the remaining pixels into white.

3. The humanoid robot trajectory tracking method according to claim 1, characterized in that, The specific operation of detecting color blocks according to ROI in step 3 is as follows: find color blocks in each region, output the coordinates of the center point of the color block and draw a cross mark on the center point of the color block. If there is no color block, output the coordinates of the center point of the entire image to be included as negligible error in the calculation process.

4. The humanoid robot trajectory tracking method according to claim 1, characterized in that, The engineering cycle is 1.5 times the time of a complete action of a humanoid robot.

5. A trajectory tracking system based on the humanoid robot trajectory tracking method according to any one of claims 1 to 4, characterized in that, include: A bipedal humanoid robot with 19 mechanical degrees of freedom, the bipedal humanoid robot including a servo control board; An OpenMV4 monocular camera vision system used to acquire images to be processed. A vision control unit is provided, wherein the vision control unit is equipped with the humanoid robot trajectory tracking method as described in any one of claims 1 to 4, and is connected to the servo control board of the bipedal humanoid robot via a UART serial port.

Images