A dynamic target tracking method, device and robot

Abstract

The present application provides a dynamic target tracking method, device and robot, the method comprising: obtaining the motion state of the tracked dynamic target in real time; performing motion prediction according to the motion state at the current moment to obtain the predicted position of the dynamic target; performing hysteresis compensation on the predicted position to obtain the compensated predicted position; performing trajectory online planning according to the compensated predicted position to obtain the planning amount of the multi-step joint motion state at the future moment, and determining the multi-step optimization trajectory in combination with the multi-step optimization objective function; controlling the robot joints to perform corresponding movements according to the multi-step optimization trajectory. The method can effectively reduce the hysteresis problem of the robot when tracking dynamic targets, etc.

Description

Technical Field

[0001] The present application relates to the technical field of robot motion planning, and in particular to a dynamic target tracking method, device and robot. Background Art

[0002] When a robot tracks a dynamic target, it uses online trajectory planning. Currently, the online planner is driven to generate online trajectories based on the error information of the tracked target. However, when tracking a dynamic target, the visual system installed on the robot has a certain delay in acquisition and calculation, that is, there is a low visual feedback frequency, which makes the online planning trajectory have a significant lag error relative to the dynamic target. Summary of the invention

[0003] In view of this, the embodiments of the present application provide a dynamic target tracking method, device and robot, which can effectively reduce the lag problem of the robot when tracking a dynamic target.

[0004] In a first aspect, an embodiment of the present application provides a dynamic target tracking method, comprising:

[0005] Obtain the motion status of the tracked dynamic target in real time;

[0006] Performing motion prediction according to the motion state at the current moment to obtain a predicted position of the dynamic target;

[0007] Performing hysteresis compensation on the predicted position to obtain a compensated predicted position;

[0008] Performing online trajectory planning according to the compensated predicted position to obtain the planning quantity of the multi-step joint motion state at the future moment, and determining the multi-step optimization trajectory in combination with the multi-step optimization objective function;

[0009] According to the multi-step optimized trajectory, the robot joints are controlled to perform corresponding movements.

[0010] In some embodiments, the motion state includes the actual position, actual speed and actual acceleration of the dynamic target at the current moment, and performing motion prediction according to the motion state at the current moment to obtain the predicted position of the dynamic target includes:

[0011] The movement of the dynamic target within a preset short time is regarded as a uniformly accelerated movement, so as to respectively calculate a first displacement generated by the dynamic target moving at the actual speed for the preset short time and a second displacement generated by the dynamic target moving at the actual acceleration for the preset short time;

[0012] The predicted position of the dynamic target is calculated based on the actual position of the dynamic target at the current moment, the first displacement, and the second displacement.

[0013] In some embodiments, the performing hysteresis compensation on the predicted position includes:

[0014] According to the planned motion position at the current moment and the actual position, the lag error corresponding to the current moment is calculated, and the lag error at the current moment and the lag error at the previous moment are accumulated to obtain the cumulative lag error; the cumulative lag error is superimposed on the predicted position.

[0015] In some embodiments, performing trajectory online planning based on the compensated predicted position to obtain a multi-step joint motion state planning quantity at a future moment includes:

[0016] Based on the compensated predicted position, the joint motion state quantity at the current moment is calculated;

[0017] According to the joint motion state quantity at the current moment and the trajectory prediction model constructed based on the relationship between the state quantity at the current moment and the state quantity at the next moment, the planning quantity of the multi-step joint motion state for trajectory planning at future continuous moments is calculated.

[0018] In some embodiments, the construction of the multi-step optimization objective function includes:

[0019] A multi-step optimization objective function is constructed with the first goal of bringing the multi-step position and multi-step velocity of the robot joint back to the origin and the second goal of minimizing the multi-step position error when tracking a dynamic target, wherein the constraints of the multi-step optimization objective function include the position, velocity, acceleration of the robot joint, and related constraints satisfied by the position error.

