CN106364580A - Planetary wheel type obstacle crossing robot stair climbing control method - Google Patents

Abstract

The invention discloses a planetary wheel type obstacle crossing robot stair climbing control method. The planetary wheel type obstacle crossing robot stair climbing control method comprises the following steps that (1) an obstacle crossing robot keeps moving forwards at a constant speed linearly, the distance between two planetary wheel sets of the same driving set and a step in the front as well as the rotating speed of the planetary wheel sets are measured, and the deflection angle is calculated; (2) the theoretical rotating speed of both the planetary wheel sets is calculated, and the planetary wheel sets move at the calculated rotating speed; (3) the L1 and the L2 in the current are detected, and the current deflection angle theta is calculated; (4) whether the current deflection angle theta exceeds the preset value or not is judged, if yes, the step (2) is executed again, and if not, the robot is controlled to continue to move forward at the constant speed linearly; (5) whether the two planetary wheel sets arrive at the step or not is judged, if not, the robot continues to move forward at the constant speed linearly, and if yes, the two planetary wheel sets turns over together to cross the step; and (6) whether the two planetary wheel sets arrive at the destination or not is judged, if yes, the steps from (1) to (5) are repeated, and if yes, the robot is stopped moving. The planetary wheel type obstacle crossing robot stair climbing control method has the advantages that control operation is simple, and the steering precision is high.

Description

A kind of planet gear type barrier-surpassing robot stair climbing control method

Technical field

The invention belongs to robot control field, more particularly, to a kind of planet gear type barrier-surpassing robot stair climbing control Method processed.

Background technology

With scientific and technical development, people have increasing need for robot and are operated under many adverse circumstances, such as: star Ball surface is explored, fire-fighting, and the scene of a fire detects, poisonous, inflammable, explosive place detection and unmanned battlefield etc..Complicated, unknown, changeable Non- structure environment very high requirement is proposed to the obstacle climbing ability of robot, barrier-surpassing robot should possess good adaptability, Stability and motility, so the control research and development for barrier-surpassing robot are with a wide range of applications.

Existing barrier-surpassing robot species respectively has advantage also respectively to have deficiency: wheel type barrier-crossing robot architecture is simple, easy to control, The ratio that bears a heavy burden is high, and energy efficiency is high, has absolute advantagess when flat road surface load is advanced compared with other species robots, but rugged Under landform, its obstacle performance is poor, and, although heavy burden ability is strong, gait of march is also fast, but needs road surface evenness for such as heavy-duty vehicle； Crawler type obstacle crossing robot Area of bearing is big, and sinking, degree is little, and landform is adaptable, but its complex structure quality is big, energy profit Low with rate, such as tank, obstacle climbing ability is strong, but volume mass is big, is typically not used to bear a heavy burden；Sufficient formula barrier-surpassing robot motion spirit Live, landform is adaptable, but its structure and control are all very complicated, the ratio that bears a heavy burden is relatively low, such as boston dynamics company The bigdog robot that r.playter, m.buehler and m.raibert et al. propose in " bigdog " one literary composition, deadweight 90kg, heavy burden 50kg, its length 1m, maximal rate is 0.7m/s.

Describe one kind inside Central China University of Science and Technology's Master's thesis " a kind of design and development of new barrier-surpassing robot " to get over Barrier robot, its small volume, light weight, employ 6 independent planetary gear set as driving, there is good obstacle climbing ability, But the driving group of this robot does not have degree of freedom in yaw direction, the right and left can not be made when climbing the stair of turning radian Planetary gear set overturns simultaneously, and before left front planetary gear set and the right side, respectively on upper and lower two-stage step, that is, vehicle body is left front for planetary gear group End and right front ends are in differing heights, and vehicle body is seriously crooked, is easily caused overturning.Therefore, research design is a kind of new climbs building Terraced control method has important using value.

Content of the invention

Disadvantages described above for prior art or Improvement requirement, the invention provides a kind of robot stair climbing controlling party Method, wherein combines the planet gear type barrier-surpassing robot feature of itself, and respective design is applied to planet gear type barrier-surpassing robot Stair climbing control method, can not only adapt to the situation that robot automobile body becomes certain angle of inclination with stair, and adapt to each Level step is that inner side is narrow, the feelings such as bigger turning step of different widths inside and outside outside width and the more little every grade of step of radius of turn Condition, it is simple to have a control operation, the advantages of turn to high precision.

