CN109333534B - Preplanned real-time gait control algorithm - Google Patents

Abstract

The invention discloses a pre-planned real-time gait control algorithm. Under the control of a biped robot controller, a biped robot kinematic coordinate system model is first established, and then the robot posture planning and stability judgment are performed, and the final completion is completed. Gait walking, real-time planning of the next posture to ensure the stability of the robot's walking. In terms of the complexity of the algorithm, the amount of information required is small, and the method is simple; in terms of the timeliness of the algorithm, because the amount of information required by the present invention is small, the complexity of implementation is low, thereby reducing the processing time of the algorithm; in terms of practical application , this algorithm can combine the actual characteristics of the robot and the terrain, pre-design the action of each step of the robot so that the robot can complete the corresponding action according to the plan, and collect the front image information in real time to avoid obstacles, which ensures the success rate of walking.

Description

Pre-planned real-time gait control algorithm

technical field

The invention relates to the technical field of gait algorithms, in particular to a pre-planned real-time gait control algorithm.

Background technique

The robot concentrates the latest research results of various disciplines, represents the highest technical level of mechatronics, and provides opportunities for practice in the field of artificial intelligence. Moreover, as one of the most flexible and adaptable robots, bipedal robots have the potential to replace humans in harsh environments, and have become a research hotspot at home and abroad. Gait planning is beneficial to improve the stable walking ability of robots, so the research in this area is of great significance. At present, common gait algorithms include: compass gait algorithm, three-dimensional linear inverted pendulum model, capture point theory, etc. The existing technologies can be divided into two categories. The first type of model is simple and easy to calculate, but its stability is poor; the second type of model has strong stability and low energy consumption, but its calculation is complicated and the amount of calculation is huge.

Therefore, how to provide a gait algorithm with simple calculation and strong stability has become a problem that those skilled in the art need to solve at present.

SUMMARY OF THE INVENTION

The purpose of the present invention is to propose a pre-planned real-time gait control algorithm, which simplifies the algorithm as much as possible under the premise of ensuring stability.

In order to realize the above-mentioned tasks, the present invention adopts the following technical solutions:

A pre-planned real-time gait control algorithm includes the following steps:

Step 1, establish the kinematic coordinate system model of the biped robot

The joints on the biped robot are equipped with steering gears that drive the rotation of the joints, wherein the bare joints of the robot legs have forward joints and lateral joints, the knee joints have forward joints, and the hip joints have forward joints, Lateral joints and horizontal rotation joints;

Step 1.1, number each joint of the robot. For joint i, determine the origin of the joint. The principle of origin determination is:

When the axis of joint i intersects with the axis of joint i+1, take the intersection point as the origin of joint i; when the axis of joint i and the axis of joint i+1 are out of plane, take the common perpendiculars of the two axes and joint i+ The intersection of the 1 axis is used as the origin of the joint i; when the axis of the joint i is parallel to the axis of the joint i+1, take the intersection of the axis of the joint i+1 and the axis of the joint i+2 and the axis of the joint i+1 as the joint The origin of i is denoted as O _i ;

Step 1.2, establish each coordinate axis X _i , Y _i , Z _i of the coordinate system at joint i to establish a coordinate system at each joint, and the determination principle is:

The Z _i coordinate axis coincides with the axis of the joint i+1, the X _i coordinate axis is along the i joint axis, the common perpendicular of the i+1 joint axis and points to the i+1 joint, and the Y _i coordinate axis is determined according to the right-hand rule;

Step 1.3, according to the coordinate system established at each joint, compare the two adjacent coordinate systems O _i with O _i-1 to obtain the difference between the two adjacent coordinate systems, that is, the transformation matrix;

The kinematic relationship between the two adjacent coordinate systems is obtained through the above transformation matrix, and the coordinate system of the left knee joint of the biped robot is taken as the reference coordinate system, and the relationship between other joint coordinate systems and the reference coordinate system can be obtained. Kinematic relationship; the transformation matrix between the coordinate system at each joint and the reference coordinate system is used as the mathematical model between the joint and the joint where the reference coordinate system is located, and all mathematical models constitute the biped robot kinematics coordinate system model;

Step 2: Plan the posture of the biped robot

Step 2.1, design the walking gait process of the biped robot

The robot changes the center of gravity by swinging the legs forward. When swinging the legs forward, the center of gravity of the body moves from the center of the legs to the support legs. Therefore, the body must first twist to the side of the support legs to move the center of gravity, that is, through the lateral direction of the hip joint. The joints are used to adjust the center of gravity; in order to prevent the body from tipping to the side of the swinging leg when the leg is lifted during lateral twisting, the lateral twisting and leg lifting are designed in steps in the gait design; the joint gait planning of the robot when walking is as follows:

First, adjust the center of gravity by twisting the hip joint of the right leg and the lateral joint of the bare joint to the right, and then keep the lateral twist in place, and then bend the bare joint of the left leg, the knee joint and the forward joint of the hip joint to realize the left leg. Lifting; lowering the left leg is the opposite action, that is, first twist the left side through the hip joint of the right leg and the bare joint, and then bend the bare joint of the left leg, the knee joint and the forward joint of the hip joint;

Step 2.2, plan the posture of each joint when the robot walks

