A mobile manipulator anti-overturning motion planning method and system for uneven terrain

Abstract

A mobile manipulator anti-overturning motion planning method and system for uneven terrain, the planning method comprising the following steps: establishing a forward kinematics model of the mobile manipulator and constructing a configuration space; performing spatial sampling in the configuration space, generating candidate nodes based on adaptive obstacle repulsion and Gaussian heuristic strategy; checking and filtering the candidate nodes based on dexterity and overturning risk dual physical constraints, constructing an anti-overturning safety search tree, and extracting discrete path key nodes from the starting point to the target point; taking the discrete path key nodes as hard constraints, generating multi-order continuous smooth trajectories through time parameterization optimization, and issuing to the control system for execution. The present application solves the problems of singular jamming and vehicle overturning that easily occur when the mobile manipulator is working in uneven terrain with large load, and effectively solves the motor transient impact caused by the traditional path planning broken line trajectory.

Description

Technical Field

[0001] This invention relates to the field of robot control and motion planning technology, and in particular to a method and system for anti-tipping motion planning of a mobile robotic arm oriented towards non-flat terrain. Background Technology

[0002] With the continuous development of technologies in fields such as deep space exploration and special disaster relief, mobile robotic arms, consisting of a mobile chassis and a multi-degree-of-freedom manipulator, are being used more and more widely due to their combination of wide-range mobility and precision operation capabilities. In typical high-precision missions such as lunar station construction and star surface sampling, mobile robotic arms typically adopt a working mode where the chassis is parked and positioned, while the robotic arm performs large-range swinging operations.

[0003] However, when faced with a complex and uneven environment filled with craters, rocks and slopes, existing mobile robotic arm path planning and trajectory generation algorithms have revealed many technical defects in actual engineering deployments.

[0004] The primary problem is that, in real-world uneven terrain, mobile chassis typically exhibit large roll and pitch angles when parked. Especially in microgravity environments such as the lunar surface, when the robotic arm extends significantly in a specific direction carrying a large mass load, the vehicle's joint center of gravity is highly likely to deviate from the supporting polygon formed by the chassis wheels and the inclined plane, potentially leading to an irreversible rollover disaster. Existing purely geometric obstacle avoidance algorithms are simply unable to prevent such safety hazards from a static perspective.

[0005] Furthermore, traditional planning algorithms lack guidance during spatial search, resulting in enormous computational consumption. Due to the prevalent use of globally uniform random sampling strategies, a massive number of invalid nodes and redundant collision detections occur when facing narrow rocky passages or complex obstacle groups, leading to extremely slow convergence and failing to meet the needs of spaceborne or vehicle-mounted edge computing devices with severely limited computing power. Simultaneously, random sampling typically only verifies whether path nodes have geometric collisions with the environment, without deeply considering the configuration of the robotic arm itself. This results in planned obstacle avoidance paths often containing or closely approximating kinematic singularities on the robotic arm, forcing the actual motors to output infinite angular velocities at these locations to maintain motion, easily causing the robotic arm to jam, lose control, or suffer hardware damage.

[0006] In summary, there is an urgent need for an anti-tipping motion planning method that can integrate chassis tilt posture, take into account high computational efficiency, avoid kinematic singularities, and fundamentally eliminate inertial impact, so as to ensure the safety of mobile robotic arms operating under heavy loads in complex and uneven terrain. Summary of the Invention

[0007] This invention provides a method and system for anti-tipping motion planning of a mobile robotic arm for non-flat terrain, in order to solve the technical problems mentioned in the background art.

[0008] To achieve the above objectives, the technical solution of the present invention is implemented as follows: This invention provides a method for anti-tipping motion planning of a mobile robotic arm oriented towards non-flat terrain, comprising the following steps: S1. Establish the forward kinematic model of the mobile robotic arm and construct the configuration space; S2. Perform spatial sampling within the configuration space and generate candidate nodes based on an adaptive obstacle rejection and Gaussian heuristic strategy. S3. Based on the dual physical constraints of dexterity and overturning risk, the candidate nodes are verified and filtered to construct an anti-overturning safety search tree and extract the key nodes of the discrete path from the starting point to the target point. S4. Treat the key nodes of the discrete path as hard constraints, generate a multi-order continuous smooth trajectory through time parameterization optimization, and send it to the control system for execution.

[0009] Furthermore, step S1 specifically includes the following steps: S11. Obtain the attitude information of the mobile robotic arm chassis when it is parked through vehicle-mounted sensors, and construct a real world coordinate system and tilted support polygon; S12. Based on the given DH parameters, establish the forward kinematics model of the mobile robotic arm; then construct the tool coordinate system at the end of the mobile robotic arm. S13. Construct a vehicle static model that integrates the chassis tilt attitude, and then construct a global vehicle joint static center of mass model based on the vehicle static model. S14. Using the joint degrees of freedom of the mobile robotic arm as the search dimension, and combining the positive kinematics model with the global vehicle joint static centroid model, construct a configuration space for planning.

[0010] Furthermore, step S12 specifically includes the following steps: S121. Construct a forward kinematics model based on the given DH parameters, with the following expression: ; ; ; in, Indicates the first... The rotation angle of each joint; This indicates the number of times the mobile robotic arm reads data in real time via an onboard encoder. Configuration vectors of joints, where ; Indicates the first Each joint has a pre-calibrated and solidified initial mechanical assembly zero-point offset constant. In the standard DH parameter method, the first The axis of the first joint and the first The common normal distance of the link length between the axes of the joints; Indicates the first The axis of the first joint and the first The torsional angle of the link between the joint axes; Indicates the common normal of adjacent links in the th case. Offset axial distance on the axis of each joint; Indicates the first The joint coordinate system is connected to the first... Homogeneous transformation matrix of the joint coordinate system; This indicates the total number of joints in the mobile robotic arm, which corresponds to the joint degrees of freedom of the robotic arm. This represents the pose matrix of the flange coordinate system relative to the base coordinate system Base; S122. Then, for the mobile robotic arm, extend the tool coordinate system based on the flange coordinate system and construct a compensation matrix for the tool coordinate system relative to the flange coordinate system. The expression is as follows: ; in, This represents the spatial translation transformation operator matrix, used to calculate and generate standard translation transformations based on three independent input translation parameters. Homogeneous translation matrix of order 1; This indicates the translation parameter of the tool coordinate system relative to the flange coordinate system; This represents the spatial rotation transformation operator matrix, used to generate a standard rotation transformation based on three independent input rotation angle parameters. Homogeneous rotation matrix of order 1; This indicates the rotation parameters of the tool coordinate system relative to the flange coordinate system; S123, Constructing the tool coordinate system in the world coordinate system pose matrix below The expression is as follows: ; in, The base coordinate system (Base) of the mobile robotic arm is represented relative to the world coordinate system. The rotation matrix.

