Path planning method and device, hand robot and medium

Abstract

The invention discloses a path planning method and device, a hand robot and a medium. The invention relates to the technical field of robots. The method comprises the following steps: determining position information of a tail end joint of the hand robot at a plurality of acquisition moments, and taking the position information of at least three adjacent acquisition moments as a stroke group; based on at least three pieces of position information in the current travel group, determining a bending attribute of middle position information in the current travel group; when it is determined that the bending attribute meets the preset condition, target position information is determined based on the middle position information when the preset condition is met; and determining a target driving path corresponding to the hand robot based on the first position information and the last position information in the multiple pieces of position information, each piece of target position information and a preset cubic spline curve. The problem of low path planning accuracy caused by point-to-point path planning based on joint space is solved, and the effects of reducing the control calculation amount and improving the path planning accuracy of the hand robot are achieved.

Description

Technical Field

[0001] The present invention relates to the field of robotics technology, and in particular to a path planning method, device, hand robot and medium. Background Art

[0002] With the development of computer technology, more and more hand robots are used in the medical industry or other industries. In order to realize the automation and precision control of hand robots, trajectory planning is usually performed for hand robots, and the hand robots are controlled to perform operations through the planned trajectory.

[0003] At present, the trajectory planning method for hand robots is usually to plan the trajectory according to the joint space. This method directly interpolates the joint variables and finally establishes the curve of the joint variables changing over time. Since this method cannot predict the trajectory changes of the end during the movement, it is only suitable for point-to-point operation tasks at the end, cannot guarantee the smoothness of the movement, and has the problem of low trajectory planning accuracy. Summary of the invention

[0004] The present invention provides a path planning method, device, hand robot and medium, so as to reduce the control calculation amount while improving the accuracy of the hand robot's path planning, thereby achieving the technical effect of ensuring the stability of the hand robot's movement.

[0005] According to one aspect of the present invention, a path planning method is provided, the method comprising:

[0006] Determine the position information of the end joint of the hand robot at multiple acquisition moments, and regard the position information of at least three adjacent acquisition moments as a travel group;

[0007] For each of the travel groups, based on at least three pieces of position information in the current travel group, determining a bending attribute of the middle position information in the current travel group;

[0008] For each of the bending attributes, when it is determined that the bending attribute satisfies a preset condition, the target position information is determined based on the intermediate position information when the preset condition is satisfied; wherein the preset condition includes that the curvature in the bending attribute is greater than a curvature threshold, and / or that the curvature in the bending attribute is greater than a curvature threshold;

[0009] Based on the first position information and the last position information of the position information at the multiple acquisition moments, each target position information, and a preset cubic spline curve, a target driving path corresponding to the hand robot is determined.

[0010] According to another aspect of the present invention, a path planning device is provided, the device comprising:

[0011] A travel group determination module, used to determine the position information of the end joint of the hand robot at multiple acquisition moments, and to take the position information of at least three adjacent acquisition moments as a travel group;

[0012] a bending attribute determination module, configured to determine, for each of the trip groups, a bending attribute of the middle position information in the current trip group based on at least three pieces of position information in the current trip group;

[0013] a target position information determination module, configured to determine, for each of the bending attributes, target position information based on the intermediate position information when the preset condition is met when it is determined that the bending attribute satisfies a preset condition; wherein the preset condition includes that the curvature in the bending attribute is greater than a curvature threshold, and / or that the curvature in the bending attribute is greater than a curvature threshold;

[0014] The target driving path determination module is used to determine the target driving path corresponding to the hand robot based on the first position information and the last position information in the position information at the multiple acquisition moments, each target position information, and a preset cubic spline curve.

[0015] According to another aspect of the present invention, there is provided a hand robot, the hand robot comprising:

[0016] at least one processor; and a memory in communication with the at least one processor; wherein,

[0017] The memory stores a computer program that can be executed by the at least one processor, and the computer program is executed by the at least one processor so that the at least one processor can execute the path planning method described in any embodiment of the present invention.

[0018] According to another aspect of the present invention, a computer-readable storage medium is provided, wherein the computer-readable storage medium stores computer instructions, and the computer instructions are used to enable a processor to implement the path planning method described in any embodiment of the present invention when executed.

[0019] According to another aspect of the present invention, a computer program product is provided, comprising a computer program, which, when executed by a processor, implements the path planning method as described in any embodiment of the present invention.

[0020] The technical solution of the embodiment of the present invention determines the position information of the end joint of the hand robot at multiple acquisition moments, and regards the position information of at least three adjacent acquisition moments as a travel group; determines the bending property of the middle position information in the current travel group based on at least three position information in the current travel group; when it is determined that the bending property meets the preset condition, determines the target position information based on the middle position information when the preset condition is met; the preset condition includes that the curvature in the bending property is greater than the curvature threshold, and / or the curvature in the bending property is greater than the curvature threshold; based on the first position information and the last position information in the position information at multiple acquisition moments, each target position information and a preset cubic spline curve, determines the target driving path corresponding to the hand robot, and solves the problem of low path planning accuracy caused by point-to-point trajectory planning based on joint space in the prior art. The invention realizes that by regarding the position information of at least three adjacent acquisition moments of the end joint of the hand robot as a travel group, and then, according to at least three position information in the current travel group, determines the bending property of the middle position information in the current travel group, and by judging whether the bending property meets the preset condition, determines whether the middle position information is the target position information, reduces the control calculation amount, and ensures the path planning accuracy. Furthermore, based on the first position information and the last position information in the position information at multiple acquisition moments, each target position information and a preset cubic spline curve, cubic spline interpolation processing is implemented to improve the smoothness of the target driving path planning, thereby ensuring the technical effect of the smoothness of the hand robot's movement.

[0021] It should be understood that the contents described in this section are not intended to identify the key or important features of the embodiments of the present invention, nor are they intended to limit the scope of the present invention. Other features of the present invention will become easily understood through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following briefly introduces the drawings required for use in the description of the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0023] Figure 1 is a flow chart of a path planning method provided according to Embodiment 1 of the present invention;

[0024] Figure 2 is a schematic diagram of a path planning method provided according to Embodiment 1 of the present invention;

[0025] Figure 3 is a schematic diagram of a path planning method provided according to Embodiment 1 of the present invention;

[0026] Figure 4 is a schematic diagram of a path planning method provided according to Embodiment 1 of the present invention;

[0027] Figure 5 is a flow chart of a path planning method provided according to Embodiment 2 of the present invention;

[0028] Figure 6 is a schematic diagram of a path planning method provided according to Embodiment 2 of the present invention;

[0029] Figure 7 is a schematic diagram of the structure of a path planning device provided according to Embodiment 3 of the present invention;

[0030] Figure 8 It is a schematic diagram of the structure of a hand robot that implements the path planning method of an embodiment of the present invention. DETAILED DESCRIPTION

[0031] In order to enable those skilled in the art to better understand the scheme of the present invention, the technical scheme in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of the present invention.

[0032] It should be noted that the terms "first", "second", etc. in the specification and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects, and are not necessarily used to describe a specific order or sequence. It should be understood that the data used in this way can be interchanged where appropriate, so that the embodiments of the present invention described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions, for example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units that are clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0033] Embodiment 1

[0034] Figure 1 1 is a flow chart of a path planning method provided according to the first embodiment of the present invention. This embodiment is applicable to planning the driving path of a hand robot. The method can be executed by a path planning device. The path planning device can be implemented in the form of hardware and / or software. The path planning device can be configured in the hand robot. Figure 1 As shown, the method includes:

[0035] S110, determining position information of the end joint of the hand robot at multiple collection moments, and taking the position information of at least three adjacent collection moments as a travel group.

[0036] Among them, the hand robot may refer to a robot that can handle work tasks. For example, the hand robot may be a surgical robot that can perform operations such as suturing, pulling, and peeling; or, the hand robot may also be an industrial robot that can perform operations such as grasping, carrying, and operating tools. The hand robot is composed of a plurality of connecting rod joints connected together, and the end joint may refer to the endmost connecting rod joint among a plurality of connecting rod joints connected in sequence in the hand robot. The acquisition moment may refer to the moment when the joint position information is collected during the actual operation of the hand robot. The position information may be three-dimensional coordinate information in Cartesian space, including the coordinate values ​​of the X-axis, Y-axis, and Z-axis.

[0037] In this embodiment, the current operation information (such as the current rotation angle, position, etc.) of each joint of the hand robot can be collected during the actual operation of the hand robot, and the three-dimensional coordinates of the end joints of the hand robot in the Cartesian space can be calculated based on the current operation information of each joint. The three-dimensional coordinates are the position information of the end joints at the current collection time. Correspondingly, the position information of the end joints at multiple collection times can be determined, and the position information at multiple collection times constitutes the continuous motion information of the end joints in space. Further, the position information of at least three adjacent collection times can be sorted in a travel group according to the collection time. That is to say, if the travel group includes three position information sorted by collection time, then the second position information in the travel group is the intermediate position information.

[0038] Exemplarily, taking the hand robot as a surgical robot, the surgical robot includes a hand controller, and the method for determining the position information of the end joints of the hand robot at multiple acquisition times can be: during the doctor's operation of the hand robot, the hand controller acquires the current rotation angles of each joint of the hand robot, and calculates the position information of the end joints in Cartesian space according to the current rotation angles of each joint; if the doctor's operation is not over, the position information of the end joint continues to be determined; if the operation is over, the collected continuous position information is obtained, and features are extracted from the continuous position information to obtain feature points on the hand motion curve.

[0039] In this embodiment, the position information of the end joints of the hand robot at multiple acquisition moments is determined, including: in the process of the first object controlling the movement of the hand robot, the joint parameters of at least two joints in the hand robot at multiple acquisition moments are obtained; for each acquisition moment, based on the joint parameters of the adjacent joints at the current acquisition moment, the transformation matrix from the previous joint to the next joint in the adjacent joints is determined; based on the transformation matrices corresponding to all the adjacent joints, the position information of the end joints of the hand robot at the current acquisition moment is determined.

[0040] Among them, the first object may refer to a user who operates the hand robot. Joint parameters include but are not limited to connecting rod length, connecting rod angle, connecting rod offset and joint angle. Connecting rod length refers to the length of the common perpendicular line between two adjacent joints. Connecting rod offset refers to the shortest distance between two adjacent joints. Connecting rod angle refers to the angle between two adjacent joints. Joint angle refers to the angle between a joint and its previous joint. The at least two joints can be six joints or seven joints, without limitation. Adjacent joints may refer to two joints connected to each other.

[0041] Specifically, during the process of the first object controlling the hand robot to operate, the hand robot moves, and the joint parameters of each joint in the hand robot at multiple acquisition moments can be collected in real time. For a pair of adjacent joints, the transformation matrix from the previous joint to the next joint in the pair of adjacent joints can be determined based on the DH (DH represents Denavit-Hartenberg parameter) modeling rule and the joint parameters of the pair of adjacent joints at the current acquisition moment. Furthermore, each transformation matrix can be multiplied in sequence according to the connection order of the nodes to obtain the positive solution matrix of the end joint. The positive solution matrix is ​​a The matrix of the positive solution matrix includes a posture matrix representing the posture information of the end joint and a position matrix representing the position information of the end joint. The position matrix in the positive solution matrix can be extracted as the position information of the end joint of the hand robot at the current acquisition time. Accordingly, the position information of each end joint at each acquisition time can be obtained.

[0042] Exemplarily, the DH modeling rule is adopted to establish the DH coordinate system of the hand robot, and the DH parameters (ie, joint parameters) of each joint of the hand robot are obtained. The DH parameters are shown in Table 1.

[0043] Table 1

[0044]

[0045] Among them, the DH parameters include four parameters, which are: , Indicates the joint number, . Indicates the length of the connecting rod, which is the joint along Axis, from Move to distance; Represents the connecting rod rotation angle, which is the joint Around Axis, from Rotate to Angle Indicates the connecting rod offset, which is the joint along Axis, from Move to distance; Represents the joint angle, which is the joint Around Axis, from Rotate to Angle represents π; It refers to The X coordinate axis of the joint, It refers to The Z coordinate of the joint.

[0046] Furthermore, according to the DH modeling rule, the transformation matrix from the i-th joint to the i+1-th joint is calculated , the transformation matrix It can be expressed as:

[0047] ;

[0048] Further, according to the transformation matrix, the positive solution matrix is ​​calculated , the positive solution matrix It can be expressed as:

[0049] ; Among them, n matrix is the direction cosine of the joint's X-axis relative to the Cartesian coordinate system. is the direction cosine of the joint's Y coordinate axis relative to the Cartesian coordinate system. The p matrix is ​​the direction cosine of the joint's Z coordinate axis relative to the Cartesian coordinate system. is the position part of the end joint with respect to the Cartesian coordinate system. Take the position part of the positive solution matrix , as the position information of the end joint in Cartesian space; ; Wherein, each position information represents a trajectory point, and k represents the kth trajectory point collected.

[0050] See also Figure 2When the doctor uses the hand robot to perform surgery, the position information of each joint of the hand robot is collected, the positive matrix of each collection moment is calculated in real time, and the three-dimensional coordinates of the Cartesian space are stored to obtain the position information of the end joint at each collection moment. These position information can be used as standard action feature points. When the surgery is completed, the continuous three-dimensional space trajectory curve of the standard action feature points can be obtained. These standard action feature points can be used To express.

[0051] ;

[0052] in, is the set of standard action feature points collected, is the total number of feature point sets, that is, the total number of position information of the end joints, The end joint Standard action feature points.

[0053] S120 . For each stroke group, determine the bending attribute of the middle position information in the current stroke group based on at least three position information in the current stroke group.

[0054] The curvature attribute can be used to characterize the curvature of the path passing through the location information. For example, the curvature attribute can be curvature, and the larger the curvature value, the curvier the path passing through the location information. It should be noted that the method for determining the curvature attribute corresponding to the intermediate location information in each trip group is the same, and any of the trip groups can be used as the current trip group to determine the curvature attribute corresponding to the intermediate location information in the current trip group as an example for introduction.

[0055] In this embodiment, a curve representing the driving trajectory can be constructed based on at least three position information in the current trip group, and the peak value of the curve can be calculated (for example, the peak value can be the difference between the highest value and the lowest value); the bending attribute corresponding to the middle position information in the current trip group is determined based on the peak value, for example, the larger the peak value, the larger the bending attribute value. Alternatively, the slope of the curve passing through the middle position information in the current trip group is calculated (for example, the slope can be the value of the horizontal value of two points compared to the vertical height value), and the bending attribute corresponding to the middle position information is determined based on the slope, for example, the smaller the slope, the smaller the bending attribute value.

[0056] Optionally, the bending attribute may be curvature, and the bending attribute of the middle position information in the current stroke group is determined based on at least three position information in the current stroke group, including: determining the length of the connection corresponding to the current stroke group based on the first connection between the first position information and the last position information in the current stroke group; determining the length of the vertical line corresponding to the current stroke group based on the middle position information in the current stroke group and the first connection; and determining the curvature of the middle position information based on the connection length and the vertical line length.

[0057] In this embodiment, the difference processing can be performed on the first position information and the last position information in the current trip group to obtain the length of the line between the two positions. The first position information and the last position information in the current trip group are connected to obtain the line between the two positions, and the line is used as the first line. A perpendicular line is drawn to the first line with the middle position information as the coordinate point to determine the length of the perpendicular line from the middle position information to the first line. Further, the perpendicular line length and the line length can be processed as a quotient, and the obtained quotient value is used as the curvature of the middle position information. For example, the first The three locations in a trip group are: . is the index of the trip group; Indicates the trip group The middle position information in is also called the middle point; Indicates the trip group The first location information in Indicates the trip group The third position information (i.e. the last position information) in and is the interval point. The curvature It can be expressed as: .in, For the The length of the line connecting two interval points in a stroke group, For the The length of the perpendicular line drawn from the middle point of a stroke group to the interval point.

[0058] For example, see Figure 3 , assuming that the position information of the end joint at multiple acquisition moments are: ; As a travel group, is the vertical length, is the length of the line, the middle point The curvature is . Accordingly, it can be determined based on this method The curvature of each midpoint in each stroke group.

[0059] Optionally, the bending attribute may be curvature, and the bending attribute of the intermediate position information in the current travel group is determined based on at least three position information in the current travel group, including: determining the second line based on the first position information and the intermediate position information in the current travel group; determining the third line based on the intermediate position information and the last position information in the current travel group; determining the curvature of the intermediate position information based on the angle between the second line and the third line.

[0060] In this embodiment, the first position information and the middle position information in the current trip group can be connected to obtain the connection line between the two position points as the second connection line. The middle position information and the last position information in the current trip group can be connected to obtain the connection line between the two position points as the third connection line. Further, the angle between the second connection line and the third connection line can be determined, and the angle can be used as the curvature of the middle position information. For example, Representation vector and The angle of Representative The middle point in the trip group For example, see Figure 3 , The middle point in the trip group The curvature is and Angle .

[0061] S130. For each bending attribute, when it is determined that the bending attribute satisfies a preset condition, determine target position information based on the intermediate position information when the preset condition is satisfied.

[0062] It should be noted that when the amount of joint data collected from the hand robot is too large, it will lead to too much position information of the terminal joints, which will increase the computing memory and waste resources. In order to reduce the amount of calculation while ensuring the accuracy of path planning, it is possible to determine whether to use the intermediate position information as the final target position information based on the bending property of the intermediate position information. The preset condition refers to the condition for determining whether the intermediate position information corresponding to the bending property is used as the final target position information. For example, the preset condition may be that the bending property is greater than a preset threshold. Optionally, the preset condition includes that the curvature in the bending property is greater than a curvature threshold, and / or that the curvature in the bending property is greater than a curvature threshold.

[0063] In this embodiment, when the curvature attribute of the intermediate position information in the current trip group is greater than a preset threshold, the path passing through the intermediate position information is considered to be too curvy, and the intermediate position information that meets the preset condition at this time can be used as the target position information. When the curvature attribute of the intermediate position information in the current trip group is not greater than the preset threshold, the intermediate position information is filtered. In other words, the intermediate position information that meets the preset condition can be retained as the target position information, and the intermediate position information that does not meet the preset condition can be discarded.

[0064] In order to further improve the extraction accuracy of the target position information, the preset threshold in the preset condition can be determined by using an adaptive threshold adjustment method, so as to reduce redundant feature points while maximizing the accuracy of trajectory recovery.

[0065] Optionally, before determining that the curvature attribute satisfies a preset condition, the path planning method further includes: determining a first length mean based on the length of the connecting line corresponding to each travel group and the number of travel groups; determining a second length mean based on the length of the vertical line corresponding to each travel group and the number of travel groups; determining a curvature threshold based on the first length mean and the second length mean; accordingly, determining that the curvature attribute satisfies the preset condition includes: determining that the curvature attribute satisfies the preset condition when the curvature in the curvature attribute is greater than the curvature threshold.

[0066] In this embodiment, the lengths of the connecting lines determined based on the first position information and the last position information in each travel group may be summed to obtain a sum value, and then the sum value and the number of travel groups may be divided to obtain a first length mean. The lengths of the perpendicular lines from the intermediate position information in each travel group to the corresponding first connecting line may be summed to obtain a sum value, and then the sum value and the number of travel groups may be divided to obtain a second length mean. The first length mean and the second length mean may be divided to obtain a curvature threshold.

[0067] For example, the length of the connection between all interval points can be calculated based on formula (1): The mean (i.e. the first length mean), calculate the length of all vertical lines based on formula (2) The mean (i.e., the second length mean), the curvature threshold is calculated based on formula (3) .

[0068] Among them, formula (1) can be expressed as: (1); Indicates the number of groups.

[0069] Formula (2) can be expressed as: (2).

[0070] Formula (3) can be expressed as: (3).

[0071] Furthermore, the curvature in the curvature attribute of the intermediate position information in the current stroke group may be greater than the curvature threshold. , it is determined that the bending property meets the preset conditions.

[0072] Optionally, before determining that the bending attribute meets the preset condition, the path planning method also includes: when the bending attribute includes curvature, determining a curvature threshold based on the curvature corresponding to the intermediate position information in each stroke group and the number of stroke groups; accordingly, determining that the bending attribute meets the preset condition includes: when the curvature in the bending attribute is greater than the curvature threshold, determining that the bending attribute meets the preset condition.

[0073] In this embodiment, the curvature corresponding to the intermediate position information in each trip group is summed to obtain a sum value, and then the sum value is divided by the number of trip groups to obtain a curvature threshold. In this way, when the curvature in the curvature attribute of the intermediate position information in the current trip group is greater than the curvature threshold, it is determined that the curvature attribute meets the preset condition. Exemplarily, the curvature threshold can be calculated based on formula (4): . Formula (4) can be expressed as: (4).

[0074] See also Figure 4 , we can traverse each intermediate position information of the trip group, calculate the curvature and curvature of each intermediate position information, and calculate the curvature threshold and curvature threshold. If the curvature of the intermediate position information is greater than the curvature threshold (i.e. ), and / or the curvature is greater than the curvature threshold (i.e. ), then the intermediate position information is retained as the target position information, otherwise the intermediate position information is not retained, and the intermediate position information of the next trip group is directly traversed until the intermediate position information of all trip groups is determined to be completed, that is, the traversal is completed. The first position information and the last position information of the position information at multiple acquisition times and each target position information can be used as the standard position information of the standard hand action to obtain a standard position information set , is the number of standard position information. When the hand robot simulates the user's standard action, the stored standard position information can be retrieved to restore the trajectory of the hand robot's action.

[0075] S140, determining a target driving path corresponding to the hand robot based on the first position information and the last position information in the position information at multiple acquisition moments, each target position information, and a preset cubic spline curve.

[0076] In this embodiment, the hand motion trajectory curve can be generated based on each target position information, the first position information and the last position information in the position information at multiple acquisition moments, in combination with the sequential timing relationship of the acquisition corresponding to the position information. Furthermore, a preset cubic spline curve can be used to perform spline interpolation processing in three-dimensional space on the hand motion trajectory curve, restore the standard motion trajectory, and obtain the target driving path corresponding to the hand robot, so as to control the hand robot to move according to the target driving path, effectively solve the problem of missing position information, improve the accuracy of trajectory recovery, and meet the accuracy requirements of the hand robot.

[0077] For example, see Figure 2 , in the storage standard location information collection Afterwards, the standard position information set can be read, and the standard motion trajectory can be restored according to the standard position information to control the movement of the hand robot until the movement of the hand robot is completed.

[0078] The technical solution provided in this embodiment determines the position information of the end joint of the hand robot at multiple acquisition moments, and regards the position information of at least three adjacent acquisition moments as a travel group; determines the bending property of the middle position information in the current travel group based on at least three position information in the current travel group; determines the target position information based on the middle position information when the preset condition is met when the bending property is determined to meet the preset condition; determines the target driving path corresponding to the hand robot based on the first position information and the last position information in the position information at multiple acquisition moments, each target position information and a preset cubic spline curve, thereby solving the problem of low path planning accuracy caused by point-to-point trajectory planning based on joint space in the prior art. The invention realizes that by regarding the position information of at least three adjacent acquisition moments of the end joint of the hand robot as a travel group, and then, according to at least three position information in the current travel group, determines the bending property of the middle position information in the current travel group, and by judging whether the bending property meets the preset condition, determines whether the middle position information is the target position information, reduces the control calculation amount, and ensures the path planning accuracy. Furthermore, based on the first position information and the last position information in the position information at multiple acquisition moments, each target position information and a preset cubic spline curve, cubic spline interpolation processing is implemented to improve the smoothness of the target driving path planning, thereby ensuring the technical effect of the smoothness of the hand robot's movement.

[0079] Embodiment 2

[0080] Figure 5It is a flow chart of a path planning method provided according to the second embodiment of the present invention. On the basis of the above embodiment, when determining the target driving path corresponding to the hand robot, multiple travel intervals can be determined based on each target position information, the first position information and the last position information in the position information of multiple acquisition moments. Then, based on the current travel interval and the preset cubic spline curve, the piecewise cubic spline function corresponding to the current travel interval is determined, and each travel interval and its corresponding piecewise cubic spline function are combined to determine the target driving path corresponding to the hand robot. The specific implementation method can refer to the technical solution of this embodiment. Among them, the technical terms that are the same as or corresponding to the above embodiment are not repeated here.

[0081] like Figure 5 As shown, the method specifically comprises the following steps:

[0082] S210 , determining a plurality of travel intervals based on each target location information, the first location information and the last location information among the location information at a plurality of collection moments.

[0083] The travel interval includes location information of at least two adjacent collection moments.

[0084] In this embodiment, the first position information, all target position information, and the last position information in the position information at multiple collection moments may be arranged in order, and each position information may be divided to obtain multiple travel intervals.

[0085] For example, Figure 6 As shown, The position information loaded when the hand action needs to be realized is the small interval composed of every two position information. It can be considered as a travel interval, and each travel interval corresponds to a piecewise cubic spline function. Get the parameterized representation of its cubic spline , complete the cubic spline interpolation in three-dimensional space.

[0086] S220 . For each travel interval, based on the current travel interval and a preset cubic spline curve, determine a piecewise cubic spline function corresponding to the current travel interval.

[0087] It should be noted that the method for determining the piecewise cubic spline function corresponding to each current travel interval is the same, and any of the travel intervals can be taken as the current travel interval to determine the piecewise cubic spline function corresponding to the current travel interval as an example for introduction. The piecewise cubic spline function is a parameterized representation of the cubic spline curve, that is, after assigning values ​​to the parameters of the cubic spline curve, the piecewise cubic spline function can be obtained. In other words, the piecewise cubic spline function is a function after assigning values ​​to the parameters of the cubic spline curve.

[0088] It can be understood that: according to the preset cubic spline curve, a piecewise cubic spline function corresponding to the parameterized representation of the current travel interval can be determined. Assume that the current travel interval is the i-th travel interval , determine the preset cubic spline curve used by the piecewise cubic spline function corresponding to the i-th travel interval It can be expressed as:

[0089] ;

[0090] in, is the sampling step length, Between 0 and 1; the preset cubic spline curve set used by the piecewise cubic spline function corresponding to each travel interval for: ; All are position coefficients. When there are n travel intervals, there are n preset cubic spline curves, each of which contains 12 unknown position coefficients, and a total of 12n unknown position coefficients. After obtaining these position coefficients, the piecewise cubic spline function corresponding to each travel interval can be obtained, and finally the target driving path can be obtained according to the piecewise cubic spline function.

[0091] In this embodiment, the specific position coefficients in the preset cubic spline curve are obtained as follows:

[0092] Step (1): Since the left and right position information in each travel interval is known, the left and right position information in the travel interval is the position information of the two endpoints of the preset cubic spline curve corresponding to the travel interval. That is, the position information of the two endpoints of each preset cubic spline curve is known. For example, the position information of the two endpoints is and ,Right now:

[0093] ;

[0094] ; for The three-dimensional coordinates of for The three-dimensional coordinates of In When the left endpoint of the preset cubic spline curve is =0, Indicates the sampling step length in the i-th travel interval =0 when the preset cubic spline curve; in the travel range In When the left endpoint of the preset cubic spline curve is =1, Indicates the sampling step length in the i-th travel interval =1 when the default cubic spline curve is used.

[0095] Further, , Substitute the preset cubic spline curves respectively In this paper, we can get formula (5) and formula (6):

[0096] Formula (5) is expressed as: (5);

[0097] Formula (6) is expressed as: (6);

[0098] By expanding formula (5) and formula (6), we can obtain 6 equations corresponding to the i-th preset cubic spline curve. The 6 equations are:

[0099]

[0100] Each preset cubic spline curve corresponds to 6 equations, a total of A preset cubic spline curve can be obtained Equation.

[0101] Step (2): Considering that the curve of the entire target driving path needs to be smooth, that is, it can be differentiated everywhere, so each connection point of the preset cubic spline curve can be differentiated, and we have:

[0102] , for The derivative of for The derivative of

[0103] , for The derivative of for The derivative of .

[0104] right Taking a derivative, we can get formula (7):

[0105] Formula (7) is expressed as: (7);

[0106] right Taking the second derivative, we can get formula (8):

[0107] Formula (8) is expressed as: (8);

[0108] Depend on It is known that = 1 is substituted into formula (7), =0, i=i+1 are substituted into formula (7) to obtain formula (9):

[0109] Formula (9) is expressed as: (9);

[0110] Depend on It is known that Substituting = 1 into formula (8), =0, i=i+1 are substituted into formula (8) to obtain formula (10);

[0111] Formula (10) is expressed as: (10);

[0112] By expanding formula (9) and formula (10), we can obtain 6 equations corresponding to the connection of two preset cubic spline curves. The 6 equations are:

[0113]

[0114] The preset cubic spline curves include n, and accordingly, the curves of the entire target driving path include curve connecting points, we can get Equation.

[0115] Step (3): For the two endpoints of the curve of the entire target driving path, free endpoints can be used, and: ; . Then, =0, i=0 is substituted into formula (7), and formula (11) is obtained; =1, i=n-1 are substituted into formula (7) to obtain formula (12);

[0116] Formula (11) is expressed as: (11);

[0117] Formula (12) is expressed as: (12);

[0118] Expanding formula (11) and formula (12), we can obtain 6 equations, which are:

[0119]

[0120] Combining the above steps (1), (2), and (3), in step (1), we can get The equation is as follows, where :

[0121]

[0122] Step (2) yields The equation is as follows, where :

[0123]

[0124] In step (3), we can get the following 6 equations:

[0125]

[0126] total equation, and The number of unknown position coefficients in is the same, and the cubic spline curve can be solved The position coefficient of the equation is obtained to obtain the piecewise cubic spline function corresponding to the i-th travel interval.

[0127] S230: Determine a target driving path corresponding to the hand robot based on each travel interval and its corresponding piecewise cubic spline function.

[0128] In this embodiment, the current travel interval can be interpolated according to the piecewise cubic spline function corresponding to the current travel interval to obtain a target travel interval corresponding to the current travel interval. Then, based on the position information in each target travel interval, the target driving path corresponding to the hand robot is determined.

[0129] The technical solution of this embodiment determines multiple travel intervals based on each target position information, the first position information and the last position information in the position information at multiple acquisition moments, and then determines a piecewise cubic spline function corresponding to the current travel interval based on the current travel interval and a preset cubic spline curve; interpolation is performed in the travel interval through the piecewise cubic spline function to obtain a target driving path corresponding to the hand robot, thereby improving the smoothness of the target driving path and achieving the technical effect of ensuring the smoothness of the hand robot's movement.

[0130] Embodiment 3

[0131] Figure 7is a schematic diagram of the structure of a path planning device provided according to Embodiment 3 of the present invention. Figure 7 As shown, the device includes: a trip group determination module 310, a curve attribute determination module 320, a target position information determination module 330 and a target driving path determination module 340.

[0132] Among them, the travel group determination module 310 is used to determine the position information of the end joint of the hand robot at multiple collection moments, and take the position information of at least three adjacent collection moments as a travel group; the bending attribute determination module 320 is used to determine the bending attribute of the intermediate position information in the current travel group for each of the travel groups based on at least three of the position information in the current travel group; the target position information determination module 330 is used to determine the target position information for each of the bending attributes based on the intermediate position information when the preset condition is met when it is determined that the bending attribute meets the preset condition; wherein the preset condition includes that the curvature in the bending attribute is greater than the curvature threshold, and / or that the curvature in the bending attribute is greater than the curvature threshold; the target driving path determination module 340 is used to determine the target driving path corresponding to the hand robot based on the first position information and the last position information in the position information at the multiple collection moments, each of the target position information, and a preset cubic spline curve.

[0133] The technical solution of this embodiment determines the position information of the end joint of the hand robot at multiple acquisition moments, and regards the position information of at least three adjacent acquisition moments as a travel group; determines the bending property of the middle position information in the current travel group based on at least three position information in the current travel group; determines the target position information based on the middle position information when the preset condition is met when the bending property is determined to meet the preset condition; determines the target driving path corresponding to the hand robot based on the first position information and the last position information in the position information at multiple acquisition moments, each target position information and a preset cubic spline curve, thereby solving the problem of low path planning accuracy caused by point-to-point trajectory planning based on joint space in the prior art. The invention realizes that by regarding the position information of at least three adjacent acquisition moments of the end joint of the hand robot as a travel group, and then, according to at least three position information in the current travel group, determines the bending property of the middle position information in the current travel group, and by judging whether the bending property meets the preset condition, determines whether the middle position information is the target position information, reduces the control calculation amount, and ensures the path planning accuracy. Furthermore, based on the first position information and the last position information in the position information at multiple acquisition moments, each target position information and a preset cubic spline curve, cubic spline interpolation processing is implemented to improve the smoothness of the target driving path planning, thereby ensuring the technical effect of the smoothness of the hand robot's movement.

[0134] Based on the above device, optionally, the trip group determination module 310 includes:

[0135] A joint parameter determination unit, used to obtain joint parameters of at least two joints in the hand robot at multiple acquisition moments during the process of the first object controlling the movement of the hand robot; wherein the joint parameters include connecting rod length, connecting rod rotation angle, connecting rod offset and joint angle;

[0136] A transformation matrix determination unit, configured to determine, for each of the acquisition moments, a transformation matrix from a preceding joint to a succeeding joint in the adjacent joints based on joint parameters of the adjacent joints at the current acquisition moment;

[0137] The position information determining unit is used to determine the position information of the end joint of the hand robot at the current acquisition moment based on the transformation matrices corresponding to all the adjacent joints.

[0138] Based on the above device, optionally, the bending attribute includes curvature, and the bending attribute determination module 320 includes:

[0139] A line length determining unit, for determining a line length corresponding to the current trip group based on a first line between the first position information and the last position information in the current trip group;

[0140] a vertical line length determining unit, configured to determine a vertical line length corresponding to the current stroke group based on the intermediate position information in the current stroke group and the first connecting line;

[0141] The curvature determination unit is used to determine the curvature of the intermediate position information based on the length of the connecting line and the length of the vertical line.

[0142] On the basis of the above device, optionally, the device further includes:

[0143] A first length mean value determining unit, configured to determine a first length mean value based on the connection length corresponding to each travel group and the number of the travel groups;

[0144] A second length mean value determining unit, configured to determine a second length mean value based on the vertical line length corresponding to each stroke group and the number of the stroke groups;

[0145] a curvature threshold determination unit, configured to determine a curvature threshold based on the first mean length and the second mean length;

[0146] On the basis of the above device, optionally, the target position information determination module 330 is used to determine that the bending attribute meets a preset condition when the curvature in the bending attribute is greater than the curvature threshold.

[0147] Based on the above device, optionally, the bending attribute includes curvature, and the bending attribute determination module 320 includes:

[0148] A second connection line determination unit, used to determine a second connection line based on the first position information and the intermediate position information in the current trip group;

[0149] The third connection line is determined based on the middle position information and the last position information in the current trip group;

[0150] The curvature determining unit is used to determine the curvature of the intermediate position information based on the angle between the second connecting line and the third connecting line.

[0151] On the basis of the above device, optionally, the device further includes:

[0152] a curvature threshold determination unit, configured to determine a curvature threshold based on the curvature corresponding to the intermediate position information in each of the travel groups and the number of the travel groups when the bending attribute includes curvature;

[0153] Based on the above device, optionally, the target position information determination module 330 is used to determine that the bending attribute meets a preset condition when the curvature in the bending attribute is greater than the curvature threshold.

[0154] Based on the above device, optionally, the target driving path determination module 340 includes:

[0155] A travel interval determination unit, configured to determine a plurality of travel intervals based on each of the target location information, the first location information and the last location information of the location information at the plurality of collection moments; wherein the travel interval includes location information of at least two adjacent collection moments;

[0156] A piecewise cubic spline function determining unit, configured to determine, for each of the travel intervals, a piecewise cubic spline function corresponding to the current travel interval based on the current travel interval and a preset cubic spline curve;

[0157] The target driving path determination unit is used to determine the target driving path corresponding to the hand robot based on each of the travel intervals and the corresponding piecewise cubic spline function.

[0158] The path planning device provided in the embodiment of the present invention can execute the path planning method provided in any embodiment of the present invention, and has the corresponding functional modules and beneficial effects of the execution method.

[0159] Embodiment 4

[0160] Figure 8 is a schematic diagram of the structure of a hand robot that implements the path planning method of an embodiment of the present invention. The hand robot is intended to represent various forms of digital computers, such as laptop computers, desktop computers, workbenches, personal digital assistants, servers, blade servers, mainframe computers, and other suitable computers. The hand robot can also represent various forms of mobile devices, such as personal digital processing, cellular phones, smart phones, wearable devices (such as helmets, glasses, watches, etc.) and other similar computing devices. The components shown herein, their connections and relationships, and their functions are merely examples and are not intended to limit the implementation of the present invention described and / or claimed herein.

[0161] like Figure 8 As shown, the hand robot 10 includes at least one processor 11, and a memory connected to the at least one processor 11, such as a read-only memory 12, a random access memory 13, etc., wherein the memory stores a computer program that can be executed by at least one processor, and the processor 11 can perform various appropriate actions and processes according to the computer program stored in the read-only memory 12 or the computer program loaded from the storage unit 18 to the random access memory 13. In the random access memory 13, various programs and data required for the operation of the hand robot 10 can also be stored. The processor 11, the read-only memory 12, and the random access memory 13 are connected to each other through a bus 14. An input / output (I / O) interface 15 is also connected to the bus 14.

[0162] A number of components in the hand robot 10 are connected to the I / O interface 15, including: an input unit 16, such as a keyboard, a mouse, etc.; an output unit 17, such as various types of displays, speakers, etc.; a storage unit 18, such as a disk, an optical disk, etc.; and a communication unit 19, such as a network card, a modem, a wireless communication transceiver, etc. The communication unit 19 allows the hand robot 10 to exchange information / data with other devices through a computer network such as the Internet and / or various telecommunication networks.

[0163] The processor 11 may be a variety of general and / or special processing components with processing and computing capabilities. Some examples of the processor 11 include, but are not limited to, a central processing unit (CPU), a graphics processing unit (GPU), various dedicated artificial intelligence (AI) computing chips, various processors running machine learning model algorithms, a digital signal processor (DSP), and any appropriate processor, controller, microcontroller, etc. The processor 11 executes the various methods and processes described above, such as a path planning method.

[0164] In some embodiments, the path planning method may be implemented as a computer program, which is tangibly contained in a computer-readable storage medium, such as a storage unit 18. In some embodiments, part or all of the computer program may be loaded and / or installed on the hand robot 10 via the read-only memory 12 and / or the communication unit 19. When the computer program is loaded into the random access memory 13 and executed by the processor 11, one or more steps of the path planning method described above may be performed. Alternatively, in other embodiments, the processor 11 may be configured to execute the path planning method in any other appropriate manner (e.g., by means of firmware).

[0165] Various embodiments of the systems and techniques described above herein may be implemented in digital electronic circuit systems, integrated circuit systems, field programmable gate arrays (FPGAs), application-specific integrated circuits (ASICs), application-specific standard products (ASSPs), systems on chips (SOCs), complex programmable logic devices (CPLDs), computer hardware, firmware, software, and / or combinations thereof. These various embodiments may include: being implemented in one or more computer programs that can be executed and / or interpreted on a programmable system including at least one programmable processor, which may be a dedicated or general-purpose programmable processor that can receive data and instructions from a storage system, at least one input device, and at least one output device, and transmit data and instructions to the storage system, the at least one input device, and the at least one output device.

[0166] Computer programs for implementing the methods of the present invention may be written in any combination of one or more programming languages. These computer programs may be provided to a processor of a general-purpose computer, a special-purpose computer, or other programmable data processing device, so that when the computer program is executed by the processor, the functions / operations specified in the flow chart and / or block diagram are implemented. The computer program may be executed entirely on the machine, partially on the machine, partially on the machine and partially on a remote machine as a stand-alone software package, or entirely on a remote machine or server.

[0167] In the context of the present invention, a computer-readable storage medium may be a tangible medium that may contain or store a computer program for use by or in conjunction with an instruction execution system, device, or equipment. A computer-readable storage medium may include, but is not limited to, electronic, magnetic, optical, electromagnetic, infrared, or semiconductor systems, devices, or equipment, or any suitable combination of the foregoing. Alternatively, a computer-readable storage medium may be a machine-readable signal medium. A more specific example of a machine-readable storage medium may include an electrical connection based on one or more lines, a portable computer disk, a hard disk, a random access memory, a read-only memory, an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.

[0168] To provide interaction with a user, the systems and techniques described herein may be implemented on a hand robot having: a display device (e.g., a CRT (cathode ray tube) or LCD (liquid crystal display) monitor) for displaying information to the user; and a keyboard and pointing device (e.g., a mouse or trackball) through which the user can provide input to the hand robot. Other types of devices may also be used to provide interaction with the user; for example, the feedback provided to the user may be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback); and input from the user may be received in any form (including acoustic input, voice input, or tactile input).

[0169] The systems and techniques described herein may be implemented in a computing system that includes backend components (e.g., as a data server), or a computing system that includes middleware components (e.g., an application server), or a computing system that includes frontend components (e.g., a user computer with a graphical user interface or a web browser through which a user can interact with implementations of the systems and techniques described herein), or a computing system that includes any combination of such backend components, middleware components, or frontend components. The components of the system may be interconnected by any form or medium of digital data communication (e.g., a communication network). Examples of communication networks include: a local area network (LAN), a wide area network (WAN), a blockchain network, and the Internet.

[0170] A computing system may include a client and a server. The client and the server are generally remote from each other and usually interact through a communication network. The client and server relationship is generated by computer programs running on the corresponding computers and having a client-server relationship with each other. The server may be a cloud server, also known as a cloud computing server or cloud host, which is a host product in the cloud computing service system to solve the defects of difficult management and weak business scalability in traditional physical hosts and VPS services.

[0171] An embodiment of the present invention further provides a computer program product, including a computer program, which, when executed by a processor, implements the path planning method provided by any embodiment of the present invention.