[0020] In some embodiments, the expression of the multi-step optimization objective function is as follows:

[0021]

[0022] Where P k is the position matrix composed of k-step position planning quantities, V k is the speed matrix composed of k-step speed planning quantities, A k is the acceleration matrix composed of k-step acceleration planning quantities, It is composed of the predicted position after k-step compensation; ω1, ω2 and ω3 are three preset weights respectively.

[0023] In some embodiments, the calculation formula for the predicted position of the dynamic target is as follows:

[0024]

[0025] In the formula, p prois the predicted position of the dynamic target, p fb 、v fb and a fb are respectively the actual position, actual speed and actual acceleration of the dynamic target at the current moment, and dt represents a preset short time.

[0026] In a second aspect, an embodiment of the present application further provides a dynamic target tracking device, comprising:

[0027] An acquisition module is used to acquire the motion state of the tracked dynamic target in real time;

[0028] A motion prediction module, used to perform motion prediction according to the motion state at the current moment to obtain the predicted position of the dynamic target;

[0029] A spatial compensation module, used for performing hysteresis compensation on the predicted position to obtain a compensated predicted position;

[0030] A trajectory planning module is used to perform online trajectory planning according to the compensated predicted position, obtain the planning quantity of the multi-step joint motion state at the future moment, and determine the multi-step optimization trajectory in combination with the multi-step optimization objective function;

[0031] The joint control module is used to control the robot joints to perform corresponding movements according to the multi-step optimized trajectory.

[0032] In a third aspect, an embodiment of the present application further provides a robot, comprising a processor and a memory, wherein the memory stores a computer program, and the processor is used to execute the computer program to implement the above-mentioned dynamic target tracking method.

[0033] In a fourth aspect, an embodiment of the present application further provides a readable storage medium storing a computer program, which, when executed on a processor, implements the above-mentioned control method for a robot to track a dynamic target.

[0034] The embodiments of the present application have the following beneficial effects:

[0035] The dynamic target tracking method proposed in this application obtains the motion state of the tracked dynamic target in real time, and predicts the motion of the dynamic target in a short time to obtain the predicted position of the dynamic target; then uses the generated hysteresis to compensate the predicted position to obtain the compensated predicted position; then uses the model prediction idea to perform online trajectory planning based on the compensated predicted position to determine the multi-step joint optimization trajectory; finally, controls the joint motion of the robot based on the multi-step optimized trajectory to achieve real-time tracking of the dynamic target. This method predicts the motion of the dynamic target in time, then compensates for the hysteresis in space, and combines model prediction for online trajectory optimization, which can effectively reduce the hysteresis problem of the robot when tracking dynamic targets, and also obtain a smooth motion trajectory, thereby improving the stability of the robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings required for use in the embodiments will be briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present application and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without paying creative work.

[0037] Figure 1 A schematic diagram of the structure of a robot according to an embodiment of the present application is shown;

[0038] Figure 2 A flow chart of a dynamic target tracking method according to an embodiment of the present application is shown;

[0039] Figure 3 A flow chart showing motion prediction of a dynamic target tracking method according to an embodiment of the present application is shown;

[0040] Figure 4 A flow chart showing hysteresis compensation of a dynamic target tracking method according to an embodiment of the present application is shown;

[0041] Figure 5 A schematic structural diagram of a dynamic target tracking device according to an embodiment of the present application is shown. DETAILED DESCRIPTION

[0042] The technical solutions in the embodiments of the present application will be described clearly and completely below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all of the embodiments.

[0043] The components of the embodiments of the present application generally described and shown in the drawings herein may be arranged and designed in various configurations. Therefore, the following detailed description of the embodiments of the present application provided in the drawings is not intended to limit the scope of the application claimed for protection, but merely represents the selected embodiments of the present application. Based on the embodiments of the present application, all other embodiments obtained by those skilled in the art without making creative work belong to the scope of protection of the present application.

