Step height calculation method for humanoid robot

Abstract

The application discloses a kind of step height calculation method of humanoid robot, obtains depth image based on the visual sensor of the self setting of humanoid robot, and the height obtained by automatic calculation is provided for humanoid robot real-time calculation to adjust the height of landing point to realize the autonomous and stable climbing steps, so as to not need to rely on external assistance to complete the autonomous robot on a variety of steps A kind of general calculation method.The application solves the problem that the existing humanoid robot adopts fixed height control algorithm when climbing steps, and cannot realize real-time calculation according to step height to adjust the height of landing point when climbing steps.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of simulation robot technology, and particularly relates to a step height calculation method of a humanoid robot. BACKGROUND

[0002] Humanoid robots are divided into two categories from the perspective of joint driving, active humanoid robots and passive humanoid robots. Each joint of the active humanoid robot is configured with a driving device, and the tracking of the joint trajectory is realized through high gain control of each joint of the robot. The passive humanoid robot does not have a driving device for each joint, but makes full use of gravity or joints with passive characteristics during walking to realize the energy efficiency of passive walking.

[0003] The existing humanoid robot adopts a fixed height control algorithm when climbing a step, and cannot realize real-time calculation according to the step height to adjust the landing point height when climbing a step.

[0004] The information disclosed in this BACKGROUND section is only for the purpose of increasing the understanding of the background of the present application and should not be taken as an acknowledgment or any form of suggestion that this information forms prior art that is publicly known. SUMMARY

[0005] In order to overcome the defects of the prior art, a step height calculation method of a humanoid robot is provided to solve the problem that the existing humanoid robot adopts a fixed height control algorithm when climbing a step, and cannot realize real-time calculation according to the step height to adjust the landing point height when climbing a step.

[0006] In order to achieve the above-mentioned purpose, a step height calculation method of a humanoid robot is provided, comprising the following steps:

[0007] a. Collecting ground point cloud data of the sole of the robot through a vision sensor installed on the upper part of the robot;

[0008] b. Obtaining a ground plane and a first normal line of the ground plane through least square fitting;

[0009] c. Based on a preset step height range, dividing the point cloud data of the ground into a plurality of slice spaces in the direction of the first normal line within a step height range above the ground plane;

[0010] d. Based on the first normal line, fitting a top surface of a first step in a plurality of slice spaces;

[0011] e. Based on the position of the top surface, determining a first point cloud cluster of the top surface in the ground point cloud data;

[0012] f. edge extraction is performed on the first point cloud cluster to obtain the contour of the top surface;

[0013] g. a profile line of the contour close to the robot is used to determine an initial vertical surface of the first step and obtain a second normal line of the initial vertical surface;

[0014] h. based on the initial vertical surface, a second point cloud cluster of an actual vertical surface is obtained from the ground point cloud data;

[0015] i. steps c-h are repeated to obtain the first point cloud cluster and the second point cloud cluster of the remaining steps from bottom to top to determine the real-time height of each step.

[0016] Further, after step i is implemented, it further comprises: step j, the robot steps on the current step based on the real-time height of the current step, and the real-time height of the step is corrected through kinematics verification of the robot.

[0017] Further, the robot comprises a lifting foot and a supporting foot, and step j comprises:

[0018] when the lifting foot of the robot is lifted and falls on the top surface of the current step, a first coordinate value of the lifting foot and a second coordinate value of the supporting foot are obtained;

[0019] based on the first coordinate value and the second coordinate value, a height difference value between the lifting foot and the supporting foot is obtained;

[0020] based on the comparison between the height difference value and the real-time height of the current step, the real-time height of the step is corrected through kinematics verification of the robot.

[0021] Further, before step b is implemented, the ground point cloud data is pre-processed, and the pre-processing comprises filtering, removing noise points and stray points.

[0022] Further, the thickness of the slice space is 3-5 cm.

[0023] The beneficial effects of the present application are that the step height calculation method of the humanoid robot in the present application obtains a depth image based on the visual sensor set on the humanoid robot, and automatically calculates the height through image processing means, and then provides the humanoid robot with real-time calculation to adjust the foot landing height to realize autonomous and stable climbing of steps, so that a general calculation method is provided for the robot to autonomously climb various steps without relying on external assistance. BRIEF DESCRIPTION OF DRAWINGS