[0011] Furthermore, step S13 specifically includes the following steps: S131. Introduce a vehicle mass topology distribution structure to construct a vehicle static model that incorporates chassis tilt attitude; S132. Based on the vehicle's mass topology and combining the forward kinematics model and the vehicle static model, a global vehicle joint static centroid model is constructed, including the vehicle chassis and the dynamic robotic arm. The expression for the global vehicle joint static centroid model is as follows: ; in, In the configuration vector Below, the combined center of mass of the entire vehicle is located in the world coordinate system. The three-dimensional spatial coordinate vector; This indicates the total mass of the chassis of the mobile composite robot; This indicates that the chassis's independent center of mass is in the world coordinate system. The absolute position vector in three-dimensional space; Indicates the first robotic arm The total independent mass of each moving link in the system is a known physical constant in advance, where... ; Indicates the first The absolute center of mass of each link is in the world coordinate system. The three-dimensional physical space coordinate vector in the text.

[0012] Furthermore, step S2 specifically includes the following steps: S21. Construct a Gaussian probability distribution model with the line connecting the current end of the mobile robotic arm and the target point as the main axis; dynamically adjust the covariance matrix of the Gaussian probability distribution model according to environmental perception information to achieve adaptive scaling of the barrier-free area and the obstacle area. S22. Set the mixed sampling probability threshold, and generate preliminary sampling points based on the Gaussian probability distribution model and covariance matrix, combined with global uniform random sampling and Gaussian heuristic sampling. S23. Combining the forward kinematics model, a dynamic repulsive force field based on the obstacle bounding box is introduced into the generated preliminary sampling points, and the acceptance probability of the current preliminary sampling points is calculated. S24. Based on the Monte Carlo rejection sampling mechanism, the generated random number is compared with the acceptance probability. If it fails, return to S22 for resampling. If it succeeds, output the candidate node.

[0013] Furthermore, the expression for the Gaussian probability distribution model in S21 is as follows: ; in, Represents the probability density function; n This represents the dimension of the joint configuration space of the mobile robotic arm; Indicates the sampling point; This represents the mean vector of a Gaussian distribution. T Indicates transpose; Let be the determinant of the covariance matrix; Denotes the covariance matrix. The calculation formula is as follows: ; in, The preset gain coefficient; The Euclidean distance is the node closest to the target in the current search tree or the distance from the current end to the nearest obstacle surface. Represents the identity matrix.

[0014] Furthermore, step S3 specifically includes the following steps: S31. Traverse the current anti-overturning safety search tree and find the distance to the candidate node. The nearest tree node is used to extend towards the candidate node with a preset maximum growth step size, generating a new node to be verified. ; S32, For the new node Perform basic bounding box interference detection and dexterity verification. If both pass, proceed to S33; otherwise, return to S2. S33. Calculate the total centroid of the current configuration using the global vehicle joint static centroid model, and project the total centroid onto the chassis parking ramp. If the projection point exceeds the inclined support polygon, it is determined that there is a risk of overturning, and the current candidate node is removed, and the process returns to S2; otherwise, a new node that passes the dual physical constraint verification is obtained. Then it enters S34; S34. For new nodes that pass the dual physical constraint verification Perform local tree topology optimization to minimize path accumulation cost and validate new nodes through multiple physical constraints. Formally incorporated into the anti-overturning safety search tree; S35. Determine whether the target point has been reached. If so, backtrack to extract the key nodes of the discrete path from the starting point to the target point. Otherwise, return to S2.

[0015] Furthermore, step S31 specifically includes the following steps: S311, For candidate nodes Calculate candidate nodes The set of nodes in the anti-overturning safety search tree The Euclidean distance; S312. Find candidate nodes based on Euclidean distance. nearest node ; S313, along the nearest node Pointing to candidate nodes Direction, with a preset maximum growth step size Extend the process to obtain a new node to be verified. The expression is as follows: ; in, This indicates Euclidean distance.

[0016] Furthermore, step S4 specifically includes the following steps: S41. Based on the Euclidean distance between adjacent key nodes in the discrete path, and according to the set average execution speed, allocate a time period for each trajectory segment. S42. In order to ensure that the trajectory is absolutely continuous in the four physical levels of position, velocity, acceleration and jerk, construct a seventh-order time polynomial model for each discrete path segment; solve for the fourth derivative of the seventh-order time polynomial model of each discrete path segment with respect to time. S43. Using the sum of the square integrals of the fourth derivatives of each discrete path with respect to time as the objective function, derive and construct the quadratic objective cost matrix. S44. Take the key points of the path as hard constraints for position, and extract the velocity, acceleration and jerk continuity conditions at the intersection of each discrete path segment to construct a global linear equality constraint matrix. S45. Input the quadratic objective cost matrix and the global linear equality constraint matrix into the quadratic programming solver to calculate the polynomial coefficients that minimize the objective function. Substitute the polynomial coefficients into the seventh-order time polynomial model to generate a multi-order continuous smooth trajectory. Generate control commands based on the multi-order continuous smooth trajectory and discretize them according to the control cycle to send them to the control system for execution.

