A motion control method for a mobile manipulator in a dynamic scene and application thereof

Abstract

The application discloses a kind of dynamic scene under the motion control method of mobile manipulator and its application, using model predictive control, the state and control input quantity in future period are predicted, the deviation between the end pose of manipulator and target pose is added as a quadratic cost item to cost function, and set several collision points in the prominent position of mobile manipulator, the distance between these collision points and the obstacle in space is added as a penalty term to cost function, while the motion speed of robot, motion acceleration, the joint angle motion range of manipulator, joint angle acceleration and robot self-collision avoidance are added as constraint to model predictive controller, solve the optimal control input queue and act on the controller of robot, solve the defects, such as adaptability difference, stability deficiency of existing mobile manipulator in face of dynamic complex scene, improve the working stability of mobile manipulator robot.

Description

Technical Field

[0001] This invention discloses a method for controlling the movement of a robotic arm in dynamic scenarios and its application. Background Technology

[0002] With the continuous development of robotics technology in recent years, robots are increasingly replacing traditional human labor, with automated machinery replacing manual operation. Mobile dual-arm robots, also known as mobile robotic arms, are automated devices capable of large-scale, flexible operations in industries such as manufacturing, agriculture, and services. Robotic arms mounted on mobile robot platforms offer a larger workspace than traditional fixed robotic arms. For example, the disinfection robot disclosed in Chinese Patent Application No. CN202221700704.6 features dual-arm robots whose robotic arms can be equipped with various actuators at their ends. The robot consists of a two-degree-of-freedom mobile chassis supporting two four-degree-of-freedom robotic arms, requiring precise movement of the robotic arm ends to designated positions during operation.

[0003] The motion control technology of mobile robotic arms mainly covers two aspects: the motion control of the robotic arm itself and the motion control of the mobile robot. Firstly, the motion control of the mobile robotic arm primarily includes the motion control of the robotic arm's shutdown and the end effector, as well as overall motion control. The end effector must possess precise and stable motion capabilities, such as grasping, rotation, and elevation / lowering. To achieve these functions, multiple motors need to be controlled and their movements coordinated, using small motion adjustments to control the precise position and orientation of the end effector in space. Secondly, it also includes path planning and trajectory generation for the robot's mobile chassis. Robot mobile chassis are generally classified into wheeled, tracked, serpentine, and jumping types. Among these, wheeled mobile robots have advantages such as low cost, flexible movement, convenient control, and suitability for relatively flat indoor environments, thus gaining widespread application. The motion control of mobile robots needs to ensure that the robot can move stably and accurately to the designated location when performing tasks, requiring kinematic and dynamic simulations. During the movement of the mobile robotic arm, the motion of the robotic arm in three-dimensional space needs to be considered.

[0004] In practical applications, the motion control of mobile robotic arms needs to consider many other factors, such as environmental perception, obstacle avoidance, and task planning. During the robot path planning process, it is necessary not only to ensure obstacle avoidance of the robot chassis, but also to ensure obstacle avoidance of the robotic arm and its end effector. Moreover, the actual motion environment is a dynamic and complex scenario with various stationary and moving obstacles. Effectively coordinating the control of the robot's mobile chassis and robotic arm is a complex task. Compared with traditional fixed robotic arms, mobile robotic arms have more redundant degrees of freedom and weaker structural rigidity. At the same time, actual operations require coupled control of the mobile chassis and robotic arm, which makes the kinematic and dynamic algorithms complex. This results in slow real-time response speed of the robot to dynamic information during actual operations, which not only easily leads to low work efficiency, but also reduces stability. Summary of the Invention

[0005] The technical problem solved by this invention is to address the shortcomings of existing mobile robotic arms, such as poor adaptability and insufficient stability in dynamic and complex scenarios. This invention provides a motion control method and application for mobile robotic arms in dynamic scenarios, which couples the control of the robotic arm chassis and the robotic arm itself, ensuring that the mobile robotic arm can avoid dynamic and static obstacles in the environment during operation.

[0006] This invention is achieved using the following technical solution:

[0007] This invention first discloses a motion control method for a mobile robotic arm in a dynamic scene.

[0008] The mobile chassis and the mobile robot arm are jointly modeled to construct a kinematic model of the mobile robot arm. During the movement of the mobile robot arm, the position, attitude and velocity of the mobile chassis, as well as the angular velocity and angle of each joint of the robot arm, are updated in real time.

[0009] During the movement of the mobile robotic arm, it continuously identifies surrounding obstacles and classifies them into stationary obstacles, obstacles moving at a constant speed, and obstacles moving randomly according to their motion state. Several collision points are set at the protruding position of the mobile robotic arm to obtain the distance between it and the obstacles.

