JP2016170616A - Contacting external force calculation system, rolling resistance torque detection method - Google Patents

Abstract

PROBLEM TO BE SOLVED: To solve the problems in which: a contacting external force applied to a moving body when the moving body contacts with an external environment, is calculated by a motion equation of a driving wheel; and when road surface characteristic is changed though the moving body does not contact with the external environment, it is misunderstood that the contacting external force is applied to the moving body because the moving body contacts with the external environment.SOLUTION: A life support system 1 comprises: a rolling resistance torque acquisition unit 26 which obtains rolling resistance torque of the left driving wheel 7L and the right driving wheel 7R received from road surface 17 where the life support system is moving on; a contacting external force torque calculation unit 27 which calculates contacting external force torque acting on the left driving wheel 7L and the right driving wheel 7R when a life support robot 3 contacts with the external environment on the basis of rotation information, driving torque, the rolling resistance torque, motion equations of rotation of the left driving wheel 7L and the right driving wheel 7R; and a contacting external force calculation unit 28 which calculates the contacting external force acting on the life support robot 3 when the life support robot 3 contacts with the external environment on the basis of the contacting external force torque.SELECTED DRAWING: Figure 2

Description

  The present invention relates to a contact external force calculation system and a rolling resistance torque detection method.

  In recent years, mobile bodies that move autonomously while avoiding external environments such as obstacles have been developed.

  Patent Document 1 discloses a technique for calculating a contact external force that acts on a moving body when the moving body contacts an external environment without providing a special collision sensor. Specifically, by detecting rotation information of the drive wheel and substituting this rotation information and drive torque into the equation of motion of the drive wheel, a contact external force torque that acts on the drive wheel when the moving body contacts the external environment And the contact external force acting on the moving body is calculated based on the contact external force torque.

JP 2014-44625 A

  However, in the configuration of Patent Document 1 described above, when the road surface characteristics change even though the moving body is not in contact with the external environment, the mobile body is in contact with the external environment and misrecognized that a contact external force acts on the moving body. There was a problem.

  Therefore, an object of the present invention is to calculate the contact external force acting on the moving body by the moving body coming into contact with the external environment by the equation of motion of the driving wheel, even though the moving body is not in contact with the external environment. It is an object of the present invention to provide a technique for solving the problem that when a change occurs, the moving body comes into contact with the external environment and erroneously recognizes that a contact external force has acted on the moving body.

According to a first aspect of the present invention, there is provided a contact external force calculation system that calculates a contact external force that acts on a moving body by the moving body coming into contact with an external environment, based on an equation of motion of rotation of the driving wheel, the driving wheel Rotation information acquisition means for acquiring the rotation information of the vehicle, drive torque acquisition means for acquiring drive torque for driving the drive wheels, and rolling for acquiring the rolling resistance torque received by the drive wheels from the currently moving road surface Based on the resistance torque acquisition means, the rotation information, the driving torque, the rolling resistance torque, and the equation of motion of the rotation of the driving wheel, the moving body comes into contact with the external environment to the driving wheel. Based on the contact external force torque, the contact external force torque calculating means for calculating the contact external force torque that acts on the movable body is applied to the mobile body by contacting the external environment. A contact force calculating means for calculating the contact force that, with a contact force calculating system is provided. According to the above configuration, when calculating the contact external force acting on the moving body by the moving body coming into contact with the external environment by the equation of motion of the rotation of the driving wheel, from the road surface currently moving Since the rolling resistance torque received by the driving wheel is taken into account, when the road surface characteristic changes even though the moving body is not in contact with the external environment, the moving body comes into contact with the external environment and contacts the moving body. The problem of misrecognizing that the contact external force has acted can be solved.
The contact external force calculation system further includes storage means for storing the road surface image information and the rolling resistance torque in association with each other. The rolling resistance torque acquisition means refers to the storage means based on image information obtained by imaging the road surface that is currently moving, so that the driving wheel receives the driving wheel from the road surface that is currently moving. Get the rolling resistance torque. According to the above configuration, the rolling resistance torque corresponding to the road surface can be acquired by acquiring image information obtained by imaging the road surface that is currently moving.
The rolling resistance torque stored in the storage means moves the moving body on the road surface under the condition that the moving body does not come into contact with the external environment, and the rotation information of the driving wheel during the movement of the moving body. And a driving torque for driving the driving wheel is acquired and calculated based on the rotation information, the driving torque, and a motion equation of rotation of the driving wheel.
According to a second aspect of the present invention, there is provided a contact external force calculation method for calculating a contact external force acting on a moving body by the moving body coming into contact with an external environment based on an equation of motion of rotation of the drive wheel, Obtaining rotation information, obtaining a driving torque for driving the driving wheel, obtaining a rolling resistance torque received by the driving wheel from a currently moving road surface, and the rotation information. Calculating, based on the driving torque, the rolling resistance torque, and the equation of motion of rotation of the driving wheel, a contact external force torque that acts on the driving wheel when the moving body contacts the external environment; Calculating a contact external force that acts on the moving body when the moving body comes into contact with the external environment based on the contact external force torque. A method is provided. According to the above method, when calculating the contact external force acting on the moving body by the moving body coming into contact with the external environment from the equation of motion of the rotation of the driving wheel, the road surface currently moving is calculated. Since the rolling resistance torque received by the driving wheel is taken into account, when the road surface characteristic changes even though the moving body is not in contact with the external environment, the moving body comes into contact with the external environment and contacts the moving body. The problem of misrecognizing that the contact external force has acted can be solved.

  According to the present invention, when calculating the contact external force acting on the moving body by the moving body coming into contact with the external environment from the road surface that is currently moving, Since the rolling resistance torque received by the driving wheel is taken into consideration, when the road surface characteristics change even though the moving body is not in contact with the external environment, the moving body comes into contact with the external environment and The problem of misrecognizing that a contact external force has been applied can be solved.

