Optimal trajectory planning method, system, medium and equipment for double-image guided robot

Abstract

The invention relates to the technical field of image guide robots, and discloses a dual-image guide robot optimal trajectory planning method and system, a medium and equipment, and the method comprises the steps: converting a task space constraint into a parameter space constraint through a path speed decoupling mode, and planning trajectory parameters in the parameter space constraint; based on the planned trajectory parameters, parameterizing a trajectory function through the double-image guided robot to obtain a synchronous time optimal trajectory of the double-image guided robot; wherein in the dual-image guided robot parameterized trajectory function, the parameterized trajectories of the left and right robots conform to a chained derivation rule for trajectory parameters, the parameterized trajectories of the left and right robots meet synchronous motion constraints, and the trajectory parameters of the right robot and the trajectory parameters of the left robot meet a geometrical relationship and conform to the chained derivation rule. According to the method, the trajectory planning process is simplified, the planning efficiency is improved, the time optimality of the planned trajectory can be ensured, and the dual-image guided robot trajectory motion within the shortest time under the constraint limitation is realized.

Description

Technical Field

[0001] The present invention relates to the technical field of image-guided robots, and in particular to an optimal trajectory planning method, system, medium and equipment for a dual-image-guided robot. Background Art

[0002] The statements in this section merely provide background information related to the present invention and do not necessarily constitute prior art.

[0003] Image-guided robots refer to robots used to assist image guidance. Through the collaboration of two image-guided robots, image acquisition or surgical procedures can be controlled more accurately, reducing damage to surrounding tissues and the risk of complications such as bleeding and infection. Dual image-guided robots can perform image guidance in surgical operations simultaneously, and can also play a role in different stages of the same operation, shortening the operation time and improving the efficiency of the operation. However, in the process of dual image-guided robot collaboration, how to reasonably plan the motion trajectory of the two image-guided robots so that the overall action is both efficient and precise is still a challenge that needs to be solved. Especially under the conditions of multi-target tasks and environmental constraints, how to reasonably allocate action tasks, avoid conflicts and optimize operation time has become the core issue in trajectory planning.

[0004] Traditional trajectory planning methods usually focus on the motion optimization of a single image-guided robot, while ignoring the collaboration and synchronization between dual image-guided robots, which often leads to inefficient time and even conflicts or operational errors in complex tasks. With the increasing demand for robots to perform complex tasks in the medical field, especially in scenarios that require high precision and efficient collaboration, how to simultaneously optimize the motion trajectory of dual image-guided robots and maximize the speed and efficiency of task completion has become an urgent problem to be solved. Summary of the invention

[0005] In order to solve the above problems, the present invention provides an optimal trajectory planning method, system, medium and equipment for a dual-image guided robot, innovatively constrains the trajectory of the dual-image guided robot in the task space, and considers the synchronous motion constraints under the condition of satisfying the task constraints, thereby converting the multi-dimensional trajectory planning problem under the dual constraints into a one-dimensional parameter planning problem, and substituting the parameter space planning result into the parameterized trajectory function of the dual-image guided robot to obtain the dual-image guided robot synchronous time optimal trajectory that satisfies the dual constraints of task space and synchronous motion, thereby simplifying the trajectory planning process, improving the planning efficiency, and being able to ensure the time optimality of the planned trajectory, thereby realizing the dual-image guided robot trajectory motion in the shortest time under the aforementioned constraints.

[0006] In order to achieve the above object, the present invention adopts the following technical solution:

[0007] A first aspect of the present invention provides a dual-image guided robot optimal trajectory planning method, comprising:

[0008] Obtaining the task space constraints of dual-image guided robots;

[0009] The task space constraint is converted into a parameter space constraint by a path-speed decoupling form, and trajectory parameters are planned in the parameter space constraint;

[0010] Based on the planned trajectory parameters, the dual-image guided robot parameterized trajectory function was used to obtain the optimal trajectory of the dual-image guided robot in synchronization time.

[0011] Among them, in the parameterized trajectory function of the dual-image guided robot, the parameterized trajectories of the left and right robots both conform to the chain differentiation rule for the trajectory parameters, the parameterized trajectories of the left and right robots satisfy the synchronous motion constraints, the trajectory parameters of the right robot and the trajectory parameters of the left robot satisfy the geometric relationship, and conform to the chain differentiation rule.

[0012] Furthermore, the step of planning trajectory parameters in the parameter space constraints includes:

[0013] The task space constraints of the left robot are converted into parameter space constraints to obtain the velocity boundary and acceleration boundary of the trajectory parameters of the left robot;

[0014] The task space constraints of the right robot are converted into parameter space constraints to obtain the velocity boundary and acceleration boundary of the trajectory parameters of the right robot;

[0015] Convert the velocity bounds and acceleration bounds of the right robot trajectory parameters into equivalent velocity bounds and acceleration bounds of the right robot trajectory parameters;

[0016] According to the dual-image guided robot trajectory parameter velocity boundary and acceleration boundary, the final parameter velocity and acceleration boundary are obtained;

[0017] Based on the final parameter velocity and acceleration boundaries, the parameter position and parameter velocity of a planning cycle are obtained through a nonlinear filter, and the parameter position and parameter velocity constitute the trajectory parameters; the nonlinear filter tracks the expected parameter position with the maximum tracking ability under the constraint of the parameter boundary, ensuring that at least one constraint boundary is touched at any time in the planning cycle.

[0018] Furthermore, the parameterized trajectory of the left robot is:

[0019] T l (t) = L(u(t))

[0020]

[0021] Among them, u is the trajectory parameter of the left robot; represents the first derivative of u; represents the second derivative of u; T l (t) is the parameterized trajectory of the left robot; L(u(t)) is the trajectory function of the left robot; L′(u) represents the first-order derivative of the trajectory function L(u(t)) with respect to the trajectory parameter u, and L″(u) represents the second-order derivative of the trajectory function L(u(t)) with respect to the trajectory parameter u.

[0022] Furthermore, the parameterized trajectory of the right robot is:

[0023] T r (t) = R(μ(t))

[0024]

[0025] Among them, μ is the trajectory parameter of the right robot; represents the first derivative of μ(t); represents the second-order derivative of μ(t); T r (t) is the parameterized trajectory of the right robot; R(μ(t)) is the trajectory function of the right robot; R′(μ) represents the first-order derivative of the trajectory function R(μ(t)) with respect to the trajectory parameter μ, and R″(μ) represents the second-order derivative of the trajectory function R(μ(t)) with respect to the trajectory parameter μ.

[0026] Furthermore, the synchronous motion constraint is:

[0027] μ(t)=f(u(t))

[0028]

[0029] Where f′(u(t)) represents f″(u) represents f is the geometric relationship function; u is the trajectory parameter of the left robot; μ is the trajectory parameter of the right robot; represents the first derivative of u; represents the second derivative of u; represents the first derivative of μ(t); represents the second derivative of μ(t).

[0030] Furthermore, the parameter space constraints transformed from the task space constraints of the left robot are:

[0031]

[0032] Among them, u is the trajectory parameter of the left robot; represents the first derivative of u; L(u(t)) is the trajectory function of the left robot; L′(u) represents the first derivative of the trajectory function L(u(t)) with respect to the trajectory parameter u, and L″(u) represents the second derivative of the trajectory function L(u(t)) with respect to the trajectory parameter u; represents the boundary of the left robot parameter velocity, is the upper bound of the parameter speed, and the lower bound of the parameter speed Set to zero, is the velocity constraint boundary of the left robot’s task space, and They represent the upper and lower bounds of the task space velocity constraint of the left robot respectively; represents the bounds of the left robot parameter acceleration, and They represent the upper and lower bounds of the left robot parameter acceleration, is the acceleration constraint boundary of the left robot task space, and They represent the upper and lower bounds of the task space acceleration constraint of the left robot respectively.

[0033] Furthermore, the calculation formula of the parameter space constraint obtained by transforming the task space constraint of the right robot is as follows:

[0034]

[0035] Among them, μ is the trajectory parameter of the right robot; represents the first derivative of μ; R(μ(t)) is the trajectory function of the right robot; R′(μ) represents the first derivative of the trajectory function R(μ(t)) with respect to the trajectory parameter μ, and R″(μ) represents the second derivative of the trajectory function R(μ(t)) with respect to the trajectory parameter μ; represents the boundary of the right robot parameter velocity, is the upper bound of the parameter speed, and the lower bound of the parameter speed Set to zero, is the task space velocity constraint boundary of the right robot, and They represent the upper and lower bounds of the task space velocity constraint of the right robot respectively; represents the bounds of the right robot parameter acceleration, and They represent the upper and lower bounds of the right robot parameter acceleration, is the acceleration constraint boundary of the right robot task space, and They represent the upper and lower bounds of the task space acceleration constraint of the right robot respectively.

[0036] A second aspect of the present invention provides a dual-image guided robot optimal trajectory planning system, comprising:

[0037] A data acquisition module is configured to: acquire task space constraints of a dual-image guided robot;

[0038] A constraint conversion module is configured to: convert the task space constraint into a parameter space constraint in a path-speed decoupling form, and plan trajectory parameters in the parameter space constraint;

[0039] A trajectory planning module is configured to: obtain a synchronous time optimal trajectory of the dual-image guided robot by parameterizing a trajectory function of the dual-image guided robot based on the planned trajectory parameters;

[0040] Among them, in the parameterized trajectory function of the dual-image guided robot, the parameterized trajectories of the left and right robots both conform to the chain differentiation rule for the trajectory parameters, the parameterized trajectories of the left and right robots satisfy the synchronous motion constraints, the trajectory parameters of the right robot and the trajectory parameters of the left robot satisfy the geometric relationship, and conform to the chain differentiation rule.

[0041] The third aspect of the present invention provides a computer-readable storage medium having a computer program stored thereon, which is executed by a processor. When the program is executed by the processor, the steps in the optimal trajectory planning method for a dual-image guided robot as described above are implemented.

[0042] The fourth aspect of the present invention provides a computer device, including a memory, a processor, and a computer program stored in the memory and running on the processor, wherein when the processor executes the program, the steps in the optimal trajectory planning method for a dual-image guided robot as described above are implemented.

[0043] Compared with the prior art, the present invention has the following beneficial effects:

[0044] The present invention provides an optimal trajectory planning method for a dual-image-guided robot, which innovatively constrains the trajectory of the dual-image-guided robot in the task space, and considers the synchronous motion constraints under the condition of satisfying the task constraints, thereby converting the multi-dimensional trajectory planning problem under the dual constraints into a one-dimensional parameter planning problem, and substituting the parameter space planning result into the parameterized trajectory function of the dual-image-guided robot to obtain the dual-image-guided robot synchronous time optimal trajectory that satisfies the dual constraints of the task space and the synchronous motion, thereby simplifying the trajectory planning process, improving the planning efficiency, and being able to ensure the time optimality of the planned trajectory, thereby realizing the dual-image-guided robot trajectory motion in the shortest time under the aforementioned constraints, and further improving the safety and efficiency of the operation.

[0045] The present invention provides an optimal trajectory planning method for a dual-image-guided robot, which allows the trajectory of the dual-image-guided robot to be constrained in the task space. Compared with the joint space, the constraints in the task space are more intuitive and simple, ensuring that the left and right robots can work together during the operation to achieve seamless docking and precise coordination.

[0046] The present invention provides a dual-image guided robot optimal trajectory planning method, whose nonlinear filter will track the desired parameter position with the maximum tracking ability while satisfying the task constraints and synchronous motion constraints, ensuring that at least one constraint boundary is triggered at any time in the planning process, thereby verifying the time optimality of the planned trajectory and further improving the safety and efficiency of the operation. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] The accompanying drawings, which constitute a part of the specification of the present invention, are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention, but do not constitute limitations of the present invention.

[0048] Figure 1 This is a flow chart of an optimal trajectory planning method for a dual-image guided robot according to Embodiment 1 of the present invention;

[0049] Figure 2 This is a principle block diagram of a dual-image guided robot optimal trajectory planning method according to Embodiment 1 of the present invention;

[0050] Figure 3 is a trajectory planning curve diagram of the left robot in the task space according to the first embodiment of the present invention;

[0051] Figure 4 This is a trajectory planning curve diagram of the right robot in the task space according to the first embodiment of the present invention. DETAILED DESCRIPTION

[0052] The present invention will be further described below in conjunction with the accompanying drawings and embodiments.

[0053] It should be noted that the following detailed descriptions are exemplary and are intended to provide further explanation of the present invention. Unless otherwise specified, all technical and scientific terms used herein have the same meanings as those commonly understood by those skilled in the art to which the present invention belongs.

[0054] In the absence of conflict, the embodiments of the present invention and the features in the embodiments may be combined with each other. The present invention is further described below with reference to the accompanying drawings and embodiments.

[0055] Embodiment 1

[0056] The purpose of this embodiment is to provide an optimal trajectory planning method for a dual-image guided robot.

[0057] Since the motion trajectory of the dual-image-guided robot is multi-dimensional, it is not conducive to trajectory planning. This embodiment provides an optimal trajectory planning method for a dual-image-guided robot. The motion trajectory of the dual-image-guided robot is designed to be parameterized, and then the multi-dimensional task space trajectory planning problem is converted into a one-dimensional parameter space trajectory planning problem through path speed decoupling. The parameter space planning result is substituted into the parameterized trajectory function of the dual-image-guided robot to obtain the optimal synchronous time trajectory of the dual-image-guided robot that meets the dual constraints of task space and synchronous motion, thereby further improving the safety and efficiency of the operation.

[0058] This embodiment provides a dual-image guided robot optimal trajectory planning method, such as Figure 1 and Figure 2 As shown, including:

[0059] S1. Design the motion trajectory of the dual-image guided robot in a parameterized form; the dual-image guided robot in this embodiment includes a left robot and a right robot.

[0060] S2, using dual images to guide the robot's parametric motion trajectory, converting the given task space constraints into parameter space constraints through path-velocity decoupling, and planning trajectory parameters in the parameter space constraints;

[0061] S3. Substitute the parameter space planning result (trajectory parameter planning result) into the parameterized trajectory function of the dual-image guided robot to obtain the optimal trajectory of the dual-image guided robot synchronization time that meets the dual constraints of task space and synchronous motion.

[0062] S1 specifically includes:

[0063] S1-1. Design the parametric trajectory of the left robot. The parametric trajectory of the left robot complies with the chain derivation rule for parameters.

[0064] S1-2, design the parameterized trajectory of the right robot, the parameterized trajectory of the right robot also conforms to the chain derivation rule for parameters;

[0065] S1-3. Design the motion trajectories of the left and right robots to satisfy the synchronous motion constraint. The trajectory parameters μ of the right robot and the trajectory parameters u of the left robot satisfy a certain geometric relationship f and conform to the chain derivation rule.

[0066] The calculation formula for the parameterized trajectory of the left robot of S1-1 is as follows:

[0067] T l (t) = L(u(t))

[0068]

[0069] Among them, u is the trajectory parameter of the left robot, which is related to the time t; represents the first derivative of u; represents the second derivative of u; T l (t) is the motion trajectory of the left robot in the task space, which is also related to time t;

[0070] L(u(t)) is the trajectory function of the left robot; the trajectory parameter u is the variable of the function, and the trajectory function conforms to the chain derivative rule for the variable u. L′(u) represents the first-order derivative of the trajectory function L(u(t)) with respect to the trajectory parameter u, that is, L″(u) represents the second-order derivative of the trajectory function L(u(t)) with respect to the trajectory parameter u, that is

[0071] The calculation formula for the parameterized trajectory of the right robot of S1-2 is as follows:

[0072] T r (t) = R(μ(t))

[0073]

[0074] Among them, μ is the trajectory parameter of the right robot, which is related to time t; represents the first derivative of μ(t); represents the second-order derivative of μ(t); T r (t) is the motion trajectory of the right robot in the task space, which is also related to time t;

[0075] R(μ(t)) is the trajectory function of the right robot; the trajectory parameter μ is the variable of the function. The trajectory function conforms to the chain derivative rule for the variable μ. R′(μ) represents the first-order derivative of the trajectory function R(μ(t)) with respect to the trajectory parameter μ, that is, R″(μ) represents the second-order derivative of the trajectory function R(μ(t)) with respect to the trajectory parameter μ, that is

[0076] The calculation formula for the synchronous motion constraint of S1-3 is as follows:

[0077] μ(t)=f(u(t))

[0078]

[0079] Where f′(u(t)) represents f″(u) represents That is, the first and second derivatives of the geometric relationship function f with respect to the left robot trajectory parameter u.

[0080] S2 specifically includes:

[0081] S2-1, transform the given task space constraints of the left robot into parameter space constraints;

[0082] S2-2, converting the given task space constraints of the right robot into parameter space constraints;

[0083] S2-3, converting the velocity boundary and acceleration boundary of the trajectory parameter of the right robot into the velocity boundary and acceleration boundary of the trajectory parameter of the equivalent right robot;

[0084] S2-4, obtaining the final parameter velocity and acceleration boundaries of the robot according to the obtained dual-image guided parameter velocity and acceleration boundaries;

[0085] S2-5. The final parameter velocity and acceleration boundary are sent into a nonlinear filter to obtain the parameter position and parameter velocity of this round of planning cycle.

[0086] The calculation formula of parameter space constraint of S2-1 obtained by transforming the task space constraint of the left robot is as follows:

[0087]

[0088] Among them, in the formula for obtaining the parameter speed, set represents the boundary of the left robot parameter velocity, is the upper bound of the desired parameter speed, and the lower bound of the parameter speed Set to zero, is the task space velocity constraint boundary of the left robot, set and They represent the upper and lower bounds of the task space velocity constraint of the left robot respectively.

[0089] Among them, in the formula for obtaining parameter acceleration, Indicates the boundary of the left robot parameter acceleration, set and They represent the upper and lower bounds of the required left robot parameter acceleration, is the acceleration constraint boundary of the left robot task space, set and They represent the upper and lower bounds of the task space acceleration constraint of the left robot respectively.

[0090] Among them, u is the trajectory parameter of the left robot; represents the first derivative of u.

[0091] For obtaining the upper bound of parameter speed For example, and The value of depends on the positive and negative of L′(u) and L″(u). When L′(u)>0, On the contrary, when L′(u)<0, The extreme case is when L′(u) = 0, the upper bound of the parameter velocity depends on When L″(u)>0, On the contrary, when L″(u)<0, A more extreme case is if L″(u)=0, then

[0092] For obtaining the parameter acceleration boundary For example, the parameter acceleration boundary is divided into the parameter acceleration upper bound and lower bound Determining the upper and lower bounds of the parameter acceleration depends on The value of, further, The value of is closely related to L′(u). When L′(u)>0, find the upper bound of the acceleration parameter. Find the lower bound of the parameter acceleration, On the contrary, when L′(u)<0, find the upper bound of the parameter acceleration, Find the lower bound of the parameter acceleration, The extreme case is if L′(u)=0, then

[0093] The calculation formula of parameter space constraint of S2-2 obtained by transforming the task space constraint of the right robot is as follows:

[0094]

[0095] Among them, in the formula for obtaining the parameter speed, set represents the boundary of the right robot parameter velocity, is the upper bound of the desired parameter speed, and the lower bound of the parameter speed Set to zero, is the task space velocity constraint boundary of the right robot, set and They represent the upper and lower bounds of the task space velocity constraint of the right robot respectively.

[0096] Among them, in the formula for obtaining parameter acceleration, Indicates the boundary of the right robot parameter acceleration, set and They represent the upper and lower bounds of the required right robot parameter acceleration, is the acceleration constraint boundary of the right robot task space, set and They represent the upper and lower bounds of the task space acceleration constraint of the right robot respectively.

[0097] Among them, μ is the trajectory parameter of the right robot; represents the first derivative of μ.

[0098] For obtaining the upper bound of parameter speed For example, and The value of depends on the positive and negative of R′(μ) and R″(μ). When R′(μ)>0, On the contrary, when R′(μ)<0, In the extreme case, when R′(μ)=0, the upper bound of the speed parameter is determined by When R″(μ)>0, On the contrary, when R″(μ)<0, The extreme case is if R″(μ)=0, then

[0099] For obtaining the parameter acceleration boundary For example, the parameter acceleration boundary is divided into the parameter acceleration upper bound and the lower bound Determining the upper and lower bounds of the parameter acceleration depends on The value of, further, The value of is closely related to R′(μ). When R′(μ)>0, find the upper bound of the parameter acceleration. Find the lower bound of the parameter acceleration, On the contrary, when R′(μ)<0, find the upper bound of the parameter acceleration, Find the lower bound of the parameter acceleration, The extreme case is if R′(μ)=0, then

[0100] The calculation formulas for the velocity boundary and acceleration boundary of the equivalent right robot trajectory parameters of S2-3 are as follows:

[0101]

[0102] Among them, u n Indicates the parameter position at the current moment, Represents the parameter speed at the current moment, so it is equivalent to the right robot trajectory parameter speed boundary The value of R′(f(u n )), R″(f(u n )) and f′(u n ) at the current moment, equivalent to the right robot trajectory parameter acceleration boundary The value of R′(f(u n )) and f′(u n ) at the current moment needs to be analyzed specifically based on the specific design solution.

[0103] The final parameter velocity and acceleration boundaries of S2-4 are calculated as follows:

[0104]

[0105] in, and represents the final upper and lower bounds of the parameter velocity, and Indicates the final upper and lower bounds of the parameter acceleration.

[0106] The calculation formula of the nonlinear filter of S2-5 is as follows:

[0107]

[0108] Among them, u n and They represent the parameter position and parameter speed of this round of planning cycle respectively, represents the parameter speed of the previous planning cycle, T represents the planning cycle, Represents the parameter acceleration returned by the nonlinear filter in this round of planning cycle. The final parameter velocity and acceleration boundaries are sent to a nonlinear filter. The nonlinear filter will track the desired parameter position with the maximum tracking ability under the constraint of the parameter boundary, thereby ensuring that at least one constraint boundary is touched at any time in the planning time, thereby verifying the time optimality of trajectory planning.

[0109] The calculation formula for the optimal trajectory of the S3 dual-image guided robot synchronization time is as follows:

[0110] T l (t) = L(u n (t))

[0111] T r (t) = R(f(u n (t)))

[0112] Among them, the parameter position u obtained in this round of planning cycle n They are respectively substituted into the trajectory function of the dual-image guided robot to obtain the optimal motion trajectory of the dual-image guided robot in synchronization time that meets the task space and synchronous motion constraints.

[0113] The present embodiment provides an optimal trajectory planning method for a dual-image guided robot. Aiming at the dual-image guided robot trajectory planning problem, the dual-image guided robot motion trajectory is innovatively constrained from the task space, and considering that the dual-image guided robot has high requirements for the coordinated motion of the two arms when performing specific tasks, an additional synchronous motion constraint of the dual-image guided robot terminal is imposed. First, the special parameterized trajectory design of the dual-image guided robot allows the multi-dimensional motion trajectory planning problem to be converted into a one-dimensional trajectory parameter planning problem by decoupling the path speed, which simplifies the trajectory planning process and improves the efficiency of trajectory planning; secondly, the synchronous motion constraint of the dual-image guided robot in the task space can still be mapped in the parameter space through this process, and the planned trajectory parameters in the parameter space still meet the dual constraints. The nonlinear filter will track the expected parameter position with the maximum tracking ability while satisfying the task space and synchronous motion constraints, thereby ensuring that at least one constraint boundary is triggered at any time in the planning process, thereby verifying the time optimality of the planned trajectory.

[0114] According to Table 1, the task space constraint boundary of the dual image guided robot is set, and the optimal trajectory of the dual image guided robot synchronization time is planned using the method of this embodiment. The results are as follows: Figure 3 and Figure 4 As shown, it can be seen that during the time from 0 to 0.36 s, the angular acceleration boundary of the dual-image guided robot in the task space is touched; during the time from 0.36 to 1.61 s, the lower limit of the linear velocity of the left robot is touched; during the time from 1.61 to 2.41 s, the angular velocity boundary of the dual-image guided robot is touched; during the time from 2.41 to 2.87 s, the angular acceleration boundary of the dual-image guided robot in the task space is touched again. Figure 3 and Figure 4 The results shown prove that at any time during the planning process at least one constraint is hit, demonstrating the time optimality of the planning trajectory.

[0115] Table 1. Dual-image guided robot task space constraint boundaries

[0116]

[0117]

[0118] Table 2. Experimental results

[0119]

[0120] In this embodiment, the motion trajectory of the dual-image-guided robot is designed in a parameterized form; the given task space constraints are converted into parameter space constraints by using the parameterized motion trajectory of the dual-image-guided robot through the path-speed decoupling form; the optimal synchronous time trajectory of the dual-image-guided robot that satisfies the dual constraints of task space and synchronous motion is obtained by using the parameter space planning result; the dual-image-guided robot trajectory is innovatively constrained in the task space, and the synchronous motion constraints are considered under the condition of satisfying the task constraints, and the multi-dimensional trajectory planning problem under the dual constraints is converted into a one-dimensional parameter planning problem, which simplifies the trajectory planning process, improves the planning efficiency, and can ensure the time optimality of the planned trajectory, thereby realizing the dual-image-guided robot trajectory motion in the shortest time under the aforementioned constraints, which helps to improve the safety and efficiency of the operation.

[0121] Embodiment 2

[0122] The purpose of the second embodiment is to provide a dual-image guided robot optimal trajectory planning system, including:

[0123] A data acquisition module is configured to: acquire task space constraints of a dual-image guided robot;

[0124] A constraint conversion module is configured to: convert the task space constraint into a parameter space constraint in a path-speed decoupling form, and plan trajectory parameters in the parameter space constraint;

[0125] A trajectory planning module is configured to: obtain a synchronous time optimal trajectory of the dual-image guided robot by parameterizing a trajectory function of the dual-image guided robot based on the planned trajectory parameters;

[0126] Among them, in the parameterized trajectory function of the dual-image guided robot, the parameterized trajectories of the left and right robots both conform to the chain differentiation rule for the trajectory parameters, the parameterized trajectories of the left and right robots satisfy the synchronous motion constraints, the trajectory parameters of the right robot and the trajectory parameters of the left robot satisfy the geometric relationship, and conform to the chain differentiation rule.

[0127] It should be noted here that the various modules in this embodiment correspond one-to-one to the various steps in Example 1, and the specific implementation process is the same, which will not be repeated here.

[0128] Embodiment 3

[0129] This embodiment provides a computer-readable storage medium on which a computer program is stored. The program is executed by a processor. When the program is executed by the processor, the steps in the dual-image guided robot optimal trajectory planning method as described in the above embodiment 1 are implemented.

[0130] Embodiment 4

[0131] This embodiment provides a computer device, including a memory, a processor, and a computer program stored in the memory and running on the processor. When the processor executes the program, the steps in the dual-image guided robot optimal trajectory planning method as described in the above embodiment 1 are implemented.

[0132] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

[0133] Although the above describes the specific implementation mode of the present invention in conjunction with the accompanying drawings, it is not intended to limit the scope of protection of the present invention. Those skilled in the art should understand that various modifications or variations that can be made by those skilled in the art on the basis of the technical solution of the present invention without creative work are still within the scope of protection of the present invention.

Claims

1. A dual-image guided robot optimal trajectory planning method, characterized in that: include: Obtaining the task space constraints of dual-image guided robots; The task space constraint is converted into a parameter space constraint by a path-speed decoupling form, and trajectory parameters are planned in the parameter space constraint; Based on the planned trajectory parameters, the dual-image guided robot parameterized trajectory function was used to obtain the optimal trajectory of the dual-image guided robot in synchronization time. Among them, in the parameterized trajectory function of the dual-image guided robot, the parameterized trajectories of the left and right robots both conform to the chain differentiation rule for the trajectory parameters, the parameterized trajectories of the left and right robots satisfy the synchronous motion constraints, the trajectory parameters of the right robot and the trajectory parameters of the left robot satisfy the geometric relationship, and conform to the chain differentiation rule.

2. The method for optimal trajectory planning of a dual-image guided robot as claimed in claim 1, characterized in that: The step of planning trajectory parameters in parameter space constraints includes: The task space constraints of the left robot are converted into parameter space constraints to obtain the velocity boundary and acceleration boundary of the trajectory parameters of the left robot; The task space constraints of the right robot are converted into parameter space constraints to obtain the velocity boundary and acceleration boundary of the trajectory parameters of the right robot; Convert the velocity bounds and acceleration bounds of the right robot trajectory parameters into equivalent velocity bounds and acceleration bounds of the right robot trajectory parameters; According to the dual-image guided robot trajectory parameter velocity boundary and acceleration boundary, the final parameter velocity and acceleration boundary are obtained; Based on the final parameter velocity and acceleration boundaries, the parameter position and parameter velocity of a planning cycle are obtained through a nonlinear filter, and the parameter position and parameter velocity constitute the trajectory parameters; the nonlinear filter tracks the expected parameter position with the maximum tracking ability under the constraint of the parameter boundary, ensuring that at least one constraint boundary is touched at any time in the planning cycle.

3. The method for optimal trajectory planning of a dual-image guided robot as claimed in claim 1, characterized in that: The parameterized trajectory of the left robot is: T l (t)=L(u(t)) Among them, u is the trajectory parameter of the left robot; represents the first derivative of u; represents the second derivative of u; T l (t) is the parameterized trajectory of the left robot; L(u(t)) is the trajectory function of the left robot; L′(u) represents the first-order derivative of the trajectory function L(u(t)) with respect to the trajectory parameter u, and L″(u) represents the second-order derivative of the trajectory function L(u(t)) with respect to the trajectory parameter u.

4. The method for optimal trajectory planning of a dual-image guided robot as claimed in claim 1, characterized in that: The parameterized trajectory of the right robot is: T r (t)=R(μ(t)) Among them, μ is the trajectory parameter of the right robot; represents the first derivative of μ(t); represents the second-order derivative of μ(t); T r (t) is the parameterized trajectory of the right robot; R(μ(t)) is the trajectory function of the right robot; R′(μ) represents the first-order derivative of the trajectory function R(μ(t)) with respect to the trajectory parameter μ, and R″(μ) represents the second-order derivative of the trajectory function R(μ(t)) with respect to the trajectory parameter μ.

5. The method for optimal trajectory planning of a dual-image guided robot as claimed in claim 1, characterized in that: The synchronous motion constraints are: μ(t)=f(u(t)) Where f′(u(t)) represents f″(u) represents f is the geometric relationship function; u is the trajectory parameter of the left robot; μ is the trajectory parameter of the right robot; represents the first derivative of u; represents the second derivative of u; represents the first derivative of μ(t); represents the second derivative of μ(t).

6. The method for optimal trajectory planning of a dual-image guided robot as claimed in claim 1, characterized in that: The parameter space constraints transformed from the task space constraints of the left robot are: Among them, u is the trajectory parameter of the left robot; represents the first derivative of u; L(u(t)) is the trajectory function of the left robot; L′(u) represents the first derivative of the trajectory function L(u(t)) with respect to the trajectory parameter u, and L″(u) represents the second derivative of the trajectory function L(u(t)) with respect to the trajectory parameter u; represents the boundary of the left robot parameter velocity, is the upper bound of the parameter speed, and the lower bound of the parameter speed Set to zero, is the velocity constraint boundary of the left robot’s task space, and They represent the upper and lower bounds of the task space velocity constraint of the left robot respectively; represents the bounds of the left robot parameter acceleration, and They represent the upper and lower bounds of the left robot parameter acceleration, is the acceleration constraint boundary of the left robot task space, and They represent the upper and lower bounds of the task space acceleration constraint of the left robot respectively.

7. The method for optimal trajectory planning of a dual-image guided robot as claimed in claim 1, characterized in that: The calculation formula of the parameter space constraint obtained by transforming the task space constraint of the right robot is as follows: Among them, μ is the trajectory parameter of the right robot; represents the first derivative of μ; R(μ(t)) is the trajectory function of the right robot; R′(μ) represents the first derivative of the trajectory function R(μ(t)) with respect to the trajectory parameter μ, and R″(μ) represents the second derivative of the trajectory function R(μ(t)) with respect to the trajectory parameter μ; represents the boundary of the right robot parameter velocity, is the upper bound of the parameter speed, and the lower bound of the parameter speed Set to zero, is the task space velocity constraint boundary of the right robot, and They represent the upper and lower bounds of the task space velocity constraint of the right robot respectively; represents the bounds of the right robot parameter acceleration, and They represent the upper and lower bounds of the right robot parameter acceleration, is the acceleration constraint boundary of the right robot task space, and They represent the upper and lower bounds of the task space acceleration constraint of the right robot respectively.

8. A dual-image guided robot optimal trajectory planning system, characterized in that: include: A data acquisition module is configured to: acquire task space constraints of a dual-image guided robot; A constraint conversion module is configured to: convert the task space constraint into a parameter space constraint in a path-speed decoupling form, and plan trajectory parameters in the parameter space constraint; A trajectory planning module is configured to: obtain a synchronous time optimal trajectory of the dual-image guided robot by parameterizing a trajectory function of the dual-image guided robot based on the planned trajectory parameters; Among them, in the parameterized trajectory function of the dual-image guided robot, the parameterized trajectories of the left and right robots both conform to the chain differentiation rule for the trajectory parameters, the parameterized trajectories of the left and right robots satisfy the synchronous motion constraints, the trajectory parameters of the right robot and the trajectory parameters of the left robot satisfy the geometric relationship, and conform to the chain differentiation rule.

9. A computer-readable storage medium having a computer program stored thereon, the program being executed by a processor, characterized in that: When the program is executed by a processor, the steps in a dual-image guided robot optimal trajectory planning method as described in any one of claims 1-7 are implemented.

10. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the program, the steps in the optimal trajectory planning method for a dual-image guided robot are implemented as described in any one of claims 1-7.

Images