A working dog and robot collaborative operation system

Abstract

The present invention relates to the technical field of special operations, and particularly relates to a cooperative operation system of a working dog and a robot. The system includes: receiving a set of task point position information and obtaining the starting position information of the robot's load carrier, generating candidate execution paths based on the starting position information of the robot's load carrier and the set of task point position information, calculating the execution prediction feature values of the candidate execution paths, determining whether the candidate execution paths meet the path execution conditions based on the execution prediction feature values, obtaining the coordinates of each task point position on the execution path, calculating the execution driving angles between adjacent task point positions, controlling the robot to travel between adjacent task points on the execution path according to the execution driving angles, calculating the correction angle error values during the travel, and controlling the robot to adjust the travel angle according to the angle error values, which can give full play to the biological characteristics of the working dog and the technological advantages of the robot to achieve efficient and flexible special operation tasks.

Description

Technical Field

[0001] The present invention relates to the technical field of special operations, and particularly to a cooperative operation system for working dogs and robots. Background Art

[0002] Working dogs (such as police dogs, search and rescue dogs, military dogs, etc.) play an important role in the fields of public security, anti-terrorism, rescue, etc. However, working dogs have some limitations when performing tasks, such as limited load-bearing capacity and insufficient tolerance in extreme environments. Quadruped robots have good mobility and load-bearing capacity, can carry a variety of scientific and technological devices (such as video acquisition devices, sensors, detectors, positioning modules, capture nets, weapons, etc.), and can walk stably in complex terrains. However, quadruped robots lack the sense of smell and autonomous judgment ability of biological dogs and need to rely on manual operation or preset programs.

[0003] Currently, the applications of working dogs and robotic dogs are mostly carried out independently, and an effective cooperative operation mode has not been formed, so the advantages of both cannot be fully utilized. When carrying the load required at the task point, the path planning is not accurate enough, resulting in low efficiency of cooperative operation.

[0004] Therefore, there is an urgent need for a cooperative operation system for working dogs and robots to solve the above problems. Summary of the Invention

[0005] The purpose of the present invention is to provide a cooperative operation system for working dogs and robots: aiming to organically combine working dogs and quadruped robots to achieve efficient and flexible special operation tasks through accurate path planning.

[0006] The purpose of the present invention can be achieved by the following technical solutions:

[0007] A cooperative operation system for working dogs and robots, the system includes:

[0008] A working dog task positioning module, used to mark the position information of the task points based on the positioning system on the working dog, form a set of task point position information, and send the set of task point position information to the path planning module;

[0009] A path planning module, used to receive the set of task point position information and obtain the starting position information of the robot's load. Based on the starting position information of the robot's load and the set of task point position information, generate candidate execution paths, calculate the execution prediction eigenvalue of the candidate execution paths, and judge whether the candidate execution paths meet the path execution conditions based on the execution prediction eigenvalue. If so, use the candidate execution paths as the execution paths and send them to the robot execution module;

[0010] A robot execution module, used to receive the execution paths and complete the task requirements according to the execution paths: obtain the coordinates of each task point position on the execution paths , calculate the execution travel angle between adjacent task point positions , according to the execution travel angle Control the robot to travel between adjacent task points in the execution path. During the travel, calculate the correction angle error value, and control the robot to adjust the travel angle according to the angle error value.

[0011] Furthermore, generating a number of candidate execution paths based on the robot's load starting point position information and the set of task point position information specifically includes the following process:

[0012] Set the load weight at the robot's load starting point as the starting transportation volume, set the load weight required at the task point position as the task demand volume, and accumulate the task demand volume and record it as the path demand volume.

[0013] If the starting transportation volume is greater than or equal to the path demand volume, then connect the task points in sequence according to the distance between the task points and the robot's load position to form a candidate execution path;

[0014] If the starting transportation volume is less than the path demand volume, then obtain the candidate execution path according to the rough combination method.

[0015] Furthermore, obtaining the candidate execution path according to the rough combination method specifically includes the following process:

[0016] Step 1: Set as the rough combination parameter, set a navigation point at each task point, and perform rough combination on each navigation point according to the sampling order of the sampling interval value to obtain a number of rough combination groups;

