JP2022078372A - Mobile working robot control device - Google Patents

Abstract

To provide a control device allowing a mobile working robot to advance without a main body direction of the robot from being changed due to influence of a carpet with less marks on the carpet without using an expensive member or sensor such as a camera and a gyro sensor.SOLUTION: A mobile working robot control device comprises: rotational speed instructing means for instructing a rotational speed of a wheel calculated from control of the wheel based on a prescribed moving track; disturbance estimating means for estimating a disturbance which occurs in each wheel when a main body moves using an output from current measuring means of a motor and an output from rotational speed measuring means of each wheel; and straight advance controlling means for controlling each wheel by adjusting disturbance information of each wheel from the disturbance estimating means so that the main body straightly advances.SELECTED DRAWING: Figure 1

Description

The present invention relates to a mobile work robot control device.

In recent years, various mobile work robots that are equipped with travel drive devices / sensors, travel control means, etc. in work equipment and perform automatic work have been developed or put into practical use. An example of a mobile work robot is a self-propelled vacuum cleaner. The self-propelled vacuum cleaner is a traveling steering means that uses two left and right drive wheels placed coaxially and a driven wheel installed in front of or behind the main body, a cleaning means consisting of a suction type vacuum cleaner and a brush, and obstacles. A detection means is provided, and the floor surface is cleaned by moving straight back and forth on the floor surface as shown in FIG. 10 while recognizing the surrounding wall surface. (For example, refer to Patent Document 1)

Japanese Unexamined Patent Publication No. 2001-258806 Japanese Patent Application Laid-Open No. 2003-180586 Japanese Unexamined Patent Publication No. 08-20039

In general, when the mobile work robot travels on a flat floor, particularly when there is nothing that hinders the movement of the drive wheels, the mobile robot travels toward the initially designated place as instructed. However, when traveling on the carpet, the drive wheels are affected by the mesh of the carpet and the direction of the hair flow of the carpet, and are swept in that direction.

For example, when traveling straight on the floor that is not affected by the carpet, the moving work robot main body travels along the straight track L1 in FIG. By reciprocating and repeating the traveling along the straight track L1 as shown in FIG. 10, it is possible to efficiently clean the vehicle without leaving any residue. However, on the carpet, the lateral displacement of the velocity v occurs due to the influence of the hair flow of the carpet, and even if the straight traveling control is performed along the linear trajectory L1, the main body is a linear trajectory whose direction is changed by the lateral displacement angle θ from the linear trajectory L1. Drive along L2. However, at this time, the main body direction d1 does not change, and the main body direction d1 and the direction of the straight track L2 do not match. Since it travels on a track deviated from L1, for example, if it is used to move straight back and forth on the floor surface and clean the floor surface by a certain width, there is a problem that uncleaned residue is left on the carpet.

Further, as a result of lateral displacement, the driven wheel and the brush receive a lateral force from the carpet, so that the main body turns at an angular velocity β as shown in FIG. It travels along the track L3 that deviates from the straight track L1. When only lateral displacement occurs, the main body direction d1 is always constant as shown in FIG. 4, but when turning occurs, the main body direction d2 in FIG. 5 always changes during traveling.

In Patent Document 2, in order to deal with lateral displacement and turning, a method of detecting the moving distance and turning speed by using a camera that images the floor surface and adding correction is considered, but the device is expensive because the camera is mounted. There is a problem of becoming. Further, if there is no mark such as a pattern or a shadow of a carpet hair on the floor surface, there is a problem that the difference in the floor surface image cannot be detected, and the moving distance and the turning speed cannot be detected.

In Patent Document 3, as a method of dealing with lateral displacement, an angular difference between the main body direction d1 and the actual moving direction (same as θ in FIG. 4) is detected by using a roller that can change the direction independently of the main body direction. A method of correcting by swinging the target direction of the main body to the opposite side to the lateral displacement based on the difference is considered.

However, since a gyro sensor is used for detecting the direction of the main body, there is a problem that the device becomes expensive. In addition, for correction, it is necessary to keep the direction of the main body toward the target main body against the force of turning the main body, and a gyro sensor is also required for this control.

