A real-time spatial path planning method

Abstract

This invention discloses a real-time spatial path planning method, comprising the following steps: modeling the mobile chassis as a moving mass in a global coordinate system; establishing a coordinate system for the first joint; establishing a coordinate system for the second joint; establishing an end effector workspace with the origin of the second joint coordinate system as the center and the connecting rod length as the radius; obtaining a set of candidate path points from the intersection of the workspace and the linear constraint equations; mapping this set of points as the constraint space to the joint space of the end effector after obtaining the constraint intersection space; and then, combining the constraint conditions, obtaining the workspace path point set, which represents the coordinates of the end effector of the top-attacking mechanism in the global coordinate system at the next moment. The workspace and path modeling method of this invention greatly reduces the computational load when solving for path points, meeting the application requirements of real-time systems.

Description

Technical Field

[0001] The invention belongs to the field of path planning, in particular to a real-time space path planning method. Background Art

[0002] In existing spatial path planning algorithms, the space is gridded or divided into small areas by random sampling, and the path between points is planned by heuristic search. However, the computational complexity of these path planning algorithms increases exponentially as the spatial dimension increases, and the computational time often cannot meet the needs of real-time online systems. This results in the planned path not being guaranteed to be the shortest path between the starting point and the end point. In addition, the traditional path planning algorithm only considers the position path planning of the robot end without considering the movement of the robot mounting base. Therefore, we propose an online real-time spatial path planning algorithm that takes into account the factors of the robot end effector and the mobile base. Summary of the invention

[0003] The purpose of the present invention is to overcome the shortcomings of the prior art and provide a real-time spatial path planning method.

[0004] The object of the present invention is to provide a real-time spatial path planning method, comprising the following steps:

[0005] 1) Model the mobile chassis as a moving mass point in the global coordinate system;

[0006] 2) The first joint is established as a coordinate system. The coordinate system of the first joint is a right-handed coordinate system with the rotation center as the z-axis, the y-axis and the movement center x-axis coinciding. The relative position relationship between the chassis mass coordinate system and the first joint coordinate system is obtained through the homogeneous transformation matrix. The relative position of the chassis and the first joint is obtained through the 4×4 homogeneous transformation matrix T 10 Representation: The second joint is established as a coordinate system. The second joint coordinate system is a right-handed coordinate system with the second joint rotation center as the z-axis and the y-axis vertically downward. The relative position relationship between the first joint coordinate system and the second joint coordinate system is obtained by the homogeneous transformation matrix. The relative position relationship between the first joint and the second joint coordinate system is expressed by the homogeneous transformation matrix T 21 Indicates that the coordinates of the origin of the second joint coordinate system in the global coordinate system are obtained; the end coordinate system is established at the end of the second link, and the coordinate system of the end of the actuator is consistent with the direction of the second joint coordinate system. The position transformation relationship between this coordinate system and the second joint coordinate system is expressed by the homogeneous transformation matrix T 32 Represented, where the transformation matrix is ​​defined as follows:

[0007]

[0008] where θ 1is the rotation angle of the first joint, θ 2 is the rotation angle of the second joint, x ij ,y ij ,z ij are the displacements of the ith coordinate system relative to the jth coordinate system in the x, y, and z directions, respectively.

[0009] End position P 3 The coordinates in the coordinate system {0} can be expressed as:

[0010] P=T 10 T 21 T 32 P 3

[0011] In the real-time path planning stage, we first read the odometer data to obtain the global coordinates P of the mobile base. 0 ,Pass

[0012] The coordinates of the origin of coordinate system {2} in the global coordinate system can be obtained by the following formula:

[0013]

[0014] 3) With the origin of the second joint coordinate system as the center and the length of the connecting rod as the radius, establish the end ball working space, which is expressed by the following equation C:

[0015] (xP 2,x ) 2 +(yP 2,y ) 2 +(zP 2,z ) 2 =r 2

[0016] End point P s To the end point P e The straight line constraint equation L is:

[0017]

[0018] 4) Solve the intersection of the ball workspace and the line constraint equation C∪L to obtain the path candidate point set. The solution equation is as follows:

[0019] When P s,x ≠P e,x When , the following system of equations is solved:

[0020]

[0021] in b yx =P s,y -P s,x k yx, b zx =P s,z -P s,x k zx

[0022] When P s,x =P e,x ,P s,y ≠P e,y When , the following system of equations is solved:

[0023]

[0024] In the above formula b zy =P s,z -P s,y k zy