For achieving the above object, the present invention proposes a kind of planet gear type barrier-surpassing robot stair climbing control method, including Following steps:

(1) so that barrier-surpassing robot is remained a constant speed straight ahead under original state, then measure the same driving of barrier-surpassing robot In group two planetary gear set foremost with front stairway step apart from l₁And l₂, and measure the rotating speed of each planetary gear set, according to described l₁And l₂Calculate and obtain deflection angle theta；

(2) theoretical rotational speed of each planetary gear set is calculated by deflection angle control algolithm, and by the theoretical rotational speed calculating It is input in the motor driver of each planetary gear set to drive planetary gear set motion；

(3) detect current l after an adjustment cycle₁And l₂, and calculate current deflection angle theta；

(4) judge whether current yaw angle θ exceedes preset value: if so, then proceed to step (2), if it is not, then controlling obstacle detouring machine Device people continues uniform rectilinear and advances；

(5) judge in barrier-surpassing robot driving group whether two planetary gear set reach ladder: if it is not, before then continuing uniform rectilinear Enter, if so, then two planetary gear set overturn together, cross step；

(6) judge whether two planetary gear set arrive at: if it is not, then entering new pose adjustment process, repeat step (1)～(5), if so, then terminate to move, complete the control of planet gear type barrier-surpassing robot stair climbing in this way.

As it is further preferred that described deflection angle theta adopts equation below to obtain:

$\mathrm{\θ}=arctan\frac{l_1-l_2}{l_{ab}};$

In formula: l_abRepresent the distance between central point of two planetary gear set in the same driving group of barrier-surpassing robot.

As it is further preferred that the described theoretical rotational speed being calculated each planetary gear set by deflection angle control algolithm, specifically Including:

(2.1) calculate t cycle time that barrier-surpassing robot pose adjusts first:

$t=k_1\×\frac{l_1+l_2}{({\mathrm{\ω}}_1+{\mathrm{\ω}}_2)\×r};$

In formula: k₁For ratio correction coefficient, ω₁,ω₂It is respectively the rotating speed of left and right two-wheeled in planetary gear set, r is planetary gear The radius of single wheel in group；

(2.2) then calculate pose adjustment amplitude a:

$a=k_2\frac{\mathrm{\θ}}{t};$

In formula: k₂For proportionality coefficient, θ is deflection angle；

(2.3) calculate the difference of rotating speed again:

$\mathrm{\δ}\mathrm{\ω}=asin(\frac{2\mathrm{\π}}{t}\×t);$

In formula: t is the time of pose adjustment experience in the cycle, and t ∈ (0, t)；

(2.4) finally calculate the theoretical rotational speed of two planetary gear set in the same driving group of planet gear type barrier-surpassing robot ω₁'、ω₂':

ω₁'=ω₁+δω,ω₂'=ω₂-δω.

As it is further preferred that judging in barrier-surpassing robot driving group, whether two planetary gear set reach ladder, particularly as follows:

Judge l₁And l₂Whether meet following condition: l₁=l₂=l_min, wherein, l_minL when reaching step for barrier-surpassing robot Value.

As it is further preferred that described planet gear type barrier-surpassing robot is preferably four planetary gear barrier-surpassing robots or six row Star-wheel barrier-surpassing robot.

As it is further preferred that when described planet gear type barrier-surpassing robot is four planetary gear barrier-surpassing robot, it is provided with Two groups of driving groups in front and back, two planetary gear set in every group of driving group are all controlled by described step (1)-(6).

As it is further preferred that when described planet gear type barrier-surpassing robot is six planetary gear barrier-surpassing robot, it is provided with Before in after three groups of driving groups, two planetary gear set in every group of driving group are all controlled by described step (1)-(6).

As it is further preferred that described stair are preferably spiral stairway or curved stairs.

As it is further preferred that the preset value described in step (4) is specially 0～3 °, deflection angle is in this angular range Interior so that in same driving group left and right two-wheeled cross step simultaneously.

In general, by the contemplated above technical scheme of the present invention compared with prior art, mainly possess following Technological merit:

1. the construction featuress based on planet gear type barrier-surpassing robot for the present invention specially devise a kind of new-type stair climbing control Method processed, it can not only adapt to the situation that robot automobile body becomes certain angle of inclination with stair, and adapts to every one-level step It is all that inner side is narrow, the feelings of the bigger turning step of the different widths inside and outside outside width and the more little every grade of step of radius of turn Condition, can achieve the steering cat ladder motion of any given deflection angle, improves turning flexibility ratio and the turning precision of barrier-surpassing robot.

2. the present invention there may be the problem of difference in height for left and right planetary gear set, when driving group crosses step, by a left side Distance between right planetary gear set and step is compared, the rotating speed of adjustment two planetary gears, makes two planetary gears reach ladder simultaneously, enters And ensure about car body highly substantially unchanged it is ensured that the stationarity of turning cat ladder.

3. the present invention passes through the real-time rotating speed adjusting two planetary gears, controls the distance between left and right planetary gear set and step, To different steps, there is very strong adaptability, can ensure to make two planetary gears reach ladder for different steps simultaneously.

Brief description

Fig. 1 is the structural representation of six planetary gear barrier-surpassing robots；

Fig. 2 is the front driving group structural representation of six planetary gear barrier-surpassing robots；

Fig. 3 is front vehicle body and the aftercarriage articulated position schematic diagram of six planetary gear barrier-surpassing robots；

Fig. 4 is the pose top view of barrier-surpassing robot relative step；

Fig. 5 (a)-(f) is the top view that barrier-surpassing robot pose adjusts process；

Fig. 6 is driving group control method flow chart before barrier-surpassing robot；

Fig. 7 is barrier-surpassing robot control system block diagram；

Fig. 8 is the structural representation of four planetary gear barrier-surpassing robots.

Specific embodiment

In order that the objects, technical solutions and advantages of the present invention become more apparent, below in conjunction with drawings and Examples, right The present invention is further elaborated.It should be appreciated that specific embodiment described herein is only in order to explain the present invention, and It is not used in the restriction present invention.As long as additionally, involved technical characteristic in each embodiment of invention described below The conflict of not constituting each other just can be mutually combined.

As shown in Figures 1 to 3, the planet gear type barrier-surpassing robot using in the present invention, including car body and be used for supporting car body Three driving groups, described car body includes front vehicle body 100 and aftercarriage 200, and front vehicle body 100 passes through horizontally disposed jointed shaft 10 Be hinged on aftercarriage 200, described three driving groups by order from front to back be followed successively by front driving group 300 (front-axle beam), in Driving group 400 (central sill) and rear driving group 500 (back rest).

Wherein, front driving group 300 includes 303, two Power Drive Units of support beam 305 and 306, two ultrasonic distances Sensor 307 and 308, two planetary gear set 301 and 302；Planetary gear set 301 includes three radius size identical wheels, wheel The radius of son is taken as r.Arrange about two planetary gear set, left planetary gear set 301 is arranged in left driving means 305, left driving Device 305 is used for driving left planetary gear set 301 mobile, and right planetary gear set 302 is arranged in right driving means 306, and right driving fills Put 306 to be used for driving right planetary gear set 302 mobile, ultrasonic distance sensor 307 is fixed in left driving means 305, ultrasonic Ripple range sensor 308 is fixed in right driving means 306, and ultrasonic distance sensor 307 and 308 is located at planetary gear set 301 On the line of 302 central point.Left and right two driving means 305 and 306 are fixed in support beam 303, and driving group 300 is passed through perpendicular The jointed shaft 304 of straight setting is hinged in front vehicle body.The structure of described middle driving group 400 and rear driving group 500 and front driving The structure of group 300 is identical.

With reference to Fig. 4～7 explanation barrier-surpassing robot in the case that vehicle body becomes certain angle of inclination with stair, cross every One-level step be all that the narrow, outside in inner side is wide and the more little every grade of step of radius of turn inside and outside the bigger turning platform of different widths The control method of rank (such as spiral stairway, spiral stairses, curved stairs).

