A mechanical arm control system of a small corridor inspection and cleaning robot

Abstract

The application discloses a mechanical arm control system of a small corridor inspection and cleaning robot, relates to the technical field of mechanical arm control, and comprises a signal generation module and a signal compensation module. The signal generation module determines the activity space of a gripper, generates a movement signal, or judges whether to generate a joint control signal. The signal compensation module configures a driving current and compensates the driving current. The application effectively avoids hard limit impact based on the second joint angle, the upper arm length and the forearm length, automatically superimposes a high-frequency micro-amplitude compensation current when the joint is in a mixed friction or boundary friction state, prolongs the service life of the joint mechanical structure, establishes a priority order, and interlocks the logic between the joint control signal and the movement signal, effectively avoiding the misoperation of an actuator caused by signal conflict.

Description

Technical Field

[0001] This invention relates to the technical field of robotic arm control, and in particular to a robotic arm control system and system for a small corridor inspection and cleaning robot. Background Technology

[0002] With the acceleration of urbanization and the prevalence of high-rise buildings, the demand for daily inspection and cleaning maintenance of building corridors is increasing. Traditional manual inspection and cleaning methods suffer from problems such as high labor intensity, low efficiency, complex working environments, and safety hazards. To solve these problems, small corridor inspection and cleaning robots have emerged, which use robotic arms to automatically identify, locate, grab, and clean up garbage in corridors.

[0003] However, the existing robotic arm control system of corridor inspection and cleaning robots has the following drawbacks: First, the workspace planning of the robotic arm is unreasonable, lacking a dynamic definition mechanism for the gripper's activity space. Existing technologies typically use fixed Cartesian coordinate system boundaries or simple spherical envelopes to describe the reachable range of the robotic arm, failing to fully consider the series constraint relationships of the robotic arm's joints in the narrow space of a corridor. This causes the gripper to frequently trigger hard stops when approaching the work boundary, resulting in joint impact, decreased positioning accuracy, and even damage to the mechanical structure.

[0004] Secondly, the joint motion control precision is insufficient, especially during low-speed start-up and reversal phases, where there is a significant dead-zone friction effect. Existing robotic arm control systems mostly employ traditional PID closed-loop position control strategies, failing to effectively compensate for the nonlinear frictional characteristics of joints during low-speed operation. In corridor inspection and cleaning scenarios, the robotic arm needs to frequently perform small-angle adjustments and precise positioning. The abrupt changes in static and dynamic friction cause joint crawling, severely affecting the gripper's positioning accuracy and operational stability.

[0005] Third, the lack of a priority interlocking mechanism in multi-signal collaborative control makes it easy for actuators to malfunction when signals conflict. In existing technologies, joint control signals, motion signals, and telescopic rod drive signals are often generated and output independently. When the robotic arm is at the boundary of the activity space and needs to move its body and rotate its joints at the same time, the lack of a clear priority decision logic can easily lead to interference between the robot's body movement and the robotic arm's movement, increasing the risk of collision.

[0006] In addition, the corridor environment is characterized by narrow space, uneven ground, and complex distribution of obstacles. Under the current technological conditions, it is not possible to solve the problem of high-precision and high-stability operation of robotic arms in confined spaces. This can easily lead to the risk of collision between the robotic arm and the corridor environment, as well as the problem of increased joint wear and shortened service life due to insufficient friction compensation. Summary of the Invention

[0007] To address the aforementioned technical problems, this invention provides a robotic arm control system for a small corridor inspection and cleaning robot. Based on dynamic modeling of the activity space driven by the second joint angle, Stribeck friction compensation with sound pressure level monitoring, and multi-signal priority interlocking, the system significantly reduces the collision risk of the robotic arm in confined spaces and the impact of joint dead zone friction on positioning accuracy, thereby improving the operational stability and control accuracy of the robotic arm in complex corridor environments.

[0008] This invention provides a robotic arm control system for a small corridor inspection and cleaning robot, the system including a signal generation module and a signal compensation module; The signal generation module determines the activity space of the gripper based on the angle of the second joint and the length of the forearm, and generates a movement signal or determines whether to generate a joint control signal based on the current position of the gripper and the distribution relationship of the activity space. The signal compensation module responds to the joint control signal, configures the drive current to drive the corresponding joint, and monitors the sound intensity of the joint in real time during the control process. Based on the sound intensity and the Stribeck friction model, it compensates the drive current. The compensation is expressed as the superposition of a high-frequency, low-amplitude current with the driving current to reduce the dead zone friction of the joint.

[0009] Furthermore, the robot's robotic arm includes a first joint, a second joint, and a gripper joint; The part connecting the first joint and the second joint is the upper arm, the part connecting the second joint and the gripper joint is the forearm, the end of the gripper joint is equipped with a gripper, and the part connecting the gripper and the gripper joint is a telescopic rod. The first joint is used to drive the robotic arm to lift or lower; the second joint is used to drive the robotic arm to rotate in the horizontal plane; and the gripper joint is used to drive the extension and retraction of the telescopic rod and the opening and closing of the gripper. The gripper is used to grab or release the garbage.

[0010] Furthermore, the angle of the second joint is calculated using the vertical coordinate of the waste on the ground plane, the length of the upper arm, the length of the forearm, and the height of the waste relative to the second joint. The vertical coordinate of the garbage on the ground plane, i.e., the y-coordinate, is obtained in a three-dimensional coordinate system. The height of the garbage relative to the second joint is expressed as the difference between the z-coordinate of the second joint and the z-coordinate of the garbage's location.

[0011] Furthermore, the movement space of the gripper is represented as a cylinder in which the gripper can move; The logic for acquiring three-dimensional space includes: First, based on the position of the second joint, determine the radius of rotation of the gripper joint in the horizontal plane, and within this radius of rotation, determine the circular surface in which the gripper joints are distributed; Construct a cylinder with the maximum length of the telescopic rod as the height of the cylinder and the circular surface as the base of the cylinder; This cylinder serves as the working space for the gripper.

[0012] Furthermore, the current position of the gripper and its distribution in the activity space include, among, being subordinate to or not subordinate to; The term "membership" refers to the fact that the current gripper is located either inside the activity space or on the surface of the activity space.

[0013] Furthermore, the logic for generating the joint control signal includes: If the distribution relationship is subordinate, determine whether to generate a joint control signal; if the distribution relationship is non-subordinate, generate a movement signal. The joint control signal is represented as a driving current, which includes the current direction and the current magnitude. The direction of the current is controlled by a current commutator, and the magnitude of the current is controlled by a PWM controller.

[0014] Furthermore, if a movement signal is generated, the robot body is driven to move so that the distribution relationship becomes subordinate; The logic for determining whether a joint control signal has been generated includes: Determine whether the debris and the forearm are distributed on the same straight line; If yes, then no joint control signal is generated; otherwise, a joint control signal is generated. The robot body moves using wheels at its bottom.

[0015] Furthermore, if no joint control signal is generated, the telescopic rod is driven to extend or retract the length of that straight-line distance based on the straight-line distance between the gripper joint and the waste. If a joint control signal is generated, the second joint is driven to rotate so that the gripper joint and the garbage are on the same straight line. Then, based on the straight-line distance between the gripper joint and the garbage, the telescopic rod is driven to extend or retract the length of that straight-line distance.

[0016] Furthermore, the driving current is obtained through analysis of historical data; The sound intensity of the joint is represented as the sound intensity of the second joint, and the sound intensity characterizes the friction state of the second joint during operation; The Stribeck friction model is used to generate high-frequency, low-amplitude currents to suppress static friction and improve the smoothness of low-speed motion.

[0017] Furthermore, the robot's movement method is changed from using wheels on its bottom to a tracked movement mechanism.

[0018] This invention provides a robotic arm control system for a small corridor inspection and cleaning robot. Compared with the prior art, this invention has the following advantages: 1) Based on the second joint angle, upper arm length, and forearm length, the gripper's activity space is modeled as a cylinder with the second joint projection point as the center, the sum of the arm lengths as the radius, and the maximum length of the telescopic rod as the height. A safety margin is set, which can dynamically update the activity space as the second joint rotates, and provide early warning and deceleration when the gripper approaches the boundary, effectively avoiding hard limit impact and improving the safety of the robotic arm in narrow corridor spaces. 2) The sound pressure level of the second joint is collected in real time by an acoustic sensor array. Combined with the Stribeck friction model, a high-frequency micro-amplitude compensation current is automatically superimposed when the joint is in a mixed friction or boundary friction state. This causes the joint to generate controllable micro-amplitude vibration to break the static friction viscous state, which significantly reduces the dead zone friction force in the low-speed stage, improves the positioning accuracy and motion stability of the joint, and extends the service life of the joint mechanical structure. 3) A priority ordering system was established for safety protection signals, early warning signals, movement signals, joint control signals, and telescopic rod drive signals, as well as interlocking logic between joint control signals and movement signals. This effectively avoids actuator malfunctions caused by signal conflicts and ensures that each execution unit of the robotic arm works in a coordinated and orderly manner in the complex environment of the corridor. Attached Figure Description

[0019] To more clearly illustrate the technical solutions in the embodiments of this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0020] Figure 1 A basic flowchart of the robotic arm control system of a small corridor inspection and cleaning robot provided in one embodiment of the present invention; Figure 2 A schematic diagram of the main control chip and its pins for a small corridor inspection and cleaning robot provided in one embodiment of the present invention; Figure 3 This is a schematic diagram of a distance-measuring interface for a small corridor inspection and cleaning robot according to an embodiment of the present invention; Detailed Implementation

[0021] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0022] One embodiment of the present invention provides a robotic arm control system for a small corridor inspection and cleaning robot, which can be based on... Figure 1 The process framework shown is designed by combining the following steps: The signal generation module determines the activity space of the gripper based on the angle of the second joint and the length of the forearm, and generates a movement signal or determines whether to generate a joint control signal based on the current position of the gripper and the distribution relationship of the activity space. The signal compensation module responds to the joint control signal, configures the drive current to drive the corresponding joint, and monitors the sound intensity of the joint in real time during the control process. Based on the sound intensity and the Stribeck friction model, it compensates the drive current. The compensation is expressed as the superposition of a high-frequency, low-amplitude current with the driving current to reduce the dead zone friction of the joint.

[0023] It should be noted that this application applies to multi-joint serial robotic arms, specifically robotic arm structures containing at least two rotary joints, wherein the first joint connects the robotic arm base and the first arm rod, and the second joint connects the first arm rod and the second arm rod. Typical application scenarios include industrial assembly, collaborative robots, medical surgical robots, and humanoid robot upper limbs, etc., which require flexible movement with multiple degrees of freedom.

[0024] In multi-joint serial robotic arms, the second joint (elbow joint / intermediate joint) is the key joint that determines the workspace range and end-effector positioning accuracy of the robotic arm. Its motion characteristics directly affect the flexibility and operational capabilities of the entire robotic arm. In the prior art, the first joint (shoulder joint / base joint) usually adopts a mature rotating base structure, and the technical solution is relatively fixed. However, since the second joint is located in the middle of the arm, it needs to bear both bending moment and torque from the load at the far end. Moreover, its structural design directly affects the internal wiring of the arm, joint compactness, and transmission efficiency. It is the link with the most prominent structural contradictions in the prior art. Therefore, the structural optimization of the second joint in this application has more significant technical value.

[0025] It should be noted that this invention, based on the second joint angle, upper arm length, and forearm length, models the gripper's activity space as a cylinder with the second joint projection point as the center, the sum of the arm lengths as the radius, and the maximum length of the telescopic rod as the height. A safety margin is set, allowing for dynamic updates to the activity space as the second joint rotates. It also provides early warning and deceleration when the gripper approaches the boundary, effectively avoiding hard-limit impacts and improving the robotic arm's operational safety in narrow spaces like stairwells. Furthermore, it uses an acoustic sensor array to collect the sound pressure level of the second joint in real time, combined with the Stribeck friction model, to detect when the joint is under mixed friction or... In boundary friction conditions, a high-frequency micro-amplitude compensation current is automatically superimposed to generate controllable micro-amplitude vibrations in the joint to break the static friction and viscosity state. This significantly reduces the dead zone friction force in the low-speed stage, improves the positioning accuracy and motion stability of the joint, and extends the service life of the joint's mechanical structure. Priority sorting of safety protection signals, warning signals, movement signals, joint control signals, and telescopic rod drive signals has been established, as well as interlocking logic between joint control signals and movement signals. This effectively avoids actuator malfunctions caused by signal conflicts and ensures that each execution unit of the robotic arm works in a coordinated and orderly manner in the complex environment of the corridor.

[0026] The robot's robotic arm includes a first joint, a second joint, and a gripper joint; The part connecting the first joint and the second joint is the upper arm, the part connecting the second joint and the gripper joint is the forearm, the end of the gripper joint is equipped with a gripper, and the part connecting the gripper and the gripper joint is a telescopic rod. The first joint is used to drive the robotic arm to lift or lower; the second joint is used to drive the robotic arm to rotate in the horizontal plane; and the gripper joint is used to drive the extension and retraction of the telescopic rod and the opening and closing of the gripper. The gripper is used to grab or release the garbage.

[0027] Specifically, the first joint, the second joint, and the gripper joint are arranged in series along the extension direction of the robotic arm, and the rotation axes of the three are orthogonal to each other. The rotation axis of the first joint is arranged horizontally so that the upper arm can swing in a vertical plane; the rotation axis of the second joint is arranged vertically so that the forearm can rotate in a horizontal plane; the rotation axis of the gripper joint is arranged along the extension direction of the forearm so that the telescopic rod can extend and retract around the axis of the forearm. The sum of the lengths of the upper arm and the forearm is greater than or equal to the difference between the robot's height and the maximum allowable working height in the corridor, and less than or equal to the sum of the robot's height and the standard floor height of the corridor; the maximum telescopic length of the telescopic rod is greater than or equal to the length of the forearm, and less than or equal to the sum of the lengths of the forearm and the upper arm. The lifting angle range of the first joint is configured such that the projection of the upper arm end in the vertical direction is always within the support surface of the robot body; the rotation angle range of the second joint is configured for full circumferential rotation or limited circumferential rotation in the horizontal plane, and the angle range of the limited circumferential rotation is greater than or equal to 270 degrees. The gripper includes at least two clamping parts arranged opposite each other. The opening and closing stroke of the clamping parts is linked to the extension and retraction stroke of the telescopic rod, and the extension and retraction of the telescopic rod takes precedence over the opening and closing stroke of the clamping parts.

[0028] The angle of the second joint is calculated using the vertical coordinate of the waste on the ground plane, the length of the upper arm, the length of the forearm, and the height of the waste relative to the second joint. The vertical coordinate of the garbage on the ground plane, i.e., the y-coordinate, is obtained in a three-dimensional coordinate system. The height of the garbage relative to the second joint is expressed as the difference between the z-coordinate of the second joint and the z-coordinate of the garbage's location.

[0029] Specifically, the expression for calculating the angle of the second joint is: ; in, For the angle of the second joint, It is 180 degrees in radians. Let be the vertical coordinate of the garbage on the ground plane. For the height of the second joint, The length of the upper arm. This refers to the length of the forearm.

[0030] Specifically, the three-dimensional coordinates take any location in the indoor space as the origin, with the x-axis pointing in the length direction of the indoor space, the y-axis pointing in the width direction of the indoor space, and the z-axis perpendicular to the ground plane.

[0031] Specifically, the angle calculation of the second joint also includes the following steps: S11, obtain the coordinates of the garbage in the three-dimensional coordinate system (x1, y1, z1), and the coordinates of the second joint in the three-dimensional coordinate system (x2, y2, z2); S12, calculate the offset of the garbage relative to the second joint in the horizontal plane. The offset in the horizontal plane includes the vertical coordinate offset Δy along the y-axis and the horizontal coordinate offset Δx along the x-axis, where Δy = y1−y2 and Δx = x1−x2. S13. Based on the vertical coordinate offset Δy and the horizontal coordinate offset Δx, determine the polar coordinate angle α of the garbage relative to the second joint in the horizontal plane. The polar coordinate angle α is expressed as the azimuth angle of the garbage relative to the second joint in the horizontal plane, α=arctan(Δy / Δx). S14. Based on the polar coordinate angle α, determine whether the second joint needs to rotate. If the absolute value of the polar coordinate angle α is greater than the preset angle threshold, generate a joint control signal to drive the second joint to rotate so that the waste and the forearm are distributed on the same straight line. If the absolute value of the polar coordinate angle α is less than or equal to the preset angle threshold, determine that the waste and the forearm are already distributed on the same straight line and do not generate a joint control signal. S15, after determining that the debris and forearm are distributed on the same straight line, calculate the height of the debris relative to the second joint z=|z2−z1|, and the vertical coordinate y of the debris in the ground plane, y=[(Δx)] 2 +(Δy) 2 ] 1 / 2 ; S16, Substitute the upper arm length L1, forearm length L2, ordinate y, and height z into the calculation expression for the angle of the second joint, and solve to obtain the angle θ of the second joint; S17. Based on the angle θ of the second joint obtained by the solution, determine whether the angle θ is within the preset rotation angle range of the second joint. If it is within the preset rotation angle range, generate a joint control signal based on the angle θ. If it is not within the preset rotation angle range, generate a movement signal to drive the robot body to move and adjust the relative position between the robot body and the garbage until the angle θ of the second joint obtained by the recalculation is within the preset rotation angle range.

[0032] Specifically, the preset angle threshold is set according to the rotational accuracy of the second joint and the positioning accuracy of the robotic arm, and the value range of the angle threshold is 0.5 degrees to 2 degrees. The preset rotation angle range of the second joint is configured as full circumferential rotation in the horizontal plane or limited circumferential rotation, and the angle range of the limited circumferential rotation is greater than or equal to 270 degrees; when the rotation angle of the second joint reaches the boundary value of the preset rotation angle range, a boundary limit signal is generated to stop the continued rotation of the second joint and trigger the generation of a movement signal.

[0033] Furthermore, in step S16, before solving for the angle θ of the second joint, a step of validating the input parameters is also included; The steps for validity verification include: Verify whether the ordinate y and the height z satisfy a first condition, which is expressed as follows: ; If the condition is met, the solution process continues; if not, the current location of the trash exceeds the reachable workspace of the robotic arm, a movement signal is generated to drive the robot body to move, and the relative position between the robot body and the trash is adjusted until the input meets the first condition.

[0034] Specifically, the origin of the three-dimensional coordinate system is configured as the geometric center of the robot body in its initial state, or as a fixed reference point in the corridor space. When the origin is configured as a fixed reference point in the corridor space, the robot body's coordinate position in the three-dimensional coordinate system is obtained in real time through the positioning module on the robot body, and the coordinate position of the garbage in the three-dimensional coordinate system is obtained through the vision recognition device on the robot body.

[0035] Specifically, the z-coordinate z1 of the garbage location is obtained through one of the following methods: Method 1: The robot uses a depth camera to obtain the height information of the garbage relative to the ground plane, and calculates z1 by combining the z coordinate reference value of the ground plane in the three-dimensional coordinate system. Method 2: Using the LiDAR mounted on the robot, the robot scans the corridor floor and garbage point cloud data. Based on the z-axis distribution characteristics of the point cloud data, it identifies garbage point cloud clusters and extracts their average z-coordinate as z1. Method 3: Based on a preset corridor floor height model, combined with the pixel position of the garbage in the two-dimensional image and the camera calibration parameters, z1 is calculated using triangulation.

[0036] Specifically, the z-coordinate z2 of the second joint in the three-dimensional coordinate system is calculated based on the height of the robot body, the lifting angle of the first joint, and the length of the upper arm. z2 = H1 + L1⋅sinϕ, where H1 is the height of the robot body and ϕ is the lifting angle of the first joint. The lifting angle ϕ of the first joint is obtained in real time by the tilt sensor mounted on the robot body, or calculated by the feedback value of the encoder of the first joint.

[0037] Specifically, the calculated angle θ of the second joint is used to generate the target angle command in the joint control signal. The target angle command is sent to the servo driver of the second joint through the motion controller. Based on the target angle command and the current actual angle of the second joint, the servo driver uses a position closed-loop control algorithm to drive the second joint to rotate to the target angle. The position closed-loop control algorithm is either a PID control algorithm or an adaptive fuzzy control algorithm.

[0038] Specifically, during the rotation of the second joint, the angular deviation between the current angle of the second joint and the target angle is monitored in real time. When the absolute value of the angular deviation is less than or equal to the preset positioning accuracy threshold, it is determined that the second joint has reached the target position and the output of the joint control signal is stopped. When the rotation time of the second joint is greater than the preset timeout threshold and the angular deviation is still greater than the positioning accuracy threshold, it is determined that the rotation process is abnormal, a fault alarm signal is generated and the system switches to the safe mode.

[0039] The working space of the gripper is represented as the cylinder in which the gripper can move; The logic for acquiring three-dimensional space includes: First, based on the position of the second joint, determine the radius of rotation of the gripper joint in the horizontal plane, and within this radius of rotation, determine the circular surface in which the gripper joints are distributed; Construct a cylinder with the maximum length of the telescopic rod as the height of the cylinder and the circular surface as the base of the cylinder; This cylinder serves as the working space for the gripper.

[0040] Specifically, the radius of rotation is calculated as the sum of the upper arm length and the forearm length, i.e., R = L1 + L2. Determining a circular surface involves the following steps: With the projection point of the second joint in the horizontal plane as the center and the rotation radius R as the radius, draw a circle in the horizontal plane. The area enclosed by this circle is the circular surface. The construction of a cylinder also includes the following limitations: The coordinates of the center of the cylinder's base are consistent with the coordinates of the projection point of the second joint in the horizontal plane; The axis of the cylinder is parallel to the vertical direction; The cylinder extends upwards from the horizontal plane, and its extension range is from the ground plane to the maximum length of the telescopic rod above the ground plane.

[0041] The boundary determination logic for the gripper's active space includes: Obtain the real-time coordinates (x, y) of the gripper joint in the three-dimensional coordinate system. g y g , z g ); Calculate the radial distance d of the gripper joint relative to the projection point of the second joint in the horizontal plane, d=[(x g -x2) 2 +(y g -y2) 2 ] 1 / 2 ; If the radial distance d is less than or equal to R, and the z-coordinate of the gripper joint is z g Satisfies that z2≤z g ≤z2+L max If so, the gripper location is determined to be within the activity space; If the radial distance d is greater than R, or z g Less than z2, or z g Greater than z2+L max If so, the gripper location is determined not to belong to the activity space; Among them, L max Z is the maximum length of the telescopic rod, and Z2 is the z-coordinate of the second joint in the three-dimensional coordinate system.

[0042] The activity space is dynamically updated during the rotation of the second joint, and the update frequency is consistent with the angle feedback frequency of the second joint. When the second joint rotates to a new angular position, the rotation radius and circular surface are recalculated with the new second joint position, and the cylinder is reconstructed.

[0043] Furthermore, the activity space also includes a safety margin. Safety margin space is defined as the extended space formed after reserving preset safety distances in both the radial and vertical directions of the cylinder. The radial safety distance is configured to be 1 / 4 to 1 / 2 of the robot body width; the height safety distance is configured to be 5% to 10% of the maximum length of the telescopic rod. When the gripper is located in the transition zone between the cylinder and the safety margin space, an early warning signal is generated to indicate that the current gripper position is approaching the boundary of the activity space.

[0044] The current position of the gripper and its distribution in the activity space, including whether it is subordinate or not; The term "membership" refers to the fact that the current gripper is located either inside the activity space or on the surface of the activity space.

[0045] Specifically, the distribution relationship also includes critical membership; The critical membership means that the current gripper is located outside the surface of the activity space and inside the surface of the safety margin space, or is located on the surface of the safety margin space. The term "not belonging" means that the current gripper is located outside the surface of the safety margin space.

[0046] Specifically, the logic for determining distribution relationships includes: Obtain the real-time coordinates (x, y) of the gripper joint in the three-dimensional coordinate system. g y g , z g ); Calculate the radial distance d of the gripper joint relative to the projection point of the second joint in the horizontal plane, d=[(x g -x2) 2 +(y g −y2) 2 ] 1 / 2 ; If the radial distance d is less than or equal to the rotation radius R, and the z-coordinate of the gripper joint is z g satisfying z2≤z g ≤z2+L max If so, it is determined to be a subordinate state; If the radial distance d is greater than the rotation radius R, and less than or equal to R + ΔRsafe , or z g Greater than z2+L max And less than or equal to z2+L max +ΔH safe , or z g Less than z2 and greater than or equal to z2−ΔH safe If so, it is determined to be a critical membership state; If the radial distance d is greater than R + ΔR safe , or z g Greater than z2+L max +ΔH safe , or z g Less than z2−ΔH safe If so, it is determined to be a non-subordinate state; Where, ΔR safe For radial safety distance, ΔH safe This refers to the safe distance in the vertical direction.

[0047] Specifically, there is a mapping relationship between the distribution relationship and the signal generation. When the state is subordinate, the joint control signal generation logic is executed. When the state is critically subordinate, a warning signal is generated. At the same time, the joint control signal generation logic continues to be executed, and the movement speed of the telescopic rod is reduced to below the preset safe speed. When the state is non-subordinate, the output of the joint control signal is stopped, a movement signal is generated to drive the robot body to move, and the state change is monitored in real time until the state changes from non-subordinate to subordinate or critically subordinate.

[0048] Specifically, the preset safety speed is configured to be 30% to 50% of the maximum extension speed of the telescopic pole.

[0049] Specifically, when the state is unaffiliated, the generation of the mobile signal also includes the following steps: Calculate the shortest regression path between the gripper joint and the boundary of the active space; Based on the shortest regression path, the direction and distance of movement of the robot body are determined, so that after the robot body moves, the gripper joint re-enters the activity space. The logic for determining the shortest regression path is as follows: In the horizontal plane, draw a circle with the second joint projection point as the center and the rotation radius R as the radius, calculate the shortest distance and corresponding direction between the gripper joint projection point and this circle, and in the vertical direction, calculate the gripper joint z-coordinate and the interval [z2, z2+L]. max Distance and corresponding direction of the nearest endpoint.

[0050] The logic for generating the joint control signal includes: If the distribution relationship is subordinate, determine whether to generate a joint control signal; if the distribution relationship is non-subordinate, generate a movement signal. The joint control signal is represented as a driving current, which includes the current direction and the current magnitude. The direction of the current is controlled by a current commutator, and the magnitude of the current is controlled by a PWM controller.

[0051] Specifically, the logic for generating joint control signals includes: Obtain the distribution relationship between the current gripper position and the activity space. The distribution relationship includes three states: membership, critical membership, and non-membership. If the distribution relationship is subordinate, the process enters the joint control signal generation and judgment process; if the distribution relationship is critically subordinate, a warning signal is generated, and the process enters the joint control signal generation and judgment process, and the movement speed of the telescopic rod is reduced to a preset safe speed; if the distribution relationship is non-subordinate, the output of the joint control signal is stopped, a movement signal is generated to drive the robot body to move, and the state change is monitored in real time until the state changes from non-subordinate to subordinate or critically subordinate.

[0052] In cases where the distribution relationship is membership or critical membership, it is further determined whether the waste and the forearm are distributed on the same straight line: if the waste and the forearm are already distributed on the same straight line, no joint control signal is generated, and the telescopic rod drive signal is generated directly based on the straight-line distance between the gripper joint and the waste; if the waste and the forearm are not distributed on the same straight line, a joint control signal is generated to drive the second joint to rotate so that the waste and the forearm are distributed on the same straight line.

[0053] Furthermore, the parameter configuration logic for the joint control signal includes: The direction of the current is consistent with the target rotation direction of the second joint. The target rotation direction is determined by the sign of the polar coordinate angle α. When α>0, the direction of the current corresponds to the positive rotation of the second joint; when α<0, the direction of the current corresponds to the negative rotation of the second joint; when α=0, no joint control signal is generated.

[0054] Based on the absolute value |α| of the polar coordinate angle α, the target rotation angle is determined. Based on the angle deviation Δθ between the current actual angle of the second joint and the target rotation angle, the reference value of the drive current is calculated through the PID control algorithm. The PWM controller generates a PWM waveform with adjustable duty cycle according to the reference value to control the magnitude of the drive current. The duty cycle of the PWM waveform is positively correlated with the angle deviation Δθ. When the angle deviation Δθ is less than the preset positioning accuracy threshold, the duty cycle of the PWM waveform gradually decreases to zero to achieve smooth braking of the second joint.

[0055] Furthermore, the logic for generating mobile signals includes: When the distribution relationship is non-membership, perform the following steps: S21, calculate the shortest regression path between the gripper joint and the boundary of the activity space; S22, based on the shortest regression path, determine the direction and distance of movement of the robot body, so that after the robot body moves, the gripper joint re-enters the activity space; S23, Generate a movement signal, which includes a movement direction command and a movement distance command; S24, drive the robot body to move according to the movement direction command and movement distance command; S25, during the movement, the coordinate position of the gripper joint is updated in real time, and the distribution relationship is recalculated; S26, when the distribution relationship changes from non-membership to membership or critical membership, stop the output of the movement signal and switch to the joint control signal generation and judgment process.

[0056] Furthermore, the logic for determining the shortest regression path includes: In the horizontal plane, draw a circle with the projection point of the second joint as the center and the rotation radius R as the radius. Calculate the shortest distance between the projection point of the gripper joint and this circle, and the corresponding direction. In the vertical direction, calculate the z-coordinate of the gripper joint and the interval [z2, z2+L]. max The distance and corresponding direction of the nearest endpoint; the vector synthesis of the shortest distance direction in the horizontal plane and the shortest endpoint direction in the vertical direction to obtain the three-dimensional direction vector of the shortest regression path; the movement distance, taking the larger value between the shortest distance in the horizontal plane and the shortest distance in the vertical direction, to ensure that the gripper joint can completely return to the interior of the activity space.

[0057] Furthermore, there is a priority interlocking mechanism between joint control signals and movement signals, including: When a joint control signal is being output, the generation of a movement signal is prohibited; when a movement signal is being output, the generation of a joint control signal is prohibited; when a warning signal is being output, the joint control signal and the warning signal are allowed to be output simultaneously, but the movement signal and the warning signal are prohibited from being output simultaneously; a priority interlocking mechanism is implemented through the central controller to prioritize multiple signals arriving at the same time.

[0058] The priority order is as follows: First priority: safety protection signals, including boundary limit signals and fault alarm signals; Second priority: warning signals; Third priority: movement signals; Fourth priority: joint control signals; Fifth priority: telescopic rod drive signals. When multiple signals request output at the same time, they are decided according to the above priority order. High-priority signals are output first, and low-priority signals enter the waiting queue. After the high-priority signals have been executed, they are output in the order of the queue.

[0059] Furthermore, the logic for determining the current magnitude, based on the PWM controller, includes the following steps: S31, Obtain the current actual angle θ of the second joint. current and the target rotation angle θ target Calculate the angular deviation Δθ, Δθ = θ target −θ current The angle deviation Δθ is normalized to obtain the normalized angle deviation Δθ. norm , Δθ norm =Δθ / θ max , where θ max This represents the maximum permissible rotation angle of the second joint in a single operation.

[0060] S32, normalize the angle deviation Δθ norm As input to the PID control algorithm, the reference value I of the drive current is calculated. base The output expression of the PID control algorithm is I. base =K p ·Δθ norm +K i ·∫Δθ norm dt+K d ·d(Δθ norm ) / dt, where K p K is the proportionality coefficient. i K is the integral coefficient. d The differential coefficient; the proportional coefficient K p The value ranges from 2.0 A / rad to 5.0 A / rad, and the integral coefficient K i The value ranges from 0.1A / (rad·s) to 0.5A / (rad·s), and the differential coefficient K d The value ranges from 0.05 A·s / rad to 0.2 A·s / rad.

[0061] S33, reference value I for drive current base The current value I is obtained by performing a limiting process. lim ;if I base Greater than the maximum allowable current I max , then I lim =I max ;if I base Less than the minimum allowable current I min , then I lim =I min ;if I base In [I] min I max Within the interval, then I lim =I base Maximum allowable current I max I is determined based on the rated current of the servo motor of the second joint. max=1.2I rated Minimum allowable current I min I is determined based on the starting current required to overcome the static friction of the second joint. min =0.3I rated Among them, I rated This is the rated current of the servo motor for the second joint.

[0062] S34, the current value I after limiting lim The mapping is to the PWM duty cycle D; the mapping relationship is a linear mapping, D=(I lim / I max )·D max ; where D max D is the maximum duty cycle of the PWM controller. max =100%; when I lim =I max When, D=D max ; when I lim =I min When, D=D min , where D min D is the minimum effective duty cycle of the PWM controller. min =5%, used to ensure reliable conduction of power switching devices.

[0063] S35, PWM waveform generation: The PWM controller generates a PWM waveform with a fixed frequency based on the duty cycle D; the frequency f of the PWM waveform... PWM According to the electrical time constant τ of the second joint servo motor e It is determined that fPWM = 1 / (10·τe); the amplitude VPWM of the PWM waveform is equal to the DC bus voltage V of the servo driver. dc The duty cycle D of the PWM waveform during a single PWM cycle T PWM The internal temperature remains constant, T PWM =1 / f PWM .

[0064] S36, during the PWM waveform output process, real-time acquisition of the actual drive current I of the second joint servo motor. actual Calculate the current deviation ΔI, ΔI = I lim -I actual If the absolute value of the current deviation ΔI is greater than the preset current tracking error threshold ΔI th Then, the PWM duty cycle D is fine-tuned; the fine-tuning amount is ΔD, ΔD=K c ·ΔI, where K c K is the current correction factor. cThe value range is 0.01% / A to 0.05% / A; the corrected duty cycle D', D'=D+ΔD, is used as the new duty cycle value, and the process returns to S35 to regenerate the PWM waveform; if the absolute value of the current deviation ΔI is less than or equal to ΔI th If the current PWM duty cycle D remains unchanged, the PWM waveform will continue to be output; the current tracking error threshold ΔI th ΔI is determined based on the current control accuracy requirements. th =0.05·I rated .

[0065] S37, Braking current control: When the absolute value of the angle deviation Δθ is less than or equal to the preset positioning accuracy threshold θ th When the second joint is close to the target position, the PWM duty cycle D is gradually reduced, causing the drive current to decrease at a preset decay slope, thus achieving smooth braking of the second joint; the decay slope k dec The value of k is determined based on the moment of inertia J of the second joint and the current angular velocity ω. dec =J·ω / t brake , where t brake The preset braking time; when the PWM duty cycle D drops to zero, the PWM waveform output stops and the joint control signal terminates.

[0066] Furthermore, in step S31, the logic for determining the target rotation angle includes: Based on the polar coordinate angle α, calculate the target angle that the second joint needs to rotate; if the absolute value of the polar coordinate angle α, |α|, is greater than the preset angle threshold α... th , then θ target = θ current + α; If the absolute value of the polar coordinate angle α, |α|, is less than or equal to the preset angle threshold α. th If the debris and forearm are already distributed on the same straight line, no joint control signal is generated, θ target No value assigned; angle threshold α th The settings are based on the rotational accuracy of the second joint and the positioning accuracy of the robotic arm, α th The value ranges from 0.5 degrees to 2 degrees.

[0067] Furthermore, in step S32, the adaptive adjustment logic of the PID control algorithm includes: During the acceleration phase of the second joint rotation, the proportionality coefficient K is increased. p To shorten the response time; during the uniform rotation phase of the second joint, increase the integral coefficient K. i To eliminate steady-state error; during the deceleration phase of the second joint rotation, the differential coefficient K is increased. dTo suppress overshoot; the determination of the acceleration phase, constant speed phase, and deceleration phase is based on the rate of change of the angle deviation Δθ, d(Δθ) / dt: When |d(Δθ) / dt| is greater than the preset acceleration threshold and Δθ and d(Δθ) / dt have the same sign, it is determined to be an acceleration phase; when |d(Δθ) / dt| is less than the preset constant speed threshold, it is determined to be a constant speed phase; when |d(Δθ) / dt| is greater than the preset deceleration threshold and Δθ and d(Δθ) / dt have opposite signs, it is determined to be a deceleration phase.

[0068] Furthermore, in step S35, the modulation method of the PWM waveform is either bipolar PWM modulation or unipolar PWM modulation. When bipolar PWM modulation is used, the output voltage of the PWM waveform is at +V. dc and −V dc Switching between forward and reverse directions is suitable for applications where the second joint requires rapid forward and reverse switching; when using unipolar PWM modulation, the output voltage of the PWM waveform is +V. dc Switch between 0 and -V, or between -V and 0. dc Switching between 0 and 0 is suitable for situations where the second joint rotates in one direction; the modulation method is selected based on the target rotation angle θ. target With the current actual angle θ current The sign of the difference is determined when θ target > θ current When θ is at that time, positive unipolar PWM modulation is used; when θ is at that time, positive unipolar PWM modulation is used. target < θ current When the direction is reversed, unipolar PWM modulation is used; when frequent commutation is required, bipolar PWM modulation is switched.

[0069] If a movement signal is generated, the robot body is driven to move so that the distribution relationship becomes a membership relationship; The logic for determining whether a joint control signal has been generated includes: Determine whether the debris and the forearm are distributed on the same straight line; If yes, then no joint control signal is generated; otherwise, a joint control signal is generated. The robot body moves using wheels at its bottom.

[0070] Specifically, the logic for determining whether a joint control signal has been generated is based on the relationship between the current gripper's position and the distribution of the activity space, as well as the spatial collinearity between the debris and the forearm, and is used for graded determination, including: First-level determination: If the current gripper position and the distribution relationship of the activity space are not subordinate, a movement signal is generated to drive the robot body to move, so that the distribution relationship changes to subordinate or critical subordinate; the robot body moves through the wheels on its bottom; if the current gripper position and the distribution relationship of the activity space are critical subordinate, a warning signal is generated, and at the same time, a movement signal is generated to drive the robot body to move, so that the distribution relationship changes to subordinate; if the current gripper position and the distribution relationship of the activity space are subordinate, then proceed to the second-level determination. Second-level judgment: Under the condition of a subordinate distribution relationship, determine whether the garbage and the forearm are distributed on the same straight line; if so, no joint control signal is generated, and a telescopic rod drive signal is generated directly based on the straight-line distance between the gripper joint and the garbage, driving the telescopic rod to extend or retract by the length of that straight-line distance, so that the gripper reaches the location of the garbage; if not, a joint control signal is generated to drive the second joint to rotate, so that the garbage and the forearm are distributed on the same straight line; after the second joint rotates to the correct position, a telescopic rod drive signal is generated again based on the straight-line distance between the gripper joint and the garbage, driving the telescopic rod to extend or retract by the length of that straight-line distance, so that the gripper reaches the location of the garbage; The third level of determination: Before generating the telescopic rod drive signal, it is determined whether the straight-line distance is within the effective stroke range of the telescopic rod. If yes, the telescopic rod drive signal is generated directly; if not, the movement signal is regenerated to drive the robot body to move and adjust the relative position between the robot body and the garbage until the straight-line distance is within the effective stroke range of the telescopic rod, and then the telescopic rod drive signal is generated again. The effective stroke range of the telescopic rod is represented as the interval between the minimum extension length and the maximum extension length of the telescopic rod.

[0071] Furthermore, in the first-level determination, the generation of the mobile signal also includes a mobile path planning step, including: Obtain the corridor environment map information between the robot's current position and the target position; Based on the corridor environment map information, the robot's movement path is planned, avoiding fixed and temporary obstacles in the corridor; Fixed obstacles include stair handrails, fire hydrants, and electrical distribution boxes; temporary obstacles include pedestrians, temporarily stored items, and open doors and windows. Once the movement path is planned, a movement signal is generated that includes movement direction instructions, movement distance instructions, and movement speed instructions.

[0072] Furthermore, in the second-level judgment, the logic for determining whether the waste and the forearm are distributed on the same straight line includes: Calculate the polar coordinate angle α of the garbage relative to the second joint in the horizontal plane, α=arctan(Δy / Δx), where Δy is the vertical coordinate offset of the garbage relative to the second joint, and Δx is the horizontal coordinate offset of the garbage relative to the second joint. The absolute value of the polar coordinate angle α is compared with a preset angle threshold. If the absolute value of the polar coordinate angle α is less than or equal to the preset angle threshold, it is determined that the garbage and the forearm are distributed on the same straight line; if the absolute value of the polar coordinate angle α is greater than the preset angle threshold, it is determined that the garbage and the forearm are not distributed on the same straight line. The preset angle threshold is set based on the rotational accuracy of the second joint and the positioning accuracy of the robotic arm, and the value ranges from 0.5 degrees to 2 degrees.

[0073] Furthermore, the second-level determination, during the rotation of the second joint, also includes a real-time monitoring step, including: Real-time monitoring of the angle deviation between the current angle and the target angle of the second joint; When the absolute value of the angle deviation is less than or equal to the preset positioning accuracy threshold, it is determined that the second joint has reached the target position, the output of the joint control signal is stopped, and the generation of the telescopic rod drive signal is switched; when the rotation time of the second joint is greater than the preset timeout threshold and the angle deviation is still greater than the positioning accuracy threshold, it is determined that the rotation process is abnormal, a fault alarm signal is generated, and the system switches to safety mode.

[0074] Furthermore, in the third-level determination, the logic for determining the effective travel range of the telescopic rod includes: Obtain the minimum extension length L of the telescopic pole min and maximum extension length L max ; The effective travel range of the telescopic boom is represented as the interval [Lmin, Lmax]. Minimum extension length L min , represents the length of the telescopic rod after it is fully retracted; maximum extended length L max The value is set based on the sum of the forearm length and the upper arm length, L. max It is less than or equal to the sum of the forearm length and the upper arm length.

[0075] Furthermore, the robot body moves via wheels on its bottom, including drive wheels and omnidirectional wheels; Drive wheels, equipped with independent servo motors, provide the robot body with the power to move; omnidirectional wheels are used to assist the robot body in steering and maintaining balance. The movement methods of the robot body also include differential steering and omnidirectional movement; Differential steering achieves robot steering by controlling the speed difference between the left and right drive wheels; omnidirectional movement enables the robot to move in any direction on the horizontal plane by configuring Mecanum wheels or omnidirectional wheels.

[0076] If no joint control signal is generated, the telescopic rod is driven to extend or retract by the length of the straight-line distance between the gripper joint and the garbage. If a joint control signal is generated, the second joint is driven to rotate so that the gripper joint and the garbage are on the same straight line. Then, based on the straight-line distance between the gripper joint and the garbage, the telescopic rod is driven to extend or retract the length of that straight-line distance.

[0077] Specifically, the coordinate position (x, y) of the gripper joint in the three-dimensional coordinate system. g y g , z g And the coordinates of the garbage in the three-dimensional coordinate system (x1, y1, z1); Calculate the radial distance d between the gripper joint and the debris in the horizontal plane. xy d xy =[(x1-x g ) 2 +(y1−y g ) 2 ] 1 / 2 ; Calculate the vertical height difference d between the gripper joint and the debris. z d z =|z1−z g |; Straight-line distance, expressed as spatial straight-line distance d line d line =(d xy 2 +d z 2 ) 1 / 2 .

[0078] The telescopic control logic for the telescopic pole includes: Get the current extension length L of the telescopic pole current ; If the straight-line distance d line Greater than the current extension length L of the telescopic pole current This generates a signal to extend the telescopic rod, driving it to extend outward at a preset speed, with an extension length of d. line -L current ; If the straight-line distance d line Less than the current extension length L of the telescopic pole current This generates a telescopic rod retraction signal, driving the telescopic rod to retract inward at a preset retraction speed, with a retraction length of L.current -d line If the straight-line distance d line Equal to the current extended length L of the telescopic pole current If the telescopic rod is not driven, the opening and closing action of the gripper will be executed directly.

[0079] The extension and retraction speeds of the telescopic pole are configured separately: the extension speed is configured to be 60% to 80% of the maximum extension speed of the telescopic pole to ensure the positioning accuracy of the grabber during the approach to the waste; the retraction speed is configured to be 80% to 100% of the maximum extension speed of the telescopic pole to improve the reset efficiency after waste collection.

[0080] The safety monitoring logic during the extension and retraction of the telescopic pole includes: Real-time monitoring of the current extension length of the telescopic pole; if the current extension length reaches the maximum extension length L of the telescopic pole... max And the straight-line distance d line Still greater than L max If the position of the garbage exceeds the effective travel range of the telescopic rod, the output of the telescopic rod drive signal is stopped, and a movement signal is generated to drive the robot body to move and adjust the relative position between the robot body and the garbage. Real-time monitoring of the current extension length of the telescopic pole; if the current extension length reaches the minimum extension length L of the telescopic pole... min And the straight-line distance d line Still less than L min If the garbage is too close to the gripper joint, the output of the telescopic rod drive signal is stopped, and a movement signal is generated to drive the robot body to move and adjust the relative position between the robot body and the garbage. The drive current during the telescopic pole's extension and retraction process is monitored in real time. If the instantaneous value of the drive current exceeds the preset overload current threshold, it is determined that the telescopic pole's movement is obstructed, a fault alarm signal is generated, and the system switches to a safe mode. The overload current threshold is determined based on the rated current of the telescopic pole's drive motor and is 1.5 times the rated current.

[0081] The grabbing control logic after the gripper reaches the location of the trash includes: When the telescopic pole extends to a straight distance d line Once the position is stable, a gripper closing signal is generated, driving the gripper's clamping part to perform a closing action; The gripper closing signal includes clamping force control commands and closing stroke control commands; The clamping force control command is configured based on the type of waste and the estimated weight. The type of waste is identified by the vision recognition module on the robot body, and the estimated weight is measured in real time by the pressure sensor on the robot body during the grasping process. The closed stroke control command is configured based on the outer contour dimensions of the waste in three-dimensional space. The outer contour dimensions are calculated from the point cloud data obtained by the vision recognition module.

[0082] The recovery control logic after the gripper closes includes: Once the gripper closes and the clamping force reaches a preset stable threshold, a telescopic rod retraction signal is generated, driving the telescopic rod to retract to a preset retraction length. The preset retraction length is configured as the minimum extension length L of the telescopic rod. min With the maximum extension length L of the telescopic pole max The intermediate value between these values, or dynamically adjusted according to the size of the waste, to ensure that the waste does not collide with the robot body and other parts of the robotic arm; After the telescopic rod retracts to the preset retraction length, a first joint control signal is generated, driving the first joint to rise or fall, transporting the garbage to the top of the garbage bin mounted on the robot body; after the garbage reaches the top of the garbage bin, a gripper opening signal is generated, driving the gripper's clamping part to perform an opening action and release the garbage; After the waste is released, a reset signal is generated for the telescopic rod, which drives the telescopic rod to retract to its minimum extended length L. min Simultaneously, a first joint reset signal is generated, driving the first joint to return to its initial lifting angle.

[0083] The driving current is obtained through analysis of historical data; The sound intensity of the joint is represented as the sound intensity of the second joint, and the sound intensity characterizes the friction state of the second joint during operation; The Stribeck friction model is used to generate high-frequency, low-amplitude currents to suppress static friction and improve the smoothness of low-speed motion.

[0084] Specifically, the sound intensity of the joint is collected in real time by an acoustic sensor array mounted on the robot body; the acoustic sensor array includes at least two microphones, which are respectively arranged on both sides of the second joint to eliminate the interference of ambient background noise. Sound intensity is represented by the sound pressure level of the structural acoustic signal generated during the operation of the second joint. The sound pressure level is calculated by bandpass filtering and root mean square value of the collected sound signal. The frequency range of the bandpass filter is configured to be from 100Hz to 4000Hz to cover the characteristic frequencies generated by joint bearing friction and gear meshing. The Stribeck friction model describes the nonlinear relationship between the friction force and the relative sliding velocity of the second joint during low-speed operation. The nonlinear relationship is expressed as follows: ; in, For the total friction force, Coulomb friction, For the maximum static friction force, The coefficient of viscous friction is... The relative sliding velocity of the second joint. For Stribeck speed.

[0085] A high-frequency, low-amplitude current is generated by comparing the sound pressure level with a preset sound pressure level reference value. If the sound pressure level is greater than the preset sound pressure level reference value, the second joint is determined to be in a mixed friction or boundary friction state, and a high-frequency, low-amplitude compensation current is generated. If the sound pressure level is less than or equal to the preset sound pressure level reference value, the second joint is determined to be in a fluid lubrication state, and no compensation current is generated.

[0086] The frequency of the compensation current is configured as the reciprocal of the electrical time constant of the joint servo motor, and the amplitude of the compensation current is configured as 5% to 15% of the drive current reference value.

[0087] The superposition of the compensation current and the drive current is achieved through a current synthesis circuit. The current synthesis circuit superimposes the compensation current as an AC component onto the DC component of the drive current to form a synthesized drive current. The output of the synthesized drive current causes the second joint to generate micro-vibration during low-speed rotation. The amplitude of the micro-vibration is configured to not exceed 10 times the resolution of the joint encoder in order to break the viscous state of static friction and reduce the impact of dead zone friction on positioning accuracy.

[0088] Furthermore, the logic for determining the pressure rating baseline value includes: The acoustic signal of the second joint under no-load uniform speed operation is collected, and the no-load sound pressure level is calculated as the initial reference value. During the actual operation of the second joint, the sound pressure level is dynamically updated in the form of a sliding time window. The duration of the sliding time window is configured to be 1 to 5 seconds. Dynamic update means calculating the average sound pressure level within the current sliding time window and using this average value as the updated sound pressure level reference value.

[0089] The calculation logic for relative sliding speed includes: Obtain the target rotational angular velocity and the current actual angular velocity of the second joint; calculate the angular velocity deviation, which is expressed as the difference between the target rotational angular velocity and the current actual angular velocity; multiply the angular velocity deviation by the equivalent rotation radius of the second joint to obtain the relative sliding speed; the equivalent rotation radius is configured as the radius of the output shaft of the second joint reducer.

[0090] The parameter F in the Stribeck friction model c F s F v and v s The identification logic includes: In the offline calibration phase of the second joint, the second joint is driven to rotate using a preset angular velocity sequence, which covers the range from rest to the rated angular velocity. At each angular velocity point, the driving current and output torque of the second joint are collected, and the friction torque corresponding to that angular velocity point is calculated. Based on the friction torque data corresponding to each angular velocity point, the parameters of the Stribeck friction model are obtained by fitting using the least squares method. The offline calibration phase is performed before the robot leaves the factory or during the maintenance cycle, and the calibration results are stored in the non-volatile memory of the joint servo driver.

[0091] Furthermore, the adaptive adjustment logic for the amplitude of the compensation current includes: Calculate the sound pressure level deviation between the current sound pressure level and the sound pressure level reference value; if the sound pressure level deviation is greater than the preset sound pressure level deviation threshold, increase the amplitude of the compensation current, and the increase is positively correlated with the sound pressure level deviation; if the sound pressure level deviation is less than or equal to the preset sound pressure level deviation threshold, keep the current compensation current amplitude unchanged. The maximum value of the compensation current amplitude is limited to 20% of the driving current reference value to avoid excessive micro-vibrations from damaging the joint's mechanical structure.

[0092] The start / stop control logic for the compensation current includes: During the start-up phase of the second joint rotation, if the current actual angular velocity is less than the preset low-speed threshold, the output of the compensation current is activated. During the constant-speed phase of the second joint rotation, if the current actual angular velocity is greater than or equal to the preset low-speed threshold, the output of the compensation current is stopped. During the braking phase of the second joint rotation, if the current actual angular velocity decreases from greater than or equal to the preset low-speed threshold to less than the preset low-speed threshold, the output of the compensation current is restarted. The low-speed threshold is determined according to the Stribeck speed and is configured to be 1.5 to 3 times the Stribeck speed.

[0093] In one embodiment, such as Figures 1-3The robot's main control core uses the ESP32-S3 development board, serving as the central hub for overall task scheduling. It is responsible for parsing visual data, real-time reading of two HC-SR04P ranging values, controlling chassis movement, scheduling robotic arm actions, and coordinating the entire inspection process. This chip boasts stable computing performance and ample resources, effectively supporting the multi-task scheduling needs of small mobile robots. It can support parallel scheduling of ultrasonic signal acquisition, serial communication, motor speed control, and servo motor control. High-speed and stable communication with the K230D BOX vision module is achieved through a hardware serial port, with multiple GPIOs reserved for interface with ultrasonic Trig and Echo pins. Two HC-SR04P ultrasonic ranging modules are symmetrically installed on the left and right front sides of the robot's chassis. Hardware parameters: effective ranging range 2cm~400cm, fast response speed, unaffected by object color, texture, or lighting, can effectively identify visually indistinguishable physical obstacles such as hotel room doors, fire hydrants, and cleaning carts in hotel corridors; Wiring scheme: the Trig pin and Echo pin of each module are connected to the independent GPIO port of ESP32-S3, the main control cyclically outputs trigger pulses and collects the echo duration to calculate the actual distance to the obstacle; Power supply isolation: the ultrasonic module uses an independent regulated power supply branch to isolate voltage fluctuations caused by motor start and stop, avoiding ranging data jumps and false triggers.

[0094] At the landing of the third-floor staircase in the hotel, there was a discarded 500ml plastic beverage bottle on the ground. The robot was parked about 1.2 meters away from the bottle, in standby mode.

[0095] Robot body parameters: Body height 450mm; Upper arm length 350mm; Forearm length 280mm; Minimum extension length of telescopic rod 50mm, maximum extension length 500mm; Second joint (horizontal rotation joint) preset rotation angle range: -135° to +135° (limited circumferential rotation, total 270°); Initial posture of the robotic arm: First joint lifting angle 30° (upper arm raised upwards); Second joint current angle 0° (forearm pointing directly forward of the robot); Current extension length of telescopic rod 100mm; Gripper in open state.

[0096] The depth camera mounted on the robot body identifies the beverage bottle and obtains its coordinate position in a three-dimensional coordinate system. This three-dimensional coordinate system takes the geometric center position of the robot body in the initial state as the origin, the x-axis points to the length of the corridor, the y-axis points to the width of the corridor, and the z-axis is perpendicular to the ground plane and upwards.

[0097] The following coordinates were obtained using a depth camera: beverage bottle coordinates: (800mm, 900mm, 0mm); current coordinates of the second joint: (0mm, 0mm, 625mm). The horizontal offset Δx is 800mm, Δy is 900mm, and the polar coordinate angle α is 48.37°. The preset angle threshold is 1°. Since 48.37° is greater than 1°, it is determined that the beverage bottle and the forearm are not on the same straight line, and a joint control signal needs to be generated to drive the second joint to rotate. The height of the trash relative to the second joint is 625mm; the vertical coordinate of the trash in the ground plane is 1204.16mm; therefore, the first condition is not met, and it is determined that the current position of the beverage bottle exceeds the reachable workspace of the robotic arm. Since the beverage bottle is outside the reachable workspace of the robotic arm, the system generates a movement signal to drive the robot body to move. Shortest regression path calculation: The maximum radius of the workspace that the robotic arm can reach in the horizontal plane is 630mm; the radial distance of the current beverage bottle from the projection point of the second joint in the horizontal plane is 1204.16mm; the distance to be shortened in the horizontal plane is 574.16mm, so the direction of movement is in the opposite direction from the projection point of the second joint to the projection point of the beverage bottle, that is, the opposite direction to α (-48.37° direction); Robot body movement: The differential wheel at the bottom of the robot drives the robot to move about 600mm in the target direction (leaving a margin). During the movement, the lidar on the robot scans the corridor environment in real time to avoid fixed obstacles such as stair railings. After the movement is completed, the coordinates of the beverage bottle are remeasured as (500mm, 400mm, 0mm).

[0098] Recalculate the offset Δx = 500mm, Δy = 400mm; the polar coordinate angle α is 38.66°; the vertical coordinate of the trash on the ground plane is 640.31mm, and the height of the trash relative to the second joint is 625mm; the first condition is still not met, so continue to move the robot body to shorten the distance again; After the second movement, the coordinates of the beverage bottle were measured as (300mm, 200mm, 0mm), which still did not meet the first condition. After the third fine-tuning, the coordinates were (70mm, 35mm, 0mm), which met the first condition, and the process could continue.

[0099] The target rotation angle is 26.57°, the angle deviation is 26.57°, the maximum allowable rotation angle of the second joint in a single operation is 135°, the reference current is 0.6A, the PWM duty cycle is 25%, the PWM frequency is 50Hz, and the current direction is α > 0, corresponding to the positive rotation of the second joint.

[0100] After the second joint begins to rotate, the acoustic sensor array on the robot body (with two microphones positioned on either side of the second joint) collects sound signals in real time. The collected sound signals are bandpass filtered from 100Hz to 4000Hz, and the root mean square value is calculated to obtain the sound pressure level (SPL). The currently measured SPL is 72dB, while the reference SPL (dynamically updated via a sliding time window) is 65dB. It is determined that the second joint is in a state of mixed friction or boundary friction, requiring the generation of a high-frequency micro-amplitude compensation current. The compensation current frequency is 500Hz, and the amplitude is 0.048A. This synthesized current causes the second joint to produce micro-vibrations during low-speed rotation, with an amplitude not exceeding 10 times the resolution of the joint encoder, effectively breaking the viscous state of static friction.

[0101] During the rotation of the second joint, the system monitors in real time: 1) Angle deviation: the deviation between the current angle and the target angle of 26.57°; 2) Rotation time: the duration of rotation; 3) Sound pressure level: continuously monitored to dynamically adjust the compensation current; when the absolute value of the angle deviation is less than or equal to the positioning accuracy threshold of 0.5°, it is determined that the second joint has reached the target position and the joint control signal output is stopped; if the rotation time exceeds the timeout threshold (e.g., 10 seconds) and the angle deviation is still greater than 0.5°, a fault alarm signal is generated and the system switches to safe mode.

[0102] After the second joint rotates into position, calculate the straight-line distance between the gripper joint and the beverage bottle. The current coordinates of the gripper joint (when the telescopic rod is not extended): After the second joint rotates horizontally by 26.57°, the forearm is aligned with the beverage bottle; the distance between the gripper joint and the projection point of the second joint in the horizontal plane is 280mm; the z-coordinate of the gripper joint is 625mm; the coordinates of the beverage bottle (80mm, 40mm, 0mm) have been converted to a local coordinate system with the second joint as the origin, and the radial distance is approximately 83.6mm.

[0103] In fact, at this point, the forearm is already aligned with the direction of the beverage bottle, and it is necessary to calculate the length that the telescopic rod needs to extend: the horizontal distance from the current position of the gripper joint to the beverage bottle is 196.36mm; the vertical distance is 625mm; after the position is adjusted, the gripper joint and the beverage bottle are at the same horizontal height, with a straight distance of 150mm.

[0104] Telescopic rod control: Current extension length 100mm; Target extension length 150mm; Required extension length: 50mm. Generate telescopic rod extension signal, extension speed configured to 70% of maximum extension speed; After the telescopic rod extends to the correct position, generate gripper closure signal: Visual recognition device identifies the beverage bottle as a lightweight plastic container; Configure appropriate clamping force (e.g., 5N); Calculate the outer contour dimensions of the beverage bottle based on point cloud data, configure the closure stroke; After the gripper closes, the clamping force reaches a stable threshold, confirming successful gripping.

[0105] The robot's movement method is changed from using wheels on its bottom to a tracked movement mechanism.

[0106] Specifically, the tracked mobile mechanism includes drive tracks, tension wheels, load-bearing wheels, drive motors, and suspension buffer components; The drive track surrounds the outer periphery of the load-bearing wheel and the tension wheel. The ground contact section of the drive track forms a ground pressure with the ground. The ground pressure is less than or equal to a preset pressure threshold, which is determined based on the bearing capacity of the stairwell floor. The tensioning wheel, located at the front or rear end of the drive track, is used to adjust the tension of the drive track. The tension adjustment range is configured to keep the sag of the drive track within a preset sag range. The load-bearing wheels are evenly distributed along the ground contact section of the drive track. The wheel spacing of the load-bearing wheels is configured to be less than or equal to the tread width of the stair step, so that when the tracked moving mechanism crosses the step, at least two load-bearing wheels are simultaneously supported on the tread of the same step. The drive motor is connected to the drive wheel of the drive track through a reducer. The output speed of the drive motor is fed back to the central controller in real time through an encoder. The central controller calculates the actual moving speed of the robot body based on the encoder feedback value. The suspension buffer assembly, located between the load-bearing wheels and the robot body, is used to absorb the vibration and impact when the tracked mobile mechanism travels on uneven stairwell floors. The buffer stroke of the suspension buffer assembly is configured to be greater than or equal to the maximum value of the unevenness of the stairwell floor.

[0107] It should be noted that this invention, based on the second joint angle, upper arm length, and forearm length, models the gripper's activity space as a cylinder with the second joint projection point as the center, the sum of the arm lengths as the radius, and the maximum length of the telescopic rod as the height. A safety margin is set, allowing for dynamic updates to the activity space as the second joint rotates. It also provides early warning and deceleration when the gripper approaches the boundary, effectively avoiding hard-limit impacts and improving the robotic arm's operational safety in narrow spaces like stairwells. Furthermore, it uses an acoustic sensor array to collect the sound pressure level of the second joint in real time, combined with the Stribeck friction model, to detect when the joint is under mixed friction or... In boundary friction conditions, a high-frequency micro-amplitude compensation current is automatically superimposed to generate controllable micro-amplitude vibrations in the joint to break the static friction and viscosity state. This significantly reduces the dead zone friction force in the low-speed stage, improves the positioning accuracy and motion stability of the joint, and extends the service life of the joint's mechanical structure. Priority sorting of safety protection signals, warning signals, movement signals, joint control signals, and telescopic rod drive signals has been established, as well as interlocking logic between joint control signals and movement signals. This effectively avoids actuator malfunctions caused by signal conflicts and ensures that each execution unit of the robotic arm works in a coordinated and orderly manner in the complex environment of the corridor.

[0108] The various embodiments in this specification are described in a progressive manner. For directly identical or similar parts of the embodiments, refer to each other. Each embodiment focuses on its differences from other embodiments. In particular, the system embodiments are basically similar to the method embodiments, so the description is relatively simple; relevant parts can be referred to the descriptions in the method embodiments. It should be noted that the technical features of the above embodiments can be combined arbitrarily. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as the combination of these technical features does not contradict each other, it should be considered within the scope of this specification.

[0109] The above-described embodiments are merely preferred embodiments of the present invention, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of the invention. It should be noted that those skilled in the art can make various improvements and substitutions without departing from the principles of the present invention, and these improvements and substitutions should also be considered within the scope of protection of the present invention. Therefore, the scope of protection of this invention should be determined by the aforementioned scope.

Claims

1. A robotic arm control system for a small corridor inspection and cleaning robot, characterized in that, The system includes a signal generation module and a signal compensation module; The signal generation module determines the activity space of the gripper based on the angle of the second joint and the length of the forearm, and generates a movement signal or determines whether to generate a joint control signal based on the current position of the gripper and the distribution relationship of the activity space. The signal compensation module responds to the joint control signal, configures the drive current to drive the corresponding joint, and monitors the sound intensity of the joint in real time during the control process. Based on the sound intensity and the Stribeck friction model, it compensates the drive current. The compensation is expressed as the superposition of a high-frequency, low-amplitude current with the driving current to reduce the dead zone friction of the joint.

2. The robotic arm control system of the small corridor inspection and cleaning robot as described in claim 1, characterized in that, The robot's robotic arm includes a first joint, a second joint, and a gripper joint; The part connecting the first joint and the second joint is the upper arm, the part connecting the second joint and the gripper joint is the forearm, the end of the gripper joint is equipped with a gripper, and the part connecting the gripper and the gripper joint is a telescopic rod. The first joint is used to drive the robotic arm to lift or lower; the second joint is used to drive the robotic arm to rotate in the horizontal plane; and the gripper joint is used to drive the extension and retraction of the telescopic rod and the opening and closing of the gripper. The gripper is used to grab or release the garbage.

3. The robotic arm control system of a small corridor inspection and cleaning robot as described in claim 1, characterized in that, The angle of the second joint is calculated using the vertical coordinate of the waste on the ground plane, the length of the upper arm, the length of the forearm, and the height of the waste relative to the second joint. The vertical coordinate of the garbage on the ground plane, i.e., the y-coordinate, is obtained in a three-dimensional coordinate system. The height of the garbage relative to the second joint is expressed as the difference between the z-coordinate of the second joint and the z-coordinate of the garbage's location.

4. The robotic arm control system of a small corridor inspection and cleaning robot as described in claim 1, characterized in that, The working space of the gripper is represented as the cylinder in which the gripper can move; The logic for acquiring three-dimensional space includes: First, based on the position of the second joint, determine the radius of rotation of the gripper joint in the horizontal plane, and within this radius of rotation, determine the circular surface in which the gripper joints are distributed; Construct a cylinder with the maximum length of the telescopic rod as the height of the cylinder and the circular surface as the base of the cylinder; This cylinder serves as the working space for the gripper.

5. The robotic arm control system of a small corridor inspection and cleaning robot as described in claim 1, characterized in that, The current position of the gripper and its distribution in the activity space, including whether it is subordinate or not; The term "membership" refers to the fact that the current gripper is located either inside the activity space or on the surface of the activity space.

6. The robotic arm control system of a small corridor inspection and cleaning robot as described in claim 1, characterized in that, The logic for generating the joint control signal includes: If the distribution relationship is subordinate, determine whether to generate a joint control signal; if the distribution relationship is non-subordinate, generate a movement signal. The joint control signal is represented as a driving current, which includes the current direction and the current magnitude. The direction of the current is controlled by a current commutator, and the magnitude of the current is controlled by a PWM controller.

7. The robotic arm control system of a small corridor inspection and cleaning robot as described in claim 1, characterized in that, If a movement signal is generated, the robot body is driven to move so that the distribution relationship becomes a membership relationship; The logic for determining whether a joint control signal has been generated includes: Determine whether the debris and the forearm are distributed on the same straight line; If yes, then no joint control signal is generated; otherwise, a joint control signal is generated. The robot body moves using wheels at its bottom.

8. The robotic arm control system of a small corridor inspection and cleaning robot as described in claim 1, characterized in that, If no joint control signal is generated, the telescopic rod is driven to extend or retract by the length of the straight-line distance between the gripper joint and the garbage. If a joint control signal is generated, the second joint is driven to rotate so that the gripper joint and the garbage are on the same straight line. Then, based on the straight-line distance between the gripper joint and the garbage, the telescopic rod is driven to extend or retract the length of that straight-line distance.

9. The robotic arm control system of a small corridor inspection and cleaning robot as described in claim 1, characterized in that, The driving current is obtained through analysis of historical data; The sound intensity of the joint is represented as the sound intensity of the second joint, and the sound intensity characterizes the friction state of the second joint during operation; The Stribeck friction model is used to generate high-frequency, low-amplitude currents to suppress static friction and improve the smoothness of low-speed motion.

10. The robotic arm control system of a small corridor inspection and cleaning robot as described in claim 7, characterized in that, The robot's movement method is changed from using wheels on its bottom to a tracked movement mechanism.

Images