Disclosure of Invention
The invention aims to provide a pose control method, a pose control device, a storage medium and a controller, so as to improve the safety of pose control.
To achieve the above object, a first part of embodiments of the present disclosure provides a pose control method applied to a guide frame including a moving platform and a driving device for driving pose change of the moving platform, the method including:
determining the current pose of the movable platform;
determining a target control amount for controlling the driving device under the condition that the current pose of the movable platform is different from a target pose;
and driving the driving device according to the target control amount so as to enable the movable platform to reach the target pose from the current pose.
Optionally, the guide frame further comprises a universal joint, the universal joint comprising a sphere;
the current pose comprises a current pose matrix, and the determining the current pose of the movable platform comprises:
determining a first coordinate of the sphere under a coordinate system of a driving device;
determining a second parameter coordinate of the sphere under a moving platform coordinate system;
determining a target node according to the partial connection relation between the driving device and the movable platform;
constructing a conversion matrix between the driving device coordinate system and the moving platform coordinate system according to the rotation angle parameters of the target nodes, wherein the conversion matrix comprises a trigonometric function of the rotation angle parameters of each target node;
determining the rotation angle corresponding to the rotation angle parameter according to the relation among the first coordinate, the second parameter coordinate and the conversion matrix;
and determining the current gesture matrix according to the rotation angle of the target node.
Optionally, the driving device comprises a base and a first driving assembly, the first driving assembly comprises a first end connecting rod and two first linear drivers, and the ball body is fixedly connected with the first end connecting rod;
The determining a first coordinate of the sphere in a drive device coordinate system includes:
constructing a first plane reference triangle according to a first hinging position at which the two first linear drivers are respectively hinged with the base and a second hinging position at which the two first linear drivers are hinged through the first tail end connecting rod;
determining second hinge coordinates of the second hinge position according to first hinge coordinates of two first hinge positions in the first plane reference triangle and the current length of each first linear driver;
and determining the first coordinate according to the second hinge coordinate, the distance between the sphere and the second hinge position and the size of an included angle between a left first linear driver and the first end connecting rod, wherein the left first linear driver is one of the two first linear drivers, and the first linear driver is fixedly connected with the first end connecting rod.
Optionally, the universal joint further comprises a ball sleeve, the ball sleeve is fixedly connected to a first end of a connecting rod of the movable platform, a second end of the connecting rod is rotatably connected to a first connecting position of a mounting plate of the movable platform, and an output end of a second driving assembly of the driving device is rotatably connected to a second connecting position of the mounting plate; the method further comprises the steps of:
Constructing the moving platform coordinate system by taking the second connecting position as an origin;
the determining the second parameter coordinates of the sphere under the moving platform coordinate system comprises the following steps:
determining a second planar reference triangle from the first connection location, the second connection location, and the sphere;
and determining the second parameter coordinates according to the geometric relationship of the second plane reference triangle.
Optionally, the second driving assembly comprises a second end link, a connecting piece and two second linear drivers, a first end of the connecting piece is rotatably connected to the second end link (421), and an output end of the second driving assembly represents a second end of the connecting piece;
the determining a target node according to the partial connection relationship between the driving device and the movable platform includes:
determining a third articulation position at which the two second linear drives are articulated by the second end link as a first target node;
determining a first connection location of the first end of the connector to the second end link as a second target node;
and determining a second connection position of the output end of the second driving assembly and the mounting plate as a third target node.
Optionally, the constructing a transformation matrix between the driving device coordinate system and the moving platform coordinate system according to the rotation angle parameter of the target node includes:
respectively constructing a coordinate system by taking the first target node, the second target node and the third target node as origins to obtain a serial coordinate system;
and constructing the transformation matrix according to the serial coordinate system.
Optionally, the determining the target control amount for controlling the driving device when the current pose of the moving platform is different from the target pose includes:
determining a second target coordinate of the sphere under the driving device coordinate system according to a target posture matrix in the target pose and a first target coordinate of the sphere under the moving platform coordinate system;
determining a first target length of the left first linear drive according to the second target coordinates, the two first hinged coordinates and the size of an included angle between the left first linear drive and the first end connecting rod;
and determining a first target control amount of the left first linear driver according to the current length of the left first linear driver and the first target length.
Optionally, the determining the target control amount for controlling the driving device when the current pose of the moving platform is different from the target pose includes:
determining a second target length of the right first linear drive from the first target length of the left first linear drive, the two first articulation coordinates, and a distance between the sphere and the second articulation position;
and determining a second target control amount of the right first linear driver according to the current length of the right first linear driver and the second target length.
Optionally, the universal joint comprises a connecting column fixedly connected to the sphere, the sphere is connected to the first end connecting rod through the connecting column, the ball sleeve of the universal joint comprises a containing groove for containing the sphere and an opening for the connecting column to penetrate, and the edge of the opening forms a limiting part capable of being abutted against the connecting column;
before determining the current pose matrix according to the rotation angle of the target node, the method further comprises:
judging whether an included angle between the current axis of the sphere and the reference axis of the sphere is smaller than a preset threshold value or not based on the rotation angle of the target node;
The determining the current gesture matrix according to the rotation angle of the target node includes:
and under the condition that the included angle is smaller than the preset threshold value, determining the current gesture matrix according to the rotation angle of the target node.
A second part of the embodiments of the present disclosure provides a pose control device applied to a guide frame including a movable platform and a driving device for driving pose change of the movable platform, the device including:
the first determining module is used for determining the current pose of the movable platform;
the second determining module is used for determining a target control quantity for controlling the driving device under the condition that the current pose of the movable platform is different from a target pose;
and the driving module is used for driving the driving device according to the target control quantity so as to enable the movable platform to reach the target pose from the current pose.
Optionally, the guide frame further comprises a universal joint, the universal joint comprising a sphere; the current pose comprises a current pose matrix, and the first determining module comprises:
a first determining submodule for determining a first coordinate of the sphere in a coordinate system of a driving device;
The second determining submodule is used for determining second parameter coordinates of the sphere under a moving platform coordinate system;
the third determining submodule is used for determining a target node according to the partial connection relation between the driving device and the movable platform;
the construction submodule is used for constructing a conversion matrix between the driving device coordinate system and the movable platform coordinate system according to the rotation angle parameters of the target nodes, wherein the conversion matrix comprises trigonometric functions of the rotation angle parameters of the target nodes;
a fourth determining submodule, configured to determine a rotation angle corresponding to the rotation angle parameter according to the relation among the first coordinate, the second parameter coordinate, and the transformation matrix;
and a fifth determining submodule, configured to determine the current pose matrix according to the rotation angle of the target node.
Optionally, the driving device comprises a base and a first driving assembly, the first driving assembly comprises a first end connecting rod and two first linear drivers, and the ball body is fixedly connected with the first end connecting rod;
the first determination submodule is used for: constructing a first plane reference triangle according to a first hinging position at which the two first linear drivers are respectively hinged with the base and a second hinging position at which the two first linear drivers are hinged through the first tail end connecting rod; determining second hinge coordinates of the second hinge position according to first hinge coordinates of two first hinge positions in the first plane reference triangle and the current length of each first linear driver; and determining the first coordinate according to the second hinge coordinate, the distance between the sphere and the second hinge position and the size of an included angle between a left first linear driver and the first end connecting rod, wherein the left first linear driver is one of the two first linear drivers, and the first linear driver is fixedly connected with the first end connecting rod.
Optionally, the universal joint further comprises a ball sleeve, the ball sleeve is fixedly connected to a first end of a connecting rod of the movable platform, a second end of the connecting rod is rotatably connected to a first connecting position of a mounting plate of the movable platform, and an output end of a second driving assembly of the driving device is rotatably connected to a second connecting position of the mounting plate; the apparatus further comprises:
the construction module is used for constructing the moving platform coordinate system by taking the second connecting position as an origin;
the second determination submodule is used for: determining a second planar reference triangle from the first connection location, the second connection location, and the sphere; and determining the second parameter coordinates according to the geometric relationship of the second plane reference triangle.
Optionally, the second driving assembly includes a second end link, a connecting member, and two second linear drivers, a first end of the connecting member is rotatably connected to the second end link, and an output end of the second driving assembly characterizes a second end of the connecting member;
the third determination submodule is configured to: determining a third articulation position at which the two second linear drives are articulated by the second end link as a first target node; determining a first connection location of the first end of the connector to the second end link as a second target node; and determining a second connection position of the output end of the second driving assembly and the mounting plate as a third target node.
Optionally, the constructing sub-module is configured to: respectively constructing a coordinate system by taking the first target node, the second target node and the third target node as origins to obtain a serial coordinate system; and constructing the transformation matrix according to the serial coordinate system.
Optionally, the second determining module includes:
the first execution module is used for determining a second target coordinate of the sphere under the driving device coordinate system according to a target posture matrix in the target pose and a first target coordinate of the sphere under the moving platform coordinate system;
the second execution module is used for determining a first target length of the left first linear driver according to the second target coordinates, the two first hinged coordinates and the size of an included angle between the left first linear driver and the first tail end connecting rod;
and the third execution module is used for determining a first target control quantity of the left first linear driver according to the current length of the left first linear driver and the first target length.
Optionally, the second determining module includes:
a fourth execution module for determining a second target length of the right first linear drive based on the first target length of the left first linear drive, the two first articulation coordinates, and a distance between the sphere and the second articulation position;
And the fifth execution module is used for determining a second target control quantity of the right first linear driver according to the current length of the right first linear driver and the second target length.
Optionally, the universal joint comprises a connecting column fixedly connected to the sphere, the sphere is connected to the first end connecting rod through the connecting column, the ball sleeve of the universal joint comprises a containing groove for containing the sphere and an opening for the connecting column to penetrate, and the edge of the opening forms a limiting part capable of being abutted against the connecting column;
the apparatus further comprises: the judging module is used for judging whether the included angle between the current axis of the sphere and the reference axis of the sphere is smaller than a preset threshold value or not based on the rotation angle of the target node before the current gesture matrix is determined according to the rotation angle of the target node;
the fifth determination submodule is configured to: and under the condition that the included angle is smaller than the preset threshold value, determining the current gesture matrix according to the rotation angle of the target node.
A third part of an embodiment of the present disclosure provides a non-transitory computer-readable storage medium having stored thereon a computer program which, when executed by a processor, implements the steps of the method of any of the first parts.
A fourth part of the embodiments of the present disclosure provides a controller, comprising:
a memory having a computer program stored thereon;
a processor for executing the computer program in the memory to implement the steps of the method of any of the first parts.
By adopting the technical scheme, at least the following beneficial technical effects can be achieved:
the method comprises the steps of determining the current pose of a movable platform of a guide frame, and determining a target control amount for controlling a driving device of the guide frame under the condition that the current pose of the movable platform is different from a target pose. The driving device is driven according to the target control quantity, so that the movable platform of the guide frame reaches the target pose from the current pose. Compared with the mode of controlling the guide frame to move to the target position by the active driving of the mechanical arm in the related art, the mode of controlling the pose of the moving platform on the guide frame by the active driving of the guide frame is safer because the active driving range of the guide frame is smaller and collision is not easy to be caused in the driving process.
Additional features and advantages of the present disclosure will be set forth in the detailed description which follows.
Detailed Description
Specific embodiments of the present disclosure are described in detail below with reference to the accompanying drawings. It should be understood that the detailed description and specific examples, while indicating and illustrating the disclosure, are not intended to limit the disclosure.
In the present disclosure, unless otherwise indicated, terms such as "inner" and "outer" are used to refer to the outline of the corresponding parts themselves, and furthermore, the terms "first", "second", etc. are used in the present disclosure to distinguish one element from another without order or importance. When the following description refers to the accompanying drawings, the same numbers in different drawings refer to the same or similar elements, unless otherwise indicated.
It should be noted that, all actions for acquiring signals, information or data in the present disclosure are performed under the condition of conforming to the corresponding data protection rule policy of the country of the location and obtaining the authorization given by the owner of the corresponding device.
In order to facilitate easier understanding of the technical solutions of the present disclosure by those skilled in the art, an example of a guide frame to which the pose control method of the present disclosure may be applied is described below. Referring to fig. 1-7, the guide frame includes a base 1, a movable platform 2, and a drive assembly 4.
The movable platform 2 includes a mounting plate 210 and a tracker, the mounting plate 210 is provided with a positioning portion for positioning a surgical instrument, the tracker is disposed on the mounting plate 210 for the navigation system to identify and acquire the positioning portion and a position of the mounting plate 210 in the navigation system, for example, a three-dimensional model of the movable platform 2 may be pre-led into the navigation system, and the tracker is used for being identified by a navigation camera of the navigation system to construct an image and a posture of the three-dimensional model of the movable platform in the navigation system to display an image of a real-time position of the positioning portion in a display of the navigation system.
The base 1 can directly or indirectly drive the movable platform 2 to move through the driving assembly 4 so that the positioning part moves to the target position.
Thus, the precise movement and positioning of the positioning portion can be achieved through the active control and guidance of the driving assembly 4, so that the operation process can be conveniently carried out, and compared with the complex active driving structure of the mechanical arm, the guide frame with the active control function provided by the present disclosure is obviously lower in cost.
In some embodiments, referring to fig. 1, the movable platform 2 further includes a link 220, a first end of the link 220 being rotatably connected to the first connection location 211 of the mounting plate 210 about a first pivot axis. The guide frame further comprises a universal joint 3, wherein the universal joint 3 comprises a sphere 310 and a ball sleeve 320 movably sleeved on the sphere 310, and the ball sleeve 320 is fixedly connected to a second end, opposite to the first end, of the connecting rod 220. The driving component 4 is disposed on the base 1 and is respectively connected with the ball 310 and the second connecting position 212 of the mounting plate 210, the first connecting position 211, the second connecting position 212 and the ball 310 are respectively located at three endpoints of the first imaginary triangle, and the driving component 4 is used for driving the ball 310 to move towards a first position relative to the base 1 and/or driving the second connecting position 212 to move towards a second position relative to the base 1, so as to indirectly adjust the spatial position of the movable platform 2 and enable the positioning part to move to a target position.
The driving assembly is connected to the movable platform through a universal joint and a connecting rod, so that the adjustment of multiple postures of the movable platform can be realized, and the positioning part can be positioned to the target position accurately.
The driving assembly 4 may be configured in any suitable manner according to actual design requirements, for example, referring to fig. 1 to 3, the driving assembly 4 may include a first driving assembly 410 and a second driving assembly 420, the ball 310 being fixedly connected to an output end of the first driving assembly 410, and an output end of the second driving assembly 420 being rotatably connected to the second connection location 212 about a second pivot axis parallel to the first pivot axis. Thus, spatial position transformations of ball 310 and second connection site 212 may be driven by first drive assembly 410 and second drive assembly 420, respectively, to achieve precise control of the spatial positions of ball 310 and second connection site 212.
In some embodiments, referring to fig. 4, the first driving assembly 410 may include a first end connecting rod 411 and two first linear drivers 412, wherein two fixed ends of the two first linear drivers 412 are respectively hinged to the base 1, the driving end of one first linear driver 412 is fixedly connected to the first end connecting rod 411, and the driving end of the other first linear driver 412 is hinged to the first end connecting rod 411, so that the first end connecting rod 411 moves in a first plane where the two first linear drivers 412 are located, and the sphere 310 is fixedly connected to the first end connecting rod 411.
As shown in fig. 4 and 5, the driving end of the other first linear actuator 412 may be hinged to the first end connecting rod 411 through the second connecting shaft 413, so that the two fixing ends of the two first linear actuators 412 and the second connecting shaft may form three ends of the second imaginary triangle in the first plane. Therefore, since the distance between the two fixed ends is fixed and known, by controlling the telescopic length of the two first linear drivers 412, the position transformation of the sphere 310 in the first plane can be precisely controlled, so as to drive the adjustment of the position and the posture of the movable platform 2.
In some embodiments, referring to fig. 3 and 7, the second driving assembly 420 may include a second end connecting rod 421, a connecting piece 422, and two second linear drivers 423, wherein two fixed ends of the two second linear drivers 423 are respectively hinged to the base 1, a driving end of one second linear driver 423 is fixedly connected to the second end connecting rod 421, and a driving end of the other second linear driver 423 is hinged to the second end connecting rod 421, so that the second end connecting rod 421 moves in a second plane where the two second linear drivers 423 are located, one end of the connecting piece 422 is rotatably connected to the second end connecting rod 421 about a third pivot axis parallel to the second plane, and the other end of the connecting piece 422 is an output end of the second driving assembly 420.
Wherein, the driving end of the other second linear actuator 423 may be hinged to the second end connecting rod 421 by, for example, a first connecting shaft 424, and one end of the connecting member 422 may be rotatably connected to the second end connecting rod 421 about a third pivot axis parallel to the second plane, and the other end of the connecting member 423 may be rotatably connected to the second connecting position about the second pivot axis, so that three joint positions having three degrees of freedom may be formed at the first connecting shaft 424, at the connection between the connecting member 423 and the second end connecting rod 421, and at the connection between the connecting member 423 and the second connecting position 212, to achieve multi-pose adjustment of the movable platform 2, prevention of jamming, and the like. Further, the two fixed ends of the two second linear drivers 423 and the first connecting shaft 424 may form three end points of a third imaginary triangle in the second plane. Thus, since the distance between the two fixed ends is fixed and known, by controlling the telescopic lengths of the two second linear drivers 423, the position change of the first connecting shaft 424 or the second end connecting rod 421 in the second plane can be precisely controlled, so as to drive the adjustment of the position and the posture of the movable platform 2.
In some embodiments, referring to fig. 7, the connection member 422 may include a U-shaped plate 4221 and a rotation shaft 4222, where the U-shaped plate 4221 includes two first plates disposed opposite to each other and a second plate connected between the two first plates, the two first plates are rotatably connected to the mounting plate 210 about a second pivot axis through a second pivot shaft 4223, one end of the rotation shaft 4222 is fixedly connected to the second end link 421, and the other end is rotatably connected to the second plate about a third pivot axis. Wherein, the mounting plate 210 may include a connection plate 213, and the second connection position 212 may be a first through hole formed in the connection plate 213, and when the connection member 422 is mounted, the connection plate 213 may be partially inserted into the opening of the U-shaped plate 4221, and then penetrates through the two first plate bodies and the second connection position 212 through the second pivot shaft 4223.
In addition, referring to fig. 4, the link 220 may include a mounting base 221 and two third plates 222 spaced apart from each other on the mounting base 221, the two third plates 222 being rotatably connected to the mounting plate 210 about a first pivot axis by a first pivot shaft 223, the mounting base 221 being provided with a mounting groove for fixedly connecting the ball socket 320. Specifically, the first connection location 211 may be a second through hole formed on the connection plate 213, and the connection plate 213 may be partially inserted between the two third plates 222 at the time of installation, and then penetrates the two third plates 222 and the first connection location through the first pivot shaft 223.
In some embodiments, referring to fig. 5 and 6, the universal joint 3 may include a connection post 330 fixedly connected to the ball 310, the ball 310 is connected to the driving assembly 4 through the connection post 330, the ball socket 320 includes a receiving groove for receiving the ball 310 and an opening 321 through which the connection post 330 passes, and an edge of the opening 321 forms a limit portion capable of abutting against the connection post 330. In this way, the range of the swing angle of the ball cover 320 relative to the ball 310 can be limited by the edge of the opening 321, for example, the range of the swing angle of the ball cover 320 relative to the ball 310 can be limited between 0 ° and 25 ° according to practical application requirements.
In some embodiments, the first plane in which the two first linear drivers 412 lie is parallel to the second plane in which the two second linear drivers 423 lie. Therefore, the calculation process of the driving component for driving the pose change of the movable platform can be simplified.
Further, the first linear actuator 412 and/or the second linear actuator 423 may be an electric push rod, a telescopic cylinder, or the like. The stroke of the linear driver can be accurately controlled through the electric push rod, so that the telescopic length of each linear driver can be controlled. When a telescopic cylinder, such as a hydraulic cylinder or an air cylinder, is adopted, in order to realize the control of the telescopic length, a displacement sensor, such as a magnetic grid sensor, can also be arranged on the base so as to monitor and feed back the stroke of the telescopic cylinder in real time.
In some embodiments, the driving stroke of the first linear driver 412 may be greater than the driving stroke of the second linear driver 423 to achieve more flexible pose adjustment of the motion platform.
In some embodiments, the base 1 is provided with a limiting frame 110, where the limiting frame 110 has a strip hole 111 extending parallel to the first plane or the second plane, and the strip hole 111 is used for passing through the two first linear drivers 412 or the two second linear drivers 423 to limit the two first linear drivers 412 or the two second linear drivers 423 to move along a direction inclined to the first plane or the second plane.
Fig. 2 shows an exemplary embodiment of the stop block 110 with a bar-shaped hole 111 for two first linear drives 412 to pass through, in order to limit the two first linear drives 412 to a first plane.
Alternatively, the base 1 may be provided with a limiting hole 120, where the limiting hole 120 is used for the two first linear drivers 412 or the two second linear drivers 423 to penetrate, so as to limit the two first linear drivers 412 or the two second linear drivers 423 to move along a direction inclined to the first plane or the second plane.
Fig. 2 exemplarily shows an embodiment in which a limiting hole 120 is provided in the base 1 through which the two second linear drivers 423 are inserted, so as to limit the two second linear drivers 423 to the second plane.
In some embodiments, referring to fig. 2, the base 1 may be constructed as a hollow housing, the limiting hole 120 is formed on a sidewall of the base 1, two second linear drivers 423 are partially located inside the hollow housing and the driving end extends outside the hollow housing to be connected with the second end connecting rod 421, the limiting frame 110 is disposed on an outer wall of the hollow housing, and two first linear drivers 412 are disposed on an outer wall of the hollow housing and penetrate the bar-shaped hole 111. By this arrangement, the two second linear drives 423 and the two first linear drives 412 can be separated to facilitate assembly and maintenance of the guide frame.
Furthermore, as shown with reference to fig. 1, a protective housing is provided on the base 1, which is arranged around two of said first linear drives 412.
The pose control method of the present disclosure is described in detail below with reference to one example of a guide frame shown in fig. 1-7.
Fig. 8 is a flowchart illustrating a pose control method according to an exemplary embodiment of the present disclosure. The pose control method is applied to a guide frame, and the guide frame comprises a movable platform and a driving device for driving pose change of the movable platform. As shown in fig. 8, the pose control method includes the steps of:
s11, determining the current pose of the movable platform;
s12, determining a target control amount for controlling the driving device under the condition that the current pose of the movable platform is different from a target pose;
and S13, driving the driving device according to the target control quantity so as to enable the movable platform to reach the target pose from the current pose.
In some embodiments, the mobile platform may be used to carry surgical instruments. Thus, by the pose control method, the pose of the surgical instrument can be controlled by controlling the pose of the movable platform, and the aim of robot assisted surgery is fulfilled.
In other embodiments, a guide hole, such as a guide hole for guiding the puncture path, may be provided on the movable platform. Thus, through the pose control method disclosed by the invention, the pose control of the guide hole can be realized by controlling the pose of the movable platform, and the aim of robot assisted surgery is further realized.
By adopting the method, the target control quantity of the driving device for controlling the guide frame is determined by determining the current pose of the movable platform of the guide frame and under the condition that the current pose of the movable platform is different from the target pose. The driving device is driven according to the target control quantity, so that the movable platform of the guide frame reaches the target pose from the current pose. Compared with the mode of controlling the guide frame to move to the target position by the active driving of the mechanical arm in the related art, the mode of controlling the pose of the moving platform on the guide frame by the active driving of the guide frame is safer because the active driving range of the guide frame is smaller and collision is not easy to be caused in the driving process.
Since this approach of the present disclosure achieves the goal of the guide frame being actively driven to control the pose of the mobile platform, it should be noted that the guide frame of the present disclosure may be assembled not only on the mechanical arm for use, but also on other mechanical bases. The present disclosure is not particularly limited thereto.
Optionally, the guide frame further comprises a universal joint, the universal joint comprising a sphere; the current pose comprises a current pose matrix, and the determining the current pose of the movable platform comprises:
determining a first coordinate of the sphere under a coordinate system of a driving device; determining a second parameter coordinate of the sphere under a moving platform coordinate system; determining a target node according to the partial connection relation between the driving device and the movable platform; constructing a conversion matrix between the driving device coordinate system and the moving platform coordinate system according to the rotation angle parameters of the target nodes, wherein the conversion matrix comprises a trigonometric function of the rotation angle parameters of each target node; determining the rotation angle corresponding to the rotation angle parameter according to the relation among the first coordinate, the second parameter coordinate and the conversion matrix; and determining the current gesture matrix according to the rotation angle of the target node.
One embodiment of determining a first coordinate of a sphere in a coordinate system of a driving device is that the driving device comprises a base and a first driving assembly, wherein the first driving assembly comprises a first tail end connecting rod and two first linear drivers, and the sphere is fixedly connected with the first tail end connecting rod; the determining a first coordinate of the sphere in a drive device coordinate system includes:
Constructing a first plane reference triangle according to a first hinging position at which the two first linear drivers are respectively hinged with the base and a second hinging position at which the two first linear drivers are hinged through the first tail end connecting rod; determining second hinge coordinates of the second hinge position according to first hinge coordinates of two first hinge positions in the first plane reference triangle and the current length of each first linear driver; and determining the first coordinate according to the second hinge coordinate, the distance between the sphere and the second hinge position and the size of an included angle between a left first linear driver and the first end connecting rod, wherein the left first linear driver is one of the two first linear drivers, and the first linear driver is fixedly connected with the first end connecting rod.
According to the connection relation and the connection mode between the components, the first hinging positions of the two first linear drivers, which are respectively hinged with the base, are known coordinates of the first hinging coordinates of the two first hinging positions under the coordinate system of the driving device. The current length of each first linear drive is a known length. The distance between the sphere and the second hinge position is a known distance. The angle between the left first linear actuator and the first end link is of a known angle magnitude, such as 160.
The first planar reference triangle is constructed from two first articulation positions and two second articulation positions where the first linear drives are articulated by the first end links. Illustratively, as shown in fig. 9, a first planar reference triangle (A1, B1, C1) is constructed with two first hinge positions A1 and B1, and a second hinge position C1.
The second hinge coordinates of the second hinge position in the driving device coordinate system can be determined through geometrical operation of the triangle according to the first hinge coordinates of the two first hinge positions in the first plane reference triangle and the current length of each first linear driver.
It should be noted here that knowing the triangle three-side length and two point coordinates, the third vertex coordinate, such as the C1 point coordinate in fig. 9, can be found. The principle derivation procedure is as follows:
first, three coordinate systems xoy, x ' o ' y ', x "o" y "are created with reference to fig. 10.
Next, it can be seen from fig. 10 that the coordinate of the C point with respect to the coordinate system composed of x 'oy' is (acosθ, asinθ).
Then, solving the orthonormal basis of the x 'oy' coordinate system to obtain a linear change matrix T. The solution 1 is as follows: determining a unit vector of xLet b→ ＝(m,n)，a→ ⊥b→ Then a→ *b→ =0, then we can deriveWherein (1) >The value of m is chosen according to the situation, namely +.>The linear variation matrix T is->
Solution 2: the rotation matrix according to the rotation angle alpha isIf alpha is 0 deg., thenWherein->Then->
Assuming that the translation vector to the xoy coordinate system is A, the C point coordinate under the xoy coordinate system is obtained according to T
With continued reference to fig. 9, the first coordinate of the sphere in the drive coordinate system may be determined based on the resulting second articulation coordinate C1, the distance between the sphere (e.g., D1 in fig. 9) and the second articulation position, and the magnitude of the angle between the left first linear actuator and the first end link of 160 °. For example, referring to FIG. 11, the first coordinate of the sphere (e.g., D in FIG. 11) in the drive mechanism coordinate system isWherein (1)>
Optionally, the universal joint further comprises a ball sleeve, the ball sleeve is fixedly connected to a first end of a connecting rod of the movable platform, a second end of the connecting rod is rotatably connected to a first connecting position of a mounting plate of the movable platform, and an output end of a second driving assembly of the driving device is rotatably connected to a second connecting position of the mounting plate; the method further comprises the steps of:
and constructing the moving platform coordinate system by taking the second connecting position as an origin.
Referring to fig. 12, a movable platform coordinate system X3Y3Z3 is constructed with the second connection position (point C in fig. 12) as the origin O3.
The determining the second parameter coordinates of the sphere under the moving platform coordinate system comprises the following steps:
determining a second planar reference triangle from the first connection location, the second connection location, and the sphere; and determining the second parameter coordinates according to the geometric relationship of the second plane reference triangle.
Referring to fig. 13, a second plane reference triangle (Pt 3, pt2, pt 1) is determined according to the first connection bit Pt3, the second connection bit Pt2, and the sphere Pt1, and a second parameter coordinate is determined according to the geometric relationship of the second plane reference triangle
Optionally, the second driving assembly includes a second end link, a connecting member, and two second linear drivers, a first end of the connecting member is rotatably connected to the second end link, and an output end of the second driving assembly characterizes a second end of the connecting member;
the determining a target node according to the partial connection relationship between the driving device and the movable platform includes:
determining a third articulation position at which the two second linear drives are articulated by the second end link as a first target node; determining a first connection location of the first end of the connector to the second end link as a second target node; and determining a second connection position of the output end of the second driving assembly and the mounting plate as a third target node.
Optionally, the constructing a transformation matrix between the driving device coordinate system and the moving platform coordinate system according to the rotation angle parameter of the target node includes: respectively constructing a coordinate system by taking the first target node, the second target node and the third target node as origins to obtain a serial coordinate system; and constructing the transformation matrix according to the serial coordinate system.
Since the left side linear driver is fixedly connected with the end connecting rod and the right side linear driver is hinged with the left side linear driver through the end connecting rod in each group of driving components, the length change of the linear driver can cause the angle change of the included angle at the hinged position of the linear driver, and therefore, in the embodiment of the disclosure, the first target node is used as a rotation axis (x 1, y1, z 1) to participate in the calculation of the coordinate transformation process.
For example, referring to fig. 14, the coordinate system with the first target node as the origin is the coordinate system x0y0z0 in fig. 14, the coordinate system with the second target node as the origin is the coordinate system x2y2z2 in fig. 14, and the coordinate system with the third target node as the origin is the coordinate system x3y3z3 in fig. 14.
In one embodiment, referring to fig. 12 and 14, the driving device coordinate system, that is, the xyz coordinate system, the rotation axis coordinate system x1y1z1 in fig. 12, the coordinate system x0y0z0 constructed with the first target node as the origin, the coordinate system x2y2z2 constructed with the second target node as the origin, and the coordinate system x3y3z3 constructed with the third target node as the origin are mapped to the coordinate system as shown in fig. 15 with the view angle of the plane in which the second driving element is located to the perpendicular direction of the plane in which the first driving element is located.
According to the serial coordinate system shown in fig. 15, the transformation matrix T between the driving device coordinate system and the moving platform coordinate system can be constructed by the following procedure.
θ1 、θ2 、θ3 The rotation angle of the first target node, the rotation angle of the second target node, and the rotation angle of the third target node, respectively, t=tx×t1×t2×t3,
then
According to the first coordinatesSecond parameter coordinate->Conversion matrixThe relation between the two is T x p=d, and the rotation angle parameter pair is determinedThe corresponding rotation angle. The specific process is as follows:
from t=p=d, getAccording to->ObtainingIs provided with->According to sin theta32 +cosθ32 =1, the range of values of θ3 is +.>Instead of cos θ3, sin θ3 may be set to x. Then->According to the root formula->Calculating x, and taking the value of +.>Will beBy taking the formula (1), θ can be obtained2 ，θ2 The initial value of (2) is 90. θ1 From the length and included angle of the two sides of the triangle, it is known that theta1 =90-20- θ=70- θ, wherein +.>
To be found theta1 、θ2 、θ3 Carry-inAnd obtaining a conversion matrix.
It should be noted that, the translation vector between the driving device coordinate system and the moving platform coordinate system may be obtained by solving the third plane reference triangle shown in fig. 16, and the solving process is similar to that of the first plane reference triangle, which is not repeated herein.
Optionally, the determining the target control amount for controlling the driving device when the current pose of the moving platform is different from the target pose includes:
determining a second target coordinate of the sphere under the driving device coordinate system according to a target posture matrix in the target pose and a first target coordinate of the sphere under the moving platform coordinate system; determining a first target length of the left first linear drive according to the second target coordinates, the two first hinged coordinates and the size of an included angle between the left first linear drive and the first end connecting rod; and determining a first target control amount of the left first linear driver according to the current length of the left first linear driver and the first target length.
Illustratively, the target pose matrix T (3*3) and the position vector (3*1) V satisfy the equationWhere x represents the distance between the plane of the first drive assembly and the plane of the second drive assembly, which is a known value, such as 36 units of length. Can get->The corresponding geometry of this equation is shown in fig. 17. The values of m and n can be known by solving the equation, and thus the second target coordinates of the sphere in the drive device coordinate system.
Further, referring to fig. 18, the first target length of the left first linear actuator may be determined based on the second target coordinates, the two first articulation coordinates, and the angle between the left first linear actuator and the first end link, e.g., 160 °. The calculation principle is as follows:
referring to FIG. 18, knowing the e, d lengths and a-d angles of 160, the triangle is solved to obtainI.e. the first target length of the left hand first linear drive. The first target control amount of the left first linear driver may be determined based on the current length of the left first linear driver and the first target length thereof.
Optionally, the determining the target control amount for controlling the driving device when the current pose of the moving platform is different from the target pose includes:
determining a second target length of the right first linear drive from the first target length of the left first linear drive, the two first articulation coordinates, and a distance between the sphere and the second articulation position; and determining a second target control amount of the right first linear driver according to the current length of the right first linear driver and the second target length.
Illustratively, with continued reference to FIG. 18, it can be appreciated thatIs provided withThen->According to sin theta2 +cosθ2 =1, and cosθ is set as x, and x is obtained by sorting2 -2kcosβ·x+k2 -s2 ＝0，/>I.e. the second target length of the right first linear drive. A second target control amount of the right first linear drive is determined based on the current length of the right first linear drive and its second target length.
Optionally, the universal joint comprises a connecting column fixedly connected to the sphere, the sphere is connected to the first end connecting rod through the connecting column, the ball sleeve of the universal joint comprises a containing groove for containing the sphere and an opening for the connecting column to penetrate, and the edge of the opening forms a limiting part capable of being abutted against the connecting column;
before determining the current pose matrix according to the rotation angle of the target node, the method further comprises:
and judging whether an included angle between the current axis of the sphere and the reference axis of the sphere is smaller than a preset threshold or not based on the rotation angle of the target node. Wherein the reference axis is the symmetry axis of the connecting column shown in fig. 6.
The determining the current gesture matrix according to the rotation angle of the target node includes: and under the condition that the included angle is smaller than the preset threshold value, determining the current gesture matrix according to the rotation angle of the target node. Wherein the preset threshold is an empirical value of 25 deg..
The pose control method fully embodies and integrates the displacement and the angle deviation of each guide frame node caused by the motion process, and can meet the expected high-precision requirement of the kinematic pose through strict parameter calibration. On one hand, the limiting conditions of the mechanical movement range of the joint can be conveniently controlled in advance, for example, the included angle between the current axis of the control sphere and the reference axis of the sphere is smaller than or equal to 25 degrees, so that the problems of collision, occlusion and even damage to mechanical and electrical components of the equipment caused by the fact that the joint rotation angle calculated by any length of the linear driver in forward kinematics exceeds the mechanical rotation critical value of the device, such as the 25-degree critical value, are avoided, and on the other hand, the data processing process is simplified, and the multi-solution condition is avoided.
Fig. 19 is a block diagram illustrating a pose control apparatus according to an exemplary embodiment of the present disclosure, which is applied to a guide frame including a movable platform and a driving device for driving pose changes of the movable platform. As shown in fig. 19, the apparatus includes:
a first determining module 1901, configured to determine a current pose of the mobile platform;
a second determining module 1902, configured to determine a target control amount for controlling the driving device when the current pose of the moving platform is different from a target pose;
And a driving module 1903, configured to drive the driving device according to the target control amount, so that the moving platform reaches the target pose from the current pose.
By adopting the device, the target control quantity of the driving device for controlling the guide frame is determined by determining the current pose of the movable platform of the guide frame and determining the target control quantity under the condition that the current pose of the movable platform is different from the target pose. The driving device is driven according to the target control quantity, so that the movable platform of the guide frame reaches the target pose from the current pose. Compared with the mode of controlling the guide frame to move to the target position by the active driving of the mechanical arm in the related art, the mode of controlling the pose of the moving platform on the guide frame by the active driving of the guide frame is safer because the active driving range of the guide frame is smaller and collision is not easy to be caused in the driving process.
Optionally, the guide frame further comprises a universal joint, the universal joint comprising a sphere; the current pose includes a current pose matrix, and the first determining module 1901 includes:
a first determining submodule for determining a first coordinate of the sphere in a coordinate system of a driving device;
The second determining submodule is used for determining second parameter coordinates of the sphere under a moving platform coordinate system;
the third determining submodule is used for determining a target node according to the partial connection relation between the driving device and the movable platform;
the construction submodule is used for constructing a conversion matrix between the driving device coordinate system and the movable platform coordinate system according to the rotation angle parameters of the target nodes, wherein the conversion matrix comprises trigonometric functions of the rotation angle parameters of the target nodes;
a fourth determining submodule, configured to determine a rotation angle corresponding to the rotation angle parameter according to the relation among the first coordinate, the second parameter coordinate, and the transformation matrix;
and a fifth determining submodule, configured to determine the current pose matrix according to the rotation angle of the target node.
Optionally, the driving device comprises a base and a first driving assembly, the first driving assembly comprises a first end connecting rod and two first linear drivers, and the ball body is fixedly connected with the first end connecting rod;
the first determination submodule is used for: constructing a first plane reference triangle according to a first hinging position at which the two first linear drivers are respectively hinged with the base and a second hinging position at which the two first linear drivers are hinged through the first tail end connecting rod; determining second hinge coordinates of the second hinge position according to first hinge coordinates of two first hinge positions in the first plane reference triangle and the current length of each first linear driver; and determining the first coordinate according to the second hinge coordinate, the distance between the sphere and the second hinge position and the size of an included angle between a left first linear driver and the first end connecting rod, wherein the left first linear driver is one of the two first linear drivers, and the first linear driver is fixedly connected with the first end connecting rod.
Optionally, the universal joint further comprises a ball sleeve, the ball sleeve is fixedly connected to a first end of a connecting rod of the movable platform, a second end of the connecting rod is rotatably connected to a first connecting position of a mounting plate of the movable platform, and an output end of a second driving assembly of the driving device is rotatably connected to a second connecting position of the mounting plate; the apparatus further comprises:
the construction module is used for constructing the moving platform coordinate system by taking the second connecting position as an origin;
the second determination submodule is used for: determining a second planar reference triangle from the first connection location, the second connection location, and the sphere; and determining the second parameter coordinates according to the geometric relationship of the second plane reference triangle.
Optionally, the second driving assembly includes a second end link, a connecting member, and two second linear drivers, a first end of the connecting member is rotatably connected to the second end link, and an output end of the second driving assembly characterizes a second end of the connecting member;
the third determination submodule is configured to: determining a third articulation position at which the two second linear drives are articulated by the second end link as a first target node; determining a first connection location of the first end of the connector to the second end link as a second target node; and determining a second connection position of the output end of the second driving assembly and the mounting plate as a third target node.
Optionally, the constructing sub-module is configured to: respectively constructing a coordinate system by taking the first target node, the second target node and the third target node as origins to obtain a serial coordinate system; and constructing the transformation matrix according to the serial coordinate system.
Optionally, the second determining module 1902 includes:
the first execution module is used for determining a second target coordinate of the sphere under the driving device coordinate system according to a target posture matrix in the target pose and a first target coordinate of the sphere under the moving platform coordinate system;
the second execution module is used for determining a first target length of the left first linear driver according to the second target coordinates, the two first hinged coordinates and the size of an included angle between the left first linear driver and the first tail end connecting rod;
and the third execution module is used for determining a first target control quantity of the left first linear driver according to the current length of the left first linear driver and the first target length.
Optionally, the second determining module 1902 includes:
a fourth execution module for determining a second target length of the right first linear drive based on the first target length of the left first linear drive, the two first articulation coordinates, and a distance between the sphere and the second articulation position;
And the fifth execution module is used for determining a second target control quantity of the right first linear driver according to the current length of the right first linear driver and the second target length.
Optionally, the universal joint comprises a connecting column fixedly connected to the sphere, the sphere is connected to the first end connecting rod through the connecting column, the ball sleeve of the universal joint comprises a containing groove for containing the sphere and an opening for the connecting column to penetrate, and the edge of the opening forms a limiting part capable of being abutted against the connecting column;
the apparatus further comprises: the judging module is used for judging whether the included angle between the current axis of the sphere and the reference axis of the sphere is smaller than a preset threshold value or not based on the rotation angle of the target node before the current gesture matrix is determined according to the rotation angle of the target node;
the fifth determination submodule is configured to: and under the condition that the included angle is smaller than the preset threshold value, determining the current gesture matrix according to the rotation angle of the target node.
The specific manner in which the various modules perform the operations in the apparatus of the above embodiments have been described in detail in connection with the embodiments of the method, and will not be described in detail herein.
Fig. 20 is a block diagram of a controller 700, according to an exemplary embodiment of the present disclosure. As shown in fig. 20, the controller 700 may include: a processor 701, a memory 702. The controller 700 may also include one or more of a multimedia component 703, an input/output (I/O) interface 704, and a communication component 705.
The processor 701 is configured to control the overall operation of the controller 700, so as to complete all or part of the steps in the above-mentioned pose control method. The memory 702 is used to store various types of data to support operation at the controller 700, which may include, for example, instructions for any application or method operating on the controller 700, as well as application-related data, such as contact data, messages sent and received, pictures, audio, video, and so forth. The Memory 702 may be implemented by any type or combination of volatile or non-volatile Memory devices, such as static random access Memory (Static Random Access Memory, SRAM for short), electrically erasable programmable Read-Only Memory (Electrically Erasable Programmable Read-Only Memory, EEPROM for short), erasable programmable Read-Only Memory (Erasable Programmable Read-Only Memory, EPROM for short), programmable Read-Only Memory (Programmable Read-Only Memory, PROM for short), read-Only Memory (ROM for short), magnetic Memory, flash Memory, magnetic disk, or optical disk. The multimedia component 703 can include a screen and an audio component. Wherein the screen may be, for example, a touch screen, the audio component being for outputting and/or inputting audio signals. For example, the audio component may include a microphone for receiving external audio signals. The received audio signals may be further stored in the memory 702 or transmitted through the communication component 705. The audio assembly further comprises at least one speaker for outputting audio signals. The I/O interface 704 provides an interface between the processor 701 and other interface modules, which may be a keyboard, mouse, buttons, etc. These buttons may be virtual buttons or physical buttons. The communication component 705 is used for wired or wireless communication between the controller 700 and other devices. Wireless communication, such as Wi-Fi, bluetooth, near field communication (Near Field Communication, NFC for short), 2G, 3G, 4G, NB-IOT, eMTC, or other 5G, etc., or one or a combination of more of them, is not limited herein. The corresponding communication component 705 may thus comprise: wi-Fi module, bluetooth module, NFC module, etc.
In an exemplary embodiment, the controller 700 may be implemented by one or more application specific integrated circuits (Application Specific Integrated Circuit, abbreviated ASIC), digital signal processor (Digital Signal Processor, abbreviated DSP), digital signal processing device (Digital Signal Processing Device, abbreviated DSPD), programmable logic device (Programmable Logic Device, abbreviated PLD), field programmable gate array (Field Programmable Gate Array, abbreviated FPGA), controller, microcontroller, microprocessor, or other electronic components for performing the above-described pose control method.
In another exemplary embodiment, a computer readable storage medium is also provided, comprising program instructions which, when executed by a processor, implement the steps of the above-described pose control method. For example, the computer readable storage medium may be the memory 702 including program instructions described above, which are executable by the processor 701 of the controller 700 to perform the pose control method described above.
In another exemplary embodiment, a computer program product is also provided, comprising a computer program executable by a programmable apparatus, the computer program having code portions for performing the above-described pose control method when executed by the programmable apparatus.
The preferred embodiments of the present disclosure have been described in detail above with reference to the accompanying drawings, but the present disclosure is not limited to the specific details of the above embodiments, and various simple modifications may be made to the technical solutions of the present disclosure within the scope of the technical concept of the present disclosure, and all the simple modifications belong to the protection scope of the present disclosure.
In addition, the specific features described in the above embodiments may be combined in any suitable manner without contradiction. The various possible combinations are not described further in this disclosure in order to avoid unnecessary repetition.
Moreover, any combination between the various embodiments of the present disclosure is possible as long as it does not depart from the spirit of the present disclosure, which should also be construed as the disclosure of the present disclosure.