[0017] Furthermore, the expression for the seventh-order time polynomial model in S42 is as follows: ; in, Indicates the first discrete path trajectory in The joint position command value at any given time; The coefficients of the polynomial to be solved; This represents the relative running time within the current trajectory segment, satisfying... ,in For the first The time period for which the segment trajectory is assigned; The expression for the fourth derivative of the seventh-order time polynomial model in S42 with respect to time is as follows: ; in, This represents the fourth derivative of the seventh-order time polynomial model with respect to time.j This indicates the degree of a polynomial monomial. Represents the relative time of the independent variable t of j -4th power; The expression for the objective function in S43 is as follows: ; in, The global total objective cost is represented by the sum of the square integrals of the fourth derivatives of the positions with respect to time for all trajectory segments. This represents the index of the currently calculated discrete path segment, satisfying... ; This represents the total number of segments into which the entire planned path is divided; Indicates the first The length of the time period allocated to a segment of discrete path trajectory; Indicates the first The fourth derivative function of the discrete path trajectory with respect to time; Let represent the polynomial coefficient vector. The expression for the polynomial coefficient vector is: .

[0018] In another aspect, the present invention provides a mobile robotic arm anti-tipping motion planning system for non-flat terrain, configured to execute a mobile robotic arm anti-tipping motion planning method for non-flat terrain, comprising: The modeling and space construction module is used to acquire the environmental and attitude information of the mobile robotic arm under the chassis parking state, and to construct a vehicle static model and configuration space that integrates the chassis tilt attitude. A heuristic sampling module is used to perform spatial sampling within the configuration space and generate candidate nodes based on an adaptive barrier rejection and Gaussian heuristic strategy. The constraint filtering and planning module is used to verify and filter the candidate nodes based on dual physical constraints of dexterity and overturning risk, construct an anti-overturning safety search tree, and extract key points of discrete paths. The trajectory optimization module is used to take the key points of the discrete path as hard constraints, generate a multi-order continuous smooth trajectory through time parameterization optimization, generate control commands based on the multi-order continuous smooth trajectory, and send them to the control system for execution.

[0019] The beneficial effects of this invention are: 1. Improved the system's resistance to overturning in uneven terrain; This invention incorporates the tilt angle of non-flat parking terrain into the vehicle's static model and uses the relationship between the gravity projection of the joint center of mass and the tilted support polygon as a constraint condition for node growth. This method can effectively avoid the risk of vehicle instability caused by the large-scale swinging of the robotic arm in rugged or microgravity environments, improving the system's safety when operating in complex terrain.

[0020] 2. Improved path search efficiency in complex environments; This invention employs an adaptive obstacle rejection and Gaussian heuristic strategy, which can dynamically adjust the sampling range based on the obstacle distribution characteristics of the environment. While ensuring algorithm completeness, it effectively reduces invalid sampling nodes and redundant collision detection times in complex, unstructured environments, lowering the computational overhead and making it more suitable for mobile computing platforms with limited computing resources.

[0021] 3. Enhanced the dexterity and reliability of the mobile robotic arm's movements; This invention introduces dual physical constraints of dexterity and overturning risk during the node expansion phase, ensuring that the planned discrete paths not only meet the geometric requirements of collision-free operation but also maintain a favorable kinematic state. This mechanism effectively avoids the risk of the robotic arm falling into kinematically singular postures during execution, helping to reduce abnormal loads and wear on the chassis servo motors of the mobile robotic arm.

[0022] 4. The trajectory smoothness was optimized and the system dynamic disturbances were reduced; This invention employs a seventh-order time polynomial model to optimize the trajectory instead of the traditional discrete piecewise linear path, ensuring the continuity of the planned trajectory in terms of position, velocity, acceleration, and jerk. This method significantly reduces the transient inertial impact during the movement of the mobile robotic arm, alleviates the swaying caused by the reaction force of the mobile robotic arm's movements on the parking chassis, and provides a more stable trajectory support for the high-precision operation of the mobile robotic arm. Attached Figure Description

[0023] Figure 1 This is a flowchart of the anti-tipping motion planning method for the mobile robotic arm in this invention. Detailed Implementation

[0024] To facilitate understanding of the present invention, a more complete description will be given below with reference to the accompanying drawings. Preferred embodiments of the invention are shown in the drawings. However, the invention can be implemented in many other different forms and is not limited to the embodiments described herein. Rather, these embodiments are provided to provide a thorough and complete understanding of the disclosure of the invention.

[0025] Reference Figure 1This application provides a method for anti-tipping motion planning of a mobile robotic arm for non-flat terrain, enabling safe, efficient, and extremely smooth collaborative operation and motion planning of the mobile robotic arm in complex unstructured environments with microgravity, rugged terrain, and limited computing power. The method for anti-tipping motion planning of the mobile robotic arm includes the following steps: S1. Establish the forward kinematic model of the mobile robotic arm and construct the configuration space; S2. Spatial sampling is performed in the configuration space, and candidate nodes are generated based on the adaptive obstacle rejection and Gaussian heuristic strategy. The generation of candidate nodes by the adaptive obstacle rejection and Gaussian heuristic strategy helps to reduce the blindness of the search in high-dimensional space and reduce invalid collision detection in unstructured environments, so that the algorithm can focus on fast convergence in the target direction, thereby greatly releasing airborne computing resources. S3. Based on the dual physical constraints of dexterity and overturning risk, the candidate nodes are verified and filtered to construct an anti-overturning safety search tree and extract the key nodes of the discrete path from the starting point to the target point. Introducing the dual physical constraints of dexterity and overturning risk can eliminate dangerous postures at the source, which not only avoids the motor jamming caused by the robotic arm getting stuck in kinematic singularities, but also completely avoids the vehicle overturning disaster caused by the center of gravity shift in the parking environment on the lunar surface and other slopes, thus achieving absolute physical safety. S4. Treat the key nodes of the discrete path as hard constraints, generate a multi-order continuous smooth trajectory through time parameterization optimization, and send it to the control system for execution.

[0026] In some embodiments, S1 specifically includes the following steps: S11. Obtain the attitude information of the mobile robotic arm chassis when it is parked through vehicle-mounted sensors, and construct a real world coordinate system and tilted support polygon; S12. Based on the given DH parameters, establish the forward kinematics model of the mobile robotic arm; then construct the tool coordinate system at the end of the mobile robotic arm. S13. Construct a vehicle static model that integrates the chassis tilt attitude, and then construct a global vehicle joint static center of mass model based on the vehicle static model. S14. Using the joint degrees of freedom of the mobile robotic arm as the search dimension, and combining the positive kinematics model with the global vehicle joint static centroid model, construct a configuration space for planning.

