WO2023021734A1 - 移動装置、および移動装置制御方法、並びにプログラム - Google Patents

移動装置、および移動装置制御方法、並びにプログラム Download PDF

Info

Publication number
WO2023021734A1
WO2023021734A1 PCT/JP2022/006848 JP2022006848W WO2023021734A1 WO 2023021734 A1 WO2023021734 A1 WO 2023021734A1 JP 2022006848 W JP2022006848 W JP 2022006848W WO 2023021734 A1 WO2023021734 A1 WO 2023021734A1
Authority
WO
WIPO (PCT)
Prior art keywords
cost
route
mobile device
leg
path
Prior art date
Application number
PCT/JP2022/006848
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 CN202280055976.1A priority Critical patent/CN117897665A/zh
Publication of WO2023021734A1 publication Critical patent/WO2023021734A1/ja

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions

Definitions

  • the present disclosure relates to mobile devices, mobile device control methods, and programs. Specifically, the present invention relates to a mobile device such as a walking robot that moves by driving a plurality of legs, a mobile device control method, and a program.
  • Robots that move by driving multiple legs are used in various fields. Robots can perform various tasks in factories and disaster sites where it is difficult for people to enter.
  • a walking robot For example, if the legs of a walking robot are placed on a highly uneven or sloping surface, the robot may fall over and become unable to move forward.
  • many robots are equipped with sensors such as cameras to recognize the environment around the robot, and use sensor detection information to select a route for stable walking or running. running control to move.
  • the data obtained from the sensor is used to generate an environmental map containing the surrounding 3D shape data, and the generated environmental map is referenced to determine the movement route.
  • walking robots require multiple legs, such as quadrupeds and hexapods, to be grounded at different positions, and in order to walk stably, it is necessary to analyze the shape of each grounding position of each leg. Is required. A large amount of calculation is required to analyze the shape of the ground contact surface of each leg, and as a result, there arises a problem that the traveling speed of the robot decreases.
  • Patent Document 1 Japanese Unexamined Patent Application Publication No. 2013-066996 discloses a technique of analyzing the contact position of a leg of a walking robot. This patent document 1 discloses a technique for stably running by repeatedly calculating positions where each leg of the robot just touches the robot running surface without sinking into it.
  • Patent Document 1 in order to calculate the three-dimensional position where each leg and the contact surface touch, iterative calculations are performed using three-dimensional point cloud data. If the amount of computation is large and the robot travels on a long route, this computation must be repeated many times, resulting in a problem of reduced robot travel speed.
  • An object of the present invention is to provide a mobile device, a mobile device control method, and a program that enable traveling.
  • a first aspect of the present disclosure includes: a route planning unit that determines a travel route of the mobile device; The route planning unit For each of the multiple route candidates, calculate the route cost by applying a cost calculation algorithm in which the cost is lower for routes that allow stable travel, The mobile device determines the route candidate with the lowest calculated route cost as the optimum travel route.
  • a second aspect of the present disclosure is A mobile device control method to be executed in a mobile device,
  • the mobile device has a route planning unit that determines a travel route of the mobile device,
  • the route planning unit For each of the multiple route candidates, calculate the route cost by applying a cost calculation algorithm in which the cost is lower for routes that allow stable travel,
  • a moving device control method for determining a route candidate with the lowest calculated route cost as an optimum travel route.
  • a third aspect of the present disclosure is A program for executing mobile device control processing in a mobile device,
  • the mobile device has a route planning unit that determines a travel route of the mobile device,
  • the program causes the route planning unit to: For each of the plurality of route candidates, a route cost is calculated by applying a cost calculation algorithm in which the cost is lower for a route that allows stable travel,
  • the program executes a process of determining the route candidate with the lowest calculated route cost as the optimum travel route.
  • the program of the present disclosure is, for example, a program that can be provided in a computer-readable format to an information processing device or computer system capable of executing various program codes via a storage medium or communication medium.
  • processing according to the program is realized on the information processing device or computer system.
  • a system is a logical collective configuration of a plurality of devices, and the devices of each configuration are not limited to being in the same housing.
  • a device and a method for efficiently determining an optimal route along which a mobile device such as a walking robot can safely travel are realized.
  • it has a route planning unit that determines the traveling route of a mobile device such as a walking robot, and the route planning unit determines that the route that allows stable traveling for each of a plurality of route candidates has a lower cost.
  • a calculation algorithm is applied to calculate the route cost, and the route candidate with the lowest calculated route cost is determined as the optimum travel route.
  • the route planning section sets multiple sampling points along the route, applies a cost calculation algorithm that increases the cost as the difference between the left and right leg contact heights at each sampling point increases, and calculates the cost corresponding to each sampling point.
  • the added value of the costs of the sampling points is calculated as the route cost.
  • FIG. 10 is a diagram for explaining an unstable state when the legs of the robot are placed on a stepped obstacle;
  • FIG. 4 is a diagram illustrating an example in which a robot selects a flat route and travels;
  • FIG. 4 is a diagram illustrating an example in which a robot selects a flat route and travels; It is a figure explaining the structural example of the robot which is a moving apparatus of this indication.
  • FIG. 4 is a diagram illustrating specific examples of global routes generated by a global route planner and partial routes generated by a partial route planner;
  • FIG. 4 is a diagram showing a flowchart describing a detailed sequence of optimum partial route determination processing executed by an optimum trunk route determination unit of a partial route planning unit of the mobile device of the present disclosure;
  • FIG. 5 is a diagram illustrating a specific example of processing executed by an optimum trunk path determination unit;
  • FIG. 11 is a diagram illustrating an example in which a higher cost is calculated as the lateral tilt of the body portion (main body portion) of the robot increases.
  • FIG. 11 is a diagram illustrating an example in which a higher cost is calculated as the lateral tilt of the body portion (main body portion) of the robot increases.
  • FIG. 11 is a diagram illustrating an example in which a higher cost is calculated as the lateral tilt of the body portion (main body portion) of the robot increases.
  • FIG. 11 is a diagram illustrating an example in which a higher cost is calculated as the lateral tilt of the body portion (main body portion) of the robot increases.
  • FIG. 4 is a diagram showing a flowchart describing a detailed sequence of optimum partial route determination processing executed by an optimum trunk route determination unit of a partial route planning unit of the mobile device of the present disclosure;
  • FIG. 5 is a diagram illustrating a specific example of processing executed by an optimum trunk path determination unit;
  • FIG. 4 is a diagram showing a flowchart describing a detailed sequence of optimum partial route determination processing executed by an optimum trunk route determination unit of a partial route planning unit of the mobile device of the present disclosure
  • FIG. 4 is a diagram showing a flowchart describing a detailed sequence of optimum partial route determination processing executed by an optimum trunk route determination unit of a partial route planning unit of the mobile device of the present disclosure
  • FIG. 4 is a diagram showing a flowchart describing a detailed sequence of optimum partial route determination processing executed by an optimum trunk route determination unit of a partial route planning unit of the mobile device of the present disclosure
  • FIG. 5 is a diagram illustrating a specific example of processing executed by an optimum trunk path determination unit;
  • FIG. 5 is a diagram illustrating a specific example of processing executed by an optimum trunk path determination unit
  • FIG. 5 is a diagram illustrating a specific example of processing executed by an optimum trunk path determination unit;
  • FIG. 5 is a diagram illustrating a specific example of processing executed by an optimum trunk path determination unit;
  • FIG. 5 is a diagram illustrating a specific example of processing executed by an optimum trunk path determination unit;
  • FIG. 5 is a diagram illustrating a specific example of processing executed by an optimum trunk path determination unit;
  • FIG. 5 is a diagram illustrating a specific example of processing executed by an optimum trunk path determination unit;
  • FIG. 4 is a diagram showing a flowchart describing a detailed sequence of optimum partial route determination processing executed by an optimum trunk route determination unit of a partial route planning unit of the mobile device of the present disclosure;
  • FIG. 4 is a diagram showing a flowchart describing a detailed sequence of optimum partial route determination processing executed by an optimum trunk route determination unit of a partial route planning unit of the mobile device of the present disclosure;
  • FIG. 4 is a diagram showing a flowchart
  • FIG. 4 is a diagram showing a flowchart describing a detailed sequence of optimum partial route determination processing executed by an optimum trunk route determination unit of a partial route planning unit of the mobile device of the present disclosure
  • FIG. 4 is a diagram showing a flowchart describing a detailed sequence of optimum partial route determination processing executed by an optimum trunk route determination unit of a partial route planning unit of the mobile device of the present disclosure
  • It is a figure explaining the structural example of a hexapod-type robot.
  • FIG. 4 is a diagram showing a flowchart describing a detailed sequence of optimum partial route determination processing executed by an optimum trunk route determination unit of a partial route planning unit of the mobile device of the present disclosure
  • FIG. 4 is a diagram showing a flowchart describing a detailed sequence of optimum partial route determination processing executed by an optimum trunk route determination unit of a partial route planning unit of the mobile device of the present disclosure
  • FIG. 4 is a diagram showing a flowchart describing a detailed sequence of optimum partial
  • FIG. 4 is a diagram showing a flowchart describing a detailed sequence of optimum partial route determination processing executed by an optimum trunk route determination unit of a partial route planning unit of the mobile device of the present disclosure
  • FIG. 4 is a diagram showing a flowchart describing a detailed sequence of optimum partial route determination processing executed by an optimum trunk route determination unit of a partial route planning unit of the mobile device of the present disclosure
  • It is a figure explaining the structural example of a four-legged robot.
  • FIG. 5 is a diagram illustrating a specific example of processing executed by an optimum trunk path determination unit
  • FIG. 4 is a diagram illustrating an example of robot control controlled by data received from a controller operated by a user; It is a figure explaining the hardware configuration example of the mobile apparatus of this indication.
  • FIG. 1 is a diagram showing a four-legged robot 10 that is an example of the mobile device of the present disclosure.
  • the robot 10 is a walking robot that moves by individually driving four legs.
  • the four-legged robot shown in FIG. 1 is an example of the mobile device of the present disclosure, and the processing of the present disclosure can be used in a walking robot having two or more legs.
  • the quadrupedal robot 10 has a body (main body) 11, legs 12, grounding portions 13, and a visual sensor (camera) 15 for recognizing the surrounding environment of the robot. Also, each leg has a joint portion 21 and a joint portion 22 .
  • the robot 10 shown in FIG. 1 is a four-legged walking robot having two legs in front and two legs in back.
  • robot 10 has visual sensor 15 .
  • the visual sensor 15 is a sensor for acquiring environmental information such as obstacles around the robot 10 and the shape of the running surface. For example, it is configured by a camera.
  • the visual sensor 15 is, for example, a monocular camera, a stereo camera, an omnidirectional camera, an infrared camera, a LiDAR (Light Detection and Ranging), a TOF (Time of Flight) sensor, or a combination thereof. It is possible. Both LiDAR and TOF sensors are sensors capable of measuring object distances.
  • FIG. 2 shows the robot 10 shown in FIG. (a) Front view (b) Side view (c) Rear view These three views are shown.
  • each leg 12 with respect to the body (main body) 11 that is, the joint 21 of the leg-mounted part, can be seen from the front and rear of the robot. It has the structure which can be rotated in the left-right direction.
  • the joints 21 at the upper ends of the legs 12 are configured to be rotatable in the front-rear direction as well. In this manner, the joints 21 at the upper ends of the legs 12 are configured to be rotatable in both the front-rear direction and the left-right direction with respect to the body portion (body portion) 11 .
  • each leg is provided with a joint 22 corresponding to a so-called knee joint at the intermediate portion.
  • the joint portion 22 can rotate the lower half of the leg portion 12 in the front-rear direction with respect to the upper leg portion.
  • FIG. 3 is a diagram showing an example of an environment in which the robot 10 shown in FIG. 1 runs.
  • the environment in which the robot 10 moves includes, for example, various obstacles as shown in FIG. , vary.
  • the robot 10 may overturn, resulting in damage or failure.
  • the legs are placed on an uneven surface, a stepped surface, or a sloped surface without considering the step or the angle of inclination, the robot 10 may fall, resulting in damage or failure. be.
  • a map of the surrounding environment generated based on the sensor acquisition information acquired by the visual sensor 15 such as a camera, that is, the three-dimensional shape information of the object including the running surface around the robot.
  • a process of determining a travel route is performed using the environment map possessed by the vehicle.
  • the travel route is basically generated, for example, as a route connecting the shortest distance to a predetermined goal point.
  • the route will be modified to avoid it.
  • the walking robot 10 is a low obstacle, for example, it is possible to lift its legs and climb over the obstacle to advance. may proceed as is.
  • the shortest route is "route 1" indicated by an arrow in the figure, that is, from the current position of the robot 10 to the destination (goal) in a straight line. is the route.
  • a stepped obstacle 30 exists on the right side of the path 1 in the direction in which the robot 10 travels.
  • This step obstacle 30 has a height that can be climbed by raising one leg of the robot 10 . Therefore, the robot 10 may travel along the route 1 based on the determination that the stepped obstacle 30 can be overcome.
  • FIG. 5 shows a state in which the right leg of the robot 10 is grounded on the stepped obstacle 30 and the front and rear legs of the left side of the robot 10 are grounded on a low running surface.
  • the body portion (main body portion) 11 tilts in the horizontal direction and becomes unstable. Furthermore, the range of motion of the right leg of the robot 10 grounded on the stepped obstacle 30 becomes extremely small. That is, it becomes impossible to move the leg forward and backward. As a result, the traveling speed is extremely slowed down.
  • the walking robot 10 can run on a running surface with some steps, but unfavorable situations such as a decrease in stability and a decrease in running speed occur. In such a case, for example, it is preferable to select "Route 2" as shown in FIG.
  • the robot 10 can travel at high speed without tilting the body 11 to the left or right and without reducing the movable range of the legs. As a result, the destination (goal) can be reached quickly and safely.
  • the present disclosure calculates costs for a plurality of routes that a robot, which is a mobile device, can travel.
  • the cost is a cost that has a smaller value as stable driving is possible and a larger value as unstable driving increases.
  • costs are calculated for a plurality of routes that a robot, which is a mobile device, can travel, and evaluation processing for each route is executed. Furthermore, based on the result of this evaluation process, the route with the lowest cost, that is, the route that allows stable running is selected, and the selected route is traveled. Specific examples of the present disclosure will be described below.
  • FIG. 8 is a diagram showing a configuration example of the robot 10 that is the mobile device of the present disclosure.
  • the robot 10 has an environment information analysis section 110 , a global route planning section 120 , a partial route planning section 130 , a mobile device control section 140 and a driving section 150 .
  • the environment information analysis unit 110 has a sensor 111 and an environment map generation unit 112 .
  • the global route planner 120 has a global route generator 121 and a desired body state generator 122 .
  • the partial route planning unit 130 has an optimum trunk route determining unit 131 and a route following target speed calculating unit 132 .
  • the mobile device control unit 140 has a drive information generation unit 141 .
  • the environmental information analysis unit 110 integrates the self-position and recognition information of the robot 10 to generate a map of the robot's surroundings.
  • the sensor 111 corresponds to the visual sensor (camera) 15 described above with reference to FIG. Acquire images and distance information for identifying
  • the sensor 111 can be, for example, a monocular camera, a stereo camera, an omnidirectional camera, an infrared camera, a LiDAR (Light Detection and Ranging), a TOF (Time of Flight) sensor, or a combination thereof.
  • LiDAR Light Detection and Ranging
  • TOF Time of Flight
  • the environment map generation unit 112 uses environment information around the robot 10 from the sensor 111 to perform self-location calculation processing, environment map creation processing, and the like.
  • SLAM Simultaneous localization and mapping
  • SLAM is known as a representative process for creating an environment map and estimating a self-location.
  • SLAM is a process of executing self-position estimation processing (localization) and environment map creation processing (mapping) in parallel using, for example, information acquired by a sensor such as a camera, such as a photographed image of the robot's running environment.
  • the three-dimensional positions of the feature points can be estimated, and a map that enables the position of objects around the robot to be grasped, a so-called environment map ( mapping), and also the self-position (robot position) can be estimated (localization).
  • mapping environment map
  • robot position self-position
  • SLAM is not limited to the visual SLAM that uses camera-captured images as described above, and there are various other methods.
  • LiDAR SLAM that uses LiDAR (Light Detection and Ranging), which is a sensor that measures the distance to an obstacle by laser light.
  • the environment map generation unit 112 calculates the self-position of the robot 10 using the detection information of the sensor 111 and generates an environment map, which is a peripheral map of the robot.
  • the environment map generated by the environment map generation unit 112 and self-location information are used by the target body state generation unit 122 of the global route planning unit 120, the optimal body route determination unit 131 of the partial route planning unit 130, and the mobile device control unit 140. It is output to the drive information generator 141 .
  • the global route planning unit 120 generates a global route from the start position of the robot 10 to the final goal position.
  • the global route planning unit 120 also generates the robot passage estimated time at each position of the global route from the start position of the robot 10 to the final goal position.
  • the partial route planning unit 130 generates a partial route by correcting the global route from the start position of the robot 10 generated by the global route planning unit 120 to the final goal position for each partial route. do.
  • the partial route planning unit 130 for example, for a part of the route within the range where the obstacles and the running surface shape can be analyzed by the sensor 111 of the robot 10, is a route that avoids obstacles and steps as much as possible and can be traveled safely. Corrected partial paths are generated sequentially.
  • FIG. 9 shows the route from the start position (S) of the robot 10 to the goal position (G).
  • the route indicated by the solid line in FIG. 9 is the global route. That is, it is a global route from the start position (S) of the robot 10 to the final goal position (G).
  • a route indicated by a dotted line in FIG. 9 is a partial route. The partial paths are generated sequentially as the robot 10 moves.
  • the global route planning unit 120 generates a global route that is the shortest route from the start position (S) of the robot 10 to the final goal position (G).
  • the partial route planning unit 130 sequentially generates a safe travel route avoiding obstacles and steps as much as possible within a range where the sensor 111 can analyze the obstacles and the running surface shape.
  • the partial route planning unit 130 When the robot 10 travels along the global route generated by the global route planning unit 120, there is a possibility that the robot 10 will run over the stepped obstacle shown in FIG.
  • the partial route planning unit 130 generates partial routes for avoiding such situations.
  • the partial route planning unit 130 When the robot 10 is at the start position (S) shown in FIG. 9, the range in which the sensor 111 of the robot 10 can analyze the obstacles and the shape of the running surface is from the start position (S) shown in FIG. The range is up to First, the partial route planning unit 130 generates a partial route indicated by a dotted line by modifying the partial route (S to SG1) so as to avoid obstacles and steps as much as possible so that the vehicle can travel safely. The robot 10 travels along the partial route indicated by the dotted line. By traveling along the route indicated by the dotted line, the robot 10 can travel stably without running over a step obstacle and leaning left or right.
  • the robot 10 travels stably from the start position (S) to subgoal 1 (SG1) according to the partial route indicated by the dotted line generated by the partial route planning unit.
  • a partial route (SG1 to SG2) indicated by a dotted line is generated for a section of the partial route (SG1 to SG2) in which obstacles and the running surface shape can be analyzed by the sensor 111 of the robot .
  • This partial route (SG1 to SG2) is also a partial route that allows the vehicle to travel safely while avoiding obstacles and steps as much as possible.
  • the partial route planning unit 130 sequentially generates partial routes by modifying the global routes generated by the global route planning unit 120 . That is, within a range where the sensor 111 can analyze the obstacles and the shape of the running surface, a safe route is sequentially generated while avoiding obstacles and steps as much as possible.
  • the global path planner 120 has the global path generator 121 and the desired torso state generator 122 .
  • the partial route planning unit 130 has an optimum trunk route determining unit 131 and a route following target speed calculating unit 132 . Processing of each of these components will be described.
  • the global route generator 121 of the global route planner 120 generates a global route for the robot 10 to move from the start position to the goal position. Further, the global route generation unit 121 generates the robot passage estimated time at each position of the global route from the start position of the robot 10 to the final goal position. The global path generated by global path generation section 121 is output to desired body state generation section 122 .
  • the target torso state generation unit 122 calculates the torso state of the robot at each position when traveling on the global route generated by the global route generation unit 121 .
  • the body state is three-dimensional data of the horizontal position (x, y) of the robot 10 and the angle ( ⁇ ) indicating the orientation of the robot 10, that is, (x, y, ⁇ ).
  • the trunk of the robot 10 is the trunk (main body) 11 shown in FIG.
  • a user-definable body of the robot is used as an example of a reference point for determining the travel path of the robot 10 .
  • the reference point is defined as a point of specific coordinates on one user-specifiable link coordinate system of the robot 10, and the reference point does not necessarily have to be a point on the body. Alternatively, it may be a point in the air outside the robot.
  • the torso state is data indicating both the position and orientation of the torso (main body) 11 shown in FIG.
  • this body state is, for example, the body state at each position (x, y) on the global path indicated by the solid line in FIG. Calculate the body state (x, y, ⁇ ).
  • the fuselage state (x, y, ⁇ ) at each position (x, y) on the global path generated by the target fuselage state generation unit 122 is combined with the global path information generated by the global path generation unit 121 and the partial path planning unit 130 It is output to the optimum trunk path determination unit 131 .
  • the optimum trunk route determining unit 131 of the partial route planning unit 130 inputs the following information. (a) the self-position of the robot 10 and the environmental map generated by the environmental information analysis unit 110; (b) global route information generated by the global route planning unit 120 and body state (x, y, ⁇ ) information at each position (x, y) on the global route;
  • the optimum trunk route determination unit 131 corrects the global route information generated by the global route planning unit 120 to create a partial route that can safely travel while avoiding obstacles and steps as much as possible. Routes (partial routes) are generated sequentially.
  • the optimum body route determining unit 131 allows the sensor 111 to avoid obstacles and bumps as much as possible in the range where the sensor 111 can analyze the obstacles and the shape of the running surface. Possible partial paths (partial paths indicated by dotted lines in FIG. 9) are generated sequentially.
  • the optimal body route determination unit 131 of the partial route planning unit 130 sets a plurality of routes along which the robot 10 can travel within the determined section of the partial route, and calculates the cost for each route.
  • the cost is such that the more stable driving is possible, the lower the value, and the greater the unstable driving, the higher the value.
  • the optimal trunk route determination unit 131 calculates costs for a plurality of routes that the robot 10 can travel, and executes evaluation processing for each route. Furthermore, based on the result of this evaluation process, the route with the lowest cost, that is, the route that allows stable running is selected, and the selected route is traveled. A detailed sequence of this processing will be described later.
  • the optimal trunk path determination unit 131 of the partial route planning unit 130 determines the trunk state (x, y, ⁇ ) generated by the target trunk state generation unit 122 at each end point position of each partial route, for example, each subgoal position shown in FIG. Generate subpaths so that
  • the partial route generated by the optimum trunk route determination unit 131 is output to the route following target speed calculation unit 132 .
  • the route following target speed calculator 132 generates a target speed for the robot 10 to travel along the partial route generated by the optimal trunk route determiner 131 .
  • the global route generating unit 121 of the global route planning unit 120 generates the robot passage estimated time at each position of the global route from the start position of the robot 10 to the final goal position.
  • the route-following target speed calculation unit 132 sets the travel target speed of the robot 10 so that the passage time of the robot 10 at each subgoal position matches the expected robot passage time at each position on the global route calculated by the global route generation unit 121 as much as possible. Generate velocity.
  • the partial route generated by the optimal body route determining unit 131 of the partial route planning unit 130 and the target speed information of the robot 10 calculated by the route following target speed calculating unit 132 are output to the drive information generating unit 141 of the moving device control unit 140. be done.
  • the drive information generator 141 of the mobile device controller 140 receives the following information. (a) the self-position of the robot 10 and the environmental map generated by the environmental information analysis unit 110; (b) partial route generated by the partial route planning unit 130 and target speed information of the robot 10; The drive information generation unit 141 of the mobile device control unit 140 generates drive information for causing the robot 10 to travel along the partial route at the target speed based on these pieces of input information, and outputs the drive information to the drive unit 150 .
  • the drive information is, for example, the drive information of the motors of the joints of the robot 10 .
  • the driving information generating unit 141 of the moving device control unit 140 calculates the torque input of the joints of the robot 10 and determines the output to be applied to the actuators constituting the driving unit 150 .
  • the drive information generation unit 141 of the mobile device control unit 140 uses the environment map around the robot 10 generated by the environment information analysis unit 110 in order to generate control information including the ground contact positions of the legs of the robot 10 .
  • the output from the drive information generation unit 141 of the moving device control unit 140 to the drive unit 150 can take various forms, such as torque, voltage, angular velocity, and position.
  • the drive unit 150 is, for example, a motor for each joint of the robot 10, and is driven by the drive information generated by the drive information generation unit 141 of the moving device control unit 140.
  • travel processing is performed according to the generated partial route. That is, within a range where the sensor 111 can analyze the obstacles and the shape of the running surface, partial routes that can be safely traveled while avoiding obstacles and steps as much as possible are sequentially generated, and the vehicle travels along the generated partial routes. to achieve stable running.
  • the optimal trunk route determination unit 131 of the partial route planning unit 130 performs the partial route generation processing indicated by the dotted line in FIG. 9, as described above with reference to FIG. In other words, a partial route indicated by a dotted line is generated by avoiding obstacles and steps as much as possible and making it possible to travel safely.
  • the optimal trunk route determination unit 131 of the partial route planning unit 130 sets a plurality of partial route candidates that the robot 10 can travel within the partial route decision section, and calculates the cost for each partial route candidate.
  • the cost is such that the more stable driving is possible, the lower the value, and the greater the unstable driving, the higher the value.
  • the optimal trunk route determining unit 131 of the partial route planning unit 130 calculates costs for a plurality of partial route candidates that the robot 10 can travel, and executes evaluation processing for each route candidate. Furthermore, based on the result of this evaluation process, the route with the lowest cost, that is, the route that allows stable running is selected, and the selected route is traveled.
  • Example 1 is an example in which cost calculation processing is performed by applying a cost calculation algorithm in which the cost increases as the magnitude of the horizontal tilt of the body portion (body portion) 11 of the robot 10 increases. explain.
  • FIG. 10 is a flowchart for explaining the detailed sequence of the optimum partial route determination process executed by the optimum trunk route determination unit 131 of the partial route planner 130.
  • the processing according to this flow can be executed by an information processing device in the robot 10 or an information processing device capable of communicating with a moving object such as a robot according to a program stored in a storage unit of the information processing device. be. For example, it can be performed as program execution processing by a processor such as a CPU having a program execution function. Processing in each step of the flow shown in FIG. 10 will be described below.
  • Step S101 First, the optimal trunk route determining unit 131 of the partial route planning unit 130 generates a plurality of trunk route (path) candidates in the optimum route setting target section.
  • step S101 A specific example of the processing of step S101 will be described with reference to FIG. FIG. 11 shows the optimal route setting target section (SGx to SGy).
  • the optimal body route determination unit 131 of the partial route planning unit 130 detects obstacles using a partial section in the global route generated by the global route planning unit 120, that is, the sensors 111. And within the range where the shape of the running surface can be analyzed, partial routes that can be safely run while avoiding obstacles and steps as much as possible are sequentially generated. For example, an optimum partial route is generated for each range unit, such as the section from the start position (S) shown in FIG.
  • the optimal route setting target section (SGx to SGy) shown in FIG. 11 is, for example, the section from the start position (S) to subgoal 1 (SG1) shown in FIG. It corresponds to a section or the like, and is a partial section in which the sensor 111 of the robot 10 can analyze obstacles and the running surface shape.
  • the optimum trunk route determining unit 131 of the partial route planning unit 130 generates a plurality of trunk route (path) candidates in the optimum route setting target section (SGx to SGy) shown in FIG. 11, for example.
  • FIG. 11 shows an example in which six route candidates from route candidate 1 to route candidate 6 are set as trunk route (path) candidates.
  • Route candidate 1 is the shortest route of the optimum route setting target section (SGx to SGy), and route candidates 2 to 6 show an example in which a plurality of detour routes are set.
  • the number of route candidates to be set can be defined in advance.
  • the user may sequentially set.
  • the routes of route candidates to be set can also be set according to a predetermined algorithm, and may be configured to be sequentially set by the user.
  • Step S102 Next, in step S102, the following processing is executed as initialization processing.
  • Set the best path cost (best_path_cost) to infinity ( ⁇ ). i.e. Best path cost (best_path_cost) ⁇ + ⁇ Make the above initial settings.
  • Step S103 a route whose cost has not been calculated is selected, and the selected route is set to (path). For example, an unprocessed route candidate is selected as a processing target from six route candidates 1 to 6 set in the optimal route setting target section (SGx to SGy) shown in FIG. Initially, for example, route candidate 1 is selected.
  • Step S104 Next, in step S104, the route cost (path_cost) of the selected route (path) selected as the processing target in step S103 is calculated.
  • This route cost is a cost that has a smaller value as stable travel is possible and a larger value as unstable travel increases.
  • the cost value in such a state is calculated as a large value. Details of the cost calculation process will be described later.
  • both the front and rear legs on the left side of the robot 10 touch the ground at a position higher than the landing positions of the front and rear legs on the right side. also have different states.
  • FIG. 13 is a diagram showing another example that is determined to be high cost.
  • the two examples shown in FIG. 13 are an example in which only one of the left and right front legs of the robot 10 has a high contact position, and an example in which only one of the left and right rear legs of the robot 10 has a contact position at a high position. is. Even in such a case, the torso (body) 11 of the robot 10 tilts to the left and right, and the stability of the robot 10 decreases. Therefore, the cost value in such a state is calculated as a large value. .
  • both the left and right front legs of the robot 10 are grounded on the same height surface.
  • the rear legs of the robot 10 are also grounded on the left and right surfaces at the same height.
  • the body portion (body portion) 11 of the robot 10 tilts forward and backward, but tilts left and right does not occur. Therefore, the calculated cost value for such a situation is also low.
  • the cost calculation process is performed by applying a cost calculation algorithm in which the cost increases as the lateral tilt of the torso (main body) 11 of the robot 10 increases.
  • cost calculation algorithm By changing the cost calculation algorithm to be used, it is possible to calculate different values of costs according to various robot states.
  • step S104 the path cost (path_cost) of the selected path (path) selected as the processing target in step S103 is calculated. Details of the cost calculation process in step S104 will be described later with reference to FIG. 16 and subsequent figures.
  • Step S105 When the cost (path_cost) calculation process of one selected path (path) is completed in step S104, the process of step S105 is executed next.
  • step S105 the optimal trunk route determination unit 131 of the partial route planning unit 130 compares the cost (path_cost) of the selected route (path) whose cost (path_cost) was calculated in step S104 with the optimal route cost (best_path_cost). process.
  • the best path cost (best_path_cost) to be compared is the best path cost (best_path_cost) initially set in step S102 or the best path cost (best_path_cost) corresponding to the already calculated path candidate.
  • step S104 The value of the cost (path_cost) of the selected route (path) for which the cost (path_cost) was calculated in step S104 is smaller than the value of the optimum route cost (best_path_cost), that is, (path_cost) ⁇ (best_path_cost) If the comparison judgment formula is satisfied, the process proceeds to step S106. On the other hand, if the comparison judgment formula does not hold, the process proceeds to step S107.
  • Step S106 The processing of step S106 is executed when a determination of yes is made in step S105.
  • step S104 the value of the cost (path_cost) of the selected route (path) for which the cost (path_cost) was calculated in step S104 is smaller than the value of the optimum route cost (best_path_cost). (path_cost) ⁇ (best_path_cost) If the comparison determination formula is satisfied, the process of step S106 is executed.
  • the optimum trunk route determining unit 131 of the partial route planning unit 130 executes the following processing in step S106.
  • the selected route (path) for which the cost (path_cost) is calculated is set as the optimum route (best_path).
  • Optimal path cost (best_path_cost) ⁇ Calculated path cost (path_cost) Optimal path (best_path) ⁇ Selected path (path)
  • Step S107 In the process of step S107, when the data replacement process in step S106 is completed, Alternatively, in step S105, (path_cost) ⁇ (best_path_cost) Executed when the above comparison judgment expression is not satisfied.
  • the optimal trunk route determination unit 131 of the partial route planning unit 130 determines in step S107 whether or not the cost calculation process for all route candidates has been completed.
  • step S107 If there is an unprocessed route candidate, the determination in step S107 becomes No, the process returns to step S103, an unprocessed route candidate is selected, and the processing from step S104 onward is executed for the selected unprocessed candidate route.
  • step S107 If it is determined in step S107 that the cost calculation process for all route candidates has been completed, the process proceeds to step S108.
  • Step S108 When it is determined in step S107 that the cost calculation process for all route candidates has been completed, the process of step S108 is executed.
  • step S108 the optimal trunk path determining section 131 of the partial path planning section 130 outputs the optimal path (best_path) determined at this point to the path following target speed calculating section 132 in the next stage.
  • the output best path is the path with the lowest cost. In other words, this is the path in which the torso portion (body portion) 11 of the robot 10 is tilted to the left or right at the least.
  • This selected route information is output to the drive information generator 141 of the mobile device controller 140 via the route following target speed calculator 132 in the next stage.
  • the drive information generation unit 141 of the mobile device control unit 140 generates control information for driving the robot 10 along the best path (best_path) and controls the drive unit 150 .
  • the robot 10 runs along the best path (best_path). In other words, the robot 10 can travel along the optimum route that can be traveled with the smallest lateral tilt of the torso (main body) 11 among the plurality of route candidates.
  • FIG. 16 shows a detailed sequence of the process (step S104) described with reference to the flowchart of FIG. 10, that is, the process of calculating the path cost (path_cost) of the selected path (path) selected in step S103. Description will be made with reference to flowcharts.
  • path cost (path_cost) calculation process of the selected path (path) is executed according to the processes of steps S201 to S205 of the flow shown in FIG. The processing of each step will be described below in order.
  • Step S201 is an initial setting process. Make the following initial settings.
  • the initial value of the path cost (path_cost) of the selected path (path) selected for cost calculation is set to 0,
  • Cost calculation body position (i) ⁇ 0 Make these initial settings.
  • FIG. 17 shows route candidates 1 to 6 described above with reference to FIG. Route candidates 1-3, which are part of candidates 1-6, are shown.
  • step S103 of the flow shown in FIG. 10 one route candidate is sequentially selected from route candidates 1 to 6 as a cost calculation target, and step S104, that is, the processing of steps S201 to S205 of the flow shown in FIG. A process of calculating the cost of the selected route is executed.
  • Route candidates selected for cost calculation are set in order of route candidates 1, 2, 3, 4, 5, and 6, for example.
  • route candidate 1 is selected as a cost calculation target
  • step S201 The initial value of the path cost 1 (path1_cost) of the selected path 1 (path1) selected for cost calculation is set to 0,
  • a process of setting the initial value of the cost calculation torso position (i (i 0 to N)) to 0 is performed.
  • route candidate 1, route candidate 2, and route candidate 3 are routes that connect the optimal route setting target sections (SGx to SGy), but they have different lengths. there is Therefore, the number of sampling points for each route candidate is also different. The longer the route, the greater the number of sampling points, and the resulting calculated route cost tends to increase.
  • Step S202 is a step of calculating the torso state cost at one sampling point (cost calculation torso position (i)) of the selected path (path).
  • the body state cost calculation value (calc_pose_cost (body_pos[i])) at the cost calculation body position (i) of the selected path (path) is calculated, and the body state cost (pose_cost) is calculated as the calculated value (calc_pose_cost (body_pos[i] )).
  • step S202 the cost (body state cost) calculation process at one sampling point (i) of one selected path (path) is executed.
  • the detailed sequence of this cost calculation process will be described later with reference to the flowcharts of FIG. 18 and subsequent figures.
  • Step S203 When the cost (body state cost) calculation process at one sampling point (i) of one selected path (path) is completed in step S202, the process of step S203 is executed next.
  • step S203 the route cost (path_cost) of the selected route (path) is updated. That is, the processing for updating the path cost (path_cost) by adding the trunk state cost (pose_cost) calculated in step S202 to the path cost (path_cost) is executed.
  • Path cost (path_cost) ⁇ Path cost (path_cost) + Body state cost (pose_cost)
  • path cost (path_cost) update process is executed.
  • step S204 update processing of sampling points (cost calculation body position (i)) is executed.
  • i ⁇ i+1 the sampling point (cost calculation trunk position (i)) is advanced by one.
  • Step S205 the value of the sampling point (cost calculation trunk position (i)) updated by the update processing of the sampling point (cost calculation trunk position (i)) in step S204 is the number of sampling points ( N) Determine whether the value is smaller than N). i.e. i ⁇ N? It is determined whether or not the above determination formula is established.
  • step S202 determines whether there is a sampling point (cost calculation torso position (i)) for which cost calculation processing has not yet been performed. In this case, the process returns to step S202 to execute calculation processing of the trunk state cost (pose_cost) for the updated sampling point (cost calculation trunk position (i)). That is, the processing from step S202 is executed.
  • step S202 in the flowchart of FIG. 16, that is, the calculation process of the body state cost (pose_cost) at the sampling point (cost calculation body position (i)) will be described with reference to FIG. 18 and the following figures. do.
  • FIGS. 18 to 20 are flowcharts for explaining the detailed sequence of the process of step S202 in the flowchart of FIG.
  • Calculation processing of the torso state cost (pose_cost) at one sampling point (cost calculation torso position (i)) of one selected path (path) selected as a cost calculation target is performed in steps S301 to S301 shown in FIGS. It is executed according to the processing of S317. The processing of each step will be described below in order.
  • Step S301 is preparatory processing before starting the processing loop of steps S302 to S306.
  • step S301 first, the minimum height (min_heights) and maximum height (max_heights) of one leg of the robot are set as variables. i.e. (min_heights) ⁇ (array of 4 elements) (max_heights) ⁇ (4-element array) As above, the minimum height (min_heights) and maximum height (max_heights) of one leg of the robot 10 are set as variables.
  • Step S302 Steps S302 to S306 form a processing loop for processing for each leg of the robot .
  • the robot 10 has four legs.
  • step S303 In step S303, the following process is executed for the selected leg (k) to be processed.
  • the leg placement possible area of the leg (k) is extracted.
  • FIG. 21 shows the robot 10.
  • the robot 10 is at the cost calculation body position (i). In this position, the robot 10 can move each leg within its range of motion and touch the ground.
  • the movable range of each leg that is, the groundable area is indicated by a circular solid line. The robot 10 can freely move and ground each leg within the groundable area.
  • the left front leg is leg (1)
  • the right front leg is leg (2)
  • the left rear leg is leg (3)
  • the left rear leg is leg (4)
  • the left front leg (leg (1)) is The leg (1) shown in Fig. 1 can be freely moved and grounded within the grounding possible area.
  • the right front leg (leg (2)) can be freely moved and grounded within the ground-contactable area around leg (2) shown in the figure.
  • the left rear leg (leg (3)) and the left rear leg (leg (4)) can also be freely moved within the ground contactable area around each leg and grounded.
  • step S303 for the selected leg (k) to be processed, the leg arrangement possible region at the cost calculation body position (i) is extracted. That is, for one leg (k) selected from the four legs shown in FIG. 21, a circular solid line around it is extracted.
  • Steps S304-S305 Next, in steps S304 and S305, the following processing is executed for the leg (k) selected as the processing target.
  • Step S304 Cost Calculation Calculate the minimum leg placement height (min_heights(k)) within the leg placement possible area of leg (k) at body position (i).
  • Step S305 Cost Calculation Calculate the maximum leg placement height (max_heights(k)) within the leg placement possible area of leg (k) at body position (i).
  • FIG. 22 shows a processing example when the leg (k) selected as the processing target is the right front leg (leg (2)).
  • the leg arrangement area indicated by a circular solid line is shown.
  • FIG. 22 Upper left (a) and lower right (b) shown in FIG. 22 are diagrams showing different ground contact states of the right front leg (leg (2)).
  • (a) shows a state in which the right front leg (leg (2)) does not run over a stepped obstacle within the leg arrangement area and is grounded on a low plane.
  • (b) shows a state in which the right front leg (leg (2)) runs over a stepped obstacle in the leg arrangement area and touches the upper surface of the stepped obstacle.
  • leg (2) Within the area where the leg can be placed indicated by the circular solid line around the right front leg (leg (2)), there are many running surfaces with different leg contact heights.
  • Step S304 is a process of calculating the minimum leg placement height (min_heights(k)) within the leg placement possible area of leg (k). As shown in (a), processing is performed to calculate the grounding height of the leg (2) in a state where it does not ride on the step obstacle as the minimum leg placement height (min_heights(k)).
  • Step S305 is a process of calculating the maximum leg placement height (max_heights(k)) within the leg placement possible area of leg (k).
  • max_heights(k) the maximum leg placement height within the leg placement possible area of leg (k).
  • Processing is performed to calculate the grounding height of the leg (2) on the stepped obstacle shown in FIG. 22(b) as the maximum leg arrangement height (min_heights(k)).
  • steps S304 and S305 the minimum leg placement height (min_heights(k)) and the maximum leg placement height (max_heights ( k)) is calculated.
  • steps S302 to S306 form a processing loop for processing for each leg of the robot .
  • the robot 10 has four legs, and when the processing loop of steps S302-S306 is completed, the following data (1)-(4) are calculated.
  • the cost calculation fuselage position (i) (1) Minimum leg placement height (min_heights(1)) and maximum leg placement height (max_heights(1)) of left front leg (leg (1)), (2) Minimum leg placement height (min_heights(2)) and maximum leg placement height (max_heights(2)) of the right front leg (leg (2)), (3) The minimum leg placement height (min_heights(3)) and the maximum leg placement height (max_heights(3)) of the left hind leg (leg (3)), (4) Minimum leg placement height (min_heights(4)) and maximum leg placement height (max_heights(4)) of the right hind leg (leg (4)),
  • Step S311 is preparatory processing before executing the processing loop of steps S312 to S317.
  • step S311 the worst body state cost (worst_pose_cost) is set to minus infinity (- ⁇ ).
  • Worst torso state cost (worst_pose_cost) ⁇ - ⁇ Before executing the processing loop of steps S312 to S317, the above cost initial setting processing is performed.
  • Step S312 Steps S312 to S317 form a processing loop for processing for each leg arrangement state of the robot .
  • the robot 10 has four legs, and the following data are calculated in steps S302 to S306, which are the previous processing loop for each leg.
  • the cost calculation fuselage position i
  • Minimum leg placement height min_heights(1)
  • maximum leg placement height max_heights(1)
  • maximum leg placement height max_heights(2)
  • maximum leg placement height max_heights(2)
  • FIG. 23 shows these 16 examples.
  • the table shown in FIG. 16 is a table showing 16 non-combination patterns of grounding positions (Low, High) of left front leg (FL), right front leg (FR), left rear leg (BL), and right rear leg (FL).
  • Low and High correspond to "minimum leg placement height (min_heights(k))” and “maximum leg placement height (max_heights(k))” calculated in the processing loop of steps S302 to S307. i.e. Low is “minimum leg placement height (min_heights(k))” High is “maximum leg placement height (max_heights(k))” is.
  • the first entry 1 (Low, Low, Low, Low) shown in the table of FIG. 23 indicates that all the four legs of the robot 10 are on the stepped obstacle as shown in the diagram (1) on the right side of FIG. Corresponds to the state of contact with a low running surface.
  • Entry 4 (Low, High, Low, Low) shown in the table of FIG. 23 indicates that only the right front leg of the robot 10 is on a high running surface above a stepped obstacle, as shown in the diagram (2) on the right side of FIG. , and the remaining three legs are in contact with a low running surface without riding on a step obstacle.
  • the state of FIG. 24 (1) on the right side that is, the state in which all four legs of the robot 10 are in contact with a low running surface without riding on a stepped obstacle, is the first entry 1 ( Low, Low, Low, Low). 24, in which only the left front leg of the robot 10 is in contact with the high running surface above the stepped obstacle, and the remaining three legs do not ride on the stepped obstacle and run low.
  • the grounded state corresponds to entry 5 (High, Low, Low, Low) shown in the table of FIG.
  • sampling point cost calculation trunk position (i)
  • leg arrangement possible regions of all the legs are at the same height as shown on the right side of FIG.
  • leg arrangement pattern of entry 1 Low, Low, Low, Low
  • the processing loop of steps S312 to S317 should be executed only for this one leg arrangement pattern instead of 16 patterns.
  • Step S313 Steps S313 to S316 are processing steps that are sequentially executed for any one of the 16 leg arrangement patterns.
  • step S313 the following processing is executed for one leg arrangement pattern to be processed.
  • f_pose be the leg pose state (leg_pose(k)) when leg (k) is the front leg of the robot
  • b_pose be the leg pose state (leg_pose(k)) when leg (k) is the hind leg of the robot.
  • leg(k) is the front leg of the robot ⁇
  • leg(k) is the hind leg of the robot ⁇ Execute the above data definition.
  • step S314 body state cost (pose_cost) calculation processing corresponding to one leg arrangement pattern at one sampling point (cost calculation body position (i)) is executed. Specifically, the following calculation processing of the torso state cost (pose_cost) is executed.
  • Body state cost (pose_cost) (max(f_pose)-min(f_pose)) + (max(b_pose)-min(b_pose))
  • Calculation processing of the torso state cost (pose_cost) is executed according to the above formula.
  • the body state cost (pose_cost) calculated in step S314 increases as the difference in contact height between the left front leg and the right front leg increases, and increases as the difference in contact height between the left rear leg and right rear leg increases. growing.
  • this cost calculation algorithm is a cost calculation algorithm in which the greater the horizontal tilt of the trunk (main body) 11, the higher the cost.
  • Step S315) When the body state cost (pose_cost) calculation process corresponding to one leg arrangement pattern at one sampling point (cost calculation body position (i)) is completed in step S314, the calculated body state cost (pose_cost) is calculated in step S315. and the worst torso state cost (worst_pose_cost).
  • the above determination formula determines whether the torso state cost (pose_cost) corresponding to one leg arrangement pattern calculated in step S314 is greater than the initial set value or the already calculated worst torso state cost (worst_pose_cost). is a judgment formula for judging whether the torso state cost (pose_cost) corresponding to one leg arrangement pattern calculated in step S314 is greater than the initial set value or the already calculated worst torso state cost (worst_pose_cost). is a judgment formula for judging
  • step S315. If it is determined that the torso state cost (pose_cost) corresponding to one leg arrangement pattern calculated in step S314 is greater than the initial set value or the already calculated worst torso state cost (worst_pose_cost), step S315. is YES, and the process proceeds to step S316.
  • step S315 if it is determined that the torso state cost (pose_cost) corresponding to one leg arrangement pattern calculated in step S314 is not larger than the initial set value or the already calculated worst torso state cost (worst_pose_cost), step The determination in S315 is No, and the process proceeds to step S317.
  • Step S316 The process of step S316 is executed when a determination of Yes is made in step S315, that is, when the body state cost (pose_cost) corresponding to one leg arrangement pattern calculated in step S314 is set to the initial setting value or the already calculated worst Executed when it is determined that the value is greater than the torso state cost (worst_pose_cost).
  • step S316 a process of setting the worst torso state cost (worst_pose_cost) to the torso state cost (pose_cost) calculated in step S314 is executed.
  • FIG. 26 shows an example of a leg arrangement pattern that results in the worst torso state cost (worst_pose_cost).
  • the example shown in FIG. 26 is a leg arrangement pattern in which the front and rear legs on the left side of the robot 10 touch the ground at a position higher than the landing positions of the front and rear legs on the right side.
  • the value of the torso state cost (pose_cost) is large, and there is a high possibility that the cost corresponds to the worst torso state cost (worst_pose_cost).
  • Step S317 is the end step of the processing loop of steps S312 to S317.
  • the processing of steps S313 to S316 is executed for a maximum of 16 combination patterns of the contact heights of the four legs of the robot 10, and The torso state cost (pose_cost) is calculated, and the worst torso state cost (worst_pose_cost) with the highest cost among them is calculated as the torso state cost (pose_cost).
  • the process proceeds to step S203 of the flow shown in FIG.
  • step S203 the route cost (path_cost) of the selected route (path) is updated. That is, the torso state cost (pose_cost) calculated in step S202, that is, the torso state cost (pose_cost) corresponding to the worst torso state cost (worst_pose_cost) with the highest cost at one sampling point (cost calculation torso position (i)) is added to the path cost (path_cost) for which the cost is to be calculated to update the path cost (path_cost).
  • the route cost (path_cost) of the selected route (path) is updated. That is, the torso state cost (pose_cost) calculated in step S202, that is, the torso state cost (pose_cost) corresponding to the worst torso state cost (worst_pose_cost) with the highest cost at one sampling point (cost calculation torso position (i)) is added to the path cost (path_cost) for which the cost is to be calculated to update the path cost (path_cost).
  • Path cost (path_cost) ⁇ Path cost (path_cost) + Body state cost (pose_cost)
  • path cost (path_cost) update process is executed.
  • the body state cost (pose_cost) corresponding to the worst body state cost (worst_pose_cost) having the highest cost at all the sampling points (cost calculation body position (i)) set in one selection path is calculated. ) are added together to calculate the path cost (path_cost) of one selected path.
  • steps S201 to S205 shown in FIG. 16 is the processing of step S104 of the flow shown in FIG. to step S105.
  • the best path cost (best_path_cost) to be compared is the best path cost (best_path_cost) initially set in step S102 or the best path cost (best_path_cost) corresponding to the already calculated path candidate.
  • step S104 The value of the cost (path_cost) of the selected route (path) for which the cost (path_cost) was calculated in step S104 is smaller than the value of the optimum route cost (best_path_cost), that is, (path_cost) ⁇ (best_path_cost) If the comparison judgment formula is satisfied, the process proceeds to step S106. On the other hand, if the comparison judgment formula does not hold, the process proceeds to step S107.
  • Step S106 The processing of step S106 is executed when a determination of yes is made in step S105.
  • step S104 the value of the cost (path_cost) of the selected route (path) for which the cost (path_cost) was calculated in step S104 is smaller than the value of the optimum route cost (best_path_cost). (path_cost) ⁇ (best_path_cost) If the comparison determination formula is satisfied, the process of step S106 is executed.
  • the optimum trunk route determining unit 131 of the partial route planning unit 130 executes the following processing in step S106.
  • the selected route (path) for which the cost (path_cost) is calculated is set as the optimum route (best_path).
  • Optimal path cost (best_path_cost) ⁇ Calculated path cost (path_cost) Optimal path (best_path) ⁇ Selected path (path)
  • Step S107 In the process of step S107, when the data replacement process in step S106 is completed, Alternatively, in step S105, (path_cost) ⁇ (best_path_cost) Executed when the above comparison judgment expression is not established.
  • the optimal trunk route determination unit 131 of the partial route planning unit 130 determines in step S107 whether or not the cost calculation process for all route candidates has been completed.
  • step S107 If there is an unprocessed route candidate, the determination in step S107 becomes No, the process returns to step S103, an unprocessed route candidate is selected, and the processing from step S104 onward is executed for the selected unprocessed candidate route.
  • step S107 If it is determined in step S107 that the cost calculation process for all route candidates has been completed, the process proceeds to step S108.
  • Step S108 When it is determined in step S107 that the cost calculation process for all route candidates has been completed, the process of step S108 is executed.
  • step S108 the optimal trunk path determining section 131 of the partial path planning section 130 outputs the optimal path (best_path) determined at this point to the path following target speed calculating section 132 in the next stage.
  • the output best path is the path with the lowest cost. In other words, this is the path in which the torso portion (body portion) 11 of the robot 10 is tilted to the left or right at the least.
  • This selected route information is output to the drive information generator 141 of the mobile device controller 140 via the route following target speed calculator 132 in the next stage.
  • the drive information generation unit 141 of the mobile device control unit 140 generates control information for driving the robot 10 along the best path (best_path) and controls the drive unit 150 .
  • the robot 10 runs along the best path (best_path). In other words, the robot 10 can travel along the optimum route that can be traveled with the smallest lateral tilt of the torso (main body) 11 among the plurality of route candidates.
  • the processing executed by the optimum trunk route determination unit 131 of the partial route planning unit 130 is summarized as follows.
  • the optimal trunk route determination unit 131 of the partial route planning unit 130 first determines a plurality of routes in each section of the partial route generation section, for example, the partial route generation section (SGx to SGy) described above with reference to FIG. Candidates are generated, and a plurality of sampling points (cost calculation body position (i)) are set for each route candidate.
  • the cost corresponding to the leg arrangement pattern with the highest cost among the plurality of possible leg arrangement patterns is calculated as the body state cost (pose_cost).
  • the added value (total) of the body state costs (pose_cost) of all sampling points (cost calculation body position (i)) in the route candidate is calculated as the route cost (pqth_cost).
  • the route with the smallest cost value among the route costs (pqth_cost) of each of the multiple route candidates is selected as the optimum route (best_path).
  • the optimum trunk path determination unit 131 of the partial route planning unit 130 executes such processing to generate the route having the smallest cost value for each partial route generation section as the optimum route (best_path).
  • the generated optimal path (best_path) is the path with the lowest cost, and is the path that can be traveled with the torso (main body) 11 of the robot 10 tilting to the left and right as little as possible. be. With this control, the robot 10 runs along the best path (best_path). In other words, the robot 10 can travel along the optimum route that can be traveled with the smallest lateral tilt of the torso (main body) 11 among the plurality of route candidates.
  • Example 2 Example applying a user-defined cost calculation algorithm
  • cost calculation processing is performed by applying a cost calculation algorithm in which the cost increases as the lateral inclination of the torso (main body) 11 of the robot 10 increases. This is an embodiment for performing
  • Example 2 An example in which a user-defined cost calculation algorithm is applied will be described below as Example 2.
  • the configuration of the robot 10 in Example 2 is similar to the configuration described above with reference to FIG.
  • the second embodiment differs in the processing executed by the optimum trunk route determining unit 131 of the partial route planning unit 130, that is, the processing of generating a route (partial route) that allows safe travel while avoiding obstacles and steps as much as possible. .
  • the optimum body route determining unit 131 allows the sensor 111 to avoid obstacles and bumps as much as possible in the range where the sensor 111 can analyze the obstacles and the shape of the running surface. Possible partial paths (partial paths indicated by dotted lines in FIG. 9) are generated sequentially.
  • the optimal body route determination unit 131 of the partial route planning unit 130 sets a plurality of routes along which the robot 10 can travel within the determined section of the partial route, and calculates the cost for each route.
  • the cost is such that the more stable driving is possible, the lower the value, and the greater the unstable driving, the higher the value.
  • the optimal trunk route determination unit 131 calculates costs for a plurality of routes that the robot 10 can travel, and executes evaluation processing for each route. Furthermore, based on the result of this evaluation process, the route with the lowest cost, that is, the route that allows stable running is selected, and the selected route is traveled.
  • the route cost calculation process the greater the lateral inclination of the body (main body) 11 of the robot 10, the higher the cost. Processing was performed by applying a calculation algorithm.
  • a user-defined cost calculation algorithm is applied. That is, the route cost is calculated using a user-defined cost calculation function.
  • the optimum partial route determination processing sequence of the second embodiment executed by the optimum trunk route determination unit 131 of the partial route planning unit 130 will be described.
  • the basic sequence of the optimum partial route determination processing sequence of the second embodiment is the same as the processing according to the flow chart shown in FIG. 10 described above as the basic sequence of the first embodiment. Further, the process of step S104 in the flow shown in FIG. 10, that is, the sequence of the path cost (path_cost) calculation process of the selected route (path) is the same as the process according to the flowchart of FIG. 16 described in the first embodiment. Become.
  • steps S313 to S314, which are the cost calculation steps of the body state cost (pose_cost) calculation process for the sampling points (cost calculation body position (i)) described with reference to FIGS. Processing is different.
  • FIGS. 27 to 29 correspond to the detailed sequence of the process of step S202 in the flow shown in FIG. 16 described in the first embodiment.
  • calculation processing of the body state cost (pose_cost) at one sampling point (cost calculation body position (i)) of one selection path (path) selected as a cost calculation target is shown in FIGS. 29 are executed according to the processing of steps S401 to S417. The processing of each step will be described below in order.
  • Steps S401 to S406 The processing of steps S401 to S406 is the same as the processing of steps S301 to S306 described with reference to the flowchart shown in FIG. 16 in the first embodiment.
  • step S401 (min_heights) ⁇ (array of 4 elements) (max_heights) ⁇ (array of 4 elements)
  • the minimum height (min_heights) and maximum height (max_heights) of one leg of the robot 10 are set as variables.
  • Steps S402 to S406 form a processing loop for processing for each leg of the robot .
  • the following data (1) to (4) are calculated by executing the processing loop of steps S402 to S406.
  • Step S4111 The process of step S411 is the same process as the process of step S311 described with reference to the flowchart shown in FIG. .
  • step S411 the worst body state cost (worst_pose_cost) is set to minus infinity (- ⁇ ).
  • Worst torso state cost (worst_pose_cost) ⁇ - ⁇ Before executing the processing loop of steps S412 to S417, the cost initial setting processing is performed.
  • Step S412 Steps S412 to S417 form a processing loop for processing for each leg arrangement state of the robot .
  • the sampling points (cost calculation torso position (i)) have the same leg arrangement area as shown in FIGS. In the case of height planes, only the possible leg placement patterns need to be run through the processing loop.
  • Step S413 Steps S413 to S416 are processing steps that are sequentially executed for any one of the 16 leg arrangement patterns.
  • step S413 the following processing is executed for one leg arrangement pattern to be processed.
  • user_algorithm Various different setting algorithms can be applied as the user-defined cost calculation algorithm (user_algorithm). Specifically, for example, it is possible to generate and use a cost calculation algorithm that increases the calculated cost value corresponding to the robot usage environment or the robot state that is undesirable for the user.
  • Algorithm 1 A cost calculation algorithm in which the calculation cost changes according to the size of the margin until the leg fully extends when each leg of the robot is grounded.
  • the leg can move freely in any direction within the deployable area, but if the leg touches the ground near the edge of the deployable area, the leg can only move toward the interior of the deployable area.
  • Algorithm 1 is an algorithm that considers such a situation, and calculates a low cost value when the leg is grounded near the center of the placeable area, and calculates a low cost value when the leg is grounded near the edge of the placeable area. If so, the algorithm that computes the higher cost value.
  • Algorithm 2 A cost calculation algorithm in which the calculated cost changes according to the size of the roll angle of the body when all the legs of the robot are on the ground Roll of the robot body when all the four legs of the robot are on the ground This algorithm calculates a higher cost value as the inclination in the (roll) direction increases.
  • Algorithm 3 A cost calculation algorithm in which the calculation cost changes according to the size of the pitch angle of the body when all the legs of the robot are on the ground. The pitch of the robot body when all the four legs of the robot are on the ground. This algorithm calculates a higher cost value as the inclination in the (pitch) direction increases.
  • Algorithm 4 A cost calculation algorithm in which the calculated cost changes according to the height of the center of gravity of the body when all the legs of the robot are on the ground. An algorithm that calculates a higher cost value for higher or lower values.
  • Algorithm 5 A cost calculation algorithm in which the calculated cost changes according to the magnitude of the height difference between the grounding positions of all the legs when all the legs of the robot are in contact with the ground. This algorithm calculates a higher cost value as the height difference between the grounding positions of the legs increases.
  • Algorithm 6 A cost calculation algorithm that changes the calculated cost according to the state of the running surface on which the robot's legs touch the ground. This algorithm calculates a higher cost value as the situation becomes more difficult.
  • Algorithm 7 Cost Calculation Algorithm in which Calculated Cost Changes According to Distance Between Robot and Obstacle This algorithm calculates a higher cost value as the distance between the robot and the obstacle is shorter.
  • Algorithm 8 A cost calculation algorithm in which the calculated cost changes according to the distance between the grounding position of the robot's leg and the closest step position on the running surface from the grounding position. This is an algorithm that calculates a higher cost value as the distance to the bump position on the running surface is closer.
  • Algorithm 9 A cost calculation algorithm in which the calculation cost changes according to the size of a support polygon formed with all the legs of the robot in contact with the ground. This is an algorithm for calculating a higher cost value as the size of the supporting polygon formed in , with the leg grounding point as its vertex, is smaller.
  • the user-defined cost calculation algorithm can be any of the above (algorithm 1) to (algorithm 9), or a combination of various different settings. .
  • step S413 the body state cost calculation value (calc_pose_cos (user_algorithm) is calculated according to such a user-defined cost calculation algorithm (user_algorithm).
  • step S414 body state cost (pose_cost) calculation processing corresponding to one leg arrangement pattern at one sampling point (cost calculation body position (i)) is executed. Specifically, the following calculation processing of the torso state cost (pose_cost) is executed.
  • the body state cost calculation value (calc_pose_cos (user_algorithm) calculated according to the user-defined cost calculation algorithm (user_algorithm) is used as the body state cost (pose_cost).
  • Torso state cost (pose_cost) ⁇ Torso state cost calculation value (calc_pose_cos (user_algorithm)) Calculation processing of the torso state cost (pose_cost) is executed according to the above formula.
  • the body state cost (pose_cost) calculated in step S414 is a cost calculated by applying the cost calculation algorithm defined in the user-defined cost calculation algorithm (user_algorithm).
  • Step S415) When the body state cost (pose_cost) calculation process corresponding to one leg arrangement pattern at one sampling point (cost calculation body position (i)) is completed in step S414, the calculated body state cost (pose_cost) is calculated in step S415. and the worst torso state cost (worst_pose_cost).
  • the above determination formula determines whether the torso state cost (pose_cost) corresponding to one leg arrangement pattern calculated in step S414 is greater than the initial set value or the already calculated worst torso state cost (worst_pose_cost). is a judgment formula for judging whether the torso state cost (pose_cost) corresponding to one leg arrangement pattern calculated in step S414 is greater than the initial set value or the already calculated worst torso state cost (worst_pose_cost). is a judgment formula for judging
  • step S415. is determined to be Yes, and the process proceeds to step S416.
  • step S414 determines whether the trunk state cost (pose_cost) corresponding to one leg arrangement pattern calculated in step S414 is not larger than the initial set value or the already calculated worst trunk state cost (worst_pose_cost).
  • Step S416 The processing of step S416 is performed when the determination of Yes is made in step S415, that is, when the body state cost (pose_cost) corresponding to one leg arrangement pattern calculated in step S414 is set to the initial setting value or the already calculated worst Executed when it is determined that the value is greater than the torso state cost (worst_pose_cost).
  • step S416 processing is executed to set the worst torso state cost (worst_pose_cost) to the torso state cost (pose_cost) calculated in step S414.
  • Step S417 is the end step of the processing loop of steps S412 to S417.
  • the processing of steps S413 to S416 is executed for a maximum of 16 combination patterns of the contact height of the four legs of the robot 10, and The torso state cost (pose_cost) is calculated, and the worst torso state cost (worst_pose_cost) with the highest cost among them is calculated as the torso state cost (pose_cost).
  • the process proceeds to step S203 of the flow shown in FIG.
  • the second embodiment is an embodiment in which the cost calculation process is performed by applying the user-defined cost calculation algorithm (user_algorithm). That is, when calculating the body state cost (pose_cost) at one sampling point (cost calculation body position (i)) of one selection path (path) selected as a cost calculation target, the user-defined cost calculation algorithm (user_algorithm) is applied to calculate the body state cost calculation value (calc_pose_cos (user_algorithm)).
  • the user_algorithm the user-defined cost calculation algorithm
  • a user-defined cost calculation algorithm can be an algorithm that increases the calculated cost value corresponding to robot usage environments and robot states that are undesirable to the user. Therefore, by applying the second embodiment, it is possible to control the robot to travel by selecting the optimal travel route that can maintain the optimal robot state according to the use environment and the user's desire.
  • Example 3 Example using a hexapod robot
  • the first embodiment described above is an embodiment using a four-legged robot.
  • a four-legged robot is just one example of the mobile device of the present disclosure, and the processing of the present disclosure can be used in walking robots having any number of legs, two or more.
  • An example using a hexapod robot will be described below as Example 3.
  • FIG. 1 An example using a hexapod robot will be described below as Example 3.
  • FIG. 30 is a diagram showing a configuration example of the hexapod robot 10.
  • the hexapod robot 10 has a body (main body) 11, legs 12, grounding portions 13, and a visual sensor (camera) 15 for recognizing the surrounding environment of the robot.
  • each leg has a joint portion 21 and a joint portion 22 .
  • the robot 10 shown in FIG. 30 is a hexapod walking robot having two legs in the front, two legs in the back, and two legs in the middle.
  • the robot 10 has a visual sensor 15 .
  • the visual sensor 15 is a sensor for acquiring environmental information such as obstacles around the robot 10 and the shape of the running surface. For example, it is configured by a camera.
  • the visual sensor 15 is, for example, a monocular camera, a stereo camera, an omnidirectional camera, an infrared camera, a LiDAR (Light Detection and Ranging), a TOF (Time of Flight) sensor, or a combination thereof. It is possible. Both LiDAR and TOF sensors are sensors capable of measuring object distances.
  • the movable structure of the joints 21 and 22 of the six legs 12 is the same as the movable structure of the joints of the four-legged robot described above with reference to FIG. That is, the joints 21 at the upper end of each of the six legs 12 are configured to be rotatable in both the front-rear direction and the left-right direction with respect to the body portion (body portion) 11 .
  • joints 22 corresponding to so-called knee joints are provided in the middle of each of the six legs so that the lower half of the leg 12 can be rotated in the front-rear direction with respect to the upper part of the leg.
  • the configuration of the robot 10 of Example 3 to which such a hexapod type robot 10 is applied is basically the same as the configuration previously described with reference to FIG. 8, although the number of legs is different.
  • six processes are executed by the optimum body route determination unit 131 of the partial route planning unit 130, that is, the processing of generating a route (partial route) that allows safe travel while avoiding obstacles and steps as much as possible. The process takes into consideration the legs of the book.
  • the optimum body route determining unit 131 allows the sensor 111 to avoid obstacles and bumps as much as possible in the range where the sensor 111 can analyze the obstacles and the shape of the running surface. Possible partial paths (partial paths indicated by dotted lines in FIG. 9) are generated sequentially.
  • the optimal body route determination unit 131 of the partial route planning unit 130 sets a plurality of routes along which the robot 10 can travel within the determined section of the partial route, and calculates the cost for each route.
  • the cost is such that the more stable driving is possible, the lower the value, and the greater the unstable driving, the higher the value.
  • the optimal trunk route determination unit 131 calculates costs for a plurality of routes that the robot 10 can travel, and executes evaluation processing for each route. Furthermore, based on the result of this evaluation process, the route with the lowest cost, that is, the route that allows stable running is selected, and the selected route is traveled.
  • Example 1 described above with reference to FIGS.
  • a cost calculation process is performed in which the size is calculated, and the larger the slope, the higher the cost.
  • the magnitude of the lateral inclination of the body portion (main body portion) 11 is calculated, and the greater the inclination, the higher the cost. Perform cost calculation processing.
  • the optimum partial route determination processing sequence of the third embodiment executed by the optimum trunk route determination unit 131 of the partial route planning unit 130 will be described.
  • the basic sequence of the optimum partial route determination processing sequence of the third embodiment is the same as the processing according to the flow chart shown in FIG. 10 described above as the basic sequence of the first embodiment. Further, the process of step S104 in the flow shown in FIG. 10, that is, the sequence of the path cost (path_cost) calculation process of the selected route (path) is the same as the process according to the flowchart of FIG. 16 described in the first embodiment. Become.
  • step S202 in the flow shown in FIG. 16 that is, the sequence of calculation processing of the trunk state cost (pose_cost) at the sampling point (cost calculation trunk position (i)) is different.
  • FIGS. 31 to 33 correspond to the detailed sequence of the process of step S202 in the flow shown in FIG. 16 described in the first embodiment.
  • calculation processing of the body state cost (pose_cost) at one sampling point (cost calculation body position (i)) of one selection path (path) selected as a cost calculation target is shown in FIGS. 33 are executed in accordance with the processing of steps S501 to S517. The processing of each step will be described below in order.
  • Step S501 The processing of step S501 is the same as the processing of step S301 described with reference to the flowchart shown in FIG. 16 in the first embodiment.
  • step S501 (max_heights) ⁇ (array of 6 elements)
  • the minimum height (min_heights) and maximum height (max_heights) of one leg of the robot 10 are set as variables.
  • Steps S502 to S506 form a processing loop for processing for each leg of the robot .
  • the robot 10 has 64 legs.
  • the following data (1) to (6) are calculated by executing the processing loop of steps S502 to S506.
  • Step S511 The processing of step S511 is the same processing as the processing of step S311 described with reference to the flowchart shown in FIG. .
  • step S511 the worst body state cost (worst_pose_cost) is set to minus infinity (- ⁇ ).
  • Worst torso state cost (worst_pose_cost) ⁇ - ⁇ Before executing the processing loop of steps S512 to S517, the cost initial setting processing is performed.
  • Step S512 Steps S512 to S517 form a processing loop for processing for each leg arrangement state of the robot .
  • the sampling points are the In the case where the leg arrangement possible regions are planes of the same height, the processing loop should be executed only for possible leg arrangement patterns.
  • Step S513 to S516 are processing steps that are sequentially executed for any one of the 64 leg arrangement patterns.
  • step S513 the following processing is executed for one leg arrangement pattern to be processed.
  • f_pose be the leg placement state (leg_pose(k)) where leg (k) is the front leg of the robot
  • m_pose be the leg placement state (leg_pose(k)) when leg (k) is the middle leg of the robot
  • b_pose be the leg placement state (leg_pose(k)) where leg (k) is the rear leg of the robot.
  • leg(k) is the front leg of the robot ⁇ m_pose ⁇ leg_pose(k): leg (k) is the middle leg of the robot ⁇ b_pose ⁇ leg_pose(k): leg(k) is the hind leg of the robot ⁇ Execute the above data definition.
  • step S514 body state cost (pose_cost) calculation processing corresponding to one leg arrangement pattern at one sampling point (cost calculation body position (i)) is executed. Specifically, the following calculation processing of the torso state cost (pose_cost) is executed.
  • Body state cost (pose_cost) ⁇ (max(f_pose)-min(f_pose)) + (max(m_pose)-min(m_pose)) + (max(b_pose)-min(b_pose))
  • the body state cost (pose_cost) calculated in step S514 increases as the difference in contact height between the left front leg and the right front leg increases, and increases as the difference in contact height between the left middle leg and right middle leg increases. Further, the greater the difference in contact height between the left rear leg and the right rear leg, the higher the height.
  • the trunk state cost decreases as the difference in contact height between the left front leg and the right front leg decreases, decreases as the difference in contact height between the left middle leg and right middle leg decreases, and decreases as the difference in contact height between the left middle leg and the right middle leg decreases. The smaller the difference between the contact heights of the leg and the right rear leg, the lower.
  • This cost calculation algorithm is a cost calculation algorithm in which the cost increases as the lateral tilt of the trunk (main body) 11 increases, as described above with reference to FIGS.
  • Step S5 When the body state cost (pose_cost) calculation process corresponding to one leg arrangement pattern at one sampling point (cost calculation body position (i)) is completed in step S514, the calculated body state cost (pose_cost) is calculated in step S515. and the worst torso state cost (worst_pose_cost).
  • the above determination formula determines whether the torso state cost (pose_cost) corresponding to one leg arrangement pattern calculated in step S514 is greater than the initial set value or the already calculated worst torso state cost (worst_pose_cost). is a judgment formula for judging whether the torso state cost (pose_cost) corresponding to one leg arrangement pattern calculated in step S514 is greater than the initial set value or the already calculated worst torso state cost (worst_pose_cost). is a judgment formula for judging
  • step S515. is determined to be Yes, and the process proceeds to step S516.
  • step S514 if it is determined that the torso state cost (pose_cost) corresponding to one leg arrangement pattern calculated in step S514 is not larger than the initial set value or the already calculated worst torso state cost (worst_pose_cost), step The determination in S315 is No, and the process proceeds to step S517.
  • Step S516 The process of step S516 is performed when the determination of Yes is made in step S515, that is, when the body state cost (pose_cost) corresponding to one leg arrangement pattern calculated in step S514 is set to the initial setting value or the already calculated worst Executed when it is determined that the value is greater than the torso state cost (worst_pose_cost).
  • step S516 a process of setting the worst torso state cost (worst_pose_cost) to the torso state cost (pose_cost) calculated in step S514 is executed.
  • Step S517 is the end step of the processing loop of steps S512 to S517.
  • the processing of steps S513 to S516 is executed for a maximum of 64 combination patterns of the ground contact heights of the six legs of the robot 10, and The torso state cost (pose_cost) is calculated, and the worst torso state cost (worst_pose_cost) with the highest cost among them is calculated as the torso state cost (pose_cost).
  • the process proceeds to step S203 of the flow shown in FIG.
  • the third embodiment is an embodiment using a hexapod robot as shown in FIG. Therefore, the process executed by the optimum body route determining unit 131 of the partial route planning unit 130 in the third embodiment, that is, the process of generating a route (partial route) that allows safe travel while avoiding obstacles and steps as much as possible. is a process that considers six legs as described with reference to the flow shown in FIGS.
  • the optimal body path determination unit 131 of the partial path planning unit 130 of the robot 10 executes the processing described with reference to the flow shown in FIGS. , the path with the lowest cost value is generated as the best path (best_path).
  • the generated optimal path (best_path) is the path with the lowest cost, and is the path that can be traveled with the torso (main body) 11 of the robot 10 tilting to the left and right as little as possible. be.
  • the robot 10 runs along the best path (best_path). In other words, the robot 10 can travel along the optimum route that can be traveled with the smallest lateral tilt of the torso (main body) 11 among the plurality of route candidates.
  • Example 4 Example using 6-dimensional data (x, y, z, roll, pitch, yaw) as the body state (Example 5)
  • Example of settings with different degrees of freedom for the joints of the legs of the robot Example 6)
  • Example 4 Example using six-dimensional data (x, y, z, roll, pitch, yaw) as the body state) First, as (Embodiment 4), an embodiment using six-dimensional data (x, y, z, roll, pitch, yaw) as the body state will be described.
  • the torso state of the robot 10 is three-dimensional data (x, y , ⁇ ).
  • the desired trunk state generator 122 of the global route planning unit 120 of the robot 10 shown in FIG. Three-dimensional data of the position (x, y) on the horizontal plane and the angle ( ⁇ ) indicating the orientation of the robot 10, ie, (x, y, ⁇ ), is generated.
  • the optimum trunk path determination unit 131 of the partial route planning unit 130 determines the trunk state (x, y, ⁇ ) was performed to generate partial paths.
  • the torso state of the robot 10 is three-dimensional data (x, y, ⁇ ) and used.
  • the trunk state of the robot 10 is not limited to such three-dimensional data (x, y, ⁇ ), and other data can be used.
  • three-dimensional data x, y, ⁇
  • six-dimensional data x, y, z, roll , pitch, yaw
  • x, y, z, roll , pitch, yaw may be used as the body state.
  • the optimum trunk path determination unit 131 of the partial route planning unit 130 determines the trunk state (x, y, z, roll, pitch, yaw).
  • the legs of the robot 10 of the above-described embodiment for example, the first embodiment, the joints 21 at the upper ends of the legs 12 are attached to the trunk (main body) 11 by It has a structure that can be rotated in both the front-rear direction and the left-right direction.
  • the joint portion 22 corresponding to the so-called knee joint at the intermediate portion of each leg is capable of rotating the lower half of the leg portion 12 in the front-rear direction with respect to the upper leg portion.
  • the leg configuration described with reference to FIG. 2 is an example of a walking robot, and the leg movable configuration varies depending on the robot.
  • the legs of the robot 10 according to the first embodiment are such that the joints 21 at the upper ends of the legs 12 move forward and backward and left and right with respect to the trunk (main body) 11 .
  • Both have a rotatable structure, but there are also robots that have a structure in which the joints 21 at the upper ends of the legs 12 can only be rotated in the front-rear direction with respect to the body (body) 11. .
  • Such a robot configuration will be described with reference to FIGS. 34 and 35.
  • FIG. 34 shows an example of the robot 10 having a configuration in which the joints 21 at the upper ends of the legs 12 are rotatable relative to the trunk (main body) 11 only in the front-rear direction.
  • the robot 10 shown in FIG. 34 has a torso (main body) 11, legs 12, grounding portions 13, and a visual sensor (camera) 15 for recognizing the surrounding environment of the robot. Also, each leg has a joint portion 21 and a joint portion 22 .
  • the robot 10 shown in FIG. 34 is a four-legged walking robot having two legs in front and two legs in back.
  • FIG. 35 shows the robot 10 shown in FIG. (a) Front view (b) Side view (c) Rear view These three views are shown.
  • the robot 10 of the fifth embodiment differs from the robot 10 of the first embodiment in that (a) the front view and (c) the rear view show that each leg 12 is positioned relative to the trunk (main body) 11.
  • the upper end, that is, the joint portion 21 of the leg mounting portion does not have a structure that can rotate in the left-right direction as viewed from the front or rear of the robot.
  • the joints 21 at the upper ends of the legs 12 are configured to be rotatable only in the front-rear direction.
  • the joints 21 at the upper ends of the legs 12 of the robot 10 of the fifth embodiment are configured to be rotatable only in the front-rear direction with respect to the body (main body) 11 .
  • a joint portion 22 corresponding to a so-called knee joint is provided at the intermediate portion of each leg.
  • the joint portion 22 can rotate the lower half of the leg portion 12 in the front-rear direction with respect to the upper leg portion.
  • the leg arrangement possible area differs from the setting of the first embodiment described above.
  • the leg arrangement possible area is set as shown in FIG.
  • the area where each leg can be arranged is a linear area extending in the front-rear direction of the robot 10 .
  • the minimum leg placement height (min_heights(k)) and the maximum leg placement height A process of calculating the height (max_heights(k)) is performed.
  • the robot device 10 can autonomously travel under the control of the robot 10 itself. For example, as shown in FIG. It is also possible to receive commands and run.
  • the robot 10 is equipped with a control device (information processing device) for controlling the robot.
  • the controller receives detection signals from various sensors and encoders attached to the robot 10, analyzes the input signals to analyze the position (three-dimensional position) and movement of each leg, and controls each leg based on the analysis results. drive control.
  • a CPU (Central Processing Unit) 301 functions as a data processing section that executes various processes according to programs stored in a ROM (Read Only Memory) 302 or a storage section 308 . For example, the process according to the sequence described in the above embodiment is executed.
  • a RAM (Random Access Memory) 303 stores programs and data executed by the CPU 301 . These CPU 301 , ROM 302 and RAM 303 are interconnected by a bus 304 .
  • the CPU 301 is connected to an input/output interface 305 via a bus 304.
  • the input/output interface 305 includes an input unit 306 including various switches, a touch panel, a mouse, a microphone, and a user input unit, and an output unit including a display and speakers. 307 is connected.
  • Sensor detection information from various sensors 321 such as a camera and LiDAR is also input to the input unit 306 .
  • the output unit 307 also outputs drive information to a drive unit 322 that drives the mobile device.
  • the CPU 301 receives commands, situation data, and the like input from the input unit 306 , executes various processes, and outputs processing results to the output unit 307 , for example.
  • a storage unit 308 connected to the input/output interface 305 is composed of, for example, a flash memory, a hard disk, or the like, and stores programs executed by the CPU 301 and various data.
  • a communication unit 309 functions as a transmission/reception unit for data communication via a network such as the Internet or a local area network, and communicates with an external device.
  • a GPU Graphics Processing Unit
  • a dedicated processing unit for image information input from a camera may be provided as a dedicated processing unit for image information input from a camera.
  • a drive 310 connected to the input/output interface 305 drives a removable medium 311 such as a magnetic disk, an optical disk, a magneto-optical disk, or a semiconductor memory such as a memory card to record or read data.
  • a removable medium 311 such as a magnetic disk, an optical disk, a magneto-optical disk, or a semiconductor memory such as a memory card to record or read data.
  • the technique disclosed in this specification can take the following configurations. (1) having a route planning unit that determines the travel route of the mobile device; The route planning unit For each of the multiple route candidates, calculate the route cost by applying a cost calculation algorithm in which the cost is lower for routes that allow stable travel, A mobile device that determines a route candidate with the lowest calculated route cost as an optimum travel route.
  • the route planning section The mobile device according to (1) or (2), wherein the route cost is calculated by applying a cost calculation algorithm that increases the cost as the lateral tilt of the body of the mobile device increases.
  • the route planning section For each of the plurality of route candidates, setting a plurality of sampling points in the route of the route candidate and calculating the cost corresponding to each sampling point, The mobile device according to any one of (1) to (3), wherein an added value of costs of a plurality of sampling points set in one route candidate is calculated as the route cost of the route candidate.
  • the route planning section The mobile device according to (4), wherein the costs corresponding to each sampling point are calculated by applying a cost calculation algorithm in which the larger the horizontal tilt of the body of the mobile device, the higher the cost.
  • the mobile device is a walking mobile device having a plurality of legs;
  • the route planning unit The mobile device according to (4) or (5), wherein the costs corresponding to each sampling point are calculated by applying a cost calculation algorithm in which the larger the difference between the ground contact heights of the left and right legs at each sampling point, the higher the cost.
  • the route planning section analyzing a plurality of leg arrangement patterns of the mobile device at each sampling point; Calculate the cost for each of the plurality of leg arrangement patterns, The mobile device according to (6), wherein the cost corresponding to the leg arrangement pattern for which the highest cost is calculated is calculated as the cost corresponding to sampling.
  • the route planning section Analyzing the possible area of each leg of the mobile device at each sampling point, Calculate the maximum and minimum ground contact heights of each leg within the area where each leg can be placed, The mobile device according to (7), wherein the cost is calculated for each of a plurality of leg arrangement patterns that are different combinations of maximum and minimum ground contact heights of the legs of the mobile device.
  • the mobile device a global route planning unit that plans a global route for the mobile device; a partial route planning unit that plans a partial route that allows stable travel in units of partial routes of the global route planned by the global route planning unit;
  • the route planning unit A component within the partial route planning unit, generating a plurality of route candidates as the partial routes, For each of the plurality of route candidates generated, calculate the route cost by applying a cost calculation algorithm in which the more stable the route, the lower the cost;
  • the mobile device according to any one of (1) to (9), wherein the route candidate with the lowest calculated route cost is determined as the optimum travel route.
  • the moving device 11 The mobile device according to (10), further comprising a mobile device controller that generates driving information for causing the mobile device to travel along the optimum travel route determined by the partial route planner.
  • the Global Route Planning Department a target body state generation unit that generates a target body state of the mobile device on the global route planned by the global route planning unit;
  • the route planning unit The mobile device according to (10) or (11), which generates a plurality of route candidates capable of maintaining the desired torso state generated by the desired torso state generation unit.
  • the target body state of the mobile device is three-dimensional data (x, y, ⁇ ) composed of the position (x, y) of the mobile device on the xy plane and the orientation ( ⁇ ) of the mobile device.
  • the route planning unit The mobile device according to any one of (1) to (13), wherein the route cost is calculated by applying a user-defined cost calculation algorithm according to the user definition.
  • the user-defined cost calculation algorithm includes: (a) a cost calculation algorithm in which the calculation cost changes according to the amount of margin until the leg fully extends when each leg of the mobile device touches the ground; (b) a cost calculation algorithm in which the calculation cost changes according to the roll angle or pitch angle of the trunk of the mobile device when the legs are on the ground; (c) a cost calculation algorithm in which the calculation cost changes according to the height of the center of gravity of the trunk of the mobile device when the legs are on the ground; (d) a cost calculation algorithm in which the calculated cost changes according to the state of the running surface on which the legs of the mobile device touch the ground; (e) a cost calculation algorithm in which the calculation cost changes according to the distance between the mobile device and the obstacle; (f) a cost calculation algorithm in which the calculation cost changes according to the distance between the grounding position of the mobile device leg and the closest running surface step position from the grounding position; (g) a cost calculation algorithm in which the calculation cost changes according to the size of the support polygon formed in the leg contact state of the
  • a mobile device control method to be executed in a mobile device comprising: The mobile device has a route planning unit that determines a travel route of the mobile device, The route planning unit For each of the multiple route candidates, calculate the route cost by applying a cost calculation algorithm in which the cost is lower for routes that allow stable travel, A mobile device control method for determining a route candidate with the lowest calculated route cost as an optimum travel route.
  • a program for executing mobile device control processing in a mobile device The mobile device has a route planning unit that determines a travel route of the mobile device, The program causes the route planning unit to: For each of the plurality of route candidates, a route cost is calculated by applying a cost calculation algorithm in which the cost is lower for a route that allows stable travel, A program for executing a process of determining the route candidate with the lowest calculated route cost as the optimum travel route.
  • a program recording the processing sequence is installed in the memory of a computer built into dedicated hardware and executed, or the program is loaded into a general-purpose computer capable of executing various processing. It can be installed and run.
  • the program can be pre-recorded on a recording medium.
  • the program can be received via a network such as a LAN (Local Area Network) or the Internet and installed in a recording medium such as an internal hard disk.
  • a system is a logical collective configuration of a plurality of devices, and the devices of each configuration are not limited to being in the same housing.
  • a device and a method for efficiently determining an optimum route along which a mobile device such as a walking robot can travel safely can be realized.
  • it has a route planning unit that determines the traveling route of a mobile device such as a walking robot, and the route planning unit determines that the route that allows stable traveling for each of a plurality of route candidates has a lower cost.
  • a calculation algorithm is applied to calculate the route cost, and the route candidate with the lowest calculated route cost is determined as the optimum travel route.
  • the route planning section sets multiple sampling points along the route, applies a cost calculation algorithm that increases the cost as the difference between the left and right leg contact heights at each sampling point increases, and calculates the cost corresponding to each sampling point. Then, the added value of the costs of the sampling points is calculated as the route cost.

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Manipulator (AREA)

