Vehicle sensing and mechanical arm unloading system

Abstract

The invention discloses a vehicle sensing and mechanical arm unloading system, which belongs to the technical field of industrial automation, and comprises the following steps: collecting vehicle attitude information and stacking state information of urea bagged cargoes in real time through a multi-mode sensing device, and generating a three-dimensional attitude model and a cargo distribution diagram of a vehicle; according to the vehicle attitude model and the cargo distribution diagram, the initial position and angle of grabbing the urea bag by the mechanical arm are calculated, a collision-free movement track is planned by combining kinematics constraint of the mechanical arm and environment obstacle information, and the mechanical arm is controlled to execute grabbing operation through a force feedback control algorithm to complete unloading action; vehicle posture change and cargo state information are monitored in real time, if deviation is found, the grabbing position and angle are recalculated, the motion trail of the mechanical arm is updated, and closed-loop control is formed; the method has the characteristics of high adaptability, path planning optimization, closed-loop control and high efficiency, and the automation level and stability of urea loading and unloading are improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of industrial automation, specifically a vehicle perception and robotic arm unloading system. Background Art

[0002] In the production process of the urea workshop, after urea products are produced from the production line, the urea needs to be unloaded from the transport vehicle to the warehouse for storage. Traditional unloading methods mostly rely on manual labor, which has problems such as low efficiency, high labor intensity, great potential safety hazards, and difficulty in meeting the needs of large-scale production. Moreover, it is easy to cause unloading failure or safety accidents due to vehicle attitude changes or messy cargo stacking. With the development of automation technology, robotic arms have been introduced into the unloading process. Existing robotic arm systems lack flexibility and adaptability when facing vehicles of different models and different loading postures, and it is easy to encounter problems with smooth unloading. Moreover, existing automated loading and unloading technologies usually use robotic arms with fixed trajectories, which cannot adapt to vehicle attitude changes or complex stacking environments, resulting in unstable grasping and an increased risk of collision.

[0003] For example, the patent application with the publication number CN118323847A discloses an automatic unloading system and its method, including: a multi-stage telescopic conveyor belt is fixedly connected to the outer surface of the conveyor table, a support frame is fixedly connected to the outer surface of the multi-stage telescopic conveyor belt, a robotic arm is fixedly connected to the top of the support frame, and a connecting seat is fixedly connected to the output end of the robotic arm. In the coverage range of multiple suction cups, each suction cup can be compressed and held according to the size of the goods, ensuring full contact with the surfaces of each good. Therefore, the combination of multiple suction cups can respectively adhere to the surfaces of multiple goods of different sizes for adsorption, enabling the single operation of the unloading system to achieve the adsorption and grasping unloading of multiple goods of different sizes, reducing the number of times of grasping the goods during the unloading process, thereby achieving the purpose of accelerating the unloading progress, realizing automated high-efficiency unloading, and effectively improving the logistics transportation efficiency.

[0004] The above existing technologies have the following problems: They do not mention the perception ability of vehicle attitude, cargo stacking state, or environmental obstacles; they rely on the physical adsorption ability of the suction cups and have a single grasping strategy; they do not mention the motion trajectory planning and obstacle avoidance ability of the robotic arm and cannot dynamically adjust the grasping force according to the force feedback data during the grasping process. Summary of the Invention

[0005] In view of the deficiencies of the prior art, the present invention proposes a vehicle perception and robotic arm unloading system. By means of multi-modal perception devices, it can collect vehicle attitude information and the stacking state information of urea bagged goods in real time, and generate a three-dimensional attitude model of the vehicle and a distribution map of the goods. According to the vehicle attitude model and the goods distribution map, it calculates the starting position and angle for the robotic arm to grasp the urea bags. Combining the kinematic constraints of the robotic arm and the environmental obstacle information, it plans a collision-free motion trajectory, and controls the robotic arm to perform the grasping operation through a force feedback control algorithm to complete the unloading action. It monitors the vehicle attitude changes and the goods state information in real time. If any deviation is found, it recalculates the grasping position and angle and updates the robotic arm motion trajectory to form a closed-loop control, improving the automation level and stability of urea loading and unloading.

[0006] To achieve the above object, the present invention provides the following technical solutions: A vehicle perception and robotic arm unloading system, comprising: a multi-modal perception module, a grasping position and angle calculation module, a robotic arm motion planning and control module, and a real-time monitoring module; the multi-modal perception module includes a data acquisition unit and a data fusion unit; The data acquisition unit collects vehicle attitude information and the stacking state information of urea bagged goods in real time, and transmits them to the data fusion unit to generate a three-dimensional attitude model of the vehicle and a distribution map of the goods; According to the three-dimensional attitude model of the vehicle and the distribution map of the goods, the grasping position and angle calculation module calculates the starting position and the grasping angle for the robotic arm to grasp the urea bagged goods; the starting position is selected according to the stacking stability score; According to the starting position and the grasping angle, the robotic arm motion planning and control module plans a collision-free motion trajectory of the robotic arm, combines a force feedback control algorithm and a preset unloading target position, and controls the robotic arm to complete the unloading action; The real-time monitoring module monitors the vehicle attitude changes and the goods state information in real time, and updates the grasping position, the grasping angle and the motion trajectory of the robotic arm.