It is a schematic diagram of a life support system. It is a functional block diagram of a life support robot. It is explanatory drawing of the coordinate system of a life support robot. It is a functional block diagram of a server. It is a figure which shows the data structure of rolling resistance torque DB. It is a conceptual diagram in the case of moving on the inclined road surface. It is a contact external force calculation flow. It is a graph which shows the time change of rolling resistance torque. It is a graph which shows the time change of a contact external force. It is a rolling resistance torque detection flow.

  Embodiments of the present invention will be described below with reference to the drawings. FIG. 1 shows an outline of a life support system 1 (contact external force calculation system). As shown in FIG. 1, a life support system 1 includes a plurality of life support robots 3 (moving bodies) that can move autonomously within a service environment 2 and a plurality of life support robots 3 on a network 4 such as a LAN or a WAN. And a server 5 accessible via the server.

  FIG. 2 shows a functional block diagram of the life support robot 3. As shown in FIGS. 1 and 2, the life support robot 3 includes a moving body 6, a left drive wheel 7L (drive wheel), a right drive wheel 7R (drive wheel), a front driven wheel 8F (driven wheel), a rear Driven wheel 8B (driven wheel), control unit 9, camera 10 (imaging means), communication driver 11 (communication means), right motor 12R (drive means), left motor 12L (drive means), right encoder 13R, left encoder 13L , A motor driver 14, a counter 15, and a posture sensor 16 (tilt angle detecting means).

  The left driving wheel 7L, the right driving wheel 7R, the front driven wheel 8F, and the rear driven wheel 8B are wheels that are rotatably attached to the moving body 6. The left driving wheel 7L and the right driving wheel 7R are driven wheels and are arranged on the left and right with respect to the traveling direction. The front driven wheel 8F and the rear driven wheel 8B are wheels that are not driven, and are disposed forward and backward with respect to the traveling direction.

  A right motor 12R and a right encoder 13R are attached to the right drive wheel 7R. A left motor 12L and a left encoder 13L are attached to the left drive wheel 7L. The motor driver 14 controls the operations of the right motor 12R and the left motor 12L based on a command from the control unit 9. Thereby, the life support robot 3 can perform arbitrary movements such as forward and backward movement, left and right turn, and acceleration / deceleration.

  The right encoder 13R generates a pulse signal corresponding to the rotation of the output shaft of the right motor 12R and outputs the pulse signal to the counter 15. Similarly, the left encoder 13L generates a pulse signal corresponding to the rotation of the output shaft of the left motor 12L and outputs the pulse signal to the counter 15. The counter 15 counts the pulse signals output from the right encoder 13R and the left encoder 13L, and outputs the count value to the control unit 9. Accordingly, the control unit 9 can calculate and acquire the rotation information of the right driving wheel 7R and the rotation information of the left driving wheel 7L based on the count value output from the counter 15. Here, the rotation information is information including at least one of a rotation angle, a rotation angular velocity, and a rotation angular acceleration, or information including all of the rotation angle, the rotation angular velocity, and the rotation angular acceleration.

  The posture sensor 16 includes, for example, a gyro sensor and an acceleration sensor, detects the posture of the life support robot 3, and outputs the detection result to the control unit 9.

  Returning to FIG. 1, the road surface 17 of the service environment 2 of the present embodiment includes a road surface 17 </ b> A with flooring, a road surface 17 </ b> B with carpet, and a road surface 17 </ b> C with tatami mats.

  Then, the camera 10 images the road surface 17 on which the life support robot 3 is currently moving based on a command from the control unit 9 to generate image information, and outputs the generated image information to the control unit 9.

  The control unit 9 is connected to the network 4 via the communication driver 11.

  In order for the life support robot 3 configured as described above to move stably, contact that acts on the life support robot 3 by the life support robot 3 coming into contact with an external environment (for example, another life support robot 3). It is important to properly grasp external forces. In order to appropriately grasp the contact external force, for example, a plurality of contact sensors may be arranged on the outer peripheral surface of the mobile body 6 of the life support robot 3. However, if a plurality of contact sensors are provided on the outer peripheral surface of the mobile body 6 of the life support robot 3, the manufacturing cost of the life support robot 3 is increased. In contrast, in the present embodiment, the contact external force is calculated by solving the equations of motion of the right drive wheel 7R and the left drive wheel 7L.

  Here, as shown in FIG. 1, the material of the road surface 17 of the service environment 2 where the life support robot 3 plays an active role is not necessarily unified, and the road surface 17 of different materials is mixed as in this embodiment. There may be. For example, if the life support robot 3 moves from a road surface 17A that is flooring to a road surface 17B that is carpet as indicated by a white arrow in FIG. 1, the right driving wheel 7R and the left driving wheel 7L are received from the road surface 17. The rolling resistance torque will change. The rolling resistance torque takes a different value for each surface property of the road surface 17, and the right drive wheel 7R and the left drive are caused by the elastic deformation of the road surface 17 and the elastic deformation of the right drive wheel 7R and the left drive wheel 7L itself. This torque acts on the right drive wheel 7R and the left drive wheel 7L against the rotation of the wheel 7L. This change acts as a disturbance on the equations of motion of the right drive wheel 7R and the left drive wheel 7L, and thus hinders the calculation of the contact external force. Specifically, when the life support robot 3 moves from the floor surface 17A, which is flooring, to the road surface 17B, which is a carpet, the life support robot 3 is actually not in contact with the external environment. There is a possibility that the life support robot 3 may mistakenly recognize that 3 is in contact with the external environment.

  With respect to this problem, the misrecognition is solved in the present embodiment by configuring the life support robot 3 as follows.

  As shown in FIG. 2, the control unit 9 includes a CPU 20 (Central Processing Unit) as a central processing unit, a read / write free RAM 21 (Random Access Memory), and a read-only ROM 22 (Read Only Memory). Then, the CPU 20 reads out and executes the control program stored in the ROM 22, so that the control program causes the hardware such as the CPU 20 to operate as a rotation angle detection unit 24 (rotation information acquisition unit) and a drive torque acquisition unit 25 (drive). Torque acquisition means), rolling resistance torque acquisition section 26 (rolling resistance torque acquisition means), contact external force torque calculation section 27 (contact external force torque calculation means), contact external force calculation section 28 (contact external force calculation means), movement control section 29 ( Movement control means) and rolling resistance torque detection unit 30 (rolling resistance torque detection means).

  The movement control unit 29 controls the movement of the life support robot 3 by outputting a desired command to the motor driver 14.

  The rotation angle detector 24 calculates and acquires the rotation information of the right drive wheel 7R and the rotation information of the left drive wheel 7L based on the count value output from the counter 15.

  The drive torque acquisition unit 25 acquires the drive torque of the right drive wheel 7R and the left drive wheel 7L based on a command output from the movement control unit 29 to the motor driver 14.

  The rolling resistance torque acquisition unit 26 accesses the server 5 and acquires the rolling resistance torque received from the currently moving road surface 17.

  The contact external force torque calculation unit 27 is based on the rotation information acquired by the rotation angle detection unit 24, the drive torque acquired by the drive torque acquisition unit 25, and the rolling resistance torque acquired by the rolling resistance torque acquisition unit 26. By solving the equation of motion of rotation of the right driving wheel 7R and the left driving wheel 7L, contact external force torque acting on the right driving wheel 7R and the left driving wheel 7L when the life support robot 3 comes into contact with the external environment is calculated.

  Based on the contact external force torque calculated by the contact external force torque calculation unit 27, the contact external force calculation unit 28 calculates a contact external force that acts on the life support robot 3 when the life support robot 3 contacts the external environment.

  Then, the movement control unit 29 controls the movement of the life support robot 3 while considering the contact external force calculated by the contact external force calculation unit 28.

  Hereafter, each said component is demonstrated in detail.

  First, the coordinate system of the life support robot 3 will be considered with reference to FIG. It is assumed that the space in which the life support robot 3 moves is on a horizontal plane. FIG. 3 is a diagram illustrating a coordinate system of the life support robot 3 when the life support robot 3 is viewed from above. It is assumed that the coordinates of the life support robot 3 viewed from the origin of the absolute coordinate system are pB = [pBx, pBy, pBθ] T. pBx and pBy indicate the position of the center of gravity of the life support robot 3, and pBθ indicates the direction (angle) of the life support robot 3. Further, the distance between the right driving wheel 7R and the left driving wheel 7L of the life support robot 3 is 2l, the radius of the right driving wheel 7R and the left driving wheel 7L is r, and the rotation angle of the right driving wheel 7R and the left driving wheel 7L is Let θW = [θL, θR] T. Furthermore, a coordinate system of the life support robot 3 is assumed in which the traveling direction of the life support robot 3 is the X axis.

  In the coordinate system of the life support robot 3 assumed as described above, if the contact external force fB acting on the life support robot 3 is fB = [fBx, fBy, τB] T, it acts on the life support robot 3 in the absolute coordinate system. The contact external force fBA can be expressed as fBA = R (pBθ) fB. Note that R (pBθ) represents a rotational transformation matrix that is rotated by pBθ, and is expressed as the following equation (1). However, in the following formula (1), SpBθ = sin (pBθ) and CpBθ = cos (pBθ).

  Further, it is assumed that the right driving wheel 7R and the left driving wheel 7L rotate without slipping on the road surface 17. In this case, using the rotation angular velocity (first-order differential value of θL) of the right drive wheel 7R and the rotation angular velocity (first-order differential value of θR) of the left drive wheel 7R, the speed of the life support robot 3 (1 of pBx). The first-order differential value), the first-order differential value of pBy, and the angular velocity of the posture (first-order differential value of pBθ) are obtained by the following equation (2).

  By the way, for example, in the dead reckoning (DR) system (or odometry), the self position of the moving body can be calculated by integrating the speed of the moving body and the angular velocity of the posture. On the other hand, if the self-position is calculated simply using the dead reckoning method, the error of the velocity of the moving body and the angular velocity error of the posture are also integrated, so the finally calculated self-position is a problem in accuracy as absolute position information. Can occur. In particular, since the slip occurs between each drive wheel and the road surface when the mobile body turns, the above error becomes even larger.

  On the other hand, the matrix JB in the above equation (2) represents a Jacobian matrix. Therefore, using the transposed matrix JBT of the Jacobian matrix JB, the contact external forces fBx, fBy, τB acting on the center of gravity of the life support robot 3, the contact external force torque τR of the right drive wheel 7R, and the contact external force of the left drive wheel 7L The following equation (3) showing the relationship with the torque τL can be obtained. Here, the contact external force torque τW is a torque that acts on the right driving wheel 7R and the left driving wheel 7L when the life support robot 3 comes into contact with the external environment.

  The expression (3) finally obtained as described above does not include the orientation pBθ of the life support robot 3. Therefore, in the coordinate system of the life support robot 3, the contact external forces fBx, fBy, τB acting on the center of gravity of the life support robot 3, the contact external force torque τR of the right drive wheel 7R, and the contact external force torque τL of the left drive wheel 7L. Is independent of the orientation pBθ of the life support robot 3. That is, if the contact external force torque τR and the contact external force torque τL can be estimated, the contact external forces fBx, fBy, and τB acting on the center of gravity of the life support robot 3 can be calculated backward using the above equation (3).

  In this calculation process, the expression (3) that is not affected by the orientation pBθ of the life support robot 3 is used, and therefore, it is not affected by the integration error due to the dead reckoning method. Therefore, the contact external force acting on the center of gravity position of the life support robot 3 can be obtained with high accuracy. However, since the Jacobian matrix JB is not square, a pseudo inverse matrix is used as described later.

  Here, the equation of motion of rotation of the left drive wheel 7L and the right drive wheel 7R can be expressed as the following equation (4).

  In the above equation (4), IL and IR represent the equivalent moment of inertia coefficients of the left driving wheel 7L and the right driving wheel 7R, respectively, and BL and BR represent the viscous friction coefficients of the left driving wheel 7L and the right driving wheel 7R, respectively. EL and ER indicate the Coulomb friction coefficients of the left driving wheel 7L and the right driving wheel 7R, respectively. In the present embodiment, the above equation (4) modeled using viscous friction and Coulomb friction for simplification is shown. On the other hand, for example, there are cases where the friction characteristics differ between forward rotation and reverse rotation of the left drive wheel 7L and the right drive wheel 7R due to the characteristics of the drive system such as a reduction gear. In that case, characteristics obtained by actual measurement or the like may be used.

  In the above equation (4), τL and τR are driving torques τLe and τRe are contact external force torques τLr and τRr are rolling resistance torques.

  The drive torque acquisition unit 25 calculates the drive torque τL and the drive torque τR for driving the left drive wheel 7L and the right drive wheel 7R in the above equation (4), respectively. The drive torque acquisition unit 25 may calculate the drive torque τL and the drive torque τR using a torque sensor attached to the left motor 12L and the right motor 12R and the left drive wheel 7L and the right drive wheel 7R.

  In the above equation (4), τLe and τRe are contact external force torques acting on the left driving wheel 7L and the right driving wheel 7R when the life support robot 3 comes into contact with the external environment.

  The rolling resistance torque acquisition unit 26 instructs the camera 10 to image the road surface 17 on which the camera 10 is currently moving, acquires image information generated by the camera 10, and transmits the image information to the server 5. Thus, the rolling resistance torque τLr and the rolling resistance torque τRr received by the left driving wheel 7L and the right driving wheel 7R from the currently moving road surface 17 are acquired.

  Here, the server 5 will be described with reference to FIGS. 4 and 5. As shown in FIG. 4, the server 5 has a rolling resistance torque DB 32 (storage means). The rolling resistance torque DB 32 is realized by an HDD (Hard Disk Drive), for example. As shown in FIG. 5, the rolling resistance torque DB 32 stores identification information of the road surface 17, material of the road surface 17, rolling resistance torque of the road surface 17, an image of the road surface 17, and a manufacturer of the road surface 17 in association with each other. Accordingly, when the rolling resistance torque acquisition unit 26 of the life support robot 3 transmits the image information of the road surface 17 to the server 5, the server 5 acquires the identification information of the road surface 17 based on the received image information and corresponds to the road surface 17. The rolling resistance torque to be transmitted is transmitted to the life support robot 3, so that the rolling resistance torque acquisition unit 26 acquires the rolling resistance torque of the road surface 17 that is currently moving.

  In addition, when collating the image information of the road surface 17 transmitted to the server 5 by the rolling resistance torque acquisition unit 26 of the life support robot 3 and the image information stored in the rolling resistance torque DB 32, so-called feature values are used. Good. Examples of the feature amount include a color histogram, color statistics, a histogram of an outline, and a Haar-Like feature amount. As a matching algorithm, for example, the Adaboost algorithm can be used. The server 5 calculates the feature amount of the image information received from the life support robot 3 and compares the calculated feature amount with the feature amount of each image information stored in the rolling resistance torque DB 32. As a result of the comparison, the server 5 identifies the image information having the feature quantity closest to the feature quantity of the image information received from the life support robot 3 from the plurality of image information stored in the rolling resistance torque DB 32, and The identification information of the road surface 17 is acquired based on the specified image information, and the rolling resistance torque corresponding to the road surface 17 is transmitted to the life support robot 3.

  Returning to FIG. 2, the contact external force calculation unit 28 calculates the contact external force torque τLe of the left drive wheel 7L and the contact external force torque τRe of the right drive wheel 7R, respectively, using the above equation (4). The hat means that the calculation result is an estimated value.

  Here, the contact external force calculation unit 28 first detects the rotation angular acceleration (second-order differential value of θL) and (second-order differential value of θR) of the left drive wheel 7L and the right drive wheel 7R detected by the rotation angle detection unit 24. ) To calculate the equivalent inertia torque IL · (second-order differential value of θL) and IR · (second-order differential value of θR) of the left driving wheel 7L and the right driving wheel 7R. Then, the contact external force calculation unit 28 obtains the calculated equivalent inertia torque IL · (second-order differential value of θL), IR · (second-order differential value of θR) of the left driving wheel 7L and the right driving wheel 7R, and driving torque acquisition. Based on the driving torque τL and the driving torque τR acquired by the unit 25 and the rolling resistance torque τLr and the rolling resistance torque τRr acquired by the rolling resistance torque acquiring unit 26, the contact external force torque is calculated using the above equation (4). τLe and contact external force torque τRe are respectively calculated. When the acceleration of the life support robot 3 is low, the contact external force calculation unit 28 calculates the equivalent inertia torque IL · (second-order differential value of θL of the left drive wheel 7L and the right drive wheel 7R in the above equation (4). ), IR · (second-order differential value of θR) may be calculated (assuming 0), and the contact external force torque τLe and the contact external force torque τRe may be calculated respectively.

  Further, the following equation (5) for calculating the contact external force fB acting on the center of gravity position of the life support robot 3 can be derived using the above equation (3). However, the hat means an estimated value.

  In the above equation (5), (•) + indicates a pseudo inverse matrix, and for example, the pseudo inverse matrix of the matrix A is AT (AAT) −1.

  Here, in the above equation (5), the y component fBy of the contact external force is always zero. The y component fBy of the contact external force is a force component acting in the lateral direction with respect to the left driving wheel 7L and the right driving wheel 7R, and does not affect the rotational torque of the left driving wheel 7L and the right driving wheel 7R. Become.

  The contact external force calculation unit 28 calculates a contact external force fB that acts on the center of gravity of the life support robot 3 based on the calculated contact external force torque τLe, contact external force torque τRe, and the above equation (5). As described above, the contact external force fB acting on the position of the center of gravity of the life support robot 3 is calculated using only the existing sensor of the life support robot 3 without using a special sensor for measuring the contact external force fB. be able to. Therefore, the cost for the sensor can be reduced.

  Further, since the contact external force fB acting on the center of gravity of the life support robot 3 is calculated in consideration of the rolling resistance torque of the currently moving road surface 17, for example, the boundary between the road surface 17A and the road surface 17B is straddled. At this time, the life support robot 3 is not actually in contact with the external environment, but the life support robot 3 is not erroneously recognized as being in contact with the external environment.

  Next, with reference to FIG. 6, the case where the life support robot 3 exists on an inclined surface will be described. In this case, compared with the case where it exists on a horizontal surface, gravity Mg further downwardly acts on the life support robot 3 as a part of the contact external force. The contact external force calculation unit 28 calculates, for example, the slope direction component (vector component obtained by projecting the gravitational acceleration g on the slope) a of the gravitational acceleration g based on the acceleration information detected by the posture sensor 16. Then, the contact external force calculation unit 28 subtracts the gravity component Ma included in the contact external force fB from the contact external force fB calculated as described above. In this way, not only when the life support robot 3 is present on the horizontal plane but also when it is present on the slope, the net external contact force fB accompanying the contact with the external environment is detected for the contact external force fB. It is possible to calculate without using a special sensor.

  The movement control unit 29 performs impedance control of the life support robot 3 so as to suppress the contact external force fB calculated by the contact external force calculation unit 28, for example. Specifically, the movement control unit 29 calculates the speed target value (first-order differential value of Pd) of the life support robot 3 based on, for example, a virtual impedance model expressed by the following equation (6). In the following equation (6), Md is the virtual mass of the life support robot 3, Dd is the virtual viscous friction of the life support robot 3, Pd is the target position vector of the life support robot 3, and fB is The contact external force vector acting on the life support robot 3 calculated by the contact external force calculation unit 28, and Vc is the speed vector of the virtual conveyor.

  Then, based on the calculated speed target value (first-order differential value of Pd) of the life support robot 3, the movement control unit 29 uses the following equation (7) to set the speed target of the left driving wheel 7L and the right driving wheel 7R. A value (first-order differential value of θWd) is calculated.

  The movement control unit 29 controls the left motor so that the rotational angular velocities of the left driving wheel 7L and the right driving wheel 7R become the calculated speed target values of the left driving wheel 7L and the right driving wheel 7R (first-order differential value of θWd). Feedback control of 12L and right motor 12R is performed. As a result, the life support robot 3 can be controlled with high accuracy so as to mitigate an increase in the contact external force fB applied to the life support robot 3, and high safety can be realized.

  Next, with reference to FIG. 7, the flowchart of the contact external force calculation method in the life support system 1 is demonstrated.

  First, when the movement of the life support robot 3 starts (S100: YES), the rotation angle detector 24 determines the rotation information of the right drive wheel 7R and the left drive wheel based on the count value output from the counter 15. 7L rotation information is calculated and acquired (S110).

  Next, the drive torque acquisition unit 25 acquires the drive torque of the right drive wheel 7R and the left drive wheel 7L based on a command output from the movement control unit 29 to the motor driver 14 (S120).

  Next, the rolling resistance torque acquisition unit 26 instructs the camera 10 to image the road surface 17 on which the camera 10 is currently moving, and acquires image information generated by the camera 10 imaging the road surface 17 ( S130). Then, the rolling resistance torque acquisition unit 26 transmits the image information acquired from the camera 10 to the server 5, thereby receiving and acquiring the rolling resistance torque associated with the image information from the server 5 (S140).

  Next, the contact external force torque calculation unit 27 includes the rotation information acquired by the rotation angle detection unit 24, the drive torque acquired by the drive torque acquisition unit 25, and the rolling resistance torque acquired by the rolling resistance torque acquisition unit 26. Based on this, by solving the equation of rotation of the right driving wheel 7R and the left driving wheel 7L, the contact external force torque acting on the right driving wheel 7R and the left driving wheel 7L when the life support robot 3 comes into contact with the external environment is obtained. Calculate (S150).

  Next, the contact external force calculation unit 28 calculates the contact external force that acts on the life support robot 3 when the life support robot 3 comes into contact with the external environment based on the contact external force torque calculated by the contact external force torque calculation unit 27. (S160).

  Then, the movement control unit 29 performs impedance control on the movement of the life support robot 3 in consideration of the contact external force calculated by the contact external force calculation unit 28 (S170).

  Finally, the movement control unit 29 determines whether or not the life support robot 3 has reached the destination (S180), and if it is determined that the life support robot 3 has not reached the destination (S180: NO), processing is performed. Is returned to S110, and when it is determined that the life support robot 3 has reached the destination (S180: YES), the movement of the life support robot 3 is stopped (S190), and the process is terminated.

  FIG. 8 shows the time change of the rolling resistance torque acting on the left driving wheel 7L and the right driving wheel 7R when the life support robot 3 moves in the direction of the white arrow as shown in FIG. As shown in FIG. 8, when 15 seconds have elapsed from the start of movement, the life support robot 3 straddles the boundary between the floor surface 17A with the flooring and the road surface 17B with the carpet, and the rolling resistance torque is stepped. It is increasing. Similarly, when 30 seconds have elapsed from the start of movement, the life support robot 3 straddles the boundary between the carpet-covered road surface 17B and the tatami-covered road surface 17C, and the rolling resistance torque decreases stepwise. .

  In FIG. 9, the contact external force calculated without considering the difference of the rolling resistance torque according to the road surface 17 currently moving as in Patent Document 1 is shown by a thin solid line, and the currently moving force is moved as in this embodiment. The contact external force calculated in consideration of the difference in rolling resistance torque according to the road surface 17 is shown by a thick solid line. As described above, since the technique disclosed in Patent Document 1 does not consider the difference in rolling resistance torque according to the road surface 17, the life support robot 3 comes into contact with the external environment every time the type of the moving road surface 17 is switched. It is mistakenly recognized. On the other hand, in this embodiment, the contact external force is calculated in consideration of the difference in rolling resistance torque according to the road surface 17 that is currently moving. Therefore, even if the type of the moving road surface 17 is switched, the life support robot As long as 3 is not in contact with the external environment, the contact external force remains zero.

  The preferred embodiment of the present invention has been described above. The above embodiment has the following features.

  The life support system 1 (contact external force calculation system) drives the contact external force acting on the life support robot 3 to the left when the life support robot 3 (moving body) contacts an external environment (for example, another life support robot 3). It is calculated by the equation of motion of rotation of the wheel 7L and the right drive wheel 7R. The life support system 1 includes a rotation angle detection unit 24 (rotation information acquisition unit) that acquires rotation information of the left drive wheel 7L and the right drive wheel 7R, and a drive torque for driving the left drive wheel 7L and the right drive wheel 7R. Drive torque acquisition unit 25 (drive torque acquisition means) that acquires the rolling resistance torque acquisition unit 26 that acquires the rolling resistance torque received by the left drive wheel 7L and the right drive wheel 7R from the currently moving road surface 17 (rolling resistance) Torque acquisition means), rotation information, driving torque, rolling resistance torque, and the left driving wheel 7L and the right driving wheel 7R based on the equation of motion of rotation. Based on the contact external force torque calculation unit 27 (contact external force torque calculation means) that calculates the contact external force torque acting on the drive wheel 7L and the right drive wheel 7R, the life support robot 3 A contact external force calculation unit 28 (contact external force calculation means) that calculates a contact external force acting on the life support robot 3 by contacting the external environment. According to the above configuration, when the life support robot 3 comes into contact with the external environment and the contact external force acting on the life support robot 3 is calculated by the motion equation of rotation of the left drive wheel 7L and the right drive wheel 7R, Since the rolling resistance torque received by the left driving wheel 7L and the right driving wheel 7R from the road surface 17 being taken into consideration is considered, the life support robot 3 is changed when the road surface characteristics change even though the life support robot 3 is not in contact with the external environment. It is possible to solve the problem of erroneously recognizing that an external contact force acts on the life support robot 3 due to contact with the external environment.

  The life support system 1 further includes a rolling resistance torque DB 32 (storage means) that stores the image information of the road surface 17 and the rolling resistance torque in association with each other. The rolling resistance torque acquisition unit 26 refers to the rolling resistance torque DB 32 on the basis of image information obtained by imaging the currently moving road surface 17, so that the left drive wheel 7L and the left driving wheel 7L The rolling resistance torque received by the right drive wheel 7R is acquired. According to the above configuration, the rolling resistance torque corresponding to the road surface 17 can be acquired by acquiring the image information obtained by imaging the currently moving road surface 17.

  In the above embodiment, the server 5 has the rolling resistance torque DB 32 so that the rolling resistance torque can be shared with other life support robots 3. However, instead of this, the life support robot 3 may have the rolling resistance torque DB 32 and may share the rolling resistance torque by mutual communication between the plurality of life support robots 3.

  The contact external force calculation method is a method of calculating the contact external force that acts on the life support robot 3 when the life support robot 3 comes into contact with the external environment using the equation of motion of the left drive wheel 7L and the right drive wheel 7R. The contact external force calculation method includes a step of acquiring rotation information of the left driving wheel 7L and the right driving wheel 7R (S110), and a step of acquiring driving torque for driving the left driving wheel 7L and the right driving wheel 7R (S120). A step (S130-S140) of acquiring rolling resistance torque received by the left driving wheel 7L and the right driving wheel 7R from the currently moving road surface 17, rotation information, driving torque, rolling resistance torque, and left driving A step of calculating a contact external force torque acting on the left driving wheel 7L and the right driving wheel 7R when the life support robot 3 comes into contact with the external environment based on the equation of motion of rotation of the wheel 7L and the right driving wheel 7R (S150). And calculating a contact external force acting on the life support robot 3 when the life support robot 3 comes into contact with the external environment based on the contact external force torque (S160). According to the above method, when the life support robot 3 comes into contact with the external environment and the contact external force acting on the life support robot 3 is calculated by the motion equation of rotation of the left drive wheel 7L and the right drive wheel 7R, Since the rolling resistance torque received by the left driving wheel 7L and the right driving wheel 7R from the road surface 17 being taken into consideration is considered, the life support robot 3 is changed when the road surface characteristics change even though the life support robot 3 is not in contact with the external environment. It is possible to solve the problem of erroneously recognizing that an external contact force acts on the life support robot 3 due to contact with the external environment.

  Next, a rolling resistance torque detecting method for detecting the rolling resistance torque when the rolling resistance torque of the road surface 17 is unknown will be described with reference to FIG.

  First, the life support robot 3 starts moving in a specific environment (S300). The specific environment is an environment in which it is ensured that the life support robot 3 does not come into contact with the external environment and that the road surface 17 on which the life support robot 3 moves is composed of a single type of material. .

  Next, the rotation angle detection unit 24 calculates and acquires the rotation information of the right driving wheel 7R and the rotation information of the left driving wheel 7L based on the count value output from the counter 15 (S310).

  Next, the drive torque acquisition unit 25 acquires the drive torque of the right drive wheel 7R and the left drive wheel 7L based on the command output from the movement control unit 29 to the motor driver 14 (S320).

  Next, the movement control unit 29 stops the movement of the life support robot 3 (S330).

  Next, the rolling resistance torque detection unit 30 issues a command to the camera 10 to image the road surface 17 where the camera 10 is currently located, and acquires image information generated by the camera 10 imaging the road surface 17 ( S340).

  Then, the rolling resistance torque detector 30 calculates the rolling resistance torque τLr and the rolling resistance torque τRr based on the rotation information and the drive torque, and the following equation (8) (S350). The following formula (8) is obtained by deleting the term of the contact external force torque from the formula (4). Since the life support robot 3 is moving in a specific environment, it is guaranteed that the contact external force torque is always zero, so the term of the contact external force torque can be deleted.

  Then, the rolling resistance torque detector 30 uploads the calculated rolling resistance torque τLr and rolling resistance torque τRr to the server 5 together with the image information (S360). The rolling resistance torque τLr and the rolling resistance torque τRr uploaded to the server 5 and the image information are stored in the rolling resistance torque DB 32 in association with each other.

  In this way, the life support robot 3 uploads the detected rolling resistance torque to the server 5 each time, so that the detected rolling resistance torque can be shared with other life support robots 3.

  The above embodiment has the following features.

  The rolling resistance torque detection method detects the rolling resistance torque received by the right driving wheel 7R and the left driving wheel 7L from the road surface 17 on which the life support robot 3 (moving body) is currently moving. The rolling resistance torque detection method includes a step (S300) of moving the life support robot 3 on the road surface 17 under a specific environment (a condition in which the life support robot 3 does not contact the external environment), the right driving wheel 7R, and the left driving wheel. Step (S310) for acquiring 7L rotation information, step (S320) for acquiring drive torque for driving the right drive wheel 7R and the left drive wheel 7L, rotation information, drive torque, and right drive wheel 7R And calculating the rolling resistance torque received by the right driving wheel 7R and the left driving wheel 7L from the currently moving road surface 17 based on the equation of motion of rotation of the left driving wheel 7L (S350). According to the above method, the rolling resistance torque received by the right driving wheel 7R and the left driving wheel 7L from the currently moving road surface 17 can be detected.

  The rolling resistance torque detection method includes the step of generating image information by imaging the currently moving road surface 17 (S340), the step of uploading the image information and the rolling resistance torque in association with the server 5 (S360), including. According to the above method, the rolling resistance torque can be shared with other machines.

  Note that the rolling resistance torque stored in the rolling resistance torque DB 32 shown in FIG. 5 is obtained by moving the life support robot 3 on the road surface 17 under a specific environment (a condition in which the life support robot 3 does not contact the external environment). (S300), the rotation information of the right driving wheel 7R and the left driving wheel 7L is acquired (S310), the driving torque for driving the right driving wheel 7R and the left driving wheel 7L is acquired (S320), the rotation information, This is calculated based on the driving torque and the equation of motion of rotation of the right driving wheel 7R and the left driving wheel 7L (S350).