[0027] In some embodiments, S11 specifically includes the following steps: S111. Considering that the chassis of the mobile robotic arm often tilts when parking on uneven terrain, the roll angle of the mobile robotic arm chassis when parking is obtained using onboard IMU (Inertial Measurement Unit) sensors. and pitch angle ; S112. Set the world coordinate system through calibration parameters; let the base coordinate system of the mobile robotic arm chassis be Base, and the world coordinate system be... Base coordinate system relative to world coordinate system rotation matrix Represented as: ; in, The base coordinate system (Base) of the mobile robotic arm is represented relative to the absolute world coordinate system. of 3D attitude rotation matrix; This indicates that the chassis is revolved around its own base coordinate system. Axis rotation roll angle The independent homogeneous rotation component matrix; This indicates that the chassis is revolved around its own base coordinate system. Axis rotation pitch angle The independent homogeneous rotation component matrix; This indicates the chassis roll angle value measured and output by the onboard IMU sensor; This indicates the chassis pitch angle value measured and output by the onboard IMU sensor.

[0028] S113, Based on roll angle and pitch angle Solve the inclined support polygon formed by the contact point between the wheel and the non-flat terrain in the world coordinate system. .

[0029] In some embodiments, S12 specifically includes the following steps: S121. Construct a forward kinematics model based on the given DH parameters, with the following expression: ; ; ; in, Indicates the first... The actual rotation angle of each joint is calculated by substituting it into the calculation after considering the initial mechanical zero-point offset; This indicates the number of times the mobile robotic arm reads data in real time via an onboard encoder. Input values ​​for the current motion angles of each joint; Indicates the first Each joint has a pre-calibrated and solidified initial mechanical assembly zero-point offset constant; Indicates the first The axis of the first joint and the first The torsional angle of the link between the joint axes; Indicates the common normal of adjacent links in the th case. Offset axial distance on the axis of each joint; Indicates the first The joint coordinate system relative to the first joint coordinate system A joint coordinate system Homogeneous transformation matrix; This represents the joint degrees of freedom of the mobile robotic arm; in this embodiment, its specific value is 6. This represents the pose matrix of the end effector flange coordinate system relative to the base coordinate system Base.

[0030] S122. Then, for the mobile robotic arm, extend the tool coordinate system based on the flange coordinate system and construct a compensation matrix for the tool coordinate system relative to the flange coordinate system. The expression is as follows: ; in, Trans This represents the spatial translation transformation operator matrix, used to calculate and generate standard translation transformations based on three independent input translation parameters. Homogeneous translation matrix of order 1; This indicates the translation parameter of the tool coordinate system relative to the flange coordinate system; Rot This represents the spatial rotation transformation operator matrix, used to generate a standard rotation transformation based on three independent input rotation angle parameters. Homogeneous rotation matrix of order 1; This indicates the rotation parameters of the tool coordinate system relative to the flange coordinate system; S123, Then construct the tool coordinate system in the world coordinate system. pose matrix below The expression is as follows: ; in, The base coordinate system (Base) of the mobile robotic arm is represented relative to the world coordinate system. The rotation matrix.

[0031] In some embodiments, S13 specifically includes the following steps: S131. Introduce a vehicle mass topology distribution structure to construct a vehicle static model that incorporates chassis tilt attitude; S132. Based on the vehicle's mass topology and combining the forward kinematics model and the vehicle static model, a global vehicle joint static centroid model is constructed, including the vehicle chassis and the dynamic robotic arm. The expression for the global vehicle joint static centroid model is as follows: ; in, In the configuration vector Below, the combined center of mass of the entire vehicle is located in the world coordinate system. The three-dimensional spatial coordinate vector in the image changes dynamically and nonlinearly in three-dimensional space as the robotic arm swings. The total mass of the mobile composite robot chassis (including the chassis frame, onboard energy storage battery, hub motor, shell, and stationary onboard edge computing device) is a fixed, known constant obtained from pre-weighing within the system. This indicates that the chassis's independent center of mass is in the world coordinate system. The three-dimensional absolute position vector in the world coordinate system is a fixed constant vector when the chassis is parked and stationary. Indicates the first robotic arm The independent total mass of each motion link (including the internal metal frame, joint servo motor driver, reducer, and drive shaft structure) is a known physical constant in the system, where... ; Represents the cascaded matrix through forward kinematic transformation Combine the local centroid vectors of each link and incorporate them into the chassis parking rotation matrix. Then, the solution obtained the first... The absolute center of mass of each link is in the world coordinate system. The three-dimensional physical space coordinate vector in the image. In this embodiment, since the mobile robotic arm is a six-joint, six-link, six-degree-of-freedom mobile robotic arm, therefore... The value is 6.

[0032] In some embodiments, S2 specifically includes the following steps: S21. Construct a Gaussian probability distribution model with the line connecting the current end of the mobile robotic arm and the target point as the main axis; dynamically adjust the covariance matrix of the Gaussian probability distribution model according to environmental perception information to achieve adaptive scaling of the barrier-free area and the obstacle area. S22. Set the mixed sampling probability threshold, and generate preliminary sampling points based on the Gaussian probability distribution model and covariance matrix, combined with global uniform random sampling and Gaussian heuristic sampling. S23. Introduce a dynamic repulsive field based on the obstacle bounding box to the generated preliminary sampling points and calculate the acceptance probability of the current preliminary sampling points; S24. Based on the Monte Carlo rejection sampling mechanism and the forward kinematics model, compare the generated random number with the acceptance probability. If the comparison fails, return to S22; if it succeeds, output the candidate node. .

[0033] In some embodiments, the expression for the Gaussian probability distribution model in S21 is as follows: ; in, Represents the probability density function; The dimension representing the joint configuration space of the mobile robotic arm; in this embodiment, for a six-degree-of-freedom robotic arm, The specific value is 6; Represents the joint configuration vector of the sampling point; This represents the mean vector of a Gaussian distribution. Indicates transpose; Let be the determinant of the covariance matrix; Denotes the covariance matrix. The calculation formula is as follows: ; in, The preset gain coefficient; The Euclidean distance is the node closest to the target in the current search tree or the distance from the current end to the nearest obstacle surface. Represents the identity matrix.

