WO2019181351A1 - 軌道生成装置 - Google Patents

軌道生成装置 Download PDF

Info

Publication number
WO2019181351A1
WO2019181351A1 PCT/JP2019/006620 JP2019006620W WO2019181351A1 WO 2019181351 A1 WO2019181351 A1 WO 2019181351A1 JP 2019006620 W JP2019006620 W JP 2019006620W WO 2019181351 A1 WO2019181351 A1 WO 2019181351A1
Authority
WO
WIPO (PCT)
Prior art keywords
isosceles triangle
vertex
trajectory
goal
isosceles
Prior art date
Application number
PCT/JP2019/006620
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 US16/981,918 priority Critical patent/US11422562B2/en
Priority to DE112019001402.1T priority patent/DE112019001402T5/de
Publication of WO2019181351A1 publication Critical patent/WO2019181351A1/ja

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
    • B62D—MOTOR VEHICLES; TRAILERS
    • B62D15/00—Steering not otherwise provided for
    • B62D15/02—Steering position indicators ; Steering position determination; Steering aids
    • B62D15/021—Determination of steering angle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • G05D1/0248—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means in combination with a laser

Definitions

  • trajectory generation apparatus capable of easily generating trajectory data.
  • One embodiment of the trajectory generation device disclosed in this specification is a trajectory generation device that generates a trajectory on which a moving body moves.
  • the start position and the goal based on the start position of the mobile body, the start position travel direction that is the travel direction of the mobile body at the start position, the goal position of the mobile body, and the goal position travel direction that is the travel direction of the mobile body at the goal position
  • a planning unit that plans a steering angle and a steering angular velocity when the moving body moves from the start position to the goal position
  • a generation device is provided.
  • the trajectory generating device includes a curved trajectory fitting unit that applies the first curved trajectory along the equal sides of the first isosceles triangle and also applies the second curved trajectory along the equal sides of the second isosceles triangle.
  • the planning unit equalizes the lengths of the equilateral sides of the first isosceles triangle and the equilateral sides of the second isosceles triangle.
  • the planning unit arranges the first vertex on the base side of the first isosceles triangle at the start position.
  • the planning unit arranges the first vertex on the base side of the second isosceles triangle at the goal position.
  • the planning unit connects the second vertex on the base side of the first isosceles triangle and the second vertex on the base side of the second isosceles triangle.
  • the steering angle and steering angular velocity can be planned by the first isosceles triangle and the second isosceles triangle connecting the start position and the goal position.
  • the first and second curved trajectories can be generated by fitting the curved trajectories along the equal sides of the first and second isosceles triangles.
  • the planning unit may calculate the length of the isosceles under the condition that the equilateral sides of the first and second isosceles triangles are in a straight line at the connecting portion between the first isosceles triangle and the second isosceles triangle. Good.
  • the planning section is located on the first extension line that passes through the start position and extends in the start position advance direction, and is sandwiched between the equilateral sides of the first isosceles triangle at a position that is a distance equal in length from the start position.
  • the third vertex may be determined.
  • the planning unit is a position on the second extension line that passes through the goal position and extends in the direction opposite to the goal position traveling direction, and is located at a position that is a distance equal to the length of the equal side from the goal position. You may determine the 3rd vertex pinched
  • the planning unit determines a midpoint between the third vertex of the first isosceles triangle and the third vertex of the second isosceles triangle, the second vertex of the first isosceles triangle and the second vertex of the second isosceles triangle. It may be determined as two vertices. Details of the effect will be described in Examples.
  • the planning unit may determine whether the first angle is positive or negative, which is an angle formed by the equilateral side extending from the start position of the first isosceles triangle with respect to the base of the first isosceles triangle.
  • the planning unit may determine whether the second angle is positive or negative, which is an angle formed by the equilateral side extending from the second vertex of the second isosceles triangle with respect to the base of the second isosceles triangle.
  • the plan unit includes a third extension line and a first extension connecting the goal position and the third vertex of the second isosceles triangle when both the first angle and the second angle are positive or both are negative. You may determine the intersection with a line.
  • the planning unit is the first distance from the intersection point to the goal position that is a position on the third extension line. May be redetermined as the second vertex of the first isosceles triangle and the second vertex of the second isosceles triangle.
  • the planning unit determines a position that is a position on the first extension line and that has advanced the second distance from the intersection to the start position as a first isosceles triangle. As the second vertex of the second isosceles triangle and the second vertex of the second isosceles triangle. Details of the effect will be described in Examples.
  • the curve trajectory fitting unit differentiates the parametric equation indicating the curve trajectory using the parameters, so that the slope of the curve trajectory at the intersection of the vertical bisector of the base of the first isosceles triangle and the curve trajectory is The value of the specific parameter that is equal to the slope of the base of one isosceles triangle may be determined.
  • the curved trajectory fitting unit generates a curved trajectory with a normalized size by combining the trajectory obtained by the specific parameter line-symmetrically with respect to the vertical bisector of the base of the first isosceles triangle. May be.
  • the curve trajectory fitting portion is a curve trajectory whose size is normalized so that both ends of the curve trajectory whose size is normalized coincide with the first vertex and the second vertex on the base side of the first isosceles triangle.
  • the first curved trajectory may be applied along the equilateral sides of the first isosceles triangle. Details of the effect will be described in Examples.
  • the parametric equation may be a quintic equation of parameters.
  • a parameter obtained by first-order differentiation of the parametric equation may include a parameter only in the form of a square or a fourth power.
  • the block diagram which shows the structure of the forklift which concerns on an Example.
  • the flowchart which shows the production
  • the flowchart explaining the processing content of S80 The figure which shows the example of fitting of a curved track.
  • a forklift 10 shown in FIG. 1 is an unmanned forklift.
  • the forklift 10 includes a vehicle body 12, a range sensor 26, and a calculation device 30.
  • the vehicle body 12 includes a front wheel 28 and a rear wheel 29.
  • An encoder 92 is connected to the left and right front wheels 28 so that the amount of rotation of the front wheels 28 can be detected.
  • the encoder 92 detects the amount and direction of movement of the vehicle body 12 based on the operation of the left and right front wheels 28.
  • the front wheel 28 is connected to a drive wheel motor 90 via a drive mechanism (not shown), and is rotated by the drive wheel motor 90.
  • the range sensor 26 is a one-dimensional scanning type range sensor that is installed in the vehicle body 12 and scans the laser beam in one direction (in this embodiment, the horizontal direction).
  • the range sensor 26 measures the distance between the vehicle body 12 and an object in a set space set in front of the vehicle body 12. Thereby, the distance data ahead of the forklift 10 is acquired.
  • the distance data acquired by the range sensor 26 is input to the self-position estimation unit 38 of the arithmetic device 30.
  • the control device 32 is constituted by a microprocessor equipped with a CPU and the like.
  • the control device 32 is mounted on the vehicle body 12. As shown in FIG. 1, the control device 32 is communicably connected to the drive wheel motor 90, the steering device 94, and the like, and controls these operations. That is, the control device 32 controls the traveling direction and traveling speed of the forklift 10 by driving the drive wheel motor 90 and the steering device 94.
  • the arithmetic unit 30 is constituted by a microprocessor having a CPU and the like.
  • the computing device 30 includes a start position / goal position setting unit 35, a trajectory generation device 40, map data 58, a self-position estimation unit 38, and a trajectory tracking control unit 48.
  • the map data 58 is data indicating the position and size of an object (for example, a pillar, a wall, etc.) existing in the space in which the forklift 10 moves.
  • the map data 58 can be created based on the measurement result obtained by measuring in advance the object in the region where the forklift 10 moves by the range sensor 26.
  • the self-position estimation unit 38 receives the map data 58, distance data between the surrounding objects measured by the range sensor 26, and the movement amount and movement direction of the vehicle body 12 detected by the encoder 92. The self-position estimating unit 38 estimates the self-position of the forklift 10 based on these input data.
  • the start position / goal position setting unit 35 sets the start position and goal position of the forklift 10.
  • the trajectory generating device 40 includes a steering planning unit 41, a curved trajectory generating unit 42, and a curved trajectory fitting unit 43.
  • the steering plan unit 41 generates a first isosceles triangle and a second isosceles triangle that connect the start position and the goal position, thereby determining the steering direction when the forklift 10 moves from the start position to the goal position.
  • the curved trajectory generation unit 42 generates a curved trajectory with a normalized size.
  • the curved trajectory fitting unit 43 generates curved trajectory data by fitting the curved trajectory to the first and second isosceles triangles. Thereby, the steering angle and the steering angular velocity are obtained.
  • the track following control unit 48 moves the vehicle body 12 to the goal position based on the curved track data generated by the curved track fitting unit 43 and the current position of the forklift 10 (self-position calculated by odometry). Calculate the control command value.
  • the generated control command value is input to the control device 32, and the control device 32 moves the vehicle body 12 toward the goal position.
  • the precondition of the description method in this specification is demonstrated.
  • the position and orientation of the forklift 10 are sometimes referred to as “pause”.
  • the center point (also referred to as the representative point or origin) of the vehicle is the center of the left and right of the non-steered wheel. In the case of the forklift 10 of the present embodiment, it is the left and right center of the front wheel 28. This center point is assumed to be so small that tire slip is negligible. As a result, the center point does not move to the left or right of the vehicle but moves only forward and backward, which is convenient as a representative point.
  • the start position O is set as the origin of the ground coordinate system as shown in FIG.
  • the direction of the start direction A1 which is the direction of the forklift 10 at the start position, is the x-axis direction of the ground coordinate system.
  • the start direction A1 is the traveling direction of the forklift 10 at the start position O.
  • the generation of a trajectory from the start position / posture (pause) to an arbitrary goal position P having an arbitrary goal direction A2 is considered.
  • the goal direction A2 is the traveling direction of the forklift 10 when the goal position P is reached.
  • the x coordinate of the goal position P is positive (x> 0) is considered. This is because there are cases in which a proper trajectory can be generated with the same program even if x> 0 is not the goal, but if x ⁇ 0, there are many cases where it is more appropriate to go back.
  • FIG. 3 is a flow for generating a curved trajectory when a goal position instruction is input.
  • S10 the pose of the goal position P is set.
  • S 10 is a process executed by the start position / goal position setting unit 35. As shown in the example of FIG. 2, a goal position P and a goal direction A2 are set.
  • S20 it is determined whether or not the goal direction A2 is substantially equal to the start direction A1. That is, it is determined whether or not the goal direction A2 and the start direction A1 are substantially parallel.
  • substantially parallel means, for example, a case where the difference between the goal direction A2 and the start direction A1 is 3 ° or less.
  • S30 the steering angle and the steering angular velocity when the forklift 10 moves from the start position O to the goal position P are planned.
  • S30 is a process executed by the steering planning unit 41. Specifically, a first isosceles triangle and a second isosceles triangle that connect the start position O and the goal position P are generated.
  • the processing content of S30 will be described using the flowchart of FIG.
  • S110 the length L of the equilateral sides of the first and second isosceles triangles is calculated.
  • the equal lengths L of the first and second isosceles triangles are equal. This takes into account the ease of solving and the validity of the solution.
  • the length L of the equal side is set as a variable. Further, as shown in FIG. 5, a condition is defined in which the equilateral ES 12 and the equilateral ES 21 are in a straight line at the connection portion between the first isosceles triangle IT1 and the second isosceles triangle IT2. If an equation is established under this condition, it becomes a quadratic equation, so the quadratic equation is solved. Thereby, the length L of the equal side is obtained.
  • first and second isosceles triangles are arranged.
  • the first vertex VE11 on the base side of the first isosceles triangle IT1 is arranged at the start position O.
  • the first vertex VE21 on the base side of the second isosceles triangle IT2 is arranged.
  • the equilateral side of the first isosceles triangle IT1 is a position on the x-axis, which is an extension line extending in the start direction A1 direction through the start position O, at a position that is a distance of the equilateral length L from the start position O.
  • a third vertex VE13 (also referred to as S point) sandwiched between the two is determined.
  • the third vertex VE23 (also referred to as Q point) is determined.
  • an R point that is a midpoint between the third vertex VE13 (point S) of the first isosceles triangle IT1 and the third vertex VE23 (point Q) of the second isosceles triangle IT2 is obtained.
  • the R point is determined as the second vertex VE12 of the first isosceles triangle IT1 and the second vertex VE22 of the second isosceles triangle IT2.
  • generating the first isosceles triangle and the second isosceles triangle also means obtaining the position of the R point and the slopes of the equilateral ES12 and ES21 passing through the R point.
  • S140 whether or not the maximum curvature of the curved orbit is larger than a predetermined threshold when the curved orbit is applied to the equilateral side of the first isosceles triangle IT1 and the equilateral side of the second isosceles triangle IT2. to decide. If the type of curve to be applied is determined, the maximum curvature can be uniquely determined from the apex angle and size of the isosceles triangle. If the maximum curvature is larger than the threshold value (S140: YES), it is determined that the curve is too steep, and the process proceeds to S150, where an error occurs. On the other hand, when the maximum curvature is equal to or smaller than the threshold (S140: NO), it is determined that the generation of the first isosceles triangle and the second isosceles triangle is completed, and the process proceeds to S40 in FIG.
  • the threshold value of the maximum curvature may be determined so as to exclude, as an error, a curved trajectory applied to an apex angle smaller than a predetermined minimum angle (eg, a range of 60 degrees to 90 degrees).
  • a predetermined minimum angle eg, a range of 60 degrees to 90 degrees.
  • first steering direction planned by the first isosceles triangle and the second steering direction planned by the second isosceles triangle are the same direction. Specifically, in the first isosceles triangle IT1, whether the first angle AG1, which is the base angle formed by the equilateral ES11 (line OS) extending from the start position O, with respect to the base (line OR) is determined. Further, in the second isosceles triangle IT2, whether the second angle AG2, which is the base angle formed by the equilateral ES21 (line RQ) extending from the second vertex VE22 (point R) with respect to the base (line RP), is determined. To do.
  • both the first angle AG1 and the second angle AG2 are positive or both are negative, it is determined that the first steering direction and the second steering direction are the same direction (S210: YES). .
  • both the first angle AG1 and the second angle AG2 are positive. In this case, the process proceeds to S220 to change to one-time steering.
  • an extension line EX3 of the line QP connecting the goal position P and the third vertex VE23 (Q point) of the second isosceles triangle IT2 is generated. Then, an intersection C between the extension line EX3 and the x axis is determined.
  • S230 it is determined whether or not the position of the intersection C is appropriate. Specifically, it is determined whether the intersection C is behind the vector of the goal direction A2 at x> 0 and the goal position P (that is, whether the inner product of the vector of the line CP and the vector of the goal direction A2 is negative). Is done. Since the steering is performed twice in the same direction, the position of the intersection C is normally always appropriate. If a negative determination is made in S230 (S230: NO), an error is determined and the flow ends. On the other hand, when an affirmative determination is made in S230 (S230: YES), the process proceeds to S240 in order to create a new isosceles triangle with the intersection C as the center.
  • S240 it is determined whether the intersection C is closer to the start position O or the goal position P. Specifically, the first distance D1 between the start position O and the intersection point C and the second distance D2 between the intersection point C and the goal position P are compared.
  • the second isosceles triangle IT2 is crushed in a straight line. That is, the point R and the goal position P can be connected by a straight line.
  • the steering plan can be corrected with only one of the first isosceles triangle IT1. Therefore, it is possible to complete a curved trajectory by fitting a curve into one isosceles triangle by a method described later. Since the curve length at which the curvature changes substantially constant can be long, the change in curvature can be reduced as a whole. Therefore, the maximum value of the steering speed when moving at the same linear velocity can be kept low.
  • the process proceeds to S260.
  • the position on the x-axis and starting from the intersection C toward the start position O and traveling the second distance D2 is defined as the second vertex VE12 (R point) of the first isosceles triangle IT1 and the first It is redetermined as the second vertex VE22 (R point) of the two isosceles triangle IT2.
  • a new first isosceles triangle IT1 and second isosceles triangle IT2 are formed.
  • the first isosceles triangle IT1 is crushed in a straight line.
  • the plan can be corrected so that the steering plan is made with only one second isosceles triangle IT2.
  • the length L of the equilateral sides of the isosceles triangle is changed by a constant multiple in the first and second isosceles triangles in accordance with the ratio of the apex angles of the first and second isosceles triangles.
  • the isosceles triangle having the smaller apex angle that is, the one with a sharp curve
  • the apex angle is larger ( That is, the length of the equilateral side of the isosceles triangle having the gentle curve is shortened. For example, as shown in FIG.
  • an isosceles triangle having a smaller maximum curvature is selected from the isosceles triangle created in S30 and the isosceles triangle recreated in S250, S260, and S270. Then, the process proceeds to S80 in FIG.
  • a curved orbit is applied to the first and second isosceles triangles. Specifically, a first curved trajectory along the equilateral side of the first isosceles triangle IT1 and a second curved trajectory along the equilateral side of the second isosceles triangle IT2 are generated.
  • the processing content of S80 will be described using the flowchart of FIG. For generalization, only the fitting of the curved trajectory to the first isosceles triangle IT1 will be described.
  • the fitting of the curved trajectory to the second isosceles triangle IT2 is because it can be performed in exactly the same manner as the first isosceles triangle IT1, if the R point is made the origin by moving the origin and rotating.
  • a parametric curve for generating a curve in which the relationship between the curve length and the curvature is almost linear is used.
  • the parametric curve may be any curve as long as the curvature gradient starts from 0 and gradually increases.
  • a parametric curve represented by the following expressions (1) and (2) will be described.
  • x m ⁇ (t-t 5/40) ⁇ Equation (1)
  • y m ⁇ t 3/6 ⁇ formula (2)
  • the magnification m is a scaling constant for similarity transformation
  • t is a parameter.
  • equation (3) u / m (3)
  • equation (3) u ⁇ u 5 / (40 m 4 )
  • equation (4) and (5) u 3 / (6 m 2 ) (5)
  • m is not explicitly calculated, and “1 / m 2 ” may be directly calculated. This is because in equations (4) and (5), only the square and the fourth power of m are output, and it is not necessary to calculate “1 / m”.
  • equations (4) and (5) The effect of performing calculations using equations (4) and (5) will be described.
  • the slope of the base becomes zero, m diverges to infinity, and the range of t is from 0 to 0. Therefore, calculation is not possible in equations (1) and (2). It becomes possible.
  • equations (4) and (5) m and t are not explicitly used.
  • equations (4) and (5) the calculation of “1 / m 2 ” is used, and the range of “u” may be calculated with the maximum value from 0 to u. Thereby, it is possible to perform stable calculation including the case where the isosceles triangle is completely collapsed into a straight line.
  • a curve shape with a normalized size is generated. This will be specifically described.
  • the corresponding parameter t is unique when the slope is given. Therefore, the parameter t can be obtained by solving the quadratic equation. That is, by differentiating equations (1) and (2) mathematically, a curve at the intersection IS1 between the perpendicular bisector PB1 (see FIG. 12) of the base ES13 of the first isosceles triangle IT1 and the curved trajectory is obtained. The value of the parameter t is determined so that the inclination of the trajectory becomes equal to the inclination of the base ES13 of the first isosceles triangle IT1.
  • a curve shape with a normalized size is obtained with the similarity conversion magnification m in the equations (1) and (2) set to “1”.
  • a curved shape CS1 as shown in FIG. 13 is obtained.
  • the normalized curve shape is combined line-symmetrically with respect to the vertical bisector PB1 of the base ES13 of the first isosceles triangle IT1.
  • a curved trajectory CT1 whose size is normalized is generated.
  • a curved trajectory CT1 obtained by combining the curved shapes CS1 and CS2 is obtained as shown in FIG.
  • the slope GR1 (FIG. 14) of the connection portion CN of the curved shapes CS1 and CS2 is equal to the slope of the base ES13 of the first isosceles triangle IT1 of FIG. 12 due to symmetry and differentiable conditions.
  • both ends of the curved trajectory CT1 whose size is normalized coincide with the first vertex VE11 (start position O) and the second vertex VE12 (R point) on the base side of the first isosceles triangle IT1.
  • the curved trajectory CT1 is enlarged or reduced. Specifically, one end E1 of the curved trajectory CT1 in FIG. 14 is made to coincide with the start position O in FIG. At this time, the ratio between the position at which the other end E2 of the curved trajectory CT1 reaches and the point R in FIG. 12 is obtained. Then, the curved trajectory CT1 is enlarged or reduced based on the ratio obtained above. Thereby, as shown in FIG. 12, the fitting of the curved trajectory CT1 to the first isosceles triangle IT1 is completed.
  • the curved trajectory CT2 can be applied by performing the processing of S310 to S330 described above. As a result, natural curved trajectories can be generated for various combinations of the start position O and the goal position P.
  • the coefficient for fitting the curve to the isosceles triangle is stored. Specifically, the enlargement / reduction ratio obtained in S330 and the value of the parameter t obtained in S310 are stored. Note that a value obtained by squaring the reciprocal of the scaling ratio may be used as the scaling ratio. Then, the flow ends.
  • S310 and S320 are processes executed by the curved trajectory generation unit.
  • S330 is a process executed by the curved trajectory fitting unit 43.
  • the flowchart in FIG. 15 is executed, for example, at a constant control cycle (for example, every 10 milliseconds).
  • the process of the flowchart of FIG. 15 is a process executed by the trajectory tracking control unit 48.
  • the position (x, y), inclination, and curvature corresponding to the parameter t are calculated. This will be specifically described.
  • the position (x, y) is obtained by substituting 1 / "magnification m" of the parameter t into the normalized expression of the curved trajectory.
  • the slope at the position (x, y) can be obtained from the differential value of the equation representing the curved trajectory.
  • the curvature ⁇ at the position (x, y) can be obtained by second-order differentiation.
  • the steering angle ⁇ of the forklift 10 has a one-to-one correspondence with the curvature ⁇ of the curved track, as shown in the following equation (6).
  • ⁇ (cos ⁇ , sin ⁇ , 0) (v, 0, 0) + (0,0, ⁇ v) ⁇ (bx, by, 0) (6)
  • v is the forward speed of the center of the vehicle.
  • bx is the position (wheel base) of the steered wheel. by is the center point of the vehicle and half of the tread.
  • ⁇ is an appropriate constant. Therefore, the steering angle ⁇ is obtained by the following equation (7).
  • ⁇ tan -1 (bx ⁇ / (1-by ⁇ )) (7)
  • the trajectory generation device 40 of the present embodiment generates a trajectory by fitting a curved trajectory to the first isosceles triangle and the second isosceles triangle connecting the start position O and the goal position P.
  • the trajectory can be designed only by algebraic calculation such as solving a quadratic equation and coordinate transformation.
  • the curve trajectory can be calculated in a shorter time than when using a curve that requires numerical integration, such as a clothoid curve. Trajectory data can be generated within the control cycle.
  • a second effect it is possible to generate a trajectory having a small curvature change near the start point and end point of the curved trajectory.
  • the extension line (x axis) extending in the start direction A1 direction overlaps with the equilateral ES11 of the first isosceles triangle IT1, and the extension line extends in the direction opposite to the goal direction A2.
  • the isosceles triangles are arranged so that the isosceles ES22 of the two isosceles triangles IT2 overlap. This makes it possible to generate a track suitable for a forklift with a lower upper limit of the steering speed than a track created with a B-spline curve.
  • the trajectory generation device 40 of the present embodiment uses parametric equations expressed by equations (1) and (2). The effect of using equations (1) and (2) will be described.
  • the value of the parameter t is determined based on the slope of the curve.
  • the parameter t is obtained by solving the quadratic equation. Since the quintic equation of equation (1) and the cubic equation of equation (2) are differentiated into equations that include only parameters t 4 and t 2 , by looking at “t 2 ” as a variable, It can be easily reduced to a quadratic equation. That is, algebraic calculation is practically possible by using equations (1) and (2).
  • the cubic parametric equation can be calculated by algebraic calculation, the usable range is narrower than that of the quintic equation of Equation (1) and cannot be used when bending at a right angle. Therefore, it can be seen that the quintic expression of Expression (1) is an expression having the characteristics that the application range is wide and the calculation is easy.
  • a turning trajectory that switches the traveling speed back and forth may be generated. That is, an appropriate turning point may be given, and a forward trajectory may be generated by this algorithm up to the turning point. Further, a backward trajectory may be generated by this algorithm from the turning point to the goal position P.
  • a one-dimensional scanning type range sensor is used as the range sensor 26, but various sensors may be used.
  • the range sensor 26 may be a 2D LiDAR, a 3D LiDAR, a stereo camera, a monocular camera, a distance image sensor, or the like.
  • the self-position estimation unit 38 may estimate the self-position by means using satellite positioning.
  • Equations (1) and (2) are examples of parametric curves.
  • various types of parametric curves can be used.
  • the selection method of the isosceles triangle may be various modes. For example, an isosceles triangle having a smaller maximum curvature change may be selected.
  • the start direction A1 is an example of the start position traveling direction.
  • the goal direction A2 is an example of the goal position advancing direction.
  • the steering planning unit 41 is an example of a planning unit.
  • the point S is an example of a third vertex sandwiched between equal sides of the first isosceles triangle.
  • the point Q is an example of a third vertex sandwiched between equal sides of the second isosceles triangle.

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • General Physics & Mathematics (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Mechanical Engineering (AREA)
  • Chemical & Material Sciences (AREA)
  • Transportation (AREA)
  • Combustion & Propulsion (AREA)
  • Optics & Photonics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • Electromagnetism (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

移動体のスタート位置、スタート位置進行方向、ゴール位置、ゴール位置進行方向に基づいて、スタート位置とゴール位置とを接続する第１および第２の二等辺三角形を生成することで、移動体がスタート位置からゴール位置まで移動する際の操舵角および操舵角速度を計画する計画部を、軌道生成装置は備える。第１および第２の二等辺三角形の等辺に沿って第１および第２の曲線軌道を当てはめる曲線軌道あてはめ部を、軌道生成装置は備える。計画部は、第１および第２の二等辺三角形の等辺の長さを等しくする。計画部は、スタート位置に第１の二等辺三角形の底辺側の第１頂点を配置する。計画部は、ゴール位置に第２の二等辺三角形の底辺側の第１頂点を配置する。計画部は、第１の二等辺三角形の底辺側の第２頂点と第２の二等辺三角形の底辺側の第２頂点とを接続する。

Description

軌道生成装置
　本出願は、２０１８年３月１９日に出願された日本国特許出願第２０１８－０５０７９９号に基づく優先権を主張する。その出願の全ての内容はこの明細書中に参照により援用されている。本明細書に開示の技術は、軌道生成装置に関する。
　無人フォークリフトなど、移動体を自走させる技術が開発されている。移動体を自走させるには、軌道を生成する必要である。例えば特開平３－２５２７０７号公報には、軌道データを生成する技術が開示されている。
　移動体を自走させる場合には、一定の制御周期（例：１０ミリ秒毎）で軌道データを生成し、走行状態を微調整する必要がある。ここで例えば、軌道データの生成にクロソイド曲線を用いる場合には、クロソイド点の座標は初等関数で表すことは不可能であるため、数値積分が必要となる。すると制御周期内で軌道データを生成することが困難な場合がある。本明細書は、簡易に軌道データを生成することが可能な軌道生成装置を開示する。
　本明細書に開示する軌道生成装置の一実施形態は、移動体が移動する軌道を生成する軌道生成装置である。移動体のスタート位置、スタート位置における移動体の進行方向であるスタート位置進行方向、移動体のゴール位置、ゴール位置における移動体の進行方向であるゴール位置進行方向、に基づいて、スタート位置とゴール位置とを接続する第１の二等辺三角形および第２の二等辺三角形を生成することで、移動体がスタート位置からゴール位置まで移動する際の操舵角および操舵角速度を計画する計画部を、軌道生成装置は備える。第１の二等辺三角形の等辺に沿って第１の曲線軌道を当てはめるとともに、第２の二等辺三角形の等辺に沿って第２の曲線軌道を当てはめる曲線軌道あてはめ部を、軌道生成装置は備える。計画部は、第１の二等辺三角形の等辺と第２の二等辺三角形の等辺の長さを等しくする。計画部は、スタート位置に第１の二等辺三角形の底辺側の第１頂点を配置する。計画部は、ゴール位置に第２の二等辺三角形の底辺側の第１頂点を配置する。計画部は、第１の二等辺三角形の底辺側の第２頂点と第２の二等辺三角形の底辺側の第２頂点とを接続する。
　スタート位置とゴール位置とを接続する第１の二等辺三角形および第２の二等辺三角形によって、操舵角および操舵角速度を計画ことができる。そして、第１および第２の二等辺三角形の等辺に沿って曲線軌道を当てはめることで、第１および第２の曲線軌道を生成することができる。曲線軌道を生成するに際し、数値積分などの複雑な計算を用いる必要がない。制御周期内に曲線軌道を生成することが可能となる。
　計画部は、第１の二等辺三角形と第２の二等辺三角形との接続部において第１および第２の二等辺三角形の等辺が一直線になる条件下で、等辺の長さを算出してもよい。計画部は、スタート位置を通りスタート位置進行方向に伸びる第１延長線上の位置であって、スタート位置から等辺の長さの距離を進んだ位置に、第１の二等辺三角形の等辺に挟まれた第３頂点を決定してもよい。計画部は、ゴール位置を通りゴール位置進行方向の反対方向に伸びる第２延長線上の位置であって、ゴール位置から等辺の長さの距離を進んだ位置に、第２の二等辺三角形の等辺に挟まれた第３頂点を決定してもよい。計画部は、第１の二等辺三角形の第３頂点と第２の二等辺三角形の第３頂点との中点を、第１の二等辺三角形の第２頂点および第２の二等辺三角形の第２頂点として決定してもよい。効果の詳細は実施例で説明する。
　計画部は、第１の二等辺三角形の底辺に対して第１の二等辺三角形のスタート位置から伸びる等辺がなす角度である第１角度の正負を判断してもよい。計画部は、第２の二等辺三角形の底辺に対して第２の二等辺三角形の第２頂点から伸びる等辺がなす角度である第２角度の正負を判断してもよい。計画部は、第１角度と第２角度との両方が正または両方が負である場合に、ゴール位置と第２の二等辺三角形の第３頂点とを結んだ第３延長線と第１延長線との交点を決定してもよい。計画部は、交点とスタート位置との第１距離が交点とゴール位置との第２距離よりも小さい場合に、第３延長線上の位置であって交点を始点としてゴール位置へ向かって第１距離を進んだ位置を、第１の二等辺三角形の第２頂点および第２の二等辺三角形の第２頂点として再決定してもよい。計画部は、第１距離が第２距離よりも大きい場合に、第１延長線上の位置であって交点を始点としてスタート位置へ向かって第２距離を進んだ位置を、第１の二等辺三角形の第２頂点および第２の二等辺三角形の第２頂点として再決定してもよい。効果の詳細は実施例で説明する。
　曲線軌道あてはめ部は、パラメータを用いて曲線軌道を示すパラメトリック方程式を微分することで、第１の二等辺三角形の底辺の垂直二等分線と曲線軌道との交点における曲線軌道の傾きが、第１の二等辺三角形の底辺の傾きと等しくなるような特定パラメータの値を決定してもよい。曲線軌道あてはめ部は、特定パラメータによって得られた軌道を、第１の二等辺三角形の底辺の垂直二等分線に対して線対称に組み合わせることで、サイズが正規化された曲線軌道を生成してもよい。曲線軌道あてはめ部は、サイズが正規化された曲線軌道の両端部が第１の二等辺三角形の底辺側の第１頂点および第２頂点と一致するように、サイズが正規化された曲線軌道を拡大または縮小することで、第１の曲線軌道を第１の二等辺三角形の等辺にそって当てはめてもよい。効果の詳細は実施例で説明する。
　パラメトリック方程式は、パラメータの５次式であってもよい。パラメトリック方程式を一階微分した式には、２乗または４乗された形でのみパラメータが含まれていてもよい。
実施例に係るフォークリフトの構成を示すブロック図。 スタート位置およびゴール位置を示す図。 曲線軌道の生成方法を示すフローチャート。 Ｓ３０の処理内容を説明するフローチャート。 第１および第２の二等辺三角形の形成例を示す図。 第１および第２の二等辺三角形の形成例を示す図。 Ｓ４０の処理内容を説明するフローチャート。 第１および第２の二等辺三角形の形成例を示す図。 第１および第２の二等辺三角形の形成例を示す図。 第１および第２の二等辺三角形の形成例を示す図。 Ｓ８０の処理内容を説明するフローチャート。 曲線軌道の当てはめ例を示す図。 曲線形状の形成例を示す図。 曲線軌道の形成例を示す図。 軌道上での位置姿勢を計算する方法を示すフローチャート。
（フォークリフト１０の構成）
　以下、図面を参照して、本実施例のフォークリフト１０について説明する。図１に示すフォークリフト１０は、無人フォークリフトである。フォークリフト１０は、車体１２、測域センサ２６、演算装置３０を備えている。
　車体１２は、前輪２８及び後輪２９を備えている。左右の前輪２８には、エンコーダ９２が接続されており、前輪２８の回転量を検出可能となっている。エンコーダ９２は、左右の前輪２８の動作に基づいて、車体１２の移動量及び移動方向を検出する。前輪２８は、図示しない駆動機構を介して駆動輪モータ９０が接続されており、駆動輪モータ９０によって回転駆動されるようになっている。
　測域センサ２６は、車体１２に設置され、一方向（本実施例では、水平方向）にレーザ光を走査する１次元走査型の測域センサである。測域センサ２６は、車体１２の前方に設定された設定空間内の物体と車体１２との距離を計測する。これにより、フォークリフト１０の前方の距離データを取得する。測域センサ２６で取得された距離データは、演算装置３０の自己位置推定部３８に入力される。
　制御装置３２は、ＣＰＵ等を備えたマイクロプロセッサによって構成されている。制御装置３２は、車体１２に搭載されている。図１に示すように、制御装置３２は、駆動輪モータ９０、操舵装置９４等と通信可能に接続されており、これらの動作を制御する。すなわち、制御装置３２は、駆動輪モータ９０及び操舵装置９４を駆動することで、フォークリフト１０の進行方向及び走行速度を制御する。
　演算装置３０は、ＣＰＵ等を備えたマイクロプロセッサによって構成されている。演算装置３０は、スタート位置／ゴール位置設定部３５、軌道生成装置４０、地図データ５８、自己位置推定部３８、軌道追従制御部４８、を備える。地図データ５８は、フォークリフト１０が移動する空間内に存在する物体（例えば、柱、壁等）の位置及び大きさを示すデータである。地図データ５８は、例えば、フォークリフト１０が移動する領域内の物体を測域センサ２６によって予め測定し、この測定結果に基づいて作成しておくことができる。
　自己位置推定部３８には、地図データ５８と、測域センサ２６により計測される周辺物体との距離データと、エンコーダ９２により検出された車体１２の移動量及び移動方向が入力される。自己位置推定部３８は、これらの入力データに基づいて、フォークリフト１０の自己位置を推定する。スタート位置／ゴール位置設定部３５は、フォークリフト１０のスタート位置とゴール位置を設定する。
　軌道生成装置４０は、操舵計画部４１、曲線軌道生成部４２、曲線軌道当てはめ部４３を備えている。操舵計画部４１は、スタート位置とゴール位置とを接続する第１の二等辺三角形および第２の二等辺三角形を生成することで、フォークリフト１０がスタート位置からゴール位置まで移動する際の操舵方向を計画する。曲線軌道生成部４２は、サイズが正規化された曲線軌道を生成する。曲線軌道当てはめ部４３は、第１および第２の二等辺三角形に曲線軌道を当てはめることで、曲線軌道データを生成する。これにより、操舵角および操舵角速度が求まる。
　軌道追従制御部４８は、曲線軌道当てはめ部４３で生成された曲線軌道データと、フォークリフト１０の現在位置（オドメトリにより算出される自己位置）とに基づいて、ゴール位置まで車体１２を移動するための制御指令値を算出する。生成された制御指令値は、制御装置３２に入力され、制御装置３２は、車体１２をゴール位置に向かって移動させる。
（前提条件）
　本明細書における、記載方法の前提条件を説明する。フォークリフト１０の位置と向きを合わせて、「ポーズ」と呼ぶ場合がある。車輛の中心点(代表点または原点ともいう)は、操舵しない輪の左右の中心とする。本実施例のフォークリフト１０の場合、前輪２８の左右の中心である。この中心点は、タイヤの滑りが無視できる程度に小さいと仮定する。これにより、中心点は車輛の左右には動かず前後だけに動くため、代表点として都合が良い。一般性を失わないため、図２に示すように、スタート位置Ｏを地面座標系の原点とする。また、スタート位置におけるフォークリフト１０の向きであるスタート向きＡ１の方向を、地面座標系のｘ軸方向とする。スタート向きＡ１は、スタート位置Ｏにおけるフォークリフト１０の進行方向である。本実施例では、スタート位置姿勢(ポーズ)から、任意のゴール向きＡ２を有する任意のゴール位置Ｐまでの軌道の生成を考える。ここでゴール向きＡ２は、ゴール位置Ｐに到達した時点におけるフォークリフト１０の進行方向である。また本実施例では、ゴール位置Ｐのｘ座標が正(ｘ＞０)の場合のみ考えることとする。必ずしもゴールでｘ＞０でなくても全く同じプログラムで妥当な軌道が生成出来るケースも存在するが、ｘ＜０の場合はバックする方が妥当なケースが多いためである。
（最初の曲線軌道の生成方法）
　図３のフローチャートを用いて、最初の曲線軌道の生成方法を説明する。図３は、ゴール位置の指示が入力されたときに、曲線軌道を生成する場合のフローである。
　Ｓ１０において、ゴール位置Ｐのポーズが設定される。Ｓ１０は、スタート位置／ゴール位置設定部３５によって実行される処理である。図２の例に示すように、ゴール位置Ｐおよびゴール向きＡ２が設定される。
　Ｓ２０において、ゴール向きＡ２が、スタート向きＡ１と略等しいか否かが判断される。すなわち、ゴール向きＡ２とスタート向きＡ１とが略平行であるか否かが判断される。ここで「略平行」とは、例えば、ゴール向きＡ２とスタート向きＡ１との差が３°以下である場合である。ゴール向きＡ２がスタート向きＡ１と略等しくない場合（Ｓ２０：ＮＯ）には、Ｓ３０へ進む。
　Ｓ３０において、フォークリフト１０がスタート位置Ｏからゴール位置Ｐまで移動する際の操舵角および操舵角速度を計画する。Ｓ３０は、操舵計画部４１によって実行される処理である。具体的には、スタート位置Ｏとゴール位置Ｐとを接続する第１の二等辺三角形および第２の二等辺三角形を生成する。
　図４のフローチャートを用いて、Ｓ３０の処理内容を説明する。Ｓ１１０において、第１および第２の二等辺三角形の等辺の長さＬを計算する。第１および第２の二等辺三角形の等辺の長さＬは、等しい。これは、解きやすさと解の妥当性を考慮したものである。等辺の長さＬを変数とおく。また図５に示すように、第１の二等辺三角形ＩＴ１と第２の二等辺三角形ＩＴ２との接続部において等辺ＥＳ１２と等辺ＥＳ２１が一直線になる条件を定める。この条件下で方程式を立てると２次方程式となるため、その２次方程式を解く。これにより、等辺の長さＬが求められる。
　Ｓ１２０において、第１および第２の二等辺三角形を配置する。図５を用いて具体例を説明する。スタート位置Ｏに、第１の二等辺三角形ＩＴ１の底辺側の第１頂点ＶＥ１１を配置する。ゴール位置Ｐに、第２の二等辺三角形ＩＴ２の底辺側の第１頂点ＶＥ２１を配置する。スタート位置Ｏを通りスタート向きＡ１方向に伸びる延長線であるｘ軸上の位置であって、スタート位置Ｏから等辺の長さＬの距離を進んだ位置に、第１の二等辺三角形ＩＴ１の等辺に挟まれた第３頂点ＶＥ１３（Ｓ点ともいう）を決定する。ゴール位置Ｐを通りゴール向きＡ２の反対方向に伸びる延長線上の位置であって、ゴール位置Ｐから等辺の長さＬの距離を進んだ位置に、第２の二等辺三角形ＩＴ２の等辺に挟まれた第３頂点ＶＥ２３（Ｑ点ともいう）を決定する。
　Ｓ１３０において、第１の二等辺三角形ＩＴ１の第３頂点ＶＥ１３（Ｓ点）と、第２の二等辺三角形ＩＴ２の第３頂点ＶＥ２３（Ｑ点）との中点であるＲ点を求める。そしてＲ点を、第１の二等辺三角形ＩＴ１の第２頂点ＶＥ１２および第２の二等辺三角形ＩＴ２の第２頂点ＶＥ２２として決定する。換言すると、第１の二等辺三角形および第２の二等辺三角形を生成するとは、Ｒ点の位置、および、Ｒ点を通る等辺ＥＳ１２およびＥＳ２１の傾きを求めることでもある。
　Ｓ１４０において、第１の二等辺三角形ＩＴ１の等辺および第２の二等辺三角形ＩＴ２の等辺に曲線軌道を当てはめた場合に、その曲線軌道の最大曲率が、予め定められた閾値より大きいか否かを判断する。当てはめる曲線のタイプが決まっていれば、二等辺三角形の頂角とサイズから最大曲率を一意に求めることができる。最大曲率が閾値より大きい場合（Ｓ１４０：ＹＥＳ）には、カーブが急峻過ぎると判断され、Ｓ１５０へ進み、エラーとされる。一方、最大曲率が閾値以下の場合（Ｓ１４０：ＮＯ）には、第１の二等辺三角形および第２の二等辺三角形の生成が完了したと判断され、図３のＳ４０へ進む。
　最大曲率の閾値は、予め定められた最小角度（例：６０度から９０度の範囲）よりも小さい頂角に当てはめられた曲線軌道を、エラーとして除外するように定めてもよい。これにより、例えば図６の例のように、第２の二等辺三角形ＩＴ２の第３頂点ＶＥ２３の頂角が９０°よりも小さい場合に、曲線軌道の生成を中止することができる。
　図３のフローに説明を戻す。Ｓ４０において、一回操舵への変更を試行する。図７のフローチャートおよび図５を用いて、Ｓ４０の処理内容を説明する。
　Ｓ２１０において、第１の二等辺三角形によって計画される１回目の操舵方向と、第２の二等辺三角形によって計画される２回目の操舵方向が同方向であるか否かを判断する。具体的には、第１の二等辺三角形ＩＴ１において、底辺（線ＯＲ）に対して、スタート位置Ｏから伸びる等辺ＥＳ１１（線ＯＳ）がなす底角である第１角度ＡＧ１の正負を判断する。また、第２の二等辺三角形ＩＴ２において、底辺（線ＲＰ）に対して、第２頂点ＶＥ２２（Ｒ点）から伸びる等辺ＥＳ２１（線ＲＱ）がなす底角である第２角度ＡＧ２の正負を判断する。
　第１角度ＡＧ１と第２角度ＡＧ２の両方が正、または、両方が負である場合には、１回目の操舵方向と２回目の操舵方向が同方向であると判断される（Ｓ２１０：ＹＥＳ）。例えば、図８に示すように、第１角度ＡＧ１および第２角度ＡＧ２の両方が正である場合である。この場合、１回操舵へ変更するためにＳ２２０へ進む。
　Ｓ２２０において、図８に示すように、ゴール位置Ｐと第２の二等辺三角形ＩＴ２の第３頂点ＶＥ２３（Ｑ点）とを結んだ線ＱＰの延長線ＥＸ３を生成する。そして延長線ＥＸ３とｘ軸との交点Ｃを決定する。
　Ｓ２３０において、交点Ｃの位置が適切であるか否かが判断される。具体的には、交点Ｃが、ｘ＞０かつゴール位置Ｐのゴール向きＡ２のベクトルの後ろ側にあるか（すなわち線ＣＰのベクトルとゴール向きＡ２のベクトルの内積が負であるか）が判断される。同方向に二回操舵していることから、通常は、交点Ｃの位置は必ず適切になる。Ｓ２３０において否定判断される場合（Ｓ２３０：ＮＯ）には、エラー判断され、フローが終了する。一方、Ｓ２３０において肯定判断される場合（Ｓ２３０：ＹＥＳ）には、交点Ｃを中心に新たな二等辺三角形を作成するために、Ｓ２４０へ進む。
　Ｓ２４０において、交点Ｃが、スタート位置Ｏとゴール位置Ｐの何れに近いかを判断する。具体的には、スタート位置Ｏと交点Ｃとの第１距離Ｄ１と、交点Ｃとゴール位置Ｐとの第２距離Ｄ２とを比較する。
　第１距離Ｄ１の方が第２距離Ｄ２よりも小さい場合には、交点Ｃがスタート位置Ｏに近い場合であると判断され、Ｓ２５０へ進む。Ｓ２５０において、延長線ＥＸ３上の位置であって交点Ｃを始点としてゴール位置Ｐへ向かって第１距離Ｄ１を進んだ位置を、第１の二等辺三角形ＩＴ１の第２頂点ＶＥ１２（Ｒ点）および第２の二等辺三角形ＩＴ２の第２頂点ＶＥ２２（Ｒ点）として再決定する。これにより、図９の例に示すように、新たな第１の二等辺三角形ＩＴ１および第２の二等辺三角形ＩＴ２が形成される。そしてＳ２８０へ進む。
　図９において、第２の二等辺三角形ＩＴ２は一直線に潰れている。すなわち、Ｒ点とゴール位置Ｐとを直線で結ぶことができる。これにより、第１の二等辺三角形ＩＴ１の１つだけで操舵計画を立てるように修正することができる。従って、１つの二等辺三角形に後述する方法で曲線をはめ込むことで、曲線軌道を完成させることが可能となる。曲率がほぼ一定に変化する曲線長が長くとれるため、全体として曲率の変化を小さくすることができる。従って、同じ線速度で移動するとした場合の操舵速度の最大値を低く抑えることが可能となる。
　また、第２距離Ｄ２の方が第１距離Ｄ１よりも小さい場合には、交点Ｃがゴール位置Ｐに近い場合であると判断され、Ｓ２６０へ進む。Ｓ２６０において、ｘ軸上の位置であって交点Ｃを始点としてスタート位置Ｏへ向かって第２距離Ｄ２を進んだ位置を、第１の二等辺三角形ＩＴ１の第２頂点ＶＥ１２（Ｒ点）および第２の二等辺三角形ＩＴ２の第２頂点ＶＥ２２（Ｒ点）として再決定する。これにより、新たな第１の二等辺三角形ＩＴ１および第２の二等辺三角形ＩＴ２が形成される。第１の二等辺三角形ＩＴ１は一直線に潰れている。これにより、第２の二等辺三角形ＩＴ２の１つだけで操舵計画を立てるように、計画を修正することができる。
　一方、Ｓ２１０において、第１角度ＡＧ１と第２角度ＡＧ２の一方が正であり他方が負である場合には、１回目の操舵方向と２回目の操舵方向が異なる方向であると判断される（Ｓ２１０：ＮＯ）。この場合、２回操舵を１回操舵へ変更することは行わない。従ってＳ２７０へ進む。図５に示す例では、第１角度ＡＧ１が負であり第２角度ＡＧ２が正であるため、Ｓ２１０で否定判断される。
　Ｓ２７０において、第１および第２の二等辺三角形の頂角の比率に応じて、二等辺三角形の等辺の長さＬを第１および第２の二等辺三角形で定数倍に変化させる。具体的には、第１および第２の二等辺三角形のうち、頂角が小さい方（すなわちカーブが急である方）の二等辺三角形の等辺の長さを長くし、頂角が大きい方（すなわちカーブが緩やかである方）の二等辺三角形の等辺の長さを短くする。例えば図６に示すように、第２の二等辺三角形ＩＴ２の頂角が小さい場合には、図１０に示すように、第２の二等辺三角形ＩＴ２の等辺の長さをＬからＬ２へ長くするとともに、第１の二等辺三角形ＩＴ１の等辺の長さをＬからＬ１へ短くする。これにより、第１の二等辺三角形ＩＴ１を小さく、第２の二等辺三角形ＩＴ２を大きくすることができる。よって、第２の二等辺三角形ＩＴ２の面積を大きくすることができるため、第２の二等辺三角形ＩＴ２で計画される曲線軌道の最大曲率を小さくする（すなわちカーブを緩やかにする）ことが可能となる。
　Ｓ２８０において、Ｓ３０で作成した二等辺三角形と、Ｓ２５０、Ｓ２６０、Ｓ２７０で再作成した二等辺三角形のうち、最大曲率が小さい方の二等辺三角形を選択する。そして図３のＳ８０へ進む。
　図３のフローに説明を戻す。Ｓ２０において、ゴール向きＡ２が、スタート向きＡ１と略等しい場合（Ｓ２０：ＹＥＳ）にはＳ５０へ進む。Ｓ５０において、ゴール位置Ｐのｘ座標値が０に近いか否かを判断する。肯定判断された場合（Ｓ５０：ＹＥＳ）には、スタート向きＡ１とゴール向きＡ２とが略等しく、かつ、スタート位置Ｏとゴール位置Ｐとが真横に位置する場合である。このようなケースでは、大回りする曲線軌道を用いてゴールに至るよりも、途中で切り返すことが適切であると考えられる。そのため、Ｓ６０へ進み、エラー(軌道生成不可能)とする。
　一方、Ｓ５０において否定判断された場合（Ｓ５０：ＮＯ）は、Ｓ７０へ進む。この場合は、ゴール向きＡ２がスタート向きＡ１と略等しく、スタート位置Ｏとゴール位置Ｐとがある程度離れている場合である。このような場合、Ｓ１１０で説明した２次方程式の二つの解の和と差が無限大に発散するため、計算が困難となる。すなわち、この条件を満たす点は、二次方程式の２個の解ではなく１個の解となる。そこで、Ｓ７０へ進み、二次方程式の解の公式ではなく、級数展開で解を求める。級数展開は、周知の方法を用いればよいため、ここでは詳細な説明を省略する。これにより、ゴール向きＡ２がスタート向きＡ１と略等しい場合においても、安定して連続的な解を求めることが出来る。
　Ｓ８０において、第１および第２の二等辺三角形に曲線軌道を当てはめる。具体的には、第１の二等辺三角形ＩＴ１の等辺に沿った第１の曲線軌道、および、第２の二等辺三角形ＩＴ２の等辺に沿った第２の曲線軌道を生成する。
　図１１のフローチャートを用いて、Ｓ８０の処理内容を説明する。一般化のため、第１の二等辺三角形ＩＴ１に対する曲線軌道の当てはめについてのみ説明する。第２の二等辺三角形ＩＴ２に対する曲線軌道の当てはめは、原点移動と回転移動することでＲ点を原点にすれば、第１の二等辺三角形ＩＴ１と全く同じ方法で行うことが出来るためである。
　ここで、曲線軌道の当てはめには、以下の法則が成立する。（法則１）二等辺三角形に対称に曲線を当てはめる。（法則２）スタート位置Ｏで、曲率および傾きともにゼロである。（法則３）傾きが連続であると言う条件から、図１２に示すように、二等辺三角形の底辺ＥＳ１３の垂直二等分線ＰＢ１と曲線軌道との交点ＩＳ１における曲線軌道の傾きは、二等辺三角形の底辺ＥＳ１３（直線ＯＲ）の傾きと等しい。
　曲線軌道の当てはめには、曲線長と曲率の関係が線形に近い曲線を生成するためのパラメトリック曲線を使用する。パラメトリック曲線は、曲率傾きが０から始まって、漸増していく曲線であれば、いずれの曲線であってもよい。本実施例では下式（１）（２）で表されるパラメトリック曲線を用いる場合を説明する。
　　ｘ＝ｍ×（ｔ－ｔ５／４０）　・・・式（１）
　　ｙ＝ｍ×ｔ３／６　・・・式（２）
　ここで、倍率ｍは相似変換のスケーリングの定数であり、ｔはパラメータである。本実施例では、ｍ＝１の場合(すなわち正規化されている場合）を説明する。
　なお、実際の計算では、下式（３）に示すようなパラメータｕを用いて計算してもよい。
　　ｔ＝ｕ／ｍ　・・・式（３）
　式（３）を式（１）および（２）に代入すると、下式（４）および（５）が得られる。
　　ｘ＝ｕ－ｕ５／（４０ｍ４）　・・・式（４）
　　ｙ＝ｕ３／（６ｍ２）　・・・式（５）
　式（４）および（５）においてｍは明示的には計算せず、「１／ｍ２」を直接計算すればよい。式（４）および（５）ではｍの２乗および４乗しか出てこないため、「１／ｍ」を計算する必要がないためである。
　式（４）および（５）を用いて計算を行う効果を説明する。二等辺三角形がつぶれて直線になる場合、底辺の傾きがゼロになり、ｍは無限大に発散し、ｔの範囲が０から０となるため、式（１）および（２）では計算が不可能になる。しかし、式（４）および（５）では、ｍとｔを明示的に使用していない。式（４）および（５）では、「１／ｍ２」の計算を用いるとともに、「ｕ」の範囲は０からｕの最大値で計算すればよい。これにより、二等辺三角形が完全につぶれて直線になる場合も含めて、安定して計算をすることができる。実用上は、二等辺三角形がつぶれるケースが多いため、式（４）および（５）を用いた計算方法は有益である。また、パラメータｕは、二等辺三角形にフィットさせた後の曲線長にほぼ一致する。従って、真の曲線長を計算せずに、パラメータｕを擬似曲線長として用いて速度パターンを作成できる。数値積分を不要にすることができるため、速度パターンの設計を簡易に行うことが可能になる。
　Ｓ３１０において、サイズを正規化した曲線形状を生成する。具体的に説明する。式（１）および（２）の曲線では、傾きを与えた時、対応するパラメータｔは一意である。よって、二次方程式を解くことでパラメータｔを求めることができる。すなわち、式（１）および（２）を数式的に微分することで、第１の二等辺三角形ＩＴ１の底辺ＥＳ１３の垂直二等分線ＰＢ１（図１２参照）と曲線軌道との交点ＩＳ１における曲線軌道の傾きが、第１の二等辺三角形ＩＴ１の底辺ＥＳ１３の傾きと等しくなるようなパラメータｔの値を決定する。決定したパラメータｔの値を用いて、式（１）および（２）の相似変換の倍率ｍを「１」とした、サイズを正規化した曲線形状が得られる。図１２の第１の二等辺三角形ＩＴ１の例では、図１３に示すような曲線形状ＣＳ１が得られる。
　Ｓ３２０において、正規化された曲線形状を、第１の二等辺三角形ＩＴ１の底辺ＥＳ１３の垂直二等分線ＰＢ１に対して線対称に組み合わせる。これにより、サイズが正規化された曲線軌道ＣＴ１が生成される。図１２の第１の二等辺三角形ＩＴ１の例では、図１４に示すように、曲線形状ＣＳ１およびＣＳ２を組み合わせた曲線軌道ＣＴ１が得られる。曲線形状ＣＳ１およびＣＳ２の接続部分ＣＮの傾きＧＲ１（図１４）は、対称性と微分可能の条件より、図１２の第１の二等辺三角形ＩＴ１の底辺ＥＳ１３の傾きと等しい。
　Ｓ３３０において、サイズが正規化された曲線軌道ＣＴ１の両端部が、第１の二等辺三角形ＩＴ１の底辺側の第１頂点ＶＥ１１（スタート位置Ｏ）、および第２頂点ＶＥ１２（Ｒ点）と一致するように、曲線軌道ＣＴ１を拡大または縮小する。具体的には、図１４の曲線軌道ＣＴ１の一方の端部Ｅ１を、図１２のスタート位置Ｏに一致させる。このとき、曲線軌道ＣＴ１の他方の端部Ｅ２が到達する位置と、図１２のＲ点との比率を求める。そして、上記で求めた比率に基づいて、曲線軌道ＣＴ１を拡大または縮小する。これにより、図１２に示すように、第１の二等辺三角形ＩＴ１に対する曲線軌道ＣＴ１の当てはめが完了する。
　第２の二等辺三角形ＩＴ２についても、上述したＳ３１０～Ｓ３３０の処理を行うことにより、曲線軌道ＣＴ２を当てはめることができる。これにより、様々なスタート位置Ｏおよびゴール位置Ｐの組み合わせに対し、自然な曲線軌道を生成することができる。
　Ｓ３４０において、二等辺三角形に曲線を当てはめる場合の係数を保存する。具体的には、Ｓ３３０で求めた拡縮比率と、Ｓ３１０で取得したパラメータｔの値を保存する。なお、拡縮比率の逆数を二乗した値を、拡縮比率として用いてもよい。そしてフローを終了する。
　なお、Ｓ３１０およびＳ３２０は、曲線軌道生成部４２によって実行される処理である。Ｓ３３０は、曲線軌道当てはめ部４３によって実行される処理である。
（軌道上での位置姿勢の計算方法）
　図１５のフローチャートを用いて、フォークリフト１０の移動中において、軌道上での位置姿勢を計算する方法を説明する。図１５のフローチャートは、例えば、一定の制御周期（例：１０ミリ秒毎）で実行される。図１５のフローチャートの処理は、軌道追従制御部４８によって実行される処理である。
　Ｓ４１０において、第１および第２の二等辺三角形の４つの等辺ＥＳ１１、ＥＳ１２、ＥＳ２１、ＥＳ２２の何れの辺に対応する曲線を計算すべきかを判断する。具体的に説明する。４つの等辺に対応する、パラメータｔの最大値(それぞれで０から最大値まで変化する)が求められている。よって、与えられたパラメータｔの値と大小比較することで、何れの等辺に対応する曲線を計算すべきかが容易に判断できる。
　Ｓ４２０において、パラメータｔに対応する位置(ｘ，ｙ)、傾き、曲率を計算する。具体的に説明する。パラメータｔの「倍率ｍ」分の１を、曲線軌道の正規化された式に代入することで、位置(ｘ，ｙ)が求まる。曲線軌道を示す式の微分値から、位置(ｘ，ｙ)での傾きを求めることができる。また二階微分によって、位置(ｘ，ｙ)での曲率κを求めることができる。
　なお、フォークリフト１０の操舵角φは、下式（６）に示すように、曲線軌道の曲率κと一対一に対応している。
　　λ(cosφ, sinφ, 0) = (v, 0, 0) + (0,0,κv)×(bx, by, 0)　・・・式（６）
　ここで、vは車輛の中心の前進速度である。bxは、操舵輪の位置(ホイールベース）である。byは、車輛の中心点であり、トレッドの半分である。λは適当な定数である。従って、下式（７）によって、操舵角φが求まる。
　　φ = tan -1 (bx κ / (1 - by κ))　・・・式（７）
（効果）
　ガイドレスで無人走行するフォークリフトを実現するには、仮想的に走行ラインを引くこと、すなわち軌道生成が必要である。代表的な軌道生成の方法として、クロソイド曲線を用いた方法や、Ｂ－スプライン曲線を用いた方法が挙げられる。またフォークリフトの特徴として、操舵速度の上限が低いことが挙げられる。これはパワーステアリングであるため、ハンドルを高速に回せないためである。そして軌道生成にクロソイド曲線を用いる場合には、クロソイド点の座標は初等関数で表すことは不可能であるため、数値積分が必要となる。すると制御周期内で軌道データを生成することが困難な場合がある。また軌道生成にＢ－スプライン曲線を用いる場合には、曲線軌道の始点と終点近くでの曲率変化が大きくなってしまう傾向がある。そのため、曲率変化がフォークリフトの操舵速度の上限を越えてしまい、フォークリフトが曲線軌道を追従することが困難となる場合がある。
　本実施形態の軌道生成装置４０は、スタート位置Ｏとゴール位置Ｐとを接続する第１の二等辺三角形および第２の二等辺三角形に曲線軌道を当てはめることで、軌道を生成する。これにより、第１の効果として、二次方程式の求解および座標変換という代数計算のみで軌道の設計が可能になる。クロソイド曲線などの数値積分が必要な曲線を用いる場合に比して、短時間で曲線軌道を計算できる。制御周期内で軌道データを生成することが可能になる。また、第２の効果として、曲線軌道の始点と終点近くでの曲率変化が小さい軌道を生成することができる。すなわち、スタート位置Ｏおよびゴール位置Ｐで、曲率・傾きともにゼロである曲線軌道を生成することができる。これは、図５に示すように、スタート向きＡ１方向に伸びる延長線（ｘ軸）と第１の二等辺三角形ＩＴ１の等辺ＥＳ１１とが重なるとともに、ゴール向きＡ２の反対方向に伸びる延長線上に第２の二等辺三角形ＩＴ２の等辺ＥＳ２２が重なるように、二等辺三角形を配置するためである。これにより、Ｂ－スプライン曲線で作成した軌道に比して、操舵速度の上限が低いフォークリフトに適した軌道を生成することができる。
　本実施形態の軌道生成装置４０は、式（１）および（２）で表されるパラメトリック方程式を用いている。式（１）および（２）を用いる効果を説明する。Ｓ３１０では、曲線の傾きを元にしてパラメータｔの値を決定している。このとき、二次方程式を解くことでパラメータｔを求める。そして、式（１）の５次式および式（２）の３次式は、微分するとパラメータｔのｔ4とｔ2のみを含んだ式となるため、「ｔ2」を変数としてみることで２次方程式に容易に帰着することができる。すなわち式（１）および（２）を用いることで、代数計算が実用的に可能になる。なお、３次式のパラメトリック方程式は代数計算での計算が可能であるが、式（１）の５次式に比して利用できる範囲が狭く、直角に曲がる時に利用できない。よって式（１）の５次式は、適用範囲が広く、かつ計算が容易であるという特徴を備えた式であることが分かる。
　以上、本明細書が開示する技術の実施例について詳細に説明したが、これらは例示に過ぎず、請求の範囲を限定するものではない。請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。
（変形例）
　Ｓ６０において、エラーにする代わりに、進行速度を前後に切りかえる切り返し軌道を生成してもよい。すなわち、切り返し点を適当に与え、切り返し点まで本アルゴリズムで前進用の軌道を生成してもよい。さらに切り返し点からゴール位置Ｐまで、本アルゴリズムで後退用の軌道を生成してもよい。
　本実施例では、測域センサ２６として１次元走査型の測域センサを用いたが、各種のセンサを用いてもよい。例えば測域センサ２６は、2D LiDAR、3D LiDAR、ステレオカメラ、単眼カメラ、距離画像センサなどでもよい。自己位置推定部３８は、衛星測位を用いる手段によって自己位置を推定してもよい。
　式（１）および（２）は、パラメトリック曲線の一例である。本実施例の技術では、様々なタイプのパラメトリック曲線を使用することが可能である。
　Ｓ２８０において、二等辺三角形の選択方法は様々な態様であってよい。例えば、曲率変化の最大値が小さい方の二等辺三角形を選択してもよい。
　本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。
　スタート向きＡ１は、スタート位置進行方向の一例である。ゴール向きＡ２は、ゴール位置進行方向の一例である。操舵計画部４１は、計画部の一例である。Ｓ点は、第１の二等辺三角形の等辺に挟まれた第３頂点の一例である。Ｑ点は、第２の二等辺三角形の等辺に挟まれた第３頂点の一例である。

Claims (5)

  1. 　移動体が移動する軌道を生成する軌道生成装置であって、
    　前記移動体のスタート位置、前記スタート位置における前記移動体の進行方向であるスタート位置進行方向、前記移動体のゴール位置、前記ゴール位置における前記移動体の進行方向であるゴール位置進行方向、に基づいて、前記スタート位置と前記ゴール位置とを接続する第１の二等辺三角形および第２の二等辺三角形を生成することで、前記移動体が前記スタート位置から前記ゴール位置まで移動する際の操舵角および操舵角速度を計画する計画部と、
    　前記第１の二等辺三角形の等辺に沿って第１の曲線軌道を当てはめるとともに、前記第２の二等辺三角形の等辺に沿って第２の曲線軌道を当てはめる曲線軌道あてはめ部と、
    　を備え、
    　前記計画部は、
    　　前記第１の二等辺三角形の等辺と前記第２の二等辺三角形の等辺の長さを等しくし、
    　　前記スタート位置に前記第１の二等辺三角形の底辺側の第１頂点を配置し、
    　　前記ゴール位置に前記第２の二等辺三角形の底辺側の第１頂点を配置し、
    　　前記第１の二等辺三角形の底辺側の第２頂点と前記第２の二等辺三角形の底辺側の第２頂点とを接続する、軌道生成装置。
  2. 　前記計画部は、
    　　前記第１の二等辺三角形と前記第２の二等辺三角形との接続部において前記第１および第２の二等辺三角形の等辺が一直線になる条件下で、前記等辺の長さを算出し、
    　　前記スタート位置を通り前記スタート位置進行方向に伸びる第１延長線上の位置であって、前記スタート位置から前記等辺の長さの距離を進んだ位置に、前記第１の二等辺三角形の等辺に挟まれた第３頂点を決定し、
    　　前記ゴール位置を通り前記ゴール位置進行方向の反対方向に伸びる第２延長線上の位置であって、前記ゴール位置から前記等辺の長さの距離を進んだ位置に、前記第２の二等辺三角形の等辺に挟まれた第３頂点を決定し、
    　　前記第１の二等辺三角形の前記第３頂点と前記第２の二等辺三角形の前記第３頂点との中点を、前記第１の二等辺三角形の前記第２頂点および前記第２の二等辺三角形の前記第２頂点として決定する、
    　請求項１に記載の軌道生成装置。
  3. 　前記計画部は、
    　　前記第１の二等辺三角形の底辺に対して前記第１の二等辺三角形の前記スタート位置から伸びる等辺がなす角度である第１角度の正負を判断し、
    　　前記第２の二等辺三角形の底辺に対して前記第２の二等辺三角形の前記第２頂点から伸びる等辺がなす角度である第２角度の正負を判断し、
    　　前記第１角度と前記第２角度との両方が正または両方が負である場合に、前記ゴール位置と前記第２の二等辺三角形の第３頂点とを結んだ第３延長線と前記第１延長線との交点を決定し、
    　　前記交点と前記スタート位置との第１距離が前記交点と前記ゴール位置との第２距離よりも小さい場合に、前記第３延長線上の位置であって前記交点を始点として前記ゴール位置へ向かって前記第１距離を進んだ位置を、前記第１の二等辺三角形の前記第２頂点および前記第２の二等辺三角形の前記第２頂点として再決定し、
    　　前記第１距離が前記第２距離よりも大きい場合に、前記第１延長線上の位置であって前記交点を始点として前記スタート位置へ向かって前記第２距離を進んだ位置を、前記第１の二等辺三角形の前記第２頂点および前記第２の二等辺三角形の前記第２頂点として再決定する、
    　請求項２に記載の軌道生成装置。
  4. 　前記曲線軌道あてはめ部は、
    　　パラメータを用いて前記曲線軌道を示すパラメトリック方程式を微分することで、前記第１の二等辺三角形の底辺の垂直二等分線と曲線軌道との交点における曲線軌道の傾きが、第１の二等辺三角形の底辺の傾きと等しくなるような特定パラメータの値を決定し、
    　　前記特定パラメータによって得られた軌道を、前記第１の二等辺三角形の底辺の垂直二等分線に対して線対称に組み合わせることで、サイズが正規化された曲線軌道を生成し、
    　　前記サイズが正規化された曲線軌道の両端部が前記第１の二等辺三角形の底辺側の前記第１頂点および前記第２頂点と一致するように、前記サイズが正規化された曲線軌道を拡大または縮小することで、前記第１の曲線軌道を前記第１の二等辺三角形の等辺にそって当てはめる、
    　請求項１～３の何れか１項に記載の軌道生成装置。
  5. 　前記パラメトリック方程式は、前記パラメータの５次式であり、
    　前記パラメトリック方程式を一階微分した式には、２乗または４乗された形でのみ前記パラメータが含まれている、請求項４に記載の軌道生成装置。
PCT/JP2019/006620 2018-03-19 2019-02-21 軌道生成装置 WO2019181351A1 (ja)

Priority Applications (2)

Application Number Priority Date Filing Date Title
US16/981,918 US11422562B2 (en) 2018-03-19 2019-02-21 Path generation device
DE112019001402.1T DE112019001402T5 (de) 2018-03-19 2019-02-21 Pfaderzeugungsvorrichtung

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2018-050799 2018-03-19
JP2018050799A JP7027206B2 (ja) 2018-03-19 2018-03-19 軌道生成装置

Publications (1)

Publication Number Publication Date
WO2019181351A1 true WO2019181351A1 (ja) 2019-09-26

Family

ID=67987052

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2019/006620 WO2019181351A1 (ja) 2018-03-19 2019-02-21 軌道生成装置

Country Status (4)

Country Link
US (1) US11422562B2 (ja)
JP (1) JP7027206B2 (ja)
DE (1) DE112019001402T5 (ja)
WO (1) WO2019181351A1 (ja)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112198065B (zh) * 2020-08-27 2024-07-12 京东方科技集团股份有限公司 一种柔性屏的弯折测试设备及弯折测试方法
CN114115268B (zh) * 2021-11-24 2024-04-19 广州小鹏自动驾驶科技有限公司 路径生成方法、装置、交通工具及存储介质
JP2023098264A (ja) * 2021-12-28 2023-07-10 三菱ロジスネクスト株式会社 移動体の制御方法、移動体及びプログラム

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH07129243A (ja) * 1993-11-08 1995-05-19 Shinko Electric Co Ltd 無人車の操舵制御装置
JP2000132228A (ja) * 1998-10-23 2000-05-12 Hitachi Zosen Corp 移動体の誘導方法
JP2010073080A (ja) * 2008-09-22 2010-04-02 Komatsu Ltd 無人車両の走行経路生成方法
JP2017010292A (ja) * 2015-06-23 2017-01-12 株式会社明電舎 Ａｇｖ軌道計算装置及び方法
JP2018002082A (ja) * 2016-07-07 2018-01-11 アルパイン株式会社 移動経路生成装置および移動経路生成方法

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2855147B2 (ja) 1990-03-02 1999-02-10 トヨタ自動車株式会社 視覚付無人車
JP4729345B2 (ja) * 2005-05-30 2011-07-20 博明 山口 移動機構、移動機構用部材及び該移動機構の制御方法
DE102005058809A1 (de) * 2005-12-09 2007-06-14 Hella Kgaa Hueck & Co. Bahnplanung
US10118696B1 (en) * 2016-03-31 2018-11-06 Steven M. Hoffberg Steerable rotating projectile
US11828859B2 (en) * 2016-05-07 2023-11-28 Canyon Navigation, LLC Navigation using self-describing fiducials
US10663966B2 (en) * 2017-03-29 2020-05-26 Mitsubishi Electric Research Laboratories, Inc. Vehicle motion control system and method

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH07129243A (ja) * 1993-11-08 1995-05-19 Shinko Electric Co Ltd 無人車の操舵制御装置
JP2000132228A (ja) * 1998-10-23 2000-05-12 Hitachi Zosen Corp 移動体の誘導方法
JP2010073080A (ja) * 2008-09-22 2010-04-02 Komatsu Ltd 無人車両の走行経路生成方法
JP2017010292A (ja) * 2015-06-23 2017-01-12 株式会社明電舎 Ａｇｖ軌道計算装置及び方法
JP2018002082A (ja) * 2016-07-07 2018-01-11 アルパイン株式会社 移動経路生成装置および移動経路生成方法

Also Published As

Publication number Publication date
US11422562B2 (en) 2022-08-23
JP7027206B2 (ja) 2022-03-01
DE112019001402T5 (de) 2020-12-24
US20210109529A1 (en) 2021-04-15
JP2019164434A (ja) 2019-09-26

Similar Documents

Publication Publication Date Title
US20200149906A1 (en) Path planning method, system and device for autonomous driving
WO2019181351A1 (ja) 軌道生成装置
CN107585163B (zh) 移动路线生成装置以及移动路线生成方法
Pérez et al. Trajectory generator for autonomous vehicles in urban environments
CN111806467A (zh) 一种基于车辆行驶规律的变速动态换道轨迹规划方法
CN113204236A (zh) 一种智能体路径跟踪控制方法
CN111830979A (zh) 一种轨迹优化方法和装置
CN110320906B (zh) 一种基于麦克纳姆轮的四驱agv小车差速直线行驶姿态调整方法
US20230211786A1 (en) Path-controlling module, associated path-controlling device and associated method
CN105867372B (zh) 全方位移动机器人狭窄直角弯下转向运动规划方法及系统
Dai et al. Integration of sliding mode based steering control and PSO based drive force control for a 4WS4WD vehicle
Zhang et al. Smooth path and velocity planning under 3D path constraints for car-like vehicles
CN114889643A (zh) 一种运动障碍物的三元素自主避障方法
Kumagai et al. Achievement of recognition guided teleoperation driving system for humanoid robots with vehicle path estimation
CN108334074B (zh) 控制系统
Xing et al. Lane change strategy for autonomous vehicle
Meng et al. A Path Planning and Model Predictive Control for Automatic Parking System
EP4290020A1 (en) System, method, and work vehicle
Fnadi et al. Local obstacle-skirting path planning for a fast Bi-steerable rover using bézier curves
Gim et al. Parametric continuous curvature path for smooth steering with car-like vehicles
JP4097844B2 (ja) 移動体の誘導装置
Das et al. Path tracking and control for parallel parking
Dai et al. Online path tracking and motion optimization of a 4WS4WD vehicle
Avanzini et al. A control strategy taking advantage of inter-vehicle communication for platooning navigation in urban environment
Mohammadi et al. Authoritative intelligent perfect parallel parking based on fuzzy logic controller for car-type mobile robot

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

Country of ref document: EP

Kind code of ref document: A1

122 Ep: pct application non-entry in european phase

Ref document number: 19771439

Country of ref document: EP

Kind code of ref document: A1