[0025] When P s,x =P e,x , P s,y =P e,y , P s,z ≠P e,z When solving the following system of equations:

[0026]

[0027] 5) After obtaining the intersection of the constraint intersection space, the point set is used as the constraint space to map to the joint space of the end effector. At this time, the workspace path point set can be obtained by combining the constraint conditions. This point set is the coordinate of the end of the topping mechanism in the global coordinate system at the next moment. The method of mapping from the three-dimensional space global coordinate to the joint space is as follows:

[0028] The position of the end point P of the cotton topping actuator relative to the coordinate system {2} can be expressed by the following formula:

[0029]

[0030] Then the mapping relationship from Cartesian three-dimensional space to the actuator joint space can be expressed as:

[0031]

[0032] In the above formula, h=z,

[0033] After obtaining the joint space, in order to maximize the operating efficiency of the end actuator, the optimal path point is solved in the workspace path point set through the time and energy optimal constraints. The optimization equation is expressed as:

[0034]

[0035] In the above equation, θ 1 ',θ 2 ' is the angle value of the actuator at the current moment, S free To create a collision-free space for the actuator, the optimal solution of the above optimization equation can be solved to obtain the three-dimensional position of the end of the cotton topping mechanism at the next moment and the corresponding joint angle value. The joint value is sent to the motor through the communication protocol to drive the topping mechanism to the specified joint position, so as to maximize the operation efficiency. After completing a path point solution, the current coordinate position of the end is compared with the end point coordinate. If the end point is not reached, the robot coordinates are obtained from the odometer to solve the path point at the next moment until the end point of the end operation is reached.

[0036] Preferably, in step 5), the constraints include joint space constraints, collision-free space constraints, time constraints, and energy optimal constraints.

[0037] The present invention has the following advantages:

[0038] 1. The present invention is a real-time spatial path planning algorithm, which models the robot's end workspace as a spherical space and the robot's working path as a straight line from the starting point to the end point. The robot's end working spherical space is solved according to the current position of the robot's base and the robot model, and the global three-dimensional space path candidate point set of the robot's end mechanism at the next moment is solved by solving the spherical space and the working path straight line. Finally, the optimal path point coordinates are solved from the candidate point set through the time and energy optimal constraints. The above-mentioned workspace and path modeling method greatly reduces the amount of calculation when solving the path point, meeting the application requirements of the real-time system.

[0039] 2. The present invention is a real-time spatial path planning algorithm, which uses the position of the robot base and the robot mathematical model as input modeling workspace, uses the robot end workspace and the end position task as constraints, and finally outputs the path from the starting point to the end point of the robot end. Since the algorithm of the present invention fully considers the position of the mobile base, it can calculate the motion position of the end at the next moment according to the real-time position of the robot base fed back by the sensor, so it is suitable for robots with uniform and non-uniform motion and end actuators. BRIEF DESCRIPTION OF THE DRAWINGS

[0040] Figure 1 It is a schematic diagram of the process of the present invention;

[0041] Figure 2 It is a schematic diagram of the coordinate system construction of the present invention;

[0042] Figure 3 It is a schematic diagram of the position of the end point of the actuator of the present invention relative to the coordinate system. DETAILED DESCRIPTION

[0043] In order to make the purpose, technical solution and advantages of the embodiments of the present invention more clear, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the accompanying drawings in the embodiments of the present invention.

[0044] Embodiment, a real-time spatial path planning method, the method is used for an intelligent cotton topping device to plan the spatial position of a topping device in real time to achieve the function of cotton topping.

[0045] The cotton topping actuator is composed of two connecting rods and two motors. The connecting rod 1 connects the motor 1 and the motor 2, and the second connecting rod is connected to the rotation axis of the second motor, so that the rotation axes of the two motors are perpendicular to each other in space. When in use, multiple cotton topping actuators are installed on the mobile chassis and fixedly connected to the chassis, and remain relatively still with the chassis during the movement.