[0034] In some embodiments, S22 specifically includes the following steps: S221. Set the Gaussian heuristic sampling probability threshold as follows: Before generating each sampling point, generate a set that follows... Uniformly distributed random numbers ; S222, if Then, global uniform random sampling is performed throughout the configuration space to generate preliminary sampling points obtained through global uniform random sampling. To ensure the probabilistic completeness of the algorithm and avoid getting trapped in local minima; uniform initial sampling points. The expression is as follows: ; in, For all elements to obey A uniformly distributed 6-dimensional random column vector. This indicates that the corresponding elements of the vector are multiplied one by one. The vector representing the upper limit of the extreme pose of the mobile robotic arm; The vector representing the lower limit of the extreme pose of the mobile robotic arm; S223, if Then Gaussian heuristic sampling is initiated, using the Gaussian probability distribution model and covariance matrix to generate initial sampling points, the expression of which is as follows: ; in, This represents the initial sampling point vector generated through Gaussian heuristic sampling; This represents the mean vector of the Gaussian probability distribution model, i.e., the joint configuration vector corresponding to the target point; This indicates that the vector follows a zero-mean vector with a covariance matrix of... A multidimensional Gaussian distributed random perturbation column vector.

[0035] In some embodiments, S24 specifically includes the following steps: S241. Set the absolute safety rejection threshold for obstacles. Extract the generated preliminary sampling points Using the forward kinematics model, calculate the actual shortest distance between the robotic arm and the obstacle under this configuration. ; S242, Based on the actual shortest distance Calculate the current preliminary sampling points Probability of being ultimately accepted The calculation formula is as follows: ; S243. Enter the Monte Carlo rejection sampling decision stage and generate the second conformity. Uniformly distributed random numbers ;like If so, the initial sampling point will be accepted and used as the final candidate node for this round. and output; if If the initial sampling point is determined to be repelled by the strong repulsive force of the obstacle, it is discarded and then returned to S22 to resample until a candidate node is successfully output.

[0036] In some embodiments, S3 specifically includes the following steps: S31. Traverse the current anti-overturning safety search tree and find the distance to the candidate node. The nearest tree node is used to extend towards the candidate node with a preset maximum growth step size, generating a new node to be verified. ; S32, For the new node Perform basic bounding box interference detection and dexterity verification. If both pass, proceed to S33; otherwise, return to S2. S33. Calculate the total centroid of the current configuration using the global vehicle joint static centroid model, and project the total centroid onto the chassis parking ramp. If the projection point exceeds the inclined support polygon, it is determined that there is a risk of overturning, and the current candidate node is removed, and the process returns to S2; otherwise, a new node that passes the dual physical constraint verification is obtained. Then it enters S34; S34. For new nodes that pass the dual physical constraint verification Perform local tree topology optimization to minimize path accumulation cost and validate new nodes through multiple physical constraints. Formally incorporated into the anti-overturning safety search tree; S35. Determine whether the target point has been reached. If so, backtrack to extract the key nodes of the discrete path from the starting point to the target point. Otherwise, return to S2.

[0037] In some embodiments, S31 specifically includes the following steps: S311, For candidate nodes Calculate candidate nodes The set of nodes in the anti-overturning safety search tree The Euclidean distance; S312. Find candidate nodes based on Euclidean distance. nearest node ; S313, along the nearest node Pointing to candidate nodes Direction, with a preset maximum growth step size Extend the process to obtain a new node to be verified. The expression is as follows: ; in, This indicates Euclidean distance.

[0038] In some embodiments, S32 specifically includes the following steps: S321. Construct new nodes using forward kinematics models. The 3D bounding boxes of each link of the mobile robotic arm in the given configuration are used to detect the distance from the nearest node to the new node. Check if the local path segment geometrically interferes with environmental obstacles; if a collision occurs, discard the new node. ; S322. If there is no collision, perform a dexterity check. The specific operation of the dexterity check is as follows: Extract new node Jacobian matrix of the mobile robotic arm in the configuration And using the Jacobian matrix Calculate the operability index The calculation formula is as follows: ; Where wz represents the operability index of the mobile robotic arm under the new node configuration; Mathematical operators used to calculate the determinant of a square matrix; This indicates that the mobile robotic arm is at the new node to be verified. The basic Jacobian matrix under the configuration; This represents the new node joint configuration vector to be verified; S323, Set the singularity security threshold as follows ,like If the configuration is too rigid, it is likely to cause the joint motor to jam and overload. In this case, it should be forcibly pruned and removed, and then returned to S2. Otherwise, it should proceed to S33.

[0039] In some embodiments, S33 specifically includes the following steps: S331, Extract the chassis rotation matrix And extract the rotation matrix. The third column is used as a plane Unit normal vector in world coordinate system ;in, Representing planes respectively The X, Y, and Z axis directional components of the unit normal vector in the world coordinate system; S332, Select the inclined support polygon Any wheel contact point As a plane Given points on the current configuration; calculate the total centroid under the current configuration. Let the coordinates of the global centroid be... , will the total mass center Along the world coordinate system The direction of gravity, i.e., the negative direction of the Z-axis, has the following direction vector: Projected downwards onto the plane where the chassis is located. Find the projection point Based on the theorem of the intersection of a line and a plane in space, this projection point... The analytical expression is: ; ; in, These represent the X-axis, Y-axis, and Z-axis coordinates of any selected wheel contact point in the world coordinate system, respectively. These represent the X-axis, Y-axis, and Z-axis coordinates of the global centroid in the world coordinate system under the current configuration; This indicates that the total centroid is projected onto the plane along the negative Z-axis. The corresponding longitudinal projection distance parameter.

[0040] S333, Obtain the projection point Then, the vector cross product method is used to determine whether it is located within the inclined support polygon. Internally, a tilted support polygon is set. The contact points of the four wheels are arranged in a counter-clockwise order as follows: For inclined support polygons Each edge vector and the vector from vertex to projection point ;in, Represents a polygon with inclined support Upper i Each wheel contact point; then calculate its cross product: ; in, This represents the cross product of the edge vector and the vector of the projection point; S334, If all cross product result vectors with the normal vector of the inclined plane The dot product of all these terms is greater than zero, that is... If it always holds true, then it means that the projection point... Located in the inclined support polygon Internally, this posture is safe; if any result is less than or equal to zero, it is determined that when the mobile robotic arm moves to this node, the center of gravity of the mobile robotic arm will become unstable and a tipping disaster will occur, and the new node will be discarded. .

