Massage mechanical arm path planning and control method

Abstract

The invention relates to a mechanical arm control technology, and discloses a path planning method of a mechanical arm for massaging burnt skin, which comprises a pressure control module, a path planning module, a motor driving module and an error correction module. The pressure control module and the motor driving module achieve control over pressure and the pose of the mechanical arm through closed-loop design, and the precision and the anti-interference capacity are improved. The path planning module uses an RANSAC algorithm and mechanical arm pose requirements of discrete path points obtained through path fitting to obtain inverse kinematics through a numerical method, and the rotation angle of each motor is obtained; the error correction module achieves smooth and stable operation of the mechanical arm through methods such as PID control. Through the combination of the modules, the mechanical arm can automatically massage a burnt part, and the mechanical arm can replace part of the requirement for manual massage due to burn, and has a good medical instrument application prospect.

Description

Technical Field

[0001] This invention belongs to the field of robotic arm control technology, and in particular relates to a method for controlling and planning the path of a massage robotic arm. Background Technology

[0002] Blood vessels connect to all parts of the body. While they themselves do not have therapeutic functions, maintaining their patency in areas of skin burns is essential. Open blood vessels ensure a stable blood supply to the wound, providing the necessary nutrients for healing. Simultaneously, white blood cells and immune cells in the blood effectively reduce complications, alleviate patient pain, and promote recovery.

[0003] When using a robotic arm for massage, a binocular camera is needed to capture skin images to create a 3D model of the burned area. The robotic arm is then controlled according to a given massage path. However, due to the elasticity of the skin, the path planning is highly uncertain, and excessive local pressure must be avoided to prevent the burned skin from rupturing.

[0004] Therefore, how to achieve path planning and force control of elastic surfaces is a problem that needs to be solved. Summary of the Invention

[0005] The purpose of this invention is to provide a method for planning massage paths for skin burn sites and a closed-loop control method for robotic arms based on the RANSAC algorithm, so as to solve the above-mentioned shortcomings.

[0006] To achieve the above objectives, the system includes a pressure control module, a path planning module, a motor drive module, and an error correction module.

[0007] The pressure control module controls the pressure applied to the skin by the robotic arm. It uses sensors (such as force sensors and torque sensors) to provide real-time feedback on the contact force of the end effector and adjusts the pressure according to the set target pressure. Considering the dynamic characteristics of the robotic arm's end effector in contact with the skin surface, the module adjusts the interaction between force and motion based on the target force and the displacement relationship of the end effector. This allows the end effector to maintain a certain contact force while smoothly moving along the massage path curve of the burned skin.

[0008] In force control, a target pressure F is set. target And based on the actual contact force F fed back by the sensor actual Adjust the position of the end effector. Δ=F target -F actual If the actual contact force F actual Greater than the target force F targetPressure can be reduced by contracting the end effector's movement; conversely, it increases the pressure on the end effector. In practical applications, due to the unavoidable vibrations during robotic arm movement and the shifts in the center of gravity caused by posture adjustments, it is impossible to maintain constant F. target =F actual Therefore, a force control error Δ is introduced. By setting the force control error value, the actual pressure fluctuates within the allowable error range, avoiding misjudgments by the sensor due to the movement of the robotic arm, thus achieving the force control effect.

[0009] Due to the elastic surface properties of skin, when a certain force is applied to the skin, the point of force application and the surrounding skin will indent along the direction of the force. If the robotic arm moves along the real-time scanning path, there will be significant errors. Therefore, before performing robotic arm massage, it is necessary to model the contact force between the skin surface and the robotic arm, as well as the deformation of the contact surface. A simplified model could be: F=k·δ Where F is the contact force, k is the skin surface stiffness coefficient, and δ is the indentation depth, this model illustrates the relationship between the force applied to the skin by the robotic arm and the indentation depth.

[0010] For the path planning module, to reduce massage path deviations caused by human activity, the robotic arm's running path and pose need to be replanned in real time using a binocular depth camera to avoid ineffective massage. However, due to the elastic nature of skin, if the robotic arm is adjusted by refitting a preset route, the pose of the robotic arm will have a large error, and the real-time calculation of the curvature changes of the deformed part will cause computational complexity and slow response. To achieve robotic arm pose adjustment, save computing power, and improve response time, the RANSAC algorithm is used to perform planar fitting and segmentation of the skin surface of the burn area. step: 1. Randomly select 3 points from the 3D point cloud of the skin surface model to fit a plane. 2. Calculate the parameters of the plane equation using the three-point rule, and obtain the plane equation ax + by + cz + d = 0. 3. Check if other points lie on the plane, and calculate the distance from each point to the plane: 4. If the distance from a point to a plane is less than a certain set threshold, then the point is considered to belong to the current plane. 5. Based on the stiffness coefficient of the pressed skin, draw a circle with radius r, using the pose of the end effector as the normal vector. The size of r is negatively correlated with the stiffness coefficient k. Points within the circle's coverage area do not participate in plane fitting and segmentation. The purpose is to reduce the influence of elastic deformation on the preset path and reduce the number of cutting surfaces.