[0024] Other features, objects and advantages of the present application will become more apparent from the following detailed description of non-limiting embodiments made with reference to the accompanying drawings.

[0025] Figure 1 The figure is a schematic diagram of the state of lifting the foot when the first step of the embodiment of the human-simulating robot.

[0026] Figure 2 The figure is a schematic diagram of the overall structure design of the human-simulating robot.

[0027] Figure 3 The figure is a schematic diagram of the local space coordinate system of each motor of the human-simulating robot.

[0028] Figure 4 The figure is a schematic diagram of the joint coordinate system of each joint of the human-simulating robot.

[0029] Figure 5 The figure is a schematic diagram of the joint connecting rod of each joint of the human-simulating robot.

[0030] Figure 6 The figure is a schematic diagram of the coordinate system and connecting rod of the joint bending of the human-simulating robot. DETAILED DESCRIPTION

[0031] The application will be described in further detail below with reference to the drawings and embodiments. It can be understood that the specific embodiments described herein are only used to explain the related application, and are not a limitation on the application. In addition, it should be noted that only the parts related to the application are shown in the drawings for ease of description.

[0032] It should be noted that the embodiments in the present application and the features in the embodiments can be combined with each other without conflict. The present application will be described in detail below with reference to the drawings and embodiments.

[0033] Referring to Figures 1 to 6 The present application provides a step height calculation method for a human-simulating robot, which comprises the following steps:

[0034] a. Collecting the ground point cloud data of the sole of the robot 1 through the vision sensor 3 installed on the upper part of the robot 1.

[0035] Referring to Figure 1 In the present embodiment, the vision sensor is a depth camera. The vision sensor is installed on the head of the robot. The installation height H of the vision sensor, i.e. the vertical distance from the vision sensor to the sole of the robot, is known data information.

[0036] Taking the contact point between the robot's foot and the ground plane as the origin, and the direction perpendicular to the ground plane upward as the Z direction, the distribution of various large planes to the visual sensor can be divided by the plane height values ​​of each step in the Z direction, based on the height H of the robot's own visual sensor. Among them, the ground plane has the largest area in the histogram. Based on the H value (near the distance from the visual sensor to the ground), the ground point cloud clusters, i.e., the ground point cloud data, are then determined.

[0037] b. Obtain the ground plane D and its first normal by fitting the data using the least squares method.

[0038] After determining the point cloud clusters on the ground plane, the ground point cloud data is preprocessed, including filtering, noise removal, and artifact removal.

[0039] After preprocessing the ground point cloud data, the ground plane is fitted using the least squares method, and the normal to the ground plane can be calculated simultaneously. From the K nearest neighbors of any point on the ground plane, the covariance matrix C for each point is calculated:

[0040] C = 1 / k × ∑ k i＝1 (Pi-P)×(Pi-P) T );

[0041] Where k is the number of neighboring points of point Pi, and P represents the three-dimensional centroid of the nearest neighbor element.

[0042] c. Based on a preset step height range, within a step height range above the ground plane D, the point cloud data of the ground is divided into multiple slice spaces along the first normal direction.

[0043] d. Based on the first normal, the top surface 20 of the first step 2 is obtained by fitting in multiple slice spaces.

[0044] As a preferred implementation method, the thickness of the slicing space is 3cm to 5cm.

[0045] Specifically, along the Z-axis, the space within a step height range above the ground plane is divided into multiple cut spaces. The height of the cut spaces can be customized, but considering that the top surface of the step may not be completely parallel to the ground plane, it should be able to surround the top surface of the step and the height at which it generates noise (e.g., 3cm to 5cm). In addition, the conventional height of each step is 15cm to 30cm. Thus, using the ground plane as the reference plane, up to a height of 30cm above the ground plane, the space within this height range is divided into multiple pieces, such as 6 pieces (5cm each) or 10 pieces (3cm each), that is, the thickness of the sliced ​​space is 3cm to 5cm.

[0046] e. Based on the position of the top surface 20, determine the first point cloud cluster of the top surface 20 in the ground point cloud data.