Assuming that the lateral direction of the hip joint does not change, the steps to solve the walking trajectory are as follows:

(1) Draw a graph of the change of the hip joint and the bare joint with time, thereby obtaining the kinematic trajectory of the hip joint and the bare joint;

(2) Compared with the human walking trajectory, according to the force of the human walking process, the key points of the robot's hip joint and bare joint movement force are identified, and then all the key points are interpolated by means of cubic spline interpolation;

(3) Fit the key points processed by the hip joint and the bare joint with the polynomials in the kinematic trajectories of the hip joint and the bare joint in step (1) to obtain a smooth trajectory of the hip joint and the bare joint; then the trajectory is The trajectory formed after each walk of the hip joint and the bare joint;

According to the same method, the trajectories formed by other joints on the legs of the robot during walking can be obtained; taking the trajectory of the joints involved in one walking process of the biped robot as the planned posture, after obtaining the planned posture, pass the rudder at the joints. The machine-driven robot walks according to the planned posture.

Further, the pre-planned real-time gait control algorithm also includes:

Step 3, judge the stability of attitude planning

According to the planned posture in step 2, the stability calculation is performed every time the robot is ready to settle down, to judge whether the current robot meets the stability requirements, and the judgment is based on whether the stable point of the robot is within the set range. If the robot is in, do not intervene and make the robot walk according to the planned posture, otherwise the walking will be suspended. After adjusting the posture of the joints of the robot's legs through the robot's controller, the stability is recalculated until the stability meets the requirements. The calculation method is:

Calculate the centroid position of the robot:

表示第 i个关节的质心位置；COM_i为第i次采样时机器人的质心位置；即当前为第i次采样；Among them, n represents the number of joints in the whole body of the robot, m _i represents the mass of the ith joint,

Indicates the position of the center of mass of the i-th joint; COM _i is the position of the robot's center of mass in the i-th sampling; that is, the current is the i-th sampling;

Compute the stationary point W:

W=-a+COM

in,

In the above formula, t _s is the sampling time, COM _i , COM _i-1 , COM _i-2 are the position of the robot's center of mass at the i-th, i-1st and i-2th sampling times, respectively, and COM is the robot standing The position of the center of mass when not moving;

Determine the position of the stable point W. If W is within the set range, it is considered that the robot walks stably, otherwise it is unstable.

Further, when the robot walks, it judges and avoids obstacles according to the following methods:

Step 4, a binocular camera is installed on the head of the robot, which includes two left and right monocular cameras, which are used for collecting image information in front of the robot;

The left and right cameras of the binocular camera are calibrated respectively, and the plane equations of the ground in the left and right camera coordinate systems are calculated respectively;

Step 5, collect the front image of the robot through the binocular camera;

Step 6: Perform distortion and epipolar correction on the two images collected by the left and right cameras, eliminate the distortion, constrain the matching points on a straight line, reduce false matching and greatly shorten the matching time, and finally obtain a parallax map by matching;

Step 7: Calculate the three-dimensional coordinates (x, y, z) of the matching point in the disparity map in the left camera coordinate system, obtain its height and horizontal distance, and compare with the set threshold to determine whether an obstacle exists;

Here, use the cvFindCon-tours contour detection function in the OpenCV vision library to detect the contour of the object and mark it with its bounding rectangle;

Discrimination algorithm for obstacles: First, randomly extract several white pixels on the object in the circumscribed rectangular frame in the disparity map, and calculate its three-dimensional coordinates (x, y, z) in the left camera coordinate system, where x and z represent respectively. The height distance and horizontal distance of the object relative to the left camera, y represents the left and right offset distance of the object relative to the optical center of the left camera, and then judge whether the object in the rectangular frame is an obstacle according to the set threshold;

Step 8, if it is judged that there is no obstacle ahead, make the robot travel according to the planned path; if it is judged that there is an obstacle ahead, make the robot stop moving, and obtain the parameter information of the obstacle;

Step 9: Perform obstacle avoidance path planning.

Further, the described algorithm for judging obstacles specifically includes:

Step 7.1, randomly extract a number of white pixels _i =1, ₂ , 3 _{, .} The size of _i is sorted;

Step 7.2, take the median z ₀ of the horizontal distance _zi of all white pixels, and set the threshold φ, for the white pixel, for example, the horizontal distance zi of the white pixel _i satisfies |z _i -z ₀ |>φ, then the white pixel i is removed as an abnormal point, and the remaining white pixels constitute the set I;

Step 7.3, calculate the average value z of the horizontal distance _zi of the white pixels in the set I as the horizontal distance between the obstacles in the circumscribed rectangle and the robot; take the maximum value x _max of the _three -dimensional coordinates of the white pixels in the set I as the the height of the obstacle;

时，则判定外接矩形框内的物体为障碍物。Step 7.4, set the height threshold x _d and the horizontal distance threshold z _d , when x _max >x _d is satisfied, and

, the object in the bounding rectangle is determined to be an obstacle.

Further, if it is judged that there is no obstacle ahead, the robot will travel according to the planned path; if it is judged that there is an obstacle ahead, the robot will be stopped, and the parameter information of the obstacle will be obtained, specifically including:

Step 8.1, take the width of the circumscribed rectangular frame of the obstacle as the width of the obstacle, denoted as Q, and the center of the circumscribed rectangular frame as the center point of the obstacle, denoted as O;

Step 8.2, obtain the three-dimensional coordinates d _i (x, y, z) of O through depth detection, where y represents the offset distance _Li of the center point relative to the optical center of the left camera; obtain the leftmost white one in the outside rectangular frame The coordinates of the pixel in the left camera coordinate system (x _L , y _L , z _L );

Step 8.3, according to the width of the obstacle, calculate the ordinate of the center point O of the obstacle, that is, the offset distance L _o of the point O relative to the optical center of the left camera:

Step 8.4, since the binocular camera is installed on the central axis of the robot, the offset distance d of the center point O of the obstacle relative to the central axis of the robot is:

Where d>0 means right, d<0 means left, and d=0 means center.

Further, the described obstacle avoidance path planning specifically includes:

Step 9.1, set the length of the obstacle as W;

Step 9.2, when the obstacle is left relative to the robot, the robot turns right at an angle θ and goes straight for a distance X along the parallel safety distance. When the line connecting the robot center and the obstacle center is perpendicular to the planned path, turn left at an angle 2θ and go straight for a distance X, make it return to the planned path;

For the obstacle avoidance process of the robot, there are:

In the above formula, L is the body width of the robot; θ and 2θ are the rotation angles of the robot; D is the distance of the obstacle relative to the robot; y _L is the offset distance of the left boundary of the obstacle relative to the center of the robot; Errors and safety concerns increase the compensation distance.

Compared with the prior art, the present invention has the following technical characteristics:

1. In terms of the complexity of the algorithm, the required amount of information is small, and the method is simple; in terms of the timeliness of the algorithm, because the present invention requires a small amount of information, the complexity of implementation is low, thereby reducing the processing time of the algorithm.

2. In terms of practical application, this algorithm can combine the actual characteristics of the robot and the terrain, design the movements of each step of the robot in advance, so that the robot can complete the corresponding movements according to the plan, and collect the front image information in real time to avoid obstacles. success rate of walking.

3. The algorithm has strong gait stability and simple calculation, which conforms to engineering ideas. It has strong practicability in the field of practical operation, and has a certain guiding role in the field of biped robot gait algorithm.

Description of drawings

Fig. 1 is the schematic flow chart of the algorithm of the present invention;

(a) and (b) of Fig. 2 are respectively the physical appearance structure schematic diagram of the robot of the present invention, and the schematic diagram of the installation position of the steering gear on the robot;

3 is a schematic diagram of establishing a coordinate system at a joint in this algorithm;

Fig. 4 is the structural representation of joint part;

Fig. 5 is the schematic diagram of different synchronous states when the robot walks forward;

6 is a schematic diagram of the deflection angle of the robot leg when walking;

FIG. 7 is a schematic diagram of the force on the foot of the robot.

8 is a schematic diagram of obstacle contour detection in an obstacle avoidance algorithm;

FIG. 9 is a schematic diagram of the obstacle avoidance path planning of the obstacle avoidance algorithm.

Detailed ways

As shown in FIG. 1 , the basic idea of the present invention is that, under the control of the biped robot controller, the biped robot kinematic coordinate system model is first established, and then the robot attitude planning and stability judgment are performed, and finally the gait is completed. Walk, plan the next posture in real time to ensure the stability of the robot's walking. A pre-planned real-time gait control algorithm includes the following steps:

Step 1, establish the kinematic coordinate system model of the biped robot

The joints on the body of the biped robot are equipped with steering gears that drive the rotation of the joints. As shown in (a) and (b) of Figure 2, the biped robot in this embodiment has a total of 20 steering gears. The legs of the robot are distributed with multiple joints, and each leg has 6 degrees of freedom through the drive of the steering gear, namely: the bare joint has two degrees of freedom forward and lateral, which are called the forward joint and the ankle joint respectively. Lateral joints (15, 17 and 16, 18 in Fig. 2); the knee joint has one forward degree of freedom (13, 14 in Fig. 2), called the anterior joint of the knee joint; the hip joint has three degrees of freedom , including the forward, lateral and horizontal rotational degrees of freedom, which are respectively called the anterior joint, lateral joint and horizontal rotational joint of the hip joint (7, 9, 11 and 8, 10, 12 in Figure 2). In the upper body of the robot, the elbow joint has one degree of freedom (5 and 6 in Figure 2); the shoulder joint has two degrees of freedom (1, 3 and 2, 4 in Figure 2), and the neck joint has one degree of freedom (Figure 2). 19), one degree of freedom for the head (20 in Figure 2).

The key of the biped robot is composed of movably connected rods, and the connection of two adjacent rods is called a joint, as shown in Figure 4. The rod refers to the part on the torso of the robot. For example, the elbow is used as a joint, and the forearm and the arm are regarded as a rod respectively; on a biped robot, the adjacent rods are connected movably. , so that the angle between adjacent rods can be changed, for example, the connection of adjacent rods constitutes a rotating shaft structure. The steering gear is installed at each joint to drive the joint to rotate, and the steering gear can be, for example, a servo motor.