[0044] Hereinafter, the terms "including", "having" and their cognates that can be used in various embodiments of the present application are intended only to indicate specific features, numbers, steps, operations, elements, components or a combination of the foregoing items, and should not be understood as first excluding the existence of one or more other features, numbers, steps, operations, elements, components or a combination of the foregoing items or increasing the possibility of one or more features, numbers, steps, operations, elements, components or a combination of the foregoing items. In addition, the terms "first", "second", "third" and the like are only used to distinguish descriptions and cannot be understood as indicating or implying relative importance.

[0045] Unless otherwise defined, all terms (including technical terms and scientific terms) used herein have the same meanings as those generally understood by those skilled in the art to which the various embodiments of the present application belong. The terms (such as those defined in generally used dictionaries) will be interpreted as having the same meanings as the contextual meanings in the relevant technical field and will not be interpreted as having idealized meanings or overly formal meanings unless clearly defined in the various embodiments of the present application.

[0046] In conjunction with the accompanying drawings, some embodiments of the present application are described in detail below. In the absence of conflict, the following embodiments and features in the embodiments can be combined with each other.

[0047] Please refer to Figure 1 , which is a schematic diagram of the structure of the robot proposed in the embodiment of the present application. Exemplarily, the robot includes a memory 11, a processor 12 and a perception unit 13, wherein the memory 11 and the perception unit 13 are both connected to the processor 12, the memory 11 stores a corresponding computer program, and the processor 12 is used to execute the computer program to implement the dynamic target tracking method in the embodiment of the present application, which can effectively solve the lag problem of the robot in the prior art when performing dynamic target tracking.

[0048] The memory 11 may be, but is not limited to, a random access memory (RAM), a read only memory (ROM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), an electrically erasable read-only memory (EEPROM), etc. The memory 11 is used to store a computer program, and the processor 12 may execute the computer program accordingly after receiving an execution instruction.

[0049] Among them, the processor 12 can be an integrated circuit chip with signal processing capabilities. The processor 12 can be a general-purpose processor, including a central processing unit (CPU), a graphics processing unit (GPU) and a network processor (NP), a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field-programmable gate array (FPGA) or other programmable logic devices, discrete gates or transistor logic devices, and at least one of discrete hardware components. The general-purpose processor can be a microprocessor or the processor can also be any conventional processor, etc., which can implement or execute the disclosed methods, steps and logic block diagrams in the embodiments of the present application.

[0050] Among them, the perception unit 13 mainly refers to the sensors set on the robot, which are used to transmit necessary external information and self-state information to the robot's control system, thereby providing necessary conditions for the robot's decision-making. In some cases, these sensors can be divided into internal sensors and external sensors, wherein the internal sensors are functional elements used to measure the robot's own state, and their function is to measure kinematic quantities and mechanical quantities, which are used for the robot to perceive its own motion state, so that the robot can move according to the specified parameters such as position, trajectory and speed. The external sensors mainly perceive the mutual information between the robot itself and the environment, such as visual information, and specifically include but are not limited to proximity sensors, visual sensors, etc. For example, in this embodiment, the real-time position, speed, acceleration and other motion states of the dynamic target tracked by the robot are obtained through visual feedback. It should be understood that the sensors listed above can be selectively set according to the actual application scenario of the robot.

[0051] It can be understood that in this embodiment, the robot can be, but is not limited to, an industrial robot arm, a surgical robot arm, a mobile robot, etc. Taking a mobile robot as an example, it can be a wheeled robot with rolling wheels, such as an inspection robot, etc., or a footed robot with two feet, three feet or more, etc., that is, the robot here is a generalized robot, and its existence form is not limited. The method of this embodiment is mainly used in various application scenarios that require dynamic target tracking, for example, it can include but is not limited to, a mobile robot performing real-time following of a moving target, an industrial robot arm performing dynamic grinding operations, dynamic trajectory tracking operations, etc., or a surgical robot arm performing dynamic cutting operations, etc. It can be understood that as the object being tracked, the state of the tracked target in this embodiment is moving and changing. Moreover, the method of this embodiment can also be used in scenarios such as tracking dynamic targets such as vehicles.