When the gyro sensor is not used, the angle between the main body direction d2 and the straight track L1 is not known. Therefore, even if the roller detects the angle difference between the main body 1 direction and the actual moving direction, the straight track L1 and the actual moving direction The difference θ cannot be detected. In addition, the angle obtained by the rollers is a value that combines both the effects of lateral slippage and the effects of turning, so when turning is caused by the carpet, the angle difference between the main body direction and the actual moving direction due to lateral slippage is accurately detected. I can't.

Since lateral displacement detection and correction itself using the angle between the roller and the main body can be realized at a relatively low cost, straight-line control is possible if the main body direction can be maintained without using an expensive sensor such as a gyro sensor. Become.

The present invention provides a control device for moving a mobile robot to move forward without changing the direction of the main body due to the influence of the carpet even on a carpet with few marks without using expensive members such as a camera and a gyro sensor. With the goal.

In order to solve the above-mentioned conventional problems, the present invention has a rotation speed indicating means for instructing the rotation speed of the wheel based on the target trajectory, and the rotation speed and the rotation speed instruction value of the wheel even if a disturbance is applied to the wheel. A rotation speed control means that keeps the difference sufficiently small, a current measurement means that measures the current flowing through the motor, a rotation speed measuring means that measures the rotation speed of the motor and wheels, and a wheel based on the current flowing through the motor and the rotation speed of the wheels. Disturbance estimation means for estimating the disturbance applied to the vehicle, front-rear slip speed estimation means for calculating how much the left and right wheels are slipping back and forth from the estimated disturbance, and left-right difference in the estimated slip speed of each wheel. It is a mobile work robot control device provided with a straight-ahead control means for correcting a rotation speed command value and canceling a turn.

As a result, the turning can be canceled based on the information of the current flowing through the motor and the rotation speed of the wheels of the motor, so that the instruction from the rotation speed indicating means is corrected and the same rotation speed is indicated to the left and right motors. There is no difference in the forward speed of the left and right wheels, and it is possible to realize forward movement without changing the direction of the main body even on the rug. This configuration eliminates the need to detect turning using a camera, gyro sensor, or the like.

INDUSTRIAL APPLICABILITY According to the present invention, a mobile work robot can move forward on a carpet without changing its main body direction without using an expensive sensor such as a camera or a gyro sensor.

A control block diagram according to an embodiment of the present invention. Floor plan of mobile work robot Block diagram of the disturbance observer A diagram illustrating lateral displacement of a mobile work robot with respect to the traveling direction. A diagram illustrating lateral displacement and turning of a mobile work robot with respect to the traveling direction. Explanatory drawing of the disturbance acting on the wheel and the front-back slip speed of the wheel Relationship between the disturbance acting on the wheel and the front-back slip speed of the wheel Explanatory drawing of the main body turning caused by the difference in rotation speed between the left and right wheels Explanatory drawing to prevent turning of the main body by correcting the rotation speed of the wheels Explanatory diagram of straight-ahead round-trip movement Control block diagram without the present invention

Hereinafter, embodiments of the present invention will be described with reference to FIGS. 1 and 2. The present invention is not limited to the embodiments.

FIG. 2 is a plan view showing a schematic configuration of a mobile work robot. Reference numeral 1 is a main body of the mobile work robot, 2L and 2R are wheels arranged on the left and right sides of the main body 1, respectively, and the rotation axes are coaxial. Further, the radii of the wheels 2L and 2R are the same. This rotating shaft is fixed to the main body. The 3L and 3R are motors that drive the wheel 2L and the wheel 2R, respectively. The wheels 2L and the motor 3L and the wheels 2R and the motor 3R can be rotated independently on the left and right, and the direction of the main body 1 can be changed by the difference in the rotation speeds. 4L and 4R are current measuring means for measuring the current flowing through the motors 3L and 3R, respectively, and 5L and 5R are rotational speed measuring means for measuring the rotational speed of the wheels 2L and 2R, respectively. 6 is a driving wheel. Reference numeral 7 is a control unit, and the control unit 7 performs wheel rotation speed control and straight-ahead control.

FIG. 11 shows a conventional general control block diagram that does not use the method of the present invention.