The idea of this step is to realize the transformation of the coordinates on the two rods at the joint through the transformation of the coordinate system at each joint of the robot; for the multi-joint robot of this scheme, each joint consists of two If the robot is composed of rods, the robot can be regarded as a multi-rod system. By using the coordinate transformation multiple times, the relationship between the coordinate systems can be established to form a kinematic coordinate system model. The specific steps are as follows:

Step 1.1, number each joint of the robot. For joint i, determine the origin of the joint. The principle of origin determination is:

When the axis of joint i (the axis of rotation of the joint) intersects with the axis of joint i+1, take the intersection point as the origin of joint i, when the axis of joint i and the axis of joint i+1 are out of plane, take the two axes The intersection of the vertical line and the axis of joint i+1 is used as the origin of joint i; when the axis of joint i is parallel to the axis of joint i+1, take the common perpendicular of the axis of joint i+1 and the axis of joint i+2 and the axis of joint i The intersection of the +1 axis is taken as the origin of the joint i, denoted as O _i .

Step 1.2, establish each coordinate axis X _i , Y _i , Z _i of the coordinate system at joint i to establish a coordinate system at each joint, and the determination principle is:

The Z _i coordinate axis coincides with the axis of the joint i+1, the X _i coordinate axis is along the i joint axis, the common perpendicular of the i+1 joint axis and points to the i+1 joint, and the Y _i coordinate axis is determined according to the right-hand rule.

As shown in Figure 3, in this scheme, the coordinate system O ₀ -X ₀ Y ₀ Z ₀ of the left knee joint (forward joint) of the biped robot is the reference coordinate system, and the coordinate system of the left ankle joint is O ₆ -X ₆ Y ₆ Z ₆ , the coordinate system of the right knee joint is O ₁₂ -X ₁₂ Y ₁₂ Z ₁₂ . The latter two are virtual coordinate systems, which only provide assistance and reference for calculation results.

According to the above method, a coordinate system is established at each joint of the robot.

则相邻的坐标系O_i与O_i-1之间的变换矩阵为：Step 1.3, according to the coordinate system established at each joint, the two adjacent coordinate systems O _i are compared with O _i-1 (that is, the coordinate system at joint i and the coordinate system at joint i-1). The gap is

Then the transformation matrix between adjacent coordinate systems O _i and O _i-1 is:

In the above formula, s represents the distance from the coordinate system O _i-1 to the coordinate system O _i , c represents the angle from the coordinate system O _i-1 to the coordinate system O _i , and a _i-1 represents the distance from X _i to X _i+1 , d _i represents the measured distance from X _i to the coordinate system O _i , and θ _i represents the rotation angle from X _i to X _i-1 . Since one joint i corresponds to one rod, the kinematic relationship between adjacent rods can be obtained through the above matrix.

The kinematic relationship between the two adjacent coordinate systems is obtained through the above transformation matrix, thereby obtaining the left ankle joint coordinate system O ₆ -X ₆ Y ₆ Z ₆ and the reference coordinate system O ₀ -X ₀ Y ₀ Z ₀ In the same way, the kinematic relationship between other joint coordinate systems and the reference coordinate system can be obtained.

In this scheme, the transformation matrix between the coordinate system at each joint and the reference coordinate system is used as the mathematical model between the joint and the joint where the reference coordinate system is located, and all the mathematical models constitute the biped robot kinematics coordinate system model.

Step 2: Plan the posture of the biped robot

In this scheme, the posture planning of the humanoid robot is based on the established robot kinematics coordinate system model, that is, it is represented by the coordinates of each joint in the kinematics coordinate system model established in step 1. Proceed as follows:

Step 2.1, design the walking gait process of the biped robot

In order to make the planning easy, the forward walking gait design is divided into right shift of the center of gravity (right leg support first), left leg lift, center of gravity shift to the middle of the legs, left shift of gravity center, right leg lifted, right leg lowered, The center of gravity is moved to eight stages between the legs, as shown in Figure 5.

The gait planning of this scheme is to change the center of gravity by swinging the leg (raised leg) forward. When the swinging leg swings forward, the body's center of gravity moves from the center of the legs to the supporting leg. The ground-contacting leg) twists on one side to move the center of gravity, that is, adjusts the center of gravity through the lateral joint of the hip joint; in order to prevent the body from tipping to the side of the swinging leg due to lifting the leg during lateral twisting, the lateral twisting is used in the gait design. and leg lift step-by-step design. Taking raising the left foot first as an example, the joint gait planning of the robot during one walk is as follows:

First, adjust the center of gravity by twisting the right side of the hip joint of the right leg and the lateral joint of the bare joint, keep the lateral twist in place, and then lift the left leg (swing leg), specifically: the naked joint of the left leg, knee joint and The forward joint of the hip joint is bent to achieve the lift of the left leg; the lowering of the left leg is the opposite action, that is, the left leg is twisted through the hip joint of the right leg and the bare joint first, and then the bare joint of the left leg, the knee joint and the hip joint are twisted on the left side. The forward joint is bent (the direction of bending is opposite to that of lifting), and the left foot is on the ground at this time; the same is true for lifting and landing of the right leg.

Step 2.2, plan the posture of each joint when the robot walks

The forward motion means that the robot's legs swing forward or successively, and the lateral motion means that the robot's legs swing left or right. The forward and lateral movements are modeled separately to complete the forward walking plan of the robot.

