A robot-assisted navigation method

Abstract

This invention discloses a robot-assisted navigation method, comprising the steps of: acquiring the robot's speed and assistance information, and constructing its assisted motion model; performing path planning for the robot, collecting the robot's heading angle and speed information during path execution, constructing its active traction motion model, and controlling the robot's movement in conjunction with the presence or absence of obstacles and the aforementioned assisted motion model. This invention constructs both an assisted motion model and an active traction motion model for the robot, and selects and controls them according to actual conditions, flexibly combining the robot's assisted motion and active traction motion. It is simple and quick to operate, responds rapidly, and allows for efficient collaborative interaction with assistant doctors, effectively reducing the doctor's additional workload.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robots, in particular to a power-assisted navigation method of a robot. BACKGROUND

[0002] In recent years, with the increasing maturity of technology, the application field of industrial robots is also increasing, and the demand continues to rise. Robots can replace or assist humans to complete various work, and medical robots are the same.

[0003] As a kind of advanced robot platform, endoscopic surgery robot is more and more favored by doctors. Its design concept is to implement complex surgical procedures through minimally invasive methods. The endoscopic surgery robot is composed of a surgeon console, a patient operating table and an imaging system. Among them, the patient operating table as a mobile operation platform needs to interact with the assistant doctor for a long time, and its flexible and convenient movement ability can effectively reduce the additional work burden of the assistant doctor. Therefore, constructing the corresponding power-assisted navigation system has great significance for improving the use experience of the endoscopic surgery robot. SUMMARY

[0004] The present application aims to solve the above problems, and provides a power-assisted navigation method of a robot, which can efficiently interact with the assistant doctor and effectively reduce the additional work burden of the doctor.

[0005] Technical scheme:

[0006] A power-assisted navigation method of a robot, comprising the steps of:

[0007] obtaining the speed information and the power information of the robot movement, and constructing the power-assisted motion model thereof;

[0008] planning the path of the robot, collecting the heading angle and speed information of the robot during the execution of the path, constructing the active traction motion model thereof, and combining whether there is an obstacle and the aforementioned power-assisted motion model to control the motion of the robot.

[0009] The speed information includes linear acceleration and rotational angular velocity of the robot movement, which is obtained by measuring with a sensor.

[0010] The linear acceleration of the robot movement is integrated to obtain the movement speed of the robot.

[0011] The power-assisted motion model is constructed according to the following steps:

[0012] obtaining the thrust F x and F y of the robot in the straight line direction of the robot and the direction perpendicular thereto.

[0013] According to the linear relationship between the thrust and the speed, the command speed v of the robot under the action of the hand thrust assistance is obtained f and the command angular velocity ω f :

[0014]

[0015]

[0016] wherein v is the moving speed of the robot, which is obtained by integrating the linear acceleration of the robot movement; ω is the rotational angular speed of the robot during movement; m is the mass of the robot; J is the rotational inertia of the robot; l is the distance between the force point of the hand thrust and the front wheel of the robot in the straight moving direction of the robot; K1 and K2 are coefficients for adjusting the assistance effect.

[0017] The active traction motion model is constructed according to the following steps:

[0018] The real-time heading deflection angle θ of the robot is obtained:

[0019]

[0020] wherein, is the angle difference between the real-time heading angle of the robot and the reference heading angle, the reference heading angle is the tangent direction of the planned path; k is a proportional coefficient; e is the distance between the current position of the robot and the planned path in the direction perpendicular to the straight moving direction of the robot; v is the moving speed of the robot;

[0021] The traction angular speed of the robot at the current position is calculated:

[0022]

[0023] The traction linear speed of the robot is generated by using a PI controller:

[0024] v n =k p *(Δd)+k i *∫Δd*dt

[0025] wherein Δd is the distance difference between the current position and the position of the next target point in the planned path, k p is the proportional coefficient of the distance error, and k i is the integral coefficient of the distance error.