Abstract

歩行型ロボット等の移動装置が安全に走行できる最適経路を効率的に決定する装置、方法を提供する。歩行型ロボット等の移動装置の走行経路を決定する経路計画部を有し、経路計画部は、複数の経路候補各々について安定走行が可能な経路ほど低コストとなるコスト算出アルゴリズムを適用して経路コストを算出し、算出した経路コストが最低コストの経路候補を最適な走行経路として決定する。経路計画部は経路中に複数のサンプリング点を設定し、各サンプリング点における左右の脚の接地高さの差分が大きいほど高コストとなるコスト算出アルゴリズムを適用して、各サンプリング点対応のコストを算出し、サンプリング点のコストの加算値を経路コストとして算出する。

Description

移動装置、および移動装置制御方法、並びにプログラム
　本開示は、移動装置、および移動装置制御方法、並びにプログラムに関する。具体的には複数の脚を駆動して移動する歩行型ロボット等の移動装置、および移動装置制御方法、並びにプログラムに関する。
　複数の脚を駆動して移動する歩行型ロボットが様々なフィールドで用いられている。ロボットは、工場や人が入ることが困難な災害現場などでも様々な作業を行うことが可能である。
　しかし、歩行型ロボットが安定した移動を行なうためには、進行方向の障害物や、走行面（歩行面）の凹凸、傾斜などを把握して、安定した走行が可能なルートを識別し、さらに、走行面の形状に応じた脚の着地を行うことが必要となる。
　例えば、凹凸の激しい面や傾斜の大きい面に歩行型ロボットの脚を置いてしまうと、ロボットが倒れ、前に進むことができなくなるといった事態が発生する可能性がある。
　このような問題を防止するため、ロボットの多くは、ロボット周囲の環境を認識するためのカメラ等のセンサを装着し、センサ検出情報を用いて安定した歩行や走行を行うための経路を選択して移動する制御を実行している。
　例えば、センサから得られたデータを利用して周囲の３次元形状データを含む環境地図を生成し、生成した環境地図を参照して移動経路を決定する処理などを行う。
　しかし、歩行型ロボットは、４脚や６脚など複数の脚を各々異なる位置に接地させることが必要であり、安定した歩行を行うためには、各脚の接地位置各々の形状を解析することが必要となる。
　各脚の接地面形状を解析するための計算量は多大であり、その結果、ロボットの進行速度が低下してしまうといった問題が発生する。
　なお、歩行型ロボットの脚の接地位置の解析を行う技術を開示した従来技術として、例えば特許文献１（特開２０１３－０６６９９６号公報）がある。
　この特許文献１は、ロボットの各脚がロボット走行面にめり込まずにちょうど接するような位置を繰り返し計算で求めて、安定した走行を行う技術について開示している。
　しかし、この特許文献１では、各脚と接地面とが接触する３次元位置を算出するため、３次元点群データを利用した繰り返し演算を行っており、ロボットの一歩の接地位置を算出するための演算量が大きく、ロボットの走行経路が長い場合は、この演算を多数回、繰り返し実行する必要があり、結果としてロボットの走行スピードが低下するという問題がある。
特開２０１３－０６６９９６号公報
　本開示は、例えば上記問題点に鑑みてなされたものであり、歩行型ロボットの処理負荷や処理データ量を大きく増大させることなく、安定したロボット姿勢を実現する脚の接地位置を解析して安全走行を行うことを可能とした移動装置、および移動装置制御方法、並びにプログラムを提供することを目的とする。
　本開示の第１の側面は、
　移動装置の走行経路を決定する経路計画部を有し、
　前記経路計画部は、
　複数の経路候補各々について、安定走行が可能な経路ほど低コストとなるコスト算出アルゴリズムを適用して経路コストを算出し、
　算出した経路コストが最低コストの経路候補を最適な走行経路として決定する移動装置にある。
　さらに、本開示の第２の側面は、
　移動装置において実行する移動装置制御方法であり、
　前記移動装置は、移動装置の走行経路を決定する経路計画部を有し、
　前記経路計画部が、
　複数の経路候補各々について、安定走行が可能な経路ほど低コストとなるコスト算出アルゴリズムを適用して経路コストを算出し、
　算出した経路コストが最低コストの経路候補を最適な走行経路として決定する移動装置制御方法にある。
　さらに、本開示の第３の側面は、
　移動装置において移動装置制御処理を実行させるプログラムであり、
　前記移動装置は、移動装置の走行経路を決定する経路計画部を有し、
　前記プログラムは、前記経路計画部に、
　複数の経路候補各々について、安定走行が可能な経路ほど低コストとなるコスト算出アルゴリズムを適用して経路コストを算出させ、
　算出した経路コストが最低コストの経路候補を最適な走行経路として決定する処理を実行させるプログラムにある。
　なお、本開示のプログラムは、例えば、様々なプログラム・コードを実行可能な情報処理装置やコンピュータ・システムに対して、コンピュータ可読な形式で提供する記憶媒体、通信媒体によって提供可能なプログラムである。このようなプログラムをコンピュータ可読な形式で提供することにより、情報処理装置やコンピュータ・システム上でプログラムに応じた処理が実現される。
　本開示のさらに他の目的、特徴や利点は、後述する本開示の実施例や添付する図面に基づくより詳細な説明によって明らかになるであろう。なお、本明細書においてシステムとは、複数の装置の論理的集合構成であり、各構成の装置が同一筐体内にあるものには限らない。
　本開示の一実施例の構成によれば、歩行型ロボット等の移動装置が安全に走行できる最適経路を効率的に決定する装置、方法が実現される。
　具体的には、例えば、歩行型ロボット等の移動装置の走行経路を決定する経路計画部を有し、経路計画部は、複数の経路候補各々について安定走行が可能な経路ほど低コストとなるコスト算出アルゴリズムを適用して経路コストを算出し、算出した経路コストが最低コストの経路候補を最適な走行経路として決定する。経路計画部は経路中に複数のサンプリング点を設定し、各サンプリング点における左右の脚の接地高さの差分が大きいほど高コストとなるコスト算出アルゴリズムを適用して、各サンプリング点対応のコストを算出し、サンプリング点のコストの加算値を経路コストとして算出する。
　本構成により、歩行型ロボット等の移動装置が安全に走行できる最適経路を効率的に決定する装置、方法が実現される。
　なお、本明細書に記載された効果はあくまで例示であって限定されるものではなく、また付加的な効果があってもよい。
本開示の移動装置の一例である４脚ロボットの構成例について説明する図である。 本開示の移動装置の一例である４脚ロボットの構成例について説明する図である。 移動装置の走行環境の一例について説明する図である。 移動装置の走行例について説明する図である。 段差障害物にロボットの脚を置いた場合の不安定な状態について説明する図である。 ロボットが平坦な経路を選択して走行する例について説明する図である。 ロボットが平坦な経路を選択して走行する例について説明する図である。 本開示の移動装置であるロボットの構成例について説明する図である。 大域経路計画部が生成する大域的経路と、部分経路計画部が生成する部分経路の具体例について説明する図である。 本開示の移動装置の部分経路計画部の最適胴体経路決定部が実行する最適部分経路決定処理の詳細シーケンスについて説明するフローチャートを示す図である。 最適胴体経路決定部が実行する処理の具体例について説明する図である。 ロボットの胴体部（本体部）の左右の傾きが大きいほど高いコストが算出される例について説明する図である。 ロボットの胴体部（本体部）の左右の傾きが大きいほど高いコストが算出される例について説明する図である。 ロボットの胴体部（本体部）の左右の傾きが大きいほど高いコストが算出される例について説明する図である。 ロボットの胴体部（本体部）の左右の傾きが大きいほど高いコストが算出される例について説明する図である。 本開示の移動装置の部分経路計画部の最適胴体経路決定部が実行する最適部分経路決定処理の詳細シーケンスについて説明するフローチャートを示す図である。 最適胴体経路決定部が実行する処理の具体例について説明する図である。 本開示の移動装置の部分経路計画部の最適胴体経路決定部が実行する最適部分経路決定処理の詳細シーケンスについて説明するフローチャートを示す図である。 本開示の移動装置の部分経路計画部の最適胴体経路決定部が実行する最適部分経路決定処理の詳細シーケンスについて説明するフローチャートを示す図である。 本開示の移動装置の部分経路計画部の最適胴体経路決定部が実行する最適部分経路決定処理の詳細シーケンスについて説明するフローチャートを示す図である。 最適胴体経路決定部が実行する処理の具体例について説明する図である。 最適胴体経路決定部が実行する処理の具体例について説明する図である。 最適胴体経路決定部が実行する処理の具体例について説明する図である。 最適胴体経路決定部が実行する処理の具体例について説明する図である。 最適胴体経路決定部が実行する処理の具体例について説明する図である。 最適胴体経路決定部が実行する処理の具体例について説明する図である。 本開示の移動装置の部分経路計画部の最適胴体経路決定部が実行する最適部分経路決定処理の詳細シーケンスについて説明するフローチャートを示す図である。 本開示の移動装置の部分経路計画部の最適胴体経路決定部が実行する最適部分経路決定処理の詳細シーケンスについて説明するフローチャートを示す図である。 本開示の移動装置の部分経路計画部の最適胴体経路決定部が実行する最適部分経路決定処理の詳細シーケンスについて説明するフローチャートを示す図である。 ６脚型のロボットの構成例について説明する図である。 本開示の移動装置の部分経路計画部の最適胴体経路決定部が実行する最適部分経路決定処理の詳細シーケンスについて説明するフローチャートを示す図である。 本開示の移動装置の部分経路計画部の最適胴体経路決定部が実行する最適部分経路決定処理の詳細シーケンスについて説明するフローチャートを示す図である。 本開示の移動装置の部分経路計画部の最適胴体経路決定部が実行する最適部分経路決定処理の詳細シーケンスについて説明するフローチャートを示す図である。 ４脚型のロボットの構成例について説明する図である。 ４脚型のロボットの構成例について説明する図である。 最適胴体経路決定部が実行する処理の具体例について説明する図である。 ユーザの操作するコントローラからの受信データによって制御されるロボット制御例について説明する図である。 本開示の移動装置のハードウェア構成例について説明する図である。
　以下、図面を参照しながら本開示の移動装置、および移動装置制御方法、並びにプログラムの詳細について説明する。なお、説明は以下の項目に従って行なう。
　１．本開示の移動装置の概要について
　２．本開示の移動装置の構成例について
　３．本開示の移動装置が実行する処理（実施例１）の詳細について
　４．（実施例２）ユーザ定義のコスト算出アルゴリズムを適用した実施例
　５．（実施例３）６脚型のロボットを利用した実施例
　６．その他の実施例について
　７．本開示の移動装置のハードウェア構成例について
　８．本開示の構成のまとめ
　　［１．本開示の移動装置の概要について］
　まず、図１以下を参照して本開示の移動装置の概要について説明する。
　図１は本開示の移動装置の一例である４脚型のロボット１０を示す図である。
　ロボット１０は、４本の脚を個別に駆動して移動する歩行型ロボットである。
　なお、図１に示す４脚型のロボットは本開示の移動装置の一例であり、本開示の処理は２脚以上の複数の脚を有する歩行型ロボットにおいて利用可能である。
　図１に示すように、４脚ロボット１０は，胴体部（本体部）１１、脚部１２、接地部１３、ロボットの周囲環境を認識するための視覚センサ（カメラ）１５を有する。また各脚には関節部２１、関節部２２を有する。
　図１に示すロボット１０は前方に２本の脚、後方に２本の脚を有する４脚型の歩行型ロボットである。
　４本の各脚を個別に持ち上げて前後に動かすことで脚移動、すなわち歩行走行を行なうことが可能となる。
　図１に示すように、ロボット１０は、視覚センサ１５を有してる。視覚センサ１５は、ロボット１０の周囲の障害物や、走行面形状などの環境情報を取得するためのセンサである。例えば、カメラによって構成される。
　なお、視覚センサ１５は、例えば、単眼カメラ、ステレオカメラ、全方位カメラ、赤外カメラ、ＬｉＤＡＲ（Ｌｉｇｈｔ　Ｄｅｔｅｃｔｉｏｎ　ａｎｄ　Ｒａｎｇｉｎｇ）、ＴＯＦ（Ｔｉｍｅ　ｏｆ　Ｆｌｉｇｈｔ）センサなどのいずれか、またはこれらの組み合わせ構成が利用可能である。
　なお、ＬｉＤＡＲ、ＴＯＦセンサは、いずれもオブジェクト距離を計測可能なセンサである。
　図１に示すロボット１０の詳細構成例について図２を参照して説明する。
　図２には、図１に示すロボット１０の、
　（ａ）前面図
　（ｂ）側面図
　（ｃ）背面図
　これらの３つの図を示している。
　（ａ）前面図、および（ｃ）背面図から理解されるように、胴体部（本体部）１１に対する脚部１２各々の上端、すなわち脚装着部の関節部２１は、ロボット前面や後面から見て左右方向に回動可能な構成を有する。
　さらに、（ｂ）側面図から理解されるように、脚部１２各々の上端の関節部２１は、前後方向にも回動可能な構成を有する。
　このように、脚部１２各々の上端の関節部２１は、胴体部（本体部）１１に対して、前後方向、左右方向いずれにも回動可能な構成を持つ。
　さらに、（ｂ）側面図から理解されるように、各脚の中間部にはいわゆる膝関節に相当する関節部２２が設けられている。この関節部２２は、脚部１２の下側半分を、脚上側部に対して前後方向に回動させることが可能となっている。
　図３は、図１に示すロボット１０が走行を行う環境の一例を示す図である。
　ロボット１０が移動する環境は、例えば図３に示すように様々な障害物が存在し、また、走行面も平面とは限らず、でこぼこな面、段差のある面、傾斜のある面、階段など、様々である。
　例えば、ロボット１０が障害物に衝突すると、ロボット１０が転倒し、損傷や故障が発生する可能性がある。
　また、でこぼこな面、段差のある面、傾斜のある面などに段差や傾斜角などを考慮せずに脚を接地した場合にも、ロボット１０が転倒し、損傷や故障が発生する可能性がある。
　このような事態の発生を防止するため、カメラ等の視覚センサ１５が取得したセンサ取得情報に基づいて生成される周囲の環境地図、すなわち、ロボット周囲の走行面を含むオブジェクトの３次元形状情報を有する環境地図を利用して、走行経路を決定する処理が行なわれる。
　走行経路は、基本的には、例えば、まず予め規定したゴール地点までの最短距離を結ぶ経路として生成され、その経路上にロボット１０が進行できない大きな障害物がある場合には、その障害物を避けるようにルートが修正されることになる。
　しかし、歩行型のロボット１０は、例えば高さの低い障害物であれば、脚を持ち上げて障害物上に登って進むことも可能であり、段差の小さい障害物については走行経路を変更せずにそのまま進む場合がある。
　しかし、例えば図１に示すような４脚型のロボット１０の場合、左側の脚と右側の脚の高さの差が大きくなるとロボット１０の左右のバランスが崩れ、転倒してしまう可能性がある。
　この具体例について、図４を参照して説明する。
　図４に示すようにロボット１０が目的地（ゴール）に向かう場合、その最短ルートは図に矢印で示す「経路１」、すなわちロボット１０の現在位置から、直線状に目的地（ゴール）に向かう経路である。
　この経路１のロボット１０の進行方向右側には段差障害物３０が存在する。この段差障害物３０は、ロボット１０の片側の脚を上部に上げることで、乗り上げることが可能な高さである。従って、ロボット１０は、段差障害物３０が踏破可能であるとの判断に基づいて、経路１に従って走行する可能性がある。
　しかし、この段差障害物３０にロボット１０の右側の脚を置いた場合、ロボット１０は極めて不安定な状態になる。
　具体例を図５に示す。
　図５は、段差障害物３０の上にロボット１０の右側の脚を接地し、ロボット１０の左側の前後の脚を低い走行面に接地した状態を示している。
　ロボット１０は、図５に示すように左右の脚の接地高さが大きく異なると、胴体部（本体部）１１が左右方向に傾き、不安定な状態となる。
　さらに、段差障害物３０の上に接地したロボット１０の右側の脚は可動域が極端に小さくなる。すなわち、脚を前後に大きく移動させることができなくなる。この結果、進行速度が極端に低下する。
　このように、歩行型のロボット１０は、多少の段差の走行面を走行することは可能であるが、安定性の低下や、走行速度の低下など、好ましくない状況が発生する。
　このような場合、例えば図６に示すような「経路２」、すなわち迂回する平坦な経路を選択して走行することが好ましい。
　図７に示すように、このような平坦な「経路２」を走行すれば、ロボット１０は胴体部１１を左右に傾かせることなく、また脚の可動範囲を減少させることなく高速で走行することが可能となり、結果として、目的地（ゴール）まで早く安全に到達することができる。
　本開示は、移動装置であるロボットの走行可能な複数の経路についてコストを算出する。コストは安定した走行が可能なほど値が小さく、不安定な走行が多くなるほど値が大きくなる値となるコストである。本開示の処理では、移動装置であるロボットの走行可能な複数の経路についてコストを算出して、各経路の評価処理を実行する。
　さらに、この評価処理の結果に基づいて最もコストの低い経路、すなわち安定走行が可能な経路を選択し、選択した経路を走行する。
　以下、本開示の具体的な実施例について説明する。
　　［２．本開示の移動装置の構成例について］
　まず、本開示の移動装置の構成例について説明する。
　図８は、本開示の移動装置であるロボット１０の構成例を示す図である。
　図８に示すように、ロボット１０は、環境情報解析部１１０、大域経路計画部１２０、部分経路計画部１３０、移動装置制御部１４０、駆動部１５０を有する。
　環境情報解析部１１０は、センサ１１１と、環境地図生成部１１２を有する。
　大域経路計画部１２０は、大域経路生成部１２１と、目標胴体状態生成部１２２を有する。
　部分経路計画部１３０は、最適胴体経路決定部１３１と、経路追従目標速度算出部１３２を有する。
　移動装置制御部１４０は、駆動情報生成部１４１を有する。
　環境情報解析部１１０は、ロボット１０の自己位置と認識情報の統合を行い、ロボットの周辺地図を生成する。
　センサ１１１は、先に図１を参照して説明した視覚センサ（カメラ）１５に相当し、例えば、カメラによって構成され、ロボット１０周囲のオブジェクトの位置、形状、さらにロボット１０の走行面の形状などを識別するための画像や距離情報などを取得する。
　なお、センサ１１１は、例えば、単眼カメラ、ステレオカメラ、全方位カメラ、赤外カメラ、ＬｉＤＡＲ（Ｌｉｇｈｔ　Ｄｅｔｅｃｔｉｏｎ　ａｎｄ　Ｒａｎｇｉｎｇ）、ＴＯＦ（Ｔｉｍｅ　ｏｆ　Ｆｌｉｇｈｔ）センサなどのいずれか、またはこれらの組み合わせ構成が利用可能である。
　なお、ＬｉＤＡＲ、ＴＯＦセンサは、いずれもオブジェクト距離を計測可能なセンサである。
　センサ１１１の検出情報は、環境地図生成部１１２に入力される。
　環境地図生成部１１２は、センサ１１１によるロボット１０周囲の環境情報を用いて、自己位置算出処理や環境地図作成処理などを行う。
　なお、環境地図の作成処理や自己位置推定処理を行う代表的処理としてＳＬＡＭ（ｓｉｍｕｌｔａｎｅｏｕｓ　ｌｏｃａｌｉｚａｔｉｏｎ　ａｎｄ　ｍａｐｐｉｎｇ）が知られている。
　ＳＬＡＭは、例えばカメラ等のセンサ取得情報、例えばロボット走行環境の撮影画像を用いて、自己位置推定処理（ローカリゼーション）と環境地図作成処理（ｍａｐｐｉｎｇ）を並行して実行する処理である。
　例えば、ロボットのカメラ撮影画像に含まれる特徴点の軌跡を解析することで、特徴点の３次元位置を推定して、ロボット周囲のオブジェクト位置等を把握可能とした地図、いわゆる環境地図を作成（ｍａｐｐｉｎｇ）し、さらに自己位置（ロボット位置）も推定（ローカリゼーション）することができる。
　なお、ＳＬＡＭには、上記のカメラ撮影画像を用いるビジュアルＳＬＡＭに限らず、様々な手法がある。例えば、レーザ光による障害物までの距離を計測するセンサであるＬｉＤＡＲ（Ｌｉｇｈｔ　Ｄｅｔｅｃｔｉｏｎ　ａｎｄ　Ｒａｎｇｉｎｇ）を用いて行うＬｉＤＡＲ　ＳＬＡＭ等がある。
　このように、環境地図生成部１１２は、センサ１１１の検出情報を用いてロボット１０の自己位置を算出するとともにロボットの周辺地図である環境地図を生成する。
　環境地図生成部１１２が生成した環境地図や自己位置情報は、大域経路計画部１２０の目標胴体状態生成部１２２と、部分経路計画部１３０の最適胴体経路決定部１３１と、移動装置制御部１４０の駆動情報生成部１４１に出力される。
　大域経路計画部１２０は、ロボット１０のスタート位置から最終的なゴール位置までの大域的な経路を生成する。なお、大域経路計画部１２０は、ロボット１０のスタート位置から最終的なゴール位置までの大域的経路各位置におけるロボット通過予定時間も生成する。
　これに対して部分経路計画部１３０は、大域経路計画部１２０が生成したロボット１０のスタート位置から最終的なゴール位置までの大域的経路を、一部の部分経路単位で修正した部分経路を生成する。
　部分経路計画部１３０は、例えば、ロボット１０のセンサ１１１によって障害物や走行面形状を解析可能な範囲内の一部経路について、障害物や段差を可能な限り避けて安全に走行可能な経路に修正した部分経路を、順次、生成する。
　図９を参照して、大域経路計画部１２０が生成する大域的経路と、部分経路計画部１３０が生成する部分経路の具体例について説明する。
　図９には、ロボット１０のスタート位置（Ｓ）からゴール位置（Ｇ）までの経路を示している。
　図９に実線で示す経路が大域的経路である。すなわち、ロボット１０のスタート位置（Ｓ）から最終的なゴール位置（Ｇ）までの大域的経路である。
　一方、図９に点線で示す経路が部分経路である。この部分経路は、ロボット１０の移動に伴い、順次、生成される。
　例えば、大域経路計画部１２０はロボット１０のスタート位置（Ｓ）から最終的なゴール位置（Ｇ）まで、最短な経路となるように大域的経路を生成する。
　これに対して、部分経路計画部１３０は、センサ１１１によって障害物や走行面形状を解析可能な範囲について、障害物や段差を可能な限り避けて安全に走行可能な経路を順次、生成する。
　大域経路計画部１２０の生成した大域経路を走行した場合、ロボット１０は、図９に示す段差障害物に乗り上げて、左右に傾いてしまう可能性がある。
　部分経路計画部１３０は、このような状態を避けて走行するための部分経路を生成する。
　ロボット１０が図９に示すスタート位置（Ｓ）にある場合、ロボット１０のセンサ１１１によって障害物や走行面形状を解析可能な範囲は、図９に示すスタート位置（Ｓ）からサブゴール１（ＳＧ１）までの範囲である。部分経路計画部１３０は、まず、この一部経路（Ｓ～ＳＧ１）について、障害物や段差を可能な限り避けて安全に走行可能な経路に修正した点線で示す部分経路を生成する。
　ロボット１０は、この点線で示す部分経路に従って走行する。点線で示す経路を走行することで、ロボット１０は段差障害物に乗り上げて左右に傾くといった状態を発生させることなく、安定した走行を行うことができる。
　ロボット１０が、スタート位置（Ｓ）からサブゴール１（ＳＧ１）まで、部分経路計画部が生成した点線で示す部分経路に従って安定走行を行い、サブゴール１（ＳＧ１）に到着すると、部分経路計画部１３０は、次に、ロボット１０のセンサ１１１によって障害物や走行面形状を解析可能な一部経路（ＳＧ１～ＳＧ２）区間について点線で示す部分経路（ＳＧ１～ＳＧ２）を生成する。
　この部分経路（ＳＧ１～ＳＧ２）も、障害物や段差を可能な限り避けて安全に走行可能な部分経路である。
　このように、部分経路計画部１３０は、大域経路計画部１２０が生成した大域経路を修正した部分経路を順次、生成する。すなわち、センサ１１１によって障害物や走行面形状を解析可能な範囲について、障害物や段差を可能な限り避けて安全に走行可能な経路を順次、生成する。
　前述したように、大域経路計画部１２０は、大域経路生成部１２１と、目標胴体状態生成部１２２を有する。
　部分経路計画部１３０は、最適胴体経路決定部１３１と、経路追従目標速度算出部１３２を有する。
　これらの各構成部の処理について説明する。
　大域経路計画部１２０の大域経路生成部１２１は、ロボット１０がスタート位置からゴール位置まで移動するための大域的経路を生成する。また、大域経路生成部１２１は、ロボット１０のスタート位置から最終的なゴール位置までの大域的経路各位置におけるロボット通過予定時間を生成する。
　大域経路生成部１２１が生成した大域経路は目標胴体状態生成部１２２に出力される。
　目標胴体状態生成部１２２は、大域経路生成部１２１が生成した大域経路を走行する際の各位置におけるロボットの胴体状態を算出する。胴体状態とは、ロボット１０の水平面上の位置（ｘ，ｙ）と、ロボット１０の向きを示す角度（θ）、即ち（ｘ，ｙ，θ）の３次元データである。
　なお、ロボット１０の胴体とは、図１に示す胴体部（本体部）１１である。本実施例では、ロボット１０の走行経路を決定する際の基準点の一例として、ユーザが定義可能なロボットの胴体を利用している。基準点は、ロボット１０のユーザ指定可能な１つのリンク座標系上の特定座標の点として定められるものであり、基準点は必ずしも胴体上の点でなければならないわけではない。また、ロボット外部の空中の点であってもよい。
　胴体状態は、図１に示す胴体部（本体部）１１の位置と姿勢を併せて示すデータである。
　なお、この胴体状態は、例えば、図９に実線で示す大域経路上の各位置（ｘ，ｙ）における胴体状態であり、目標胴体状態生成部１２２は、ロボットの走行に伴い、遂次変化する胴体状態（ｘ，ｙ，θ）を算出する。
　目標胴体状態生成部１２２が生成した大域経路上の各位置（ｘ，ｙ）における胴体状態（ｘ，ｙ，θ）は、大域経路生成部１２１が生成した大域経路情報とともに部分経路計画部１３０の最適胴体経路決定部１３１に出力される。
　部分経路計画部１３０の最適胴体経路決定部１３１は、以下の各情報を入力する。
　（ａ）環境情報解析部１１０が生成したロボット１０の自己位置と環境地図、
　（ｂ）大域経路計画部１２０が生成した大域経路情報と、大域経路上の各位置（ｘ，ｙ）における胴体状態（ｘ，ｙ，θ）情報、
　最適胴体経路決定部１３１は、これらの情報を利用して、大域経路計画部１２０が生成した大域経路情報を修正した部分経路、すなわち、障害物や段差を可能な限り避けて安全に走行可能な経路（部分経路）を順次、生成する。
　先に図９を参照して説明したように、最適胴体経路決定部１３１は、センサ１１１によって障害物や走行面形状を解析可能な範囲について、障害物や段差を可能な限り避けて安全に走行可能な部分経路（図９に点線で示す部分経路）を順次、生成する。
　部分経路計画部１３０の最適胴体経路決定部１３１は、部分経路の決定区間内にロボット１０が走行可能な複数の経路を設定し、各経路についてのコストを算出する。コストは安定した走行が可能なほど値が小さく、不安定な走行が多くなるほど値が大きくなるようなコストである。最適胴体経路決定部１３１は、ロボット１０の走行可能な複数の経路についてコストを算出して、各経路の評価処理を実行する。
　さらに、この評価処理の結果に基づいて最もコストの低い経路、すなわち安定走行が可能な経路を選択し、選択した経路を走行する。
　この処理の詳細シーケンスについては後段で説明する。
　なお、最適胴体経路決定部１３１は、ロボット１０の胴体部（本体部）１１の経路を決定する。すなわち各脚各々の経路を決定する処理は行わず、ロボット１０の１つの基準点（例えば胴体部（本体部）の重心位置）の経路（＝胴体経路）を決定する。
　部分経路計画部１３０の最適胴体経路決定部１３１は、各部分経路の各終点位置、例えば図９に示す各サブゴール位置では、目標胴体状態生成部１２２が生成した胴体状態（ｘ，ｙ，θ）になるように部分経路を生成する。
　最適胴体経路決定部１３１の生成した部分経路は、経路追従目標速度算出部１３２に出力される。
　経路追従目標速度算出部１３２は、最適胴体経路決定部１３１の生成した部分経路をロボット１０が走行する際の目標速度を生成する。
　前述したように、大域経路計画部１２０の大域経路生成部１２１は、ロボット１０のスタート位置から最終的なゴール位置までの大域的経路各位置におけるロボット通過予定時間を生成する。
　経路追従目標速度算出部１３２は、各サブゴール位置におけるロボット１０の通過時間を、大域経路生成部１２１が算出した大域的経路各位置におけるロボット通過予定時間にできるだけ一致させるように、ロボット１０の走行目標速度を生成する。
　部分経路計画部１３０の最適胴体経路決定部１３１が生成した部分経路と、経路追従目標速度算出部１３２が算出したロボット１０の目標速度情報は、移動装置制御部１４０の駆動情報生成部１４１に出力される。
　移動装置制御部１４０の駆動情報生成部１４１は、以下の各情報を入力する。
　（ａ）環境情報解析部１１０が生成したロボット１０の自己位置と環境地図、
　（ｂ）部分経路計画部１３０が生成した部分経路とロボット１０の目標速度情報、
　移動装置制御部１４０の駆動情報生成部１４１は、これらの入力情報に基づいて、ロボット１０を部分経路に従って目標速度で走行させるための駆動情報を生成して駆動部１５０に出力する。
　駆動情報とは、例えばロボット１０の各関節部のモータの駆動情報等である。
　移動装置制御部１４０の駆動情報生成部１４１は、ロボット１０の関節のトルク入力を計算して駆動部１５０を構成するアクチュエータに印加する出力を決定する。
　なお、移動装置制御部１４０の駆動情報生成部１４１は、ロボット１０の脚の接地位置を含めた制御情報を生成するため、環境情報解析部１１０が生成したロボット１０の周辺の環境地図を用いる。
　なお、移動装置制御部１４０の駆動情報生成部１４１から駆動部１５０に対する出力は、例えば、トルク、あるいは電圧や角速度、位置など、様々な態様が可能である。
　駆動部１５０は、例えばロボット１０の各関節部のモータ等であり、移動装置制御部１４０の駆動情報生成部１４１の生成した駆動情報によって駆動される。
　　［３．本開示の移動装置が実行する処理（実施例１）の詳細について］
　次に、本開示の移動装置が実行する処理（実施例１）の詳細について説明する。
　本開示の移動装置、すなわちロボット１０は、先に図８を参照して説明したように、部分経路計画部１３０が、大域経路計画部１２０が生成した大域経路を修正した部分経路を順次、生成し、生成した部分経路に従った走行処理を行う。
　すなわち、センサ１１１によって障害物や走行面形状を解析可能な範囲について、障害物や段差を可能な限り避けて安全に走行可能な部分経路を順次、生成し、生成した部分経路に従って走行を行うことで、安定した走行を実現する。
　前述したように部分経路計画部１３０の最適胴体経路決定部１３１は、先に図９を参照して説明したように、図９に点線で示す部分経路の生成処理を行う。すなわち、障害物や段差を可能な限り避けて安全に走行可能な経路に修正した点線で示す部分経路を生成する。
　部分経路計画部１３０の最適胴体経路決定部１３１は、部分経路の決定区間内にロボット１０が走行可能な複数の部分経路候補を設定し、各部分経路候補についてコストを算出する。コストは安定した走行が可能なほど値が小さく、不安定な走行が多くなるほど値が大きくなるようなコストである。
　部分経路計画部１３０の最適胴体経路決定部１３１は、ロボット１０の走行可能な複数の部分経路候補についてコストを算出して、各経路候補の評価処理を実行する。
　さらに、この評価処理の結果に基づいて最もコストの低い経路、すなわち安定走行が可能な経路を選択し、選択した経路を走行する。
　なお、コスト算出アルゴリズムには様々なアルゴリズムが適用可能である。
　まず、実施例１として、ロボット１０の胴体部（本体部）１１の左右の傾きの大きさが大きいほど高コストとなるコスト算出アルゴリズムを適用してコスト算出処理を行う実施例を実施例１として説明する。
　実施例１の処理について、図１０以下を参照して説明する。
　図１０以下を参照して部分経路計画部１３０の最適胴体経路決定部１３１が実行する最適部分経路決定処理の詳細シーケンスについて説明する。
　図１０は、部分経路計画部１３０の最適胴体経路決定部１３１が実行する最適部分経路決定処理の詳細シーケンスについて説明するフローチャートである。
　なお、このフローに従った処理は、ロボット１０内の情報処理装置、あるいはロボット等の移動体と通信可能な情報処理装置において情報処理装置の記憶部に格納されたプログラムに従って実行することが可能である。例えばプログラム実行機能を有するＣＰＵ等のプロセッサによるプログラム実行処理として行うことができる。
　以下、図１０に示すフローの各ステップの処理について説明する。
　　（ステップＳ１０１）
　まず、部分経路計画部１３０の最適胴体経路決定部１３１は、最適経路設定対象区間に、複数の胴体経路（ｐａｔｈ）候補を生成する。
　なお、前述したように、最適胴体経路決定部１３１は、ロボット１０の胴体部（本体部）１１の経路を決定する。すなわち各脚各々の経路を決定する処理は行わず、ロボット１０の１つの基準点（例えば胴体部（本体部）の重心位置）の経路（＝胴体経路）を決定する。
　図１１を参照して、このステップＳ１０１の処理の具体例について説明する。
　図１１には、最適経路設定対象区間（ＳＧｘ～ＳＧｙ）を示している。
　先に図９を参照して説明したように、部分経路計画部１３０の最適胴体経路決定部１３１は、大域経路計画部１２０が生成した大域経路中の一部区間、すなわち、センサ１１１によって障害物や走行面形状を解析可能な範囲について、障害物や段差を可能な限り避けて安全に走行可能な部分経路を順次、生成する。
　例えば図９に示すスタート位置（Ｓ）から、サブゴール１（ＳＧ１）までの区間など、センサ１１１によって障害物や走行面形状を解析可能な範囲単位で最適部分経路を生成する。
　図１１に示す最適経路設定対象区間（ＳＧｘ～ＳＧｙ）は、例えば図９に示すスタート位置（Ｓ）からサブゴール１（ＳＧ１）までの区間や、サブゴール１（ＳＧ１）からサブゴール２（ＳＧ２）までの区間等に相当し、ロボット１０のセンサ１１１によって障害物や走行面形状を解析可能な部分的な区間である。
　まず、部分経路計画部１３０の最適胴体経路決定部１３１は、例えば図１１に示す最適経路設定対象区間（ＳＧｘ～ＳＧｙ）に、複数の胴体経路（ｐａｔｈ）候補を生成する。
　図１１には、胴体経路（ｐａｔｈ）候補として、経路候補１～経路候補６まで６つの経路候補を設定した例を示している。
　経路候補１は、最適経路設定対象区間（ＳＧｘ～ＳＧｙ）の最短経路であり、経路候補２～６まで、複数の迂回経路を設定した例を示している。
　なお、設定する経路候補の数は、予め規定しておくことが可能である。また、ユーザが、遂次、設定してもよい。また、設定する経路候補のルートについても、予め規定したアルゴリズムに従って設定可能であり、ユーザが逐次設定する構成としてもよい。
　　（ステップＳ１０２）
　次に、ステップＳ１０２において、初期設定処理として、以下の処理を実行する。
　最適経路コスト（ｂｅｓｔ＿ｐａｔｈ＿ｃｏｓｔ）を無限大（∞）に設定する。すなわち、
　最適経路コスト（ｂｅｓｔ＿ｐａｔｈ＿ｃｏｓｔ）←＋∞
　上記の初期設定を行う。
　　（ステップＳ１０３）
　次に、ステップＳ１０３において、コスト未算出経路を選択し、選択経路を（ｐａｔｈ）とする。
　例えば、図１１に示す最適経路設定対象区間（ＳＧｘ～ＳＧｙ）に設定した６つの経路候補１～６から、未処理の経路候補を処理対象として選択する。初期的には、例えば、経路候補１を選択する。
　　（ステップＳ１０４）
　次に、ステップＳ１０４において、ステップＳ１０３で処理対象として選択した選択経路（ｐａｔｈ）の経路コスト（ｐａｔｈ＿ｃｏｓｔ）を算出する。
　この経路コストは、安定した走行が可能なほど値が小さく、不安定な走行が多くなるほど値が大きくなるようなコストである。
　本実施例１では、ロボット１０の胴体部（本体部）１１に左右の傾きが発生する胴体状態が不安定であると判定し、胴体部（本体部）１１の左右の傾きが大きいほど高いコストを算出する。
　例えば図１２に示すように、ロボット１０の左側の脚と右側の脚の接地位置の高さの差分が大きいほど高コストであると判定される。
　ロボット１０の左側の脚と右側の脚の接地位置の高さの差分が大きいほど、ロボット１０の胴体部（本体部）１１の左右の傾きが大きくなり、ロボット１０の安定性が低下するため、このような状態のコストの値は大きな値として算出される。
　なお、コスト算出処理の詳細は後段で説明する。
　図１２に示す例は、ロボット１０の左側の前後の脚のいずれもが、右側の前後の脚の着地位置より高い位置に接地しているが、高コストと判定される状態には、この他にも様々な状態がある。
　図１３は高コストと判定される別の例を示す図である。図１３に示す２つの例は、ロボット１０の左右の前脚の一方の脚のみ接地位置が高い位置にある例と、ロボット１０の左右の後脚の一方の脚のみ接地位置が高い位置にある例である。
　このような場合にも、ロボット１０の胴体部（本体部）１１の左右の傾きが大きくなり、ロボット１０の安定性が低下するため、このような状態のコストの値は大きな値として算出される。
　一方、例えば図１４に示すように、ロボット１０が平面上に位置する場合、すなわち、ロボット１０の４本の脚の接地位置の高さに差がない場合は、ロボット１０の胴体部（本体部）１１に左右の傾きが発生しない。従って、このような状態の場合の算出コスト値は低くなる。
　また、例えば図１５に示すように、ロボット１０が階段を昇るような場合、ロボット１０の前脚は左右いずれも同一高さの面に接地されている。また、ロボット１０の後脚も、左右、同一高さの面に接地されている。このような場合、ロボット１０の胴体部（本体部）１１に前後の傾きは発生するが、左右の傾きは発生しない。従って、このような状態の場合の算出コスト値も低くなる。
　このように本実施例では、ロボット１０の胴体部（本体部）１１の左右の傾きの大きさが大きいほど高コストとなるコスト算出アルゴリズムを適用してコスト算出処理を行う。
　なお、具体例については後段で説明するが、コスト算出アルゴリズムの設定は様々な設定が可能である。利用するコスト算出アルゴリズムを変更すれば、様々なロボット状態に応じて異なる値のコスト算出が可能となる。
　上述したように、ステップＳ１０４では、ステップＳ１０３で処理対象として選択した選択経路（ｐａｔｈ）の経路コスト（ｐａｔｈ＿ｃｏｓｔ）を算出する。
　このステップＳ１０４のコスト算出処理の詳細については、後段において、図１６以下を参照して説明する。
　　（ステップＳ１０５）
　ステップＳ１０４において、１つの選択経路（ｐａｔｈ）のコスト（ｐａｔｈ＿ｃｏｓｔ）算出処理が完了すると、次に、ステップＳ１０５の処理を実行する。
　部分経路計画部１３０の最適胴体経路決定部１３１は、ステップＳ１０５において、ステップＳ１０４でコスト（ｐａｔｈ＿ｃｏｓｔ）算出を行った選択経路（ｐａｔｈ）のコスト（ｐａｔｈ＿ｃｏｓｔ）と、最適経路コスト（ｂｅｓｔ＿ｐａｔｈ＿ｃｏｓｔ）との比較処理を行う。
　比較対象となる最適経路コスト（ｂｅｓｔ＿ｐａｔｈ＿ｃｏｓｔ）は、ステップＳ１０２で初期設定した最適経路コスト（ｂｅｓｔ＿ｐａｔｈ＿ｃｏｓｔ）、あるいは既に算出済みの経路候補対応の最適経路コスト（ｂｅｓｔ＿ｐａｔｈ＿ｃｏｓｔ）である。
　ステップＳ１０４でコスト（ｐａｔｈ＿ｃｏｓｔ）算出を行った選択経路（ｐａｔｈ）のコスト（ｐａｔｈ＿ｃｏｓｔ）の値が、最適経路コスト（ｂｅｓｔ＿ｐａｔｈ＿ｃｏｓｔ）の値より小さい、すなわち、
　（ｐａｔｈ＿ｃｏｓｔ）＜（ｂｅｓｔ＿ｐａｔｈ＿ｃｏｓｔ）
　上記比較判定式が成立する場合は、ステップＳ１０６に進む。
　一方、比較判定式が成立しない場合は、ステップＳ１０７に進む。
　　（ステップＳ１０６）
　ステップＳ１０６の処理は、ステップＳ１０５においてｙｅｓの判定がなされた場合に実行する。
　すなわち、ステップＳ１０４でコスト（ｐａｔｈ＿ｃｏｓｔ）算出を行った選択経路（ｐａｔｈ）のコスト（ｐａｔｈ＿ｃｏｓｔ）の値が、最適経路コスト（ｂｅｓｔ＿ｐａｔｈ＿ｃｏｓｔ）の値より小さい、すなわち、
　（ｐａｔｈ＿ｃｏｓｔ）＜（ｂｅｓｔ＿ｐａｔｈ＿ｃｏｓｔ）
　上記比較判定式が成立する場合に、ステップＳ１０６の処理を実行する。
　この場合、部分経路計画部１３０の最適胴体経路決定部１３１は、ステップＳ１０６において、以下の処理を実行する。
　ステップＳ１０４で算出した選択経路（ｐａｔｈ）の経路コスト（ｐａｔｈ＿ｃｏｓｔ）を、最適経路コスト（ｂｅｓｔ＿ｐａｔｈ＿ｃｏｓｔ）とし、
　コスト（ｐａｔｈ＿ｃｏｓｔ）を算出した選択経路（ｐａｔｈ）を、最適経路（ｂｅｓｔ＿ｐａｔｈ）とする。
　すなわち、以下のデータ置き換え処理を実行する。
　最適経路コスト（ｂｅｓｔ＿ｐａｔｈ＿ｃｏｓｔ）←算出経路コスト（ｐａｔｈ＿ｃｏｓｔ）
　最適経路（ｂｅｓｔ＿ｐａｔｈ）←選択経路（ｐａｔｈ）
　この処理は、選択経路（ｐａｔｈ）の算出経路コスト（ｐａｔｈ＿ｃｏｓｔ）が最も低コストである場合に、この選択経路（ｐａｔｈ）を、最適経路（ｂｅｓｔ＿ｐａｔｈ）に設定し、選択経路（ｐａｔｈ）の算出経路コスト（ｐａｔｈ＿ｃｏｓｔ）を、最適経路コスト（ｂｅｓｔ＿ｐａｔｈ＿ｃｏｓｔ）に設定する処理である。
　　（ステップＳ１０７）
　ステップＳ１０７の処理は、ステップＳ１０６におけるデータ置き換え処理が完了した場合、
　あるいはステップＳ１０５において、
　（ｐａｔｈ＿ｃｏｓｔ）＜（ｂｅｓｔ＿ｐａｔｈ＿ｃｏｓｔ）
　上記比較判定式が成立しなかった場合に実行する。
　この場合、部分経路計画部１３０の最適胴体経路決定部１３１は、ステップＳ１０７において、全ての経路候補のコスト算出処理が完了したか否かを判定する。
　未処理の経路候補がある場合は、ステップＳ１０７の判定がＮｏとなり、ステップＳ１０３に戻り、未処理の経路候補を選択して、選択した未処理の候補経路についてステップＳ１０４以下の処理を実行する。
　ステップＳ１０７において、全ての経路候補のコスト算出処理が完了したと判定した場合は、ステップＳ１０８に進む。
　　（ステップＳ１０８）
　ステップＳ１０７において、全ての経路候補のコスト算出処理が完了したと判定した場合は、ステップＳ１０８の処理を実行する。
　部分経路計画部１３０の最適胴体経路決定部１３１は、ステップＳ１０８において、この時点で決定されている最適経路（ｂｅｓｔ＿ｐａｔｈ）を、次段の経路追従目標速度算出部１３２に出力する。
　なお、出力される最適経路（ｂｅｓｔ＿ｐａｔｈ）は、最も低コストの経路（ｐａｔｈ）である。すなわちロボット１０の胴体部（本体部）１１の左右の傾きの発生度合が最も小さい経路である。
　この選択経路情報は、次段の経路追従目標速度算出部１３２を介して移動装置制御部１４０の駆動情報生成部１４１に出力される。
　移動装置制御部１４０の駆動情報生成部１４１は、最適経路（ｂｅｓｔ＿ｐａｔｈ）に従ってロボット１０を駆走行させるための制御情報を生成して駆動部１５０を制御する。
　この制御により、ロボット１０は、最適経路（ｂｅｓｔ＿ｐａｔｈ）に従って走行する。すなわち、ロボット１０は、複数の経路候補中、胴体部（本体部）１１の左右の傾きを最も小さくして走行可能な最適経路に従って走行することが可能となる。
　次に、図１０のフローチャートを参照して説明した（ステップＳ１０４）の処理、すなわち、ステップＳ１０３で選択した選択経路（ｐａｔｈ）の経路コスト（ｐａｔｈ＿ｃｏｓｔ）の算出処理の詳細シーケンスについて、図１６に示すフローチャートを参照して説明する。
　選択経路（ｐａｔｈ）の経路コスト（ｐａｔｈ＿ｃｏｓｔ）算出処理は、図１６に示すフローのステップＳ２０１～Ｓ２０５の処理に従って実行される。
　以下、各ステップの処理について、順次、説明する。
　　（ステップＳ２０１）
　ステップＳ２０１は、初期設定処理である。
　以下の初期設定を行う。
　コスト算出対象として選択した選択経路（ｐａｔｈ）の経路コスト（ｐａｔｈ＿ｃｏｓｔ）の初期値を０、
　コスト算出胴体位置（ｉ（ｉ＝０～Ｎ））の初期値を０に設定する。
　すなわち、
　経路コスト（ｐａｔｈ＿ｃｏｓｔ）←０
　コスト算出胴体位置（ｉ）←０
　これらの初期設定を行う。
　図１７を参照して、ステップＳ２０１において実行する初期設定の詳細について説明する。
　図１７には、先に図１１を参照して説明した経路候補１～６、すなわち、図１０に示すフローのステップＳ１０１で、最適経路設定対象区間（ＳＧｘ～ＳＧｙ）に設定した６本の経路候補１～６の一部の経路候補１～３を示している。
　図１０に示すフローのステップＳ１０３では、経路候補１～６から、順次、１つの経路候補をコスト算出対象として選択し、ステップＳ１０４、すなわち、図１６に示すフローのステップＳ２０１～Ｓ２０５の処理によって、選択経路のコストの算出処理が実行される。
　コスト算出対象として選択される経路候補は、例えば経路候補１から，経路候補２，３，４，５，６の順に設定される。
　まず、経路候補１がコスト算出対象として選択された場合、ステップＳ２０１では、
　コスト算出対象として選択した選択経路１（ｐａｔｈ１）の経路コスト１（ｐａｔｈ１＿ｃｏｓｔ）の初期値を０、
　コスト算出胴体位置（ｉ（ｉ＝０～Ｎ））の初期値を０に設定する処理を行う。
　コスト算出胴体位置（ｉ（ｉ＝０～Ｎ））は、図１７に示す各経路候補上に等間隔に設定されたサンプリング点（ｉ）、すなわちコスト算出胴体位置（ｉ＝０，１，・・Ｎ）である。
　これらのサンプリング点（ｉ）は、各候補経路上に等間隔で設定される。例えば０．５ｍ間隔でサンプリング点が設定される。
　これらのサンプリング点は、コスト算出胴体位置（ｉ＝０，１，・・Ｎ）を示すサンプリング点である。
　各サンプリング点で、それぞれコストを算出し、１つの経路上の全てのサンプリング点対応のコストの総計（加算値）を経路コスト（ｐａｔｈ＿ｃｏｓｔ）として算出する。
　なお、図１７から理解されるように、経路候補１、経路候補２、経路候補３は、それぞれ、最適経路設定対象区間（ＳＧｘ～ＳＧｙ）を結ぶ経路であるが、各々、長さが異なっている。従って、各経路候補のサンプリング点数も異なる。
　長い経路ほどサンプリング点の数は多くなり、結果として算出される経路コストの値も大きくなる傾向になる。
　ステップＳ２０１では、
　経路コスト（ｐａｔｈ＿ｃｏｓｔ）←０
　コスト算出胴体位置（ｉ）←０
　これらの初期設定が実行される。
　経路コストの初期値を０として、コスト算出胴体位置（ｉ）の初期値を０、すなわち、図も１７に示すように、最適経路設定対象区間（ＳＧｘ～ＳＧｙ）のスタート位置（ＳＧｘ）に最も近いサンプリング点（ｉ＝０）をコスト算出位置として選択する処理が行われる。
　　（ステップＳ２０２）
　ステップＳ２０２は、選択経路（ｐａｔｈ）の１つのサンプリング点（コスト算出胴体位置（ｉ））における胴体状態コストの算出処理ステップである。
　すなわち、選択経路（ｐａｔｈ）のコスト算出胴体位置（ｉ）における胴体状態コスト算出値（ｃａｌｃ＿ｐｏｓｅ＿ｃｏｓｔ（ｂｏｄｙ＿ｐｏｓ［ｉ］））を算出し、胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）を算出値（ｃａｌｃ＿ｐｏｓｅ＿ｃｏｓｔ（ｂｏｄｙ＿ｐｏｓ［ｉ］））とする処理を実行する。
　胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）←胴体状態コスト算出値（ｃａｌｃ＿ｐｏｓｅ＿ｃｏｓｔ（ｂｏｄｙ＿ｐｏｓ［ｉ］））
　上記の胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）算出処理を実行する。
　なお、先に図１２～図１５を参照して説明したように、本実施例におけるコスト（胴体状態コスト）の算出処理においては、胴体部（本体部）１１の左右の傾きが大きいほど高いコストとなるようなコスト算出アルゴリズムが適用される。
　このように、ステップＳ２０２では、１つの選択経路（ｐａｔｈ）の１つのサンプリング点（ｉ）におけるコスト（胴体状態コスト）算出処理を実行する。
　このコスト算出処理の詳細シーケンスについては、後段で図１８以下のフローチャートを参照して説明する。
　　（ステップＳ２０３）
　ステップＳ２０２において、１つの選択経路（ｐａｔｈ）の１つのサンプリング点（ｉ）におけるコスト（胴体状態コスト）算出処理が完了すると、次にステップＳ２０３の処理を実行する。
　ステップＳ２０３では、選択経路（ｐａｔｈ）の経路コスト（ｐａｔｈ＿ｃｏｓｔ）の更新処理を行う。
　すなわち、ステップＳ２０２で算出した胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）を、経路コスト（ｐａｔｈ＿ｃｏｓｔ）に加算して、経路コスト（ｐａｔｈ＿ｃｏｓｔ）を更新する処理を実行する。
　経路コスト（ｐａｔｈ＿ｃｏｓｔ）←経路コスト（ｐａｔｈ＿ｃｏｓｔ）＋胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）
　上記の経路コスト（ｐａｔｈ＿ｃｏｓｔ）更新処理を実行する。
　　（ステップＳ２０４）
　次に、ステップＳ２０４において、サンプリング点（コスト算出胴体位置（ｉ））の更新処理を実行する。
　すなわち、選択経路（ｐａｔｈ）のコスト算出胴体位置（ｉ（ｉ＝０～Ｎ））を更新する。
　ｉ←ｉ＋１
　上記式に従って、サンプリング点（コスト算出胴体位置（ｉ））を１つ進める。
　　（ステップＳ２０５）
　ステップＳ２０５では、ステップＳ２０４におけるサンプリング点（コスト算出胴体位置（ｉ））の更新処理によって更新されたサンプリング点（コスト算出胴体位置（ｉ））の値が選択経路（ｐａｔｈ）に設定したサンプリング点数（Ｎ）より小さな値であるか否かを判定する。
　すなわち、
　ｉ＜Ｎ？
　上記判定式が成立するか否かを判定する。
　上記判定式が成立する場合は、コスト算出処理が未処理のサンプリング点（コスト算出胴体位置（ｉ））が存在することを意味する。
　この場合は、ステップＳ２０２に戻り、更新されたサンプリング点（コスト算出胴体位置（ｉ））についての胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）の算出処理を実行する。すなわちステップＳ２０２以下の処理を実行する。
　一方、上記判定式が成立しない場合は、コスト算出処理が未処理のサンプリング点（コスト算出胴体位置（ｉ））が存在せず、選択経路（ｐａｔｈ）上の全てのサンプリング点（コスト算出胴体位置（ｉ＝０～Ｎ））についての胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）の算出処理が完了したことを意味する。
　この場合は、図１０に示すフローのステップＳ１０５に進む。
　次に、図１８以下を参照して、図１６のフローチャート中のステップＳ２０２の処理、すなわち、サンプリング点（コスト算出胴体位置（ｉ））における胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）の算出処理の詳細シーケンスについて説明する。
　図１８～図２０は、図１６のフローチャート中のステップＳ２０２の処理の詳細シーケンスについて説明するフローチャートである。
　コスト算出対象として選択された１つの選択経路（ｐａｔｈ）の１つのサンプリング点（コスト算出胴体位置（ｉ））における胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）の算出処理は、図１８～図２０に示すステップＳ３０１～Ｓ３１７の処理に従って実行される。
　以下、各ステップの処理について、順次、説明する。
　　（ステップＳ３０１）
　ステップＳ３０１は、ステップＳ３０２～ステップＳ３０６の処理ループ開始前の事前準備処理である。
　ステップＳ３０１では、まず、ロボットの１つの脚の最小高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ）と、最大高さ（ｍａｘ＿ｈｅｉｇｈｔｓ）を変数に設定する。
　すなわち、
　（ｍｉｎ＿ｈｅｉｇｈｔs）←（４要素の配列）
　（ｍａｘ＿ｈｅｉｇｈｔs）←（４要素の配列）
　上記のように、ロボット１０の１つの脚の最小高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ）と、最大高さ（ｍａｘ＿ｈｅｉｇｈｔｓ）を変数に設定する。
　　（ステップＳ３０２）
　ステップＳ３０２～ステップＳ３０６は、ロボット１０の脚単位の処理の処理ループである。
　本実施例ではロボット１０は４本の脚を有している。脚識別子をｋ（ｋ＝１，２，３，４）として、処理対象の脚（ｋ）を順次、選択し、選択した処理対象の脚（ｋ）についてステップＳ３０３～ステップＳ３０５の処理を実行する。
　　（ステップＳ３０３）
　ステップＳ３０３では、選択した処理対象の脚（ｋ）について以下の処理を実行する。
　コスト算出胴体位置（ｉ）における胴体位置（ｂｏｄｙ＿ｐｏｓ）と、周囲環境地図を参照して、脚（ｋ）の脚配置可能領域を抽出する。
　このステップＳ３０３の処理の具体例について図２１を参照して説明する。
　図２１には、ロボット１０を示している。このロボット１０はコスト算出胴体位置（ｉ）にあるものとする。
　この位置において、ロボット１０は、各脚を各脚の可動範囲内で動かして接地することができる。
　図には、各脚の可動範囲、すなわち接地可能領域をサークル状の実線で示している。
　ロボット１０は、各脚を接地可能領域内で自在に移動させて接地させることができる。
　例えば、左前脚を脚（１）、右前脚を脚（２）、左後脚を脚（３）、左後脚を脚（４）とした場合、左前脚（脚（１））は、図に示す脚（１）周囲の接地可能領域内で自在に移動させて接地させることができる。
　右前脚（脚（２））は、図に示す脚（２）周囲の接地可能領域内で自在に移動させて接地させることができる。
　左後脚（脚（３））、左後脚（脚（４））もそれぞれ各脚周囲の接地可能領域内で自在に移動させて接地させることができる。
　ステップＳ３０３では、選択した処理対象の脚（ｋ）について、コスト算出胴体位置（ｉ）における脚配置可能領域を抽出する。すなわち、図２１に示す４本の脚から選択された１つの脚（ｋ）について、その周囲のサークル状の実線で示す脚配置可能領域を抽出する。
　　（ステップＳ３０４～Ｓ３０５）
　次に、ステップＳ３０４とステップＳ３０５において、処理対象として選択した脚（ｋ）について、以下の処理を実行する。
　ステップＳ３０４＝コスト算出胴体位置（ｉ）における脚（ｋ）の脚配置可能領域内の最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（ｋ））を算出する。
　ステップＳ３０５＝コスト算出胴体位置（ｉ）における脚（ｋ）の脚配置可能領域内の最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（ｋ））を算出する。
　この処理の具体例について、図２２を参照して説明する。
　図２２には、処理対象として選択した脚（ｋ）を右前脚（脚（２））とした場合の処理例を示している。
　右前脚（脚（２））の周囲には、サークル状の実線で示す脚配置可能領域を示している。
　図２２に示す左上の（ａ）と右下の（ｂ）の各図は、右前脚（脚（２））の異なる接地状態を示す図である。
　（ａ）は、右前脚（脚（２））が、脚配置可能領域内にある段差障害物に乗り上げておらず、低い平面に接地した状態を示している。
　（ｂ）は、右前脚（脚（２））が、脚配置可能領域内にある段差障害物に乗り上げ、段差障害物の上面に接地した状態を示している。
　右前脚（脚（２））の周囲のサークル状の実線で示す脚配置可能領域内には、ことように脚の接地高さが異なる走行面が混在する。
　ステップＳ３０４は、脚（ｋ）の脚配置可能領域内の最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（ｋ））を算出する処理であり、図２２に示す右前脚（脚（２））の場合、図２２（ａ）に示すように、段差障害物に乗らない状態の脚（２）の接地高さを、最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（ｋ））として算出する処理を行う。
　また、ステップＳ３０５は、脚（ｋ）の脚配置可能領域内の最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（ｋ））を算出する処理であり、図２２に示す右前脚（脚（２））の場合、図２２（ｂ）に示す段差障害物上に乗った脚（２）の接地高さを、最大脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（ｋ））として算出する処理を行う。
　このように、ステップＳ３０４～Ｓ３０５では、コスト算出胴体位置（ｉ）における脚（ｋ）の脚配置可能領域内の最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（ｋ））と、最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（ｋ））を算出する。
　前述したように、ステップＳ３０２～Ｓ３０６は、ロボット１０の脚単位の処理の処理ループである。
　本実施例ではロボット１０は４本の脚を有しており、ステップＳ３０２～Ｓ３０６の処理ループが完了すると、以下の各データ（１）～（４）が算出されることになる。
　コスト算出胴体位置（ｉ）における、
　（１）左前脚（脚（１））の最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（１））と、最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（１））、
　（２）右前脚（脚（２））の最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（２））と、最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（２））、
　（３）左後脚（脚（３））の最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（３））と、最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（３））、
　（４）右後脚（脚（４））の最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（４））と、最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（４））、
　ステップＳ３０２～Ｓ３０６の処理ループが完了すると、図１９に示すフローのステップＳ３１１に進む。
　　（ステップＳ３１１）
　ステップＳ３１１は、ステップＳ３１２～ステップＳ３１７の処理ループ実行前の事前準備処理である。
　ステップＳ３１１では、最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）をマイナス無限大（－∞）に設定する。
　最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）←－∞
　ステップＳ３１２～ステップＳ３１７の処理ループ実行前に、上記のコスト初期設定処理を行う。
　　（ステップＳ３１２）
　ステップＳ３１２～ステップＳ３１７は、ロボット１０の脚配置状態単位の処理の処理ループである。
　ステップＳ３１２～Ｓ３１７は、コスト算出胴体位置（ｉ）における各脚（ｋ＝１～４）各々の、
　「最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（ｋ））」と、
　「最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（ｋ））」
　これらの各組み合わせ（２×２×２×２）＝１６通り、各々についての処理ループである。
　本実施例ではロボット１０は４本の脚を有しており、先の脚単位の処理ループであるステップＳ３０２～Ｓ３０６において、以下のデータが算出されている。
　コスト算出胴体位置（ｉ）における、
　（１）左前脚（脚（１））の最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（１））と、最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（１））、
　（２）右前脚（脚（２））の最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（２））と、最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（２））、
　（３）左後脚（脚（３））の最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（３））と、最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（３））、
　（４）右後脚（脚（４））の最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（４））と、最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（４））、
　ステップＳ３１２～Ｓ３１７は、これら各脚（ｋ＝１～４）各々の、
　「最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（ｋ））」と、
　「最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（ｋ））」
　これらの各組み合わせ（２×２×２×２）＝１６通り、各々についての処理ループである。
　図２３を参照して、ステップＳ３１２～Ｓ３１７の処理ループにおいて実行される（２×２×２×２）＝１６通りの各脚（ｋ＝１～４）の設定例について説明する。
　ステップＳ３１２～Ｓ３１７の処理ループでは、ロボット１０の４本の脚の接地高さの組み合わせパターン１６通り各々について、順次処理が行われる。
　図２３にはこの１６通りの例を示している。
　図２３に示す表は、
　左前脚（ＦＬ）、右前脚（ＦＲ）、左後脚（ＢＬ）、右後脚（ＦＬ）の接地位置（Ｌｏｗ，Ｈｉｇｈ）の無組み合わせパターン１６通りを示す表である。
　なお、ＬｏｗとＨｉｇｈは、ステップＳ３０２～Ｓ３０７の処理ループで算出した「最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（ｋ））」と、「最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（ｋ））」に対応する。すなわち、
　Ｌｏｗは、「最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（ｋ））」
　Ｈｉｇｈは、「最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（ｋ））」
　である。
　図２３の表に示す最初のエントリ１（Ｌｏｗ，Ｌｏｗ，Ｌｏｗ，Ｌｏｗ）は、図２３の右側の図（１）に示すように、ロボット１０の４本の脚が全て、段差障害物に乗らず低い走行面に接地した状態に対応する。
　また、図２３の表に示すエントリ４（Ｌｏｗ，Ｈｉｇｈ，Ｌｏｗ，Ｌｏｗ）は、図２３の右側の図（２）に示すように、ロボット１０の右前脚のみが段差障害物上の高い走行面に接地し、残りの３本の脚が段差障害物に乗らず低い走行面に接地した状態に対応する。
　このように、ロボット１０の４本の脚の接地高さの組み合わせパターンは、１６通りであり、ステップＳ３１２～Ｓ３１７の処理ループでは、これらの組み合わせパターン１６通り各々について、順次処理が行われる。
　ただし、コスト算出胴体位置（ｉ）において、ロボット１０の４本の脚の全てがＬｏｗとＨｉｇｈの２つの異なる高さの脚配置、すなわち、「最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（ｋ））」と、「最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（ｋ））」、これら２種類の脚配置が可能であるとは限らない。
　コスト算出胴体位置（ｉ）に全く段差がない場合、あるいは一部の脚のみを段差に乗せることができるような場合もある。
　具体例について図２４を参照して説明する。
　図２４の右側に示す例は、ロボット１０の左前脚のみ、段差の上と下の２つの異なる高さの位置に接地可能であり、この左前脚以外の３本の脚は、脚配置可能領域が段差のない平面領域である。
　このような場合は、ロボット１０の４本の脚の接地高さの組み合わせパターンは、１６通りではなく、２通りのみとなる。
　図２４の右側の図（１）の状態、すなわち、ロボット１０の４本の脚が全て、段差障害物に乗らず低い走行面に接地した状態は、図２４の表に示す最初のエントリ１（Ｌｏｗ，Ｌｏｗ，Ｌｏｗ，Ｌｏｗ）に相当する。
　また、図２４の右側の図（２）の状態、すなわち、ロボット１０の左前脚のみが段差障害物上の高い走行面に接地し、残りの３本の脚が段差障害物に乗らず低い走行面に接地した状態は、図２４の表に示すエントリ５（Ｈｉｇｈ，Ｌｏｗ，Ｌｏｗ，Ｌｏｗ）に相当する。
　サンプリング点（コスト算出胴体位置（ｉ））が、図２４右側に示すように、左前脚のみを段差に乗せることができるような設定の場合は、上記の２通りの脚配置パターンしかない。このような場合は、ステップＳ３１２～Ｓ３１７の処理ループは、１６通りではなく、この２通りの脚配置パターンに対してのみ実行すればよい。
　また、例えばサンプリング点（コスト算出胴体位置（ｉ））が、図２５右側に示すように、全ての脚の脚配置可能領域が同一高さの平面である場合は、図２５の表に示す最初のエントリ１（Ｌｏｗ，Ｌｏｗ，Ｌｏｗ，Ｌｏｗ）のみの脚配置パターンしかない。
　このような場合は、ステップＳ３１２～Ｓ３１７の処理ループは、１６通りではなく、この１つの脚配置パターンに対してのみ実行すればよい。
　　（ステップＳ３１３）
　ステップＳ３１３～Ｓ３１６は、上記１６通りのいずれか１つの脚配置パターンに対して、順次、実行する処理ステップである。
　まず、ステップＳ３１３において、処理対象とした１つの脚配置パターンについて、以下の処理を実行する。
　ｆ＿ｐｏｓｅを、脚（ｋ）がロボットの前脚である場合の脚配置状態（ｌｅｇ＿ｐｏｓｅ（ｋ））とし、ｂ＿ｐｏｓｅを、脚（ｋ）がロボットの後脚である場合の脚配置状態（ｌｅｇ＿ｐｏｓｅ（ｋ））とする。
　すなわち、
　ｆ＿ｐｏｓｅ←｛ｌｅｇ＿ｐｏｓｅ（ｋ）：脚（ｋ）がロボットの前脚｝
　ｂ＿ｐｏｓｅ←｛ｌｅｇ＿ｐｏｓｅ（ｋ）：脚（ｋ）がロボットの後脚｝
　上記のデータ定義を実行する。
　　（ステップＳ３１４）
　次に、ステップＳ３１４において、１つのサンプリング点（コスト算出胴体位置（ｉ））における１つの脚配置パターンに対応する胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）算出処理を実行する。
　具体的には、以下の胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）の算出処理を実行する。
　前脚の最大高さと最小高さの差分（ｍａｘ（ｆ＿ｐｏｓｅ）－ｍｉｎ（ｆ＿ｐｏｓｅ））と、後脚の最大高さと最小高さの差分（ｍａｘ（ｂ＿ｐｏｓｅ）－ｍｉｎ（ｂ＿ｐｏｓｅ））との加算値を、胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）として算出する。
　すなわち、
　胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）←
　（ｍａｘ（ｆ＿ｐｏｓｅ）－ｍｉｎ（ｆ＿ｐｏｓｅ））
　＋（ｍａｘ（ｂ＿ｐｏｓｅ）－ｍｉｎ（ｂ＿ｐｏｓｅ））
　上記式に従って、胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）の算出処理を実行する。
　このステップＳ３１４において算出される胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）は、左前脚と右前脚との接地高さの差分が大きいほど大きくなり、左後脚と右後脚との接地高さの差分が大きいほど大きくなる。
　また、左前脚と右前脚との接地高さの差分が小さいほど小さくなり、左後脚と右後脚との接地高さの差分が小さいほど小さくなる。
　このコスト算出アルゴリズムは、先に図１２～図１５を参照して説明したように、胴体部（本体部）１１の左右の傾きが大きいほど高いコストとなるコスト算出アルゴリズムである。
　　（ステップＳ３１５）
　ステップＳ３１４において、１つのサンプリング点（コスト算出胴体位置（ｉ））における１つの脚配置パターンに対応する胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）算出処理を完了すると、ステップＳ３１５において、算出した胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）と、最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）との比較処理を実行する。
　すなわち、
　胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）＞最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）？
　上記判定式が成立するか否かを判定する。
　上記判定式は、ステップＳ３１４で算出した１つの脚配置パターンに対応する胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）が、初期設定値、または既に算出済みの最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）より大きい値であるか否かを判定する判定式である。
　ステップＳ３１４で算出した１つの脚配置パターンに対応する胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）が、初期設定値、または既に算出済みの最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）より大きい値であると判定した場合は、ステップＳ３１５の判定はＹｅｓとなり、ステップＳ３１６に進む。
　一方、ステップＳ３１４で算出した１つの脚配置パターンに対応する胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）が、初期設定値、または既に算出済みの最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）より大きい値でないと判定した場合は、ステップＳ３１５の判定はＮｏとなり、ステップＳ３１７に進む。
　　（ステップＳ３１６）
　ステップＳ３１６の処理は、ステップＳ３１５においてＹｅｓの判定がなされた場合、すなわち、ステップＳ３１４で算出した１つの脚配置パターンに対応する胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）が、初期設定値、または既に算出済みの最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）より大きい値であると判定した場合に実行する。
　この場合は、ステップＳ３１６において、最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）を、ステップＳ３１４で算出した胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）に設定する処理を実行する。
　すなわち、
　最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）←胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）
　上記式に従って、最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）を、ステップＳ３１４で算出した胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）に設定する処理を実行する。
　最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）となる脚配置パターンの例を図２６に示す。
　図２６に示す例は、ロボット１０の左側の前後の脚が、右側の前後の脚の着地位置より高い位置に接地した脚配置パターンである。このような脚配置パターンの場合、胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）の値は大きくなり、最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）に相当するコストとなる可能性が高い。
　　（ステップＳ３１７）
　ステップＳ３１７は、ステップＳ３１２～Ｓ３１７の処理ループの終了ステップである。
　１つのサンプリング点（コスト算出胴体位置（ｉ））において、ロボット１０の４本の脚の接地高さの組み合わせパターン、最大１６通りについて、ステップＳ３１３～ステップＳ３１６の処理を実行し、各パターン対応の胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）を算出し、これらの中で最もコストの高い最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）を、胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）として算出する。
　この処理の完了後、図１６に示すフローのステップＳ２０３に進む。
　ステップＳ２０３以下の処理について、再度、説明する。
　ステップＳ２０３では、選択経路（ｐａｔｈ）の経路コスト（ｐａｔｈ＿ｃｏｓｔ）の更新処理を行う。
　すなわち、ステップＳ２０２で算出した胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）、すなわち、１つのサンプリング点（コスト算出胴体位置（ｉ））において最もコストの高い最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）に相当する胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）を、コスト算出対象の経路コスト（ｐａｔｈ＿ｃｏｓｔ）に加算して、経路コスト（ｐａｔｈ＿ｃｏｓｔ）を更新する処理を実行する。
　経路コスト（ｐａｔｈ＿ｃｏｓｔ）←経路コスト（ｐａｔｈ＿ｃｏｓｔ）＋胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）
　上記の経路コスト（ｐａｔｈ＿ｃｏｓｔ）更新処理を実行する。
　図１６に示すステップＳ２０１～Ｓ２０５では、１つの選択経路に設定した全てのサンプリング点（コスト算出胴体位置（ｉ））において最もコストの高い最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）に相当する胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）を全て加算して、１つの選択経路の経路コスト（ｐａｔｈ＿ｃｏｓｔ）が算出される。
　図１６に示すステップＳ２０１～Ｓ２０５の処理は、図１０に示すフローのステップＳ１０４の処理であり、このステップＳ１０４で１つの選択経路の経路コスト（ｐａｔｈ＿ｃｏｓｔ）算出処理が完了すると、図１０に示すフローのステップＳ１０５に進む。
　ステップＳ１０５以下の処理について、再度、説明する。
　　（ステップＳ１０５）
　ステップＳ１０５では、ステップＳ１０４（＝図１６に示すステップＳ２０１～Ｓ２０５）でコスト（ｐａｔｈ＿ｃｏｓｔ）算出を行った選択経路（ｐａｔｈ）のコスト（ｐａｔｈ＿ｃｏｓｔ）と、最適経路コスト（ｂｅｓｔ＿ｐａｔｈ＿ｃｏｓｔ）との比較処理を行う。
　比較対象となる最適経路コスト（ｂｅｓｔ＿ｐａｔｈ＿ｃｏｓｔ）は、ステップＳ１０２で初期設定した最適経路コスト（ｂｅｓｔ＿ｐａｔｈ＿ｃｏｓｔ）、あるいは既に算出済みの経路候補対応の最適経路コスト（ｂｅｓｔ＿ｐａｔｈ＿ｃｏｓｔ）である。
　ステップＳ１０４でコスト（ｐａｔｈ＿ｃｏｓｔ）算出を行った選択経路（ｐａｔｈ）のコスト（ｐａｔｈ＿ｃｏｓｔ）の値が、最適経路コスト（ｂｅｓｔ＿ｐａｔｈ＿ｃｏｓｔ）の値より小さい、すなわち、
　（ｐａｔｈ＿ｃｏｓｔ）＜（ｂｅｓｔ＿ｐａｔｈ＿ｃｏｓｔ）
　上記比較判定式が成立する場合は、ステップＳ１０６に進む。
　一方、比較判定式が成立しない場合は、ステップＳ１０７に進む。
　　（ステップＳ１０６）
　ステップＳ１０６の処理は、ステップＳ１０５においてｙｅｓの判定がなされた場合に実行する。
　すなわち、ステップＳ１０４でコスト（ｐａｔｈ＿ｃｏｓｔ）算出を行った選択経路（ｐａｔｈ）のコスト（ｐａｔｈ＿ｃｏｓｔ）の値が、最適経路コスト（ｂｅｓｔ＿ｐａｔｈ＿ｃｏｓｔ）の値より小さい、すなわち、
　（ｐａｔｈ＿ｃｏｓｔ）＜（ｂｅｓｔ＿ｐａｔｈ＿ｃｏｓｔ）
　上記比較判定式が成立する場合に、ステップＳ１０６の処理を実行する。
　この場合、部分経路計画部１３０の最適胴体経路決定部１３１は、ステップＳ１０６において、以下の処理を実行する。
　ステップＳ１０４で算出した選択経路（ｐａｔｈ）の経路コスト（ｐａｔｈ＿ｃｏｓｔ）を、最適経路コスト（ｂｅｓｔ＿ｐａｔｈ＿ｃｏｓｔ）とし、
　コスト（ｐａｔｈ＿ｃｏｓｔ）を算出した選択経路（ｐａｔｈ）を、最適経路（ｂｅｓｔ＿ｐａｔｈ）とする。
　すなわち、以下のデータ置き換え処理を実行する。
　最適経路コスト（ｂｅｓｔ＿ｐａｔｈ＿ｃｏｓｔ）←算出経路コスト（ｐａｔｈ＿ｃｏｓｔ）
　最適経路（ｂｅｓｔ＿ｐａｔｈ）←選択経路（ｐａｔｈ）
　この処理は、選択経路（ｐａｔｈ）の算出経路コスト（ｐａｔｈ＿ｃｏｓｔ）が最も低コストである場合に、この選択経路（ｐａｔｈ）を、最適経路（ｂｅｓｔ＿ｐａｔｈ）に設定し、選択経路（ｐａｔｈ）の算出経路コスト（ｐａｔｈ＿ｃｏｓｔ）を、最適経路コスト（ｂｅｓｔ＿ｐａｔｈ＿ｃｏｓｔ）に設定する処理である。
　　（ステップＳ１０７）
　ステップＳ１０７の処理は、ステップＳ１０６におけるデータ置き換え処理が完了した場合、
　あるいはステップＳ１０５において、
　（ｐａｔｈ＿ｃｏｓｔ）＜（ｂｅｓｔ＿ｐａｔｈ＿ｃｏｓｔ）
　上記比較判定式が成立しなかった場合に実行する。
　この場合、部分経路計画部１３０の最適胴体経路決定部１３１は、ステップＳ１０７において、全ての経路候補のコスト算出処理が完了したか否かを判定する。
　未処理の経路候補がある場合は、ステップＳ１０７の判定がＮｏとなり、ステップＳ１０３に戻り、未処理の経路候補を選択して、選択した未処理の候補経路についてステップＳ１０４以下の処理を実行する。
　ステップＳ１０７において、全ての経路候補のコスト算出処理が完了したと判定した場合は、ステップＳ１０８に進む。
　　（ステップＳ１０８）
　ステップＳ１０７において、全ての経路候補のコスト算出処理が完了したと判定した場合は、ステップＳ１０８の処理を実行する。
　部分経路計画部１３０の最適胴体経路決定部１３１は、ステップＳ１０８において、この時点で決定されている最適経路（ｂｅｓｔ＿ｐａｔｈ）を、次段の経路追従目標速度算出部１３２に出力する。
　なお、出力される最適経路（ｂｅｓｔ＿ｐａｔｈ）は、最も低コストの経路（ｐａｔｈ）である。すなわちロボット１０の胴体部（本体部）１１の左右の傾きの発生度合が最も小さい経路である。
　この選択経路情報は、次段の経路追従目標速度算出部１３２を介して移動装置制御部１４０の駆動情報生成部１４１に出力される。
　移動装置制御部１４０の駆動情報生成部１４１は、最適経路（ｂｅｓｔ＿ｐａｔｈ）に従ってロボット１０を駆走行させるための制御情報を生成して駆動部１５０を制御する。
　この制御により、ロボット１０は、最適経路（ｂｅｓｔ＿ｐａｔｈ）に従って走行する。すなわち、ロボット１０は、複数の経路候補中、胴体部（本体部）１１の左右の傾きを最も小さくして走行可能な最適経路に従って走行することが可能となる。
　部分経路計画部１３０の最適胴体経路決定部１３１の実行する処理をまとめて記載すると以下のような処理となる。
　部分経路計画部１３０の最適胴体経路決定部１３１は、まず、部分経路生成区間、例えば、先に図９を参照して説明した部分経路生成区間（ＳＧｘ～ＳＧｙ）の各区間において、複数の経路候補を生成し、さらに各経路候補に複数のサンプリング点（コスト算出胴体位置（ｉ））を設定する。
　次に、複数のサンプリング点（コスト算出胴体位置（ｉ））各々について可能な複数の脚配置パターン中、最もコストが高くなる脚配置パターン対応のコストを胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）として算出する。
　次に、経路候補内の全てのサンプリング点（コスト算出胴体位置（ｉ））の胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）の加算値（総計）を経路コスト（ｐｑｔｈ＿ｃｏｓｔ）として算出する。
　最後に、複数の経路候補各々の経路コスト（ｐｑｔｈ＿ｃｏｓｔ）の中で、最も小さなコスト値を持つ経路を最適経路（ｂｅｓｔ＿ｐａｔｈ）として選択する。
　部分経路計画部１３０の最適胴体経路決定部１３１は、このような処理を実行して、部分経路生成区間の各々について、最も小さなコスト値を持つ経路を、最適経路（ｂｅｓｔ＿ｐａｔｈ）として生成する。
　生成した最適経路（ｂｅｓｔ＿ｐａｔｈ）は、最も低コストの経路（ｐａｔｈ）であり、ロボット１０の胴体部（本体部）１１の左右の傾きの発生度合を最も小さくして走行することが可能な経路である。
　この制御により、ロボット１０は、最適経路（ｂｅｓｔ＿ｐａｔｈ）に従って走行する。すなわち、ロボット１０は、複数の経路候補中、胴体部（本体部）１１の左右の傾きを最も小さくして走行可能な最適経路に従って走行することが可能となる。
　　［４．（実施例２）ユーザ定義のコスト算出アルゴリズムを適用した実施例］
　次に、実施例２として、ユーザ定義のコスト算出アルゴリズムを適用した実施例について説明する。
　図１０～図２６を参照して説明した実施例１は、ロボット１０の胴体部（本体部）１１の左右の傾きの大きさが大きいほど高コストとなるコスト算出アルゴリズムを適用してコスト算出処理を行う実施例である。
　しかし、前述したように、コスト算出アルゴリズムには様々なアルゴリズムが適用可能である。利用するコスト算出アルゴリズムを変更すれば、様々なロボット状態に応じて異なる値のコスト算出が可能となる。
　以下、実施例２として、ユーザ定義のコスト算出アルゴリズムを適用した実施例について説明する。
　実施例２におけるロボット１０の構成は、先に図８を参照して説明した構成と同様の構成である。
　実施例２では、部分経路計画部１３０の最適胴体経路決定部１３１において実行する処理、すなわち、障害物や段差を可能な限り避けて安全に走行可能な経路（部分経路）を生成する処理が異なる。
　先に図９を参照して説明したように、最適胴体経路決定部１３１は、センサ１１１によって障害物や走行面形状を解析可能な範囲について、障害物や段差を可能な限り避けて安全に走行可能な部分経路（図９に点線で示す部分経路）を順次、生成する。
　部分経路計画部１３０の最適胴体経路決定部１３１は、部分経路の決定区間内にロボット１０が走行可能な複数の経路を設定し、各経路についてのコストを算出する。コストは安定した走行が可能なほど値が小さく、不安定な走行が多くなるほど値が大きくなるようなコストである。最適胴体経路決定部１３１は、ロボット１０の走行可能な複数の経路についてコストを算出して、各経路の評価処理を実行する。
　さらに、この評価処理の結果に基づいて最もコストの低い経路、すなわち安定走行が可能な経路を選択し、選択した経路を走行する。
　先に図１０～図２６を参照して説明した実施例１では、経路のコスト算出処理として、ロボット１０の胴体部（本体部）１１の左右の傾きの大きさが大きいほど高コストとなるコスト算出アルゴリズムを適用した処理を行っていた。
　これに対して、本実施例２では、ユーザ定義のコスト算出アルゴリズムを適用している。
　すなわち、ユーザ定義のコスト算出関数を利用して経路コストを算出する。
　部分経路計画部１３０の最適胴体経路決定部１３１が実行する本実施例２の最適部分経路決定処理シーケンスについて説明する。
　本実施例２の最適部分経路決定処理シーケンスの基本シーケンスは、先に実施例１の基本シーケンスとして説明した図１０に示すフローチャートに従った処理と同様のシーケンスである。
　また、図１０に示すフロー中のステップＳ１０４の処理、すなわち、選択経路（ｐａｔｈ）の経路コスト（ｐａｔｈ＿ｃｏｓｔ）算出処理のシーケンスも、先に実施例１で説明した図１６のフローチャートに従った処理となる。
　本実施例２では、図１６に示すフロー中のステップＳ２０２の処理、すなわち、サンプリング点（コスト算出胴体位置（ｉ））における胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）の算出処理において適用するコスト算出アルゴリズムが異なるものとなる。
　すなわち、実施例１において、図１８～図２０を参照して説明したサンプリング点（コスト算出胴体位置（ｉ））の胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）算出処理のコスト算出ステップであるステップＳ３１３～ステップＳ３１４の処理が異なる処理となる。
　図２７～図２９に示すフローチャートを参照して、本実施例２のサンプリング点（コスト算出胴体位置（ｉ））における胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）の算出処理の詳細シーケンスについて説明する。
　図２７～図２９に示すフローチャートは、先の実施例１で説明した図１６に示すフロー中のステップＳ２０２の処理の詳細シーケンスに相当する。
　本実施例２において、コスト算出対象として選択された１つの選択経路（ｐａｔｈ）の１つのサンプリング点（コスト算出胴体位置（ｉ））における胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）の算出処理は、図２７～図２９に示すステップＳ４０１～Ｓ４１７の処理に従って実行される。
　以下、各ステップの処理について、順次、説明する。
　　（ステップＳ４０１～Ｓ４０６）
　ステップＳ４０１～Ｓ４０６の処理は、先の実施例１において図１６に示すフローチャートを参照して説明したステップＳ３０１～Ｓ３０６の処理と同様の処理である。
　ステップＳ４０１において
　（ｍｉｎ＿ｈｅｉｇｈｔｓ）←（４要素の配列）
　（ｍａｘ＿ｈｅｉｇｈｔｓ）←（４要素の配列）
　上記のように、ロボット１０の１つの脚の最小高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ）と、最大高さ（ｍａｘ＿ｈｅｉｇｈｔｓ）を変数に設定する。
　ステップＳ４０２～Ｓ４０６は、ロボット１０の脚単位の処理の処理ループである。
　ステップＳ４０２～Ｓ４０６の処理ループの実行により、以下の各データ（１）～（４）が算出される。
　コスト算出胴体位置（ｉ）における、
　（１）左前脚（脚（１））の最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（１））と、最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（１））、
　（２）右前脚（脚（２））の最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（２））と、最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（２））、
　（３）左後脚（脚（３））の最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（３））と、最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（３））、
　（４）右後脚（脚（４））の最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（４））と、最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（４））、
　ステップＳ４０２～Ｓ４０６の処理ループが完了すると、図２８に示すフローのステップＳ４１１に進む。
　　（ステップＳ４１１）
　ステップＳ４１１の処理は、先の実施例１において図１７に示すフローチャートを参照して説明したステップＳ３１１の処理と同様の処理であり、ステップＳ４１２～ステップＳ４１７の処理ループ実行前の事前準備処理である。
　ステップＳ４１１では、最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）をマイナス無限大（－∞）に設定する。
　最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）←－∞
　ステップＳ４１２～ステップＳ４１７の処理ループ実行前に、上記のコスト初期設定処理を行う。
　　（ステップＳ４１２）
　ステップＳ４１２～ステップＳ４１７は、ロボット１０の脚配置状態単位の処理の処理ループである。
　すなわち、先に図２３を参照して説明したように、コスト算出胴体位置（ｉ）における各脚（ｋ＝１～４）各々の、
　「最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（ｋ））」と、
　「最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（ｋ））」
　これらの各組み合わせ（２×２×２×２）＝１６通り、各々についての処理ループである。
　なお、先に図２４、図２５を参照して説明したように、サンプリング点（コスト算出胴体位置（ｉ））が、図２４、図２５に示すように、各脚の脚配置可能領域が同一高さの平面であるような場合は、可能な脚配置パターンのみ処理ループを実行すればよい。
　　（ステップＳ４１３）
　ステップＳ４１３～Ｓ４１６は、上記１６通りのいずれか１つの脚配置パターンに対して、順次、実行する処理ステップである。
　まず、ステップＳ４１３において、処理対象とした１つの脚配置パターンについて、以下の処理を実行する。
　ユーザの定義したコスト算出アルゴリズム（ｕｓｅｒ＿ａｌｇｏｒｉｔｈｍ）に従って、胴体状態コスト算出値（ｃａｌｃ＿ｐｏｓｅ＿ｃｏｓ（ｕｓｅｒ＿ａｌｇｏｒｉｔｈｍ）を算出する。
　なお、ユーザが定義するコスト算出アルゴリズム（ｕｓｅｒ＿ａｌｇｏｒｉｔｈｍ）としては様々な異なる設定のアルゴリズムが適用可能である。具体的には、例えば、ロボット使用環境やユーザにとって望ましくないロボット状態に対応する算出コスト値が高くなるようなコスト算出アルゴリズムを生成して利用することができる。
　ユーザ定義コスト算出アルゴリズム（ｕｓｅｒ＿ａｌｇｏｒｉｔｈｍ）の具体例としては例えば、以下のようなアルゴリズムが考えられる。
　（アルゴリズム１）ロボットの各脚の接地状態における脚の伸び切りまでの余裕の大きさに応じて算出コストが変化するコスト算出アルゴリズム
　例えば、脚が配置可能領域の中央付近に接地された場合は、その脚は配置可能領域内で各方向に自在に動かすことができるが、脚が配置可能領域の端部付近に接地された場合は、その脚は配置可能領域の内部方向にしか動かすことができずロボット１０の動きは制限されることになる。
　アルゴリズム１は、このような状態を考慮したアルゴリズムであり、脚が配置可能領域の中央付近に接地されている場合は低いコスト値を算出し、脚が配置可能領域の端部付近に接地されている場合は、高いコスト値を算出するアルゴリズムである。
　（アルゴリズム２）ロボットの全ての脚が接地した状態における胴体部のロール（ｒｏｌｌ）角度の大きさに応じて算出コストが変化するコスト算出アルゴリズム
　ロボットの４脚全てが接地した状態でロボット胴体のロール（ｒｏｌｌ）方向の傾きが大きいほど高いコスト値を算出するアルゴリズムである。
　（アルゴリズム３）ロボットの全ての脚が接地した状態における胴体部のピッチ（ｐｉｔｃｈ）角度の大きさに応じて算出コストが変化するコスト算出アルゴリズム
　ロボットの４脚全てが接地した状態でロボット胴体のピッチ（ｐｉｔｃｈ）方向の傾きが大きいほど高いコスト値を算出するアルゴリズムである。
　（アルゴリズム４）ロボットの全ての脚が接地した状態における胴体部の重心の高さに応じて算出コストが変化するコスト算出アルゴリズム
　ロボットの４脚全てが接地した状態でロボット胴体の重心位置が基準高さより高いほど、または低いほど高いコスト値を算出するアルゴリズムである。
　（アルゴリズム５）ロボットの全ての脚が接地した状態において、全ての脚の接地位置の高低差の大きさに応じて算出コストが変化するコスト算出アルゴリズム
　ロボットの４脚全てが接地した状態で全ての脚の接地位置の高低差が大きいほど高いコスト値を算出するアルゴリズムである。
　（アルゴリズム６）ロボットの脚が接地する走行面の状態に応じて算出コストが変化するコスト算出アルゴリズム
　ロボットの脚が接地する走行面の状態、例えば凹凸が多い、あるいは滑りやすい等、安定した接地が困難な状態であるほど高いコスト値を算出するアルゴリズムである。
　（アルゴリズム７）ロボットと障害物との距離に応じて算出コストが変化するコスト算出アルゴリズム
　ロボットと障害物の距離が近いほど高いコスト値を算出するアルゴリズムである。
　（アルゴリズム８）ロボットの脚の接地位置と、その接地位置から最も近い走行面段差位置との距離に応じて算出コストが変化するコスト算出アルゴリズム
　ロボットの脚の接地位置と、その接地位置から最も近い走行面段差位置との距離が近いほど高いコスト値を算出するアルゴリズムである。
　（アルゴリズム９）ロボットの全ての脚が接地した状態において形成される脚接地点を頂点とした支持多角形の大きさに応じて算出コストが変化するコスト算出アルゴリズム
　ロボットの全ての脚が接地した状態において形成される脚接地点を頂点とした支持多角形の大きさが小さいほど高いコスト値を算出するアルゴリズムである。
　このように、ユーザが定義するコスト算出アルゴリズム（ｕｓｅｒ＿ａｌｇｏｒｉｔｈｍ）は、例えば上記の（アルゴリズム１）～（アルゴリズム９）のいずれか、または、これらの組み合わせなど、様々な異なる設定のアルゴリズムが適用可能である。
　ステップＳ４１３では、このようなユーザ定義コスト算出アルゴリズム（ｕｓｅｒ＿ａｌｇｏｒｉｔｈｍ）に従って、胴体状態コスト算出値（ｃａｌｃ＿ｐｏｓｅ＿ｃｏｓ（ｕｓｅｒ＿ａｌｇｏｒｉｔｈｍ）を算出する。
　　（ステップＳ４１４）
　次に、ステップＳ４１４において、１つのサンプリング点（コスト算出胴体位置（ｉ））における１つの脚配置パターンに対応する胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）算出処理を実行する。
　具体的には、以下の胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）の算出処理を実行する。
　ユーザ定義コスト算出アルゴリズム（ｕｓｅｒ＿ａｌｇｏｒｉｔｈｍ）に従って、算出した胴体状態コスト算出値（ｃａｌｃ＿ｐｏｓｅ＿ｃｏｓ（ｕｓｅｒ＿ａｌｇｏｒｉｔｈｍ）を、胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）とする。
　胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）←胴体状態コスト算出値（ｃａｌｃ＿ｐｏｓｅ＿ｃｏｓ（ｕｓｅｒ＿ａｌｇｏｒｉｔｈｍ））
　上記式に従って、胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）の算出処理を実行する。
　このステップＳ４１４において算出される胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）は、ユーザ定義コスト算出アルゴリズム（ｕｓｅｒ＿ａｌｇｏｒｉｔｈｍ）において定義されたコスト算出アルゴリズムを適用して算出されたコストとなる。
　　（ステップＳ４１５）
　ステップＳ４１４において、１つのサンプリング点（コスト算出胴体位置（ｉ））における１つの脚配置パターンに対応する胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）算出処理を完了すると、ステップＳ４１５において、算出した胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）と、最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）との比較処理を実行する。
　すなわち、
　胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）＞最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）？
　上記判定式が成立するか否かを判定する。
　上記判定式は、ステップＳ４１４で算出した１つの脚配置パターンに対応する胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）が、初期設定値、または既に算出済みの最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）より大きい値であるか否かを判定する判定式である。
　ステップＳ４１４で算出した１つの脚配置パターンに対応する胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）が、初期設定値、または既に算出済みの最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）より大きい値であると判定した場合は、ステップＳ４１５の判定はＹｅｓとなり、ステップＳ４１６に進む。
　一方、ステップＳ４１４で算出した１つの脚配置パターンに対応する胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）が、初期設定値、または既に算出済みの最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）より大きい値でないと判定した場合は、ステップＳ３１５の判定はＮｏとなり、ステップＳ４１７に進む。
　　（ステップＳ４１６）
　ステップＳ４１６の処理は、ステップＳ４１５においてＹｅｓの判定がなされた場合、すなわち、ステップＳ４１４で算出した１つの脚配置パターンに対応する胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）が、初期設定値、または既に算出済みの最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）より大きい値であると判定した場合に実行する。
　この場合は、ステップＳ４１６において、最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）を、ステップＳ４１４で算出した胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）に設定する処理を実行する。
　すなわち、
　最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）←胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）
　上記式に従って、最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）を、ステップＳ４１４で算出した胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）に設定する処理を実行する。
　　（ステップＳ４１７）
　ステップＳ４１７は、ステップＳ４１２～Ｓ４１７の処理ループの終了ステップである。
　１つのサンプリング点（コスト算出胴体位置（ｉ））において、ロボット１０の４本の脚の接地高さの組み合わせパターン、最大１６通りについて、ステップＳ４１３～ステップＳ４１６の処理を実行し、各パターン対応の胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）を算出し、これらの中で最もコストの高い最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）を、胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）として算出する。
　この処理の完了後、図１６に示すフローのステップＳ２０３に進む。
　このように本実施例２では、コスト算出処理を、ユーザ定義コスト算出アルゴリズム（ｕｓｅｒ＿ａｌｇｏｒｉｔｈｍ）を適用して行う実施例である。
　すなわち、コスト算出対象として選択された１つの選択経路（ｐａｔｈ）の１つのサンプリング点（コスト算出胴体位置（ｉ））における胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）を算出する際、ユーザ定義コスト算出アルゴリズム（ｕｓｅｒ＿ａｌｇｏｒｉｔｈｍ）を適用した胴体状態コスト算出値（ｃａｌｃ＿ｐｏｓｅ＿ｃｏｓ（ｕｓｅｒ＿ａｌｇｏｒｉｔｈｍ）を算出する。
　ユーザ定義コスト算出アルゴリズム（ｕｓｅｒ＿ａｌｇｏｒｉｔｈｍ）は、ロボット使用環境やユーザにとって望ましくないロボット状態に対応する算出コスト値を高くするようなアルゴリズムとすることが可能である。
　従って、本実施例２を適用することで、使用環境やユーザの希望に応じた最適なロボット状態を維持可能な最適な走行経路を選択してロボットを走行させる制御が可能となる。
　　［５．（実施例３）６脚型のロボットを利用した実施例］
　次に、実施例３として、６脚型のロボットを利用した実施例について説明する。
　先に説明した実施例１は４脚型のロボットを利用した実施例であった。
　前述したように、４脚型のロボットは本開示の移動装置の一例に過ぎず、本開示の処理は２脚以上の任意数の脚を有する歩行型ロボットにおいて利用可能である。
　以下、実施例３として６脚型のロボットを利用した実施例について説明する。
　図３０は、６脚型のロボット１０の構成例を示す図である。
　図３０に示すように、６脚ロボット１０は，胴体部（本体部）１１、脚部１２、接地部１３、ロボットの周囲環境を認識するための視覚センサ（カメラ）１５を有する。また各脚には関節部２１、関節部２２を有する。
　図３０に示すロボット１０は前方に２本の脚、後方に２本の脚、さらに中間部にも２本の脚を有する６脚型の歩行型ロボットである。
　６本の各脚を個別に持ち上げて前後に動かすことで脚移動、すなわち歩行走行を行なうことが可能となる。
　図３０に示すように、ロボット１０は、視覚センサ１５を有してる。視覚センサ１５は、ロボット１０の周囲の障害物や、走行面形状などの環境情報を取得するためのセンサである。例えば、カメラによって構成される。
　なお、視覚センサ１５は、例えば、単眼カメラ、ステレオカメラ、全方位カメラ、赤外カメラ、ＬｉＤＡＲ（Ｌｉｇｈｔ　Ｄｅｔｅｃｔｉｏｎ　ａｎｄ　Ｒａｎｇｉｎｇ）、ＴＯＦ（Ｔｉｍｅ　ｏｆ　Ｆｌｉｇｈｔ）センサなどのいずれか、またはこれらの組み合わせ構成が利用可能である。
　なお、ＬｉＤＡＲ、ＴＯＦセンサは、いずれもオブジェクト距離を計測可能なセンサである。
　６本の脚部１２の各関節部２１，２２の可動構成は、先に図２を参照して説明した４脚型のロボットの関節部の可動構成と同様である。
　すなわち、６本の脚部１２各々の上端の関節部２１は、胴体部（本体部）１１に対して、前後方向、左右方向いずれにも回動可能な構成を持つ。
　さらに、６本の各脚の中間部にはいわゆる膝関節に相当する関節部２２は、脚部１２の下側半分を、脚上側部に対して前後方向に回動させることが可能となっている。
　このような６脚型のロボット１０を適用した実施例３のロボット１０の構成は、脚の数は異なるが、基本的に先に図８を参照して説明した構成と同様の構成である。
　実施例３では、部分経路計画部１３０の最適胴体経路決定部１３１において実行する処理、すなわち、障害物や段差を可能な限り避けて安全に走行可能な経路（部分経路）を生成する処理が６本の脚を考慮した処理となる。
　先に図９を参照して説明したように、最適胴体経路決定部１３１は、センサ１１１によって障害物や走行面形状を解析可能な範囲について、障害物や段差を可能な限り避けて安全に走行可能な部分経路（図９に点線で示す部分経路）を順次、生成する。
　部分経路計画部１３０の最適胴体経路決定部１３１は、部分経路の決定区間内にロボット１０が走行可能な複数の経路を設定し、各経路についてのコストを算出する。コストは安定した走行が可能なほど値が小さく、不安定な走行が多くなるほど値が大きくなるようなコストである。最適胴体経路決定部１３１は、ロボット１０の走行可能な複数の経路についてコストを算出して、各経路の評価処理を実行する。
　さらに、この評価処理の結果に基づいて最もコストの低い経路、すなわち安定走行が可能な経路を選択し、選択した経路を走行する。
　先に図１０～図２６を参照して説明した実施例１では、経路のコスト算出処理として、ロボット１０の４本の脚の接地パターン各々について、胴体部（本体部）１１の左右の傾きの大きさを算出し、傾きが大きいほど高コストとなるコスト算出処理を行っていた。
　これに対して、本実施例３では、ロボット１０の６本の脚の接地パターン各々について、胴体部（本体部）１１の左右の傾きの大きさを算出し、傾きが大きいほど高コストとなるコスト算出処理を行う。
　部分経路計画部１３０の最適胴体経路決定部１３１が実行する本実施例３の最適部分経路決定処理シーケンスについて説明する。
　本実施例３の最適部分経路決定処理シーケンスの基本シーケンスは、先に実施例１の基本シーケンスとして説明した図１０に示すフローチャートに従った処理と同様のシーケンスである。
　また、図１０に示すフロー中のステップＳ１０４の処理、すなわち、選択経路（ｐａｔｈ）の経路コスト（ｐａｔｈ＿ｃｏｓｔ）算出処理のシーケンスも、先に実施例１で説明した図１６のフローチャートに従った処理となる。
　本実施例３では、図１６に示すフロー中のステップＳ２０２の処理、すなわち、サンプリング点（コスト算出胴体位置（ｉ））における胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）の算出処理のシーケンスが異なるものとなる。
　図３１～図３３に示すフローチャートを参照して、本実施例３のサンプリング点（コスト算出胴体位置（ｉ））における胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）の算出処理の詳細シーケンスについて説明する。
　図３１～図３３に示すフローチャートは、先の実施例１で説明した図１６に示すフロー中のステップＳ２０２の処理の詳細シーケンスに相当する。
　本実施例３において、コスト算出対象として選択された１つの選択経路（ｐａｔｈ）の１つのサンプリング点（コスト算出胴体位置（ｉ））における胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）の算出処理は、図３１～図３３に示すステップＳ５０１～Ｓ５１７の処理に従って実行される。
　以下、各ステップの処理について、順次、説明する。
　　（ステップＳ５０１）
　ステップＳ５０１の処理は、先の実施例１において図１６に示すフローチャートを参照して説明したステップＳ３０１の処理と同様の処理である。
　ステップＳ５０１において
　（ｍｉｎ＿ｈｅｉｇｈｔｓ）←（６要素の配列）
　（ｍａｘ＿ｈｅｉｇｈｔｓ）←（６要素の配列）
　上記のように、ロボット１０の１つの脚の最小高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ）と、最大高さ（ｍａｘ＿ｈｅｉｇｈｔｓ）を変数に設定する。
　　（ステップＳ５０２～Ｓ５０６）
　ステップＳ５０２～Ｓ５０６は、ロボット１０の脚単位の処理の処理ループである。
　本実施例３ではロボット１０は６４本の脚を有している。脚識別子をｋ（ｋ＝１，２，３，４，５，６）として、処理対象の脚（ｋ）を順次、選択し、選択した処理対象の脚（ｋ）についてステップＳ５０３～ステップＳ５０５の処理を実行する。
　ステップＳ５０２～Ｓ５０６の処理ループの実行により、以下の各データ（１）～（６）が算出される。
　コスト算出胴体位置（ｉ）における、
　（１）左前脚（脚（１））の最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（１））と、最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（１））、
　（２）右前脚（脚（２））の最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（２））と、最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（２））、
　（３）左中脚（脚（３））の最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（３））と、最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（３））、
　（４）右中脚（脚（４））の最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（４））と、最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（４））、
　（５）左後脚（脚（５））の最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（５））と、最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（５））、
　（６）右後脚（脚（６））の最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（６））と、最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（６））、
　ステップＳ５０２～Ｓ５０６の処理ループが完了すると、図３２に示すフローのステップＳ５１１に進む。
　　（ステップＳ５１１）
　ステップＳ５１１の処理は、先の実施例１において図１７に示すフローチャートを参照して説明したステップＳ３１１の処理と同様の処理であり、ステップＳ５１２～ステップＳ５１７の処理ループ実行前の事前準備処理である。
　ステップＳ５１１では、最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）をマイナス無限大（－∞）に設定する。
　最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）←－∞
　ステップＳ５１２～ステップＳ５１７の処理ループ実行前に、上記のコスト初期設定処理を行う。
　　（ステップＳ５１２）
　ステップＳ５１２～ステップＳ５１７は、ロボット１０の脚配置状態単位の処理の処理ループである。
　すなわち、コスト算出胴体位置（ｉ）における各脚（ｋ＝１～６）各々の、
　「最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（ｋ））」と、
　「最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（ｋ））」
　これらの各組み合わせ（２×２×２×２×２×２）＝６４通り、各々についての処理ループである。
　ただし、本実施例においても、先に図２４、図２５を参照して説明したように、サンプリング点（コスト算出胴体位置（ｉ））が、図２４、図２５に示すように、各脚の脚配置可能領域が同一高さの平面であるような場合は、可能な脚配置パターンのみ処理ループを実行すればよい。
　　（ステップＳ５１３）
　ステップＳ５１３～Ｓ５１６は、上記６４通りのいずれか１つの脚配置パターンに対して、順次、実行する処理ステップである。
　まず、ステップＳ５１３において、処理対象とした１つの脚配置パターンについて、以下の処理を実行する。
　ｆ＿ｐｏｓｅを、脚（ｋ）がロボットの前脚である場合の脚配置状態（ｌｅｇ＿ｐｏｓｅ（ｋ））とし、
　ｍ＿ｐｏｓｅを、脚（ｋ）がロボットの中脚である場合の脚配置状態（ｌｅｇ＿ｐｏｓｅ（ｋ））とし、
　ｂ＿ｐｏｓｅを、脚（ｋ）がロボットの後脚である場合の脚配置状態（ｌｅｇ＿ｐｏｓｅ（ｋ））とする。
　すなわち、
　ｆ＿ｐｏｓｅ←｛ｌｅｇ＿ｐｏｓｅ（ｋ）：脚（ｋ）がロボットの前脚｝
　ｍ＿ｐｏｓｅ←｛ｌｅｇ＿ｐｏｓｅ（ｋ）：脚（ｋ）がロボットの中脚｝
　ｂ＿ｐｏｓｅ←｛ｌｅｇ＿ｐｏｓｅ（ｋ）：脚（ｋ）がロボットの後脚｝
　上記のデータ定義を実行する。
　　（ステップＳ５１４）
　次に、ステップＳ５１４において、１つのサンプリング点（コスト算出胴体位置（ｉ））における１つの脚配置パターンに対応する胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）算出処理を実行する。
　具体的には、以下の胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）の算出処理を実行する。
　前脚の最大高さと最小高さの差分（ｍａｘ（ｆ＿ｐｏｓｅ）－ｍｉｎ（ｆ＿ｐｏｓｅ））と、
　中脚の最大高さと最小高さの差分（ｍａｘ（ｍ＿ｐｏｓｅ）－ｍｉｎ（ｍ＿ｐｏｓｅ））と、
　後脚の最大高さと最小高さの差分（ｍａｘ（ｂ＿ｐｏｓｅ）－ｍｉｎ（ｂ＿ｐｏｓｅ））、
　これらの加算値を、胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）として算出する。
　すなわち、
　胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）←
　（ｍａｘ（ｆ＿ｐｏｓｅ）－ｍｉｎ（ｆ＿ｐｏｓｅ））
　＋（ｍａｘ（ｍ＿ｐｏｓｅ）－ｍｉｎ（ｍ＿ｐｏｓｅ））
　＋（ｍａｘ（ｂ＿ｐｏｓｅ）－ｍｉｎ（ｂ＿ｐｏｓｅ））
　上記式に従って、胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）の算出処理を実行する。
　このステップＳ５１４において算出される胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）は、左前脚と右前脚との接地高さの差分が大きいほど高くなり、左中脚と右中脚との接地高さの差分が大きいほど高くなり、さらに、左後脚と右後脚との接地高さの差分が大きいほど高くなる。
　また、胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）は、左前脚と右前脚との接地高さの差分が小さいほど低くなり、左中脚と右中脚との接地高さの差分が小さいほど低くなり、左後脚と右後脚との接地高さの差分が小さいほど低くなる。
　このコスト算出アルゴリズムは、先に図１２～図１５を参照して説明したように、胴体部（本体部）１１の左右の傾きが大きいほど高いコストとなるコスト算出アルゴリズムである。
　　（ステップＳ５１５）
　ステップＳ５１４において、１つのサンプリング点（コスト算出胴体位置（ｉ））における１つの脚配置パターンに対応する胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）算出処理を完了すると、ステップＳ５１５において、算出した胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）と、最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）との比較処理を実行する。
　すなわち、
　胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）＞最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）？
　上記判定式が成立するか否かを判定する。
　上記判定式は、ステップＳ５１４で算出した１つの脚配置パターンに対応する胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）が、初期設定値、または既に算出済みの最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）より大きい値であるか否かを判定する判定式である。
　ステップＳ５１４で算出した１つの脚配置パターンに対応する胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）が、初期設定値、または既に算出済みの最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）より大きい値であると判定した場合は、ステップＳ５１５の判定はＹｅｓとなり、ステップＳ５１６に進む。
　一方、ステップＳ５１４で算出した１つの脚配置パターンに対応する胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）が、初期設定値、または既に算出済みの最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）より大きい値でないと判定した場合は、ステップＳ３１５の判定はＮｏとなり、ステップＳ５１７に進む。
　　（ステップＳ５１６）
　ステップＳ５１６の処理は、ステップＳ５１５においてＹｅｓの判定がなされた場合、すなわち、ステップＳ５１４で算出した１つの脚配置パターンに対応する胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）が、初期設定値、または既に算出済みの最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）より大きい値であると判定した場合に実行する。
　この場合は、ステップＳ５１６において、最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）を、ステップＳ５１４で算出した胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）に設定する処理を実行する。
　すなわち、
　最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）←胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）
　上記式に従って、最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）を、ステップＳ５１４で算出した胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）に設定する処理を実行する。
　　（ステップＳ５１７）
　ステップＳ５１７は、ステップＳ５１２～Ｓ５１７の処理ループの終了ステップである。
　１つのサンプリング点（コスト算出胴体位置（ｉ））において、ロボット１０の６本の脚の接地高さの組み合わせパターン、最大６４通りについて、ステップＳ５１３～ステップＳ５１６の処理を実行し、各パターン対応の胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）を算出し、これらの中で最もコストの高い最悪胴体状態コスト（ｗｏｒｓｔ＿ｐｏｓｅ＿ｃｏｓｔ）を、胴体状態コスト（ｐｏｓｅ＿ｃｏｓｔ）として算出する。
　この処理の完了後、図１６に示すフローのステップＳ２０３に進む。
　このように本実施例３は、図３０に示すような６脚型のロボットを利用した実施例である。
　従って、本実施例３における部分経路計画部１３０の最適胴体経路決定部１３１が実行する処理、すなわち、障害物や段差を可能な限り避けて安全に走行可能な経路（部分経路）を生成する処理は、図３１～図３３に示すフローを参照して説明したように６本の脚を考慮した処理となる。
　本実施例３において、ロボット１０の部分経路計画部１３０の最適胴体経路決定部１３１は、図３１～図３３に示すフローを参照して説明した処理を実行して、部分経路生成区間の各々について、最も小さなコスト値を持つ経路を、最適経路（ｂｅｓｔ＿ｐａｔｈ）として生成する。
　生成した最適経路（ｂｅｓｔ＿ｐａｔｈ）は、最も低コストの経路（ｐａｔｈ）であり、ロボット１０の胴体部（本体部）１１の左右の傾きの発生度合を最も小さくして走行することが可能な経路である。
　この制御により、ロボット１０は、最適経路（ｂｅｓｔ＿ｐａｔｈ）に従って走行する。すなわち、ロボット１０は、複数の経路候補中、胴体部（本体部）１１の左右の傾きを最も小さくして走行可能な最適経路に従って走行することが可能となる。
　なお、その他、８本の脚、１０本の脚など、４本、６本以外の数の脚を有するロボットにおいても、脚数に応じた脚の配置パターンを考慮したコスト算出処理を行うことで、実施例１や実施例３と同様、ロボットの左右の傾きを低減可能な最適走行経路を生成することが可能となる。
　　［６．その他の実施例について］
　次に、上述した実施例とは異なるその他の実施例について説明する。
　以下の３つの実施例について説明する。
　（実施例４）胴体状態として６次元データ（ｘ，ｙ，ｚ，ｒｏｌｌ，ｐｉｔｃｈ，ｙａｗ）を利用した実施例
　（実施例５）ロボットの脚の関節部の自由度が異なる設定の実施例
　（実施例６）ユーザ端末からの指令を受信して動作するロボットの実施例
　　（（実施例４）胴体状態として６次元データ（ｘ，ｙ，ｚ，ｒｏｌｌ，ｐｉｔｃｈ，ｙａｗ）を利用した実施例）
　まず、（実施例４）として、胴体状態として６次元データ（ｘ，ｙ，ｚ，ｒｏｌｌ，ｐｉｔｃｈ，ｙａｗ）を利用した実施例について説明する。
　上述した実施例１等では、ロボット１０の胴体状態として、ロボット１０の水平面上の位置（ｘ，ｙ）と、ロボット１０の向きを示す角度（θ）によって構成される３次元データ（ｘ，ｙ，θ）を利用していた。
　すなわち、図８に示すロボット１０の大域経路計画部１２０の目標胴体状態生成部１２２は、大域経路生成部１２１が生成した大域経路を走行する際の各位置におけるロボットの胴体状態として、ロボット１０の水平面上の位置（ｘ，ｙ）と、ロボット１０の向きを示す角度（θ）、即ち（ｘ，ｙ，θ）の３次元データを生成していた。
　また、部分経路計画部１３０の最適胴体経路決定部１３１は、各部分経路の各終点位置、例えば図９に示す各サブゴール位置では、目標胴体状態生成部１２２が生成した胴体状態（ｘ，ｙ，θ）になるように部分経路を生成する処理を行っていた。
　このように上述した実施例では、ロボット１０の胴体状態を、ロボット１０の水平面上の位置（ｘ，ｙ）と、ロボット１０の向きを示す角度（θ）によって構成される３次元データ（ｘ，ｙ，θ）として算出し、利用していた。
　ロボット１０の胴体状態は、このような３次元データ（ｘ，ｙ，θ）に限らず、その他のデータを利用することも可能である。
　例えば、ロボット１０の３次元空間上の位置（ｘ，ｙ，ｚ）と、ｘｙｚ３軸周りの回転角度である（ｒｏｌｌ，ｐｉｔｃｈ，ｙａｗ）によって構成される６次元データ（ｘ，ｙ，ｚ，ｒｏｌｌ，ｐｉｔｃｈ，ｙａｗ）を胴体状態として用いる構成としてもよい。
　この場合、図８に示すロボット１０の大域経路計画部１２０の目標胴体状態生成部１２２は、大域経路生成部１２１が生成した大域経路を走行する際の各位置におけるロボットの胴体状態として、ロボット１０の３次元空間上の位置（ｘ，ｙ，ｚ）と、ｘｙｚ３軸周りの回転角度である（ｒｏｌｌ，ｐｉｔｃｈ，ｙａｗ）によって構成される６次元データ（ｘ，ｙ，ｚ，ｒｏｌｌ，ｐｉｔｃｈ，ｙａｗ）を生成する。
　また、部分経路計画部１３０の最適胴体経路決定部１３１は、各部分経路の各終点位置、例えば図９に示す各サブゴール位置で、目標胴体状態生成部１２２が生成した胴体状態（ｘ，ｙ，ｚ，ｒｏｌｌ，ｐｉｔｃｈ，ｙａｗ）になるように部分経路を生成する。
　　（（実施例５）ロボットの脚の関節部の自由度が異なる設定の実施例）
　次に、（実施例５）として、ロボットの脚の関節部の自由度が異なる設定の実施例について説明する。
　上述した実施例、例えば実施例１のロボット１０の脚は、図２を参照して説明したように、脚部１２各々の上端の関節部２１が、胴体部（本体部）１１に対して、前後方向、左右方向いずれにも回動可能な構成を持つ。
　さらに、各脚の中間部のいわゆる膝関節に相当する関節部２２は、脚部１２の下側半分を、脚上側部に対して前後方向に回動させることが可能となっている。
　図２を参照して説明した脚の構成は歩行型のロボットの一例であり、脚の可動構成はロボットによって様々である。
　例えば実施例１のロボット１０の脚は、図２を参照して説明したように、脚部１２各々の上端の関節部２１が、胴体部（本体部）１１に対して、前後方向、左右方向いずれにも回動可能な構成を持つが、脚部１２各々の上端の関節部２１を、胴体部（本体部）１１に対して、前後方向のみに回動可能な構成を持つロボットも存在する。
　図３４、図３５を参照してこのようなロボット構成について説明する。
　図３４は、脚部１２各々の上端の関節部２１を、胴体部（本体部）１１に対して、前後方向のみに回動可能な構成を持つロボット１０の例を示している。
　図３４に示すロボット１０は，胴体部（本体部）１１、脚部１２、接地部１３、ロボットの周囲環境を認識するための視覚センサ（カメラ）１５を有する。また各脚には関節部２１、関節部２２を有する。
　図３４に示すロボット１０は前方に２本の脚、後方に２本の脚を有する４脚型の歩行型ロボットである。
　４本の各脚を個別に持ち上げて前後に動かすことで脚移動、すなわち歩行走行を行なうことが可能となる。
　図３４に示すロボット１０の詳細構成例について図３５を参照して説明する。
　図３５には、図３４に示すロボット１０の、
　（ａ）前面図
　（ｂ）側面図
　（ｃ）背面図
　これらの３つの図を示している。
　本実施例５のロボット１０は、実施例１のロボット１０と異なり、（ａ）前面図、および（ｃ）背面図から理解されるように、胴体部（本体部）１１に対する脚部１２各々の上端、すなわち脚装着部の関節部２１は、ロボット前面や後面から見て左右方向に回動可能な構成を有していない。
　（ｂ）側面図から理解されるように、脚部１２各々の上端の関節部２１は、前後方向にのみ回動可能な構成を有する。
　このように、本実施例５のロボット１０の脚部１２各々の上端の関節部２１は、胴体部（本体部）１１に対して、前後方向のみに回動可能な構成を持つ。
　なお、（ｂ）側面図から理解されるように、各脚の中間部にはいわゆる膝関節に相当する関節部２２が設けられている。この関節部２２は、脚部１２の下側半分を、脚上側部に対して前後方向に回動させることが可能となっている。
　このような脚の可動方向が異なるロボットを利用した場合にも、本開示の処理、すなわちコスト算出による最適経路の設定処理が可能となる。
　ただし、このようにロボット１０の脚部１２の可動範囲が前後方向のみとなる場合、脚配置可能領域は先に説明した実施例１の設定とは異なるものとなる。
　すなわち、先の実施例１では、図１８のフローチャートのステップＳ３０３～Ｓ３０５において図２１に示すサークル状の実線で示す脚配置可能領域を算出し、このサークル状の脚配置可能領域内の最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（ｋ））と、最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（ｋ））を算出する処理を行っていた。
　これに対して、ロボット１０の脚部１２の可動範囲が前後方向のみとなる場合、脚配置可能領域は、図３６に示すような設定となる。
　すなわち、各脚の配置可能領域は、ロボット１０の前後方向に延びる直線状の領域となる。
　従って、本実施例５の場合、図１８に示すフローのステップＳ３０３～Ｓ３０５では、各脚対応の直線状の脚配置可能領域における最小脚配置高さ（ｍｉｎ＿ｈｅｉｇｈｔｓ（ｋ））と、最大脚配置高さ（ｍａｘ＿ｈｅｉｇｈｔｓ（ｋ））を算出する処理を行うことになる。
　　（（実施例６）ユーザ端末からの指令を受信して動作するロボットの実施例）
　次に、（実施例６）として、ユーザ端末からの指令を受信して動作するロボットの実施例について説明する。
　ロボット装置１０は、ロボット１０自身の制御によって自律走行することも可能であるが、例えば図３７に示すように、ユーザ５０の操作するユーザ端末６０から、走行開始、走行停止、進路変更などの指示コマンドを受信して走行することも可能である。
　なお、ロボット１０には、ロボットを制御するための制御装置（情報処理装置）が搭載されている。制御装置は、ロボット１０に装着された様々なセンサやエンコーダの検出信号を入力し、入力信号の解析により、各脚の位置（３次元位置）や動きを解析し、解析結果に基づいて各脚を駆動制御する。
　　［７．本開示の移動装置のハードウェア構成例について］
　次に、本開示の移動装置のハードウェア構成例について説明する。
　図３８を参照して本開示のロボット１０等の移動装置のハードウェア構成の一例について説明する。
　ＣＰＵ（Ｃｅｎｔｒａｌ　Ｐｒｏｃｅｓｓｉｎｇ　Ｕｎｉｔ）３０１は、ＲＯＭ（Ｒｅａｄ　Ｏｎｌｙ　Ｍｅｍｏｒｙ）３０２、または記憶部３０８に記憶されているプログラムに従って各種の処理を実行するデータ処理部として機能する。例えば、上述した実施例において説明したシーケンスに従った処理を実行する。ＲＡＭ（Ｒａｎｄｏｍ　Ａｃｃｅｓｓ　Ｍｅｍｏｒｙ）３０３には、ＣＰＵ３０１が実行するプログラムやデータなどが記憶される。これらのＣＰＵ３０１、ＲＯＭ３０２、およびＲＡＭ３０３は、バス３０４により相互に接続されている。
　ＣＰＵ３０１はバス３０４を介して入出力インタフェース３０５に接続され、入出力インタフェース３０５には、各種スイッチ、タッチパネル、マウス、マイクロホン、ユーザ入力部などからなる入力部３０６と、ディスプレイ、スピーカーなどからなる出力部３０７が接続されている。
　なお、入力部３０６には、カメラ、ＬｉＤＡＲ等各種センサ３２１からのセンサ検出情報も入力される。
　出力部３０７は、移動装置の駆動を行う駆動部３２２に対する駆動情報も出力する。
　ＣＰＵ３０１は、入力部３０６から入力される指令や状況データ等を入力し、各種の処理を実行し、処理結果を例えば出力部３０７に出力する。
　入出力インタフェース３０５に接続されている記憶部３０８は、例えばフラッシュメモリ、ハードディスク等からなり、ＣＰＵ３０１が実行するプログラムや各種のデータを記憶する。
　通信部３０９は、インターネットやローカルエリアネットワークなどのネットワークを介したデータ通信の送受信部として機能し、外部の装置と通信する。
　また、ＣＰＵの他、カメラから入力される画像情報などの専用処理部としてＧＰＵ（Ｇｒａｐｈｉｃｓ　Ｐｒｏｃｅｓｓｉｎｇ　Ｕｎｉｔ）を備えてもよい。
　入出力インタフェース３０５に接続されているドライブ３１０は、磁気ディスク、光ディスク、光磁気ディスク、あるいはメモリカード等の半導体メモリなどのリムーバブルメディア３１１を駆動し、データの記録あるいは読み取りを実行する。
　　［８．本開示の構成のまとめ］
　以上、特定の実施例を参照しながら、本開示の実施例について詳解してきた。しかしながら、本開示の要旨を逸脱しない範囲で当業者が実施例の修正や代用を成し得ることは自明である。すなわち、例示という形態で本発明を開示してきたのであり、限定的に解釈されるべきではない。本開示の要旨を判断するためには、特許請求の範囲の欄を参酌すべきである。
　なお、本明細書において開示した技術は、以下のような構成をとることができる。
　（１）　移動装置の走行経路を決定する経路計画部を有し、
　前記経路計画部は、
　複数の経路候補各々について、安定走行が可能な経路ほど低コストとなるコスト算出アルゴリズムを適用して経路コストを算出し、
　算出した経路コストが最低コストの経路候補を最適な走行経路として決定する移動装置。
　（２）　前記経路計画部は、
　前記移動装置の傾きが大きいほど高コストとなるコスト算出アルゴリズムを適用して経路コストを算出する（１）に記載の移動装置。
　（３）　前記経路計画部は、
　前記移動装置の胴体部の左右の傾きが大きいほど高コストとなるコスト算出アルゴリズムを適用し経路コストを算出する（１）または（２）に記載の移動装置。
　（４）　前記経路計画部は、
　複数の経路候補各々について、経路候補の経路中に複数のサンプリング点を設定して、各サンプリング点対応のコストを算出し、
　１つの経路候補内に設定された複数のサンプリング点のコストの加算値を、その経路候補の経路コストとして算出する（１）～（３）いずれかに記載の移動装置。
　（５）　前記経路計画部は、
　前記移動装置の胴体部の左右の傾きが大きいほど高コストとなるコスト算出アルゴリズムを適用して、各サンプリング点対応のコストを算出する（４）に記載の移動装置。
　（６）　前記移動装置は、複数の脚を有する歩行型の移動装置であり、
　前記経路計画部は、
　各サンプリング点における左右の脚の接地高さの差分が大きいほど高コストとなるコスト算出アルゴリズムを適用して、各サンプリング点対応のコストを算出する（４）または（５）に記載の移動装置。
　（７）　前記経路計画部は、
　各サンプリング点における前記移動装置の複数の脚配置パターンを解析し、
　複数の脚配置パターン各々についてのコストを算出して、
　最も高いコストが算出された脚配置パターン対応コストを、サンプリング対応のコストとして算出する（６）に記載の移動装置。
　（８）　前記経路計画部は、
　各サンプリング点における前記移動装置の各脚の配置可能領域を解析し、
　各脚の配置可能領域内における各脚の接地高さの最大値と最小値を算出し、
　移動装置の各脚の接地高さの最大値と最小値との異なる組み合わせからなる複数の脚配置パターン各々についてのコストを算出する（７）に記載の移動装置。
　（９）　前記経路計画部は、
　前記移動装置のセンサによって取得される環境情報を解析して、
　各脚の配置可能領域内における各脚の接地高さの最大値と最小値を算出する（８）に記載の移動装置。
　（１０）　前記移動装置は、
　前記移動装置の大域経路を計画する大域経路計画部と、
　前記大域経路計画部の計画した大域経路の一部経路単位で安定走行可能な部分経路を計画する部分経路計画部を有し、
　前記経路計画部は、
　前記部分経路計画部内の構成要素であり、
　前記部分経路として複数の経路候補を生成して、
　生成した複数の経路候補各々について、安定走行が可能な経路ほど低コストとなるコスト算出アルゴリズムを適用して経路コストを算出し、
　算出した経路コストが最低コストの経路候補を最適な走行経路として決定する（１）～（９）いずれかに記載の移動装置。
　（１１）　前記移動装置は、
　前記部分経路計画部が決定した最適な走行経路に従って前記移動装置を走行させるための駆動情報を生成する移動装置制御部を有する（１０）に記載の移動装置。
　（１２）　前記大域経路計画部は、
　前記大域経路計画部の計画した大域経路上における前記移動装置の目標胴体状態を生成する目標胴体状態生成部を有し、
　前記経路計画部は、
　前記目標胴体状態生成部の生成した目標胴体状態を維持可能な複数の経路候補を生成する（１０）または（１１）に記載の移動装置。
　（１３）　前記移動装置の目標胴体状態は、前記移動装置のｘｙ平面上の位置（ｘ，ｙ）と前記移動装置の向き（θ）によって構成される３次元データ（ｘ，ｙ，θ）である（１２）に記載の移動装置。
　（１４）　前記経路計画部は、
　ユーザ定義に従ったユーザ定義コスト算出アルゴリズムを適用して経路コストを算出する（１）～（１３）いずれかに記載の移動装置。
　（１５）　前記ユーザ定義コスト算出アルゴリズムは、
　（ａ）移動装置各脚の接地状態における脚の伸び切りまでの余裕の大きさに応じて算出コストが変化するコスト算出アルゴリズム、
　（ｂ）移動装置の脚接地状態における胴体部のロール（ｒｏｌｌ）角度、またはピッチ（ｐｉｔｃｈ）角度の大きさに応じて算出コストが変化するコスト算出アルゴリズム、
　（ｃ）移動装置の脚接地状態における胴体部の重心の高さに応じて算出コストが変化するコスト算出アルゴリズム、
　（ｄ）移動装置の脚が接地する走行面の状態に応じて算出コストが変化するコスト算出アルゴリズム、
　（ｅ）移動装置と障害物との距離に応じて算出コストが変化するコスト算出アルゴリズム、
　（ｆ）移動装置脚接地位置と、該接地位置から最も近い走行面段差位置との距離に応じて算出コストが変化するコスト算出アルゴリズム、
　（ｇ）移動装置の脚接地状態において形成される脚接地点を頂点とした支持多角形の大きさに応じて算出コストが変化するコスト算出アルゴリズム、
　上記（ａ）～（ｇ）のアルゴリズムのいずれかまたは組み合わせである（１４）に記載の移動装置。
　（１６）　前記移動装置は、２脚以上の複数の脚を有する歩行型ロボットである（１）に記載の移動装置。
　（１７）　移動装置において実行する移動装置制御方法であり、
　前記移動装置は、移動装置の走行経路を決定する経路計画部を有し、
　前記経路計画部が、
　複数の経路候補各々について、安定走行が可能な経路ほど低コストとなるコスト算出アルゴリズムを適用して経路コストを算出し、
　算出した経路コストが最低コストの経路候補を最適な走行経路として決定する移動装置制御方法。
　（１８）　移動装置において移動装置制御処理を実行させるプログラムであり、
　前記移動装置は、移動装置の走行経路を決定する経路計画部を有し、
　前記プログラムは、前記経路計画部に、
　複数の経路候補各々について、安定走行が可能な経路ほど低コストとなるコスト算出アルゴリズムを適用して経路コストを算出させ、
　算出した経路コストが最低コストの経路候補を最適な走行経路として決定する処理を実行させるプログラム。
　なお、明細書中において説明した一連の処理はハードウェア、またはソフトウェア、あるいは両者の複合構成によって実行することが可能である。ソフトウェアによる処理を実行する場合は、処理シーケンスを記録したプログラムを、専用のハードウェアに組み込まれたコンピュータ内のメモリにインストールして実行させるか、あるいは、各種処理が実行可能な汎用コンピュータにプログラムをインストールして実行させることが可能である。例えば、プログラムは記録媒体に予め記録しておくことができる。記録媒体からコンピュータにインストールする他、ＬＡＮ（Ｌｏｃａｌ　Ａｒｅａ　Ｎｅｔｗｏｒｋ）、インターネットといったネットワークを介してプログラムを受信し、内蔵するハードディスク等の記録媒体にインストールすることができる。
　また、明細書に記載された各種の処理は、記載に従って時系列に実行されるのみならず、処理を実行する装置の処理能力あるいは必要に応じて並列的にあるいは個別に実行されてもよい。また、本明細書においてシステムとは、複数の装置の論理的集合構成であり、各構成の装置が同一筐体内にあるものには限らない。
　以上、説明したように、本開示の一実施例の構成によれば、歩行型ロボット等の移動装置が安全に走行できる最適経路を効率的に決定する装置、方法が実現される。
　具体的には、例えば、歩行型ロボット等の移動装置の走行経路を決定する経路計画部を有し、経路計画部は、複数の経路候補各々について安定走行が可能な経路ほど低コストとなるコスト算出アルゴリズムを適用して経路コストを算出し、算出した経路コストが最低コストの経路候補を最適な走行経路として決定する。経路計画部は経路中に複数のサンプリング点を設定し、各サンプリング点における左右の脚の接地高さの差分が大きいほど高コストとなるコスト算出アルゴリズムを適用して、各サンプリング点対応のコストを算出し、サンプリング点のコストの加算値を経路コストとして算出する。
　本構成により、歩行型ロボット等の移動装置が安全に走行できる最適経路を効率的に決定する装置、方法が実現される。
　　１０　ロボット
　　１１　胴体部（本体部）
　　１２　脚部
　　１３　接地部
　　１５　視覚センサ
　　２１，２２　関節部
　　５０　ユーザ
　　６０　ユーザ端末
　１１０　環境情報解析部
　１１１　センサ
　１１２　環境地図生成部
　１２０　大域経路計画部
　１２１　大域経路生成部
　１２２　目標胴体状態生成部
　１３０　部分経路計画部
　１３１　最適胴体経路決定部
　１３２　経路追従目標速度算出部
　１４０　移動装置制御部
　１４１　駆動情報生成部
　３０１　ＣＰＵ
　３０２　ＲＯＭ
　３０３　ＲＡＭ
　３０４　バス
　３０５　入出力インタフェース
　３０６　入力部
　３０７　出力部
　３０８　記憶部
　３０９　通信部
　３１０　ドライブ
　３１１　リムーバブルメディア
　３２１　センサ
　３２２　駆動部