[0041] In some embodiments, S34 specifically includes the following steps: S341, with new node Define a search radius centered on the search area. Within the search radius Find the set of all tree nodes in the neighborhood. Calculate from the starting point via Each node reaches the new node Cumulative cost The expression is as follows: ; in, The edge cost is represented by the path length cost. With security penalties Weighted combination; edge cost The calculation formula is as follows: ; in, and These are distance weight and safety penalty weight, respectively. Represents the Euclidean distance in configuration space. This represents the cost of security penalties; the calculation formulas are as follows: ; ; in, This is the normalized adjustment coefficient; S342, Traverse and calculate the set of all tree nodes. After considering the cumulative costs of all candidate nodes, select the node that minimizes the cumulative cost as the new node. Find the optimal parent node and connect them to form the trunk; S343, Traverse the set of all tree nodes Other nodes in the evaluation, if a new node is used. As their parent node, can we reduce their original accumulated cost? If so, disconnect their original parent node connection and reconnect them to the new node. This allows for local tree topology optimization.

[0042] In some embodiments, S4 specifically includes the following steps: S41. Based on the Euclidean distance between adjacent key nodes in the discrete path, and according to the set average execution speed, allocate a time period for each trajectory segment. S42. In order to ensure that the trajectory is absolutely continuous in the four physical levels of position, velocity, acceleration and jerk, construct a seventh-order time polynomial model for each discrete path segment; solve for the fourth derivative of the seventh-order time polynomial model of each discrete path segment with respect to time. S43. Using the sum of the square integrals of the fourth derivatives of each discrete path with respect to time as the objective function, derive and construct the quadratic objective cost matrix. S44. Take the key points of the path as hard constraints for position, and extract the velocity, acceleration and jerk continuity conditions at the intersection of each discrete path segment to construct a global linear equality constraint matrix. S45. Input the quadratic objective cost matrix and the global linear equality constraint matrix into the quadratic programming solver to calculate the polynomial coefficients that minimize the objective function. Substitute the polynomial coefficients into the seventh-order time polynomial model to generate a multi-order continuous smooth trajectory. Generate control commands based on the multi-order continuous smooth trajectory and discretize them according to the control cycle to send them to the control system for execution.

[0043] In some embodiments, the expression for the seventh-order time polynomial model in S42 is as follows: ; in, Indicates the first discrete path trajectory in The joint position command value at any given time; The coefficients of the polynomial to be solved; Represents the relative time of the independent variable of The power of 1, and satisfying ,in For the first The time period assigned to the segment trajectory. Let represent the degree of a polynomial monomial, and let the degree increase from 0 to 7.

[0044] The expression for the fourth derivative of the seventh-order time polynomial model in S42 with respect to time is as follows: ; in, This represents the fourth derivative of the seventh-order time polynomial model with respect to time. Let represent the degree of a polynomial monomial, and the degree increases from 4 to 7. Represents the relative time of the independent variable of Power of 1.

[0045] The expression for the objective function in S43 is as follows: ; in, Represent the objective function; k Indicates the first k Discrete path segment; M This represents the total number of segments in a discrete path; Let represent the polynomial coefficient vector, and satisfy . ; For the first The cost matrix of a discrete path segment. The Middle Line number The formula for calculating the element parsing of a column is: .

[0046] In some embodiments, the expression for the global linear equality constraint matrix in S44 is as follows: ; in, The coefficient matrix representing the global linear equality constraints; Represents a vector of constant terms; This represents a column vector of joint unknown coefficients, which consists of the polynomial coefficients of all discrete path segments.

[0047] The global linear equality constraint matrix covers the following physical conditions: The starting and ending points of each trajectory segment must strictly pass through the planned safe path points. and The expression is as follows: ; ; in, Indicates the first Time polynomial trajectory function of a discrete path segment; Indicates the first The length of the time period allocated to a segment of discrete path trajectory; Indicates the sequence number of the discrete path segment currently being calculated; Indicates the first The status of the starting safe path point corresponding to the segment of discrete path; Indicates the first The status of the safe path point corresponding to the segment discrete path.

[0048] At the intersection of two adjacent trajectory segments, their velocity, acceleration, and jerk must be equal in magnitude and in the same direction, as expressed below: ; in, Indicates the first The discrete path trajectory function with respect to time is the first segment First derivative; Let represent the order of the derivative of the trajectory function with respect to time, and when When the value is 1, 2, or 3, it corresponds to the physical quantities of velocity, acceleration, and jerk of the trajectory, respectively.

[0049] The robotic arm must have zero velocity, acceleration, and jerk at the very beginning and end of its entire motion trajectory to ensure shock-free start-stop. The expression is as follows: ; ; in, The function representing the first discrete path trajectory segment at the very beginning of the entire trajectory with respect to time is... First derivative; This represents the last one in the entire trajectory. The discrete path trajectory function with respect to time is the first segment The first derivative.

[0050] This invention constructs a vehicle static model integrating chassis tilt attitude and performs anti-tipping verification based on the joint total center of mass gravity projection. This deeply couples environmental gravity constraints with chassis attitude, ensuring absolute anti-tipping safety of the mobile robotic arm under heavy loads from a fundamental static perspective. Simultaneously, adaptive obstacle rejection and Gaussian heuristics simplify the path search process in complex obstacle environments, enhancing the invention's adaptability and computational efficiency in extremely confined spaces. Furthermore, this invention employs a seventh-order time polynomial model to optimize trajectory, replacing the traditional geometric polyline path. This eliminates transient inertial shocks caused by sudden starts and stops of the mobile robotic arm, avoiding the shaking disturbances caused by reaction forces on the chassis of the mobile robotic arm in a parked state. The pre-introduction of basic bounding box interference detection during node expansion further enhances the model's ability to protect system hardware health, promoting the reliability and generalization ability of the mobile robotic arm in extreme dynamic environments such as deep space exploration.