The forward movement is achieved by the coordinated movement of four lateral joints and six forward joints (left and right bare joints, lateral joints of the hip joint, left and right bare joints, knee joints, and forward joints of the hip joint). The movement of the joints moves the center of gravity of the mechanism, and the coordinated movement of the forward joints of the legs makes the robot walk forward. The specific implementation is as follows:

Define the joint rotation angle of the robot's legs. θ ₁ represents the deflection angle of the left leg from bottom to top; θ ₂ represents the deflection angle of the left leg from top to bottom; θ ₃ represents the right leg from bottom. The deflection angle to the top; the deflection angle of the left leg from top to bottom represented by θ ₄ , the schematic diagram is shown in Figure 6; then in the forward motion, it is realized by the motion of θ ₁ , θ ₂ , θ ₃ , and θ ₄ .

Assuming that the lateral direction of the hip joint does not change, that is, the lateral deflection angles of the four joints (that is, the two turning joints and the lateral joints of the hip joints of the left and right legs) are all 0, then solve the forward or backward movement The steps of the equation are as follows, taking the hip and ankle joints as an example:

(1) Draw the curve diagram of the hip joint and the bare joint changing with time (that is, the two-dimensional curve diagram established on the coordinate system of step 1), thereby obtaining the kinematic trajectory of the hip joint and the bare joint;

(2) Compared with the human walking trajectory, according to the force of the human walking process, the key points of the robot's hip joint and bare joint movement force are identified, and then all the key points are interpolated by means of cubic spline interpolation;

(3) Fit the key points processed by the hip joint and the bare joint with the polynomials in the kinematic trajectories of the hip joint and the bare joint in step (1) to obtain a smooth trajectory of the hip joint and the bare joint; then the trajectory is The trajectory formed after each walk for the hip and bare joints.

According to the same method, the trajectories formed by other joints on the legs of the robot during walking can be obtained; the trajectory of the joints involved in the process of one walking of the biped robot (from raising to lowering the left foot) is used as the planning posture, After obtaining the planned posture, the robot is driven by the steering gear at the joint to walk according to the planned posture.

Every time the robot lands, it will be in a certain unstable state, so it is necessary to make a stability judgment. According to the result of the stability judgment, feedback adjustment is made to the landing point of the robot.

Step 3, judge the stability of attitude planning

The influence of the ground reaction force on the foot is complex. In this scheme, it is simplified to have any point P, and the contribution of the ground reaction force to the foot can be equivalent to a force R and a moment M. The definition of a zero moment point is that there is a point P on the ground such that the net moment generated by inertial force (F=ma) and gravity (G=mg) in the direction of the axis parallel to the ground is zero. Figure 7 shows an example of the force distribution on the foot of the robot. The loads distributed along the sole have the same direction, and they are equivalent to a resultant force N. The action point on the sole of the foot through which the resultant force N passes is called for a stable point. This point can be understood as the projection of the resultant force of gravity and inertial force on the ground, and the moment of the resultant force at this point is zero in the horizontal direction.

According to the planned posture in step 2, the stability calculation is performed every time the robot is ready to settle down, to judge whether the current robot meets the stability requirements, and the judgment is based on whether the stable point of the robot is within the set range. If the robot is inside, do not intervene and make the robot walk according to the planned posture, otherwise it will suspend walking, adjust the posture of the joints of the robot legs (hip joint, knee joint, bare joint) through the robot controller, and recalculate the stability until it is stable The stability meets the requirements, and the calculation method of the stationary point is as follows:

Calculate the centroid position of the robot:

表示第i个关节的质心位置(在步骤1建立的坐标系中的位置)；COM_i为第i次采样时机器人的质心位置；即当前为第i次采样；Among them, n represents the number of joints in the whole body of the robot, n=20 in this embodiment; m _i represents the quality of the ith joint,

Indicates the position of the center of mass of the ith joint (the position in the coordinate system established in step 1); COM _i is the position of the center of mass of the robot during the ith sampling; that is, it is currently the ith sampling;

Compute the stationary point W:

W=-a+COM

in,

In the above formula, t _s is the sampling time, COM _i , COM _i-1 , COM _i-2 are the position of the robot's center of mass at the i-th, i-1st and i-2th sampling times, respectively, and COM is the robot standing The position of the centroid when not moving.

Determine the position of the stable point W. If W is within the set range, it is considered that the robot walks stably, otherwise it is unstable. In this embodiment, the setting range is a range of -5 to 5 cm before and after the projection point on the ground from the center of the foot after the robot lifts the foot.

To sum up, this scheme pre-designs the gait planning of the biped robot by imitating the human walking trajectory. According to the posture planning first and then the stability judgment, the complete posture of the robot is pre-planned to realize real-time gait control.

The pre-planned real-time gait control algorithm proposed in this scheme solves the problem of normal walking of the robot, avoids the complexity and tediousness of traditional algorithms, and pre-designs the movements of each step of the robot so that the robot can complete the corresponding movements according to the plan, ensuring the walking. success rate.

On the basis of the above technical solutions, when the robot walks according to the planned posture, the following methods are used to judge and avoid obstacles:

Step 4, a binocular camera is installed on the head of the robot, which includes two left and right monocular cameras, which are used for collecting image information in front of the robot;

The left and right cameras of the binocular camera are calibrated respectively, and the plane equations of the ground in the left and right camera coordinate systems are calculated respectively;

Step 5, collect the front image of the robot through the binocular camera;

Step 6: Perform distortion and epipolar correction on the two images collected by the left and right cameras (one image is collected by the left and right cameras at the same time), eliminate the distortion, constrain the matching points to a straight line, reduce false matching and greatly shorten the matching time, and finally Match to get a disparity map;

Step 7: Calculate the three-dimensional coordinates (x, y, z) of the matching point in the disparity map in the left camera coordinate system, obtain its height and horizontal distance, and compare with the set threshold to determine whether an obstacle exists;

Here, the contour detection function of cvFindCon-tours in the OpenCV vision library is used to detect the contour of the object and mark it with its bounding rectangle, as shown in Figure 8. According to the white pixels, the three-dimensional coordinates of the object relative to the left camera coordinate system are calculated using the binocular vision ranging principle.

Discrimination algorithm for obstacles: First, randomly extract several white pixels on the object in the circumscribed rectangular frame in the disparity map, and calculate its three-dimensional coordinates (x, y, z) in the left camera coordinate system, where x and z represent respectively. The height distance and horizontal distance of the object relative to the left camera, y represents the left and right offset distance of the object relative to the optical center of the left camera, and then judge whether the object in the rectangular frame is an obstacle according to the set threshold.

Specific steps are as follows:

Step 7.1, randomly extract a number of white pixels _i =1, ₂ , 3 _{, .} The size ordering of _i (that is, the _zi value in three-dimensional coordinates);

Step 7.2, because there are errors in the extraction and calculation process, there may be abnormal data, so take the median z ₀ of the horizontal distance _zi of all white pixels, and set the threshold φ, for the white pixels, as The horizontal distance zi of the white pixel _i satisfies |z _i -z ₀ |>φ, then the white pixel i is removed as an abnormal point, and the remaining white pixels constitute the set I;

作为外接矩形框内障碍物距离机器人的水平距离；取集合I中白色像素点三维坐标中x_i的最大值 x_max作为所述障碍物的高度；Step 7.3, calculate the average value of the horizontal distance _zi of the white pixels in the set I

As the horizontal distance of the obstacle from the robot in the circumscribed rectangular frame; take the maximum value x _max of x _i in the three-dimensional coordinates of the white pixel point in the set I as the height of the obstacle;

时，则判定外接矩形框内的物体为障碍物。Step 7.4, set the height threshold x _d and the horizontal distance threshold z _d , when x _max >x _d is satisfied, and

, the object in the bounding rectangle is determined to be an obstacle.

Step 8, if it is judged that there is no obstacle ahead, make the robot travel according to the planned path; if it is judged that there is an obstacle ahead, make the robot stop moving, and obtain the parameter information of the obstacle, as follows:

Step 8.1, take the width of the circumscribed rectangular frame of the obstacle as the width of the obstacle, denoted as Q, and the center of the circumscribed rectangular frame as the center point of the obstacle, denoted as O;

Step 8.2, obtain the three-dimensional coordinates d _i (x, y, z) of O through depth detection (that is, detect by the binocular camera), wherein y represents the offset distance _Li of the center point relative to the optical center of the left camera; obtain the outside rectangle The coordinates of the leftmost white pixel in the frame in the left camera coordinate system (x _L , y _L , z _L );

Step 8.3, according to the width of the obstacle, calculate the ordinate of the center point O of the obstacle, that is, the offset distance L _o of the point O relative to the optical center of the left camera:

Step 8.4, since the binocular camera is installed on the central axis of the robot, the offset distance d of the center point O of the obstacle relative to the central axis of the robot is:

Where d>0 means right, d<0 means left, and d=0 means center.

Step 9, carry out obstacle avoidance path planning

Step 9.1, set the length of the obstacle as W;

In the process of obstacle detection, the width of the obstacle and its orientation information relative to the robot can be obtained relatively accurately, but it is difficult to obtain the length of the obstacle. Generally, its length is assumed to be W;

Step 9.2, when the obstacle is to the left of the robot, in order to bypass the obstacle faster, set a constant safety distance according to the position of the obstacle and the laboratory environment, the robot turns right at an angle θ and follows the parallel safety distance. Go straight for a distance X, when the connection between the robot center and the obstacle center is perpendicular to the planned path, turn left at an angle of 2θ and go straight for a distance X to make it return to the planned path; the same is true when the obstacle is to the right and the center is to the right, the obstacle is to the right When the robot turns left, when the obstacle is centered, it randomly chooses to turn left or right, and I will not repeat it; the schematic diagram is shown in Figure 9: in the figure, L is the body width of the robot; θ and 2θ are the rotation angles of the robot; D is the The distance between the obstacle and the robot; y _L is the offset distance of the left boundary of the obstacle relative to the center of the robot; d is the compensation distance added considering the existing errors and safety issues (generally d=y _L ).

For the obstacle avoidance process of the robot, there are:

So far, obstacle detection and obstacle avoidance path planning can be completed.

Claims (4)

1. A pre-programmed real-time gait control algorithm, comprising the steps of:

step 1, establishing a biped robot kinematic coordinate system model

The joint of the biped robot body is provided with a steering engine for driving the joint to rotate, wherein the bare joint of the leg of the robot is provided with a forward joint and a lateral joint, the knee joint is provided with a forward joint, and the hip joint is provided with a forward joint, a lateral joint and a horizontal rotating joint;

step 1.1, numbering each joint of the robot, determining an origin of each joint for a joint i, wherein the principle of determining the origin is as follows:

when the axis of the joint i is intersected with the axis of the joint i +1, taking the intersection point as the origin of the joint i, and when the axis of the joint i is different from the axis of the joint i +1, taking the intersection point of the common perpendicular line of the two axes and the axis of the joint i +1 as the origin of the joint i; when the axis of the joint i is parallel to the axis of the joint i +1, the intersection point of the common perpendicular line of the axis of the joint i +1 and the axis of the joint i +2 and the axis of the joint i +1 is taken as the origin of the joint i and is marked as O_i；

Step 1.2, determining each coordinate axis X of a coordinate system at the joint i_i、Y_i、Z_iEstablishing a coordinate system at each joint, wherein the determination principle is as follows:

Z_ithe coordinate axis coinciding with the axis of the joint i +1, X_iThe coordinate axes are along the axis of the i-joint,The common vertical line of the axis of the i +1 joint is parallel to and points to the i +1 joint, Y_iThe coordinate axis is determined according to a right-hand rule;

step 1.3, according to the coordinate systems established at each joint, two adjacent coordinate systems O_iAnd O_i-1Comparing, obtaining the difference between two adjacent coordinate systems, namely a transformation matrix;

obtaining a kinematic relationship between two adjacent coordinate systems through the transformation matrix, recording the coordinate system of the left foot knee joint of the biped robot as a reference coordinate system, and obtaining the kinematic relationship between other joint coordinate systems and the reference coordinate system; taking a transformation matrix between a coordinate system at each joint and a reference coordinate system as a mathematical model between the joint and the joint where the reference coordinate system is located, wherein all the mathematical models form a biped robot kinematic coordinate system model;

step 2, planning the postures of the biped robot

Step 2.1, designing a walking gait process of the biped robot

The robot changes the gravity center by swinging the front pendulum of the legs, and when the front pendulum of the legs is swung, the gravity center of the body moves to the supporting legs from the centers of the two legs, so that the body firstly twists to one side of the supporting legs to move the gravity center, namely, the gravity center is adjusted by the lateral joints of the hip joints; in order to prevent the body from falling to one side of the swing leg caused by lifting the leg during side twisting, the side twisting and leg lifting are designed step by step in gait design; the joint gait plan of the robot during one walking is as follows:

firstly, the gravity center is adjusted by twisting the right side of the hip joint of the right leg and the lateral joint of the bare joint, the lateral twisting is kept in place, and then the bare joint of the left leg, the knee joint and the forward joint of the hip joint are bent to lift the left leg; the left leg is put down to perform the opposite action, namely, the left side of the hip joint and the naked joint of the right leg is twisted, and then the naked joint, the knee joint and the anterior joint of the hip joint of the left leg are bent;

step 2.2, planning the postures of all joints when the robot walks

Firstly, assuming that the lateral direction of the hip joint does not change, the step of solving the walking track is as follows:

(1) drawing a curve graph of the change of the hip joint and the bare joint along with the change of time, thereby obtaining the kinematic trajectory of the hip joint and the bare joint;

(2) comparing with a human walking track, determining key points of motion stress of hip joints and bare joints of the robot according to the stress condition in the human walking process, and then performing interpolation processing on all the key points in a cubic spline interpolation mode;

(3) respectively fitting the key points after the hip joint and the bare joint are processed with the polynomial in the kinematic trajectory of the hip joint and the bare joint in the step (1) to obtain the smooth trajectory of the hip joint and the bare joint; the track is the track formed by walking the hip joint and the bare joint at each step;

according to the same method, the tracks formed by other joints on the two legs of the robot when walking can be obtained; taking the track of the joint involved in the one-time walking process of the biped robot as a planning attitude, and driving the robot to walk according to the planning attitude through a steering engine at the joint after the planning attitude is obtained;

step 3, judging the stability of the attitude planning

According to the planning posture in the step 2, performing stability calculation when the robot is ready to fall on the foot each time, judging whether the current robot meets the stability requirement, judging whether the stability point of the robot is in a set range according to the judgment, if so, not intervening, enabling the robot to walk according to the planning posture, otherwise, stopping walking, and recalculating the stability until the stability meets the requirement after the controller of the robot adjusts the postures of the joints of the legs of the robot; the method for calculating the stable point comprises the following steps:

calculating the centroid position of the robot:

wherein n represents the number of joints of the whole body of the robot, and m_iIndicating the mass of the ith joint,

representing the centroid position of the ith joint; COM (component object model)_iThe position of the mass center of the robot at the ith sampling time is shown; i.e. the ith sample is currently;

calculating a stationary point W:

W＝-a+COM

wherein,

in the above formula, t_sFor sampling time, COM_i，COM_i-1，COM_i-2Respectively the centroid position of the robot during the ith sampling, the ith-1 sampling and the ith-2 sampling, and the COM is the centroid position of the robot when the robot stands still;