Claims (18)

  1. 　移動装置の走行経路を決定する経路計画部を有し、
    　前記経路計画部は、
    　複数の経路候補各々について、安定走行が可能な経路ほど低コストとなるコスト算出アルゴリズムを適用して経路コストを算出し、
    　算出した経路コストが最低コストの経路候補を最適な走行経路として決定する移動装置。
  2. 　前記経路計画部は、
    　前記移動装置の傾きが大きいほど高コストとなるコスト算出アルゴリズムを適用して経路コストを算出する請求項１に記載の移動装置。
  3. 　前記経路計画部は、
    　前記移動装置の胴体部の左右の傾きが大きいほど高コストとなるコスト算出アルゴリズムを適用し経路コストを算出する請求項１に記載の移動装置。
  4. 　前記経路計画部は、
    　複数の経路候補各々について、経路候補の経路中に複数のサンプリング点を設定して、各サンプリング点対応のコストを算出し、
    　１つの経路候補内に設定された複数のサンプリング点のコストの加算値を、その経路候補の経路コストとして算出する請求項１に記載の移動装置。
  5. 　前記経路計画部は、
    　前記移動装置の胴体部の左右の傾きが大きいほど高コストとなるコスト算出アルゴリズムを適用して、各サンプリング点対応のコストを算出する請求項４に記載の移動装置。
  6. 　前記移動装置は、複数の脚を有する歩行型の移動装置であり、
    　前記経路計画部は、
    　各サンプリング点における左右の脚の接地高さの差分が大きいほど高コストとなるコスト算出アルゴリズムを適用して、各サンプリング点対応のコストを算出する請求項４に記載の移動装置。
  7. 　前記経路計画部は、
    　各サンプリング点における前記移動装置の複数の脚配置パターンを解析し、
    　複数の脚配置パターン各々についてのコストを算出して、
    　最も高いコストが算出された脚配置パターン対応コストを、サンプリング対応のコストとして算出する請求項６に記載の移動装置。
  8. 　前記経路計画部は、
    　各サンプリング点における前記移動装置の各脚の配置可能領域を解析し、
    　各脚の配置可能領域内における各脚の接地高さの最大値と最小値を算出し、
    　移動装置の各脚の接地高さの最大値と最小値との異なる組み合わせからなる複数の脚配置パターン各々についてのコストを算出する請求項７に記載の移動装置。
  9. 　前記経路計画部は、
    　前記移動装置のセンサによって取得される環境情報を解析して、
    　各脚の配置可能領域内における各脚の接地高さの最大値と最小値を算出する請求項８に記載の移動装置。
  10. 　前記移動装置は、
    　前記移動装置の大域経路を計画する大域経路計画部と、
    　前記大域経路計画部の計画した大域経路の一部経路単位で安定走行可能な部分経路を計画する部分経路計画部を有し、
    　前記経路計画部は、
    　前記部分経路計画部内の構成要素であり、
    　前記部分経路として複数の経路候補を生成して、
    　生成した複数の経路候補各々について、安定走行が可能な経路ほど低コストとなるコスト算出アルゴリズムを適用して経路コストを算出し、
    　算出した経路コストが最低コストの経路候補を最適な走行経路として決定する請求項１に記載の移動装置。
  11. 　前記移動装置は、
    　前記部分経路計画部が決定した最適な走行経路に従って前記移動装置を走行させるための駆動情報を生成する移動装置制御部を有する請求項１０に記載の移動装置。
  12. 　前記大域経路計画部は、
    　前記大域経路計画部の計画した大域経路上における前記移動装置の目標胴体状態を生成する目標胴体状態生成部を有し、
    　前記経路計画部は、
    　前記目標胴体状態生成部の生成した目標胴体状態を維持可能な複数の経路候補を生成する請求項１０に記載の移動装置。
  13. 　前記移動装置の目標胴体状態は、前記移動装置のｘｙ平面上の位置（ｘ，ｙ）と前記移動装置の向き（θ）によって構成される３次元データ（ｘ，ｙ，θ）である請求項１２に記載の移動装置。
  14. 　前記経路計画部は、
    　ユーザ定義に従ったユーザ定義コスト算出アルゴリズムを適用して経路コストを算出する請求項１に記載の移動装置。
  15. 　前記ユーザ定義コスト算出アルゴリズムは、
    　（ａ）移動装置各脚の接地状態における脚の伸び切りまでの余裕の大きさに応じて算出コストが変化するコスト算出アルゴリズム、
    　（ｂ）移動装置の脚接地状態における胴体部のロール（ｒｏｌｌ）角度、またはピッチ（ｐｉｔｃｈ）角度の大きさに応じて算出コストが変化するコスト算出アルゴリズム、
    　（ｃ）移動装置の脚接地状態における胴体部の重心の高さに応じて算出コストが変化するコスト算出アルゴリズム、
    　（ｄ）移動装置の脚が接地する走行面の状態に応じて算出コストが変化するコスト算出アルゴリズム、
    　（ｅ）移動装置と障害物との距離に応じて算出コストが変化するコスト算出アルゴリズム、
    　（ｆ）移動装置脚接地位置と、該接地位置から最も近い走行面段差位置との距離に応じて算出コストが変化するコスト算出アルゴリズム、
    　（ｇ）移動装置の脚接地状態において形成される脚接地点を頂点とした支持多角形の大きさに応じて算出コストが変化するコスト算出アルゴリズム、
    　上記（ａ）～（ｇ）のアルゴリズムのいずれかまたは組み合わせである請求項１４に記載の移動装置。
  16. 　前記移動装置は、２脚以上の複数の脚を有する歩行型ロボットである請求項１に記載の移動装置。
  17. 　移動装置において実行する移動装置制御方法であり、
    　前記移動装置は、移動装置の走行経路を決定する経路計画部を有し、
    　前記経路計画部が、
    　複数の経路候補各々について、安定走行が可能な経路ほど低コストとなるコスト算出アルゴリズムを適用して経路コストを算出し、
    　算出した経路コストが最低コストの経路候補を最適な走行経路として決定する移動装置制御方法。
  18. 　移動装置において移動装置制御処理を実行させるプログラムであり、
    　前記移動装置は、移動装置の走行経路を決定する経路計画部を有し、
    　前記プログラムは、前記経路計画部に、
    　複数の経路候補各々について、安定走行が可能な経路ほど低コストとなるコスト算出アルゴリズムを適用して経路コストを算出させ、
    　算出した経路コストが最低コストの経路候補を最適な走行経路として決定する処理を実行させるプログラム。