The rotation speed indicating means 71 determines the rotation speed of the wheels 2L and 2R from the target forward speed and the track, and outputs the rotation speed instruction value to the rotation speed control means 72L and 72R. For example, in the case of a right-turning track, the instruction value for turning the wheel 2L fast and the wheel 2R slow is output. In the case of straight running, a rotation speed instruction value that makes the rotation speeds of the wheels 2L and 2R the same is output.

The rotation speed control means 72L and 72R perform rotation speed control of 2L and 2R with the rotation speed instruction value output by the rotation speed instruction means 71 as a target value. In the rotation speed control in this case, the measured values of the rotation speeds of the wheels 2L and 2R are received from the rotation speed measuring means 5L and 5R, and the measured values are sent to the motors 3L and 3R so as to be the same as the input target rotation speed. Adjust the output drive voltage.

The motors 3L and 3R are rotated by the drive voltage output from the rotation speed control means 72L and 72R, and the driving force is transmitted to the wheels 2L and 2R via a mechanism such as a gear. Here, for the sake of simplicity, it is assumed that the rotation speed of the motors 3L and 3R becomes the rotation speed of the wheels 2L and 2R as it is.

When traveling on an ideal floor surface, in the configuration of FIG. 11, the rotation speed indicating means 71 sends the same rotation speed instruction value to the rotation speed control means 72L and 72R, so that the wheels 2L and 2R rotate in the same manner. It can be turned at speed, and the wheels 2L and 2R move forward at the same speed. As a result, the main body 1 moves forward without changing the direction.

However, when traveling on a carpet, a force in the turning direction acts on the main body 1 due to lateral displacement. At this time, the wheels 2L and 2R rotate at the same rotation speed due to the action of the rotation speed control means 72L and 72R according to the rotation speed instructed by the rotation speed indicating means 71, but the main body 1 turns and changes its traveling direction. ..

By this turning, the wheels 2L and 2R slide on the rug, and a force of different magnitude is applied from the rug to the ground plane of the wheels 2L and 2R. And appear as a change in the rotational speed of the wheels 2L and 2R.

FIG. 1 shows a control block diagram in the present invention.

The straight-ahead control means 75 adds a rotation speed correction value to the output of the rotation speed indicating means 71.

The rotation speed measuring means 5L and 5R measure the rotation speed of the wheels 2L and 2R, and the current measuring means 4L and 4R measure the current flowing through the motors 3L and 3R, and capture the change in the current and the rotation speed due to the disturbance. The measured current value and rotation speed value are output to the disturbance estimation means 73L and 73R.

The disturbance estimation means 73L and 73R estimate the disturbance acting on the wheel from the rotation speed output by the rotation speed measuring means 5L and 5R and the current measurement values output by the current measuring means 4L and 4R, respectively, and the estimated values are used as the back-and-forth slip speed. It is output to the estimation means 74L and 74R. The disturbance acting on the wheels is the force generated by the turning of the carpet in the rotational direction of the wheels 2L and 2R with respect to the ground plane of the wheels.

The front-back slip speed estimation means 74L and 74R estimate the front-back slip speed of the wheels 2L and 2R from the disturbance output by the disturbance estimation means, and output the front-back slip speed estimation means to the straight-ahead control means 75. The front-back slip speed of the wheel is the speed of the ground contact surface of the wheel with respect to the direction of the main body 1. On a non-slip floor, the wheel slip speed is zero regardless of the wheel rotation speed.

The straight-ahead control means 75 calculates the turning speed of the main body 1 from the front-back slip speed of the wheels 2L and 2R, calculates the rotation speed correction value of the wheels 2L and 2R that cancels the turning, and uses the correction value as the rotation speed indicating means. It is output so as to be added to the rotation speed indicated value output by 71.

The operation and operation of the mobile work robot configured as described above will be described below.

As shown in FIG. 8, the radius of the wheels 2L and 2R is r, the distance W between the wheels 2L and the wheels 2R, and the speeds at which the axes of the wheels 2L and 2R move in the direction of the main body 1 are v _L and v _R , respectively. .. Assuming that the turning speed of the main body 1 is β, it can be expressed as β = (v _R − v _L ) / W.