[0010] Based on the kinematic model of the mobile robotic arm and the state of the obstacle, model predictive control is used to predict the motion state and control input of the mobile robotic arm over a period of time. The deviation between the end-effector pose and the target pose is used as the quadratic cost term of the model predictive control cost function, and the real-time distance between the collision point and the obstacle is used as the penalty term of the model predictive control cost function. The weight of the penalty term of the cost function is adjusted according to three obstacle classifications. The motion parameters of the mobile robotic arm are used as constraints of the model predictive control cost function. The optimal set of control inputs is solved using a nonlinear optimization solver and applied to the mobile robotic arm controller to realize the motion control of the mobile robotic arm.

[0011] In a motion control method for a mobile robotic arm in a dynamic scenario according to the present invention, before the mobile robotic arm moves, an initial path is planned based on the global map of the motion space and the target point information, and several reference points are sampled in the initial path as path points for the movement of the mobile robotic arm.

[0012] In a motion control method for a mobile robotic arm in a dynamic scenario according to the present invention, the kinematic model of the mobile robotic arm is further defined as follows:

[0013]

[0014]

[0015]

[0016] Where z represents the motion state of the mobile robotic arm, and x and y represent the coordinates of the mobile chassis. Indicates the direction of movement of the mobile chassis. The dataset represents the angles of each joint of the robotic arm, where u represents the control input to the moving robotic arm. l u r These represent the rotational angular velocities of the drive wheels of the chassis for left and right movement, respectively. Represents the rotational angular velocity of each joint of the robotic arm. The first derivative represents the motion state of the mobile robotic arm, where R0 is the first derivative of the first derivative of the second ... first derivative of the second derivative of wheel L represents the radius of the drive wheels of the mobile chassis, and L represents the wheelbase between the drive wheels of the mobile chassis.

[0017] In a motion control method for a mobile robotic arm in a dynamic scenario according to the present invention, the deviation between the end-effector pose and the target pose is further calculated using the following formula:

[0018]

[0019] Where E(x) represents the deviation between the end effector pose and the target pose, e pos (x) represents the coordinates of the robotic arm's end effector, T pos e represents the coordinates of the target point of the mobile robotic arm's movement. o (x) represents the end effector posture of the robotic arm, T o The target point orientation represents the movement of the mobile robotic arm, and the symbol ||·||2 represents the L2 norm.

[0020] In a motion control method for a mobile robotic arm in a dynamic scene according to the present invention, the mobile robotic arm further acquires obstacle information by installing a 3D depth camera to output environmental obstacle point cloud information.

[0021] The mobile robotic arm identifies obstacles using the world coordinate system as a reference, parameterizes the obstacle cluster into several obstacle spheres, and tracks each obstacle sphere. The definition of each obstacle sphere is as follows:

[0022]

[0023] Among them O i Represents the i-th obstacle sphere. R represents the coordinates of the center of the i-th obstacle sphere. i Represents the radius of the sphere containing the i-th obstacle. This represents the state of the i-th obstacle sphere; when the obstacle is stationary, When the obstacle moves at a constant speed v x ,v y ,v z This represents the velocity and direction of the obstacle sphere in the world coordinate system. When the obstacle is not moving at a uniform velocity... This indicates that the obstacle sphere is in a state of irregular motion.

[0024] Within the observation interval M, obstacles are identified and measured multiple times, and their states are classified using the following formula:

[0025]

[0026]

[0027] in P represents the rate of change of the position of the i-th obstacle sphere, which is a three-dimensional vector. i and Q i Let h1 and h2 represent the average rate of change and the standard deviation of the rate of change of the i-th obstacle sphere, respectively. Two constants h1 and h2 are used to determine the three obstacle states:

[0028] P i

[0029] P i >h1 and Q i When h2 < h2, the obstacle is moving at a constant speed;

[0030] Q i >h2 indicates an obstacle with irregular movement.

[0031] ​In a motion control method for a mobile robotic arm in a dynamic scenario according to the present invention, the mobile robotic arm further constructs a collision point sphere with each collision point as the center, and adjusts the radius of the collision point sphere until the collision point sphere covers the mobile robotic arm. In the world coordinate system, when the mobile robotic arm detects that the distance between the obstacle and the collision point sphere is 0, it is determined that a collision has occurred.

[0032] During the movement of the mobile robotic arm, the Euclidean distance between the collision point and the obstacle is calculated using the coordinates of the centers of the spheres at the collision point and the obstacle. The obstacle avoidance penalty term D(d) of the model predictive control cost function is obtained by applying the following penalty function to the Euclidean distance between the collision point and the obstacle:

[0033]

[0034] Where μ and δ are penalty coefficients, adjusted according to the state of the obstacle, d safe The set safety distance means that if the moving robotic arm and the obstacle exceed this distance, they will no longer be subject to the penalty function. d is the Euclidean distance between the collision point and the obstacle.

[0035] When the collision point of the mobile robotic arm approaches an obstacle, the penalty function will incur a large cost, causing the robot to move in the direction away from the obstacle.

[0036] In a motion control method for a mobile robotic arm in a dynamic scenario according to the present invention, the Euclidean distance between the collision point on the mobile robotic arm and the obstacle is further defined by the following formula:

[0037]

[0038] Where c pos O represents the coordinates of the collision point of the mobile robotic arm in the world coordinate system. pos The coordinates of the obstacle are determined by the coordinates of the center of the obstacle sphere. mode The state of an obstacle is represented by the state of the obstacle sphere. It predicts the position of obstacles moving at a constant speed or stationary obstacles within a future time interval ΔT. For obstacles moving irregularly... mode = [-1, -1, -1], setting different penalty coefficients, defined as follows:

[0039]

[0040] μ1>μ2, δ1>δ2.

[0041] In a motion control method for a mobile robotic arm in a dynamic scenario according to the present invention, the cost function is further as follows:

[0042]

[0043] Where J is the performance index of the cost function, N is the prediction space of the model predictive control, and E N This represents the deviation between the end effector pose of the robotic arm and the target pose. It is E N The transpose of E n The deviation at each time step from 0 to N It is E n transpose, u n The control input dataset for model predictive control includes the rotational angular velocities of each drive wheel of the mobile chassis and the rotational angular velocities of each joint of the robotic arm, where n ranges from 0 to N. The control input of the mobile robotic arm at each moment in the prediction space is calculated, d(O i ,ΔT·n) is the predicted Euclidean distance between the collision point and the obstacle at time n. Representing the obstacle avoidance penalty term, A, B, and C are the weight matrices of the model predictive control, which determine the weights of the end-tracking error and the system input in the model predictive control.

[0044] The control input queue u with minimum J is obtained by using a nonlinear optimization solver. n The output acts on the mobile robotic arm controller to control the mobile robotic arm to move toward the target.

[0045] In a motion control method for a mobile robotic arm in a dynamic scenario according to the present invention, the cost function solution process further follows the constraints of the following hardware motion limit parameters of the mobile robotic arm.

[0046]

[0047] Where z represents the motion state of the mobile robotic arm, z min z max Let u be the minimum and maximum of its limits, and u represent the control input of the mobile robotic arm. min u max Let the minimum and maximum values ​​of its limit be . The first derivative representing the motion state of the mobile robotic arm. It is the minimum and maximum of its limit.

[0048] The present invention also discloses a mobile robotic arm employing the above-described motion control method for a mobile robotic arm in a dynamic scenario.

[0049] This invention proposes a motion control method for a mobile robotic arm in a dynamic scene, which can be mainly divided into two parts:

[0050] The first part is the motion planning of the mobile robotic arm. It employs Model Predictive Control (MPC), using a nonlinear optimization algorithm to continuously solve for the optimal cost function of the mobile robotic arm's state. It predicts the state and control inputs over a future period and solves for the optimal set of control inputs, which are then applied to the robot's controller. In solving the cost function using MPC, this invention adds the deviation between the robotic arm's end-effector pose and the target pose as a quadratic cost term. Several collision points are set at the protruding positions of the mobile robotic arm, and the distances between these collision points and obstacles in space are added as a penalty term to the cost function. Simultaneously, the robot's motion speed, motion acceleration, the range of motion of the robotic arm's joint angles, joint angular acceleration, and robot self-collision avoidance are added as constraints to the MPC. Finally, the optimization problem is solved using the nonlinear optimization solution library OCS2.

[0051] The second part concerns the perception and modeling of dynamic obstacles in the environment by the mobile robotic arm. The mobile robotic arm acquires obstacle information through point cloud data of environmental obstacles output by a 3D depth camera and performs parameterization processing on the obstacles. To reduce computational load, this invention models obstacles as spheres, consisting of the coordinates of the sphere's center and radius, and continuously updates obstacle parameters to obtain the obstacle's position and velocity information. During the movement of the mobile robotic arm, obstacle parameters are continuously updated, and obstacles are classified into stationary obstacles, uniformly moving obstacles, and irregularly moving obstacles based on their motion state. This invention sets different penalty coefficients for these three obstacle classifications and applies them to the cost function as weights for the obstacle avoidance penalty term during the movement of the mobile robotic arm.