[0052] Based on the above-mentioned robot structure, an embodiment of the present application proposes a dynamic target tracking method. This method predicts the temporal motion of the dynamic target, compensates for the spatial lag, and combines model prediction for online trajectory optimization. This can not only effectively reduce the lag problem of the robot when tracking dynamic targets, but also obtain a smooth motion trajectory, thereby improving system stability, etc.

[0053] Please refer to Figure 2 , exemplary, the dynamic target tracking method includes steps S110 to S150:

[0054] S110, acquiring the motion state of the tracked dynamic target in real time.

[0055] The motion state of the dynamic target may include, but is not limited to, the actual position and posture (i.e., position and posture) at each motion moment, the actual speed, the actual acceleration, the actual jerk, etc. For example, the robot can directly or indirectly calculate the motion state of the dynamic target through methods such as visual feedback and distance perception, and then track the dynamic target based on the motion state information. It can be understood that, ideally, the motion state of the robot should be consistent with the motion state of the dynamic target, so as to achieve real-time tracking.

[0056] Taking visual feedback as an example, when tracking a dynamic target, due to the low frequency of visual feedback obtained by the robot, the posture of the visual feedback often cannot be equal to the real-time posture of the dynamic target in time, resulting in a certain lag error in the online planning trajectory obtained based on the posture of visual feedback relative to the real dynamic target. For this reason, this embodiment proposes to first predict the position of the dynamic target, which can solve the time error caused by untimely visual feedback.

[0057] S120, performing motion prediction according to the current motion state to obtain a predicted position of the dynamic target.

[0058] Among them, motion prediction refers to treating the motion of a dynamic target in a short period of time as a uniformly accelerated motion, and then predicting its future motion position, that is, the predicted position mentioned above. Exemplarily, after obtaining the actual position of the dynamic target, the speed, acceleration and other state quantities of the dynamic target at the current moment can be obtained by taking a first derivative or a second derivative of the real-time position, or by using discrete calculations of the actual positions of several adjacent frames. Then, these state quantities are combined with the motion prediction principle to calculate the predicted position of the dynamic target at the next moment.

[0059] In one embodiment, Figure 3 As shown, step S120 includes the following sub-steps:

[0060] S210, the movement of the dynamic target in a preset short time is regarded as a uniformly accelerated movement, so as to respectively calculate a first displacement generated by the dynamic target moving at an actual speed for a preset short time, and a second displacement generated by the dynamic target moving at an actual acceleration for a preset short time. S220, the predicted position of the dynamic target is calculated based on the actual position of the dynamic target at the current moment, the first displacement, and the second displacement.

[0061] If described by an expression, the calculation formula for the predicted position of the dynamic target is as follows:

[0062]

[0063] In the formula, p pro is the predicted position of the dynamic target, p fb 、v fb and a fb are the actual position, actual speed and actual acceleration of the dynamic target at the current moment, respectively; dt represents a preset short time, for example, the short time may refer to a motion moment or a control instruction cycle, etc. It can be understood that in the above formula, the first item on the right side of the equal sign is the actual position at the current moment, and the second item and the third item are the first displacement and the second displacement generated in the preset short time, respectively.

[0064] Considering that if the predicted position is directly planned online, due to the robot's own structure or other factors, there will still be a certain error in the planned motion position relative to the actual feedback position. For this reason, this embodiment will also propose to perform lag compensation in the robot's motion space.

[0065] S130, performing hysteresis compensation on the predicted position to obtain a compensated predicted position.

[0066] The lag compensation here mainly refers to compensating the predicted position by utilizing the position lag error generated between the planned motion position and the actual position of the robot.

[0067] In one embodiment, Figure 4 As shown, step S130 includes the following sub-steps:

[0068] S310, calculate the lag error corresponding to the current moment based on the planned motion position at the current moment and the actual position; S320, accumulate the lag error at the current moment and the lag error at the previous moment to obtain the cumulative lag error; S330, superimpose the cumulative lag error with the predicted position.

[0069] For example, if the online planner has planned the output motion position at the current time t to be p exp , the actual position obtained by visual feedback is p fb , then the lag error e corresponding to the current time t t is the deviation between the two, where e t =p exp -p fb In addition, there is a cumulative historical hysteresis error before the current time t, so the following formula can be used to calculate the cumulative hysteresis error:

[0070]

[0071] In the formula, is the accumulated lag error at the current moment, is the lag error of the previous moment, e t is the lag error at the current moment, and w is the preset weight value.

[0072] Therefore, the accumulated hysteresis error Compensate to the predicted position p of the dynamic target pro Then, the predicted position after compensation is taken as input and brought into the online planner for online planning to obtain the planning trajectory of the robot tracking the dynamic target.

[0073] S140, performing online trajectory planning based on the compensated predicted position, obtaining the planning quantity of the multi-step joint motion state at the future moment, and determining the multi-step optimization trajectory in combination with the multi-step optimization objective function.

[0074] In this embodiment, the model predictive control concept is used to realize online trajectory planning. Specifically, it is realized by multi-step planning plus multi-step optimization. Among them, multi-step planning mainly predicts the motion state of the robot joints at multiple moments in the future through the trajectory prediction model, that is, the planning quantity; multi-step optimization is to solve a set of future state control quantities through the optimization function, so that the actual output at multiple moments in the future is as close as possible to the expected output determined by the reference trajectory.

[0075] Exemplarily, before online planning is performed, the joint motion state quantity at the current moment can be calculated based on the predicted position after compensation; then, based on the joint motion state quantity at the current moment and the pre-built trajectory prediction model, the planning quantity of the multi-step joint motion state for trajectory planning at future continuous moments is calculated. The trajectory prediction model can be constructed based on the relationship between the state quantity at the current moment and the state quantity at the next moment. For example, the expression of the trajectory prediction model is:

[0076]

[0077]

[0078] a i+1 =a i +j i dt

[0079] In the formula, a i 、v i 、p i are all state quantities at the i-th moment (current moment), representing acceleration, velocity, and position respectively; j i is the current jerk, which can be obtained by differentiating the acceleration; similarly, a i+1 、v i+1 、p i+1 The acceleration, velocity, and position at the i+1th moment (the next moment) are represented in sequence.

[0080] It can be understood that by performing multiple iterations of the above trajectory prediction model, the prediction of multi-step positions, multi-step velocities and multi-step accelerations can be achieved, which are also referred to as planning quantities. Assuming that the joint motion state at the next k moments needs to be predicted, the k-step motion state of the predicted output can be expressed in matrix form, as follows:

[0081] P k =T p J k +B p ;

[0082] V k =T v J k +Bv ;

[0083] A k =T a J k +B a ;

[0084] Among them, the multi-step position matrix P k =[p i p i+1 p i+2 ···p i+k ] T , multi-step velocity matrix V k =[v i v i+1 v i+2 ···v i+k ] T , multi-step acceleration matrix A k =[a i a i+1 a i+2 ···a i+k ] T , multi-step acceleration matrix J k =[J i J i+1 J i+2 ···J i+k ] T , T p , B p , T v , B v , T a , B a They are the corresponding parameter matrices, which can be obtained from the corresponding parameters in the trajectory prediction model, and will not be described here. It is worth noting that in this embodiment, the predicted position after compensation (denoted as p obj ) is input into the trajectory prediction model, so the multi-step position matrix mentioned above is also expressed as

[0085] In this embodiment, a trajectory prediction model is used for multi-step planning, and the obtained multi-step planning quantity is then used as input and brought into a pre-built multi-step optimization objective function for optimal solution, thereby determining a set of optimal trajectory parameters for robot joint control.