[0011] Repeat the above steps to fit multiple planes, each time selecting a new plane from the remaining point cloud data, until the point cloud data is effectively divided into multiple planes, simplifying the 3D model of the arm into a polygonal model with multiple intersecting planes.

[0012] The 3D curve of the preset massage path is projected onto the surface of the simplified model to split the curve. The normal vector n = (a, b, c) of the projection plane of each curve segment is obtained. This normal vector is the attitude of the end effector under that path segment. The vector is converted into a rotation matrix, which can provide a reference for the attitude adjustment of the robotic arm.

[0013] The motor drive module is the core of realizing the movement of the robotic arm. Stepper motors have precise stepping control capabilities, making them suitable for position control tasks of the robotic arm. By using a microstepping driver, the step angle can be decomposed into smaller step units, improving positioning accuracy and reducing vibration, thus achieving stable control of the robotic arm's end effector posture.

[0014] To enable the robotic arm to move along a preset path, a three-dimensional curve of the path needs to be defined first. This curve can be described by parametric equations (such as x(t), y(t), z(t)), ensuring its continuity. Excessive robotic arm movement can cause vibration and other errors; therefore, the speed and acceleration of the motors need to be limited. Interpolation is performed on the time axis for each point on the path, applying maximum speed and acceleration constraints to ensure smooth movement. Then, inverse kinematics is used to calculate the angles of each joint of the robotic arm at each moment, causing the end effector to move along the three-dimensional curve.

[0015] The error correction module is essential for the continuous and normal operation of the robotic arm. Robotic arm stepper motors typically use open-loop control, but due to issues such as friction and load variations, open-loop control can lead to error accumulation. Therefore, a closed-loop control system needs to be added to the motor. Using position feedback devices such as encoders, the current position of the motor is monitored in real time and compared with the target position to prevent the stepper motor from missing steps and improve the motion accuracy of the robotic arm.

[0016] To further reduce errors, PID control is used in the robotic arm control system to regulate the movement of the motor. By adjusting the PID parameters (proportional, integral, and derivative gain), the motor's response speed, smoothness, and stability are optimized, effectively compensating for errors caused by inertia, friction, and other external factors. Beneficial effects

[0017] This invention uses the RANSAC algorithm to geometrically divide non-geometric burn areas, avoiding positioning errors during massage caused by skin elastic deformation. Instead of real-time changes to the robotic arm's pose, this method uses a simplified model to adjust poses with minor variations into a single pose, significantly reducing the demands on GPU computation and robotic arm movement, and minimizing response time requirements. Furthermore, by using closed-loop control and PID control, the robotic arm can more effectively mitigate interference from complex environments, thereby improving control accuracy and stability. By introducing time parameter equations, speed and acceleration control is achieved, ensuring a strong fit between the running path and the preset path, further realizing low-error control. Attached Figure Description

[0018] The accompanying drawings, which are included to provide a further understanding of embodiments of the invention and form part of this application, do not constitute a limitation thereof. In the drawings: Figure 1 This is a flowchart illustrating a method for geometrizing a three-dimensional arm model using a binocular depth camera, as disclosed in an embodiment of the present invention. Figure 2 This is a diagram illustrating the simplified model and vectors obtained using the RANSAC algorithm in this invention. Figure 3 This is a schematic diagram of the structure of the robotic arm path planning and control according to the present invention. Detailed Implementation

[0019] To enable those skilled in the art to better understand the present invention, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments. The described embodiments are merely some embodiments of the present invention, and not all embodiments.

[0020] Example

[0021] This invention discloses a method for simplifying an arm model into a geometric model using the RANSAC algorithm. Please refer to [link to relevant documentation]. Figure 1 , Figure 1 This invention discloses a flowchart illustrating a method for geometrizing a 3D model of an arm using a binocular depth camera. The method includes the following steps:

[0022] Step 1: Read the 3D point cloud By reading the preprocessed depth map data and performing pixel traversal, the depth value of each pixel is converted into (x, y, z) coordinates in three-dimensional space. The conversion process is based on camera parameters, and the specific calculation formula is as follows: The camera's intrinsic parameter matrix is: Calculate the three-dimensional coordinates: X = (uc) x )·d / f x Y = (vc y )·d / f y Z = d Among them, f x and f y (c) represents the focal length (in pixels in the x and y directions, respectively). x ,c y ) is the principal point coordinate of the camera's optical axis on the image, and d is the depth value at position (u,v) in the depth map.

[0023] Step 2: Point Cloud Denoising and Optimization Real-time scanning with a binocular camera calculates the depth and displacement changes between the current and previous 3D point cloud readings, removing points with excessive changes and thus eliminating data from areas of significant deformation caused by pressure. By using the RANSAC algorithm, pixels are fitted to multiple planes to obtain a 3D arm geometry model with smaller variations.

[0024] Step 3: Posture Assistance Adjustment Read the equations for each face of the arm's geometric model, calculate the normal vector for that face (pointing inwards towards the model), and then convert the normal vector into a rotation matrix. Assume a rotation vector v = (v... x ,v y ,v z It is a unit vector, and the rotation angle θ can be used to calculate the corresponding rotation matrix using Rodriguez's formula, as follows: Where I is a 3×3 identity matrix, sinθ and cosθ are the sine and cosine of the rotation angle θ, [v] * It is the antisymmetric matrix of vector v

[0025] Step 4: Coordinate system transformation To align point cloud data from multiple depth maps, coordinate system transformation is performed on each point cloud using camera extrinsic parameters. The extrinsic parameters include the rotation matrix R and the translation vector T, and the transformation formula is as follows: P_global = R * P_camera + T By transforming coordinates, the point clouds of each frame can be transformed from the camera coordinate system to the global coordinate system, thereby achieving data fusion between different frames and obtaining a complete arm model.

[0026] Step 5: Fitting the preset path The three-dimensional curve parametric equation is r(t)=(x(t),y(t),z(t)), where t is the time parameter and r(t) is the three-dimensional coordinate of any point on the curve. By fitting with straight lines, Bézier curves, etc., the three-dimensional curve is transformed into a trajectory. A series of discrete points on the curve are calculated, and the robotic arm is moved along these points to obtain the reference for the position change of the robotic arm.

[0027] Step 6: Robotic Arm Control By using inverse kinematics, for each trajectory point r(t) i ), using numerical or analytical methods, calculate the corresponding joint angle q(t) i By employing methods such as stepper motor closed-loop control, PID control, and micro-step control, the operating error of the robotic arm can be reduced, the operating path can be corrected in a timely manner, and precise control can be achieved.

[0028] Step 7: Pressure Closed-Loop Control By using a pressure sensor to detect the contact pressure between the robotic arm and the skin in real time, a hydraulic rod can be added to the end effector for displacement adjustment. By adjusting the extension and retraction of the hydraulic rod, the pressure applied to the skin can be controlled, achieving dynamic and stable pressure control.

Claims

1. A method for controlling a robotic arm used to simplify a 3D model of a burn skin massager using the RANSAC algorithm, the method comprising: Step 1: Read multi-frame depth map data. The depth map is acquired by a binocular depth camera with a resolution of 640*480 pixels. Transmit the acquired depth map data to the processing module. Step 2: Process the depth map data, including smoothing individual depth maps using filtering to reduce noise; By comparing two consecutive depth map data, pixels with large displacement and depth changes are removed and filled into the earliest depth map data, so as to reduce the impact of skin elastic deformation on path operation and correction. Step 3: Generate 3D point cloud data based on the processed depth map, convert the image coordinates (u,v) and depth value d into 3D coordinates (x,y,z) in the binocular camera coordinate system, and then convert the 3D coordinates of each pixel in the binocular camera coordinate system to the global coordinate system through rotation and translation matrices to facilitate the operation of the robotic arm. Step 4: The obtained data is fitted to a plane using the RANSAC algorithm to simplify the irregular arm model into a geometric body composed of multiple planes. The normal vector of each plane equation is calculated. By transforming the normal vectors of multiple planes covering the massage path, the posture matrix under multiple path segments is obtained, enabling the robotic arm to better adjust its posture with less computing power and improve response time. Step 5: By establishing the three-dimensional parametric equation of the massage path, the three-dimensional coordinates of any point on the curve are obtained. The true trajectory is obtained by fitting the curve with straight lines, Bézier curves, B-type curves, etc. The discrete points on the curve are calculated, and the robotic arm is controlled to move through these points to achieve the effect of controlling the robotic arm to move along the curve.

2. The control method for a robotic arm for massaging burn skin according to claim 1, characterized in that, The RANSAC algorithm is used to obtain the posture matrix. First, the three-dimensional coordinates of the depth map data are obtained. The algorithm simplifies the arm model into a polyhedral geometry. After determining the equations of multiple faces covering the massage path, the normal vector of each face is calculated and used to convert it into the posture equation under the massage path.

3. The control method for a robotic arm for massaging burn skin according to claim 1, characterized in that, To remove pixel data with excessive elastic deformation, the three-dimensional coordinates of each data point on the arm model are first scanned before applying force, and an allowable offset error value is set. During pressing, the data point changes are scanned in real time. If the depth data changes beyond the allowable value due to the deformation caused by pressing, it is discarded and covered with the initial scan value to obtain a complete massage path map with less deformation, reducing the difficulty of adjusting and correcting the path.

4. The control method for a robotic arm for massaging burn skin according to claim 1, characterized in that, By using numerical methods, a feasible solution is obtained when there is no solution for the complex inverse kinematics of a multi-axis robotic arm. By calculating the discrete points on the path, the robotic arm moves along a series of discrete points, achieving the effect of the robotic arm moving along the massage curve.

Images