[0051] In another aspect, the present invention provides a mobile robotic arm anti-tipping motion planning system for non-flat terrain, configured to execute a mobile robotic arm anti-tipping motion planning method for non-flat terrain, comprising: The modeling and space construction module is used to acquire the environmental and attitude information of the mobile robotic arm under the chassis parking state, and to construct a vehicle static model and configuration space that integrates the chassis tilt attitude. A heuristic sampling module is used to perform spatial sampling within the configuration space and generate candidate nodes based on an adaptive barrier rejection and Gaussian heuristic strategy. The constraint filtering and planning module is used to verify and filter the candidate nodes based on dual physical constraints of dexterity and overturning risk, construct an anti-overturning safety search tree, and extract key points of discrete paths. The trajectory optimization module is used to take the key points of the discrete path as hard constraints, generate a multi-order continuous smooth trajectory through time parameterization optimization, generate control commands based on the multi-order continuous smooth trajectory, and send them to the control system for execution.

[0052] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Furthermore, the technical solutions of the various embodiments of the present invention can be combined with each other, but this must be based on the ability of those skilled in the art to implement them. When the combination of technical solutions is contradictory or cannot be implemented, it should be considered that such a combination of technical solutions does not exist and is not within the scope of protection claimed by the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A method for anti-overturning motion planning of a mobile manipulator facing non-flat terrain, characterized in that, Includes the following steps: S1. Establish the forward kinematic model of the mobile robotic arm and construct the configuration space; S2. Perform spatial sampling within the configuration space and generate candidate nodes based on an adaptive obstacle rejection and Gaussian heuristic strategy. S3. Based on the dual physical constraints of dexterity and overturning risk, the candidate nodes are verified and filtered to construct an anti-overturning safety search tree and extract the key nodes of the discrete path from the starting point to the target point. S4. Treat the key nodes of the discrete path as hard constraints, generate a multi-order continuous smooth trajectory through time parameterization optimization, and send it to the control system for execution.

2. The anti-overturning motion planning method for a mobile manipulator facing non-flat terrains according to claim 1, wherein, S1 specifically includes the following steps: S11. Obtain the attitude information of the mobile robotic arm chassis when it is parked through vehicle-mounted sensors, and construct a real world coordinate system and tilted support polygon; S12. Based on the given DH parameters, establish the forward kinematics model of the mobile robotic arm; then construct the tool coordinate system at the end of the mobile robotic arm. S13. Construct a vehicle static model that integrates the chassis tilt attitude, and then construct a global vehicle joint static center of mass model based on the vehicle static model. S14. Using the joint degrees of freedom of the mobile robotic arm as the search dimension, and combining the positive kinematics model with the global vehicle joint static centroid model, construct a configuration space for planning.

3. The anti-overturning motion planning method for a mobile manipulator facing non-flat terrains according to claim 2, wherein, S12 specifically includes the following steps: S121. Construct a forward kinematics model based on the given DH parameters, with the following expression: ； ； ； in, Indicates the first... The rotation angle of each joint; This indicates the number of times the mobile robotic arm reads data in real time via an onboard encoder. Configuration vectors of joints, where ; Indicates the first Each joint has a pre-calibrated and solidified initial mechanical assembly zero-point offset constant. In the standard DH parameter method, the first The axis of the first joint and the first The common normal distance of the link length between the axes of the joints; Indicates the first The axis of the first joint and the first The torsional angle of the link between the joint axes; Indicates the common normal of adjacent links in the th case. Offset axial distance on the axis of each joint; Indicates the first The joint coordinate system is connected to the first... Homogeneous transformation matrix of the joint coordinate system; This indicates the total number of joints in the mobile robotic arm, which corresponds to the joint degrees of freedom of the robotic arm. This represents the pose matrix of the flange coordinate system relative to the base coordinate system Base; S122. Then, for the mobile robotic arm, extend the tool coordinate system based on the flange coordinate system and construct a compensation matrix for the tool coordinate system relative to the flange coordinate system. The expression is as follows: ； in, This represents the spatial translation transformation operator matrix, used to calculate and generate standard translation transformations based on three independent input translation parameters. Homogeneous translation matrix of order 1; This indicates the translation parameter of the tool coordinate system relative to the flange coordinate system; This represents the spatial rotation transformation operator matrix, used to generate a standard rotation transformation based on three independent input rotation angle parameters. Homogeneous rotation matrix of order 1; This indicates the rotation parameters of the tool coordinate system relative to the flange coordinate system; S123, Constructing the tool coordinate system in the world coordinate system pose matrix below The expression is as follows: ； in, The base coordinate system (Base) of the mobile robotic arm is represented relative to the world coordinate system. The rotation matrix.

4. The anti-tipping motion planning method for a mobile robotic arm oriented towards non-flat terrain according to claim 3, characterized in that, S13 specifically includes the following steps: S131. Introduce a vehicle mass topology distribution structure to construct a vehicle static model that incorporates chassis tilt attitude; S132. Based on the vehicle's mass topology and combining the forward kinematics model and the vehicle static model, a global vehicle joint static centroid model is constructed, including the vehicle chassis and the dynamic robotic arm. The expression for the global vehicle joint static centroid model is as follows: ； in, In the configuration vector Below, the combined center of mass of the entire vehicle is located in the world coordinate system. The three-dimensional spatial coordinate vector; This indicates the total mass of the chassis of the mobile composite robot; This indicates that the chassis's independent center of mass is in the world coordinate system. The absolute position vector in three-dimensional space; Indicates the first robotic arm The total independent mass of each moving link in the system is a known physical constant in advance, where... ; Indicates the first The absolute center of mass of each link is in the world coordinate system. The three-dimensional physical space coordinate vector in the text.

5. The anti-tipping motion planning method for a mobile robotic arm oriented towards non-flat terrain according to claim 4, characterized in that, S2 specifically includes the following steps: S21. Construct a Gaussian probability distribution model with the line connecting the current end of the mobile robotic arm and the target point as the main axis; dynamically adjust the covariance matrix of the Gaussian probability distribution model according to environmental perception information to achieve adaptive scaling of the barrier-free area and the obstacle area. S22. Set the mixed sampling probability threshold, and generate preliminary sampling points based on the Gaussian probability distribution model and covariance matrix, combined with global uniform random sampling and Gaussian heuristic sampling. S23. Combining the forward kinematics model, a dynamic repulsive force field based on the obstacle bounding box is introduced into the generated preliminary sampling points, and the acceptance probability of the current preliminary sampling points is calculated. S24. Based on the Monte Carlo rejection sampling mechanism, the generated random number is compared with the acceptance probability. If it fails, return to S22 for resampling. If it succeeds, output the candidate node.