[0047] To find the point cloud of the top surface of the step in each slice space, we can also create a histogram in the Z direction for each slice space. The point that falls most frequently on the histogram must be the top surface of the step, thus determining the top surface of the step. At the same time, repeating step b can determine the normal direction of the top surface of the step.

[0048] By comparing the normal direction of the top surface of the step with the normal direction of the ground plane, since the top surface of the step is roughly parallel to the ground plane, the angle between the normal direction of the top surface of the step and the normal direction of the ground plane should be small, such as less than 10 degrees, and then the first point cloud cluster on the top surface of the step can be determined.

[0049] f. Extract the edges of the first point cloud cluster to obtain the outline of the top surface 20.

[0050] g. Determine the initial elevation of the first step 2 using the outline of the side of the contour closest to robot 1 and obtain the second normal of the initial elevation.

[0051] h. Based on the initial facade, obtain the second point cloud cluster of the actual facade 200 from the ground point cloud data.

[0052] Since the elevation of the steps is perpendicular to the top surface of the steps and the ground plane, the plane that passes through the edge line of the top surface 20 obtained in step f and is closest to the robot is used as the initial elevation, thereby obtaining the second normal of the initial elevation.

[0053] i. Repeat steps c to h, acquiring the first and second point cloud clusters of the remaining steps from bottom to top to determine the real-time height of each step.

[0054] A cubic space is formed by expanding outwards along the second normal direction from the initial elevation as the center plane. A histogram is plotted along the second normal direction. Since the actual elevation of the steps is perpendicular to the top surface of the steps, the angle between the normal of the actual elevation and the normal of the initial elevation should be less than a certain angle value, such as less than 10 degrees. Based on this angle value, the second point cluster and the second normal of the actual elevation of the steps are determined.

[0055] Repeat steps c to h, acquiring the first and second point cloud clusters of the remaining steps from bottom to top to determine the real-time height of each step.

[0056] Step j: Robot 1 steps onto the current step based on the real-time height of the current step, and the real-time height of the step is corrected through kinematic verification by Robot 1.

[0057] Robot 1 includes a raised foot 11 and a supporting foot 12. Step j includes:

[0058] j-1. When robot 1 lifts its foot 11 and falls onto the top surface 20 of the current step, obtain the first coordinate value of the lifted foot 11 and the second coordinate value of the supporting foot 12.

[0059] j-2. The height difference between the raised foot 11 and the supporting foot 12 is calculated based on the first coordinate value and the second coordinate value.

[0060] j-3. Based on the height difference and the real-time height of the current step, the real-time height of the step is corrected through kinematic verification by robot 1.

[0061] For details, please refer to Figure 2 As shown, the bipedal robot of this invention has 12 degrees of freedom in its leg mechanism, six for each leg. The links of each joint are numbered from LLEG_J0 to LLEG_J5 (left leg) and RLEG_J0 to RLEG_J5 (right leg), respectively. Each joint has one drive motor. The extensions of the axes of the three motors at the hip joint intersect at a single point. The origin of their local coordinate system is set at this intersection point, as shown below. Figure 3 As shown.

[0062] Similarly, at the ankle joint, the extensions of the two axes intersect at a point, and the origins of the corresponding two local coordinate systems can be set at this intersection point. Here, W is the world coordinate system.

[0063] Next, we define the joint axis vector aj and the relative position vector bj to describe the relationship between adjacent local coordinate systems, such as Figure 4 , Figure 5 As shown.

[0064] The joint axis vector is a unit vector describing the rotation of a link relative to its parent link, and its direction is determined by the right-hand rule. Taking the joint axis vector of the knee joint as an example, a5 = a11 = [0 1 0]. T This setting ensures that the positive direction of joint rotation aligns with the normal bending direction of the knee joint. The relative position vector bj describes the position of the origin of a local coordinate system within its parent link coordinate system. For example... Figure 4 As shown, b3 and b4 are three local coordinate points that coincide at the hip joint, so b3 = b4 = 0 and b9 = b10 = 0. At the ankle joint, the two local coordinate points also coincide, so b7 = b13 = 0.

[0065] The method for establishing the pose of each link using forward kinematics is as follows:

[0066] The poses of each link are then established using forward kinematics:

[0067] Forward kinematics is a method for determining the position and orientation of the link end based on joint angles.

