Medical mechanical arm trajectory planning method and system based on human body model and medium

Abstract

The embodiment of the invention discloses a medical mechanical arm trajectory planning method and system based on a human body model and a storage medium, and the method comprises the steps: controlling the tail end of a mechanical arm to linearly retreat according to the current position coordinates of the tail end of the mechanical arm, and enabling the tail end of the mechanical arm to be away from a human body; and according to the position coordinates of the tail end of the mechanical arm after linear retreating and the target position coordinates, the tail end of the mechanical arm is controlled to move according to planned path points based on a human body surface model. The planned track of the mechanical arm reasonably avoids the human body according to the specific form of the human body, the mechanical arm can avoid the human body in the moving process, the risk that the mechanical arm collides with the human body does not exist in the process that the mechanical arm moves along the planned path, and after it is judged that the probe at the tail end of the mechanical arm makes contact with the surface of the human body. And the contact force between the probe and the human body surface is controlled within the preset expected contact force range, so that images collected by the probe are clear, the probe does not hurt the human body, and the probe is safe, reliable and suitable for popularization and application.

Description

Technical Field

[0001] The embodiments of the present invention relate to the technical field of medical robot arm control, and in particular to a medical robot arm trajectory planning method, system and medium based on a human body model. Background Art

[0002] In the modern medical field, robotic arms can assist doctors in performing surgeries or treatment procedures, improve the accuracy and flexibility of surgery, reduce patient trauma, shorten recovery time, and may reduce the risk of complications. Therefore, more and more operations are now using robotic arms for assisted diagnosis and treatment. When using a robotic arm for diagnosis and treatment, it is necessary to plan the trajectory of the robotic arm and control the end of the robotic arm to reach the target position for effective operation. However, the existing trajectory planning scheme for medical robotic arms is difficult to take into account the requirements of reasonable avoidance according to the specific form of the human body and accurate control of the contact force of the end of the robotic arm at the same time; in complex medical scenarios, especially for the seven-degree-of-freedom robotic arm with higher degrees of freedom and more flexible movement, there is a lack of effective trajectory planning strategies that meet the actual medical operation needs. The robotic arm cannot avoid the human body during movement, and there is a risk of collision with the human body during the movement of the robotic arm along the planned path. The control accuracy is poor and the safety is low.

[0003] The above problems need to be solved urgently. Summary of the invention

[0004] In order to solve the related technical problems, the present invention provides a medical robot arm trajectory planning method, system and medium based on a human body model to solve the problems mentioned in the above background technology part.

[0005] To achieve the above objectives, the embodiments of the present invention adopt the following technical solutions:

[0006] In a first aspect, an embodiment of the present invention provides a medical robot arm trajectory planning method based on a human body model, the method comprising:

[0007] Get the current position coordinates and target position coordinates of the end of the robot arm;

[0008] According to the current position coordinates of the end of the robotic arm, the end of the robotic arm is controlled to move back in a straight line so that the end of the robotic arm is away from the human body;

[0009] According to the position coordinates of the end of the robotic arm after it recedes in a straight line and the target position coordinates, the end of the robotic arm is controlled to move according to the planned path points based on the human body surface model.

[0010] As an optional implementation, obtaining the current position coordinates of the end of the robotic arm includes:

[0011] According to the joint measurements of the robotic arm, calculate the current position coordinates of the end of the robotic arm.

[0012] As an optional implementation, the controlling the end of the robotic arm to move backward in a straight line according to the current position coordinates of the end of the robotic arm so as to make the end of the robotic arm away from the human body includes:

[0013] Set the direction of the probe on the end of the robot arm to the straight-line backward direction;

[0014] Set the safe distance between the probe at the end of the robotic arm and the human body surface;

[0015] The end of the robotic arm is controlled to retreat in the straight-line retreat direction, wherein the retreat distance is not less than the safety distance.

[0016] As an optional implementation, the method of controlling the end of the robotic arm to move along the planned path points based on the human body surface model according to the position coordinates of the end of the robotic arm after the end of the robotic arm retreats in a straight line and the target position coordinates includes:

[0017] Acquire a human body surface model in advance;