[0046] like Figure 1 As shown in , it includes the following steps. In this model, the mobile chassis is abstracted as a mobile mass point, the mobile mass point is the geometric center of the chassis, the motor is abstracted as a joint, and the coordinates of the chassis in space are represented by the coordinates of the mass point in the global coordinate system. Figure 2 As shown, the coordinate system of the first joint is a right-handed coordinate system with the rotation center as the z-axis, the y-axis and the movement center x-axis coinciding. The relative position of the chassis and the first joint is determined by the 4×4 homogeneous transformation matrix T 10 The matrix represents the rotation and translation relationship between the chassis mass coordinate system and the first joint coordinate system. The second joint coordinate system is a right-handed coordinate system with the second joint rotation center as the z-axis and the vertical downward as the y-axis. The relative position relationship between the first joint and the second joint coordinate system is represented by the homogeneous transformation matrix T 21 The end coordinate system is established at the end of the second link. The coordinate system at the end of the actuator is in the same direction as the second joint coordinate system. The position transformation relationship between this coordinate system and the second joint coordinate system is expressed by the homogeneous transformation matrix T 32 Represented by . The transformation matrix is ​​defined as follows:

[0047]

[0048] where θ 1 is the rotation angle of the first joint, θ 2 is the rotation angle of the second joint, x ij ,y ij ,z ij are the displacements of the ith coordinate system relative to the jth coordinate system in the x, y, and z directions, respectively.

[0049] End position P 3 The coordinates in the coordinate system {0} can be expressed as:

[0050] P=T 10 T 21 T 32 P 3

[0051] In the real-time path planning stage, we first read the odometer data to obtain the global coordinates P of the mobile base. 0 ,Pass

[0052] The coordinates of the origin of coordinate system {2} in the global coordinate system can be obtained by the following formula:

[0053]

[0054] Then, the end ball working space is established with the origin of the second joint coordinate system as the center and the length r of the second link as the radius, which is expressed by the following equation C:

[0055] (xP 2,x ) 2 +(yP 2,y ) 2 +(zP 2,z ) 2 =r 2

[0056] End point P s To the end point P e The straight line constraint equation L is:

[0057]

[0058] Solve the intersection of the ball workspace and the line constraint equation C∪L to obtain the path candidate point set. The solved equation is as follows:

[0059] When P s,x ≠P e,x When , the following system of equations is solved:

[0060]

[0061] in b yx =P s,y -P s,x k yx , b zx =P s,z -P s,x k zx

[0062] When P s,x =P e,x ,P s,y ≠P e,y When , the following system of equations is solved:

[0063]

[0064] In the above formula b zy =P s,z -P s,y k zy

[0065] When P s,x =P e,x , P s,y =P e,y , P s,z ≠P e,z When solving the following system of equations:

[0066]

[0067] After solving the intersection of the constraint intersection space, the point set is mapped as the constraint space to the joint space of the end effector. At this time, the workspace path point set is solved by combining the joint space constraint conditions and the collision-free space constraint conditions. This point set is the coordinate of the end of the topping mechanism in the global coordinate system at the next moment. The method of mapping from the three-dimensional space global coordinate to the joint space is as follows:

[0068] The position of the end point P of the cotton topping actuator relative to the coordinate system {2} can be expressed by the following formula:

[0069]

[0070] Then the mapping relationship from Cartesian three-dimensional space to the actuator joint space can be expressed as:

[0071]

[0072] In the above formula, h=z, After obtaining the joint space, in order to maximize the operating efficiency of the end actuator, the optimal path point is solved in the workspace path point set through time and energy optimal constraints. The optimization equation is expressed as:

[0073]

[0074] In the above equation, θ 1 ',θ 2 ' is the angle value of the actuator at the current moment, S freeThe actuator has no collision space. Solving the optimal solution of the above optimization equation can obtain the three-dimensional position of the end of the cotton topping mechanism at the next moment and the corresponding joint angle value, and send the joint value through the communication protocol to the motor to drive the topping mechanism to the specified joint position, so as to maximize the operation efficiency. After completing a path point solution, by comparing the current coordinate position of the end with the end point coordinate, if the end point has not been reached, the robot coordinates are obtained from the odometer to solve the path point at the next moment until the end point of the end operation is reached.

[0075] The working space of the robot end of the algorithm includes the working space accessible by the joint of the end actuator and the collision-free space with other actuators on the mobile base. When solving the end path, the space is divided into collision space and non-collision space with other mechanisms through the relationship between the end mechanisms and the relative installation position and the physical parameters of the end mechanisms. For the position in the collision-free space, the three-dimensional space position of the end actuator and the joint space are mapped through the mechanism inverse kinematics, and the three-dimensional space position is projected into the joint space. The end path that meets the constraint conditions is solved with the joint motion range as the constraint condition; the algorithm takes the real-time coordinates of the robot as one of the input conditions, and by modeling the end workspace as a spherical space, the path planning problem is modeled as the intersection of the start-end straight line and the spherical space, and the path is solved by combining the constraints of the joint space and the collision-free space and the time optimal and energy optimal principles. The solution process is simplified by modeling, and the calculation efficiency is improved. The solution time of the path algorithm is less than 5ms, so that the invented algorithm can be applied in the online real-time path planning system.