[0052] Compared with the prior art, the beneficial effects of adopting the present invention are as follows:

[0053] 1) This invention utilizes model predictive control algorithms to handle various constraints of the mobile robotic arm. The deviation between the end-effector pose and the target pose is added as a quadratic cost term to the cost function, achieving precise end-effector tracking and predicting the robot's state over a future period, allowing it to anticipate potential environmental changes. The motion coupling between the mobile chassis and the robotic arm enables full-body control of the mobile robotic arm, significantly improving robot operation efficiency.

[0054] 2) This invention obtains the position and velocity information of obstacles by observing them and continuously updating their parameters. Based on the motion parameters of the obstacles, three obstacle states are set: stationary obstacles, obstacles moving at a constant speed, and obstacles moving erratically. The observer sets different penalty term coefficients for these three obstacle states and applies them to the weight of the obstacle avoidance penalty term in the cost function. The optimal control input for the mobile robotic arm is obtained by solving the problem, which takes into account both the flexibility and safety of the robot and ensures the safe operation of the robot in complex dynamic environments.

[0055] In summary, the motion control method for a mobile robotic arm in a dynamic scenario disclosed in this invention ensures that the mobile robotic arm robot can avoid dynamic and static obstacles in the environment during operation, solves the defects of existing mobile robotic arms such as poor adaptability and insufficient stability in the face of dynamic and complex scenarios, and improves the working stability of the mobile robotic arm robot. Attached Figure Description

[0056] To make the objectives, technical solutions, and advantages of the present invention clearer, the preferred embodiments of the present invention will be described in detail below with reference to the accompanying drawings, wherein:

[0057] Figure 1 This is a schematic diagram of a mobile dual-arm robot model provided in an embodiment of the present invention.

[0058] Figure 2 This is a flowchart illustrating the workflow of a mobile dual-arm robot performing spray disinfection operations in a dynamic scenario, as described in this embodiment of the invention.

[0059] Figure 3 This is a schematic diagram illustrating the motion control working principle framework of a mobile dual-arm robot performing spray disinfection operations in a dynamic scene, according to an embodiment of the present invention.

[0060] The numbers in the diagram are: 100-mobile chassis, 200-robotic arm, 300-end spraying device, 401-stationary obstacle, 402-moving obstacle, 500-target point. Detailed Implementation

[0061] The embodiments of the present invention will be further described below with reference to the accompanying drawings. These embodiments are implemented based on the technical solution of the present invention, providing detailed implementation methods and specific operating procedures. However, the scope of protection of the present invention is not limited to the following embodiments. Based on the embodiments of the present invention, embodiments made by those skilled in the art without other innovative work are all within the scope of protection of the present invention.

[0062] See Figure 1 The illustration shows a mobile dual-arm robot for spray disinfection operations. It includes a mobile chassis 100 and two robotic arms 200. At the end of each robotic arm 200 is a terminal spraying device 300 for automatically spraying disinfectant onto the surrounding work environment, reducing the workload of manual disinfection and the risk of infection. The specific mechanical structure of this mobile dual-arm robot is described in Chinese Patent Application No. CN202221700704.6, and will not be elaborated upon here.

[0063] This embodiment uses Figure 1The invention will be described in detail using the motion control of a mobile dual-arm robot during disinfection spraying as an example. The mobile dual-arm robot's chassis 100 is a differential motion chassis capable of two-dimensional planar motion, with independently controllable left and right drive wheels. Two four-degree-of-freedom robotic arms 200 are mounted on it. By controlling the motion of the mobile dual-arm robot, the end-effector spraying device 300 on the robotic arms 200 is moved to the target point for disinfection spraying. This requires controlling the movement of the mobile chassis 100 and adjusting the posture of the robotic arms 200. The mobile chassis 100 is equipped with sensors such as LiDAR, odometer, and IMU for state perception. Each joint of the robotic arms 200 is equipped with a motor encoder to acquire joint angles and angular velocities. A 3D depth camera is installed on the robot's head to acquire point cloud information of the environment within the disinfection spraying workspace. (See also...) Figure 3 The specific motion control process is as follows.

[0064] First, system modeling of the mobile robotic arm is performed. Figure 1 The mobile chassis 100 and the robotic arm 200 of the mobile dual-arm robot are jointly modeled to construct a kinematic model of the mobile robotic arm. During the movement of the mobile robotic arm, the position, attitude and speed of the mobile chassis, as well as the angular velocity and angle of each joint of the robotic arm, are updated in real time.