[0018] According to the relative relationship between the human body surface model and the robotic arm, a three-dimensional model in a base coordinate system of the robotic arm is constructed;

[0019] Calculating a path point sequence from a current position corresponding to the position coordinates of the end of the robotic arm after linear retreat to a target position corresponding to the target position coordinates;

[0020] According to the path point sequence, the end of the robot arm is controlled to move to the target position.

[0021] As an optional implementation, the shortest distance from each point on the path point sequence to the human body surface is always greater than the safe distance between the probe at the end of the robotic arm and the human body surface.

[0022] As an optional implementation, the method further includes controlling the end of the robotic arm to move along a planned path point based on a human body surface model according to the position coordinates of the end of the robotic arm after the end of the robotic arm moves straight back and the target position coordinates, and then further includes:

[0023] Determine whether the probe at the end of the robotic arm touches the human body surface;

[0024] If the judgment result is yes, the contact force between the probe and the human body surface is controlled to be within a preset expected contact force range.

[0025] In a second aspect, an embodiment of the present invention provides a medical robot arm trajectory planning system based on a human body model, the system adopts the medical robot arm trajectory planning method based on a human body model proposed in any one of the first aspects above, including:

[0026] Position acquisition module, used to obtain the current position coordinates and target position coordinates of the end of the robotic arm;

[0027] A linear retreat control module, used to control the end of the robotic arm to retreat linearly according to the current position coordinates of the end of the robotic arm, so that the end of the robotic arm is away from the human body;

[0028] The robot arm motion control module is used to control the robot arm end to move according to the planned path points based on the human body surface model according to the position coordinates of the robot arm end after linear retreat and the target position coordinates.

[0029] As an optional implementation, the straight-line retreat control module is specifically used to control the end of the robotic arm to retreat in a straight line according to the current position coordinates of the end of the robotic arm, so that the end of the robotic arm is away from the human body, including: setting the direction in which the probe on the end of the robotic arm points as the straight-line retreat direction; setting a safe distance between the probe on the end of the robotic arm and the surface of the human body; controlling the end of the robotic arm to retreat in the straight-line retreat direction, wherein the retreat distance is not less than the safety distance.

[0030] As an optional implementation, the medical robot arm trajectory planning system based on the human body model further includes:

[0031] The contact force control module is used to determine whether the probe at the end of the robot arm contacts the surface of the human body; if the determination result is yes, the contact force between the probe and the surface of the human body is controlled within a preset expected contact force range.

[0032] In a third aspect, an embodiment of the present invention provides a computer-readable storage medium, in which computer execution instructions are stored. When the computer execution instructions are executed by a processor, they are used to implement the medical robot arm trajectory planning method based on a human body model as described in any of the first aspects above.

[0033] The technical solution proposed in the embodiment of the present invention solves the shortcomings of the existing medical robot arm trajectory planning. According to the current position coordinates of the end of the robot arm, the end of the robot arm is controlled to retreat in a straight line so that the end of the robot arm is away from the human body; according to the position coordinates of the end of the robot arm after retreating in a straight line and the target position coordinates, the end of the robot arm is controlled to move according to the planned path point based on the human body surface model. The robot arm trajectory planned by the technical solution proposed in the embodiment of the present invention reasonably avoids according to the specific form of the human body, the robot arm movement process can avoid the human body, there is no risk of collision with the human body during the movement of the robot arm along the planned path, and after determining that the probe on the end of the robot arm contacts the surface of the human body, the contact force between the probe and the surface of the human body is controlled within the preset expected contact force range, which can ensure that the probe captures a clear image and that the probe does not hurt the human body, which is safe and reliable. The present invention can control the medical robot arm (for example, a seven-degree-of-freedom robot arm) to accurately move to the target position according to the planned path based on the human body surface model, and can control the safe contact between the probe at the end of the robot arm and the surface of the human body for related diagnosis and treatment operations, which is suitable for promotion and application. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0035] Figure 1 A schematic flow chart of a medical robot arm trajectory planning method based on a human body model provided in an embodiment of the present invention;

[0036] Figure 2 A schematic diagram of the principle structure of a medical robot arm trajectory planning system based on a human body model provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0037] In order to make the technical problems solved by the present invention, the technical solutions adopted and the technical effects achieved clearer, the technical solutions of the embodiments of the present invention will be further described in detail below in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of the present invention.

[0038] Embodiment 1

[0039] Please refer to Figure 1 As stated, Figure 1A schematic flow chart of a medical robot arm trajectory planning method based on a human body model provided in an embodiment of the present invention. As shown in the figure, a medical robot arm trajectory planning method 100 based on a human body model in this embodiment includes:

[0040] S101. Obtain the current position coordinates and target position coordinates of the end of the robotic arm;

[0041] S102. According to the current position coordinates of the end of the robotic arm, the end of the robotic arm is controlled to move back in a straight line so that the end of the robotic arm is away from the human body;

[0042] S103. According to the position coordinates of the end of the robotic arm after linear retreat and the target position coordinates, the end of the robotic arm is controlled to move according to the planned path points based on the human body surface model.

[0043] Exemplarily, obtaining the current position coordinates of the end of the robotic arm includes:

[0044] According to the joint measurements of the robotic arm, calculate the current position coordinates of the end of the robotic arm.

[0045] Exemplarily, the controlling the end of the robotic arm to move backward in a straight line according to the current position coordinates of the end of the robotic arm so as to make the end of the robotic arm away from the human body includes:

[0046] Set the direction of the probe on the end of the robot arm to the straight-line backward direction;

[0047] Set the safe distance between the probe at the end of the robotic arm and the human body surface;

[0048] The end of the robotic arm is controlled to retreat in the straight-line retreat direction, wherein the retreat distance is not less than the safety distance.

[0049] Exemplarily, the controlling the end of the robotic arm to move along the planned path points based on the human body surface model according to the position coordinates of the end of the robotic arm after the end of the robotic arm retreats in a straight line and the target position coordinates includes:

[0050] Acquire a human body surface model in advance;

[0051] According to the relative relationship between the human body surface model and the robotic arm, a three-dimensional model in a base coordinate system of the robotic arm is constructed;

[0052] Calculating a path point sequence from a current position corresponding to the position coordinates of the end of the robotic arm after linear retreat to a target position corresponding to the target position coordinates;

[0053] According to the path point sequence, the end of the robot arm is controlled to move to the target position.

[0054] Exemplarily, the shortest distance from each point on the path point sequence to the human body surface is always greater than the safe distance between the probe at the end of the robotic arm and the human body surface.

[0055] Exemplarily, the method further includes controlling the end of the robotic arm to move along the planned path points based on the human body surface model according to the position coordinates of the end of the robotic arm after the end of the robotic arm moves straight back and the target position coordinates, and then further includes:

[0056] Determine whether the probe at the end of the robotic arm touches the human body surface;

[0057] If the judgment result is yes, the contact force between the probe and the human body surface is controlled to be within a preset expected contact force range.

[0058] The medical robot arm trajectory planning method 100 based on the human body model proposed in this embodiment solves the shortcomings of the existing medical robot arm trajectory planning. According to the current position coordinates of the end of the robot arm, the end of the robot arm is controlled to retreat in a straight line so that the end of the robot arm is away from the human body; according to the position coordinates of the end of the robot arm after retreating in a straight line and the target position coordinates, the end of the robot arm is controlled to move according to the planned path point based on the human body surface model. The robot arm trajectory planned by the medical robot arm trajectory planning method 100 based on the human body model proposed in this embodiment reasonably avoids according to the specific form of the human body, and the robot arm can avoid the human body during the movement process. There is no risk of collision with the human body during the movement of the robot arm along the planned path. After determining that the probe on the end of the robot arm contacts the surface of the human body, the contact force between the probe and the surface of the human body is controlled within the preset expected contact force range, which can ensure that the probe captures a clear image and that the probe does not hurt the human body, which is safe and reliable. The medical robot arm trajectory planning method 100 based on the human body model proposed in this embodiment can control the medical robot arm (for example, a seven-degree-of-freedom robot arm) to accurately move to the target position according to the planned path based on the human body surface model, and can control the safe contact between the probe at the end of the robot arm and the surface of the human body for related diagnosis and treatment operations.

[0059] Embodiment 2

[0060] The medical robot arm trajectory planning method based on the human body model in this embodiment includes:

[0061] S201. Obtain the current position coordinates and target position coordinates of the end of the robotic arm;

[0062] S202. According to the current position coordinates of the end of the robotic arm, the end of the robotic arm is controlled to move back in a straight line so that the end of the robotic arm is away from the human body;

[0063] S203. According to the position coordinates of the end of the robotic arm after linear retreat and the target position coordinates, the end of the robotic arm is controlled to move according to the planned path points based on the human body surface model.

[0064] In this embodiment, obtaining the current position coordinates of the end of the robotic arm includes:

[0065] According to the joint measurements of the robotic arm, calculate the current position coordinates of the end of the robotic arm.

[0066] In this embodiment, according to the current position coordinates of the end of the robotic arm, controlling the end of the robotic arm to move back in a straight line so that the end of the robotic arm is away from the human body includes:

[0067] Set the direction of the probe on the end of the robot arm to the straight-line backward direction;

[0068] Set the safe distance between the probe at the end of the robotic arm and the human body surface;

[0069] The end of the robotic arm is controlled to retreat in the straight-line retreat direction, wherein the retreat distance is not less than the safety distance.

[0070] Specifically, first obtain the current position coordinates P of the end of the robot arm current , in the preset spatial coordinate system P current =(x c ,y c , z c , rx c ,ry c , rz c The current position coordinates of the end of the robot are calculated based on the joints of the robot through the forward kinematics of the robot. c ,y c , z c Indicates the x-axis, y-axis, and z-axis coordinates of the end position of the robot arm in the robot arm base coordinate system, rx c ,ry c , rz c Indicates the axis angle posture of the end of the robot arm in the robot arm base coordinate system.

[0071] Set the direction along the current probe as the linear backward direction. The direction vector is determined by the end joint posture of the robot arm. Assume that the direction vector is where rx c Represents the x-axis component of the axis angle attitude expression of the end of the robot arm in the base coordinate system, ry c Represents the y-axis component of the axis angle attitude of the end of the robot in the base coordinate system. c It is the z-axis component of the axis angle attitude expression of the end of the robot arm in the base coordinate system.

[0072] Specifically, in this embodiment, according to the usage, the safety distance d between the probe at the end of the robot arm and the human body surface is set. sThis variable can be defined according to the needs. The larger the value, the farther the planned path is from the human body surface model and the safer it is; the smaller the value, the closer the planned path is to the human body model.

[0073] In the scenario of use, the linear backward movement is performed, and the calculation formula of the position coordinate P1 of the end of the robot arm after the movement is as follows:

[0074]

[0075] Among them, P current Indicates the xyz axis position of the current robot probe; R current The rotation matrix representing the current robot probe pose. -d represents the z-axis relative to the current posture s distance.

[0076] Through the above steps, it can be ensured that the end of the robot arm retreats in a straight line. s Distance, the probe at the end of the robotic arm leaves the human body surface and moves to the P1 position, providing a basis for subsequent trajectory planning.

[0077] In this embodiment, the control of the end of the robotic arm to move along the planned path points based on the human body surface model according to the position coordinates of the end of the robotic arm after the end of the robotic arm retreats in a straight line and the target position coordinates includes:

[0078] Acquire a human body surface model in advance;

[0079] According to the relative relationship between the human body surface model and the robotic arm, a three-dimensional model in a base coordinate system of the robotic arm is constructed;

[0080] Calculating a path point sequence from a current position corresponding to the position coordinates of the end of the robotic arm after linear retreat to a target position corresponding to the target position coordinates;

[0081] According to the path point sequence, the end of the robot arm is controlled to move to the target position.

[0082] In this embodiment, the shortest distance from each point on the path point sequence to the human body surface is always greater than the safe distance between the probe at the end of the robot arm and the human body surface.

[0083] Specifically, in this embodiment, the human body surface model is obtained according to CT data in the preoperative planning stage, and after the relative relationship between the human body and the robot arm position is registered, the human body surface model is transformed from the CT coordinate system to the robot arm coordinate system according to the registration matrix, so that the relative distance between the probe at the end of the robot arm and the human body surface model can be obtained in the robot arm coordinate system. The relative distance between the probe at the end of the robot arm and the human body model can generally be calculated using algorithms such as collision detection.

[0084] In this embodiment, a three-dimensional model in the robot base coordinate system is constructed based on the relative relationship between the human body surface model and the robot arm. The human body surface model is originally in the CT coordinate system. After the previous registration stage is completed, the human body surface model is transformed from the CT coordinate system to the robot base coordinate system through the registration matrix. The coordinates of each point on the model constitute a set M = {P m (x m ,y m ,z m )}, calculate the distance from the current position P1 corresponding to the position coordinates of the end of the robotic arm after a straight line retreat to the target position d corresponding to the target position coordinates s The specific calculation process is as follows:

[0085]

[0086] Where T represents the relative transformation matrix from the CT coordinate system to the robot coordinate system. Represents the point cloud set of the human body surface model in the CT coordinate system, A point cloud set representing the human body surface model in the robot arm coordinate system.

[0087] When planning the path, the shortest distance from each point on the path point sequence to the human body surface is always greater than the safe distance d between the probe at the end of the robot arm and the human body surface. s , thereby effectively preventing the middle joint of the robotic arm from accidentally contacting the human body.

[0088] In this embodiment, path planning is implemented by combining a search algorithm with a distance constraint. Specifically, the path planning is based on the forward and inverse kinematics of the robot arm, from the starting point of the robot arm to the end point of the robot arm, gradually approaching the end point according to a certain distance. In the process of selecting the intermediate path point, a distance constraint is added, requiring the path point to be within a certain distance of the body surface model d s If the distance is not satisfied, the waypoints can be retrieved again. Each waypoint is retrieved and judged, and the waypoint planning from the starting point to the end point is finally completed.

[0089] Assume that the planned path point sequence is Where P i represents the i-th path point, and n represents the total number. P1 is the position coordinate of the end of the robot arm after the linear backward movement, P n The end position of this stage is the target position. The robot starts from P1 and moves to each path point P in sequence. i The changes in joint angles during movement can be solved by inverse kinematics.

[0090] Specifically, taking the seven-freedom robot as an example, let its joint angle vector be From the current position Pi-1 Move to the next path point P i The inverse kinematics equation is expressed as:

[0091]

[0092] The technical solution of this embodiment can not only meet the requirement that the end of the robot arm reaches the target position, but also select the joint posture of the robot arm to move to the target position. The medical robot arm preferably adopts a seven-freedom robot arm. It is constructed by subtracting the actual position of the end of the manipulator calculated by forward kinematics at the current assumed joint angle from the desired path point position coordinates (planned based on the human body surface model and known target position information). When , it means that a set of joint angles is found so that the end of the robot arm is exactly at the desired path point under this joint angle configuration. In other words, the joint angles obtained by inverse kinematics can drive the robot arm to move accurately to the planned path point, thereby achieving the goal of moving along the planned path. i is the end position of the robot probe. The actual movement of the robot is realized by the movement of the robot joints, so it needs to be calculated according to the situation. Indicates that the end of the robot arm is at a joint angle of When , the position coordinate calculation function is a positive kinematic calculation, and the robot arm joint is used to calculate the end position of the robot arm. Through continuous iteration (such as numerical solution), the Jacobian matrix is ​​used to iterate and solve The above equations are satisfied, thereby controlling the robot arm joints to move along the planned path until they reach the end position of this stage.

[0093] In this embodiment, according to the position coordinates of the end of the robotic arm after the end of the robotic arm retreats in a straight line and the target position coordinates, the end of the robotic arm is controlled to move according to the planned path points based on the human body surface model, and then it also includes:

[0094] Determine whether the probe at the end of the robotic arm touches the human body surface;

[0095] If the judgment result is yes, the contact force between the probe and the human body surface is controlled to be within a preset expected contact force range.

[0096] Specifically, the final position of the robot arm after moving according to the planned path is P n， The target position of the entire movement is P goal =(x g ,y g , z g , rx g ,ry g , rz g ):

[0097] where x g ,y g , z g Represents a three-dimensional position point, rx g ,ry g , rz g Represents the three-dimensional axis-angle attitude expression. Similarly, the attitude direction of the probe at the end of the robotic arm, its direction vector is recorded as From P n It starts to move in a straight line towards the human body surface. During the movement, a force sensor is installed at the end of the robot arm to obtain the contact force information F = (F x ,F y ,F z ).

[0098] The real-time position coordinates P to which the robot moves t :

[0099]

[0100] Among them, t is the distance parameter corresponding to the movement time, which is continuously updated with the movement; P n represents the nth path point, P t represents the path point at the current time t, Indicates P n and P n+1 The direction of the motion vector between the two paths is determined by the contact force threshold. Since the motion is continuous and the path points are discrete, this formula is mainly used to calculate the fitting position points between the path points. When the probe at the end of the robot arm touches the surface of the human body, the contact force threshold is determined by the contact force threshold. when When the probe at the end of the robot arm touches the human body, it is determined that the contact force control is started.

[0101] In this embodiment, for the contact force control, while satisfying the motion trajectory, the movement direction of the probe at the end of the robotic arm is consistent with the expectation. At the same time, in order to obtain a clearer ultrasound image after contacting the human body, speed control and contact force control are required to prevent the human body from being impacted by excessive speed, and to ensure that after the robotic arm contacts the human body with the probe, it can be stable and not deviate from the predetermined target, which is safe for the human body and the ultrasound image is clear.

[0102] Specifically, let the desired contact force be The current position error is ΔP = P target -P t .P target The final target position coordinates are controlled according to force feedback, and the proportional integral differential (PID) and other control methods are combined with force feedback. The control law is designed as follows:

[0103]

[0104] Among them, F desired It represents the three-dimensional vector force felt by the end of the robot probe. According to actual needs, it is only necessary to ensure that there is F in the z-axis direction. z d The expected contact force of the magnitude is , and the contact forces in the other two directions do not need to be considered. p , K i , K d They are proportional, integral and differential gain coefficients of position control respectively, K f is the gain coefficient of contact force control, which is achieved by continuously updating the joint angle Make the robot reach the target position P target The movement is stopped when the probe contact force is kept stable within the desired range. p The larger the proportional coefficient, the faster the position error is adjusted, and the smaller the coefficient, the slower the position error is adjusted; K i As an integral coefficient, a small coefficient means a slow elimination of the steady-state error, and a large coefficient means a fast elimination of the steady-state error; K d As a differential coefficient, a large value means a sensitive response to error changes, and a small value means an insensitive effect on error changes, and it has the ability to prevent error overshoot.

[0105] The medical robot arm trajectory planning method based on the human body model proposed in this embodiment solves the shortcomings of the existing medical robot arm trajectory planning. According to the current position coordinates of the end of the robot arm, the end of the robot arm is controlled to retreat in a straight line so that the end of the robot arm is away from the human body; according to the position coordinates of the end of the robot arm after retreating in a straight line and the target position coordinates, the end of the robot arm is controlled to move according to the planned path point based on the human body surface model. The robot arm trajectory planned by the medical robot arm trajectory planning method based on the human body model proposed in this embodiment reasonably avoids according to the specific form of the human body, the robot arm movement process can avoid the human body, and there is no risk of collision with the human body during the movement of the robot arm along the planned path. Moreover, after determining that the probe on the end of the robot arm contacts the surface of the human body, the contact force between the probe and the surface of the human body is controlled within the preset expected contact force range, which can ensure that the probe captures a clear image and that the probe does not hurt the human body, which is safe and reliable. The medical robot arm trajectory planning method based on the human body model proposed in this embodiment can control the medical robot arm (for example, a seven-degree-of-freedom robot arm) to accurately move to the target position according to the planned path based on the human body surface model, and can control the safe contact between the probe at the end of the robot arm and the surface of the human body for related diagnosis and treatment operations, which is suitable for promotion and application.

[0106] Embodiment 3

[0107] like Figure 2As shown, this embodiment provides a medical robot arm trajectory planning system 200 based on a human body model, which adopts the medical robot arm trajectory planning method based on a human body model proposed in any one of the above-mentioned embodiments 1 or 2, including:

[0108] The position acquisition module 201 is used to acquire the current position coordinates and the target position coordinates of the end of the robot arm;

[0109] A linear retreat control module 202 is used to control the end of the robotic arm to retreat linearly according to the current position coordinates of the end of the robotic arm, so that the end of the robotic arm is away from the human body;

[0110] The robot arm motion control module 203 is used to control the robot arm end to move along the planned path points based on the human body surface model according to the position coordinates of the robot arm end after linear retreat and the target position coordinates.

[0111] Exemplarily, the straight-line retreat control module 202 is specifically used to control the straight-line retreat of the robotic arm end according to the current position coordinates of the robotic arm end, so that the robotic arm end is away from the human body, including: setting the direction in which the probe on the robotic arm end points as the straight-line retreat direction; setting a safe distance between the probe on the robotic arm end and the surface of the human body; controlling the robotic arm end to retreat in the straight-line retreat direction, wherein the retreat distance is not less than the safety distance.

[0112] Exemplarily, the medical robot arm trajectory planning system 200 based on the human body model further includes:

[0113] The contact force control module 204 is used to determine whether the probe at the end of the robot arm contacts the human body surface; if the determination result is yes, the contact force between the probe and the human body surface is controlled within a preset expected contact force range.

[0114] Specifically, the specific working principle and process of the medical robot arm trajectory planning system based on the human body model proposed in this embodiment have been specifically described in the medical robot arm trajectory planning method based on the human body model proposed in the above-mentioned embodiments 1 and 2, and will not be repeated here.

[0115] The technical solution proposed in this embodiment solves the shortcomings of the existing medical robot arm trajectory planning. According to the current position coordinates of the end of the robot arm, the end of the robot arm is controlled to retreat in a straight line so that the end of the robot arm is away from the human body; according to the position coordinates of the end of the robot arm after retreating in a straight line and the target position coordinates, the end of the robot arm is controlled to move according to the planned path point based on the human body surface model. The robot arm trajectory planned by the technical solution proposed in this embodiment reasonably avoids according to the specific form of the human body, the robot arm movement process can avoid the human body, and there is no risk of collision with the human body during the movement of the robot arm along the planned path. Moreover, after determining that the probe on the end of the robot arm contacts the surface of the human body, the contact force between the probe and the surface of the human body is controlled within the preset expected contact force range, which can ensure that the probe captures a clear image and that the probe does not hurt the human body, which is safe and reliable. The technical solution proposed in this embodiment can control the medical robot arm (such as a seven-degree-of-freedom robot arm) to accurately move to the target position according to the planned path based on the human body surface model, and can control the safe contact between the probe at the end of the robot arm and the surface of the human body for related diagnosis and treatment operations, which is suitable for promotion and application.

[0116] Embodiment 4

[0117] This embodiment provides a computer-readable storage medium, in which computer-executable instructions are stored. When the computer-executable instructions are executed by a processor, they are used to implement the medical robot arm trajectory planning method based on the human body model as proposed in any of the above-mentioned embodiments 1 or 2.

[0118] It should be noted that the above-mentioned readable storage medium can be implemented by any type of volatile or non-volatile storage device or a combination thereof, such as static random access memory (SRAM), electrically erasable programmable read-only memory (EEPROM), erasable programmable read-only memory (EPROM), programmable read-only memory (PROM), read-only memory (ROM), magnetic memory, flash memory, magnetic disk or optical disk. The readable storage medium can be any available medium that can be accessed by a general or special-purpose computer.

[0119] Note that the above are only preferred embodiments of the present invention and the technical principles used. Those skilled in the art will understand that the present invention is not limited to the specific embodiments described herein, and that various obvious changes, readjustments and substitutions can be made by those skilled in the art without departing from the scope of protection of the present invention. Therefore, although the present invention has been described in more detail through the above embodiments, the present invention is not limited to the above embodiments, and may include more other equivalent embodiments without departing from the concept of the present invention, and the scope of the present invention is determined by the scope of the appended claims.

Claims

1. A medical robot arm trajectory planning method based on a human body model, characterized in that: include: Get the current position coordinates and target position coordinates of the end of the robot arm; According to the current position coordinates of the end of the robotic arm, the end of the robotic arm is controlled to move back in a straight line so that the end of the robotic arm is away from the human body; According to the position coordinates of the end of the robotic arm after it recedes in a straight line and the target position coordinates, the end of the robotic arm is controlled to move according to the planned path points based on the human body surface model.

2. The medical robot arm trajectory planning method based on the human body model according to claim 1, characterized in that: The obtaining of the current position coordinates of the end of the robotic arm includes: According to the joint measurements of the robotic arm, calculate the current position coordinates of the end of the robotic arm.

3. The medical robot arm trajectory planning method based on the human body model according to claim 1, characterized in that: According to the current position coordinates of the end of the robotic arm, controlling the end of the robotic arm to retreat in a straight line so that the end of the robotic arm is away from the human body includes: Set the direction of the probe on the end of the robot arm to the straight-line backward direction; Set the safe distance between the probe at the end of the robotic arm and the human body surface; The end of the robotic arm is controlled to retreat in the straight-line retreat direction, wherein the retreat distance is not less than the safety distance.

4. The medical robot arm trajectory planning method based on the human body model according to claim 1, characterized in that: The method of controlling the end of the robotic arm to move along the planned path points based on the human body surface model according to the position coordinates of the end of the robotic arm after the end of the robotic arm retreats in a straight line and the target position coordinates includes: Acquire a human body surface model in advance; According to the relative relationship between the human body surface model and the robotic arm, a three-dimensional model in a base coordinate system of the robotic arm is constructed; Calculating a path point sequence from a current position corresponding to the position coordinates of the end of the robotic arm after linear retreat to a target position corresponding to the target position coordinates; According to the path point sequence, the end of the robot arm is controlled to move to the target position.

5. The medical robot arm trajectory planning method based on the human body model according to claim 4, characterized in that: The shortest distance from each point on the path point sequence to the human body surface is always greater than the safe distance between the probe at the end of the robotic arm and the human body surface.

6. The medical robot arm trajectory planning method based on a human body model according to any one of claims 1 to 5, characterized in that: According to the position coordinates of the end of the robotic arm after the end of the robotic arm retreats in a straight line and the target position coordinates, the end of the robotic arm is controlled to move according to the planned path points based on the human body surface model, and then it also includes: Determine whether the probe at the end of the robotic arm touches the human body surface; If the judgment result is yes, the contact force between the probe and the human body surface is controlled to be within a preset expected contact force range.

7. A medical robot arm trajectory planning system based on a human body model, characterized in that: The system adopts the medical robot arm trajectory planning method based on the human body model as described in claim 1, comprising: Position acquisition module, used to obtain the current position coordinates and target position coordinates of the end of the robotic arm; A linear retreat control module, used to control the end of the robotic arm to retreat linearly according to the current position coordinates of the end of the robotic arm, so that the end of the robotic arm is away from the human body; The robot arm motion control module is used to control the robot arm end to move according to the planned path points based on the human body surface model according to the position coordinates of the robot arm end after linear retreat and the target position coordinates.

8. The medical robot arm trajectory planning system based on the human body model according to claim 7, characterized in that: The straight-line retreat control module is specifically used to control the end of the robotic arm to retreat in a straight line according to the current position coordinates of the end of the robotic arm, so that the end of the robotic arm is away from the human body, including: setting the direction of the probe on the end of the robotic arm as the straight-line retreat direction; setting a safe distance between the probe on the end of the robotic arm and the surface of the human body; controlling the end of the robotic arm to retreat in the straight-line retreat direction, wherein the retreat distance is not less than the safety distance.

9. The medical robot arm trajectory planning system based on a human body model according to any one of claims 7 or 8, characterized in that: The medical robot arm trajectory planning system based on the human body model also includes: The contact force control module is used to determine whether the probe at the end of the robot arm contacts the surface of the human body; if the determination result is yes, the contact force between the probe and the surface of the human body is controlled within a preset expected contact force range.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer-executable instructions, which, when executed by a processor, are used to implement the medical robot arm trajectory planning method based on a human body model as claimed in claim 1.

Images