WO2019188865A1 - 身体向推定装置および身体向推定プログラム - Google Patents

身体向推定装置および身体向推定プログラム Download PDF

Info

Publication number
WO2019188865A1
WO2019188865A1 PCT/JP2019/012299 JP2019012299W WO2019188865A1 WO 2019188865 A1 WO2019188865 A1 WO 2019188865A1 JP 2019012299 W JP2019012299 W JP 2019012299W WO 2019188865 A1 WO2019188865 A1 WO 2019188865A1
Authority
WO
WIPO (PCT)
Prior art keywords
quadratic function
user
distance measurement
orientation
measurement data
Prior art date
Application number
PCT/JP2019/012299
Other languages
English (en)
French (fr)
Inventor
豊 渡邊
真規 岡田
和宏 久野
▲高▼橋　正樹
礼応 萬
Original Assignee
株式会社エクォス・リサーチ
学校法人慶應義塾
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 株式会社エクォス・リサーチ, 学校法人慶應義塾 filed Critical 株式会社エクォス・リサーチ
Priority to CN201980004746.0A priority Critical patent/CN111164376A/zh
Priority to US16/652,206 priority patent/US20200242344A1/en
Priority to EP19775154.8A priority patent/EP3779356A4/en
Publication of WO2019188865A1 publication Critical patent/WO2019188865A1/ja

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V40/00—Recognition of biometric, human-related or animal-related patterns in image or video data
    • G06V40/20—Movements or behaviour, e.g. gesture recognition
    • G06V40/23—Recognition of whole body movements, e.g. for sport training
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B11/00—Measuring arrangements characterised by the use of optical techniques
    • G01B11/24—Measuring arrangements characterised by the use of optical techniques for measuring contours or curvatures
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B11/00—Measuring arrangements characterised by the use of optical techniques
    • G01B11/26—Measuring arrangements characterised by the use of optical techniques for measuring angles or tapers; for testing the alignment of axes
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/24—Classification techniques
    • G06F18/241—Classification techniques relating to the classification model, e.g. parametric or non-parametric approaches
    • G06F18/2413—Classification techniques relating to the classification model, e.g. parametric or non-parametric approaches based on distances to training or reference patterns
    • G06F18/24133—Distances to prototypes
    • G06F18/24137—Distances to cluster centroïds
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/50—Depth or shape recovery
    • G06T7/521—Depth or shape recovery from laser ranging, e.g. using interferometry; from the projection of structured light
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V40/00—Recognition of biometric, human-related or animal-related patterns in image or video data
    • G06V40/10—Human or animal bodies, e.g. vehicle occupants or pedestrians; Body parts, e.g. hands
    • G06V40/103—Static body considered as a whole, e.g. static pedestrian or occupant recognition