In FIG. 5, L1 is a target straight line orbit. On an ideal floor surface without slipping, wheels of the same radius are used for the wheels 2L and 2R, and the rotation speed indicating means 71 rotates each motor as the same rotation speed indicating value for the rotation speed controlling means 72L and 72R. By outputting the velocity ω *, the main body 1 can be driven along this trajectory. When the rotation speed indicating means 71 outputs the same rotation speed ω * to the rotation speed control means 72L and 72R, the rotation speed control means 72L and 72R have the wheels 2L and 2R according to the rotation speed ω * indicated by the given rotation speed instruction value. A drive voltage that rotates the wheels 2L and 3R is output to the motors 3L and 3R, and the rotation speeds ω of the wheels 2L and 2R are converged to ω *. When there is no slip between the floor surface and the wheels 2L and 2R, v _L = v _R = rω *, and the speed at which the wheels 2L and 2R move toward the main body is the same. At this time, the turning speed of the main body 1 becomes β = (rω * −rω *) / W = 0, and the main body 1 can be advanced without changing the direction of the main body.

As shown in FIG. 5, when there is a flow of hair on the carpet, a lateral displacement of the speed v occurs when traveling on the carpet, and this lateral displacement causes the driven wheel 6 to receive a resistance force from the carpet in the direction opposite to the lateral displacement direction. , The main body 1 receives a force in the turning direction and turns. When this turn is a left turn, the contact patch of the wheel 2L moves backward and the contact patch of the wheel 2R moves forward. The opposite is true for a right turn. As a result, different front-rear slip speeds v _sL and v _sR are generated on the ground planes of the wheels 2L and 2R, respectively. The front-back slip speed of the wheel is the speed vs. the speed of the ground contact surface of the wheel in the main body _direction (negative speed in the case of FIG. 6) shown in FIG. If there is no slip, this front-back slip speed will be zero.

When the front-back slip speed v _sL and v _sR are generated, the speed of the shafts of the wheels 2L and 2R is v _L = rω *.
As + v _sL , v _R = _{rω **} v sR, the speed is the sum of the speed due to the rotation of the wheel and the speed due to slippage. When the straight-ahead control means 75 is not used, the laterality of this speed caused by the back-and-forth sliding speed causes a turning speed β = (v _R − v _L ) / W = (v _sR −v _sL ) / W.

In the present invention, the straight-ahead control means 75 adds correction quantities ω _L and ω _R to the rotation speed ω * of the wheel instructed by the rotation speed indicating means 71 as shown in FIG. If v _L = v _R is set by adding a correction amount, the angular velocity β of turning becomes 0, so that the wheel 2L and 2R are affected by the force in the turning direction due to the influence of the carpet, and the front-back slip speed is generated. Even if it is, it can move forward without changing the direction of the main body 1.

In the present invention, a method in which the straight-ahead control means 75 determines a correction amount and advances the main body 1 without changing the direction will be described. The values of the front-back slip speed v _sL output by the front-back slip speed estimation means 74L and the front-back slip speed _{vs R} output by the front-back slip speed estimation means 74R are used for determining the correction amount.

As an example, the correction amounts ω _L and ω _R are set to (v _sR -v _sL ) / (2r) and (v _sL -v _sR ) / (2r), respectively, and the output ω of the rotation speed instruction value of the rotation speed instruction means 71 is set. By adding to *, v _L = r (ω * + ω _L ) + v _sL = rω * + (v _L + v _R ) / 2, v _R = r (ω * + ω _R ) + v sR = _rω * + (v _L ) It can be + v _R ) / 2, and v _L = v _R. As a result, the turning speed of the main body 1 becomes β = (v _R − v _L ) / W = 0, and the main body 1 can move forward without changing the direction on the carpet.