DESCRIPTION OF SYMBOLS 1 Life support system 2 Service environment 3 Life support robot 4 Network 5 Server 6 Mobile body 7L Left drive wheel 7R Right drive wheel 9 Control part 10 Camera 11 Communication driver 12R Right motor 12L Left motor 13R Right encoder 13L Left encoder 15 Counter 17 Road surface 24 Rotation angle detection unit 25 Driving torque acquisition unit 26 Rolling resistance torque acquisition unit 27 Contact external force torque calculation unit 28 Contact external force calculation unit 29 Movement control unit 32 Rolling resistance torque DB

Claims (4)

A contact external force calculation system that calculates a contact external force acting on a moving body by the moving body coming into contact with an external environment, using a motion equation of rotation of a drive wheel,
Rotation information acquisition means for acquiring rotation information of the drive wheel;
Drive torque acquisition means for acquiring drive torque for driving the drive wheels;
Rolling resistance torque acquisition means for acquiring the rolling resistance torque received by the drive wheels from the road surface that is currently moving;
Based on the rotation information, the drive torque, the rolling resistance torque, and the equation of motion of the rotation of the drive wheel, the contact external force torque acting on the drive wheel when the moving body contacts the external environment is calculated. A contact external force torque calculating means for calculating;
Based on the contact external force torque, contact external force calculation means for calculating a contact external force acting on the mobile body by the mobile body coming into contact with the external environment;
With
Contact external force calculation system. The contact external force calculation system according to claim 1,
Storage means for storing the image information of the road surface and the rolling resistance torque in association with each other;
The rolling resistance torque acquisition means refers to the storage means based on image information obtained by imaging the road surface that is currently moving, so that the driving wheel receives the driving wheel from the road surface that is currently moving. Get the rolling resistance torque,
Contact external force calculation system. The contact external force calculation system according to claim 2,
The rolling resistance torque stored in the storage means moves the moving body on the road surface under the condition that the moving body does not come into contact with the external environment, and the rotation information of the driving wheel during the movement of the moving body. And a driving torque for driving the driving wheel is obtained and calculated based on the rotation information, the driving torque, and an equation of motion of rotation of the driving wheel.
Contact external force calculation system. A contact external force calculation method for calculating a contact external force acting on a moving body by the moving body coming into contact with an external environment by an equation of motion of driving wheel rotation,
Obtaining rotation information of the drive wheels;
Obtaining a driving torque for driving the driving wheel;
Obtaining a rolling resistance torque received by the drive wheels from the road surface that is currently moving;
Based on the rotation information, the drive torque, the rolling resistance torque, and the equation of motion of the rotation of the drive wheel, the contact external force torque acting on the drive wheel when the moving body contacts the external environment is calculated. A calculating step;
Based on the contact external force torque, calculating a contact external force that acts on the moving body when the moving body contacts the external environment;
including,
Contact external force calculation method.

Images