PCT/JP2022/006848 2021-08-18 2022-02-21 移動装置、および移動装置制御方法、並びにプログラム WO2023021734A1 (ja)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202280055976.1A CN117897665A (zh) 2021-08-18 2022-02-21 移动装置、移动装置控制方法和程序

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2021133417 2021-08-18
JP2021-133417 2021-08-18

Publications (1)

Publication Number Publication Date
WO2023021734A1 true WO2023021734A1 (ja) 2023-02-23

Family

ID=85240298

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2022/006848 WO2023021734A1 (ja) 2021-08-18 2022-02-21 移動装置、および移動装置制御方法、並びにプログラム

Country Status (2)

Country Link
CN (1) CN117897665A (ja)
WO (1) WO2023021734A1 (ja)

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2013033356A (ja) * 2011-08-01 2013-02-14 Toyota Central R&D Labs Inc 自律移動装置
JP2019079375A (ja) * 2017-10-26 2019-05-23 アイシン精機株式会社 移動体

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2013033356A (ja) * 2011-08-01 2013-02-14 Toyota Central R&D Labs Inc 自律移動装置
JP2019079375A (ja) * 2017-10-26 2019-05-23 アイシン精機株式会社 移動体

Also Published As

Publication number Publication date
CN117897665A (zh) 2024-04-16