As shown in figure 5, the vehicle body that Fig. 5 (a) is the wheeled barrier-surpassing robot of original state lower planet becomes one with first segment step Fixed angle；Fig. 5 (b) is that about front-axle beam, two-wheeled adjustment of rotational speed makes two planetary gears reach first order step simultaneously；Fig. 5 (c) is front Beam two planetary gear set crosses first order step surface simultaneously；Fig. 5 (d) is that about front-axle beam, two-wheeled adjustment of rotational speed makes two planetary gears arrive simultaneously Reach second level step, about central sill, two-wheeled adjustment of rotational speed makes two planetary gears reach first order step simultaneously simultaneously；Fig. 5 (e) is front Beam two planetary gear set crosses second level step surface simultaneously, and central sill two planetary gear set crosses first order step surface simultaneously, the back rest simultaneously Left and right two-wheeled adjustment of rotational speed makes two planetary gears reach first order step simultaneously；Fig. 5 (f) is that about front-axle beam, two-wheeled adjustment of rotational speed makes two Planetary gear reaches third level step simultaneously, and central sill two planetary gear set adjustment of rotational speed makes two planetary gears reach second level step simultaneously, The back rest two planetary gear set crosses first order step surface simultaneously.

Cross step surface for ensureing that left and right two planetary gears are maintained on sustained height, with two-wheeled adjustment of rotational speed about front-axle beam As a example, the vehicle body of the wheeled barrier-surpassing robot of initial state lower planet and first segment step have a certain degree, concrete pose adjustment Step is as follows:

(1) before crossing first order step, under original state, barrier-surpassing robot is (i.e. same with initial velocity uniform rectilinear advance In one driving group, two planetary gears are rotated with initial speed, and both initial speeds are identical), initial velocity can be according to actual need Set, by two planets in the ultrasonic distance sensor measurement same driving group of barrier-surpassing robot (herein for front driving group) Wheel group foremost with preceding object thing (i.e. the side of a front stairway step closest with planetary gear set) apart from l₁ And l₂, and the rotational speed omega of two planetary gear set is measured by speed probe₁、ω₂(rotational speed omega₁、ω₂Initial value is identical), according to described l₁And l₂Calculate and obtain deflection angle theta.

Specifically, as shown in figure 4,301,302,401,402,501 and 502 correspond to six planets of barrier-surpassing robot respectively Wheel group, c₁、c₂And c₃Represent three step surfaces increasing successively.Deflection angle theta represents the inclined degree of vehicle body and ladder, l₁And l₂Point Not Biao Shi the vehicle body that records of ultrasonic distance sensor 307 and 308 to the nearest step surface in front distance, driving group in labelling Center is point o, and the position of labelling ultrasonic distance sensor 307 is point a, and the position of labelling ultrasonic distance sensor 308 is Point b, a are the central point of planetary gear set 301, and b is the central point of planetary gear set 302, then:

$\mathrm{\θ}=\arctan \frac{l_1-l_2}{l_{ab}}---\left(1\right)$

Wherein, l_abRepresent two planetary gear set in the same driving group of barrier-surpassing robot (herein for front driving group) central point it Between distance, namely the distance between ultrasonic distance sensor 307 and ultrasonic distance sensor 308.

(2) theoretical rotational speed of each planetary gear set is calculated by deflection angle control algolithm, and by the theoretical rotational speed calculating It is input in the motor driver of each planetary gear set to drive planetary gear set motion.

As shown in fig. 7, angle pid feedback control includes angle pid control module 701, speed pid control module 702 and System mode detection module 703, speed pid control module 702 controls 704, motor performing module 705 and Hall by speed pid Speed measuring module 706 forms.

Angle pid control module 701 according to currently respectively take turns angular velocity omega_iAnd current yaw angle θ is to theory (i=1,2) Rotating speed is calculated, and is specifically shown in formula (2)～(5).The theory that speed pid control module 702 draws according to angle pid module 701 The angular velocity of adjustment of rotational speed motor, system mode detection module 703 carries out real-time detection to the system state amount used in controlling, Pass root device, angular-rate sensor and ultrasonic distance sensor including angle.

Calculate the theoretical rotational speed of each planetary gear set by deflection angle control algolithm, particularly as follows:

(2.1) calculate t cycle time that barrier-surpassing robot pose adjusts:

$t=k_1\×\frac{l_1+l_2}{({\mathrm{\ω}}_1+{\mathrm{\ω}}_2)\×r}---\left(2\right)$

In formula: k₁For ratio correction coefficient it is ensured that barrier-surpassing robot completes the pose adjustment of a cycle before reaching step, 0<k₁< 0.5, k can be adjusted according to concrete road conditions₁Value；ω₁,ω₂It is respectively the rotating speed of left and right two-wheeled in planetary gear set, r is planet The radius of single wheel in wheel group；

(2.2) calculate amplitude a that pose adjusts:

$a=k_2\frac{\mathrm{\&theta;}}{t}---\left(3\right)$

In formula: θ is current yaw angle, k₂For proportionality coefficient so that completing barrier-surpassing robot pose in the adjustment cycle Adjustment, k₂Value by concrete road conditions and barrier-surpassing robot normal speed determine；

(2.3) difference of calculating rotating speed:

$\mathrm{\&delta;}\mathrm{\&omega;}=asin(\frac{2\mathrm{\&pi;}}{t}\&times;t)---\left(4\right)$

In formula: t is the time of pose adjustment experience within this cycle, and t ∈ (0, t)；

(2.4) left and right two planetary gear set in the same driving group of planet gear type barrier-surpassing robot (i.e. front driving group) are calculated Theoretical rotational speed ω₁'、ω₂':

ω₁'=ω₁+δω,ω₂'=ω₂-δω (5)

(3) after the time t in an adjustment cycle, detect current l₁And l₂, and calculate current deflection angle theta；

(4) judge whether current yaw angle θ exceedes preset value: if exceeding preset value, proceed to step (2), if not surpassing Cross preset value, then about driving group before controlling barrier-surpassing robot, two-wheeled continues to advance with initial velocity uniform rectilinear, wherein, preset Value takes 0～3 °, and deflection angle can meet left and right two-wheeled in this angular range and cross step simultaneously.

(5) judge in barrier-surpassing robot driving group, whether two planetary gear set reach ladder: if robot does not reach ladder, Continue to advance with initial velocity uniform rectilinear, if robot reaches ladder, corresponding two planets in front driving group (i.e. front-axle beam) Wheel group overturns together, crosses step.

Judge in barrier-surpassing robot driving group, whether two planetary gear set reach ladder, particularly as follows:

Judge l₁And l₂Whether meet following condition: l₁=l₂=l_min, wherein, l_minL when reaching step for barrier-surpassing robot Value (when i.e. barrier-surpassing robot reaches step, the distance of planetary gear set and step), l_minValue can be by barrier-surpassing robot two planet During the adjacent step surface in wheel group front end, the distance that measurement ultrasonic sensor reaches step obtains.

(6) judge whether two planetary gear set arrive at by manipulator: if not arriving at, barrier-surpassing robot Front-axle beam enters new pose adjustment process, repeat step (1)～(5), if arriving at, terminates to move, complete in this way The stair climbing becoming planet gear type barrier-surpassing robot front-axle beam controls.

During barrier-surpassing robot front-axle beam climbs building, central sill and the back rest also climb building simultaneously, and its control method and front-axle beam climb building Control method identical, after that is, front-axle beam crosses step, the step that central sill crosses step with front-axle beam is identical, corresponding, central sill two Planetary gear set repeat step (1)～(6)；After central sill crosses step, the step that the back rest crosses step with front-axle beam is identical, corresponding, Two planetary gear set repeat step (1)～(6) of the back rest, complete the control of planet gear type barrier-surpassing robot stair climbing with this.

The control method that the present invention relates to is equally applicable to four planetary gear barrier-surpassing robots, four planetary gear obstacle detouring machines People's structure is as shown in Figure 8.

In Fig. 8,100 represent car body, driving group before 200 representatives, driving group after 300 representatives, and front driving group 200 is passed through vertically The jointed shaft 10 of setting is hinged on car body, and the jointed shaft 20 that driving group 300 is passed through to be vertically arranged is hinged on car body.

As it will be easily appreciated by one skilled in the art that the foregoing is only presently preferred embodiments of the present invention, not in order to Limit the present invention, all any modification, equivalent and improvement made within the spirit and principles in the present invention etc., all should comprise Within protection scope of the present invention.

Claims (9)

1. a kind of planet gear type barrier-surpassing robot stair climbing control method is it is characterised in that comprise the steps:

(1) barrier-surpassing robot uniform rectilinear is made to advance under original state, then two row in the measurement same driving group of barrier-surpassing robot Star-wheel group foremost with front stairway step apart from l₁And l₂, and measure the rotating speed of each planetary gear set, according to described l₁And l₂Meter Calculate and obtain deflection angle theta；