6. The method for anti-tipping motion planning of a mobile robotic arm oriented towards non-flat terrain according to claim 5, characterized in that, The expression for the Gaussian probability distribution model in S21 is as follows: ； in, Represents the probability density function; n This represents the dimension of the joint configuration space of the mobile robotic arm; Indicates the sampling point; This represents the mean vector of a Gaussian distribution. T Indicates transpose; Let be the determinant of the covariance matrix; Denotes the covariance matrix. The calculation formula is as follows: ； in, The preset gain coefficient; The Euclidean distance is the node closest to the target in the current search tree or the distance from the current end to the nearest obstacle surface. Represents the identity matrix.

7. The method for anti-tipping motion planning of a mobile robotic arm oriented towards non-flat terrain according to claim 5, characterized in that, S3 specifically includes the following steps: S31. Traverse the current anti-overturning safety search tree and find the distance to the candidate node. The nearest tree node is used to extend towards the candidate node with a preset maximum growth step size, generating a new node to be verified. ; S32, For the new node Perform basic bounding box interference detection and dexterity verification. If both pass, proceed to S33; otherwise, return to S2. S33. Calculate the total centroid of the current configuration using the global vehicle joint static centroid model, and project the total centroid onto the chassis parking ramp. If the projection point exceeds the inclined support polygon, it is determined that there is a risk of overturning, and the current candidate node is removed, and the process returns to S2; otherwise, a new node that passes the dual physical constraint verification is obtained. Then it enters S34; S34. For new nodes that pass the dual physical constraint verification Perform local tree topology optimization to minimize path accumulation cost and validate new nodes through multiple physical constraints. Formally incorporated into the anti-overturning safety search tree; S35. Determine whether the target point has been reached. If so, backtrack to extract the key nodes of the discrete path from the starting point to the target point. Otherwise, return to S2.

8. The method for anti-tipping motion planning of a mobile robotic arm oriented towards non-flat terrain according to claim 7, characterized in that, S31 specifically includes the following steps: S311, For candidate nodes Calculate candidate nodes The set of nodes in the anti-overturning safety search tree The Euclidean distance; S312. Find candidate nodes based on Euclidean distance. nearest node ; S313, along the nearest node Pointing to candidate nodes Direction, with a preset maximum growth step size Extend the process to obtain a new node to be verified. The expression is as follows: ； in, This indicates Euclidean distance.

9. The anti-tipping motion planning method for a mobile robotic arm oriented towards non-flat terrain according to claim 7, characterized in that, S4 specifically includes the following steps: S41. Based on the Euclidean distance between adjacent key nodes in the discrete path, and according to the set average execution speed, allocate a time period for each trajectory segment. S42. In order to ensure that the trajectory is absolutely continuous in the four physical levels of position, velocity, acceleration and jerk, construct a seventh-order time polynomial model for each discrete path segment; solve for the fourth derivative of the seventh-order time polynomial model of each discrete path segment with respect to time. S43. Using the sum of the square integrals of the fourth derivatives of each discrete path with respect to time as the objective function, derive and construct the quadratic objective cost matrix. S44. Take the key points of the path as hard constraints for position, and extract the velocity, acceleration and jerk continuity conditions at the intersection of each discrete path segment to construct a global linear equality constraint matrix. S45. Input the quadratic objective cost matrix and the global linear equality constraint matrix into the quadratic programming solver to calculate the polynomial coefficients that minimize the objective function. Substitute the polynomial coefficients into the seventh-order time polynomial model to generate a multi-order continuous smooth trajectory. Generate control commands based on the multi-order continuous smooth trajectory and discretize them according to the control cycle to send them to the control system for execution.

10. The anti-tipping motion planning method for a mobile robotic arm oriented towards non-flat terrain according to claim 9, characterized in that, The expression for the seventh-order time polynomial model in S42 is as follows: ； in, Indicates the first discrete path trajectory in The joint position command value at any given time; The coefficients of the polynomial to be solved; This represents the relative running time within the current trajectory segment, satisfying... ,in For the first The time period for which the segment trajectory is assigned; The expression for the fourth derivative of the seventh-order time polynomial model in S42 with respect to time is as follows: ； in, This represents the fourth derivative of the seventh-order time polynomial model with respect to time. j This indicates the degree of a polynomial monomial. Represents the relative time of the independent variable t of j -4th power; The expression for the objective function in S43 is as follows: ； in, The global total objective cost is represented by the sum of the square integrals of the fourth derivatives of the positions with respect to time for all trajectory segments. This represents the index of the currently calculated discrete path segment, satisfying... ; This represents the total number of segments into which the entire planned path is divided; Indicates the first The length of the time period allocated to a segment of discrete path trajectory; Indicates the first The fourth derivative function of the discrete path trajectory with respect to time; Let represent the polynomial coefficient vector. The expression for the polynomial coefficient vector is: 。 11. A mobile robotic arm anti-tipping motion planning system for non-flat terrain, characterized in that, Configured to perform an anti-tipping motion planning method for a mobile robotic arm oriented towards non-flat terrain as described in any one of claims 1 to 10, comprising: The modeling and space construction module is used to acquire the environmental and attitude information of the mobile robotic arm under the chassis parking state, and to construct a vehicle static model and configuration space that integrates the chassis tilt attitude. A heuristic sampling module is used to perform spatial sampling within the configuration space and generate candidate nodes based on an adaptive barrier rejection and Gaussian heuristic strategy. The constraint filtering and planning module is used to verify and filter the candidate nodes based on dual physical constraints of dexterity and overturning risk, construct an anti-overturning safety search tree, and extract key points of discrete paths. The trajectory optimization module is used to take the key points of the discrete path as hard constraints, generate a multi-order continuous smooth trajectory through time parameterization optimization, generate control commands based on the multi-order continuous smooth trajectory, and send them to the control system for execution.

Images