[0065] Specifically, the kinematic model of the mobile robotic arm is defined as follows:

[0066]

[0067]

[0068]

[0069] Where z represents the motion state of the mobile robotic arm, and x and y represent the coordinates of the mobile chassis. Indicates the direction of movement of the mobile chassis. The dataset represents the angles of each joint of the robotic arm, where u represents the control input to the moving robotic arm. l u r These represent the rotational angular velocities of the left and right drive wheels of the mobile chassis, respectively. Represents the rotational angular velocity of each joint of the robotic arm. The first derivative represents the motion state of the mobile robotic arm, where R0 is the first derivative of the first derivative of the second ... first derivative of the second derivative of wheel L represents the radius of the drive wheels of the mobile chassis, and L represents the wheelbase between the drive wheels of the mobile chassis.

[0070] Figure 1When the mobile dual-arm robot begins its spraying disinfection operation, it plans an initial path based on the existing global map and information about the target point 500, using a sampling method (such as RRTStar). Several reference points are sampled along this path. This is done to prevent the robot from getting stuck in a local optimum while moving towards the target point 500, thus avoiding failure to reach the target point. The path planning algorithm for mobile robots is a mature and well-known technology, and will not be elaborated upon in this embodiment.

[0071] like Figure 2 As shown, in order to avoid obstacles, the mobile dual-arm robot in this embodiment continuously identifies surrounding obstacles during its movement. According to the movement state of the obstacles, they are divided into stationary obstacles 401 and moving obstacles 402. The moving obstacles 402 are further subdivided into uniformly moving obstacles and irregularly moving obstacles. At the same time, several collision points are set at the protruding position of the mobile dual-arm robot to obtain the distance between it and the obstacles.

[0072] The mobile dual-arm robot acquires obstacle information by outputting point cloud information of environmental obstacles through a 3D depth camera mounted on it. The obstacles identified by the mobile dual-arm robot are referenced to the world coordinate system. The 3D depth camera obtains the three-dimensional coordinates of the obstacles, parameterizes the obstacle cluster into several obstacle spheres, and tracks each obstacle sphere. The definition of each obstacle sphere is as follows:

[0073]

[0074] Among them O i Represents the i-th obstacle sphere. R represents the coordinates of the center of the i-th obstacle sphere. i Represents the radius of the sphere containing the i-th obstacle. This represents the state of the i-th obstacle sphere; when the obstacle is stationary... When the obstacle moves at a constant speed v x ,v y ,v z This represents the velocity and direction of the obstacle sphere in the world coordinate system. When the obstacle is not moving at a uniform velocity... This indicates that the obstacle sphere is in a state of irregular motion.

[0075] When the mobile dual-arm robot moves, it performs multiple obstacle identification measurements within the observation interval M. The obstacle type is determined based on the observed obstacle speed and the standard deviation between the observed obstacle speeds. Specifically, the obstacle state is classified using the following formula:

[0076]

[0077]

[0078] where represents the change rate of the position of the i-th obstacle sphere, which is a three-dimensional vector, P i and Q i respectively represent the average change rate and the standard deviation of the change rate of the i-th obstacle sphere. When the obstacle is stationary, P i should be close to 0; when the obstacle is moving at a constant speed, P i should be large, while Q i should be close to 0. At this time, P i is the moving speed of the obstacle; when the obstacle is in a state of irregular motion, Q i should be large. Two constant constants h1 and h2 are used to determine the three obstacle states: P i < h1 is a stationary obstacle; P i > h1 and Q i < h2 is a uniformly moving obstacle; Q i > h2 is an irregularly moving obstacle. Considering measurement errors, h1 and h2 are selected as positive numbers close to 0. In this embodiment, the constant constants h1 = 0.05 m / s and h2 = 0.5 m / s.

[0079] At the same time, a number of collision points are set at prominent positions of the mobile dual-arm robot. The positions where the collision points are set include but are not limited to the end wrist, elbow, and shoulder positions of the robotic arm, the head, waist of the robot body, and the edge position of the mobile chassis. The mobile dual-arm robot constructs collision spheres with each collision point as the center of the sphere, and adjusts the radius of the collision sphere until the collision sphere surface covers the mobile robotic arm. In the world coordinate system, when the mobile dual-arm robot detects that the distance between an obstacle and the surface of one of the collision spheres is 0, it is determined that a collision has occurred. As Figure 2 shown, taking the collision point at the end wrist of the robotic arm near the spraying device 300 at the end of the robotic arm as an example, during the movement of the mobile dual-arm robot, the Euclidean distance between the collision point and the obstacle is calculated through the center coordinates between the collision sphere and the obstacle sphere, and the Euclidean distance between the collision point and the obstacle is used to obtain the obstacle avoidance penalty term D(d) of the model predictive control cost function through the following penalty function:

[0080]

[0081] where μ and δ are penalty coefficients, which are adjusted according to the state of the obstacle, and d safe is the set safety distance. The safety distance d in this embodiment safe= 2m, beyond which the mobile robotic arm and obstacle will no longer be subject to the penalty function constraint, where d is the Euclidean distance between the collision point and the obstacle. The Euclidean distance between the collision point and the obstacle on a mobile dual-arm robot is defined by the following formula:

[0082]

[0083] Where c pos O represents the coordinates of the collision point of the mobile robotic arm in the world coordinate system. pos The coordinates of the obstacle are determined by the coordinates of the center of the obstacle sphere. mode The state of an obstacle is represented by the state of the obstacle sphere. It predicts the position of obstacles moving at a constant speed or stationary obstacles within a future time interval ΔT. For obstacles moving irregularly... mode = [-1, -1, -1], setting different penalty coefficients, defined as follows:

[0084]

[0085] μ1>μ2, δ1>δ2.

[0086] Both are positive numbers greater than 0. It is sufficient to ensure that the penalty coefficient μ1 for irregular obstacles is greater than the penalty coefficient μ2 for uniform and stationary obstacles; δ1 and δ2 can be variable or constant. When an obstacle is in an irregular motion state and is judged to pose a significant threat to the robot, a relatively large penalty coefficient (mainly determined by μ) will be used, indicating that the moving dual-arm robot should move as far away from the obstacle as possible. For stationary or uniformly moving obstacles, the system can predict their position, and a smaller penalty coefficient can be used. In this embodiment, the parameter μ1 for irregularly moving obstacles is selected as 1 × 10⁻⁶. -2 δ1=1×10 -3 For stationary obstacles and obstacles moving at a constant speed, the penalty coefficient μ2 = 5 × 10 is selected. -3 δ2=1×10 -3 This is added as a penalty term to the model's predictive control cost function to predict the robot's state and obstacle coordinates over a future period.

[0087] When the collision point of the mobile robotic arm approaches an obstacle, the penalty function will incur a large cost. At this point, the robot is guided to move in the direction away from the obstacle based on the obtained gradient information.

[0088] When a mobile dual-arm robot moves, it is also necessary to track the trajectory of the robotic arm's end effector. The deviation between the end effector's pose and the target pose is calculated using the following formula and added as a quadratic cost term to the cost function to represent the tracking term. This transforms the trajectory tracking problem into an optimization problem. The specific solution is as follows:

[0089]

[0090] Where E(x) represents the deviation between the end effector pose and the target pose, e pos (x) represents the coordinates of the robotic arm's end effector, T pos e represents the coordinates of the target point of the mobile robotic arm's movement. o (x) represents the end effector posture of the robotic arm, T o The target point orientation represents the movement of the mobile robotic arm, and the symbol ||·||2 represents the L2 norm.

[0091] Since each arm of the mobile dual-arm robot in this embodiment has only four degrees of freedom, and the end-effector spraying device sprays atomized disinfectant in all directions, the planning process will only consider the position of the end-effectors and not their orientation. The cost terms are simplified as follows:

[0092]

[0093] Finally, based on the kinematic model of the mobile robotic arm and the state of the obstacles, model predictive control is used to predict the motion state and control input of the mobile dual-arm robot in the future. The deviation between the end-effector pose and the target pose is used as the second-order cost term of the model predictive control cost function, and the real-time distance between the collision point and the obstacle is used as the penalty term of the model predictive control cost function. The weight of the penalty term of the cost function is adjusted according to the three obstacle classifications. The motion parameters of the mobile dual-arm robot are used as constraints of the model predictive control cost function. The optimal set of control inputs is solved using a nonlinear optimization solver and applied to the mobile dual-arm robot controller.

[0094] The specific cost function is as follows:

[0095]

[0096] Where J is the performance index of the cost function, N is the prediction space of the model predictive control, and E N This represents the deviation between the end effector pose of the robotic arm and the target pose. It is E N The transpose of E n The deviation at each time step from 0 to N It is E n transpose, u n The control input dataset for model predictive control includes the rotational angular velocities of each drive wheel of the mobile chassis and the rotational angular velocities of each joint of the robotic arm, where n ranges from 0 to N. The control input of the mobile robotic arm at each moment in the prediction space is calculated, d(O i ,ΔT·n) is the predicted Euclidean distance between the collision point and the obstacle at time n. Representing the obstacle avoidance penalty term, A, B, and C are the weight matrices of model predictive control, which determine the weights of the end-tracking error and system input in model predictive control. The identity matrix of model predictive control can be used directly.