[0026] When the distance between the current position of the robot and the terminal point is within a preset range, the robot is controlled to move by using the assistance motion model.

[0027] The path planning of the robot is performed as follows:

[0028] According to the start point and the end point of the robot, a specified feature passing point is set, a Bezier curve is generated according to the feature passing point, and all the feature passing points are smoothed by the Bezier curve to obtain all path interpolation points, that is, to obtain the planned path.

[0029] The path interpolation between the two adjacent feature passing points is calculated as follows:

[0030] P ε = P s *(1-ε) 3 +3P c1 *(1-ε) 2 *ε+3P c2 *(1-ε)*ε 2 +P e *ε 3

[0031] Wherein, P S and P ε are the coordinates of the two adjacent feature passing points; P c1 and P c2 are the coordinates of the intermediate control points of the Bezier curve, and the positions thereof are randomly dragged to control the generation of the Bezier curve; ε is a sparse control parameter of the interpolation point, and the value range is (0, 1).

[0032] E is increased at a fixed interval, and the distance between all adjacent points starting from the start point is calculated after each interpolation, to determine whether it is greater than the set distance threshold, if it is less than the latter point in the adjacent two points is removed, wherein, if the latter point is the end point, the former point is removed; if it is greater than, the path interpolation point obtained after the interpolation is generated.

[0033] UWB base station is used for distance measurement and positioning or RFID tag is used for positioning during path execution.

[0034] A plurality of ultrasonic obstacle avoidance sensors are arranged at the front end of the robot to sense obstacles.

[0035] If there is an obstacle at the front end of the robot, the robot moves by power assistance; if there is no obstacle at the front end of the robot, the robot moves by active traction.

[0036] The force sensor installed on the hand pushing mechanism of the robot obtains the power assistance information of the robot.

[0037] The number of force sensors is 2, and they are symmetrically installed on the hand pushing mechanism of the robot.

[0038] Beneficial effects: The application constructs a power-assisted motion model and an active traction motion model of the robot, and selects control according to actual conditions, flexibly combines the power-assisted motion and the active traction motion of the robot, is simple and convenient to operate, reacts quickly, can efficiently cooperate and interact with the assistant doctor, and effectively reduces the additional work burden of the doctor. BRIEF DESCRIPTION OF DRAWINGS

[0039] Figure 1 is a structural schematic diagram of a surgical robot;

[0040] Figure 2 is a schematic diagram of a chassis mechanism of a surgical robot;

[0041] Figure 3 is a schematic diagram of power-assisted control of a chassis of a surgical robot;

[0042] Figure 4 is a schematic diagram of a power-assisted navigation scheme of a surgical robot;

[0043] Figure 5 is a flowchart of tracking control on a planned path.

[0044] Wherein:

[0045] 11 is a chassis, 12 is a column, 13 is a pre-positioning structure, 14 is a mechanical arm, and 15 is a hand pushing mechanism.

[0046] 21 is an IMU sensor, 22 is a force sensor, and 23 is an ultrasonic obstacle avoidance sensor. DETAILED DESCRIPTION

[0047] The application will be further illustrated in combination with the drawings and specific embodiments.

[0048] As shown in the drawings, Figure 1 The surgical robot (i.e. patient operating table) includes a chassis 11 for walking and supporting, a column 12 mounted on the chassis, a pre-positioning structure 13 mounted on the top of the column 12, and a plurality of mechanical arms 14, and a hand pushing mechanism 15 for power-assisted operation is mounted on the column 12.

[0049] As shown in the drawings, Figure 1 , 2 An IMU sensor 21 is mounted on the chassis 11 of the surgical robot for measuring linear acceleration, rotational angular velocity and heading angle of the chassis when the chassis moves, two force sensors 22 are symmetrically mounted on the hand pushing mechanism of the surgical robot for measuring hand pushing force when power-assisted operation is performed, and four ultrasonic obstacle avoidance sensors 23 are arranged at the front end of the chassis of the surgical robot for sensing local obstacles, Figure 2 The position where the arrow starts represents the installation position of each sensor for simple illustration.

[0050] Then the actual speed of the current chassis and the pushing force can be calculated according to the data collected by the IMU sensor and the force sensor;

[0051] The actual speed v of the current chassis is calculated by integrating the linear acceleration of the chassis movement measured by the IMU sensor:

[0052]

[0053] Wherein, v0 is the actual speed of the chassis at the last time; a x , a y are the linear acceleration of the chassis movement in the x-axis and y-axis of the surgical robot coordinate system, respectively, which are directly measured by the IMU sensor.

[0054] The pushing force is:

[0055] F x = 0.5*(F lx +F rx )

[0056] F y = 0.5*(F ly +F ry )

[0057] Wherein, F x , F y are the pushing force in the x-axis and y-axis of the surgical robot coordinate system; F lx , F ly and F rx , F ry are the components of the force measured by the two force sensors in the x-axis and y-axis.

[0058] Wherein, the surgical robot coordinate system takes the straight direction of the chassis as the x-axis and the horizontal direction perpendicular to the straight direction of the chassis as the y-axis, as shown in Figure 3 .

[0059] The power-assisted navigation method of the surgical robot of the application comprises the following steps:

[0060] S1: constructing a power-assisted motion model of the surgical robot;

[0061] According to mechanics, the pushing force is as follows:

[0062] F x = ma

[0063] F y = Jω

[0064] Wherein, m is the mass of the surgical robot, and J is the rotational inertia of the surgical robot.

[0065] Due to the large weight of the surgical robot, it is impossible to realize the movement of the surgical robot completely through the hand pushing force. The movement can be assisted and controlled by the power mechanism in the surgical robot according to the hand pushing force to realize the corresponding moving line acceleration and rotation angular velocity. The control coefficients can be set as K1 and K2. The actual hand pushing force is as follows:

[0066] F x = K1ma

[0067] F y = K2Jω

[0068] Then the command speed v of the chassis under the action of the hand pushing force assistance can be obtained f and the command angular velocity ω f :

[0069]

[0070]

[0071] Wherein, ω is the rotation angular velocity when the chassis moves, which is directly measured and obtained by the IMU sensor; l is the distance between the hand pushing mechanism (i.e. the hand pushing force point) and the chassis front wheel in the straight moving direction of the chassis;

[0072] By adjusting the size of K1 and K2, the effect of hand pushing force assistance can be adjusted.

[0073] S2: Constructing the active traction movement model of the surgical robot; including steps:

[0074] Path planning is carried out by adopting path traction, and the surgical robot is controlled to execute the planned path, and the Stanley lateral control method is adopted to track and control the planned path in the execution process, as shown in Figure 5 , and the specific steps are as follows:

[0075] Obtaining the real-time heading deflection angle θ of the chassis of the surgical robot:

[0076]

[0077] Wherein, is the angle difference between the real-time heading angle of the chassis of the surgical robot and the reference heading angle, the real-time heading angle of the chassis of the surgical robot is measured by the IMU sensor, and the reference heading angle is the tangent direction of the planned path; k is a proportional coefficient; e is the distance between the current position of the surgical robot and the planned path in the y-axis direction in the coordinate system of the surgical robot; v is the actual speed of the chassis of the surgical robot, which is obtained in S1;

[0078] Then the traction angular velocity of the surgical robot at the current position can be calculated as:

[0079]

[0080] The traction line speed of the surgical robot is generated using a PI controller.

[0081] v n =k p *(Δd)+k i *∫Δd*dt

[0082] Where Δd is the distance difference between the current position and the next target point in the planned path, and k p k is the distance error scaling factor. i This is the integral coefficient of the distance error;

[0083] When the distance between the current position of the surgical robot and the endpoint is within a preset range, that is, when the surgical robot moves to the vicinity of the endpoint, the calculation of the traction linear velocity and traction angular velocity is stopped, and the movement of the surgical robot is controlled by an assisted motion model.

[0084] Among them, path planning is carried out using path traction, such as Figure 4 As shown, in this embodiment, the surgical robot uses UWB base stations for ranging and positioning. The path planning is as follows:

[0085] The planned path is obtained by setting specified feature crossing points based on the starting and ending points of the surgical robot, generating Bézier curves based on the feature crossing points, smoothing the path, and obtaining path interpolation points.

[0086] The path interpolation calculation between two adjacent feature crosspoints is as follows:

[0087] P ε =P s *(1-ε) 3 +3P c1 *(1-ε) 2 *ε+3P c2 *(1-ε)*ε 2 +P e *ε 3

[0088] Among them, P S and P ε These are the coordinates of two adjacent feature crossing points, P. c1 and P c2 These are the coordinates of the intermediate control point of the Bézier curve, and their positions can be randomly dragged to control the generation of the Bézier curve. ε is the sparse control parameter of the interpolation point, and its value range is (0,1).

[0089] When performing Bézier curve interpolation, ε is incremented at fixed intervals. After each interpolation, the distance between all adjacent points of the path interpolation points is calculated from the starting point. It is then determined whether the distance is greater than a set distance threshold. If it is less than the threshold, the second point among the two adjacent points is removed. If the second point is the endpoint, the first point is removed. If it is greater than the threshold, the path is generated using the path interpolation points obtained after this interpolation.

[0090] By sequentially smoothing all feature points using Bézier curves, a global path sequence is generated, thus completing path planning.

[0091] In this invention, the surgical robot needs to be located during the path planning and control of the surgical robot. This invention uses conventional indoor positioning methods to determine the position of the surgical robot. More specifically, UWB base stations can be used for ranging and positioning, or RFID tags can be used for positioning.

[0092] S3: Construct a velocity mixer and use it to control the surgical robot;

[0093] The ultrasonic obstacle avoidance sensor at the front of the chassis detects whether an obstacle is present. If so, the final commanded linear velocity and angular velocity of the surgical robot chassis are v0 and v0, respectively. f and ω f In other words, the surgical robot needs to be propelled by the operator's hand to move; otherwise, the final command linear velocity and angular velocity of the surgical robot chassis are v0 and v0, respectively. n and ω n At this point, movement can be achieved through active traction without the need for manual pushing by the operator;

[0094] Based on the final linear and angular velocities of the surgical robot, combined with the differential chassis model, the speed v of the left wheel of the chassis is calculated. l With right wheel speed v r It sends the relevant commands to the chassis drive.

[0095] In this invention, a surgical robot is used as an example, but in fact, this invention is applicable to the assisted navigation of all walking robots.

[0096] This invention collects the linear acceleration, rotational angular velocity, and heading angle of the robot chassis using an IMU sensor, and simultaneously collects the magnitude of the push force using a force sensor. This allows for the calculation of the command velocity and angular velocity of the robot chassis under the assistance of the push force, thus constructing an assisted motion model for the robot. Then, path planning and execution are performed through path traction, and the active traction velocity and angular velocity of the robot chassis are calculated, establishing an active traction motion model for the robot. Finally, based on the presence of obstacles detected by an ultrasonic obstacle avoidance sensor installed at the front of the robot chassis, the system selects between controlling the robot's movement through push force or active traction. The operation is simple and quick, with a rapid response, enabling efficient collaborative interaction with assistant doctors and effectively reducing the doctor's additional workload.

[0097] The preferred embodiments of the present invention have been described in detail above. However, the present invention is not limited to the specific details of the above embodiments. Within the scope of the technical concept of the present invention, various equivalent transformations (such as quantity, shape, position, etc.) can be made to the technical solution of the present invention, and these equivalent transformations all fall within the protection scope of the present invention.

Claims