Claims

1. A real-time spatial path planning method, characterized in that: The following steps are included: 1) Model the mobile chassis as a moving mass point in the global coordinate system; 2) The first joint is established as a coordinate system. The coordinate system of the first joint is a right-handed coordinate system with the rotation center as the z-axis, the y-axis and the movement center x-axis coinciding. The relative position relationship between the chassis mass coordinate system and the first joint coordinate system is obtained through the homogeneous transformation matrix. The relative position of the chassis and the first joint is obtained through the 4×4 homogeneous transformation matrix T 10 Representation: The second joint is established as a coordinate system. The second joint coordinate system is a right-handed coordinate system with the second joint rotation center as the z-axis and the y-axis vertically downward. The relative position relationship between the first joint coordinate system and the second joint coordinate system is obtained by the homogeneous transformation matrix. The relative position relationship between the first joint and the second joint coordinate system is expressed by the homogeneous transformation matrix T 21 Indicates that the coordinates of the origin of the second joint coordinate system in the global coordinate system are obtained; the end coordinate system is established at the end of the second link, and the coordinate system of the end of the actuator is consistent with the direction of the second joint coordinate system. The position transformation relationship between this coordinate system and the second joint coordinate system is expressed by the homogeneous transformation matrix T 32 Represented, where the transformation matrix is ​​defined as follows: Where θ1 is the rotation angle of the first joint, θ2 is the rotation angle of the second joint, and x ij ,y ij ,z ij are the displacements of the ith coordinate system relative to the jth coordinate system in the x, y, and z directions, respectively. The coordinates of the end position P3 in the coordinate system {0} can be expressed as: P=T 10 T 21 T 32 P3 In the real-time path planning stage, the odometer data is first read to obtain the global coordinates P0 of the mobile base. The coordinates of the origin of the coordinate system {2} in the global coordinate system can be obtained by the following formula: 3) With the origin of the second joint coordinate system as the center and the length of the connecting rod as the radius, establish the end ball working space, which is expressed by the following equation C: (x-P 2,x ) 2 +(y-P 2,y ) 2 +(z-P 2,z ) 2 =r 2 End point P s To the end point P e The straight line constraint equation L is: 4) Solve the intersection of the ball workspace and the line constraint equation C∪L to obtain the path candidate point set. The solution equation is as follows: When P s,x ≠P e,x When , the following system of equations is solved: in b yx =P s,y -P s,x k yx , b zx =P s,z -P s,x k zx When P s,x =P e,x ,P s,y ≠P e,y When , the following system of equations is solved: In the above formula b zy =P s,z -P s,y k zy When P s,x =P e,x , P s,y =P e,y , P s,z ≠P e,z When solving the following system of equations: 5) After obtaining the intersection of the constraint intersection space, the point set is used as the constraint space to map to the joint space of the end effector. At this time, the workspace path point set can be obtained by combining the constraint conditions. This point set is the coordinate of the end of the topping mechanism in the global coordinate system at the next moment. The method of mapping from the three-dimensional space global coordinate to the joint space is as follows: The position of the end point P of the cotton topping actuator relative to the coordinate system {2} can be expressed by the following formula: Then the mapping relationship from Cartesian three-dimensional space to the actuator joint space can be expressed as: In the above formula, h=z, After obtaining the joint space, in order to maximize the operating efficiency of the end actuator, the optimal path point is solved in the workspace path point set through time and energy optimal constraints. The optimization equation is expressed as: In the above equation, θ1' and θ2' are the angle values ​​of the actuator at the current moment, S free To create a collision-free space for the actuator, the optimal solution of the above optimization equation can be solved to obtain the three-dimensional position of the end of the cotton topping mechanism at the next moment and the corresponding joint angle value. The joint value is sent to the motor through the communication protocol to drive the topping mechanism to the specified joint position, so as to maximize the operation efficiency. After completing a path point solution, the current coordinate position of the end is compared with the end point coordinate. If the end point is not reached, the robot coordinates are obtained from the odometer to solve the path point at the next moment until the end point of the end operation is reached.

2. A real-time spatial path planning method according to claim 1, characterized in that: In step 5), the constraints include joint space constraints, collision-free space constraints, time constraints, and energy optimal constraints.

Images