[0097] The control input queue u with minimum J is obtained in real time during the movement of the mobile dual-arm robot using a nonlinear optimization solver. n The goal is to minimize the deviation between the end-effector pose and the target pose, and to maximize the control input data, while maximizing the Euclidean distance d between the collision point and the obstacle. The optimal control input data is then output to the mobile dual-arm robot controller to control the mobile robot to move toward the target and update its own state and environmental information. During the movement of the mobile dual-arm robot, the above calculation process is continuously iterated until the deviation between the end-effector and the target point reaches the set target constant threshold, that is, the movement reaches the target point.

[0098] During the nonlinear optimization solver process, the following constraints on the motion limit parameters of the mobile robotic arm hardware are also followed:

[0099]

[0100] Where z represents the motion state of the mobile robotic arm, z min z max Let u be the minimum and maximum of its limits, and u represent the control input of the mobile robotic arm. min u max Let the minimum and maximum values ​​of its limit be given. The first derivative representing the motion state of the mobile robotic arm. This refers to the minimum and maximum limits. Specifically, it includes the limitations imposed on the position and posture of the mobile chassis and the robotic arm by the motion space during the operation of the mobile robotic arm, as well as the limitations on the speed and acceleration of the mobile chassis by input control, and the limitations on the angles and angular velocities of each joint of the robotic arm.

[0101] The above are merely preferred embodiments of the present invention and do not limit the scope of the patent. Any equivalent structural or procedural transformations made based on the description and drawings of the present invention, or direct or indirect applications in other related technical fields, are similarly included within the scope of patent protection of the present invention.

Claims

1. A motion control method for a mobile robotic arm in a dynamic scene, characterized in that: The mobile chassis and the mobile robot arm are jointly modeled to construct a kinematic model of the mobile robot arm. During the movement of the mobile robot arm, the position, attitude and velocity of the mobile chassis, as well as the angular velocity and angle of each joint of the robot arm, are updated in real time. During the movement of the mobile robotic arm, it continuously identifies surrounding obstacles and classifies them into stationary obstacles, obstacles moving at a constant speed, and obstacles moving randomly according to their motion state. Several collision points are set at the protruding position of the mobile robotic arm to obtain the distance between it and the obstacles. The mobile robotic arm acquires obstacle information by outputting point cloud information of environmental obstacles through a 3D depth camera. The obstacles identified by the mobile robotic arm are parameterized into several obstacle spheres using the world coordinate system as a reference, and each obstacle sphere is tracked. The definition of each obstacle sphere is as follows: ， in Represents the i-th obstacle sphere. Represents the coordinates of the center of the i-th obstacle sphere. Represents the radius of the sphere containing the i-th obstacle. This represents the state of the i-th obstacle sphere; when the obstacle is stationary, When the obstacle moves at a constant speed, , This represents the velocity and direction of the obstacle sphere in the world coordinate system. When the obstacle is not moving at a uniform velocity... This indicates that the obstacle sphere is in a state of irregular motion. Within the observation interval M, obstacles are identified and measured multiple times, and their states are classified using the following formula: 、 ， in The rate of change of the position of the i-th obstacle sphere is a three-dimensional vector. and Let the average rate of change and the standard deviation of the rate of change of the i-th obstacle sphere be represented by two constants. , To determine the three obstacle states: At that time, it was a stationary obstacle; and The obstacle was moving at a constant speed. At times, it is an obstacle with irregular movement; Based on the kinematic model of the mobile robotic arm and the state of the obstacle, model predictive control is used to predict the motion state and control input of the mobile robotic arm over a period of time. The deviation between the end-effector pose and the target pose is used as the quadratic cost term of the model predictive control cost function, and the real-time distance between the collision point and the obstacle is used as the penalty term of the model predictive control cost function. The weight of the penalty term of the cost function is adjusted according to three obstacle classifications. The motion parameters of the mobile robotic arm are used as constraints of the model predictive control cost function. The optimal set of control inputs is solved using a nonlinear optimization solver and applied to the mobile robotic arm controller to realize the motion control of the mobile robotic arm.

2. The motion control method for a mobile robotic arm in a dynamic scene according to claim 1, characterized in that: Before the mobile robotic arm moves, it plans an initial path based on the global map of the motion space and the target point information, and samples several reference points in the initial path as path points for the movement of the mobile robotic arm.