[0017] Step 2: Combine the first navigation point and the second navigation point of each rough combination group to form a reference vector , and combine the remaining th point with the th navigation point to form a vector , where , calculate the angle between and m , where the calculation formula of the angle

[0018] ;

[0019] Step 3: Compare with the threshold angle . If the of the th point is greater than the threshold angle , then subdivide the navigation points arranged in front of the th point into a group, and arrange the navigation points in front of the For the navigation points in front of a point, repeat Steps 2 and 3 to complete the subdivision of the rough combination, and connect the navigation points in each subdivided group into the candidate execution path.

[0020] Further, calculating the execution prediction eigenvalue of the candidate execution path specifically includes the following process:

[0021] Obtain the target information of the first task point, the target information of the second task point, and so on until the target information of the G-th task point on the candidate execution path; wherein, the target information of the first task point includes the first computing power redundancy, the first computing power failure trigger frequency, and the first load transmission distance; the target information of the G-th task point includes the G-th computing power redundancy, the G-th computing power failure trigger frequency, and the G-th load transmission distance; wherein, the computing power redundancy is the computing power redundancy allocated by the path planning module for the task point, the computing power failure trigger frequency is the probability that the robot fails to reach the task point, and the load transmission distance is the load transmission distance from the starting position of the robot carrying the load to the task point;

[0022] Add the first computing power redundancy, the first computing power failure trigger frequency, and the first load transmission distance to obtain the execution prediction coefficient of the first task point; until the execution prediction coefficient of the G-th task point is calculated;

[0023] Record the sum value of all execution prediction coefficients as the execution prediction eigenvalue of the candidate execution path.

[0024] Further, judging whether the candidate execution path meets the path execution condition based on the execution prediction eigenvalue specifically includes the following process:

[0025] Load the execution prediction eigenvalue threshold, the value of which is set by the system and stored in the system, and judge whether the execution prediction eigenvalue is greater than the execution prediction eigenvalue threshold. If so, determine that the candidate execution path meets the path execution condition; if not, determine that the candidate execution path does not meet the path execution condition.

[0026] Further, calculate the execution driving angle between the positions of adjacent task points Specifically includes the following process:

[0027] ;

[0028] Among them, , , is the coordinate of the task point adjacent to .

[0029] Further, judging whether the robot needs to turn at the task point specifically includes the following process:

[0030] For the first task point of the execution path, first find out the angle formed by the connection line between it and the second task point of the execution path : ; Among them, the coordinates of the first task point are , and the coordinates of the second task point are ; Calculate the execution travel angle for the robot to reach the first task point, calculate the angle and the difference from the execution travel angle, and adjust the turning angle based on the difference;

[0031] For the last task point of the execution path, the task can be regarded as completed after reaching the end point without turning;

[0032] For other task points in the execution path, respectively calculate the angles formed by the lines connecting them to adjacent task points 、 : ; ; Among them, the coordinates of other task points in the execution path are , and the coordinates of the adjacent task points are respectively 、 , calculate and difference, judge whether the difference exceeds the preset angle threshold, if so, judge that turning is required, if not, judge that turning is not required.

[0033] Further, calculating the correction angle error value during driving specifically includes the following process:

[0034] Obtain the current position of the robot , the adjacent task point , according to the calculation formula , obtain the correction coefficient curve , where is the sampling time interval value corresponding sampling time sequence number, , and is a positive integer; based on the correction curve calculate the correction angle error value : ; Among them, is a constant set by the system, is the correction coefficient corresponding to the sampling time sequence number , is the correction coefficient corresponding to the sampling time sequence number .

[0035] Compared with the existing scheme, the beneficial effects achieved by the present invention:

[0036] Precise Task Location: The task location module of the working dog uses the positioning system on the working dog to mark the position information of the task points, forming an accurate set of task point position information. This method of field positioning based on the working dog can make full use of the flexibility of the working dog and its ability to adapt to complex environments, quickly and accurately determine the task point positions, providing reliable basic data for subsequent operations. It is especially suitable for some task scenarios where it is difficult for humans to directly reach or the environment is complex.

[0037] Efficient Path Planning: The execution path generated by the present invention is not only feasible, but also optimized in terms of execution efficiency, resource consumption, etc., effectively improving the efficiency and success rate of the robot in executing tasks, and reducing unnecessary path exploration and time waste.

[0038] Accurate Robot Execution: During the driving process, the angle error value is calculated and corrected in real time, and the driving angle is adjusted accordingly, ensuring the accuracy and stability of the robot's driving. Even in complex terrains or in the presence of interference, it can ensure that the robot accurately completes the task requirements according to the predetermined path, improving the reliability and stability of the entire collaborative operation system.

[0039] Advantages of Collaborative Operation: The working dog is responsible for quickly and accurately locating the task points, and the robot efficiently executes the tasks according to the planned path, giving full play to the flexibility of the working dog and the accurate execution ability of the robot. The advantages of the two complement each other, greatly improving the operation efficiency and quality, expanding the operation scope and application scenarios, and can be widely applied to various fields that require operations in complex environments. Description of the Drawings

[0040] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the following will briefly introduce the drawings required to be used in the embodiments. Obviously, the drawings described below are only some embodiments recorded in the present invention. For those of ordinary skill in the art, other drawings can also be obtained based on these drawings.

[0041] Figure 1 is a system block diagram of a collaborative operation system of a working dog and a robot according to an embodiment of the present invention;

[0042] Figure 2 is a working flow chart of a collaborative operation system of a working dog and a robot according to an embodiment of the present invention;

[0043] Figure 3 is another working flow chart of a collaborative operation system of a working dog and a robot according to an embodiment of the present invention. Detailed Embodiments

[0044] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0045] In addition, the described features, structures or characteristics may be combined in any suitable manner in one or more example embodiments. In the following description, numerous specific details are provided to give a thorough understanding of the example embodiments of the present disclosure. However, those skilled in the art will realize that the technical solutions of the present disclosure may be practiced without one or more of the specific details, or other methods, components, steps, etc. may be adopted. In other cases, well-known structures, methods, implementations or operations are not shown or described in detail to avoid obscuring aspects of the present disclosure.

[0046] This embodiment provides a cooperative operation system for working dogs and robots. Figure 1 It is a system block diagram of a cooperative operation system for working dogs and robots according to an embodiment of the present invention. As Figure 1 shown, the system includes:

[0047] A working dog task positioning module, configured to mark the position information of task points based on the positioning system on the working dog, form a set of task point position information, and send the set of task point position information to the path planning module;

[0048] A path planning module, configured to receive the set of task point position information and obtain the starting position information of the robot carrying goods, generate a candidate execution path based on the starting position information of the robot carrying goods and the set of task point position information, calculate the execution prediction feature value of the candidate execution path, and determine whether the candidate execution path meets the path execution condition based on the execution prediction feature value. If so, use the candidate execution path as the execution path and send it to the robot execution module;

[0049] A robot execution module, configured to receive the execution path and complete the task requirements according to the execution path: obtain the coordinates of each task point position on the execution path , calculate the execution driving angle between adjacent task point positions , and control the robot to drive between adjacent task points on the execution path according to the execution driving angle ;

[0050] A turning adjustment module, during the driving process, determines whether the robot needs to turn at a task point. If so, executes a turning instruction, calculates a correction angle error value, and controls the robot to adjust the driving angle according to the angle error value.

[0051] In summary, the present invention receives a set of task point position information and obtains the starting position information of the robot carrying goods. Based on the starting position information of the robot carrying goods and the set of task point position information, a candidate execution path is generated, the execution prediction feature value of the candidate execution path is calculated, and it is judged whether the candidate execution path meets the path execution condition based on the execution prediction feature value. If so, the execution path is received and the task requirements are completed according to the execution path: obtaining the coordinates of each task point position on the execution path, calculating the execution driving angle between adjacent task point positions, controlling the robot to drive between adjacent task points on the execution path according to the execution driving angle, calculating the correction angle error value during the driving process, and controlling the robot to adjust the driving angle according to the angle error value, which can give full play to the biological characteristics of working dogs and the technological advantages of robots to achieve efficient and flexible special operation tasks.

[0052] In some embodiments, Figure 2 is a work flow chart of a collaborative operation system of a working dog and a robot according to an embodiment of the present invention. As Figure 2 shown, generating a number of candidate execution paths based on the starting position information of the robot carrying goods and the set of task point position information specifically includes the following process:

[0053] Step S201: Set the load weight at the starting point of the robot carrying goods as the starting transportation volume, set the load weight required at the task point position as the task demand volume, and accumulate the task demand volume and record it as the path demand volume;

[0054] Step S202: If the starting transportation volume is greater than or equal to the path demand volume, connect the task points in sequence according to the distance between the task points and the position of the robot carrying goods to form a candidate execution path;

[0055] Step S203: If the starting transportation volume is less than the path demand volume, obtain the candidate execution path in the way of rough combination.

[0056] In some embodiments, obtaining the candidate execution path in the way of rough combination specifically includes the following process:

[0057] Step one: Set as the rough combination parameter, set a navigation point at each task point, and perform rough combination on each navigation point according to the sampling order of the sampling interval value to obtain a number of rough combination groups;

[0058] Step two: Combine the first navigation point and the second navigation point of each rough combination group to form a reference vector , and combine the remaining th point with the th navigation point to form a vector , where , calculate The angle between and m , where the angle is calculated as follows:

[0059] ;

[0060] Step three: Compare with the threshold angle . If the at the point is greater than the threshold angle , then subdivide the navigation points in front of the point into a group, repeat step two and step three for the navigation points in front of the point to complete the subdivision of the rough combination, and connect the navigation points in each subdivided group into a candidate execution path.

[0061] In some embodiments, Figure 3 is the flowchart of another working dog and robot collaborative operation system according to an embodiment of the present invention. As shown in Figure 3 , calculating the execution prediction eigenvalue of the candidate execution path specifically includes the following process:

[0062] Step S301: Obtain the target information of the first task point, the target information of the second task point until the target information of the Gth task point on the candidate execution path;

[0063] Among them, the target information of the first task point includes the first computing power redundancy, the first computing power failure trigger frequency, and the first load transmission distance; the target information of the Gth task point includes the Gth computing power redundancy, the Gth computing power failure trigger frequency, and the Gth load transmission distance; among them, the computing power redundancy is the computing power redundancy allocated by the path planning module for the task point, the computing power failure trigger frequency is the probability that the robot does not reach the task point, and the load transmission distance is the load transmission distance from the robot load starting position to the task point;

[0064] Step S302: Add the first computing power redundancy, the first computing power failure trigger frequency, and the first load transmission distance to obtain the execution prediction coefficient of the first task point; until the execution prediction coefficient of the Gth task point is calculated;

[0065] Step S303: Denote the sum value of all execution prediction coefficients as the execution prediction eigenvalue of the candidate execution path.

[0066] In some embodiments, judging whether the candidate execution path meets the path execution condition based on the execution prediction eigenvalue specifically includes the following process:

[0067] Load and execute the prediction feature value threshold, whose value is set by the system and stored in the system. Determine whether the execution prediction feature value is greater than the execution prediction feature value threshold. If so, determine that the candidate execution path meets the path execution condition; if not, determine that the candidate execution path does not meet the path execution condition.

[0068] In some embodiments, calculate the execution driving angle between adjacent task point positions Specifically, it includes the following process:

[0069] ;

[0070] Among them, , , is the coordinate of the task point adjacent to the adjacent task point coordinates.

[0071] In some embodiments, determining whether the robot needs to turn at a task point specifically includes the following process:

[0072] For the first task point of the execution path, first find the angle formed by the line connecting it and the second task point of the execution path : ; Among them, the coordinate of the first task point is , and the coordinate of the second task point is ; Calculate the execution driving angle when the robot reaches the first task point, calculate the difference between the angle and the execution driving angle, and adjust the turning angle based on the difference;

[0073] For the last task point of the execution path, the task can be regarded as completed after reaching the end point, and no turning is required;

[0074] For other task points in the execution path, find the angles , formed by the lines connecting them to adjacent task points respectively: ; ; Among them, the coordinates of other task points in the execution path are , and the coordinates of adjacent task points are respectively , , calculate the difference between and , and determine whether the difference exceeds the preset angle threshold. If so, determine that turning is required; if not, determine that turning is not required.

[0075] In some embodiments, control the robot to travel between adjacent task points in the execution path according to the execution driving angle, and calculate the correction angle error value during the travel, which specifically includes the following process:

[0076] Obtain the current position of the robot , the adjacent task points , according to the calculation formula , obtain the correction coefficient curve , where is the sampling time interval value corresponding to the sampling time serial number, , and is a positive integer; based on the correction curve calculate the corrected angular error value : ; where is a constant, set by the system, is the sampling time serial number corresponding correction coefficient, is the sampling time serial number corresponding correction coefficient.

[0077] The above embodiments can be implemented in whole or in part by software, hardware, firmware, or any combination thereof. When implemented using software, the above embodiments can be implemented in whole or in part in the form of a computer program product. The computer program product includes one or more computer instructions or computer programs. When the computer instructions or computer programs are loaded or executed on a computer, the processes or functions described in the embodiments of the present application are generated in whole or in part. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable devices. The computer instructions can be stored in a computer-readable storage medium, or transmitted from one computer-readable storage medium to another computer-readable storage medium. For example, the computer instructions can be transmitted from one website, computer, server, or data center to another website, computer, server, or data center by wire (such as infrared, wireless, microwave, etc.). The computer-readable storage medium can be any available medium that the computer can access or a data storage device such as a server or data center that contains one or more collections of available media. The available medium can be a magnetic medium (such as a floppy disk, hard disk, magnetic tape), an optical medium (such as a DVD), or a semiconductor medium. The semiconductor medium can be a solid-state drive.

[0078] Those of ordinary skill in the art will realize that the units and algorithm steps of the examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or in a combination of computer software and electronic hardware. Whether these functions are executed in hardware or software depends on the specific application and design constraints of the technical solution. Skilled professionals can use different methods to implement the described functions for each specific application, but such implementation should not be considered to exceed the scope of this application.

[0079] Those skilled in the art can clearly understand that for the convenience and brevity of description, the specific working processes of the systems, devices, and units described above can refer to the corresponding processes in the foregoing method embodiments, and will not be elaborated herein.

[0080] In several embodiments provided in the present application, it should be understood that the disclosed systems, devices, and methods can be implemented in other ways. For example, the device embodiments described above are merely illustrative. For example, the division of the units is only for some logical function divisions. In actual implementation, there may be other division methods. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the displayed or discussed couplings or direct couplings or communication connections to each other can be through some interfaces. The indirect couplings or communication connections of the devices or units can be in electrical, mechanical, or other forms.

[0081] The units described as separate components may or may not be physically separated. The components displayed as units may or may not be physical units, that is, they can be located in one place, or can be distributed to multiple network units. Some or all of the units can be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0082] The above is only the specific implementation manner of the present application, but the protection scope of the present application is not limited thereto. Any person skilled in the art can easily think of changes or substitutions within the technical scope disclosed in the present application, and all should be covered by the protection scope of the present application. Therefore, the protection scope of the present application should be subject to the protection scope of the claims.

Claims

1. A cooperative operation system for working dogs and robots, characterized in that, The system includes: A working dog task positioning module, which is used to mark the position information of task points based on the positioning system on the working dog, form a set of task point position information, and send the set of task point position information to the path planning module; A path planning module, which is used to receive the set of task point position information and obtain the starting position information of the robot carrying goods, generate candidate execution paths based on the starting position information of the robot carrying goods and the set of task point position information, calculate the execution prediction feature values of the candidate execution paths, and judge whether the candidate execution paths meet the path execution conditions based on the execution prediction feature values. If so, the candidate execution paths are used as the execution paths and sent to the robot execution module; Among them, generating several candidate execution paths based on the starting position information of the robot carrying goods and the set of task point position information specifically includes the following process: Set the load weight at the starting point of the robot carrying goods as the starting transportation volume, set the load weight required at the task point position as the task demand volume, and accumulate the task demand volume and record it as the path demand volume. If the starting transportation volume is greater than or equal to the path demand volume, connect the task points in sequence according to the distance between the task points and the position of the robot carrying goods to form a candidate execution path; If the starting transportation volume is less than the path demand volume, obtain the candidate execution path according to the slightly combined algorithm; A robot execution module, configured to receive an execution path and fulfill task requirements according to the execution path: obtain the coordinates of the position of each task point on the execution path , calculate the execution driving angle between the positions of adjacent task points , and control the robot to travel between adjacent task points on the execution path according to the execution driving angle ; A turning adjustment module, during the driving process, judges whether the robot needs to turn at the task point. If so, executes the turning instruction, calculates the corrected angle error value, and controls the robot to adjust the driving angle according to the angle error value.

