Path planning auxiliary adjustment device and control method

Abstract

The present invention relates to a path planning auxiliary adjustment device and a control method, which comprises an intelligent tracking module, a target positioning module, a target tracker and an adjustable fixed bracket; the adjustable fixed bracket is composed of a motion planning module and an intelligent control module; the initial position of a target placement slot is located by a tracking device, the target surface before use is synchronized to the scene in use by alignment, the control parameters are obtained by the motion planning module, the target placement slot is automatically moved to a desired position, and a fast and accurate path planning auxiliary adjustment is realized.

Description

Technical Field

[0001] The present invention relates to the technical field of positioning and navigation, and in particular to a path planning auxiliary adjustment device and a control method. Background Art

[0002] With the development of sensing technology, control technology, mechanical processing and new materials, especially the rapid development of computer, network and image processing technology in recent years, the research on intelligent navigation of mobile robots has made great progress.

[0003] In the traditional hole making and drilling process, manual drilling is still the main method. The working environment of manual drilling is harsh, the work efficiency is low, and the drilling position is not accurate, which makes the later assembly inconvenient. Manual drilling takes a long time and is not easy to operate. In addition, when drilling with a robot, the drilling deviation will also occur due to the unevenness of the drilling surface. Summary of the invention

[0004] The purpose of the present invention is to overcome the defects of the prior art and provide a fast, accurate, short-time and easy-to-operate path planning auxiliary adjustment device.

[0005] The technical solution for achieving the purpose of the present invention is: a path planning auxiliary adjustment device, comprising an intelligent control module, a target positioning module, a tracker mounting bracket and a tracker; the tracker mounting bracket is fixedly mounted on the intelligent control module; there is at least one tracker mounting seat above the tracker mounting bracket; the tracker is installed at a tracker mounting point in front of or behind the tracker mounting seat; the target positioning module is connected to the intelligent control module through a motion mechanism.

[0006] The motion mechanism described in the above technical solution includes a lower rod, a telescopic rod and a rotating head; the lower rod is meshed with a rotating gear; the telescopic rod is plugged into the lower rod; the rotating head is installed at the upper end of the telescopic rod, and the upper end of the rotating head has a first bevel gear; the first bevel gear is meshed and transmitted with a second bevel gear fixed on the rotating rod; the two ends of the rotating rod are connected to the swing module through a fixed sleeve; the rotating gear is connected to the rotating motor.

[0007] The swing module described in the above technical solution includes a fixed head and a swing head; the fixed head is connected to the rotating rod through a fixed sleeve; and the swing head is connected to the fixed head through a swing cylinder.

[0008] A control method using the path planning auxiliary adjustment device according to claim 1, comprising the following steps:

[0009] S1. First, generate a 3D model of the target area for the DCOM image before use, then complete the planning of the target surface on the 3D model, and select more than three points as the feature points for target registration. Use the guide needle to select the corresponding feature points planned before use in the target area to complete the registration of the model before use to the real target in use.

[0010] S2, fixing the adjustable fixed bracket to the target position to be planned, and fixing the target positioning module to the target fixing block of the adjustable fixed bracket;

[0011] S3, mounting the target tracker on any tracker mounting base of the adjustable fixing bracket;

[0012] S4, the intelligent tracking module automatically tracks the target tracker, obtains the current position of the target placement slot, and sends it to the motion planning module;

[0013] S5, the motion planning module reads the target position and receives the current position sent by the intelligent tracking module, then plans the motion path from the current position to the target position, and generates motion control parameters and sends them to the intelligent control module;

[0014] S6. After receiving the motion control parameters, the intelligent control module controls the movement of the adjustable fixed bracket to adjust the target placement slot to the target position.

[0015] In the above technical solution, step S4 is specifically as follows: the camera at the top of the smart tracker determines the color and distribution matrix position of the tracker mounting base through image processing Obtain the installation position of the target tracker P to determine the transformation matrix from the target tracker to the target plane of the target positioning module Finally, the initial position information of the target placement slot is calculated in,

[0016]