3. The motion control method for a mobile robotic arm in a dynamic scene according to claim 1, characterized in that: The kinematic model of the mobile robotic arm is defined as follows: ， ， ， Where z represents the motion state of the mobile robotic arm, and x and y represent the coordinates of the mobile chassis. Indicates the direction of movement of the mobile chassis. Data set representing the angles of each joint of the robotic arm. The control inputs representing the mobile robotic arm, where These represent the rotational angular velocities of the drive wheels of the chassis for left and right movement, respectively. Represents the rotational angular velocity of each joint of the robotic arm. The first derivative represents the motion state of the mobile robotic arm, where Represents the radius of the drive wheels of the mobile chassis. This represents the wheelbase between the drive wheels of the mobile chassis.

4. The motion control method for a mobile robotic arm in a dynamic scene according to claim 1, characterized in that: The deviation between the robotic arm's end-effector pose and the target pose is calculated using the following formula: ， in, This represents the deviation between the pose of the robotic arm's end effector and the target pose. Represents the coordinates of the robotic arm's end effector. The coordinates of the target point representing the movement of the mobile robotic arm. Represents the end effector posture of the robotic arm. The target point orientation representing the movement of the mobile robotic arm, symbol It represents the L2 norm.

5. The motion control method for a mobile robotic arm in a dynamic scene according to claim 1, characterized in that: The mobile robotic arm constructs a collision point sphere with each collision point as its center, and adjusts the radius of the collision point sphere until the sphere covers the mobile robotic arm. In the world coordinate system, a collision is determined to have occurred when the mobile robotic arm detects that the distance between the obstacle and the sphere is 0. During the movement of the mobile robotic arm, the Euclidean distance between the collision point and the obstacle is calculated using the coordinates of the centers of the spheres at the collision point and the obstacle. The obstacle avoidance penalty term D(d) of the model predictive control cost function is obtained by applying the following penalty function to the Euclidean distance between the collision point and the obstacle: ， in and This is a penalty coefficient, adjusted based on the state of the obstacle. The set safety distance means that if the moving robotic arm and the obstacle exceed this distance, they will no longer be subject to the penalty function. d is the Euclidean distance between the collision point and the obstacle. When the collision point of the mobile robotic arm approaches an obstacle, the penalty function will incur a large cost, causing the robot to move in the direction away from the obstacle.

6. The motion control method for a mobile robotic arm in a dynamic scene according to claim 5, characterized in that: The Euclidean distance between the collision point on the mobile robotic arm and the obstacle is defined by the following formula: ， in This represents the coordinates of the collision point of the mobile robotic arm in the world coordinate system. The coordinates of the obstacle are determined by the coordinates of the center of the obstacle sphere. The state of an obstacle is determined by the state of the obstacle sphere. This applies to obstacles moving at a constant speed or stationary obstacles in the future. Predicting the location within a time period for obstacles with irregular movement. Different penalty coefficients can be set, defined as follows: ， ＞ ， ＞ 。 7. A motion control method for a mobile robotic arm in a dynamic scene according to any one of claims 1-6, characterized in that: The cost function is as follows: ； Where J is the performance index of the cost function, and N is the prediction space of the model predictive control. This represents the deviation between the end effector pose of the robotic arm and the target pose. yes transpose, The deviation at each time step from 0 to N yes transpose, The control input dataset for model predictive control includes the rotational angular velocities of each drive wheel of the mobile chassis and the rotational angular velocities of each joint of the robotic arm, where n ranges from 0 to N. This dataset is used to solve for the control input of the mobile robotic arm at each moment in the prediction space. Let be the Euclidean distance between the collision point and the obstacle predicted at time n. Representing the obstacle avoidance penalty term, A, B, and C are the weight matrices of the model predictive control, which determine the weights of the end-tracking error and the system input in the model predictive control. The control input queue with minimum J is found using a nonlinear optimization solver. The output acts on the mobile robotic arm controller to control the mobile robotic arm to move toward the target.

8. The motion control method for a mobile robotic arm in a dynamic scene according to claim 7, characterized in that: The cost function solution process also follows the constraints of the following motion limit parameters of the mobile robotic arm hardware: 、 、 ； Where z represents the motion state of the mobile robotic arm, z min z max Let the minimum and maximum values ​​of its limit be . Represents the control input quantity of the mobile robotic arm. , Let the minimum and maximum values ​​of its limit be . The first derivative representing the motion state of the mobile robotic arm. , It is the minimum and maximum of its limit.

9. A mobile robotic arm employing the motion control method according to any one of claims 1-8.

Images