[0086] The construction of the multi-step optimization objective function depends on the set optimization target. When the number or objects of the optimization targets are different, the constructed objective function will also be different. In one embodiment, two optimization targets are taken as an example, where the multi-step position and multi-step speed of the robot joint are brought back to the origin as the first target, and the multi-step position error when tracking the dynamic target is minimized as the second target, and the multi-step optimization objective function is constructed. At this time, the expression of the multi-step optimization objective function can be:

[0087]

[0088] Where P k is the position matrix composed of k-step position planning quantities, V k is the speed matrix composed of k-step speed planning quantities, A k is the acceleration matrix composed of k-step acceleration planning quantities, It is composed of the predicted position after k-step compensation; ω1, ω2 and ω3 are three preset weights respectively. For example, each preset weight can take a value from 0 to 1, which is not limited here.

[0089] It can be understood that the state control quantity solved by the multi-step optimization objective function constructed based on the above two optimization objectives can achieve smaller lag and trajectory smoothness during compatible tracking. It is worth noting that the multi-step optimization objective function is not limited to the above expression. In another embodiment, the multi-step optimization objective function can also be constructed according to other tasks or other function types. For example, taking the task as an example, the optimization goal can also be to minimize the energy loss when tracking a dynamic target; for another example, taking the function type as an example, the square of the difference between each state quantity and the corresponding control quantity can also be used to construct the above multi-step optimization objective function, which is not limited here.

[0090] Furthermore, in order to solve a set of optimal control quantities, it is also necessary to set the constraints of the multi-step optimization objective function. For example, it may include but is not limited to physical constraints such as the motion position, speed, acceleration, jerk, etc. of each joint of the robot (such as upper and lower limits that cannot be reached), etc. In addition, more constraints can be added according to actual needs, which are not limited here.

[0091] S150, controlling the robot joints to perform corresponding movements according to the multi-step optimized trajectory.

[0092] Exemplarily, after a set of optimal control quantities (that is, the control output of the above-mentioned future k steps) is obtained by solving the multi-step optimization objective function, the robot uses it as the trajectory parameter of online planning to calculate the joint control instructions, and then performs joint motor control, so that the robot moves according to the planned trajectory. In addition, the robot continues to feedback the motion state of the current dynamic target at a set frequency, and this motion state will be reused to execute steps S120 to S150 of this embodiment, and so on, until the current dynamic target tracking task is completed or stopped. It can be understood that when the trajectory is planned online in this embodiment, a calculation is performed according to the above steps after each step is executed, thereby obtaining a trajectory that takes into account both small lag and smooth trajectory.

[0093] The dynamic target tracking method of this embodiment obtains the motion state of the tracked dynamic target in real time, and performs a short-term motion prediction on the dynamic target to obtain the predicted position of the dynamic target; then uses the generated hysteresis to perform spatial hysteresis compensation on the predicted position to obtain the compensated predicted position; then uses the model prediction idea to perform online trajectory planning based on the compensated predicted position to determine the multi-step joint optimized trajectory; finally, controls the joint movement of the robot based on the multi-step optimized trajectory to achieve real-time tracking of the dynamic target, which can not only effectively reduce the hysteresis problem of the robot when tracking the dynamic target, but also obtain a smooth motion trajectory, thereby improving the stability of the robot.

[0094] Please refer to Figure 5 Based on the method of the above embodiment, this embodiment proposes a dynamic target tracking device 100. Exemplarily, the dynamic target tracking device 100 includes:

[0095] The acquisition module 110 is used to acquire the motion state of the tracked dynamic target in real time. For example, the motion state may include but is not limited to the actual position, actual speed and actual acceleration of the dynamic target at the current moment.

[0096] The motion prediction module 120 is used to perform motion prediction according to the motion state at the current moment to obtain the predicted position of the dynamic target.

[0097] A spatial compensation module 130, configured to perform hysteresis compensation on the predicted position to obtain a compensated predicted position;

[0098] The trajectory planning module 140 is used to perform trajectory online planning according to the compensated predicted position, obtain the planning quantity of the multi-step joint motion state at the future moment, and determine the multi-step optimization trajectory in combination with the multi-step optimization objective function;

[0099] The joint control module 150 is used to control the robot joints to perform corresponding movements according to the multi-step optimized trajectory.

[0100] In one embodiment, the motion prediction module 120 includes a displacement calculation submodule and a prediction calculation submodule. The displacement calculation submodule is used to regard the movement of the dynamic target within a preset short time as a uniformly accelerated motion, so as to respectively calculate the first displacement generated by the dynamic target moving at an actual speed for a preset short time, and the second displacement generated by the dynamic target moving at an actual acceleration for a preset short time; and the prediction calculation submodule is used to calculate the predicted position of the dynamic target based on the actual position of the dynamic target at the current moment, the first displacement and the second displacement.

[0101] In one embodiment, the spatial compensation module 130 includes an error calculation submodule and a compensation superposition submodule, wherein the error calculation submodule is used to calculate the lag error corresponding to the current moment based on the planned motion position at the current moment and the actual position, and accumulate the lag error at the current moment and the lag error at the previous moment to obtain the cumulative lag error; the compensation superposition submodule is used to superimpose the cumulative lag error with the predicted position.

[0102] It can be understood that the device of this embodiment corresponds to the method of the above embodiment, and the options in the above embodiment are also applicable to this embodiment, so they will not be described repeatedly here.

[0103] In addition, the present application also provides a readable storage medium for storing the computer program used in the above-mentioned robot.

[0104] In several embodiments provided in the present application, it should be understood that the disclosed devices and methods can also be implemented in other ways. The device embodiments described above are merely schematic. For example, the flowcharts and structure diagrams in the accompanying drawings show the possible architecture, functions and operations of the devices, methods and computer program products according to multiple embodiments of the present application. In this regard, each box in the flowchart or block diagram can represent a module, a program segment or a part of a code, and the module, a program segment or a part of a code contains one or more executable instructions for implementing the specified logical function. It should also be noted that in an alternative implementation, the functions marked in the box can also occur in a different order from the order marked in the accompanying drawings. For example, two consecutive boxes can actually be executed substantially in parallel, and they can sometimes be executed in the opposite order, depending on the functions involved. It should also be noted that each box in the structure diagram and / or the flow diagram, and the combination of boxes in the structure diagram and / or the flow diagram, can be implemented with a dedicated hardware-based system that performs a specified function or action, or can be implemented with a combination of dedicated hardware and computer instructions.

[0105] In addition, the functional modules or units in the various embodiments of the present application may be integrated together to form an independent part, or each module may exist separately, or two or more modules may be integrated to form an independent part.

[0106] If the function is implemented in the form of a software function module and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present application can be essentially or partly embodied in the form of a software product that contributes to the prior art, and the computer software product is stored in a storage medium, including several instructions for a computer device (which can be a smart phone, a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present application. The aforementioned storage medium includes: various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a disk or an optical disk.

[0107] The above description is only a specific implementation manner of the present application, but the protection scope of the present application is not limited thereto. Any technician familiar with the technical field can easily think of changes or substitutions within the technical scope disclosed in the present application, which should be included in the protection scope of the present application.

Claims

1. A dynamic target tracking method, characterized in that: include: Obtain the motion status of the tracked dynamic target in real time; The motion state includes the actual position, actual speed and actual acceleration of the dynamic target at the current moment; The movement of the dynamic target within a preset short time is regarded as a uniformly accelerated movement, so as to perform a movement prediction according to the movement state at the current moment and obtain a predicted position of the dynamic target; Calculating the hysteresis error corresponding to the current moment according to the planned motion position at the current moment and the actual position, and accumulating the hysteresis error at the current moment and the hysteresis error at the previous moment to obtain a cumulative hysteresis error; superimposing the cumulative hysteresis error with the predicted position to obtain a compensated predicted position; Performing online trajectory planning according to the compensated predicted position to obtain the planning quantity of the multi-step joint motion state at the future moment, and determining the multi-step optimization trajectory in combination with the multi-step optimization objective function; According to the multi-step optimized trajectory, the robot joints are controlled to perform corresponding movements.

2. The dynamic target tracking method according to claim 1, characterized in that: The step of performing motion prediction according to the motion state at the current moment to obtain the predicted position of the dynamic target includes: respectively calculating a first displacement of the dynamic target generated by moving at the actual speed for a preset short time, and a second displacement of the dynamic target generated by moving at the actual acceleration for a preset short time; The predicted position of the dynamic target is calculated based on the actual position of the dynamic target at the current moment, the first displacement, and the second displacement.

3. The dynamic target tracking method according to claim 1, characterized in that: The online trajectory planning is performed according to the compensated predicted position to obtain the multi-step joint motion state planning quantity at the future moment, including: Based on the compensated predicted position, the joint motion state quantity at the current moment is calculated; According to the joint motion state quantity at the current moment and the trajectory prediction model constructed based on the relationship between the state quantity at the current moment and the state quantity at the next moment, the planning quantity of the multi-step joint motion state for trajectory planning at future continuous moments is calculated.

4. The dynamic target tracking method according to claim 3, characterized in that: The construction of the multi-step optimization objective function includes: A multi-step optimization objective function is constructed with the first goal of bringing the multi-step position and multi-step velocity of the robot joint back to the origin and the second goal of minimizing the multi-step position error when tracking a dynamic target, wherein the constraints of the multi-step optimization objective function include the physical constraints satisfied by the position, velocity and acceleration of the robot joint.

5. The dynamic target tracking method according to claim 4, characterized in that: The expression of the multi-step optimization objective function is as follows: Where P k is the position matrix composed of k-step position planning quantities, V k is the speed matrix composed of k-step speed planning quantities, A k is the acceleration matrix composed of k-step acceleration planning quantities, It is composed of the predicted position after k-step compensation; ω1, ω2 and ω3 are three preset weights respectively.

6. The dynamic target tracking method according to claim 1 or 2, characterized in that: The calculation formula for the predicted position of the dynamic target is as follows: In the formula, p pro is the predicted position of the dynamic target, p fb 、v fb and a fb are respectively the actual position, actual speed and actual acceleration of the dynamic target at the current moment, and dt represents a preset short time.

7. A dynamic target tracking device, characterized in that: include: An acquisition module is used to acquire the motion state of the tracked dynamic target in real time; The motion state includes the actual position, actual speed and actual acceleration of the dynamic target at the current moment; A motion prediction module, used to regard the motion of the dynamic target within a preset short time as a uniformly accelerated motion, so as to perform motion prediction according to the motion state at the current moment and obtain the predicted position of the dynamic target; A spatial compensation module, used to calculate the hysteresis error corresponding to the current moment according to the planned motion position at the current moment and the actual position, and accumulate the hysteresis error at the current moment and the hysteresis error at the previous moment to obtain a cumulative hysteresis error; superimpose the cumulative hysteresis error with the predicted position to obtain a compensated predicted position; A trajectory planning module is used to perform online trajectory planning according to the compensated predicted position, obtain the planning quantity of the multi-step joint motion state at the future moment, and determine the multi-step optimization trajectory in combination with the multi-step optimization objective function; The joint control module is used to control the robot joints to perform corresponding movements according to the multi-step optimized trajectory.

8. A robot, characterized in that: The robot comprises a processor and a memory, wherein the memory stores a computer program, and the processor is used to execute the computer program to implement the dynamic target tracking method according to any one of claims 1 to 6.

9. A readable storage medium, characterized in that: The device stores a computer program, which, when executed on a processor, implements the dynamic target tracking method according to any one of claims 1 to 6.

Images