2. The collaborative operation system for a working dog and a robot according to claim 1, characterized in that, Obtaining the candidate execution path according to the slightly combined algorithm specifically includes the following process: Step 1: Set as the rough combination parameter, set a navigation point at each task point, and perform rough combination on each navigation point in the sampling order of the sampling interval value to obtain a number of rough combination groups; Step 2: Combine the first navigation point and the second navigation point of each rough combination group to form a reference vector , and combine the remaining th point with the th navigation point to form a vector , where , calculate the angle between and 1 respectively , where the formula for the angle is as follows: ； Step 3: Compare with the threshold angle . If the value of the at the point is greater than the threshold angle , then subdivide the navigation points in front of the point into a group, repeat Steps 2 and 3 for the navigation points in front of the point to complete the subdivision of the rough combination, and connect the navigation points in each subdivided group as the candidate execution path.

3. The collaborative operation system for a working dog and a robot according to claim 1, characterized in that, Calculating the execution prediction feature values of the candidate execution path specifically Includes the following process: Obtain the target information of the first task point, the target information of the second task point until the target information of the Gth task point on the candidate execution path; among them, the target information of the first task point includes the first computing power redundancy, the first computing power failure trigger frequency, and the first load transmission distance; the target information of the Gth task point includes the Gth computing power redundancy, the Gth computing power failure trigger frequency, and the Gth load transmission distance; among them, the computing power redundancy is the computing power redundancy allocated by the path planning module for the task point, the computing power failure trigger frequency is the probability that the robot does not reach the task point, and the load transmission distance is the load transmission distance from the starting position of the robot carrying goods to the task point; Add the first computing power redundancy, the first computing power failure trigger frequency, and the first load transmission distance to obtain the execution prediction coefficient of the first task point; until the execution prediction coefficient of the Gth task point is calculated; Record the sum value of all execution prediction coefficients as the execution prediction feature value of the candidate execution path.

4. A working dog and robot collaborative operation system according to claim 1, wherein, Judging whether the candidate execution path meets the path execution conditions based on the execution prediction feature value specifically Includes the following process: Load the execution prediction feature value threshold, the value of which is set by the system and stored in the system, judge whether the execution prediction feature value is greater than the execution prediction feature value threshold. If so, determine that the candidate execution path meets the path execution conditions. If not, determine that the candidate execution path does not meet the path execution conditions.

5. The collaborative operation system for a working dog and a robot according to claim 1, characterized in that Calculate the execution driving angle between adjacent task point positions Specifically, it includes the following process: ； Among them, , , are the coordinates of the task points adjacent to .

6. The collaborative operation system for a working dog and a robot according to claim 1, wherein Judging whether the robot needs to turn at the task point specifically includes the following process: For the first task point of the execution path, first calculate the angle formed by the line connecting it to the second task point of the execution path : ; where the coordinates of the first task point are , and the coordinates of the second task point are ; Calculate the execution driving angle for the robot to reach the first task point, calculate the difference between the angle and the execution driving angle, and adjust the turning angle based on the difference; For the last task point of the execution path, the task can be considered completed after reaching the end point without the need to turn; For other task points in the execution path, calculate the angles formed by the lines connecting them to adjacent task points respectively , : ; ; Among them, the coordinates of other task points in the execution path are , and the coordinates of adjacent task points are respectively , , calculate the difference between and , and determine whether the difference exceeds a preset angle threshold. If so, it is determined that a turn is required. If not, it is determined that a turn is not required.

7. A working dog and robot collaborative operation system according to claim 1, wherein, During driving, calculating the correction angle error value specifically includes the following processes: Obtain the current position of the robot , the adjacent task point , according to the calculation formula , obtain the correction coefficient curve , where is the sampling time interval value is the corresponding sampling time serial number , and is a positive integer; based on the correction curve calculate the corrected angle error value : ; where is a constant, set by the system is the sampling time serial number corresponding correction coefficient is the sampling time serial number corresponding correction coefficient

Images