Next, a method in which the front-back slip speed estimation means 74L and 74R estimate the front-back slip speed v _sL and v _sR will be described. For the estimation of the back-and-forth slip speed, the values of the disturbance F _dL output by the disturbance estimation means 73L and the values of the disturbance F _dR output by the disturbance estimation means 73R are used. As shown in FIG. 6, the disturbance is a force _Fd applied by the carpet to the ground plane of the wheel in the direction of rotation or in the opposite direction. The disturbance F _d is generated in the direction opposite to the forward / backward sliding speed vs. when _sliding at that speed. When the rotation speed ω * based on the command instruction value output by the rotation speed indicating means 71 is constant, the front-rear slip speed vs of the wheel and the force F _d received by the wheel from the _rug are unique to the rug as shown in FIG. There is a relationship. The front-back slip speed estimation means 74L and 74R estimate the front-back slip speed of the wheel from the estimated value of the disturbance acting on the wheel by using the relationship obtained by the experiment in advance.

FIG. 7 shows the relationship of the disturbance _{Fd estimated} to be the slip speed vs L of the wheels 2L when the moving work robot is run at a constant speed on a type of carpet called a Wilton carpet. One point in FIG. 7 randomly takes a section for 2 seconds during running, and uses a value obtained by taking the average value of _vs and F _d of the section. The slip speed vs is the forward speed _vL of the shaft of the wheel 2L measured using motion capture minus the forward speed _rω of the shaft of the wheel when the wheel does not slip. The estimated disturbance Fd is estimated by the disturbance estimation means 73L. From FIG. 7, it can be seen that the larger the estimated disturbance, the smaller the sliding speed, that is, the faster the wheel's tread is sliding backwards. It has been confirmed that this tendency appears both in the right wheel 2R and in the main carpets other than Wilton carpet, such as Axminster carpet and tufted carpet.

Next, the configuration and operation of the disturbance estimation means 73L and 73R will be described with reference to FIG. The concept of the disturbance observer explained below is used to estimate the disturbance. The current flowing through the motor generates a torque _Tr that rotates the motor. However, the disturbance acting on the wheel causes the torque disturbance T _d . As a result, the torque that actually rotates the motor is T _m = _Tr −T _d . Solving this for T _d , T _d = _Tr −T _m , and the disturbance observer estimates the torque disturbance T _d based on this equation.

FIG. 3 is a block diagram showing a configuration example of the disturbance estimation means 73L and 73R. Since the disturbance estimation means 73L and 73R have the same configuration, the disturbance estimation means 73 will be referred to below. The disturbance estimation means 73 is a multiplication unit 731 that outputs the torque _Tr generated from the current by multiplying the current values from the motors 3L and 3R measured by the current measuring means 4L or the current measuring means 4R by the torque multiplier K _t of each motor. The motor is multiplied by the rotational speed value measured by the rotational speed measuring means 5L or 5R, where J is the inertial moment of the mechanical load of the motors 3L and 3R, D is the viscous resistance, and s is the Laplace operator (Js + D). The multiplying unit 732 that outputs the torque T _m actually applied to turn the motor, and the subtracting unit 733 that outputs the torque disturbance T _d of the motor by subtracting the output T _m of the multiplying unit 732 from the output _Tr of the multiplying unit 731. The output of the first-order lag filter 734 with a cutoff frequency ω ₀ that smoothes the output of the subtraction unit 733 and the output of the first-order lag filter 734 are multiplied by 1 / r, and the smoothed torque disturbance T _d acts on the edge of the wheel. It is composed of a multiplication unit 735 that converts the estimated value of disturbance F _d .

The control of FIG. 3 will be described below assuming an environment that occurs when the main body 1 actually travels on the carpet.

When a disturbance Fd is applied in the rotation direction of the wheel 2L or the wheel 2R, the disturbance reduces the rotation speed ω of the motor via the mechanical transmission function 1 / (Js + _D ) which is a load on the motor. Alternatively, the rotation speed control means 72L and 72R receive the information that the rotation speed of the motor has dropped, and a larger current I is passed in order to increase the rotation speed of the motor. When the rotation speed of the wheel decreases, the value of T _m in FIG. 3 decreases, and when the current increases, the value of _Tr in FIG. 3 increases. Since the estimated torque disturbance T _d is calculated by the subtraction unit 733 as T _d = _Tr −T _m , the torque disturbance is estimated when the current I increases or the rotation speed ω decreases due to the disturbance applied to the wheels. The value T _d and the disturbance F _d acting on the wheel will result in an increase. The opposite happens when negative disturbances are added.

In this way, the torque disturbance T _d is estimated using the disturbance observer, and the torque disturbance T _d is multiplied by the reciprocal of the wheel radius. , The disturbance F _d acting in the rotation direction of 2R can be estimated.

As described above, according to the present embodiment, the wheels are obtained from the rotation speed values of the wheels 2L and 2R output by the rotation speed measuring means 5L and 5R and the current values flowing through the motors 3L and 3R output by the current measuring means 4L and 4R. The disturbance acting on 2L and 2R is estimated by the disturbance estimation means 73L and 73R, and the front-back slip speed of the wheels 2L and 2R is estimated by the front-back slip speed estimation means 74L and 74R based on the result, and the estimated front-back slip speed is obtained. By determining the rotation speed correction values of the wheels 2L and 2R in the straight-ahead control means 75 based on this, the forward speeds of the axes of the wheels 2L and 2R become the same, so that the turning speed of the main body 1 becomes 0 and the main body 1 You can move forward without changing direction. As a result, the mobile work robot can move forward without changing the direction of the main body due to the influence of the carpet even on a carpet with few marks without using an expensive sensor such as a camera or a gyro sensor.

Since the present invention suppresses the turning of the mobile work robot by using the measured values of the current and the rotation speed of the wheels, not only the self-propelled vacuum cleaner on the rug, but also the self-propelled lawn mower and the automatic tractor. It can also be applied to such things as.

1 Main body 2L, 2R Wheel 3L, 3R Motor 4L, 4R Current measuring means 5L, 5R Rotation speed measuring means 6 Driven wheel 7 Control unit 71 Rotation speed indicating means 72L, 72R Rotation speed controlling means 73L, 73R Disturbance estimation means 74L, 74R Forward / backward slip speed estimation means 75 Straight control means 731 Multiplying part 732 Multiplying part 733 Subtracting part 734 First-order lag filter 735 Multiplying part

Claims (5)

The body and at least a pair of wheels on the same axis that move the body,
A motor that drives each wheel, a current measuring means that measures the current of the motor, and
In a mobile work robot having a rotation speed measuring means for measuring the rotation speed of each wheel and a control unit for controlling the movement of the main body.
The control unit includes a rotation speed indicating means for instructing a wheel rotation speed calculated from wheel control based on a predetermined moving track, and a rotation speed indicating means.
Disturbance estimation means for estimating the disturbance generated on each wheel when the main body is moved by using the output from the current measuring means of the motor and the output from the rotation speed measuring means of each wheel.
A mobile work robot control device comprising a straight-ahead control means that adjusts the disturbance information of each of the wheels from the disturbance estimation means and controls each of the wheels so that the main body moves straight. The straight-ahead control means calculates the turning speed of the main body calculated from the front-back slip speed estimation means that estimates the front-back slip of each wheel based on the output from the disturbance estimation means that estimates the disturbance applied to each wheel. The mobile work robot control device according to claim 1, wherein the rotation speed correction value of each wheel that cancels the turning is output and added to the rotation speed of the wheel output from the rotation speed indicating means. The disturbance estimation means is
Claim 1 or claim 1 using a disturbance observer that estimates the torque disturbance T _d = _Tr −T _m acting on each wheel from the torque Tr that rotates the motor generated from the current flowing through the motor and the torque T _m that actually rotates the motor _. 2. The mobile work robot control device according to 2. The disturbance estimation means multiplies the torque disturbance _Td estimated from the disturbance observer by the inverse of the wheel radius, so that the rotation speed measuring means of each wheel and the current measuring means of the motor can be used to obtain the respective ones. The mobile work robot control device according to any one of claims 1 to 3, wherein the disturbance F _d acting in the rotation direction of the wheel is estimated. The front-back slip speed estimation means uses the relationship between the front-back slip speed of a wheel obtained in advance and the disturbance that the wheel receives from the carpet when the rotation speed based on the command instruction value output by the rotation speed instruction means is constant. The mobile work robot control device according to any one of claims 1 to 4, wherein the front-rear slip speed of each wheel is estimated based on the estimated value of the disturbance acting on each wheel obtained from the disturbance estimation means.

Images