[0007] Specifically, the specific steps for generating the three-dimensional attitude model of the vehicle and the distribution map of the goods include: A1: Use an inertial measurement unit to collect vehicle attitude information, use a lidar to collect three-dimensional point cloud data of urea bagged goods, and use a vision camera to collect image data of urea bagged goods; the vehicle attitude information includes pitch angle, roll angle, and yaw angle; A2: Use a scale-invariant feature transform matching algorithm to associate the three-dimensional point cloud data and the image data of urea bagged goods to obtain the goods position information. Combining the vehicle attitude information, use a coordinate transformation formula to unify the vehicle attitude information and the goods position information into the same coordinate system to obtain pose fusion data.

[0008] Specifically, the specific steps of generating the three-dimensional attitude model of the vehicle and the cargo distribution map further include: A3: Construct a three-dimensional attitude model of the vehicle based on the pose fusion data, and generate a cargo distribution map; the pixel coordinates in the cargo distribution map represent the cargo positions; A4: Based on the point cloud coordinates in the three-dimensional point cloud data Calculate the centroid of each urea bag , and use the centroid of each urea bag as a new mass point, and calculate the centroid position of all urea bags as a whole according to the new mass points , where n represents the number of points in the three-dimensional point cloud data, m represents the number of urea bags, i ≤ n, k ≤ m.

[0009] Specifically, the specific steps of generating the three-dimensional attitude model of the vehicle and the cargo distribution map further include: A5: For each point in the three-dimensional point cloud data, perform a least-squares fitting plane on the coordinates of the neighborhood points; the normal vector of the plane is the surface normal vector of the point ; A6: Obtain the centroid height, stacking layers, and bottom support area of the urea bag, and by calculating the ratio of the bottom support area to the stacking layers and then multiplying by the reciprocal of the centroid height, obtain the stacking stability score of the k th urea bag .

[0010] Specifically, the specific steps of A5 include: A5.1: For each target point P in the three-dimensional point cloud data, use radius-based neighborhood search to determine its neighborhood points; A5.2: Set the neighborhood point set of the target point P as , where , N represents the number of neighborhood points; A5.3: According to the centroid calculation process of each urea bag in A4, obtain the centroid A of the neighborhood point set, and construct a covariance matrix Cov based on the centroid A, and the coordinates of the centroid A are ; A5.4: Perform singular value decomposition on the covariance matrix Cov to obtain three eigenvalues and the corresponding eigenvectors; A5.5: The eigenvector corresponding to the smallest eigenvalue is the normal vector of the local plane where the target point P is located; the normal vector of the local plane is the surface normal vector of the target point .

[0011] Specifically, the specific steps of calculating the starting position and grasping angle of the robotic arm to grasp the urea bag for loading the cargo include: B1: Obtain the three-dimensional pose model of the vehicle and the cargo distribution map, where the center of gravity position, surface normal vector, and stacking stability score of the urea bags are marked on the cargo distribution map; B2: Traverse the stacking stability scores of all urea bags and select as the grasping target, and use the center of gravity position of the grasping target as the grasping point position ; B3: Calculate the grasping angle of the robotic arm according to the surface normal vector of the urea bag , and combine it with the kinematic model of the robotic arm to calculate the angles of each joint of the robotic arm , where represents the initial direction vector of the end effector of the robotic arm, represents the environmental constraint matrix, which refers to the obstacle information in the working space of the robotic arm, represents the kinematic constraint matrix of the robotic arm, including joint angle limits, workspace range, and joint torque limits, represents the force feedback vector received by the end effector of the robotic arm during the grasping process, represents the kinematic model.

[0012] Specifically, based on the starting position and the grasping angle, the robotic arm motion planning and control module plans a collision-free motion trajectory for the robotic arm, and combines a force feedback control algorithm and a preset unloading target position to control the robotic arm to complete the unloading operation, including: C1: Obtain the starting position and the grasping angle , and at the same time obtain the kinematic constraint matrix and the environmental constraint matrix ; The starting position is the starting position of the end effector of the robotic arm when starting to execute the grasping task ; C2: Combine the starting position , the grasping angle , and , and use a path planning algorithm to generate a collision-free motion trajectory from the starting position to the grasping point position ; C3: During the grasping process, use a force sensor to collect the force feedback vector in real time, and dynamically adjust the grasping force according to the force feedback data; C4: Control the robotic arm to perform the grasping operation according to the planned collision-free motion trajectory, and move the grasped urea bag to the preset unloading target position.