judging the position of a stable point W, if W is in a set range, considering that the robot walks stably, otherwise, judging that the robot is unstable;

when the robot walks, the judgment and obstacle avoidance of the obstacle are carried out according to the following method:

step 4, installing a binocular camera on the head of the robot, wherein the binocular camera comprises a left monocular camera and a right monocular camera and is used for collecting image information in front of the robot;

calibrating a left camera and a right camera of the binocular camera respectively, and calculating plane equations of the ground under a left camera coordinate system and a right camera coordinate system respectively;

step 5, acquiring a front image of the robot through a binocular camera;

step 6, carrying out distortion and epipolar correction on the two images acquired by the left camera and the right camera, eliminating distortion, constraining the matching points on a straight line, reducing mismatching and greatly shortening matching time, and finally matching to obtain a disparity map;

step 7, calculating three-dimensional coordinates (x, y, z) of the matching points in the disparity map under a left camera coordinate system, acquiring the height and the horizontal distance of the matching points, comparing the height and the horizontal distance with a set threshold value, and judging whether an obstacle exists or not;

detecting the outline of the object by using a cvFindCon-curves outline detection function in an OpenCV visual library, and marking the outline by using a circumscribed rectangular frame of the outline;

the judgment algorithm for the obstacle is as follows: firstly, randomly extracting a plurality of white pixel points on an object in an external rectangular frame in a disparity map, calculating three-dimensional coordinates (x, y, z) of the white pixel points under a left camera coordinate system, wherein x and z respectively represent the height distance and the horizontal distance of the object relative to a left camera, y represents the left-right offset distance of the object relative to the optical center of the left camera, and then judging whether the object in the rectangular frame is an obstacle or not according to a set threshold value;

step 8, if judging that no obstacle exists in front, enabling the robot to move along the planned path; if the front part is judged to have the obstacle, the robot stops moving and parameter information of the obstacle is obtained;

and 9, planning an obstacle avoidance path.

2. The pre-programmed real-time gait control algorithm of claim 1, wherein the obstacle discrimination algorithm specifically comprises:

step 7.1, randomly extracting a plurality of white pixel points i in an external rectangular frame in the parallax image as 1, 2, 3_i(x_i,y_i,z_i) And according to the horizontal distance z_iSorting by size;

step 7.2, taking the horizontal distances z of all white pixel points_iMedian z of₀And setting a threshold phi for the horizontal distance z of the white pixel point, such as the white pixel point i_iSatisfies | z_i-z₀If the value is greater than phi, the white pixel points I are taken as abnormal points to be removed, and the residual white pixel points form a set I;

step 7.3, calculating the horizontal distance z of the white pixel points in the set I_iAverage value of (2)

The horizontal distance between the obstacle and the robot is taken as a circumscribed rectangular frame; taking x in three-dimensional coordinates of white pixel points in set I_iMaximum value x of_maxA height as the obstacle;

step 7.4, set height threshold x_dAnd a horizontal distance threshold z_dWhen x is satisfied_max＞x_dAnd is and

and if so, determining that the object in the circumscribed rectangular frame is an obstacle.

3. The pre-planned real-time gait control algorithm of claim 1, wherein if it is determined that there is no obstacle in front, the robot is caused to follow the planned path; if it is judged that there is an obstacle in front, the robot is stopped and the parameter information of the obstacle is acquired, specifically including:

step 8.1, taking the width of an external rectangular frame of the obstacle as the width of the obstacle, and marking the width as Q, and taking the center of the external rectangular frame as the center point of the obstacle, and marking the center point as O;

step 8.2, obtaining the three-dimensional coordinate d of the O through depth detection_i(x, y, z), where y represents the offset distance L of the center point from the optical center of the left camera_i(ii) a Obtaining the coordinate (x) of the leftmost white pixel point in the external rectangular frame under the coordinate system of the left camera_L,y_L，z_L)；

Step 8.3, calculating the vertical coordinate of the center point O of the obstacle according to the width of the obstacle, namely the offset distance L of the O point relative to the optical center of the left camera_o：

Step 8.4, because the binocular camera is installed on the central axis of the robot, the offset distance d of the center point O of the obstacle relative to the central axis of the robot is as follows:

where d >0 indicates to the right, d <0 indicates to the left, and d-0 indicates to the center.

4. The pre-planned real-time gait control algorithm of claim 1, wherein the planning of obstacle avoidance paths specifically comprises:

step 9.1, setting the length of the barrier as W;

9.2, when the obstacle deviates to the left relative to the robot, the robot turns right by an angle theta and moves straight by a distance X along a parallel safe distance, and when a connecting line of the center of the robot and the center of the obstacle is vertical to the planned path, the robot turns left by the angle 2 theta and moves straight by the distance X, so that the robot returns to the planned path;

for the obstacle avoidance process of the robot, the following steps are carried out:

in the above formula, L is the body width of the robot; theta and 2 theta are the rotation angles of the robot; d is the distance between the obstacle and the robot; y is_LThe offset distance of the left boundary of the obstacle relative to the center of the robot; d is the added compensation distance to account for existing errors and safety issues.

Images