Definitions

  • the present invention relates to a body orientation estimation apparatus and a body orientation estimation program that can detect the orientation of a subject even in an environment where the positional relationship between the sensor and the subject varies in various ways.
  • the distance to the head 4 is detected by the plurality of distance sensors 11a to 11e provided in the headrest 3 in the horizontal direction, and the approximate curve 5 is based on the head distance.
  • Create A part of the created approximate curve 5 is approximated by an ellipse 6, and the yaw angle of the head 4 is estimated based on the major axis of the ellipse 6.
  • the distance sensor 50 is provided, and the subject is approximated by an ellipse according to the measurement result of the distance sensor 50, and the center of gravity position is calculated. By tracking the change in the center of gravity position, the moving direction of the subject is calculated, and this is estimated as the orientation of the subject.
  • the position detection device of Patent Document 3 also measures a subject by using a plurality of distance sensors 11 to 14, approximates the contour of the subject by an ellipse from the distance measurement point group, and tracks changes in the center of gravity. The movement direction of the subject is calculated, and this is estimated as the orientation of the subject.
  • the present invention has been made to solve the above-described problems, and a body orientation estimation device and body that can detect the orientation of a subject even in an environment where the positional relationship between the sensor and the subject varies in various ways. It aims to provide a direction estimation program.
  • the body orientation estimation device of the present invention includes a distance measuring means for measuring a subject and a secondary that is approximated based on a plurality of distance measurement data measured by the distance measuring means.
  • a quadratic function calculation unit that calculates a function based on a plurality of coordinate systems, and a plurality of quadratic functions calculated by the quadratic function calculation unit, the approximation error between the plurality of distance measurement data is the smallest.
  • a quadratic function selecting unit that selects a quadratic function; and a direction estimating unit that estimates the orientation of the subject based on the quadratic function selected by the quadratic function selecting unit.
  • the body orientation estimation program of the present invention is approximated based on a distance measurement acquisition function for acquiring distance measurement data obtained by measuring a target person and a plurality of distance measurement data acquired by the distance measurement acquisition function.
  • a quadratic function calculation function for calculating a quadratic function based on a plurality of coordinate systems, and among the plurality of quadratic functions calculated by the quadratic function calculation function, the approximation error between the plurality of distance measurement data is the largest.
  • a computer implements a quadratic function selection function for selecting a small quadratic function and a direction estimation function for estimating the orientation of the subject based on the quadratic function selected by the quadratic function selection function. is there.
  • quadratic functions approximated based on a plurality of distance measurement data obtained by measuring the subject are calculated based on a plurality of coordinate systems, respectively.
  • a quadratic function having the smallest approximation error with the plurality of distance measurement data is selected from the plurality of calculated quadratic functions, and the direction of the subject is estimated based on the selected quadratic function. That is, when the quadratic function calculated based on one coordinate system cannot be matched well with the subject, the orientation of the subject is estimated based on the quadratic function calculated based on another coordinate system. Therefore, there is an effect that the orientation of the subject can be detected even in an environment where the positional relationship between the distance measuring means (sensor) and the subject changes variously.
  • A is a figure which shows the case where the approximate ellipse from which size differs is calculated by the instantaneous variation of ranging data
  • (b) is the shoulder width and thickness of a user's body, and the long axis of an approximate ellipse It is a figure which shows a short axis.
  • (A) is a figure which shows calculation of two types of quadratic functions
  • (b) is a figure which shows the case where the vertex of a quadratic function does not exist between the ranging data of both ends.
  • (A) is a figure which shows the case where the vertex of a quadratic function exists between cluster edges, and one cluster edge is outside the vertex of a quadratic function, and is separated from a vertex
  • (b) is a cluster edge It is a figure which shows the case where the vertex of a quadratic function exists in between and one cluster edge is outside the vertex of a quadratic function, and is close to the vertex T.
  • FIG. 1 is an external view of the moving body 1.
  • the moving body 1 functions as a device that can move to an appropriate position with respect to the user (subject) H in front of the user H and follow the user H.
  • the moving body 1 includes a substantially columnar exterior 2, a control unit 10 that is disposed inside the exterior 2 and controls each part of the moving body 1, a distance measuring sensor 16, a wheel 17.
  • the distance measuring sensor 16 is an apparatus that is disposed on the upper part of the exterior 2 and detects the distance between the distance measuring sensor 16 and an object (ranging) by irradiating laser light in all directions (360 degrees). .
  • the distance measuring sensor 16 transmits the distance to the object detected for each arbitrary angle to the control unit 10 in association with the angle.
  • the distance measuring sensor 16 is configured to be movable in the vertical direction, and the vertical position of the distance measuring sensor 16 is appropriately set so that the laser light from the distance measuring sensor 16 is irradiated around the shoulder of the user H in advance.
  • distance data the distance and angle detected by the distance measuring sensor 16 are referred to as “distance data”.
  • the wheel 17 is a wheel provided at each of the left and right ends of the lower part of the exterior 2.
  • Motors (not shown) are connected to the left and right wheels 17, and the moving body 1 is moved by driving the motors based on control signals from a drive unit 18 (see FIG. 2) described later.
  • the left and right motors are rotated forward and backward with the same output to move the moving body 1 forward and backward, and the motors are differentiated to change the moving direction of the moving body 1. That is, the moving body 1 can move in the front-rear direction, which is an orthogonal direction in which the wheels 17 are disposed, but cannot move directly in the left-right direction, so the moving body 1 has a nonholonomic constraint condition. It moves by the wheel 17 and the drive part 18 which have.
  • FIG. 2 is a block diagram showing an electrical configuration of the moving body 1.
  • the moving body 1 has a control unit 10, and the control unit 10 has a CPU 11, a flash ROM 12, and a RAM 13, which are connected to an input / output port 15 via a bus line 14.
  • a distance measuring sensor 16 and a drive unit 18 are further connected to the input / output port 15.
  • the CPU 11 is an arithmetic unit that controls each unit connected by the bus line 14.
  • the flash ROM 12 is a rewritable nonvolatile storage device that stores programs executed by the CPU 11, fixed value data, and the like, and stores a control program 12a. When the control program 12a is executed by the CPU 11, the main process of FIG. 3 is executed.
  • the RAM 13 is a memory for the CPU 11 to store various work data, flags, and the like in a rewritable manner when the control program 12a is executed, and distance measurement data in which distance measurement data MP measured by the distance sensor 16 is stored.
  • KF predicted position direction memory 13d Kalman filter predicted position direction memory 13d
  • a current position memory 13f and a previous value memory 13g are provided.
  • the ellipse position / direction memory 13b is a memory for storing the position and orientation of the user H estimated from the approximate ellipse C (see FIG. 4) approximated from the distance measurement data MP of the distance measurement data memory 13a.
  • the quadratic function position / direction memory 13c is a memory for storing the position and orientation of the user H estimated from the quadratic function Qxy or Qyx (see FIG. 6) approximated from the distance measurement data MP of the distance measurement data memory 13a. .
  • the KF predicted position / direction memory 13d is a memory that stores the current position and orientation of the user H estimated based on a Kalman filter prediction step (Formula 9) from a value in a previous value memory 13g described later.
  • the correlation result position direction memory 13e is the position and direction of the user H that is most likely to be the user H among the positions and directions of the plurality of users H stored in the elliptical position direction memory 13b and the quadratic function position direction memory 13c. Is a memory in which is stored.
  • the estimated current position / direction memory 13f is a memory that stores the position and orientation of the user H estimated by the Kalman filter based on the value of the correlation result position / direction memory 13e.
  • the previous value memory 13g is the previous estimated current position / direction memory. It is a memory in which the value of the memory 13f and the estimated speed and angular velocity of the user H are stored.
  • the drive unit 18 is a device for moving the moving body 1 and includes a wheel 17 (see FIG. 1), a motor (not shown) serving as a drive source of the wheel 17, and the like.
  • a control signal is input from the control unit 10 to the drive unit 18, the motor rotates based on the input control signal, and the wheel 17 is driven by the rotation of the motor to drive the moving body 1. .
  • FIG. 3 is a flowchart of the main process of the moving body 1.
  • the main process is executed immediately after the mobile unit 1 is turned on.
  • the distance measurement data MP acquired from the distance sensor 16 is stored in the distance measurement data memory 13a (S1).
  • the approximate ellipse C of the user H is calculated based on the distance measurement data MP stored in the distance measurement data memory 13a (S2).
  • the calculation of the approximate ellipse C of the user H based on the distance measurement data MP will be described with reference to FIG.
  • FIG. 4 is a diagram illustrating calculation of the approximate ellipse C of the user H.
  • the distance measurement data MP of the user H acquired from the distance measurement sensor 16 is represented by a white square.
  • One approximate ellipse C is calculated by the least square method or the like based on the distance measurement data MP.
  • the center CP of the approximate ellipse C is the “user H position”
  • the short axis directions DC1 and DC2 of the approximate ellipse C are the “user H direction”.
  • the two directions DC1 and DC2 are calculated as the directions of the user H because the direction of the user H (front or back) cannot be distinguished from the approximate ellipse C. Note that which of the orientations DC1 and DC2 is to be the final orientation of the user H is determined by the process of S7 described later.
  • the position and orientation of the user H in the approximate ellipse C are stored in the ellipse position direction memory 13b (S3).
  • the calculated approximate ellipse C fits the body size of the user H due to instantaneous variation of the distance measurement data MP, fluctuation due to the distance measurement error of the distance measurement data MP, or the like. It may vary, such as those that are far from the size of the body. Therefore, in the process of S3, it is determined whether or not the calculated approximate ellipse C is close to the size of the user H's body. Is calculated. The selection of the approximate ellipse C in the process of S3 will be described with reference to FIG.
  • FIG. 5A is a diagram illustrating a case where approximate ellipses C having different sizes are calculated based on instantaneous variation in the distance measurement data MP.
  • FIG. 5B illustrates the shoulder width W1 of the body of the user H and It is a figure which shows thickness W2, and the major axis a and the minor axis b of the approximate ellipse C.
  • the approximate ellipse C of 1 is calculated from the distance measurement data MP by the process of S2 in FIG. 3, but due to instantaneous variations in the distance measurement data MP, as shown in FIG.
  • an approximate ellipse C1 close to the size is calculated and a case where an approximate ellipse C2 having a size different from that of the approximate ellipse C1 is calculated.
  • the approximate ellipse C2 is approximated by a larger circumference than the outer circumference of the body around the shoulder of the user H. With such an approximate ellipse C2, the position and orientation of the user H cannot be accurately calculated.
  • the shoulder width W1 is exemplified as the shoulder width W1
  • “0.3 m” is exemplified as the thickness W2. That is, twice the long axis a of the approximate ellipse C is within ⁇ 10% of the shoulder width W1 of the user H, and twice the short axis b is within ⁇ 10% of the thickness W2 of the user H.
  • the approximate ellipse C is determined to be close to the size of the user H. In this way, when it is determined that the approximate ellipse C is close to the body size of the user H, the center CP and the orientations DC1 and DC2 of the approximate ellipse C are stored in the ellipse position / direction memory 13b.
  • FIG. 6A is a diagram schematically showing calculation of two types of quadratic functions Qxy and Qyx. As shown in FIG. 6A, based on the distance measurement data MP, a quadratic function Qxy approximated by a quadratic function in the xy coordinate system, and a quadratic function Qxy approximated by a quadratic function in the yz coordinate system Is calculated. The quadratic function Qxy is calculated based on Formula 2 below, and the quadratic function Qyx is calculated based on Formula 3.
  • a 1 , b 1 , and c 1 in Equation 2 are coefficients, and these coefficients are determined based on the least square method of Equation 2 and distance measurement data MP.
  • a 2 , b 2 , and c 2 in Expression 3 are also coefficients, and these coefficients are determined based on the least square method of Expression 3 and distance measurement data MP. That is, the shape of the quadratic function Qxy according to the present embodiment is upright along the x-axis, and the axis of the quadratic function Qyx is parallel to the shape of upright along the y-axis.
  • the distance sensor 16 irradiates the laser beam in all directions to acquire the distance to the user H, a part of the body of the user H is acquired as the distance measurement data MP of the user H. . That is, the distribution of the distance measurement data MP acquired from the distance sensor 16 is biased.
  • the quadratic function Qxy based on the xy coordinate system has an upward (or downward) convex shape, and the quadratic function Qyx based on the xy coordinate system protrudes to the left (or right). It is the shape.
  • the distribution of the distance measurement data MP matches the distribution of the distance measurement data MP suitable for the quadratic function Qxy or the distance measurement suitable for the quadratic function Qyx. There may be a distribution of data MP. Therefore, in the present embodiment, by calculating two quadratic functions Qxy and Qyx having different shapes from the distance measurement data MP, candidates for the quadratic function Q that can be more adapted to the distribution of the distance measurement data MP can be listed.
  • the one having the smallest approximation error with the value of the distance measurement data memory 13a is selected from the quadratic functions Qxy and Qyx, and the position and orientation of the quadratic function Qxy or Qyx are determined as the quadratic function position direction.
  • the memory 13c (S5) Save in the memory 13c (S5).
  • an approximation error with respect to the distance measurement data MP stored in the distance measurement data memory 13a is calculated for each of the quadratic functions Qxy and Qyx calculated in the process of S4.
  • the one where the approximation error of these quadratic functions Qxy and Qyx is smaller is selected as the quadratic function Q for calculating the position and orientation of the user H.
  • the position and orientation of the user H are calculated from the selected quadratic function Q.
  • the position and orientation of the user H are calculated according to the positional relationship between the vertex T of the quadratic function Q and the distance measurement data MP.
  • a method for calculating the position and orientation of the user H in the quadratic function Q will be described with reference to FIGS.
  • FIG. 6B is a diagram showing a case where the vertex T of the quadratic function Q does not exist between the distance measurement data MP at both ends.
  • ranging data corresponding to one end side of the quadratic function Q in the ranging data MP is referred to as “cluster edge E1”, and the other end side of the quadratic function Q.
  • the distance measurement data MP corresponding to is referred to as “cluster edge E2”.
  • the vertex T of the quadratic function Q is located outside the cluster edge E1 and the cluster edge E2. That is, the distance measurement data MP determines that a part of the body of the user H, specifically, the front surface (chest side) or the back surface (back side) of the body could be measured, but the distance could not be measured well on both shoulders. Is done.
  • the vertex T of the quadratic function Q is located outside the cluster edge E1 and the cluster edge E2
  • the user H is based on the cluster edge E1 and the cluster edge E2 that can be reliably measured. Is calculated.
  • the midpoint Ec between the cluster edge E1 and the cluster edge E2 is set as the position of the user H
  • the directions DQ1 and DQ2 orthogonal to the straight line connecting the cluster edge E1 and the cluster edge E2 are set as the direction of the user H.
  • the coordinate of the cluster edge E1 in the x-axis direction is X E1
  • the coordinate in the y-axis direction is Y E1
  • the coordinate of the cluster edge E2 in the x-axis direction is X E2
  • the coordinate in the y-axis direction is Y E2 .
  • the user H is based on the cluster edge E1 and the cluster edge E2 that are reliably measured.
  • the error of the position and orientation of the user H can be minimized.
  • the two directions DQ1 and DQ2 are calculated as the orientation of the user H, as in the above-described approximate ellipse C, which direction the user H is facing (front or back) from the quadratic function Q. It is not possible. Note that which of the orientations DQ1 and DQ2 is to be the final orientation of the user H is determined by the process of S7 described later.
  • the vertex T of the quadratic function Q exists between the cluster edge E1 and the cluster edge E2
  • the cluster edge E1 and the cluster edge E2 are further away from the vertex T (FIG. 7A).
  • the method for calculating the position and orientation of the user H differs between the case where either one of the cluster edge E1 or the cluster edge E2 is close to the vertex T (FIG. 7B).
  • FIG. 7A shows a case where the vertex T of the quadratic function Q exists between the cluster edges E1 and E2, and the cluster edge E2 is outside the vertex T of the quadratic function Q and away from the vertex T.
  • FIG. 7B shows a case where the vertex T of the quadratic function Q exists between the cluster edges E1 and E2, and the cluster edge E2 is outside the vertex T of the quadratic function Q and close to the vertex T. is there.
  • the cluster edge E1 is sufficiently separated from the vertex T and the cluster edge E2 is separated from the vertex T and a case where the cluster edge E2 is approaching the vertex T will be described. Needless to say, the same applies to the case where the cluster edge E1 is far from the vertex T or close to the vertex T.
  • the distance relationship between the cluster edge E2 and the vertex T is determined. Specifically, based on the distance between the midpoint Ec and the vertex T and the distance between the midpoint Ec and the cluster edge E2 shown in FIGS. 7A and 7B, the cluster edge E2 and the vertex T are shown. Is determined.
  • the distance in the x-axis direction between the middle point Ec and the vertex T is ⁇ X T
  • the distance in the y-axis direction is ⁇ Y T
  • the distance in the x-axis direction between the middle point Ec and the cluster edge E2 is ⁇ X E
  • y-axis When the following formula 6 is satisfied where ⁇ Y E is the distance in the direction, it is determined that the cluster edge E2 and the vertex T are separated.
  • the cluster edge E2 and the vertex T are separated from each other. To be judged.
  • the position and orientation of the user H are calculated based on the positions of both shoulders and the estimated cluster edges E1 and E2.
  • the middle point Ec is set as the position of the user H
  • the directions DQ1 and DQ2 orthogonal to the straight line connecting the cluster edge E1 and the cluster edge E2 are set as the direction of the user H. That is, the direction of the user H is calculated by the above-described formula 4 and formula 5.
  • the cluster edge E2 exists outside the vertex T and at a position close to the vertex T.
  • the position near the vertex T is estimated as the position near the left shoulder or right shoulder of the user H.
  • the cluster edge E2 since the cluster edge E2 is adjacent to the outside of the vertex T, the cluster edge E2 is located on the “back side” of the vertex T when viewed from the distance measuring sensor 16 (moving body 1). That is, the vertex T becomes an obstacle (disturbance) of the laser beam from the distance measuring sensor 16, does not reach the cluster edge E2, and the cluster edge E2 cannot measure the distance. As a result, there is a possibility that the cluster edge E2 cannot be accurately measured because the entire left shoulder or right shoulder cannot be measured.
  • the direction DQ1 of the user H is based on the vertex T located in the range in which the distance measurement data MP is accurately acquired, that is, the left shoulder or the right shoulder.
  • DQ2 is calculated. Specifically, the x-axis direction of the coordinates of the midpoint Ec X C, and the coordinates Y C in the y-axis direction, x-axis direction of the coordinates X T vertex T, when the y-axis direction of the coordinate and Y T,
  • the direction DQ1 of the user H is calculated by the following formula 7, and the direction DQ2 is calculated by the formula 8.
  • the directions DQ1 and DQ2 are calculated from the angle of a straight line orthogonal to the straight line connecting the middle point Ec and the vertex T.
  • the distance measurement data MP is accurately located, but the distance T is actually measured by the distance sensor 16 and the vertex T which is the left shoulder or right shoulder position on the approximated quadratic function Q.
  • the directions DQ1 and DQ2 are calculated in consideration of both the cluster edge E2 that may not have been accurately measured due to the influence of the vicinity of the vertex T, errors in the directions DQ1 and DQ2 can be suppressed.
  • the directions DQ1 and DQ2 calculated as described above are stored in the quadratic function position direction memory 13c with the direction of the user H and the midpoint Ec as the position of the user, respectively.
  • the current position and direction of the user H are predicted from the value in the previous value memory 13g based on the Kalman filter prediction step, and stored in the KF predicted position direction memory 13d (S6).
  • the previous value memory 13g stores the final current position, orientation, speed, and angular velocity of the user H estimated in the previous processing of S9 or S11 described later. When such a previous state continues, the predicted position and orientation of the user H are calculated.
  • the position in the x-axis direction of the user H stored in the previous value memory 13g is P x, k ⁇ 1
  • the position in the y-axis direction is P y, k ⁇ 1
  • the direction of the user H is P ⁇ , k ⁇ .
  • the velocity of the user H in the x-axis direction is V x, k ⁇ 1
  • the velocity in the y-axis direction is V y, k ⁇ 1
  • the angular velocity of the user H is V ⁇ , k ⁇ 1
  • the previous S9 or S11 Assuming that the elapsed time from the process is ⁇ t and the system noise components of the user H due to acceleration disturbance or the like are n xs , n ys , n ⁇ s , the predicted position P xp, k , y axis direction of the user H in the x axis direction is predicted.
  • Equation 9 is a Kalman filter prediction step.
  • the position P xp, k in the x axis direction of the user H predicted by Equation 9 above, the position P yp, k in the y axis direction, the direction P ⁇ p, k of the user H, and the velocity V xp in the x axis direction of the user H , k , the velocity V yp, k in the y-axis direction, and the angular velocity V ⁇ p, k of the user H are stored in the KF predicted position direction memory 13d.
  • correlation processing is performed based on the position and orientation of the user H stored in the elliptical position direction memory 13b, the quadratic function position direction memory 13c, and the KF predicted position direction memory 13d, and the corresponding user H position and The direction is stored in the correlation result position direction memory 13e (S7).
  • the position and orientation of the user H stored in the ellipse position / direction memory 13b and the quadratic function position / direction memory 13c are determined based on the previous values stored in the KF predicted position / direction memory 13d. ”To narrow down.
  • the position and orientation of the user H in the ellipse position direction memory 13b and the quadratic function position direction memory 13c which are greatly different from the position and orientation of the user H based on the previous value, are excluded.
  • the correlation result position memory 13e indicates that the position and orientation of the user H has not been selected. The indicated value is stored. Since the gate and nearest neighbor method is a known technique, a detailed description thereof will be omitted.
  • the position and orientation of the user H estimated to be most likely to be the user H among the positions and orientations of the user H calculated from the approximate ellipse C and the quadratic function Q are selected by the narrowing by the gate and the nearest neighbor method.
  • the position in the x-axis direction of the user H stored in the correlation result position direction memory 13e is P xc
  • the position in the y-axis direction is P yc
  • the direction of the user H is P ⁇ c
  • the direction of the user H in the x-axis direction V xc is V yc
  • the velocity in the y-axis direction is V yc
  • the angular velocity of the user H is V ⁇ c
  • the detection error components by the detection error of the distance measuring sensor 16 the likelihood function due to the movement of the user H, and the like are expressed as n xp , n Assuming that yp and n ⁇ p , the position P x, k in the x-axis direction of the user H , the position P y, k in the y-axis direction, and the direction P ⁇ , k of the user H are calculated by Equation 10.
  • the velocity V xc of the user H in the x-axis direction is the position of the user H in the x-axis direction stored in the previous value memory 13g and the position of the user H in the x-axis direction stored in the correlation result position memory 13e.
  • the velocity V yc of the user H in the y-axis direction is based on the position of the user H in the y-axis direction of the previous value memory 13g and the position of the correlation result position direction memory 13e in the y-axis direction.
  • the angular velocity V ⁇ c of the user H is calculated based on the orientation of the user H in the previous value memory 13g and the orientation of the user H in the correlation result position orientation memory 13e.
  • the observation error component that is, the detection error of the distance measuring sensor 16 and the user H's
  • the observation error component are stored in the position and orientation of the user H estimated to be the most likely user H stored in the correlation result position / direction memory 13e in the process of S7. Since errors that can occur in reality, such as movement, are included, it is possible to estimate the position and orientation of the user H with a small error from the actually observed position and orientation of the user H.
  • the position and orientation of the user H are not stored in the correlation result position / direction memory 13e (S8: No), that is, in the processing of S7, the position and orientation of the user H are selected in the correlation result position / direction memory 13e. If a value indicating that the change has not been made is stored, the value of the KF predicted position direction memory 13d is stored in the estimated current position direction memory 13f (S10). That is, in the process of S7, even if it is determined that none of the positions and orientations of the user H calculated in the processes of S2 to S5 are the user H, it is a predicted value, but the value in the previous value memory 13g. The current position and orientation of the current user H are stored in the estimated current position direction memory 13f. Thereby, the situation where the position and direction of the user H are not updated can be suppressed.
  • a control signal based on the position and orientation of the user H in the estimated current position direction memory 13f is output to the drive unit 18 (S11), and the drive unit 18 is operated. Thereby, the moving body 1 is moved based on the estimated position and orientation of the user H.
  • the position and orientation of the user H and the speed and angular velocity of the user H stored in the estimated current position / direction memory 13f are stored in the previous value memory 13g (S12). Specifically, first, the speed and angular velocity of the user H are calculated from the position and orientation of the user H in the estimated current position / direction memory 13f and the position and orientation of the user H stored in the previous value memory 13g. The speed and angular velocity of the user H are stored in the previous value memory 13g, and then stored in the position and orientation of the user H in the estimated current position / direction memory 13f. And after the process of S12, the process below S1 is repeated.
  • the moving body 1 of the present embodiment has the approximate ellipse C and the quadratic functions Qxy and Qyx based on the xy and yx axes based on the distance measurement data MP measured by the distance sensor 16. And the position and orientation of each user H are calculated. From the calculated positions and orientations of the plurality of users H, the position and orientation of the user H estimated to be most likely to be the user H is selected by the gate and nearest neighbor method, and the current user is determined from the positions and orientations of the users H. The position and orientation of H are estimated.
  • the positional relationship between the moving body 1 and the user H changes variously every moment. Therefore, a plurality of shapes such as the approximate ellipse C, quadratic functions Qxy, and Qyx and approximate curves based on a plurality of coordinate systems are calculated from the distance measurement data MP, and an approximate curve based on one shape or coordinate system works well for the user H. If not, the position and orientation of the user H are estimated based on an approximate curve based on another shape or coordinate system that is more suitable. Thereby, even if the positional relationship between the distance measuring sensor 16 and the user H changes variously, the position and orientation of the user H can be detected.
  • a plurality of shapes such as the approximate ellipse C, quadratic functions Qxy, and Qyx and approximate curves based on a plurality of coordinate systems are calculated from the distance measurement data MP, and an approximate curve based on one shape or coordinate system works well for the user H. If not, the position and orientation of the user H are estimated based on an approximate curve based
  • the quadratic function Q calculated from the distance measurement data MP has two types of functions, that is, the quadratic function Qxy based on the xy coordinate system and the quadratic function Qyx based on the yx coordinate system.
  • the present invention is not necessarily limited to this, and a quadratic function based on an xz coordinate system or a yz coordinate system may be calculated, or a quadratic function Q based on another coordinate system may be calculated.
  • the distance measurement data MP is approximated by a quadratic function.
  • the present invention is not necessarily limited to this, and a configuration in which approximation is performed by a polynomial function having a degree of 3 or more, such as a cubic function or a quartic function, may be employed.
  • the quadratic function Qxy has a shape standing upright along the x-axis, and the axis of the quadratic function Qyx stands upright along the y-axis.
  • the present invention is not necessarily limited thereto, and the quadratic functions Qxy and Qyx may be configured to be inclined toward the x axis or the y axis.
  • the distance measurement sensor 16 measures the distance of the body around the shoulder.
  • the present invention is not limited to this, and any other part may be used as long as the body of the user H is to be measured, such as the abdominal area and the waist area.
  • the position and orientation of the user H are estimated based on the approximate ellipse C and the quadratic function Q.
  • the present invention is not necessarily limited to this, and the position and orientation of the user H may be estimated based only on the quadratic function Q, or the position and orientation of the user H may be estimated based only on the approximate ellipse C. You may do it.
  • the center CP and the orientations DC1 and DC2 of the approximate ellipse C are stored in the ellipse position direction memory 13b. It was set as the structure to do. However, the present invention is not necessarily limited to this.
  • the center CP and the directions DC1 and DC2 of the approximate ellipse C are stored in the ellipse position direction memory 13b.
  • the center CP and the orientations DC1, DC2 of the approximate ellipse C are stored in the ellipse position direction memory 13b. It is also good.
  • the cluster edges E1 and E2 in the quadratic function Q are the distance measurement data MP at both ends of the distance measurement data MP.
  • the present invention is not necessarily limited to this, and a point on the quadratic function Q that approximates the distance measurement data MP at one end of the distance measurement data MP is defined as the cluster edge E1, and the other end.
  • a point that approximates the distance measurement data MP may be the cluster edge E2.
  • the position of the user H in the approximate ellipse C is the center CP of the approximate ellipse C
  • the position of the user H in the quadratic functions Qxy and Qyx is the midpoint Ec of the cluster edges E1 and E2.
  • the present invention is not necessarily limited to this, and the position of the user H in the approximate ellipse C and the quadratic function Q may be either the center CP of the approximate ellipse C or the midpoint Ec of the cluster edges E1 and E2.
  • the position of the user H calculated by another method such as a least square method based on the distance measurement data MP may be used.
  • the straight line orthogonal to the straight line connecting the midpoint Ec and the vertex T is calculated from the angle.
  • the present invention is not necessarily limited to this, and in such a case, as in the case where the cluster edge E2 and the vertex T are separated from each other, the direction is from the angle of the straight line orthogonal to the straight line connecting the cluster edge E1 and the cluster edge E2. It is good also as a structure which calculates DQ1, DQ2.
  • Control unit Body orientation estimation device
  • Control program body orientation estimation program
  • H user target person
  • Distance sensor ranging means, distance acquisition function
  • MP Distance measurement data Q, Qxy, Qyx Quadratic functions C, C1, C2 Approximate ellipse E1, E2 Cluster edge
  • S2 Ellipse calculation means S3 Direction estimation means, direction estimation function S4 Secondary function calculation means, quadratic function calculation function S5 2 Secondary function selection means, secondary function selection function, Orientation estimation means, orientation estimation function First orientation estimation means, second orientation estimation means S7 to S10 Orientation estimation means, orientation estimation function T vertex

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Data Mining & Analysis (AREA)
  • Human Computer Interaction (AREA)
  • Multimedia (AREA)
  • Mathematical Physics (AREA)
  • Computational Mathematics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • Mathematical Analysis (AREA)
  • General Engineering & Computer Science (AREA)
  • Social Psychology (AREA)
  • Psychiatry (AREA)
  • General Health & Medical Sciences (AREA)
  • Health & Medical Sciences (AREA)
  • Operations Research (AREA)
  • Algebra (AREA)
  • Software Systems (AREA)
  • Databases & Information Systems (AREA)
  • Artificial Intelligence (AREA)
  • Evolutionary Computation (AREA)
  • Electromagnetism (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Optics & Photonics (AREA)
  • Remote Sensing (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Evolutionary Biology (AREA)
  • Length Measuring Devices By Optical Means (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

【課題】センサと対象者との位置関係が様々に変化する環境下においても、対象者の向きを検出できる身体向推定装置および身体向推定プログラムを提供すること。 【解決手段】移動体１は、測距センサ１６で測距された測距データＭＰから、近似楕円Ｃと、ｘｙ軸，ｙｘ軸による二次関数Ｑｘｙ，Ｑｙｘとが算出される。ユーザＨの肩幅Ｗ１及び厚みＷ２とに近い近似楕円Ｃが選択され、測距データＭＰとの近似誤差が最も小さい二次関数Ｑｘｙ，Ｑｙｘが選択される。そして、選択された近似曲線からユーザＨの位置および向きが推定される。即ち、１の形状または座標系に基づいた近似曲線がユーザＨに適合しない場合は、他の形状または座標系に基づいた近似曲線からユーザＨの位置および向きが推定される。よって、測距センサ１６とユーザＨとの位置関係が様々に変化してもユーザＨの位置および向きを検出できる。

Description

身体向推定装置および身体向推定プログラム
　本発明は、センサと対象者との位置関係が様々に変化する環境下においても、対象者の向きを検出できる身体向推定装置および身体向推定プログラムに関するものである。
　特許文献１の顔向き検知装置によれば、ヘッドレスト３に水平方向に設けられた複数の距離センサ１１ａ～１１ｅによって頭部４までの距離をそれぞれ検知し、この頭部距離に基づいて近似曲線５を作成する。作成された近似曲線５の一部を楕円６により近似し、その楕円６の長軸に基づいて頭部４のヨー角を推定している。
　また特許文献２の人体向き推定装置によれば、距離センサ５０を備え、距離センサ５０の測定結果に応じて対象者を楕円で近似し、その重心位置を算出する。この重心位置の変化をトラッキングして、対象者の移動方向を算出し、これを対象者の向きと推定している。同様に特許文献３の位置検出装置も、複数の距離センサ１１～１４により対象者を測定し、その測距点群から対象者の輪郭を楕円で近似し、その重心位置の変化をトラッキングして、対象者の移動方向を算出し、これを対象者の向きと推定している。
特開２０１６－１７５５０９号公報 特開２０１１－０８１６３４号公報 特開２０１０－１６９５２１号公報
　特許文献１のように、ヘッドレスト３に寄りかかる頭部４の向きであれば、各センサ１１ａ～１１ｅと頭部４との位置関係は大凡決まっている。よって、頭部４までの距離に基づいて算出された楕円により頭部４のヨー角を推定することは可能である。
　しかしながら、特許文献２，３のように、センサと対象者（身体）との位置関係が様々に変化する環境下においては、センサと対象者との位置関係によっては、測距データの取れ方が変わり、近似曲線を対象者に上手く適合できない場合があった。かかる場合には、対象者の向きを検出できなかった。
　本発明は、上述した問題点を解決するためになされたものであり、センサと対象者との位置関係が様々に変化する環境下においても、対象者の向きを検出できる身体向推定装置および身体向推定プログラムを提供することを目的としている。
　この目的を達成するために本発明の身体向推定装置は、対象者を測距する測距手段と、その測距手段により測距された複数の測距データを基にして近似される二次関数を、複数の座標系に基づいてそれぞれ算出する二次関数算出手段と、その二次関数算出手段により算出された複数の二次関数うち、複数の前記測距データとの近似誤差が最も小さい二次関数を選択する二次関数選択手段と、その二次関数選択手段により選択された二次関数に基づいて前記対象者の向きを推定する向き推定手段と、を備えている。
　また本発明の身体向推定プログラムは、対象者を測距した測距データを取得する測距取得機能と、その測距取得機能により取得された複数の測距データを基にして近似される二次関数を、複数の座標系に基づいてそれぞれ算出する二次関数算出機能と、その二次関数算出機能により算出された複数の二次関数うち、複数の前記測距データとの近似誤差が最も小さい二次関数を選択する二次関数選択機能と、その二次関数選択機能により選択された二次関数に基づいて前記対象者の向きを推定する向き推定機能と、をコンピュータに実現させるものである。
　本発明の身体向推定装置および身体向推定プログラムによれば、対象者を測距した複数の測距データを基にして近似される二次関数を、複数の座標系に基づいてそれぞれ算出する。この算出された複数の二次関数うち、複数の測距データとの近似誤差が最も小さい二次関数を選択し、その選択された二次関数に基づいて、対象者の向きを推定する。即ち、１の座標系に基づいて算出された二次関数が対象者に上手く適合できない場合には、他の座標系に基づいて算出された二次関数に基づいて対象者の向きを推定する。よって、測距手段（センサ）と対象者との位置関係が様々に変化する環境下においても、対象者の向きを検出できるという効果がある。
移動体の外観図である。 移動体の電気的構成を示すブロック図である。 移動体のフローチャートである。 ユーザの近似楕円の算出を示す図である。 （ａ）は、測距データの瞬間的なバラツキによって、サイズの異なる近似楕円が算出される場合を示す図であり、（ｂ）は、ユーザの身体の肩幅および厚さと、近似楕円の長軸および短軸とを示す図である。 （ａ）は、２種類の二次関数の算出を示す図であり、（ｂ）は、両端の測距データ間に二次関数の頂点が存在しない場合を示す図である。 （ａ）は、クラスタエッジ間に二次関数の頂点が存在し、１のクラスタエッジが二次関数の頂点より外側かつ頂点から離れている場合を示す図であり、（ｂ）は、クラスタエッジ間に二次関数の頂点が存在し、１のクラスタエッジが二次関数の頂点より外側かつ頂点Ｔに近い場合を示す図である。
　以下、本発明の好ましい実施形態について、添付図面を参照して説明する。まず、図１を参照して、本実施形態における移動体１の構成を説明する。図１は、移動体１の外観図である。移動体１は、ユーザＨの前方にて、ユーザ（対象者）Ｈに対し適切な位置に移動して、ユーザＨに随行できる装置として機能する。
　図１に示す通り、移動体１は、主に略円柱状の外装２と、その外装２の内部に配設され移動体１の各部を制御する制御部１０と、測距センサ１６と、車輪１７とを有する。測距センサ１６は、外装２の上部に配置され、レーザ光を全方位（３６０度）に対して照射することで、測距センサ１６と物体との距離を検知（測距）する装置である。測距センサ１６は、任意の角度毎に検出された対象物との距離を、その角度に対応付けて制御部１０へ送信する。また、測距センサ１６は上下方向に移動可能に構成され、予め測距センサ１６からのレーザ光がユーザＨの肩周辺に照射されるよう、測距センサ１６の上下方向の位置が適宜設定される。以下、測距センサ１６から検知される距離および角度のことを「測距データ」と称す。
　車輪１７は、外装２の下部における左右両端にそれぞれ設けられる車輪である。左右の車輪１７にはモータ（図示せず）が接続され、後述する駆動部１８（図２参照）からの制御信号に基づいてモータを駆動させることで、移動体１が移動される。左右のモータを、同じ出力で正転および逆転させることで移動体１の前方移動および後方移動を行い、また、モータを差動させることで、移動体１の移動方向の変更を行う。即ち、移動体１は、かかる車輪１７が配設される直交方向である前後方向には移動可能である一方で、左右方向には直接移動できないので、移動体１は、非ホロノミックな拘束条件を有する車輪１７及び駆動部１８によって移動するものである。
　次に、図２を参照して、移動体１の電気的構成について説明する。図２は、移動体１の電気的構成を示すブロック図である。移動体１は制御部１０を有し、その制御部１０はＣＰＵ１１と、フラッシュＲＯＭ１２と、ＲＡＭ１３とを有し、これらはバスライン１４を介して入出力ポート１５にそれぞれ接続されている。入出力ポート１５には、更に、測距センサ１６と、駆動部１８とが接続されている。
　ＣＰＵ１１は、バスライン１４により接続された各部を制御する演算装置である。フラッシュＲＯＭ１２は、ＣＰＵ１１により実行されるプログラムや固定値データ等を格納した書き換え可能な不揮発性の記憶装置であり、制御プログラム１２ａが記憶される。ＣＰＵ１１によって制御プログラム１２ａが実行されると、図３のメイン処理が実行される。
　ＲＡＭ１３は、ＣＰＵ１１が制御プログラム１２ａの実行時に各種のワークデータやフラグ等を書き換え可能に記憶するためのメモリであり、測距センサ１６から測距された測距データＭＰが記憶される測距データメモリ１３ａと、楕円位置向メモリ１３ｂと、二次関数位置向メモリ１３ｃと、カルマンフィルタ予測位置向メモリ１３ｄ（以下「ＫＦ予測位置向メモリ１３ｄ」と略す）と、相関結果位置向メモリ１３ｅと、推定現在位置向メモリ１３ｆと、前回値メモリ１３ｇとがそれぞれ設けられる。
　楕円位置向メモリ１３ｂは、測距データメモリ１３ａの測距データＭＰから近似された近似楕円Ｃ（図４参照）から推定された、ユーザＨの位置および向きを記憶するメモリである。二次関数位置向メモリ１３ｃは、測距データメモリ１３ａの測距データＭＰから近似された二次関数Ｑｘｙ又はＱｙｘ（図６参照）から推定されたユーザＨの位置および向きを記憶するメモリである。
　ＫＦ予測位置向メモリ１３ｄは、後述の前回値メモリ１３ｇの値から、カルマンフィルタ予測ステップ（数式９）に基づいて推定されたユーザＨの現在の位置および向きを記憶するメモリである。相関結果位置向メモリ１３ｅは、楕円位置向メモリ１３ｂ及び二次関数位置向メモリ１３ｃに記憶された複数のユーザＨの位置および向きのうち、最もユーザＨらしいと推定されるユーザＨの位置および向きが記憶されるメモリである。
　推定現在位置向メモリ１３ｆは、相関結果位置向メモリ１３ｅの値に基づいてカルマンフィルタによって推定されたユーザＨの位置および向きが記憶されるメモリであり、前回値メモリ１３ｇは、前回の推定現在位置向メモリ１３ｆの値と、推定されたユーザＨの速度および角速度とが記憶されるメモリである。
　駆動部１８は、移動体１を移動動作させるための装置であり、車輪１７（図１参照）および車輪１７の駆動源となるモータ（図示せず）等から構成される。制御部１０から制御信号が駆動部１８に入力されると、入力された制御信号に基づいてモータが回転し、該モータの回転が動力となって車輪１７が駆動し、移動体１を動作させる。
　次に、図３～７を参照して、移動体１のＣＰＵ１１で実行されるメイン処理を説明する。図３は、移動体１のメイン処理のフローチャートである。メイン処理は移動体１の電源投入直後に実行される。メイン処理はまず、測距センサ１６から取得した測距データＭＰを測距データメモリ１３ａに保存する（Ｓ１）。Ｓ１の処理の後、測距データメモリ１３ａに記憶される測距データＭＰに基づいて、ユーザＨの近似楕円Ｃを算出する（Ｓ２）。ここで図２を参照して、測距データＭＰに基づく、ユーザＨの近似楕円Ｃの算出について説明する。
　図４は、ユーザＨの近似楕円Ｃの算出を示す図である。図４において、測距センサ１６から取得されたユーザＨの測距データＭＰを、白抜きの四角形で表す。測距データＭＰに基づいた最小二乗法等によって、１の近似楕円Ｃが算出される。本実施形態では、近似楕円Ｃの中心ＣＰが「ユーザＨの位置」とされ、近似楕円Ｃにおける短軸の向きＤＣ１，ＤＣ２が「ユーザＨの向き」とされる。向きＤＣ１，ＤＣ２の２つがユーザＨの向きとして算出されるのは、近似楕円ＣからはユーザＨがどの方向を向いているか（前面または背面）が区別できないからである。なお、向きＤＣ１，ＤＣ２のうち、いずれをユーザＨの最終的な向きとするかは、後述のＳ７の処理で判定される。
　図３に戻る。Ｓ２の処理の後、算出された近似楕円ＣがユーザＨの体のサイズに近いものである場合は、その近似楕円ＣにおけるユーザＨの位置および向きを楕円位置向メモリ１３ｂに保存する（Ｓ３）。Ｓ２の処理では、測距データＭＰの瞬間的なバラツキや、測距データＭＰの測距誤差による揺らぎ等によって、算出される近似楕円ＣがユーザＨの体のサイズにフィットしたものや、ユーザＨの体のサイズから離れたもの等、様々となる場合がある。そこで、Ｓ３の処理では、算出された近似楕円ＣがユーザＨの体のサイズに近いかどうかが判断され、近似楕円ＣがユーザＨの体のサイズに近い場合は、該近似楕円ＣからユーザＨの位置および向きが算出される。かかるＳ３の処理における近似楕円Ｃの選択について、図５を参照して説明する。
　図５（ａ）は、測距データＭＰの瞬間的なバラツキによって、サイズの異なる近似楕円Ｃが算出される場合を示す図であり、図５（ｂ）は、ユーザＨの身体の肩幅Ｗ１および厚さＷ２と、近似楕円Ｃの長軸ａ及び短軸ｂとを示す図である。図３のＳ２の処理によって、測距データＭＰから１の近似楕円Ｃが算出されるが、測距データＭＰの瞬間的なバラツキによって、図５（ａ）に示すように、ユーザＨの体のサイズに近い近似楕円Ｃ１が算出される場合と、近似楕円Ｃ１とはサイズの異なる近似楕円Ｃ２が算出される場合とが存在する。特に、近似楕円Ｃ２は、ユーザＨの肩周辺の身体の外周よりも大きな円周で近似されている。このような近似楕円Ｃ２では、ユーザＨの位置および向きを正確に算出できない。
　そこで、図３のＳ３の処理では、図５（ｂ）に示す通り、近似楕円Ｃの長軸ａ及び短軸ｂと、ユーザＨの肩周辺における標準的な肩幅Ｗ１及び厚みＷ２とに基づき、算出された近似楕円Ｃが、ユーザＨの肩周辺の外周に近いかどうかを判断する。具体的には、以下の数式１の条件を満たす近似楕円Ｃが、ユーザＨの体のサイズに近いものと判断される。
Figure JPOXMLDOC01-appb-M000001
  
　本実施形態では、肩幅Ｗ１として「０．６ｍ」が例示され、厚さＷ２として「０．３ｍ」が例示される。即ち、近似楕円Ｃの長軸ａの２倍の長さがユーザＨの肩幅Ｗ１の±１０％以内かつ、短軸ｂの２倍の長さがユーザＨの厚さＷ２の±１０％以内である場合に、近似楕円Ｃは、ユーザＨのサイズに近いと判断される。このようにして、近似楕円ＣがユーザＨの体のサイズに近いと判断された場合は、その近似楕円Ｃの中心ＣＰおよび向きＤＣ１，ＤＣ２が、楕円位置向メモリ１３ｂに保存される。
　図３に戻る。Ｓ３の処理の後、測距データメモリに記憶される測距データＭＰに基づいて、ｘｙ座標系に基づく二次関数Ｑｘｙと、ｙｘ座標系に基づく二次関数Ｑｙｘとをそれぞれ算出する（Ｓ４）。ここで、図６（ａ）を参照して、二次関数Ｑｘｙ，Ｑｙｘの算出について説明する。
　図６（ａ）は、２種類の二次関数Ｑｘｙ，Ｑｙｘの算出を模式的に示した図である。図６（ａ）に示す通り、測距データＭＰに基づいて、ｘｙ座標系において二次関数で近似された二次関数Ｑｘｙと、ｙｚ座標系において二次関数で近似された二次関数Ｑｘｙとが算出される。二次関数Ｑｘｙは以下の数式２に基づいて、二次関数Ｑｙｘは数式３に基づいてそれぞれ算出される。
Figure JPOXMLDOC01-appb-M000002
 　ここで、数式２におけるａ１，ｂ１，ｃ１は係数であり、これら係数は、数式２と測距データＭＰとの最小二乗法に基づいて決定される。また、数式３におけるａ２，ｂ２，ｃ２も係数であり、これら係数は、数式３と測距データＭＰとの最小二乗法に基づいて決定される。即ち、本実施形態の二次関数Ｑｘｙのｘ軸に沿って直立する形状とされ、二次関数Ｑｙｘの軸線はｙ軸に沿って直立する形状と平行とされる。
　ところで、測距センサ１６は、レーザ光を全方位に対して照射することでユーザＨとの距離が取得されるので、ユーザＨの身体の一部分が、ユーザＨの測距データＭＰとして取得される。即ち、測距センサ１６から取得される測距データＭＰは、その分布に偏りが生じる。また、図６（ａ）に示す通り、ｘｙ座標系による二次関数Ｑｘｙは、上（または下）に凸の形状であり、ｙｘ座標系による二次関数Ｑｙｘは、左（または右）に凸の形状である。
　即ち、二次関数Ｑｘｙと二次関数Ｑｙｘとでは形状が異なるので、測距データＭＰの分布によって、二次関数Ｑｘｙに適合する測距データＭＰの分布や、二次関数Ｑｙｘに適合する測距データＭＰの分布が存在し得る。そこで本実施形態では、測距データＭＰから形状の異なる２つの二次関数Ｑｘｙ，Ｑｙｘを算出することで、測距データＭＰの分布により適合し得る二次関数Ｑの候補を挙げることができる。
　図３に戻る。Ｓ４の処理の後、二次関数Ｑｘｙ，Ｑｙｘから、測距データメモリ１３ａの値との近似誤差が最も小さいものを選択し、その二次関数Ｑｘｙ又はＱｙｘの位置および向きを二次関数位置向メモリ１３ｃに保存する（Ｓ５）。具体的には、まず、Ｓ４の処理で算出された、二次関数Ｑｘｙ，Ｑｙｘのそれぞれについて、測距データメモリ１３ａに記憶される測距データＭＰとの近似誤差が算出される。これら二次関数Ｑｘｙ，Ｑｙｘの近似誤差が小さい方が、ユーザＨの位置および向きを算出する二次関数Ｑとして選択される。
　選択された二次関数Ｑから、ユーザＨの位置および向きが算出される。本実施形態においては、二次関数Ｑの頂点Ｔと測距データＭＰとの位置関係に応じて、ユーザＨの位置および向きを算出する。ここで、二次関数ＱにおけるユーザＨの位置および向きの算出方法を、図６（ｂ），図７を参照して説明する。
　図６（ｂ）は、両端の測距データＭＰ間に二次関数Ｑの頂点Ｔが存在しない場合を示す図である。ここで、図６（ｂ），図７において、測距データＭＰのうち、二次関数Ｑの一端側に該当する測距データを「クラスタエッジＥ１」と称し、二次関数Ｑの他端側に該当する測距データＭＰを「クラスタエッジＥ２」と称する。
　図６（ｂ）において、二次関数Ｑの頂点Ｔは、クラスタエッジＥ１とクラスタエッジＥ２との外側に位置する。即ち、測距データＭＰは、ユーザＨの身体の一部分、具体的には、身体の前面（胸側）または背面（背中側）を測距できたが、両肩を上手く測距できなかったと判断される。
　そこで、本実施形態では、二次関数Ｑの頂点ＴがクラスタエッジＥ１とクラスタエッジＥ２との外側に位置する場合に、確実に測距できたクラスタエッジＥ１及びクラスタエッジＥ２に基づいて、ユーザＨの位置および向きが算出される。具体的には、クラスタエッジＥ１とクラスタエッジＥ２との中点ＥｃがユーザＨの位置とされ、クラスタエッジＥ１とクラスタエッジＥ２とを結ぶ直線に直交する向きＤＱ１，ＤＱ２がユーザＨの向きとされる。具体的には、クラスタエッジＥ１のｘ軸方向の座標をＸＥ１，ｙ軸方向の座標をＹＥ１とし、クラスタエッジＥ２のｘ軸方向の座標をＸＥ２，ｙ軸方向の座標をＹＥ２とすると、ユーザＨの向きＤＱ１は数式４で算出され、ユーザＨの向きＤＱ２は数式５で算出される。
Figure JPOXMLDOC01-appb-M000003
 　これにより、測距データＭＰとしてユーザＨの身体の一部分（前面または背面）のみが測距された場合であっても、確実に測距されたクラスタエッジＥ１とクラスタエッジＥ２とに基づいてユーザＨの位置および向きが算出されるので、かかるユーザＨの位置および向きの誤差を最小限に抑えることができる。なお、ユーザＨの向きとして向きＤＱ１，ＤＱ２の２つが算出されるのは、上記した近似楕円Ｃと同様に、二次関数ＱからユーザＨがどの方向を向いているか（前面または背面）を区別できないからである。なお、向きＤＱ１，ＤＱ２のうち、いずれをユーザＨの最終的な向きとするかは後述のＳ７の処理で判定される。
　次に、二次関数Ｑの頂点ＴがクラスタエッジＥ１とクラスタエッジＥ２との間に存在する場合について、図７を参照して説明する。二次関数Ｑの頂点ＴがクラスタエッジＥ１とクラスタエッジＥ２との間に存在する場合は、更に、クラスタエッジＥ１及びクラスタエッジＥ２が共に頂点Ｔから離れている場合（図７（ａ））と、クラスタエッジＥ１又はクラスタエッジＥ２のいずれか一方が頂点Ｔに近い場合と（図７（ｂ））で、ユーザＨの位置および向きの算出方法が異なる。
　図７（ａ）は、クラスタエッジＥ１，Ｅ２間に二次関数Ｑの頂点Ｔが存在し、クラスタエッジＥ２が二次関数Ｑの頂点Ｔより外側かつ頂点Ｔから離れている場合を示す図であり、図７（ｂ）は、クラスタエッジＥ１，Ｅ２間に二次関数Ｑの頂点Ｔが存在し、クラスタエッジＥ２が二次関数Ｑの頂点Ｔより外側かつ頂点Ｔに近い場合を示す図である。以下では、クラスタエッジＥ１が頂点Ｔから十分に離れていて、クラスタエッジＥ２が頂点Ｔから離れている場合と、頂点Ｔに近づいている場合とを説明するが、クラスタエッジＥ２が頂点Ｔから十分に離れていて、クラスタエッジＥ１が頂点Ｔから離れている場合もしくは頂点Ｔに近い場合も、同様であることは言うまでもない。
　クラスタエッジＥ１とクラスタエッジＥ２との間に頂点Ｔが位置する場合は、まず、クラスタエッジＥ２と頂点Ｔとの距離関係が判断される。具体的には、図７（ａ），図７（ｂ）に示す、中点Ｅｃと頂点Ｔとの距離と、中点ＥｃとクラスタエッジＥ２との距離に基づいて、クラスタエッジＥ２と頂点Ｔとの位置関係が判断される。ここで、中点Ｅｃと頂点Ｔとのｘ軸方向の距離をΔＸＴ，ｙ軸方向の距離をΔＹＴとし、中点ＥｃとクラスタエッジＥ２とのｘ軸方向の距離をΔＸＥ，ｙ軸方向の距離をΔＹＥとして、以下の数式６を満たす場合は、クラスタエッジＥ２と頂点Ｔとが離れていると判断される。
Figure JPOXMLDOC01-appb-M000004
 　即ち、本実施形態では、中点ＥｃとクラスタエッジＥ２との距離が、中点Ｅｃと頂点Ｔとの距離の０．９倍より大きい場合は、クラスタエッジＥ２と頂点Ｔとが離れていると判断される。
　まず、図７（ａ）を参照して、クラスタエッジＥ２と頂点Ｔとが離れている場合の、ユーザＨの位置および向きの算出方法を説明する。図７（ａ）に示す通り、クラスタエッジＥ２と頂点Ｔとが離れているので、クラスタエッジＥ１と頂点Ｔとの間のみならず、頂点ＴとクラスタエッジＥ２との間にも多くの測距データＭＰが測距される。従って、測距データＭＰによって、両肩を含めたユーザＨの身体の前面または背面を幅広く測距されたと判断できる。この場合、頂点Ｔ付近がユーザＨの胸部もしくは背中の中心位置と推定され、クラスタエッジＥ１とクラスタエッジＥ２とは、両肩付近の位置と推定される。
　そこで本実施形態では、クラスタエッジＥ２と頂点Ｔとが離れている場合は、両肩の位置と推定されるクラスタエッジＥ１，Ｅ２とに基づいて、ユーザＨの位置および向きが算出される。具体的には、中点ＥｃがユーザＨの位置とされ、クラスタエッジＥ１とクラスタエッジＥ２とを結ぶ直線に直交する向きＤＱ１，ＤＱ２が、ユーザＨの向きとされる。即ち、ユーザＨの向きは、上述した数式４と数式５とにより算出される。これにより、両肩の位置と推定されるクラスタエッジＥ１，Ｅ２に基づいて、ユーザＨの位置および向きが算出されるので、かかるユーザＨの位置および向きを、より精度の高いものとできる。
　次に、クラスタエッジＥ２と頂点Ｔとが近い場合、即ち、上述した数式６を満たさない場合のユーザＨの位置および向きの算出方法を図７（ｂ）を参照して説明する。図７（ｂ）に示す通り、クラスタエッジＥ２は、頂点Ｔよりも外側かつ頂点Ｔとが近い位置に存在している。かかる場合は、頂点Ｔ付近の位置がユーザＨの左肩または右肩付近の位置と推定される。
　ところで、クラスタエッジＥ２は頂点Ｔよりも外側に隣接するので、測距センサ１６（移動体１）から見た場合、クラスタエッジＥ２は頂点Ｔの「裏側」に位置する。即ち、頂点Ｔが、測距センサ１６からのレーザ光の障害（邪魔）となって、クラスタエッジＥ２まで届かず、クラスタエッジＥ２は測距できない状態となっている。これによって、該当する左肩または右肩の全体が測距できないことで、クラスタエッジＥ２が正確に測距されなかった虞がある。
　そこで本実施形態では、クラスタエッジＥ１，Ｅ２に加え、測距データＭＰが正確に取得された範囲に位置する頂点Ｔ、即ち左肩または右肩とされる位置とに基づいてユーザＨの向きＤＱ１，ＤＱ２が算出される。具体的には、中点Ｅｃのｘ軸方向の座標をＸＣ，ｙ軸方向の座標ＹＣとし、頂点Ｔのｘ軸方向の座標をＸＴ，ｙ軸方向の座標をＹＴとすると、ユーザＨの向きＤＱ１は以下の数式７で算出され、向きＤＱ２は数式８で算出される。
Figure JPOXMLDOC01-appb-M000005
 　即ち、クラスタエッジＥ２と頂点Ｔとが近い場合は、中点Ｅｃと頂点Ｔとを結ぶ直線に直交する直線の角度から向きＤＱ１，ＤＱ２が算出される。即ち、正確に測距データＭＰが取得された範囲に位置するが、近似された二次関数Ｑ上の左肩または右肩の位置とされる頂点Ｔと、実際に測距センサ１６によって測距されたが、頂点Ｔ付近の影響によって正確に測距されなかった虞のあるクラスタエッジＥ２との両方が考慮されて向きＤＱ１，ＤＱ２が算出されるので、かかる向きＤＱ１，ＤＱ２の誤差を抑制できる。以上のように算出された、向きＤＱ１，ＤＱ２をユーザＨの向きとして、また、中点Ｅｃをユーザの位置として、それぞれ二次関数位置向メモリ１３ｃに保存される。
　図３に戻る。Ｓ５の処理の後、前回値メモリ１３ｇの値から、カルマンフィルタ予測ステップに基づいてユーザＨの現在の位置および向きを予測し、ＫＦ予測位置向メモリ１３ｄに保存する（Ｓ６）。前回値メモリ１３ｇには、後述する前回のＳ９又はＳ１１の処理で推定された、最終的なユーザＨの現在位置や向き、速度や角速度が記憶される。かかる前回状態が継続した場合に、予測されるユーザＨの位置および向きが算出される。
　ここで、前回値メモリ１３ｇに記憶されるユーザＨのｘ軸方向の位置をＰｘ，ｋ－１，ｙ軸方向の位置をＰｙ，ｋ－１，ユーザＨの向きをＰθ，ｋ－１，ユーザＨのｘ軸方向の速度をＶｘ，ｋ－１，ｙ軸方向の速度をＶｙ，ｋ－１，ユーザＨの角速度をＶθ，ｋ－１とし、前回のＳ９又はＳ１１の処理からの経過時間をΔｔ，加速度外乱等によるユーザＨのシステム雑音成分をｎｘｓ，ｎｙｓ，ｎθｓとすると、予測されるユーザＨのｘ軸方向の位置Ｐｘｐ，ｋ，ｙ軸方向の位置Ｐｙｐ，ｋ，ユーザＨの向きＰθｐ，ｋ，ユーザＨのｘ軸方向の速度Ｖｘｐ，ｋ，ｙ軸方向の速度Ｖｙｐ，ｋ，ユーザＨの角速度Ｖθｐ，ｋは、公知のカルマンフィルタ予測ステップである数式９によって算出される。
Figure JPOXMLDOC01-appb-M000006
 　かかる数式９で予測されたユーザＨのｘ軸方向の位置Ｐｘｐ，ｋ，ｙ軸方向の位置Ｐｙｐ，ｋ，ユーザＨの向きＰθｐ，ｋ，ユーザＨのｘ軸方向の速度Ｖｘｐ，ｋ，ｙ軸方向の速度Ｖｙｐ，ｋ，ユーザＨの角速度Ｖθｐ，ｋがＫＦ予測位置向メモリ１３ｄに記憶される。
　Ｓ６の処理の後、楕円位置向メモリ１３ｂ，二次関数位置向メモリ１３ｃ及びＫＦ予測位置向メモリ１３ｄに記憶されるユーザＨの位置および向きに基づき相関処理を行い、該当するユーザＨの位置および向きを相関結果位置向メモリ１３ｅに保存する（Ｓ７）。
　具体的には、まず、楕円位置向メモリ１３ｂ及び二次関数位置向メモリ１３ｃに記憶されるユーザＨの位置および向きを、ＫＦ予測位置向メモリ１３ｄに記憶される前回値を基準とした「ゲート」で絞り込みを行う。これによって、前回値に基づくユーザＨの位置および向きから大きく異なる、楕円位置向メモリ１３ｂ及び二次関数位置向メモリ１３ｃのユーザＨの位置および向きが排除される。
　そして、ゲートによって絞り込まれたユーザＨの位置および向きの中から、更に最近傍法によって最もユーザＨの位置および向きらしいと推定されるものが選択され、相関結果位置向メモリ１３ｅに記憶される。この際、ゲートによる絞り込み及び最近傍法によって、１つもユーザＨの位置および向きが選択されなかった場合は、相関結果位置向メモリ１３ｅには、ユーザＨの位置および向きが選択されなかった旨を示す値が記憶される。なお、ゲートおよび最近傍法は公知の技術なので、これらの詳細な説明は省略する。
　かかるゲートによる絞り込み及び最近傍法によって、近似楕円Ｃおよび二次関数Ｑから算出されたユーザＨの位置および向きのうち、最もユーザＨらしいと推定されるユーザＨの位置および向きが選択される。
　Ｓ７の処理の後、相関結果位置向メモリ１３ｅにユーザＨの位置および向きが記憶されているかを確認する（Ｓ８）。ユーザＨの位置および向きが記憶されている場合（Ｓ８：Ｙｅｓ）、相関結果位置向メモリ１３ｅの値から、公知のカルマンフィルタによってユーザの現在の位置および向きを推定し、推定現在位置向メモリ１３ｆに保存する（Ｓ９）。具体的には、相関結果位置向メモリ１３ｅに記憶されるユーザＨのｘ軸方向の位置をＰｘｃ，ｙ軸方向の位置をＰｙｃ，ユーザＨの向きをＰθｃ，ユーザＨのｘ軸方向の速度をＶｘｃ，ｙ軸方向の速度をＶｙｃ，ユーザＨの角速度をＶθｃとし、測距センサ１６の検知誤差やユーザＨの動きによる尤度関数等による観測誤差成分をｎｘｐ，ｎｙｐ，ｎθｐとすると、ユーザＨのｘ軸方向の位置Ｐｘ，ｋ，ｙ軸方向の位置Ｐｙ，ｋ，ユーザＨの向きＰθ，ｋは、数式１０によって算出される。
Figure JPOXMLDOC01-appb-M000007
 　なお、ユーザＨのｘ軸方向の速度Ｖｘｃは、前回値メモリ１３ｇに記憶されるユーザＨのｘ軸方向の位置と、相関結果位置向メモリ１３ｅに記憶されるユーザＨのｘ軸方向の位置とに基づいて算出され、同様にユーザＨのｙ軸方向の速度Ｖｙｃは、前回値メモリ１３ｇのユーザＨのｙ軸方向の位置と相関結果位置向メモリ１３ｅのｙ軸方向の位置とに基づいて算出され、ユーザＨの角速度Ｖθｃは、前回値メモリ１３ｇのユーザＨの向きと相関結果位置向メモリ１３ｅのユーザＨの向きとに基づいて算出される。
　これにより、Ｓ７の処理で相関結果位置向メモリ１３ｅに記憶された、最もユーザＨらしいと推定されるユーザＨの位置および向きに、観測誤差成分、即ち測距センサ１６の検知誤差やユーザＨの動き等、現実的に発生し得る誤差が含まれるので、実際に観測されるユーザＨの位置および向きとの誤差が小さいユーザＨの位置および向きを推定することができる。
　一方、相関結果位置向メモリ１３ｅに、ユーザＨの位置および向きが記憶されていない場合（Ｓ８：Ｎｏ）、即ち、Ｓ７の処理において、相関結果位置向メモリ１３ｅにユーザＨの位置および向きが選択されなかった旨を示す値が記憶された場合は、推定現在位置向メモリ１３ｆにＫＦ予測位置向メモリ１３ｄの値を保存する（Ｓ１０）。即ち、Ｓ７の処理において、Ｓ２～Ｓ５の処理で算出されたユーザＨの位置および向きが、いずれもユーザＨらしくないと判断された場合でも、予測値ではあるものの、前回値メモリ１３ｇの値に基づく現在のユーザＨの位置および向きが推定現在位置向メモリ１３ｆに記憶される。これにより、該ユーザＨの位置および向きが更新されない事態を抑制できる。
　Ｓ９，Ｓ１０の処理の後、推定現在位置向メモリ１３ｆのユーザＨの位置および向きに基づく制御信号を駆動部１８に出力して（Ｓ１１）、駆動部１８を動作させる。これにより、移動体１は推定されたユーザＨの位置および向きに基づいて移動動作される。
　Ｓ１２の処理の後、推定現在位置向メモリ１３ｆに記憶されるユーザＨの位置および向きと、ユーザＨの速度および角速度とを前回値メモリ１３ｇへ保存する（Ｓ１２）。具体的には、まず、推定現在位置向メモリ１３ｆのユーザＨの位置および向きと、前回値メモリ１３ｇに記憶されるユーザＨの位置および向きとにより、ユーザＨの速度および角速度を算出し、かかるユーザＨの速度および角速度を前回値メモリ１３ｇに保存した後に、推定現在位置向メモリ１３ｆのユーザＨの位置および向きに保存する。そして、Ｓ１２の処理の後は、Ｓ１以下の処理を繰り返す。
　以上説明した通り、本実施形態の移動体１は、測距センサ１６によって測距された測距データＭＰに基づいて、近似楕円Ｃと、ｘｙ軸およびｙｘ軸による二次関数Ｑｘｙ，Ｑｙｘとが算出され、それぞれのユーザＨの位置および向きが算出される。算出された複数のユーザＨの位置および向きから、ゲート及び最近傍法によって、最もユーザＨらしいと推定されるユーザＨの位置および向きが選択され、該ユーザＨの位置および向きから、現在のユーザＨの位置および向きが推定される。
　ここで、移動体１とユーザＨとの位置関係は、時々刻々と、様々に変化する。そこで、測距データＭＰから近似楕円Ｃ，二次関数Ｑｘｙ，Ｑｙｘといった複数の形状および、複数の座標系による近似曲線を算出し、１の形状または座標系に基づいた近似曲線がユーザＨに上手く適合しない場合は、より適合する他の形状あるいは座標系に基づいた近似曲線に基づいて、ユーザＨの位置および向きが推定される。これにより、測距センサ１６とユーザＨとの位置関係が様々に変化しても、ユーザＨの位置および向きを検出できる。
　以上、実施形態に基づき本発明を説明したが、本発明は上述した実施形態に何ら限定されるものではなく、本発明の趣旨を逸脱しない範囲内で種々の改良変更が可能であることは容易に推察できるものである。
　上記実施形態では、測距データＭＰから算出される二次関数Ｑとして、ｘｙ座標系による二次関数Ｑｘｙと、ｙｘ座標系による二次関数Ｑｙｘとの２種類とする構成とした。しかし、必ずしもこれに限られるものではなく、ｘｚ座標系やｙｚ座標系による二次関数を算出しても良いし、他の座標系による二次関数Ｑを算出しても良い。
　上記実施形態では、測距データＭＰを二次関数で近似する構成とした。しかし、必ずしもこれに限られるものではなく、三次関数や四次関数等、次数が３以上の多項式関数によって近似する構成としても良い。
　上記実施形態では、二次関数Ｑｘｙはｘ軸に沿って直立する形状とし、二次関数Ｑｙｘの軸線はｙ軸に沿って直立する構成とした。しかし、必ずしもこれに限られるものではなく、二次関数Ｑｘｙ，Ｑｙｘをｘ軸またはｙ軸側に傾ける形状とする構成としても良い。
　上記実施形態では、測距センサ１６によって肩周辺の胴体が測距される構成とした。しかし、これに限られるものではなく、腹部周辺や腰周辺等、ユーザＨの胴体が測距される部位であれば、他の部位でも良い。
　上記実施形態では、近似楕円Ｃと二次関数Ｑとに基づいて、ユーザＨの位置および向きを推定する構成とした。しかし、必ずしもこれに限られるものではなく、二次関数Ｑのみに基づいて、ユーザＨの位置および向きを推定しても良いし、近似楕円Ｃのみに基づいて、ユーザＨの位置および向きを推定しても良い。
　上記実施形態では、近似楕円Ｃの長軸ａ及び短軸ｂがユーザＨの肩幅Ｗ１及び厚さＷ２に近似する場合、近似楕円Ｃの中心ＣＰおよび向きＤＣ１，ＤＣ２を楕円位置向メモリ１３ｂへ記憶する構成とした。しかし、必ずしもこれに限られるものではなく、近似楕円Ｃの円周がユーザＨの肩周辺の外周の長さと近似する場合に、近似楕円Ｃの中心ＣＰおよび向きＤＣ１，ＤＣ２を楕円位置向メモリ１３ｂに記憶する構成としても良いし、近似楕円Ｃの面積がユーザＨの肩周辺の断面積と近似する場合に、近似楕円Ｃの中心ＣＰおよび向きＤＣ１，ＤＣ２を楕円位置向メモリ１３ｂに記憶する構成としても良い。
　上記実施形態では、二次関数ＱにおけるクラスタエッジＥ１，Ｅ２を、測距データＭＰのうち両端の測距データＭＰとした。しかし、必ずしもこれに限られるものではなく、二次関数Ｑ上の点であって、測距データＭＰのうち一方の端の測距データＭＰに近似する点をクラスタエッジＥ１とし、他方の端の測距データＭＰに近似する点をクラスタエッジＥ２とする構成としても良い。
　上記実施形態では、近似楕円ＣにおけるユーザＨの位置を近似楕円Ｃの中心ＣＰとし、二次関数Ｑｘｙ，ＱｙｘにおけるユーザＨの位置をクラスタエッジＥ１，Ｅ２の中点Ｅｃとする構成とした。しかし、必ずしもこれに限られるものではなく、近似楕円Ｃ及び二次関数ＱにおけるユーザＨの位置をいずれも近似楕円Ｃの中心ＣＰとしても良いし、クラスタエッジＥ１，Ｅ２の中点Ｅｃとしても良い。また、測距データＭＰに基づく最小二乗法等、他の手法によって算出されたユーザＨの位置を用いても良い。
　上記実施形態では、クラスタエッジＥ１とクラスタエッジＥ２との間に頂点Ｔが存在し、かつクラスタエッジＥ２と頂点Ｔとが近い場合は、中点Ｅｃと頂点Ｔとを結ぶ直線に直交する直線の角度から向きＤＱ１，ＤＱ２を算出する構成とした。しかし、必ずしもこれに限られるものではなく、かかる場合に、クラスタエッジＥ２と頂点Ｔとが離れている場合と同様に、クラスタエッジＥ１とクラスタエッジＥ２とを結ぶ直線に直交する直線の角度から向きＤＱ１，ＤＱ２を算出する構成としても良い。
　上記実施形態に挙げた数値は一例であり、他の数値を採用することは当然可能である。
１０　　　　　　　　制御部（身体向推定装置）
１２ａ　　　　　　　制御プログラム（身体向推定プログラム）
Ｈ　　　　　　　　　ユーザ（対象者）
１６　　　　　　　　測距センサ（測距手段、測距取得機能）
ＭＰ　　　　　　　　測距データ
Ｑ，Ｑｘｙ，Ｑｙｘ　二次関数
Ｃ，Ｃ１，Ｃ２　　　近似楕円
Ｅ１，Ｅ２　　　　　クラスタエッジ
Ｓ２　　　　　　　　楕円算出手段
Ｓ３　　　　　　　　向き推定手段、向き推定機能
Ｓ４　　　　　　　　二次関数算出手段、二次関数算出機能
Ｓ５　　　　　　　　二次関数選択手段、二次関数選択機能、
　　　　　　　　　　向き推定手段、向き推定機能
　　　　　　　　　　第１向き推定手段、第２向き推定手段
Ｓ７～Ｓ１０　　　　向き推定手段、向き推定機能
Ｔ　　　　　　　　　頂点

Claims (11)

  1. 　対象者を測距する測距手段と、
    　その測距手段により測距された複数の測距データを基にして近似される二次関数を、複数の座標系に基づいてそれぞれ算出する二次関数算出手段と、
    　その二次関数算出手段により算出された複数の二次関数うち、複数の前記測距データとの近似誤差が最も小さい二次関数を選択する二次関数選択手段と、
    　その二次関数選択手段により選択された二次関数に基づいて前記対象者の向きを推定する向き推定手段と、
    　を備えていることを特徴とする身体向推定装置。
  2. 　前記二次関数選択手段により選択された二次関数の算出に用いられた測距データのうち、その二次関数上の一方の端に該当する測距データと、他方の端に該当する測距データとの２点をクラスタエッジと称し、或いは、前記二次関数選択手段により選択された二次関数上の点であって、複数の前記測距データのうち、一方の端の測距データに近似される点と、他方の端の測距データに近似される点との２点をクラスタエッジと称し、
    　前記向き推定手段は、前記クラスタエッジを結ぶ直線に直交する方向を、前記対象者の向きとして推定する第１向き推定手段を備えていることを特徴とする請求項１記載の身体向推定装置。
  3. 　前記向き推定手段は、前記二次関数選択手段により選択された二次関数の頂点が前記クラスタエッジの間に存在しない場合に、前記第１向き推定手段によって前記対象者の向きを推定するものであることを特徴とする請求項２記載の身体向推定装置。
  4. 　前記向き推定手段は、前記二次関数選択手段により選択された二次関数の頂点が前記クラスタエッジの間に存在し且つ第１所定条件を満たす場合に、前記第１向き推定手段によって前記対象者の向きを推定するものであることを特徴とする請求項２又は３に記載の身体向推定装置。
  5. 　前記二次関数選択手段により選択された二次関数の算出に用いられた測距データのうち、その二次関数上の一方の端に該当する測距データと、他方の端に該当する測距データとの２点をクラスタエッジと称し、或いは、前記二次関数選択手段により選択された二次関数上の点であって、複数の前記測距データのうち、一方の端の測距データに近似される点と、他方の端の測距データに近似される点との２点をクラスタエッジと称し、
    　前記向き推定手段は、前記クラスタエッジの中点と前記二次関数選択手段により選択された二次関数の頂点とを結ぶ直線に直交する方向を、前記対象者の向きとして推定する第２向き推定手段を備えていることを特徴とする請求項１から４のいずれかに記載の身体向推定装置。
  6. 　前記向き推定手段は、前記二次関数選択手段により選択された二次関数の頂点が前記クラスタエッジの間に存在し且つ第２所定条件を満たす場合に、前記第２向き推定手段によって前記対象者の向きを推定するものであることを特徴とする請求項５記載の身体向推定装置。
  7. 　前記二次関数算出手段は、複数の前記測距データを基にして近似される二次関数を、ｘｙ座標系とｙｘ座標系との少なくとも２つの座標系に基づいて、それぞれ算出するものであることを特徴とする請求項１から６のいずれかに記載の身体向推定装置。
  8. 　前記測距手段により測距された複数の測距データに基づいて近似楕円を算出する楕円算出手段を備え、
    　前記向き推定手段は、その楕円算出手段により算出された楕円または前記二次関数選択手段により選択された二次関数に基づいて前記対象者の向きを推定するものであることを特徴とする請求項１から７のいずれかに記載の身体向推定装置。
  9. 　前記向き推定手段は、前記楕円算出手段により算出された楕円に基づいて推定された前記対象者の向きと、前記二次関数選択手段により選択された二次関数に基づいて推定された前記対象者の向きと、前記対象者の前回推定された向きに基づいて予測された前記対象者の今回予測向きとのうち、いずれかの向きを前記対象者の今回予測向きに基づく相関処理により前記対象者の向きとして推定するものであることを特徴とする請求項８記載の身体向推定装置。
  10. 　前記向き推定手段は、前記対象者の前記推定された向きを、更にカルマンフィルタによって更新し、その更新後の向きを前記対象者の向きとして推定するものであることを特徴とする請求項１から９のいずれかに記載の身体向推定装置。
  11. 　対象者を測距した測距データを取得する測距取得機能と、
    　その測距取得機能により取得された複数の測距データを基にして近似される二次関数を、複数の座標系に基づいてそれぞれ算出する二次関数算出機能と、
    　その二次関数算出機能により算出された複数の二次関数うち、複数の前記測距データとの近似誤差が最も小さい二次関数を選択する二次関数選択機能と、
    　その二次関数選択機能により選択された二次関数に基づいて前記対象者の向きを推定する向き推定機能と、
    　をコンピュータに実現させることを特徴とする身体向推定プログラム。
PCT/JP2019/012299 2018-03-26 2019-03-25 身体向推定装置および身体向推定プログラム WO2019188865A1 (ja)

Priority Applications (3)

Application Number Priority Date Filing Date Title
CN201980004746.0A CN111164376A (zh) 2018-03-26 2019-03-25 身体朝向推断装置以及身体朝向推断程序
US16/652,206 US20200242344A1 (en) 2018-03-26 2019-03-25 Body orientation estimation device and body orientation estimation program
EP19775154.8A EP3779356A4 (en) 2018-03-26 2019-03-25 Body orientation estimation device and body orientation estimation program

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2018-057835 2018-03-26
JP2018057835A JP2019168394A (ja) 2018-03-26 2018-03-26 身体向推定装置および身体向推定プログラム

Publications (1)

Publication Number Publication Date
WO2019188865A1 true WO2019188865A1 (ja) 2019-10-03

Family

ID=68061872

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2019/012299 WO2019188865A1 (ja) 2018-03-26 2019-03-25 身体向推定装置および身体向推定プログラム

Country Status (5)

Country Link
US (1) US20200242344A1 (ja)
EP (1) EP3779356A4 (ja)
JP (1) JP2019168394A (ja)
CN (1) CN111164376A (ja)
WO (1) WO2019188865A1 (ja)

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH09322050A (ja) * 1996-05-29 1997-12-12 Sony Corp 被写体認識装置および方法
JP2008257575A (ja) * 2007-04-06 2008-10-23 Fujitsu Ltd 入眠状態検知方法、入眠状態検知装置、入眠状態検知システム及びコンピュータプログラム
JP2009116871A (ja) * 2007-11-06 2009-05-28 Palo Alto Research Center Inc 鏡上の内容及び動作に関連した情報を用いて鏡の機能を拡張する方法及び装置
JP2010169521A (ja) 2009-01-22 2010-08-05 Nec Corp 位置検出装置、位置検出方法及びプログラム
JP2011081634A (ja) 2009-10-07 2011-04-21 Sogo Keibi Hosho Co Ltd 人体向き推定装置及び人体向き推定方法
WO2013011644A1 (ja) * 2011-07-15 2013-01-24 パナソニック株式会社 姿勢推定装置、姿勢推定方法、および姿勢推定プログラム
JP2016175509A (ja) 2015-03-19 2016-10-06 カルソニックカンセイ株式会社 顔向き検知装置及び顔向き検知方法
WO2018180503A1 (ja) * 2017-03-30 2018-10-04 株式会社エクォス・リサーチ 身体向推定装置および身体向推定プログラム

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2710557B1 (en) * 2011-05-16 2020-04-29 Max-Planck-Gesellschaft zur Förderung der Wissenschaften e.V. Fast articulated motion tracking
JP5550124B2 (ja) * 2012-08-17 2014-07-16 Ｎｅｃシステムテクノロジー株式会社 入力装置、機器、入力方法およびプログラム
US9672428B2 (en) * 2015-09-11 2017-06-06 Disney Enterprises, Inc. Systems and methods for detecting free-standing groups of individuals
CN107341179B (zh) * 2017-05-26 2020-09-18 深圳奥比中光科技有限公司 标准运动数据库的生成方法、装置及存储装置

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH09322050A (ja) * 1996-05-29 1997-12-12 Sony Corp 被写体認識装置および方法
JP2008257575A (ja) * 2007-04-06 2008-10-23 Fujitsu Ltd 入眠状態検知方法、入眠状態検知装置、入眠状態検知システム及びコンピュータプログラム
JP2009116871A (ja) * 2007-11-06 2009-05-28 Palo Alto Research Center Inc 鏡上の内容及び動作に関連した情報を用いて鏡の機能を拡張する方法及び装置
JP2010169521A (ja) 2009-01-22 2010-08-05 Nec Corp 位置検出装置、位置検出方法及びプログラム
JP2011081634A (ja) 2009-10-07 2011-04-21 Sogo Keibi Hosho Co Ltd 人体向き推定装置及び人体向き推定方法
WO2013011644A1 (ja) * 2011-07-15 2013-01-24 パナソニック株式会社 姿勢推定装置、姿勢推定方法、および姿勢推定プログラム
JP2016175509A (ja) 2015-03-19 2016-10-06 カルソニックカンセイ株式会社 顔向き検知装置及び顔向き検知方法
WO2018180503A1 (ja) * 2017-03-30 2018-10-04 株式会社エクォス・リサーチ 身体向推定装置および身体向推定プログラム

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
See also references of EP3779356A4

Also Published As

Publication number Publication date
JP2019168394A (ja) 2019-10-03
US20200242344A1 (en) 2020-07-30
EP3779356A4 (en) 2021-12-29
EP3779356A1 (en) 2021-02-17
CN111164376A (zh) 2020-05-15

Similar Documents

Publication Publication Date Title
KR100772912B1 (ko) 절대 방위각을 이용한 로봇 및 이를 이용한 맵 작성 방법
US8515612B2 (en) Route planning method, route planning device and autonomous mobile device
JP4831442B2 (ja) 衝突判定装置
JP2008087726A (ja) 車両の走行制御システム
KR102086270B1 (ko) 주행 제어 장치의 제어 방법 및 주행 제어 장치
US9802619B2 (en) Moving amount estimating apparatus, autonomous mobile body, and moving amount estimating method
US9126338B2 (en) Robot system and method for driving the same
KR20100052383A (ko) 이동 로봇의 안전경로 생성 방법 및 장치
JP7487388B2 (ja) 測定装置、測定方法、及び、プログラム
JP2020004342A (ja) 移動体制御装置
JPWO2019163124A1 (ja) 三次元位置推定装置及び三次元位置推定方法
JP2008134743A (ja) 自己位置認識方法
KR102545582B1 (ko) 교차로 충돌 회피 시스템 및 그 제어 방법
WO2018179960A1 (ja) 移動体および自己位置推定装置
WO2019188865A1 (ja) 身体向推定装置および身体向推定プログラム
JP7204612B2 (ja) 位置姿勢推定装置、位置姿勢推定方法及びプログラム
US11754403B2 (en) Self-position correction method and self-position correction device
JP2020008461A (ja) 自律移動体の位置推定装置
JP2023145130A (ja) 自律移動ロボットの停止方法および自律移動ロボット
JP2019215773A (ja) 無人搬送車の走行制御装置及び走行制御方法
US10794696B2 (en) Method for identifying noise data of laser ranging device
JP2021149229A (ja) 移動体、及び、位置推定方法
JP2002073171A (ja) 無人搬送車の走行制御方法
JP2020009252A (ja) 自律移動体の位置推定装置
JP7283341B2 (ja) 走行制御システム

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 19775154

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

ENP Entry into the national phase

Ref document number: 2019775154

Country of ref document: EP

Effective date: 20201026