[0013] Specifically, the specific steps of C2 include: C2.1: Obtain the starting position , grasping angle , grasping point position , kinematic constraint matrix of the robotic arm and environmental constraint matrix ; C2.2: Use the path planning algorithm to generate a collision-free motion trajectory from to ; C2.3: Use the cubic spline interpolation method to smooth the collision-free motion trajectory.

[0014] Specifically, the specific steps of C2 further include: C2.4: Combine the kinematic constraints of the robotic arm and environmental constraints to judge the collision-free motion trajectory after smoothing, where and respectively represent the minimum and maximum values of the angles of each joint, represents the joint angle, and respectively represent the minimum and maximum positions of the end effector of the robotic arm in the workspace, represents the actual torque of the l th joint, represents the maximum allowable torque of the l th joint; If the kinematic constraints and environmental constraints of the robotic arm are not satisfied, return to C2.2 for re-planning; If the kinematic constraints and environmental constraints of the robotic arm are satisfied, output the final collision-free motion trajectory.

[0015] Specifically, the process of dynamically adjusting the grasping force in C3 is as follows: Based on the difference between the desired grasping force and the actual grasping force to obtain the first component, multiply the first component by the grasping ratio coefficient to obtain the second component; Multiply the historical force feedback vector and the force feedback vector received by the end effector of the robotic arm during the grasping process, and combine the weight matrix to obtain the third component; Sum the second component and the third component to obtain the grasping force adjustment amount .

[0016] Specifically, the real-time monitoring module monitors the vehicle attitude change and the cargo status information in real time, and updates the grasping position, grasping angle and the motion trajectory of the robotic arm, including: D1: Use a multi-modal sensing device to collect vehicle attitude changes and cargo status information in real time and perform filtering processing; D2: Compare the filtered real-time vehicle attitude changes and cargo status information with the vehicle three-dimensional attitude model and cargo distribution map in A3 to calculate the deviation value; If the deviation value is greater than the preset deviation threshold, recalculate the grasping point position and grasping angle according to the latest vehicle attitude and cargo status information; D3: Combine the recalculated grasping point position and grasping angle, use a path planning algorithm to generate a new collision-free motion trajectory, and control the robotic arm to perform the grasping operation according to the new motion trajectory.

[0017] Compared with the prior art, the beneficial effects of the present invention are: 1. The present invention proposes a vehicle perception and robotic arm unloading system, and has optimized improvements in the architecture, operation steps and processes. The system has the advantages of simple process, low investment and operation costs, and low production work costs.

[0018] 2. The present invention proposes a vehicle perception and robotic arm unloading system. By using a multi-modal sensing device to collect vehicle attitude and urea bag stacking status information in real time, generating a vehicle three-dimensional attitude model and a cargo distribution map, and dynamically calculating the grasping position and angle of the robotic arm in combination with the stacking stability score, the grasping stability and efficiency are improved; through path planning, force feedback control and closed-loop feedback mechanism, the system can adapt to vehicle attitude changes and cargo status changes, avoid collisions and optimize the unloading trajectory, realizing efficient, stable and automated urea loading and unloading, reducing the need for manual intervention and operation risks. BRIEF DESCRIPTION OF THE DRAWINGS

[0019] Figure 1 It is a schematic diagram of the vehicle perception and robotic arm unloading system of the present invention; Figure 2 It is a principle flow chart of the vehicle perception and robotic arm unloading system of the present invention; Figure 3 It is a flow chart for generating the three-dimensional attitude model of the vehicle and the cargo distribution map in the vehicle perception and robotic arm unloading system of the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0020] Embodiment 1

[0021] Please refer to Figure 1 and Figure 2 , an embodiment provided by the present invention: a vehicle perception and robotic arm unloading system, including the following steps: A multi-modal sensing module, a grasping position and angle calculation module, a robotic arm motion planning and control module, a real-time monitoring module; A multi-modal perception module, which is used to collect vehicle attitude information and the stacking state information of urea bagged goods in real time, and generate a three-dimensional attitude model of the vehicle and a cargo distribution map through data fusion; A grasping position and angle calculation module, which calculates the starting position and grasping angle of the robotic arm to grasp urea bagged goods according to the three-dimensional attitude model of the vehicle and the cargo distribution map; A robotic arm motion planning and control module, which plans a collision-free motion trajectory of the robotic arm according to the grasping starting position and grasping angle, and controls the robotic arm to perform grasping and unloading operations; A real-time monitoring module, which is used to monitor vehicle attitude changes and cargo state information in real time, compare with the initial model, recalculate the grasping position and angle when a deviation is found, and update the motion trajectory of the robotic arm.