[0068] The Rodrigues equations give the rotation matrix of the constant angular velocity vector.

[0069] e aθ =E + a*sinθ + a^2*(1-cosθ);

[0070] In the parent link coordinate system, the rotation axis vector is aj, the origin of Σj is bj, the joint angle is qj, and E is the initial condition, that is, when the joint angle is 0, the link is in the initial state and the attitude matrix is ​​E.

[0071] like Figure 6 As shown, aj and bj are the joint axis vectors and the positions of their origins in the parent link coordinate system.

[0072] The homogeneous transformation matrix of Σj relative to its parent link is:

[0073] i Tj = [e ajqj bj, [0 0 0]1]

[0075] Let's look at it again. Figure 6 As shown in the case of two interconnected links, assuming the absolute position pi and orientation Ri of the parent link are known, then the homogeneous transformation matrix of Σi is...

[0076] Ti=[Ri pi, [0 0 0]1]

[0078] The homogeneous transformation matrix of Σj can be obtained according to the chain rule:

[0079] Tj=Ti* i Tj

[0080] From the above, the position and orientation (pj, Rj) of Σj can be obtained:

[0081] pj = pi + Ri * bj;

[0082] Rj=Ri*e ajqj

[0083] This allows us to determine the pose of all links. The pseudocode for this method is as follows:

[0084]

[0085] The method for calculating the step height of a humanoid robot according to the present invention is based on the humanoid robot's own visual sensor to acquire depth images and automatically calculate the height through image processing. This enables the humanoid robot to climb steps autonomously and stably, thus providing a universal calculation method for the robot to autonomously climb various types of steps without relying on external assistance.

[0086] The above description is merely a preferred embodiment of this application and an explanation of the technical principles employed. Those skilled in the art should understand that the scope of the invention involved in this application is not limited to technical solutions formed by specific combinations of the above-described technical features, but should also cover other technical solutions formed by arbitrary combinations of the above-described technical features or their equivalents without departing from the inventive concept. For example, technical solutions formed by substituting the above features with (but not limited to) technical features with similar functions disclosed in this application.

Claims

1. A method for calculating the step height of a humanoid robot, characterized in that, Includes the following steps: a. Collect ground point cloud data of the robot's feet using a vision sensor installed on the upper part of the robot; b. Obtain the ground plane and its first normal line by fitting the least squares method; c. Based on a preset step height range, within a step height range above the ground plane, the point cloud data of the ground is divided into multiple slice spaces along the first normal direction; d. Based on the first normal, fit the top surface of the first step in the multiple slice spaces to obtain the top surface of the first step; e. Based on the position of the top surface, determine the first point cloud cluster of the top surface in the ground point cloud data; f. Perform edge extraction on the first point cloud cluster to obtain the outline of the top surface; g. Determine the initial elevation of the first step using the contour line on the side of the outline closest to the robot and obtain the second normal of the initial elevation. h. Based on the initial facade, obtain the second point cloud cluster of the actual facade from the ground point cloud data; i. Repeat steps c to h, acquiring the first point cloud cluster and the second point cloud cluster of the remaining steps from bottom to top to determine the real-time height of each step.

2. The method for calculating the step height of a humanoid robot according to claim 1, characterized in that, After implementing step i, the procedure further includes: step j, whereby the robot steps onto the current step based on the real-time height of the current step, and corrects the real-time height of the step through kinematic verification of the robot.

3. The method for calculating the step height of a humanoid robot according to claim 2, characterized in that, The robot includes a raised leg and a supporting leg, and step j includes: When the robot's raised foot lifts up and lands on the top surface of the current step, the first coordinate value of the raised foot and the second coordinate value of the supporting foot are obtained; The height difference between the raised foot and the supporting foot is calculated based on the first coordinate value and the second coordinate value. Based on the comparison between the height difference and the real-time height of the current step, the real-time height of the step is corrected through kinematic verification by the robot.

4. The method for calculating the step height of a humanoid robot according to claim 1, characterized in that, Before implementing step b, the ground point cloud data is preprocessed, including filtering, noise removal, and artifact removal.

5. The method for calculating the step height of a humanoid robot according to claim 1, characterized in that, The thickness of the sliced ​​space is 3cm to 5cm.

Images