(2) calculated the theoretical rotational speed of each planetary gear set by deflection angle control algolithm, and the theoretical rotational speed calculating is inputted To in the motor driver of each planetary gear set to drive planetary gear set motion；

(3) detect current l after an adjustment cycle₁And l₂, and calculate current deflection angle theta；

(4) judge whether current yaw angle θ exceedes preset value: if so, then proceed to step (2), if it is not, then controlling barrier-surpassing robot Continue uniform rectilinear to advance；

(5) judge in barrier-surpassing robot driving group, whether two planetary gear set reach ladder: if it is not, then continue uniform rectilinear advancing, If so, then two planetary gear set overturn together, cross step；

(6) judge whether two planetary gear set arrive at: if it is not, then entering new pose adjustment process, repeat step (1)～ (5), if so, then terminate to move, complete the control of planet gear type barrier-surpassing robot stair climbing in this way.

2. planet gear type barrier-surpassing robot stair climbing control method as claimed in claim 1 is it is characterised in that described deflection angle θ adopts equation below to obtain:

$\mathrm{\&theta;}=\arctan \frac{l_1-l_2}{l_{ab}};$

In formula: l_abRepresent the distance between central point of two planetary gear set in the same driving group of barrier-surpassing robot.

3. planet gear type barrier-surpassing robot stair climbing control method as claimed in claim 1 or 2 it is characterised in that described by Deflection angle control algolithm calculates the theoretical rotational speed of each planetary gear set, specifically includes:

(2.1) calculate t cycle time that barrier-surpassing robot pose adjusts first:

$t=k_1\&times;\frac{l_1+l_2}{({\mathrm{\&omega;}}_1+{\mathrm{\&omega;}}_2)\&times;r};$

In formula: k₁For ratio correction coefficient, ω₁,ω₂It is respectively the rotating speed of left and right two-wheeled in planetary gear set, r is in planetary gear set The radius of single wheel；

(2.2) then calculate pose adjustment amplitude a:

$a=k_2\frac{\mathrm{\&theta;}}{t};$

In formula: k₂For proportionality coefficient, θ is deflection angle；

(2.3) calculate the difference of rotating speed again:

$\mathrm{\&delta;}\mathrm{\&omega;}=a\sin (\frac{2\mathrm{\&pi;}}{t}\&times;t);$

In formula: t is the time of pose adjustment experience in the cycle, and t ∈ (0, t)；

(2.4) finally calculate the theoretical rotational speed ω of two planetary gear set in the same driving group of planet gear type barrier-surpassing robot₁'、 ω₂':

ω₁'=ω₁+δω,ω₂'=ω₂-δω.

4. planet gear type barrier-surpassing robot stair climbing control method as claimed in claim 3 is it is characterised in that judge obstacle detouring machine In device people's driving group, whether two planetary gear set reach ladder, particularly as follows:

Judge l₁And l₂Whether meet following condition: l₁=l₂=l_min, wherein, l_minThe value of l when reaching step for barrier-surpassing robot.

5. planet gear type barrier-surpassing robot stair climbing control method as claimed in claim 4 is it is characterised in that described planetary gear Formula barrier-surpassing robot is preferably four planetary gear barrier-surpassing robots or six planetary gear barrier-surpassing robots.

6. planet gear type barrier-surpassing robot stair climbing control method as claimed in claim 5 is it is characterised in that described planetary gear When formula barrier-surpassing robot is four planetary gear barrier-surpassing robot, it is provided with two groups of driving groups of before and after, two planets in every group of driving group Wheel group is all controlled by described step (1)-(6).

7. planet gear type barrier-surpassing robot stair climbing control method as claimed in claim 6 is it is characterised in that described planetary gear Formula barrier-surpassing robot be six planetary gear barrier-surpassing robots when, its be provided with before in after three groups of driving groups, two row in every group of driving group Star-wheel group is all controlled by described step (1)-(6).

8. planet gear type barrier-surpassing robot stair climbing control method as claimed in claim 7 is it is characterised in that described stair are excellent Elect spiral stairway or curved stairs as.

9. the planet gear type barrier-surpassing robot stair climbing control method as described in any one of claim 1-8 is it is characterised in that walk Suddenly the preset value described in (4) is specially 0～3 °.