[0022] The multi-modal perception module includes: a data acquisition unit, a data preprocessing unit, a data fusion unit, and a cargo distribution map annotation unit; The data acquisition unit uses multi-modal perception devices to collect vehicle attitude and cargo stacking state data in real time; The data preprocessing unit is used to filter, denoise, and register the collected raw data to improve data quality; The data fusion unit is used to fuse the data collected by multi-modal perception devices to generate a three-dimensional attitude model of the vehicle and a cargo distribution map; The cargo distribution map annotation unit is used to annotate the centroid position, surface normal vector, and stacking stability score of urea bags in the cargo distribution map.

[0023] The grasping position and angle calculation module includes: a grasping point selection unit, a grasping angle calculation unit, and a dynamic adjustment unit; The grasping point selection unit selects the optimal grasping point according to the stacking stability score, and preferentially selects urea bags with higher stability; The grasping angle calculation unit calculates the grasping angle by combining the surface normal vector of the urea bag and the kinematic model of the robotic arm; The dynamic adjustment unit dynamically adjusts the grasping position and angle according to vehicle attitude changes.

[0024] The robotic arm motion planning and control module includes: a path planning unit, a force feedback control unit, and an unloading execution unit; The path planning unit is used to combine the kinematic constraints of the robotic arm and environmental obstacle information, and use path planning algorithms to generate a collision-free motion trajectory; The force feedback control unit is used to monitor the grasping force feedback in real time during the grasping process, adjust the grasping force, and ensure grasping stability; The unloading execution unit is used to control the robotic arm to move the urea bag from the grasping point to a preset unloading target position, such as a warehouse shelf or a conveyor belt.

[0025] The real-time monitoring module includes: a real-time monitoring unit, a deviation detection unit, and a closed-loop control unit; The real-time monitoring unit continuously collects vehicle attitude and cargo status information through multi-modal perception devices; The deviation detection unit is used to compare real-time data with the vehicle's three-dimensional attitude model and the cargo distribution map to detect deviations, such as vehicle attitude changes or cargo displacements; The closed-loop control unit is used to recalculate the grasping position and angle and update the movement trajectory of the robotic arm when a deviation is found.

[0026] Specifically, the specific process implemented by the vehicle perception and robotic arm unloading system includes: S1: Use multi-modal perception devices to collect vehicle attitude information and the stacking status information of urea bagged cargo in real time, and perform fusion processing on the vehicle attitude and stacking status information to generate a three-dimensional attitude model of the vehicle and a cargo distribution map, and mark the center of gravity position, surface normal vector, and stacking stability score of the urea bags in the cargo distribution map; the multi-modal perception devices include an inertial measurement unit, a lidar, and a vision camera; S2: Calculate the starting position and angle for the robotic arm to grasp the urea bagged cargo according to the vehicle's three-dimensional attitude model and the cargo distribution map; the starting position is selected according to the stacking stability score; S3: According to the starting position and angle, combine the kinematic constraints of the robotic arm and the environmental obstacle information to plan a collision-free movement trajectory of the robotic arm, and use a force feedback control algorithm to control the robotic arm to perform a grasping operation according to the collision-free movement trajectory, and complete the unloading action according to the preset unloading target position; S4: Real-time monitor the vehicle attitude changes and cargo status information, and compare them with the vehicle's three-dimensional attitude model and the cargo distribution map in S1. If a deviation occurs, recalculate the grasping position and angle, and update the movement trajectory of the robotic arm.

[0027] Embodiment 2 Please refer to Figure 3 , in this embodiment, the specific steps for generating the vehicle's three-dimensional attitude model and the cargo distribution map include: A1: Use an inertial measurement unit to collect vehicle attitude information, use a lidar to collect three-dimensional point cloud data of urea bagged cargo, and use a vision camera to collect image data of urea bagged cargo; the vehicle attitude information includes pitch angle, roll angle, and yaw angle; A2: Use the scale-invariant feature transform matching algorithm to associate the three-dimensional point cloud data and the image data of the urea bagged cargo to obtain cargo position information. Combine the vehicle attitude information and use the coordinate transformation formula to unify the vehicle attitude information and the cargo position information into the same coordinate system to obtain pose fusion data; A3: Construct a three-dimensional pose model of the vehicle based on the pose fusion data and generate a cargo distribution map; the pixel coordinates in the cargo distribution map represent the cargo positions; A4: Based on the point cloud coordinates in the three-dimensional point cloud data Calculate the centroid of each urea bag , and use the centroid of each urea bag As a new particle, calculate the centroid position of all urea bags as a whole according to the new particles , where n represents the number of points in the three-dimensional point cloud data, m represents the number of urea bags, i ≤ n, k ≤ m; A5: For each point in the three-dimensional point cloud data, perform a least squares fitting of the neighborhood point coordinates to a plane; the normal vector of the plane is the surface normal vector of the point ; A6: Obtain the centroid height, stacking layer number, and bottom support area of the urea bag. By calculating the ratio of the bottom support area to the stacking layer number and then multiplying by the reciprocal of the centroid height, obtain the k stacking stability score of the urea bag .

[0028] The specific steps of A5 include: A5.1: For each target point P in the three-dimensional point cloud data, use radius-based neighborhood search to determine its neighborhood points; A5.2: Set the neighborhood point set of the target point P as , where , N represents the number of neighborhood points; A5.3: According to the centroid calculation process of each urea bag in A4, obtain the centroid A of the neighborhood point set and construct a covariance matrix based on the centroid A, where T represents the transpose, and the coordinates of the centroid A are ; A5.4: Perform a singular value decomposition on the covariance matrix Cov to obtain three eigenvalues and the corresponding eigenvectors. Among them, the singular value decomposition is the prior art content in the field and is not the creative solution of this application, so it will not be elaborated here; A5.5: The eigenvector corresponding to the minimum eigenvalue is the normal vector of the local plane where the target point P is located, that is, the surface normal vector of the target point.

[0029] The specific steps of calculating the starting position and grasping angle of the robotic arm to grasp the urea bag and load the cargo include: B1: Obtain the three-dimensional pose model of the vehicle and the cargo distribution map, where the centroid position, surface normal vector, and stacking stability score of the urea bag are marked on the cargo distribution map; B2: Traverse the stacking stability scores of all urea bags and select as the grasping target, and use the center of gravity position of the grasping target as the grasping point position ; It should be noted that the grasping target is the specific urea bag that the robotic arm needs to grasp, which is selected through the stacking stability score, and the position of the grasping point is the specific coordinate that the end effector of the robotic arm needs to reach, usually the center of gravity position of the grasping target. Therefore, the grasping target determines the position of the grasping point, and the position of the grasping point is the specific implementation of the grasping target in space. Both of them jointly provide a clear task object and execution parameters for the robotic arm.

[0030] B3: Calculate the grasping angle of the robotic arm according to the surface normal vector of the urea bag , and combine with the kinematic model of the robotic arm to calculate the angles of each joint of the robotic arm , where, represents the initial direction vector of the end effector of the robotic arm, represents the environmental constraint matrix, which refers to the obstacle information in the working space of the robotic arm, represents the kinematic constraint matrix of the robotic arm, including joint angle limits, workspace range, and joint torque limits, represents the force feedback vector received by the end effector of the robotic arm during the grasping process, represents the kinematic model. represents the kinematic model.

[0031] Furthermore, the environmental constraint matrix describes the obstacle information in the working space of the robotic arm to ensure that the motion trajectory of the robotic arm is collision-free. The solution steps include: (1) Obtain three-dimensional point cloud data, segment and cluster the point cloud data, and identify obstacles, such as the vehicle structure, the positions and shapes of other urea bags; (2) Represent the obstacles as cubes and use an empty matrix to store the geometric information of the obstacles, such as positions and dimensions, to obtain the environmental constraint matrix , where it should be noted that in the motion planning of the robotic arm, a collision detection algorithm is used to check whether the robotic arm collides with the obstacles. If a collision is detected, the motion trajectory of the robotic arm is adjusted. In the present invention, the GJK algorithm is adopted for the collision detection algorithm, and the GJK algorithm is the prior art content in this field and is not the creative solution of this application, so it will not be elaborated here.

[0032] The kinematic constraint matrix of the robotic arm describes the physical limitations of the robotic arm, including joint angle limits, workspace range, and joint torque limits. The solution steps include: ([[]] a1) Obtain the angle ranges of each joint from the technical manual of the robotic arm , and use an empty matrix to store the angle limits of each joint; ( a 2) Use the Monte Carlo sampling method to calculate the workspace range of the robotic arm, that is, the area that the end effector can reach. Among them, the Monte Carlo sampling method is the prior art content in this field and is not the creative solution of this application, so it will not be elaborated here; ( a 3) Obtain the maximum torque of each joint from the technical manual of the robotic arm, and ensure that the torque of each joint does not exceed the maximum torque during the inverse kinematics calculation; ( a 4) Integrate the joint angle limits, workspace range, and joint torque limits into the empty matrix in (a1) to obtain the kinematic constraint matrix of the robotic arm .

[0033] Force feedback vector describes the force feedback information received by the end effector of the robotic arm during the grasping process and is used to dynamically adjust the grasping force. The solution steps include: (1) Install a force sensor on the end effector of the robotic arm to collect the force feedback data during the grasping process in real time , such as grasping force, torque; (2) Perform low-pass filtering on the force feedback data to remove noise and obtain the low-pass filtered force feedback data , where represents the filtering coefficient, represents the data after the previous filtering; (3) Convert the low-pass filtered force feedback data into a vector , where , , represent the three components of the grasping force, represents the three components of the torque.

[0034] Among them, according to the force feedback vector dynamically adjust the grasping force of the robotic arm to ensure grasping stability. If the grasping force is too large, reduce the force to avoid damage to the goods; if the grasping force is too small, increase the force to avoid the goods from slipping. It should be noted that during the inverse kinematics calculation, it is necessary to combine the force feedback vector to optimize the grasping angle and force of the robotic arm.

[0035] It should be understood that by introducing environmental constraints, manipulator kinematic constraints, and force feedback, the motion control problem of the manipulator can be more comprehensively described. Combining force feedback can dynamically adjust the motion trajectory of the manipulator to adapt to complex environments and task requirements. Through environmental constraints and manipulator kinematic constraints, it is ensured that the manipulator does not collide and does not exceed physical limits during the motion process, enabling the manipulator to plan a collision-free motion trajectory in a complex environment, avoiding collisions with vehicle structures or other goods, and improving the safety and reliability of the system.

[0036] According to the starting position and the grasping angle, the manipulator motion planning and control module plans a collision-free motion trajectory of the manipulator, and combines a force feedback control algorithm and a preset unloading target position to control the manipulator to complete the unloading action, including: C1: Obtain the starting position and the grasping angle , and at the same time obtain the kinematic constraint matrix of the manipulator and the environmental constraint matrix ; the starting position is the starting position of the end effector of the manipulator when starting to execute the grasping task ; C2: Combine the starting position , the grasping angle , and , and use a path planning algorithm to generate a collision-free motion trajectory from the starting position to the grasping point position ; C3: During the grasping process, use a force sensor to collect the force feedback vector in real time, and dynamically adjust the grasping force according to the force feedback data; The process of dynamically adjusting the grasping force in C3 is as follows: Based on the difference between the desired grasping force and the actual grasping force , obtain a first component, multiply the first component by the grasping proportionality coefficient , and obtain a second component; Multiply the historical force feedback vector and the force feedback vector received by the end effector of the manipulator during the grasping process, and combine the weight matrix , and obtain a third component; Sum the second component and the third component to obtain the grasping force adjustment amount .

[0037] The specific formula is: , where the historical force feedback vector refers to the grasping force feedback information of the manipulator in previous tasks.

[0038] C4: Control the robotic arm to perform the grasping operation according to the planned collision-free motion trajectory, and move the grasped urea bag to the preset unloading target position.

[0039] Further, the specific steps of C4 include: (1) Obtain the final collision-free motion trajectory and the preset unloading target position; (2) Control the robotic arm to perform the grasping operation according to the final collision-free motion trajectory; (3) During the grasping process, monitor the motion state of the robotic arm and the grasping force feedback in real time to ensure grasping stability. At the same time, during the movement process, adjust the motion trajectory of the robotic arm in real time to avoid collisions and environmental interference.

[0040] The specific steps of C2 include: C2.1: Obtain the starting position , the grasping angle , the grasping point position , the kinematic constraint matrix of the robotic arm and the environmental constraint matrix ; C2.2: Use the path planning algorithm to generate a collision-free motion trajectory from to , where the path planning algorithm uses the A* algorithm, and the A* algorithm is the prior art content in this field and is not the creative solution of this application, so it will not be elaborated here; C2.3: Smooth the collision-free motion trajectory using the cubic spline interpolation method; C2.4: Combine the kinematic constraints and environmental constraints of the robotic arm to judge the smoothed collision-free motion trajectory, where and respectively represent the minimum and maximum values of the angles of each joint, represents the joint angle, and respectively represent the minimum and maximum positions of the end effector of the robotic arm in the workspace, represents the actual torque of the l th joint, represents the maximum allowable torque of the l th joint; If the kinematic constraints and environmental constraints of the robotic arm are not satisfied, return to C2.2 for re-planning; If the kinematic constraints and environmental constraints of the robotic arm are satisfied, output the final collision-free motion trajectory.