[0017] In the above technical solution, in step S1, the tracker fixed on the target is used as the origin, and the target surface planned before is represented by the center point and three axis vectors, namely {centerPos, xVec, yVec, zVec} pre , the target surface in use can be obtained through the registration matrix:

[0018] In the above technical solution, the initial position of the target placement slot when the tracker fixed on the target is the origin in is the real-time position of the target tracker under the tracker, express The inverse matrix of .

[0019] In the above technical solution, step S6 is specifically as follows: the information position of the target plane is known, and the normal vector X of the plane where the target placement slot is located is adjusted. oste-i The normal vector X of the target plane when using intra-t The target placement slot is parallel and the center point of the surface where the target placement slot is located is on the target plane when in use. The motion planning of each control servo is obtained through the planning algorithm and executed by the intelligent control module. The target placement slot is moved to the desired position and posture by up and down, inside and outside, front and back tilt, and left and right rotation.

[0020] In the above technical solution, the planning algorithm is specifically:

[0021] (1) Calculate the normal vector X of the plane where the target placement slot is located oste-i Normal vector X of the target plane when used intra-t The transformation matrix

[0022] (2) According to the transformation matrix, the rotation parameters of the 3DOF control steering gear group are obtained;

[0023] (3) Calculate the center point position of the target placement slot plane after parallelization oste-r ;

[0024] (4) Continue along X oste-i From center oste-r Start from the projected distance on the target plane when in use, which is the distance the rod is moved up and down.

[0025] After adopting the above technical solution, the present invention has the following positive effects:

[0026] (1) The present invention locates the initial position of the target placement slot through a tracking device, synchronizes the target surface before use to the scene during use through registration, obtains control parameters through a motion planning module, and automatically moves the target placement slot to the desired position, thereby achieving fast and accurate path planning assistance. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] In order to make the content of the present invention more clearly understood, the present invention is further described in detail below according to specific embodiments and in conjunction with the accompanying drawings, wherein

[0028] Figure 1 This is a device structure diagram of Embodiment 1 of the present invention;

[0029] Figure 2 is a schematic diagram of a motion mechanism of embodiment 1 of the present invention;

[0030] Figure 3This is a schematic diagram of an application scenario of the present invention in medical surgery;

[0031] Figure 4 This is an application flow chart of Embodiment 1 of the present invention;

[0032] Figure 5 Calculating a conversion map for initial position information of the target placement slot of the present invention;

[0033] Figure 6 A schematic diagram of the pre-use planning and post-use registration of the present invention;

[0034] Figure 7 A schematic diagram of a target placement slot of the present invention with a tracker fixed on the target as the origin;

[0035] Figure 8 It is a schematic diagram of the planning of the present invention. DETAILED DESCRIPTION

[0036] (Example 1)

[0037] See Figure 1 and Figure 2 The present invention comprises an intelligent control module 1, a target positioning module 2, a tracker mounting bracket 3 and a tracker 4; the tracker mounting bracket 3 is fixedly mounted on the intelligent control module 1; there is at least one tracker mounting seat 5 above the tracker mounting bracket 3; the tracker 4 is mounted at a tracker mounting point 16 in front of or behind the tracker mounting seat 5; the target positioning module 2 is connected to the intelligent control module 1 through a motion mechanism 6.

[0038] The motion mechanism 6 includes a lower rod 7, a telescopic rod 8 and a rotating head 9; the lower rod 7 is meshed with a rotating gear 14; the telescopic rod 8 is plugged into the lower rod 7; the rotating head 9 is installed at the upper end of the telescopic rod 8, and the upper end of the rotating head 9 has a first bevel gear 10; the first bevel gear 10 is meshed and transmitted with a second bevel gear 12 fixed on the rotating rod 11; the two ends of the rotating rod 11 are connected to the swing module 15 through a fixing sleeve 13; the rotating gear 14 is connected to the rotating motor.

[0039] The target positioning module 2 is connected to the motion mechanism 6 via a lockable guide rail 17 .

[0040] (Example 2)

[0041] This embodiment is basically the same as the embodiment 1, except that the swing module 15 includes a fixed head and a swing head; the fixed head is connected to the rotating rod 11 through a fixed sleeve 13; and the swing head is connected to the fixed head through a swing cylinder.