1. A method for assisted navigation of a robot, the method comprising: The method comprises the steps of: The linear acceleration and rotational angular velocity of the robot movement and the assist information are acquired, an assist motion model of the robot is constructed, and the command velocity v of the chassis under the hand pushing force assist is obtained f and the command angular velocity ω f ​ Carrying out path planning of the robot, collecting heading angle and speed information of the robot in the process of executing the path, and constructing an active traction motion model of the robot, which is specifically as follows: Integrating linear acceleration of the robot movement to obtain movement speed of the robot, and obtaining a real-time heading deflection angle θ of the robot: ； Wherein, φ is an angle difference between the real-time heading angle of the robot and a reference heading angle, the reference heading angle is a tangent direction of the planned path; k is a proportional coefficient; e is a distance between the current position of the robot and the planned path in a direction perpendicular to the straight movement direction of the robot; v is the movement speed of the robot; Calculating a traction angular velocity of the robot at the current position: ； Generating a traction linear velocity of the robot by using a PI controller: ； Wherein, Δd is the distance difference between the current position and the position of the next target point in the planning path, k p is the distance error proportional coefficient, k i is the integral coefficient of distance error; If the obstacle appears, the final command linear velocity and angular velocity of the surgical robot are respectively and i.e. the surgical robot moves by the pushing force of the operator's hand; if not, the final command linear velocity and angular velocity of the surgical robot are respectively and at this time, the movement is realized by active traction without the pushing force of the operator's hand.

2. The assist navigation method of a surgical robot according to claim 1, characterized by: The assist motion model is constructed according to the following steps: Obtaining the thrust force F of the robot in the straight-ahead direction of the robot and in a direction perpendicular thereto x and F y ; According to the linear relationship between the thrust and the speed, the command speed v of the robot under the action of the manual thrust assistance is obtained f and the command angular speed ω f : ； ； wherein, is the moving speed of the robot, which is calculated by integrating the linear acceleration of the robot movement; is the rotational angular velocity of the robot when moving; is the mass of the robot; is the moment of inertia of the robot; is the distance between the hand pushing force application point and the front wheel of the robot in the straight moving direction of the robot; K1 and K2 are coefficients for adjusting the assistance effect.

3. The method of claim 1, wherein: When the distance between the current position of the robot and the end point is within a preset range, the assist motion model is used to control the movement of the robot.

4. The method of claim 1, wherein: The path planning of the robot is specifically as follows: Setting specified feature passing points according to the start point and the end point of the robot, generating a Bezier curve according to the feature passing points, and smoothing the Bezier curve of all the feature passing points to obtain all path interpolation points, that is, to obtain the planned path.

5. The method of claim 4, wherein: The path interpolation between two adjacent feature passing points is calculated as follows: ； where P S and P ε are the coordinates of two adjacent feature waypoints; P c1 and P c2 are the coordinates of the intermediate control points of the Bezier curve, which are randomly dragged to control the generation of the Bezier curve; and ε is a sparse control parameter of the interpolation point, and the value range is (0, 1).

6. The method of claim 5, wherein: Incrementing ε at a fixed interval, and calculating the distance between all adjacent two points from the start point after obtaining the path interpolation point after each interpolation, judging whether the distance is greater than a set distance threshold, if less than, removing the latter point of the two adjacent points, wherein, if the latter point is the end point, removing the former point; if greater than, generating a path by using the path interpolation point obtained after the interpolation.

7. The method of claim 1, wherein: In the process of executing the path, UWB base stations are used for distance measurement positioning or RFID tags are used for positioning.

8. The method of claim 1, wherein: A plurality of ultrasonic obstacle avoidance sensors are arranged at the front end of the robot to sense obstacles.

9. The method of claim 1, wherein: The force sensor is installed on the hand pushing mechanism of the robot to obtain the assist force information of the robot. 10.The method of claim 9, wherein: The number of the force sensor is 2, and the force sensor is symmetrically installed on the hand pushing mechanism of the robot.

Images