[0041] The real-time monitoring module monitors the vehicle attitude changes and cargo status information in real time, and updates the grasping position, grasping angle, and the movement trajectory of the robotic arm, including: D1: Use multi-modal perception devices to collect vehicle attitude changes and cargo status information in real time and perform filtering processing; D2: Compare the filtered real-time vehicle attitude changes and cargo status information with the vehicle three-dimensional attitude model and the cargo distribution map in A3, and calculate the deviation value; If the deviation value is greater than the preset deviation threshold, recalculate the grasping point position and grasping angle according to the latest vehicle attitude and cargo status information; D3: Combine the recalculated grasping point position and grasping angle, use the path planning algorithm to generate a new collision-free movement trajectory, and control the robotic arm to perform the grasping operation according to the new movement trajectory.

[0042] The embodiments of the present invention have been described above in conjunction with the accompanying drawings. However, the present invention is not limited to the above specific embodiments. The above specific embodiments are merely illustrative and not restrictive. Under the inspiration of the present invention, those of ordinary skill in the art can also make changes, modifications, substitutions, and variations to the above embodiments without departing from the spirit and scope of the present invention. These all fall within the protection scope of the present invention.

Claims

1. Vehicle perception and robotic arm unloading system, characterized in that, Including: A multi-modal perception module, a grasping position and angle calculation module, a robotic arm motion planning and control module, and a real-time monitoring module; the multi-modal perception module includes a data acquisition unit and a data fusion unit; The vehicle attitude information and the stacking state information of urea bagged goods are collected in real time by the data acquisition unit and transmitted to the data fusion unit to generate a three-dimensional attitude model of the vehicle and a goods distribution map; According to the three-dimensional attitude model of the vehicle and the goods distribution map, the starting position and the grasping angle of the robotic arm to grasp the urea bagged goods are calculated by the grasping position and angle calculation module; the starting position is selected according to the stacking stability score; According to the starting position and the grasping angle, the robotic arm motion planning and control module plans a collision-free motion trajectory of the robotic arm, combines a force feedback control algorithm and a preset unloading target position, and controls the robotic arm to complete the unloading action; The vehicle attitude change and the goods state information are monitored in real time by the real-time monitoring module, and the grasping position, the grasping angle and the motion trajectory of the robotic arm are updated.

2. The vehicle perception and robotic arm unloading system according to claim 1, characterized in that, The specific steps for generating the three-dimensional attitude model of the vehicle and the goods distribution map include: A1: The vehicle attitude information is collected by using an inertial measurement unit, the three-dimensional point cloud data of the urea bagged goods is collected by using a lidar, and the image data of the urea bagged goods is collected by using a vision camera; the vehicle attitude information includes pitch angle, roll angle and yaw angle; A2: The scale-invariant feature transform matching algorithm is used to associate the three-dimensional point cloud data and the image data of the urea bagged goods to obtain the goods position information. Combining the vehicle attitude information, the vehicle attitude information and the goods position information are unified into the same coordinate system by using a coordinate transformation formula to obtain pose fusion data.

3. The vehicle perception and robotic arm unloading system according to claim 2, characterized in that, The specific steps for generating the three-dimensional attitude model of the vehicle and the goods distribution map further include: A3: According to the pose fusion data, a three-dimensional attitude model of the vehicle is constructed and a goods distribution map is generated; the pixel coordinates in the goods distribution map represent the goods position; A4: Based on the point cloud coordinates in the three-dimensional point cloud data Calculate the centroid of each urea bag , and use the centroid of each urea bag as a new particle, and calculate the centroid position of all urea bags as a whole according to the new particle , where n represents the number of points in the three-dimensional point cloud data m represents the number of urea bags, i ≤ n, k ≤ m 4. The vehicle perception and robotic arm unloading system according to claim 3, characterized in that, The specific steps for generating the three-dimensional attitude model of the vehicle and the goods distribution map further include: A5: For each point in the three-dimensional point cloud data, a plane is fitted by least squares to the coordinates of the neighboring points; the normal vector of the plane is the surface normal vector of the point ; A6: Obtain the center of gravity height, stacking layers, and bottom support area of the urea bag. By calculating the ratio of the bottom support area to the stacking layers and then multiplying it by the reciprocal of the center of gravity height, the stacking stability score of the k th urea bag is obtained. .

5. The vehicle perception and robotic arm unloading system according to claim 4, characterized in that, The specific steps of A5 include: A5.1: For each target point P in the three-dimensional point cloud data, its neighborhood points are determined by using radius-based neighborhood search; A5.2: Set the neighborhood point set of the target point P as , where , and N represents the number of neighborhood points; A5.3: According to the calculation process of the centroid of each urea bag in A4, the centroid A of the neighborhood point set is obtained, and the covariance matrix Cov is constructed based on the centroid A, and the coordinates of the centroid A are ; A5.4: The covariance matrix Cov is subjected to singular value decomposition to obtain three eigenvalues and the corresponding eigenvectors; A5.5: The eigenvector corresponding to the minimum eigenvalue is the normal vector of the local plane where the target point P is located; the normal vector of the local plane is the surface normal vector of the target point .