[0042] See Figure 3 , this embodiment 1 or 2 can be applied to medical osteotomy surgery, and its process is referred to Figure 4 .

[0043] The control method of the first or second embodiment comprises the following steps:

[0044] S1. First, generate a 3D model of the target area for the DCOM image before use, then complete the planning of the target surface on the 3D model, and select more than three points as the feature points for target registration. Use the guide needle to select the corresponding feature points planned before use in the target area to complete the registration of the model before use to the real target in use.

[0045] S2, fixing the adjustable fixed bracket to the target position to be planned, and fixing the target positioning module 6 to the target fixing block 5 of the adjustable fixed bracket;

[0046] S3, installing the target tracker 1 on any tracker mounting base 2 of the adjustable fixing bracket;

[0047] S4, the intelligent tracking module automatically tracks the target tracker 1, obtains the current position of the target placement slot, and sends it to the motion planning module;

[0048] S5, the motion planning module reads the target position and receives the current position sent by the intelligent tracking module, then plans the motion path from the current position to the target position, and generates motion control parameters and sends them to the intelligent control module;

[0049] S6. After receiving the motion control parameters, the intelligent control module controls the movement of the adjustable fixed bracket to adjust the target placement slot to the target position.

[0050] Step S4 is as follows: the camera on the top of the smart tracker determines the color and distribution matrix position of the tracker mounting base 2 through image processing. Obtain the installation position of the target tracker 1P, thereby determining the conversion matrix from the target tracker 1 to the target plane of the target positioning module 6 Finally, the initial position information of the target placement slot is calculated in, See Figure 5 .

[0051] The possible installation positions of the tracker P are front left (fl), front middle (fm), front right (fr), back left (bl), back middle (bm) and back right (br). For example, when the image analysis program determines that the current installation point is orange, the installation side is the front side; and the free installation holes are on both sides of the tracker, then it can be known that the installation position is in the middle, so the current installation position of P is fm.

[0052] In step S1, the tracker fixed on the target is used as the origin, and the target surface planned before is represented by the center point and three axis vectors, namely {centerPos, xVec, yVec, zVec} pre , the target surface in use can be obtained through the registration matrix:

[0053] The initial position of the target placement slot with the tracker fixed on the target as the origin See Figure 7 ,in is the real-time position of the target tracker under the tracker, express The inverse matrix of .

[0054] Step S6 is specifically as follows: the information position of the target plane is known, and for the target, the plane where the target placement slot is located is moved to the target plane when in use, that is, the normal vector X of the plane where the target placement slot is located is adjusted. oste-i (x-axis of the oste-i coordinate system) and the normal vector X of the target plane when using intra-t (the x-axis of the intra-t coordinate system) and the center point of the surface where the target placement slot is located (the origin of the oste-i coordinate system) is on the target plane when in use. The motion planning of each control servo is obtained through the planning algorithm and executed by the intelligent control module. The target placement slot is moved to the desired position and posture by up and down, inside and outside, front and back tilt and left and right rotation. Figure 8 .

[0055] The specific planning algorithm is:

[0056] (1) Calculate the normal vector X of the plane where the target placement slot is located oste-i Normal vector X of the target plane when used intra-t The transformation matrix

[0057] (2) According to the transformation matrix, the rotation parameters of the 3DOF control steering gear group are obtained;

[0058] (3) Calculate the center point position of the target placement slot plane after parallelization oste-r ;

[0059] (4) Continue along X oste-i From center oste-r Start from the projected distance on the target plane when in use, which is the distance the rod is moved up and down.