Similar Documents

Publication Publication Date Title
KR102230144B1 (ko) 인공 지능 심층 학습 타겟 탐지 및 속도 퍼텐셜 필드 알고리즘 기반 장애물 회피 및 자율 주행 방법 및 장치
KR102492242B1 (ko) 지형 인식 스텝 플래닝 시스템
US7769491B2 (en) Obstacle avoiding apparatus, obstacle avoiding method, obstacle avoiding program, and mobile robot apparatus
JP5945419B2 (ja) 脚式移動ロボットの脚体運動軌道生成装置。
US11548151B2 (en) Robotically negotiating stairs
JP4929506B2 (ja) 脚式移動ロボット
US9969086B1 (en) Achieving a target gait in a legged robot based on steering commands
Brunner et al. Hierarchical rough terrain motion planning using an optimal sampling-based method
KR101664575B1 (ko) 모바일 로봇의 장애물 회피 시스템 및 방법
Chestnutt et al. Biped navigation in rough environments using on-board sensing
KR20050028859A (ko) 환경 인식 장치 및 방법, 경로 계획 장치 및 방법, 및로봇 장치
JP4670807B2 (ja) 移動経路作成方法、自律移動体および自律移動体制御システム
JP2006205348A (ja) 障害物回避装置、障害物回避方法及び障害物回避プログラム並びに移動型ロボット装置
JP2010134742A (ja) 障害物回避機能を有する移動制御装置
Wahrmann et al. Vision-based 3d modeling of unknown dynamic environments for real-time humanoid navigation
Čížek et al. Foothold placement planning with a hexapod crawling robot
Norouzi et al. Planning high-visibility stable paths for reconfigurable robots on uneven terrain
JP5212939B2 (ja) 自律移動装置
Haddeler et al. Explore bravely: Wheeled-legged robots traverse in unknown rough environment
WO2023021734A1 (ja) 移動装置、および移動装置制御方法、並びにプログラム
Kim et al. Single 2D lidar based follow-me of mobile robot on hilly terrains
CN116009558A (zh) 一种结合运动学约束的移动机器人路径规划方法
JP5895682B2 (ja) 障害物検出装置及びそれを備えた移動体
JP7358108B2 (ja) 情報処理装置、情報処理方法及びプログラム
CN114967722B (zh) 一种摇臂式机动平台自主越台阶障碍方法

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: 22858059

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE