Underground support transfer robot forearm pose sensing method
Through multi-sensor data fusion and Kalman gain algorithm, the problem of inaccurate attitude perception in harsh light and rugged ground environments is solved, and more accurate attitude information output and operation accuracy are achieved.
Patent Information
- Application Number
- CN202411450206.4
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-10-17
- Publication Date
- 2024-11-15
- Estimated Expiration
- Not applicable · inactive patent
AI Technical Summary
In the harsh light and rugged ground environment, traditional visual perception methods lead to inaccurate perception of forearm posture, affecting the robot's working accuracy.
Using multi-sensor data fusion method, combining vision sensors and attitude perception sensors, the updated state estimates and covariance matrix are calculated through the Kalman gain algorithm, and the position dynamic correction is performed to output more accurate Cartesian coordinate system positions and Euler. Angle posture information.
It improves the accuracy and robustness of posture perception, recognizes and corrects posture perception errors caused by rugged light and ground, and improves the robot's operation accuracy.
Smart Images

Figure CN118952235A_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the field of robot perception technology, and in particular relates to a method for perceiving the position and posture of a forearm of an underground support transporting robot. Background Art
[0002] The underground advance support bracket is used to support and stabilize the tunnel roof structure. Transporting the advance support bracket underground often requires a large number of people to move it, which is prone to accidents. Therefore, it is very important to realize unmanned intelligent transportation of the bracket handling robot. Intelligent perception of the forearm posture is the key technology for the bracket handling robot to realize intelligent transportation, which can improve the working conditions of the workers and reduce underground safety accidents.
[0003] Today's underground support handling robots work in a harsh environment with dim light and dust. Traditional visual perception of the forearm posture cannot meet the underground working conditions. At the same time, the bottom of the tunnel is rugged and uneven, and large pits and puddles may appear in front of the handling robot. When the forearm supports the support, the position and posture information output by the sensor will produce inertial errors, resulting in inaccurate posture perception, affecting the working accuracy of the robot. Therefore, a method for perceiving the forearm posture of an underground support handling robot is urgently needed. Summary of the invention
[0004] The purpose of the present invention is to solve the problems that the working environment of today's underground support handling robots is harsh, the light is dim, and there is dust. The traditional visual perception of the forearm posture cannot meet the underground working conditions. At the same time, the bottom of the tunnel is rugged, and large pits and puddles may appear in front of the handling robot, which will cause the forearm to support the support. The position and posture information output by the sensor will produce inertial errors, thereby causing inaccurate posture perception and affecting the working accuracy of the robot. A method for perceiving the forearm posture of an underground support handling robot is proposed.
[0005] In order to achieve the above-mentioned purpose, the present invention adopts the following technical solution: a method for sensing the position and posture of the forearm of an underground support handling robot, comprising the following steps: S1. Collect data; S2, preprocessing the collected data; S3, establish mathematical models and systems; S4, calculating Kalman gain; S5. Calculate the updated state estimate and covariance matrix through the Kalman gain; S6. Output relevant location information; S7. Perform dynamic posture correction.
[0006] Furthermore, the method effectively combines the data of visual sensors and attitude perception sensors, and realizes real-time data integration through fusion algorithms, thereby effectively improving the accuracy and robustness of attitude perception. At the same time, the method can fuse and analyze visual and attitude data to identify and correct attitude perception errors caused by poor lighting conditions and uneven terrain, thereby providing more accurate attitude information and improving the application accuracy of the overall method. Multi-sensor data fusion is also applied in the overall method, and the fused data is further processed and analyzed, so as to effectively identify and correct attitude perception errors caused by poor lighting conditions and uneven terrain.
[0007] As a further description of the above technical solution: In S1, data collection is performed, specifically, using a visual sensor to collect image data of the support handling robot, a posture sensor, a laser tracker, and an articulated arm measuring machine, or one or more of these to record position and angle data of the robot's forearm.
[0008] As a further description of the above technical solution: In S2, the collected data is preprocessed, specifically, denoising and enhancing the visual image, filtering and correcting the posture sensor data, and reducing errors caused by light conditions and robot motion.
[0009] As a further description of the above technical solution: In S3, a mathematical model of the change of the forearm position and posture state over time and a control strategy of the system in the next time step are established to predict the position and posture state of the robot forearm in the next time step.
[0010] As a further description of the above technical solution: In S4, the actually observed data is mapped to the state space, and the Kalman gain is calculated according to the difference between the predicted value and the actual measurement.
[0011] As a further description of the above technical solution: In S5, the predicted state in the prediction phase and the observed value obtained in the measurement phase are combined to calculate the updated state estimation value and covariance matrix through the Kalman gain.
[0012] As a further description of the above technical solution: In S6, the position information of the forearm in the Cartesian coordinate system after fusion optimization is output.
[0013] As a further description of the above technical solution: In the above S7, the posture information of the forearm expressed by Euler angles is output to perform dynamic posture correction.
[0014] As a further description of the above technical solution: In S5, a mathematical model is established to reflect the three-degree-of-freedom position information and posture information of the forearm of the bracket handling robot; wherein the position of the forearm is described by using a Cartesian coordinate system to describe the position of the forearm. Assume that the position of the robot's end effector in the Cartesian coordinate system is f{q}=(x,y,z), wherein x,y,z represent the displacement of the robot in the x,y and z directions respectively; use a group of positive kinematic equations to map the joint angle to the position of the end effector, calculate the position of the end effector by a closed-form solution, and describe the posture angle: wherein the posture of the forearm is described by Euler angles, which include roll, pitch, and yaw angles, and are used to describe the direction of the robot's end effector. The mathematical model is used to reflect the three-degree-of-freedom position information and posture information of the forearm of the bracket handling robot, and to control the position and posture of its end effector in real time.
[0015] As a further description of the above technical solution: In S7, when dynamic posture correction is performed, the predicted state obtained in the prediction stage and the observed value obtained in the measurement stage are combined, and the optimized forearm position information and posture information are calculated through the Kalman gain. This information is output in the form of position and Euler angle in a Cartesian coordinate system to ensure that the support handling robot can adjust its posture in real time during operation to cope with environmental changes and position errors.
[0016] In summary, due to the adoption of the above technical solution, the beneficial effects of the present invention are: In the present invention, the method effectively combines the data of the visual sensor and the attitude perception sensor, realizes real-time data integration through the fusion algorithm, thereby effectively improving the accuracy and robustness of attitude perception, and at the same time, the method can fuse and analyze the visual and attitude data to identify and correct the attitude perception errors caused by poor lighting conditions and rugged terrain, thereby providing more accurate attitude information and improving the application accuracy of the overall method. Multi-sensor data fusion is also applied in the overall method, and the fused data is further processed and analyzed, so that the attitude perception errors caused by poor lighting conditions and rugged terrain can be effectively identified and corrected. BRIEF DESCRIPTION OF THE DRAWINGS
[0017] Figure 1 The present invention is a flow chart of a method for sensing the forearm posture of an underground support handling robot.
[0018] Figure 2 This is the first illustration of the MATLAB simulation results after joint correction by multi-sensor data fusion algorithm in a method for sensing the forearm posture of an underground support handling robot.
[0019] Figure 3This is the second illustration of the MATLAB simulation results after joint correction by multi-sensor data fusion algorithm in a method for sensing the forearm posture of an underground support handling robot. DETAILED DESCRIPTION
[0020] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention. Embodiment 1:
[0021] See also Figure 1 The present invention provides a technical solution: a method for sensing the position and posture of a forearm of an underground support handling robot, comprising the following steps: S1. Perform data collection, specifically, use a visual sensor to collect image data of the bracket handling robot, and a posture sensor to record the position and angle data of the robot's forearm; S2. Preprocess the collected data, specifically, reduce noise and enhance the visual image, filter and correct the attitude sensor data, and reduce errors caused by light conditions and robot motion; S3, establish a mathematical model of the change of forearm position and posture state over time and the control strategy of the system in the next time step, and predict the position and posture state of the robot forearm in the next time step; S4, mapping the actual observed data to the state space, and calculating the Kalman gain based on the difference between the predicted value and the actual measurement; S5. Combine the predicted state in the prediction phase and the observed value obtained in the measurement phase, calculate the updated state estimate and covariance matrix through the Kalman gain, and establish a mathematical model to reflect the three-degree-of-freedom position information and posture information of the forearm of the bracket handling robot; the position of the forearm is described by using the Cartesian coordinate system to describe the position of the forearm. Assume that the position of the robot's end effector in the Cartesian coordinate system is f{q}=(x,y,z), where x,y,z represent the displacement of the robot in the x,y and z directions respectively; use the forward kinematics equations to map the joint angle to the position of the end effector, and calculate the position of the end effector through a closed-form solution. The posture angle description: the posture of the forearm is described by Euler angles, which include roll, pitch, and yaw angles, which are used to describe the direction of the robot's end effector. The mathematical model is used to reflect the three-degree-of-freedom position information and posture information of the forearm of the bracket handling robot, and control the position and posture of its end effector in real time; S6, outputting the position information of the forearm in the Cartesian coordinate system after fusion optimization; S7. Output the posture information of the forearm expressed in Euler angles for dynamic posture correction. When performing dynamic posture correction, the predicted state obtained in the prediction stage and the observed value obtained in the measurement stage are combined, and the optimized forearm position information and posture information are calculated through the Kalman gain. This information is output in the form of position and Euler angles in a Cartesian coordinate system to ensure that the support handling robot can adjust its posture in real time during operation to cope with environmental changes and position errors.
[0022] In this embodiment, the method effectively combines the data of the visual sensor and the posture perception sensor, and realizes real-time data integration through a fusion algorithm, thereby effectively improving the accuracy and robustness of posture perception. Embodiment 2:
[0023] See also Figure 1 The present invention provides a technical solution: a method for sensing the position and posture of a forearm of an underground support handling robot, comprising the following steps: S1. Perform data collection, specifically, use a laser tracker and an articulated arm measuring machine to record the position and angle data of the robot forearm; S2. Preprocess the collected data, specifically, reduce noise and enhance the visual image, filter and correct the attitude sensor data, and reduce errors caused by light conditions and robot motion; S3, establish a mathematical model of the change of forearm position and posture state over time and the control strategy of the system in the next time step, and predict the position and posture state of the robot forearm in the next time step; S4, mapping the actual observed data to the state space, and calculating the Kalman gain based on the difference between the predicted value and the actual measurement; S5. Combine the predicted state in the prediction phase and the observed value obtained in the measurement phase, calculate the updated state estimate and covariance matrix through the Kalman gain, and establish a mathematical model to reflect the three-degree-of-freedom position information and posture information of the forearm of the bracket handling robot; the position of the forearm is described by using the Cartesian coordinate system to describe the position of the forearm. Assume that the position of the robot's end effector in the Cartesian coordinate system is f{q}=(x,y,z), where x,y,z represent the displacement of the robot in the x,y and z directions respectively; use the forward kinematics equations to map the joint angle to the position of the end effector, and calculate the position of the end effector through a closed-form solution. The posture angle description: the posture of the forearm is described by Euler angles, which include roll, pitch, and yaw angles, which are used to describe the direction of the robot's end effector. The mathematical model is used to reflect the three-degree-of-freedom position information and posture information of the forearm of the bracket handling robot, and control the position and posture of its end effector in real time; S6, outputting the position information of the forearm in the Cartesian coordinate system after fusion optimization; S7. Output the posture information of the forearm expressed in Euler angles for dynamic posture correction. When performing dynamic posture correction, the predicted state obtained in the prediction stage and the observed value obtained in the measurement stage are combined, and the optimized forearm position information and posture information are calculated through the Kalman gain. This information is output in the form of position and Euler angles in a Cartesian coordinate system to ensure that the support handling robot can adjust its posture in real time during operation to cope with environmental changes and position errors.
[0024] In this embodiment, the method can fuse and analyze visual and posture data to identify and correct posture perception errors caused by poor lighting conditions and uneven terrain, thereby providing more accurate posture information and improving the application accuracy of the overall method. Embodiment 3:
[0025] See also Figure 1 The present invention provides a technical solution: a method for sensing the position and posture of a forearm of an underground support handling robot, comprising the following steps: S1. Perform data collection, specifically, use an articulated arm measuring machine to record the position and angle data of the robot forearm; S2. Preprocess the collected data, specifically, reduce noise and enhance the visual image, filter and correct the attitude sensor data, and reduce errors caused by light conditions and robot motion; S3, establish a mathematical model of the change of forearm position and posture state over time and the control strategy of the system in the next time step, and predict the position and posture state of the robot forearm in the next time step; S4, mapping the actual observed data to the state space, and calculating the Kalman gain based on the difference between the predicted value and the actual measurement; S5. Combine the predicted state in the prediction phase and the observed value obtained in the measurement phase, calculate the updated state estimate and covariance matrix through the Kalman gain, and establish a mathematical model to reflect the three-degree-of-freedom position information and posture information of the forearm of the bracket handling robot; the position of the forearm is described by using the Cartesian coordinate system to describe the position of the forearm. Assume that the position of the robot's end effector in the Cartesian coordinate system is f{q}=(x,y,z), where x,y,z represent the displacement of the robot in the x,y and z directions respectively; use the forward kinematics equations to map the joint angle to the position of the end effector, and calculate the position of the end effector through a closed-form solution. The posture angle description: the posture of the forearm is described by Euler angles, which include roll, pitch, and yaw angles, which are used to describe the direction of the robot's end effector. The mathematical model is used to reflect the three-degree-of-freedom position information and posture information of the forearm of the bracket handling robot, and control the position and posture of its end effector in real time; S6, outputting the position information of the forearm in the Cartesian coordinate system after fusion optimization; S7. Output the posture information of the forearm expressed in Euler angles for dynamic posture correction. When performing dynamic posture correction, the predicted state obtained in the prediction stage and the observed value obtained in the measurement stage are combined, and the optimized forearm position information and posture information are calculated through the Kalman gain. This information is output in the form of position and Euler angles in a Cartesian coordinate system to ensure that the support handling robot can adjust its posture in real time during operation to cope with environmental changes and position errors.
[0026] In this embodiment, multi-sensor data fusion is also applied in the method, and the fused data is further processed and analyzed, so as to effectively identify and correct the posture perception errors caused by poor lighting conditions and uneven terrain.
[0027] In addition, it should be noted that the MATLAB simulation results after joint correction by multi-sensor data fusion algorithm are shown in the figure. Figure 2-Figure 3 .
[0028] In S5, a mathematical model is established to reflect the three-degree-of-freedom position information and posture information of the forearm of the bracket handling robot; wherein the position of the forearm is described by using a Cartesian coordinate system to describe the position of the forearm. Assume that the position of the robot's end effector in the Cartesian coordinate system is f{q}=(x,y,z), wherein x,y,z represent the displacement of the robot in the x,y and z directions respectively; use a group of positive kinematic equations to map the joint angle to the position of the end effector, calculate the position of the end effector by a closed-form solution, and describe the posture angle: wherein the posture of the forearm is described by Euler angles, which include roll, pitch, and yaw angles, and are used to describe the direction of the robot's end effector. The mathematical model is used to reflect the three-degree-of-freedom position information and posture information of the forearm of the bracket handling robot, and to control the position and posture of its end effector in real time.
[0029] In S5, a mathematical model is established to reflect the three-degree-of-freedom position information and posture information of the forearm of the bracket handling robot; it is assumed that the joint angles of the forearm of the three-degree-of-freedom robot are θ1, θ2 and θ3, and the length of each joint is L1, L2 and L3.
[0030] A1: Calculate the end position coordinates of joint 1: x1=L1cos(θ1) y1=L1sin(θ1) z1=0 A2: Calculate the end position coordinates of joint 2: x2=L1cos(θ1)+L2cos(θ1+θ2) y2=L1sin(θ1)+L2sin(θ1+θ2) z2=0 A3: Calculate the end position coordinates of joint 3: x3= L1cos(θ1) + L2cos(θ1+θ2) + L3cos(θ1+θ2+θ3) y3=L1sin(θ1)+L2sin(θ1+θ2)+ L3sin(θ1+θ2+θ3) z3=0 Based on the derivation of the forward kinematics mathematical model formula of the three-degree-of-freedom robot forearm, the position (p(t+δt)) and posture (φ(t+δt),θ(t+δt),ψ(t+δt)) of the next time step (t+δt) are predicted.
[0031] Among them, the attitude angle description (φ(t+δt), θ(t+δt), ψ(t+δt) is the description of the roll, pitch and yaw angles of the forearm using the Euler angle, where they are the rotation angle φ, the nutation angle θ, and the precession angle (i.e., the precession angle) ψ, respectively. The following symbols have the same explanation and will not be repeated; A4. Control strategy PID controller: can adjust the error between the predicted and actual state to adjust the robot's motion.
[0032] Set the desired position P(E) (t+δt) and posture (φE(t+δt),θE(t+δt),ψE(t+δt)); where (φE(t+δt),θE(t+δt),ψE(t+δt) are the Euler angle descriptions of the desired posture of the forearm, and (t+δt) represents the next time step.
[0033] Calculation error: (φE(t+δt)=P(E)φE(t+δt)-P(t+δt) Desired posture: η(t+δt)=(φE(t+δt)-φ(t+δt), θE(t+δt)- θ(t+δt), ψE(t+δt)- ψ(t+δt)) The PID controller is used to calculate the output u(t+δt) to adjust the position and posture of the robot at the time (t+δt).
[0034] In S5, the predicted state in the prediction phase and the observed value obtained in the measurement phase are combined to calculate the updated state estimate and covariance matrix through the Kalman gain, and a mathematical model is established to reflect the three-degree-of-freedom position information and posture information of the forearm of the bracket handling robot. The specific method of calculating the Kalman gain is as follows: B1. Establish the observation model (H) matrix The observation model (H) is a matrix used to map the state variables of the system into the observation space. The state variables include the position and pose of the robot end effector, and the (H) matrix describes the expected image features and IMU readings calculated from the state variables.
[0035] For image data, the (H) matrix is needed to convert the robot's 3D position and posture information into the expected position and posture features on the 2D image plane. The first is the geometric transformation, which converts the robot's 3D coordinates into the camera's 2D image coordinates. The next step is feature simulation, which calculates the key features that should appear in the image, such as edges, corners, textures, etc., based on the robot's expected position and posture.
[0036] For IMU data, the (H) matrix performs a dynamic mapping, directly mapping the robot's angular velocity and linear acceleration states to IMU readings.
[0037] B2. Set the prior state estimation error covariance matrix (P - ) This matrix represents the uncertainty in the system's estimate of its state before a new observation is obtained. It is usually initialized based on knowledge of the system's past dynamics and external disturbances.
[0038] B3. Set the observation noise covariance matrix (R) The (R) matrix describes the statistical properties of the observation noise, i.e., the uncertainty of the image data and the IMU data. The image data is affected by illumination variations, while the IMU data is affected by the mechanical vibration of the forearm.
[0039] B4. Calculation of Kalman Gain Using the above (H), (P - ), and (R) to calculate the Kalman gain (K), using the formula K=P - H T (HP - H T +R) -1 ,This gain (K) measures how much weight should be given to the observations when updating the state estimate.
[0040] B5. Update state estimate The Kalman gains are combined with new observations from the image and IMU data to update the estimate of the position and attitude of the robot end effector.
[0041] B6. Update the covariance of the estimated errors.
[0042] The above description is only a preferred specific implementation manner of the present invention, but the protection scope of the present invention is not limited thereto. Any technician familiar with the technical field can make equivalent replacements or changes according to the technical scheme and inventive concept of the present invention within the technical scope disclosed by the present invention, which should be covered by the protection scope of the present invention.
Claims
1. A method for sensing the position of the forearm of an underground support handling robot, characterized in that: The steps include: S1. Collect data; S2, preprocessing the collected data; S3, establish mathematical models and systems; S4, calculating Kalman gain; S5. Calculate the updated state estimate and covariance matrix through the Kalman gain; S6. Output relevant location information; S7. Perform dynamic posture correction.
2. The method for sensing the forearm posture of a downhole support handling robot according to claim 1, characterized in that: In S1, data collection is performed, specifically, using a visual sensor to collect image data of the support handling robot, a posture sensor, a laser tracker, and an articulated arm measuring machine, or one or more of these to record position and angle data of the robot's forearm.
3. The method for sensing the position and posture of the forearm of a downhole support handling robot according to claim 1, characterized in that: In S2, the collected data is preprocessed, specifically, denoising and enhancing the visual image, filtering and correcting the posture sensor data, and reducing errors caused by light conditions and robot motion.
4. The method for sensing the forearm posture of a downhole support handling robot according to claim 1, characterized in that: In S3, a mathematical model of the change of the forearm position and posture state over time and a control strategy of the system in the next time step are established to predict the position and posture state of the robot forearm in the next time step.
5. The method for sensing the position and posture of the forearm of a downhole support handling robot according to claim 1, characterized in that: In S4, the actually observed data is mapped to the state space, and the Kalman gain is calculated according to the difference between the predicted value and the actual measurement.
6. The method for sensing the position and posture of the forearm of a downhole support handling robot according to claim 1, characterized in that: In S5, the predicted state in the prediction phase and the observed value obtained in the measurement phase are combined to calculate the updated state estimation value and covariance matrix through the Kalman gain.
7. The method for sensing the position and posture of the forearm of a downhole support handling robot according to claim 1, characterized in that: In S6, the position information of the forearm in the Cartesian coordinate system after fusion optimization is output.
8. The method for sensing the position and posture of the forearm of a downhole support handling robot according to claim 1, characterized in that: In the above S7, the posture information of the forearm expressed by Euler angles is output to perform dynamic posture correction.
9. The method for sensing the position and posture of the forearm of a downhole support handling robot according to claim 1, characterized in that: In said S5, a mathematical model is established to reflect the three-degree-of-freedom position information and posture information of the forearm of the support handling robot; wherein the position of the forearm is described as using a Cartesian coordinate system to describe the position of the forearm; Assume that the position of the robot's end effector in the Cartesian coordinate system is f{q}=(x,y,z), where x,y,z represent the displacement of the robot in the x,y and z directions respectively; use the forward kinematics equations to map the joint angles to the position of the end effector, and calculate the position of the end effector through a closed-form solution. The posture angle description: the posture of the forearm is described by Euler angles, which include roll, pitch, and yaw angles, and are used to describe the direction of the robot's end effector. The mathematical model is used to reflect the position and posture information of the three degrees of freedom of the forearm of the bracket handling robot, and to control the position and posture of its end effector in real time.
10. The method for sensing the position and posture of the forearm of a downhole support handling robot according to claim 1, characterized in that: In S7, when dynamic posture correction is performed, the predicted state obtained in the prediction stage and the observed value obtained in the measurement stage are combined, and the optimized forearm position information and posture information are calculated through the Kalman gain. This information is output in the form of position and Euler angle in a Cartesian coordinate system to ensure that the support handling robot can adjust its posture in real time during operation to cope with environmental changes and position errors.
Citation Information
Patent Citations
Sensing and positioning method of vehicle-mounted mechanical arm
CN111890373A
Self-adaptive positioning optimization and mapping method for mine autonomous detection robot
CN116619358A
Visual and inertial measurement unit-based attitude estimation method and crane jib pose estimation device
CN117842846A
Mechanical arm control method and system based on multi-mode driving and storage medium
CN118752495A
Motion capture method and system of robotic arm, medium, and electronic device
US20230286163A1