6. The vehicle perception and robotic arm unloading system according to claim 5, wherein, The specific steps for calculating the starting position and the grasping angle of the robotic arm to grasp the urea bagged goods include: B1: Obtain the three-dimensional attitude model of the vehicle and the goods distribution map, wherein the center of gravity position, the surface normal vector and the stacking stability score of the urea bag are marked on the goods distribution map; B2: Traverse the stacking stability scores of all urea bags and select as the grasping target, and use the center of gravity position of the grasping target as the grasping point position ; B3: Based on the surface normal vector of the urea bag Calculate the gripping angle of the robot arm , combined with the kinematic model of the robot arm, calculate the angles of each joint of the robot arm ,in, represents the initial direction vector of the end effector of the robot arm, Represents the environmental constraint matrix, which refers to the obstacle information in the robot workspace. Represents the kinematic constraint matrix of the robot arm, including joint angle limit, workspace range and joint torque limit, It represents the force feedback vector received by the end effector of the robot arm during the grasping process. Represents the kinematic model.

7. The vehicle perception and robotic arm unloading system according to claim 6, wherein, According to the starting position and the grasping angle, the robotic arm motion planning and control module plans a collision-free motion trajectory of the robotic arm, combines a force feedback control algorithm and a preset unloading target position, and controls the robotic arm to complete the unloading action, including: C1: Obtain the starting position and the grasping angle , and at the same time obtain the kinematic constraint matrix of the robotic arm and the environmental constraint matrix ; The starting position is the starting position of the end effector of the robotic arm when starting to execute the grasping task ; C2: Combining starting position , grasping angle , and , use the path planning algorithm to generate a collision-free motion trajectory from the starting position to the grasping point position ; C3: During the grasping process, a force sensor is used to collect the force feedback vector in real time , and the grasping force is dynamically adjusted according to the force feedback data; C4: Control the robotic arm to perform a grasping operation according to the planned collision-free motion trajectory and move the grasped urea bag to the preset unloading target position.

8. The vehicle perception and robotic arm unloading system according to claim 7, wherein The specific steps of C2 include: C2.1: Obtain the starting position , grasping angle , grasping point position , kinematic constraint matrix of the robotic arm and environmental constraint matrix ; C2.2: Generate a collision-free motion trajectory from to using a path planning algorithm; C2.3: Use the cubic spline interpolation method to smooth the collision-free motion trajectory.

9. The vehicle perception and robotic arm unloading system according to claim 8, wherein, The specific steps of C2 also include: C2.4: Incorporate the kinematic constraints of the robotic arm and environmental constraints to judge the collision-free motion trajectory after smoothing, where and respectively represent the minimum and maximum values of the angles of each joint, represents the joint angle, and respectively represent the minimum position and the maximum position of the end effector of the robotic arm in the workspace, represents the l th actual torque of the joint, represents the l th maximum allowable torque of the joint; If the kinematic constraints and environmental constraints of the robotic arm are not satisfied, return to C2.2 for re-planning; If the kinematic constraints and environmental constraints of the robotic arm are satisfied, output the final collision-free motion trajectory.

10. The vehicle perception and robotic arm unloading system according to claim 9, characterized in that, The process of dynamically adjusting the grasping force in C3 is: Based on the difference between the expected grasping force and the actual grasping force a first component is obtained, and the first component is multiplied by the grasping ratio coefficient to obtain a second component; The historical force feedback vector and the force feedback vector received by the end effector of the robotic arm during the grasping process are multiplied, combined with the weight matrix to obtain the third component; Sum the second component and the third component to obtain the grasping force adjustment amount .

11. The vehicle perception and robotic arm unloading system according to claim 10, characterized in that, The real-time monitoring module monitors the vehicle attitude changes and cargo status information in real time, and updates the grasping position, grasping angle, and the motion trajectory of the robotic arm, including: D1: Use multi-modal perception devices to collect the vehicle attitude changes and cargo status information in real time and perform filtering processing; D2: Compare the filtered real-time vehicle attitude changes and cargo status information with the vehicle three-dimensional attitude model and the cargo distribution map in A3 to calculate the deviation value; If the deviation value is greater than the preset deviation threshold, recalculate the grasping point position and grasping angle according to the latest vehicle attitude and cargo status information; D3: Combine the recalculated grasping point position and grasping angle, use the path planning algorithm to generate a new collision-free motion trajectory, and control the robotic arm to perform the grasping operation according to the new motion trajectory.

Images