[0172] In the process of implementation, the computer program product can be written in one or more programming languages ​​or a combination thereof to perform the computer program code of the present invention, including object-oriented programming languages, such as Java, Smalltalk, C++, and conventional procedural programming languages, such as "C" language or similar programming languages. The program code can be executed entirely on the user's computer, partially on the user's computer, as an independent software package, partially on the user's computer and partially on a remote computer, or completely on a remote computer or server. In the case of a remote computer, the remote computer can be connected to the user's computer through any type of network, including a local area network (LAN) or a wide area network (WAN), or can be connected to an external computer (for example, using an Internet service provider to connect through the Internet).

[0173] It should be understood that the various forms of processes shown above can be used to reorder, add or delete steps. For example, the steps described in the present invention can be executed in parallel, sequentially or in different orders, as long as the desired results of the technical solution of the present invention can be achieved, and this document does not limit this.

[0174] The above specific implementations do not constitute a limitation on the protection scope of the present invention. It should be understood by those skilled in the art that various modifications, combinations, sub-combinations and substitutions can be made according to design requirements and other factors. Any modification, equivalent substitution and improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims

1. A path planning method, characterized in that: Applied to a hand robot, the method comprises: Determine the position information of the end joint of the hand robot at multiple acquisition moments, and regard the position information of at least three adjacent acquisition moments as a travel group; For each of the travel groups, based on at least three pieces of position information in the current travel group, determining a bending attribute of the middle position information in the current travel group; For each of the bending attributes, when it is determined that the bending attribute satisfies a preset condition, the target position information is determined based on the intermediate position information when the preset condition is satisfied; wherein the preset condition includes that the curvature in the bending attribute is greater than a curvature threshold, and / or that the curvature in the bending attribute is greater than a curvature threshold; Based on the first position information and the last position information of the position information at the multiple acquisition moments, each target position information, and a preset cubic spline curve, a target driving path corresponding to the hand robot is determined.

2. The path planning method according to claim 1, characterized in that: The step of determining the position information of the end joints of the hand robot at multiple acquisition moments includes: In the process of the first object controlling the movement of the hand robot, obtaining joint parameters of at least two joints in the hand robot at multiple acquisition moments; wherein the joint parameters include connecting rod length, connecting rod rotation angle, connecting rod offset and joint angle; For each of the acquisition moments, based on the joint parameters of the adjacent joints at the current acquisition moment, determining a transformation matrix from a previous joint to a next joint in the adjacent joints; Based on the transformation matrices corresponding to all the adjacent joints, the position information of the end joints of the hand robot at the current acquisition moment is determined.

3. The path planning method according to claim 1, characterized in that: The curvature attribute includes curvature, and determining the curvature attribute of the middle position information in the current stroke group based on at least three pieces of position information in the current stroke group includes: Determine, based on a first connection line between the first position information and the last position information in the current travel group, a connection line length corresponding to the current travel group; Determining the length of a vertical line corresponding to the current stroke group based on the middle position information in the current stroke group and the first connecting line; The curvature of the intermediate position information is determined based on the length of the connecting line and the length of the perpendicular line.

4. The path planning method according to claim 3, characterized in that: Before determining that the bending property meets the preset condition, the method further includes: Determine a first length mean based on the connection length corresponding to each travel group and the number of the travel groups; Determine a second length mean based on the vertical line length corresponding to each stroke group and the number of the stroke groups; Determining a curvature threshold based on the first length mean and the second length mean; Accordingly, determining that the bending attribute satisfies a preset condition includes: If the curvature in the curvature attribute is greater than the curvature threshold, it is determined that the curvature attribute meets a preset condition.

5. The path planning method according to claim 1, characterized in that: The bending attribute includes curvature, and determining the bending attribute of the middle position information in the current stroke group based on at least three pieces of position information in the current stroke group includes: Determine a second connection line based on the first position information and the middle position information in the current trip group; Determining a third connection line based on the middle position information and the last position information in the current trip group; The curvature of the intermediate position information is determined based on the angle between the second connecting line and the third connecting line.

6. The path planning method according to claim 5, characterized in that: Before determining that the bending attribute satisfies a preset condition, the method further includes: When the bending attribute includes curvature, determining a curvature threshold based on the curvature corresponding to the intermediate position information in each of the travel groups and the number of the travel groups; Accordingly, determining that the bending attribute satisfies a preset condition includes: When the curvature in the bending attribute is greater than the curvature threshold, it is determined that the bending attribute meets a preset condition.

7. The path planning method according to claim 1, characterized in that: The method of determining a target driving path corresponding to the hand robot based on the first position information and the last position information of the position information at the plurality of acquisition moments, each target position information, and a preset cubic spline curve comprises: Determine multiple travel intervals based on each of the target location information, the first location information and the last location information among the location information at the multiple collection moments; wherein the travel interval includes location information of at least two adjacent collection moments; For each of the travel intervals, based on the current travel interval and a preset cubic spline curve, determining a piecewise cubic spline function corresponding to the current travel interval; Based on each of the travel intervals and the corresponding piecewise cubic spline function, a target driving path corresponding to the hand robot is determined.

8. A path planning device, characterized in that: Deployed in hand robots, including: A travel group determination module, used to determine the position information of the end joint of the hand robot at multiple acquisition moments, and to take the position information of at least three adjacent acquisition moments as a travel group; a bending attribute determination module, configured to determine, for each of the trip groups, a bending attribute of the middle position information in the current trip group based on at least three pieces of position information in the current trip group; a target position information determination module, configured to determine, for each of the bending attributes, target position information based on the intermediate position information when the preset condition is met when it is determined that the bending attribute satisfies a preset condition; wherein the preset condition includes that the curvature in the bending attribute is greater than a curvature threshold, and / or that the curvature in the bending attribute is greater than a curvature threshold; The target driving path determination module is used to determine the target driving path corresponding to the hand robot based on the first position information and the last position information in the position information at the multiple acquisition moments, each target position information, and a preset cubic spline curve.

9. A hand robot, characterized in that: The hand robot comprises: at least one processor; and a memory communicatively connected to the at least one processor; wherein, The memory stores a computer program executable by the at least one processor, and the computer program is executed by the at least one processor so that the at least one processor can execute the path planning method according to any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer instructions, and the computer instructions are used to enable a processor to implement the path planning method according to any one of claims 1 to 7 when executed.

Images