[0060] The specific embodiments described above further illustrate the objectives, technical solutions and beneficial effects of the present invention in detail. It should be understood that the above description is only a specific embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A path planning auxiliary adjustment device, characterized in that: The invention comprises an intelligent control module (1), a target positioning module (2), a tracker mounting bracket (3) and a tracker (4); the tracker mounting bracket (3) is fixedly mounted on the intelligent control module (1); at least one tracker mounting seat (5) is provided above the tracker mounting bracket (3); the tracker (4) is mounted on a tracker mounting point (16) in front of or behind the tracker mounting seat (5); the target positioning module (2) is connected to the intelligent control module (1) via a motion mechanism (6); The motion mechanism (6) comprises a lower rod (7), a telescopic rod (8) and a rotating head (9); the lower rod (7) is meshed with a rotating gear (14); the telescopic rod (8) is plugged into the lower rod (7); the rotating head (9) is mounted on the upper end of the telescopic rod (8), and the upper end of the rotating head (9) is provided with a first bevel gear (10); the first bevel gear (10) is meshed with a second bevel gear (12) fixed on the rotating rod (11) for transmission; the two ends of the rotating rod (11) are connected to a swing module (15) via a fixing sleeve (13); and the rotating gear (14) is connected to a rotating motor.

2. The path planning auxiliary adjustment device according to claim 1, characterized in that: The swing module (15) comprises a fixed head and a swing head; the fixed head is connected to the rotating rod (11) via a fixed sleeve (13); and the swing head is connected to the fixed head via a swing cylinder.

3. The path planning auxiliary adjustment device according to claim 1, characterized in that: The target positioning module (2) is connected to the motion mechanism (6) via a lockable guide rail (17).

4. A control method using the path planning auxiliary adjustment device according to any one of claims 1 to 3, characterized in that: The following steps are involved: S1. First, generate a 3D model of the target area for the DCOM image before use, then complete the planning of the target surface on the 3D model, and select more than three points as the feature points for target registration. Use the guide needle to select the corresponding feature points planned before use in the target area to complete the registration of the model before use to the real target in use. ; S2, fixing the adjustable fixed bracket to the target position to be planned, and fixing the target positioning module to the target fixing block of the adjustable fixed bracket; S3, mounting the target tracker on any tracker mounting base of the adjustable fixing bracket; S4, the intelligent tracking module automatically tracks the target tracker, obtains the current position of the target placement slot, and sends it to the motion planning module; S5, the motion planning module reads the target position and receives the current position sent by the intelligent tracking module, then plans the motion path from the current position to the target position, and generates motion control parameters and sends them to the intelligent control module; S6. After receiving the motion control parameters, the intelligent control module controls the movement of the adjustable fixed bracket to adjust the target placement slot to the target position.

5. The control method according to claim 4, characterized in that: Step S4 is as follows: the camera on the top of the smart tracker determines the color and distribution matrix position of the tracker mounting base through image processing. , obtain the installation position of the target tracker P, and thus determine the transformation matrix from the target tracker to the target plane of the target positioning module , and finally calculate the initial position information of the target placement slot ,in, = .

6. The control method according to claim 4, characterized in that: In step S1, the tracker fixed on the target is used as the origin, and the target surface planned before is represented by the center point and three axis vectors, namely {centerPos, xVec, yVec, zVec} pre , through the registration matrix, the target surface in use can be obtained as {centerPos, xVec, yVec, zVec} intra-t = {centerPos,xVec,yVec,zVec} pre-t .

7. The control method according to claim 6, characterized in that: The initial position of the target placement slot with the tracker fixed on the target as the origin = -1 ,in is the real-time position of the target tracker under the tracker, -1 express The inverse matrix of .

8. The control method according to claim 4, characterized in that: Step S6 is as follows: given the information position of the target plane, adjust the normal vector X of the plane where the target placement slot is located. oste-i The normal vector X of the target plane when using intra-t The target placement slot is parallel and the center point of the surface where the target placement slot is located is on the target plane when in use. The motion planning of each control servo is obtained through the planning algorithm and executed by the intelligent control module. The target placement slot is moved to the desired position and posture by up and down, inside and outside, front and back tilt, and left and right rotation.

9. The control method according to claim 8, characterized in that: The specific planning algorithm is: (1) Calculate the normal vector X of the plane where the target placement slot is located oste-i Normal vector X of the target plane when used intra-t The transformation matrix ; (2) According to the transformation matrix, the rotation parameters of the 3DOF control steering gear group are obtained; (3) Calculate the center point position of the target placement slot plane after parallelization oste-r ; (4) Continue along X oste-i From center oste-r Start from the projected distance on the target plane when in use, which is the distance the rod is moved up and down.

Images